Skip to content
GitLab
Unverified Commit 2a578be9 authored by stonneau's avatar stonneau Committed by GitHub
Browse files

Merge pull request #19 from pFernbach/devel 

Devel
parents c61085c9 fd6eac27
Hide whitespace changes
Inline Side-by-side
data/srdf/slalom_debris.srdf 0 → 100644
View file @ 2a578be9
<robot name="chair">
<link name="base_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://hpp-rbprm-corba/meshes/slalom_debris.stl"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://hpp-rbprm-corba/meshes/slalom_debris.stl"/>
</geometry>
</collision>
</link>
</robot>
data/urdf/slalom_debris.urdf 0 → 100755
View file @ 2a578be9
<robot name="chair">
<link name="base_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://hpp-rbprm-corba/meshes/slalom_debris.stl"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<mesh filename="package://hpp-rbprm-corba/meshes/slalom_debris.stl"/>
</geometry>
</collision>
</link>
</robot>
script/scenarios/demos/darpa_hyq.py
View file @ 2a578be9
#Importing helper class for RBPRM
from hpp.corbaserver.rbprm.hyq import Robot
from hpp.corbaserver.rbprm.hyq_contact6D import Robot
from hpp.corbaserver.problem_solver import ProblemSolver
from hpp.gepetto import Viewer
......@@ -18,51 +18,35 @@ from hpp.corbaserver import Client
# This time we load the full body model of HyQ
fullBody = Robot ()
fullBody.setJointBounds ("root_joint", [-2,5, -1, 1, 0.3, 4])
fullBody.client.robot.setDimensionExtraConfigSpace(6)
fullBody.client.robot.setExtraConfigSpaceBounds([0,0,0,0,0,0,0,0,0,0,0,0])
# Setting a number of sample configurations used
nbSamples = 10000
ps = tp.ProblemSolver(fullBody)
r = tp.Viewer (ps, viewerClient=tp.r.client)
v = tp.Viewer (ps, viewerClient=tp.v.client)
rootName = 'base_joint_xyz'
cType = "_3_DOF"
cType = "_6_DOF"
def addLimbDb(limbId, heuristicName, loadValues = True, disableEffectorCollision = False):
fullBody.addLimbDatabase(str(db_dir+limbId+'.db'), limbId, heuristicName,loadValues, disableEffectorCollision)
fullBody.addLimb(fullBody.rLegId,fullBody.rleg,fullBody.rfoot,fullBody.offset,fullBody.normal, fullBody.legx, fullBody.legy, nbSamples, "random", 0.1, cType)
fullBody.addLimb(fullBody.lLegId,fullBody.lleg,fullBody.lfoot,fullBody.offset,fullBody.normal, fullBody.legx, fullBody.legy, nbSamples, "random", 0.05, cType)
fullBody.addLimb(fullBody.rArmId,fullBody.rarm,fullBody.rhand,fullBody.offset,fullBody.normal, fullBody.legx, fullBody.legy, nbSamples, "random", 0.05, cType)
fullBody.addLimb(fullBody.rLegId,fullBody.rleg,fullBody.rfoot,fullBody.offset,fullBody.normal, fullBody.legx, fullBody.legy, nbSamples, "forward", 0.1, cType)
fullBody.addLimb(fullBody.lLegId,fullBody.lleg,fullBody.lfoot,fullBody.offset,fullBody.normal, fullBody.legx, fullBody.legy, nbSamples, "forward", 0.05, cType)
fullBody.addLimb(fullBody.rArmId,fullBody.rarm,fullBody.rhand,fullBody.offset,fullBody.normal, fullBody.legx, fullBody.legy, nbSamples, "forward", 0.05, cType)
fullBody.addLimb(fullBody.lArmId,fullBody.larm,fullBody.lhand,fullBody.offset,fullBody.normal, fullBody.legx, fullBody.legy, nbSamples, "random", 0.05, cType)
#~ fullBody.runLimbSampleAnalysis(rLegId, "jointLimitsDistance", True)
#~ fullBody.runLimbSampleAnalysis(lLegId, "jointLimitsDistance", True)
#~ fullBody.runLimbSampleAnalysis(rarmId, "jointLimitsDistance", True)
#~ fullBody.runLimbSampleAnalysis(larmId, "jointLimitsDistance", True)
"""
q_init = [-2.0,
0.0,
0.6838277139631803,
0.0,
0.0,
0.0,
1.0,
0.14279812395541294,
0.934392553166556,
-0.9968239786882757,
-0.06521258938340457,
-0.8831796268418511,
1.150049183494211,
-0.06927610020154493,
0.9507443168724581,
-0.8739975339028809,
0.03995660287873871,
-0.9577096766517215,
0.9384602821326071]
"""
q_init=fullBody.referenceConfig[::]; q_init[0:7] = tp.q_init[0:7]; q_init[2]=fullBody.referenceConfig[2]+0.02
q_goal = fullBody.referenceConfig[::]; q_goal[0:7] = tp.q_goal[0:7]; q_goal[2]=fullBody.referenceConfig[2]+0.02
q_init=fullBody.referenceConfig[::] + [0]*6; q_init[0:7] = tp.q_init[0:7];
#q_init[2]=fullBody.referenceConfig[2]
q_goal = fullBody.referenceConfig[::]+ [0]*6; q_goal[0:7] = tp.q_goal[0:7];
#q_goal[2]=fullBody.referenceConfig[2]
q_init = fullBody.generateContacts(q_init, [0,0,1])
......@@ -73,12 +57,13 @@ fullBody.setStartState(q_init,[fullBody.rLegId,fullBody.lArmId,fullBody.lLegId,f
fullBody.setEndState(q_goal,[fullBody.rLegId,fullBody.lArmId,fullBody.lLegId,fullBody.rArmId])
r(q_init)
v(q_init)
configs = []
from hpp.gepetto import PathPlayer
pp = PathPlayer (fullBody.client, r)
pp = PathPlayer (fullBody.client, v)
import time
......@@ -86,49 +71,49 @@ import time
#DEMO METHODS
def initConfig():
r.client.gui.setVisibility("hyq", "ON")
v.client.gui.setVisibility("hyq", "ON")
tp.cl.problem.selectProblem("default")
tp.r.client.gui.setVisibility("toto", "OFF")
tp.r.client.gui.setVisibility("hyq_trunk_large", "OFF")
r(q_init)
tp.v.client.gui.setVisibility("toto", "OFF")
tp.v.client.gui.setVisibility("hyq_trunk_large", "OFF")
v(q_init)
def endConfig():
r.client.gui.setVisibility("hyq", "ON")
v.client.gui.setVisibility("hyq", "ON")
tp.cl.problem.selectProblem("default")
tp.r.client.gui.setVisibility("toto", "OFF")
tp.r.client.gui.setVisibility("hyq_trunk_large", "OFF")
r(q_goal)
tp.v.client.gui.setVisibility("toto", "OFF")
tp.v.client.gui.setVisibility("hyq_trunk_large", "OFF")
v(q_goal)
def rootPath():
r.client.gui.setVisibility("hyq", "OFF")
v.client.gui.setVisibility("hyq", "OFF")
tp.cl.problem.selectProblem("rbprm_path")
tp.r.client.gui.setVisibility("toto", "OFF")
r.client.gui.setVisibility("hyq", "OFF")
tp.r.client.gui.setVisibility("hyq_trunk_large", "ON")
tp.v.client.gui.setVisibility("toto", "OFF")
v.client.gui.setVisibility("hyq", "OFF")
tp.v.client.gui.setVisibility("hyq_trunk_large", "ON")
tp.pp(0)
tp.r.client.gui.setVisibility("hyq_trunk_large", "OFF")
r.client.gui.setVisibility("hyq", "ON")
tp.v.client.gui.setVisibility("hyq_trunk_large", "OFF")
v.client.gui.setVisibility("hyq", "ON")
tp.cl.problem.selectProblem("default")
def genPlan(stepsize=0.06):
tp.cl.problem.selectProblem("default")
r.client.gui.setVisibility("hyq", "ON")
tp.r.client.gui.setVisibility("toto", "OFF")
tp.r.client.gui.setVisibility("hyq_trunk_large", "OFF")
v.client.gui.setVisibility("hyq", "ON")
tp.v.client.gui.setVisibility("toto", "OFF")
tp.v.client.gui.setVisibility("hyq_trunk_large", "OFF")
global configs
start = time.clock()
configs = fullBody.interpolate(stepsize, 8, 0, filterStates = False, testReachability=False, quasiStatic=True)
end = time.clock()
start = time.time()
configs = fullBody.interpolate(stepsize, 5, 0., filterStates = True,testReachability = False)
end = time.time()
print "Contact plan generated in " + str(end-start) + "seconds"
def contactPlan(step = 0.5):
r.client.gui.setVisibility("hyq", "ON")
v.client.gui.setVisibility("hyq", "ON")
tp.cl.problem.selectProblem("default")
tp.r.client.gui.setVisibility("toto", "OFF")
tp.r.client.gui.setVisibility("hyq_trunk_large", "OFF")
tp.v.client.gui.setVisibility("toto", "OFF")
tp.v.client.gui.setVisibility("hyq_trunk_large", "OFF")
for i in range(0,len(configs)):
r(configs[i]);
v(configs[i]);
time.sleep(step)
......
script/scenarios/demos/darpa_hyq_path.py
View file @ 2a578be9
......@@ -21,13 +21,24 @@ rbprmBuilder.boundSO3([-0.4,0.4,-0.3,0.3,-0.3,0.3])
# Creating an instance of HPP problem solver and the viewer
from hpp.corbaserver.problem_solver import ProblemSolver
ps = ProblemSolver( rbprmBuilder )
r = Viewer (ps)
from hpp.gepetto import ViewerFactory
vf = ViewerFactory (ps)
from hpp.corbaserver.affordance.affordance import AffordanceTool
afftool = AffordanceTool ()
#~ afftool.loadObstacleModel (packageName, "darpa", "planning", r, reduceSizes=[0.05,0.,0.])
afftool.loadObstacleModel ("hpp-rbprm-corba", "darpa", "planning", vf,reduceSizes=[0.1,0,0])
v = vf.createViewer()
#afftool.visualiseAffordances('Support', v, [0.25, 0.5, 0.5])
# Setting initial and goal configurations
q_init = rbprmBuilder.getCurrentConfig ();
q_init [0:3] = [-2, 0, 0.63]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
q_init [0:3] = [-2, 0, 0.64]; rbprmBuilder.setCurrentConfig (q_init); v (q_init)
q_goal = q_init [::]
q_goal [0:3] = [3, 0, 0.63]; r (q_goal)
q_goal [0:3] = [3, 0, 0.64]; v (q_goal)
#~ q_goal [0:3] = [-1.5, 0, 0.75]; r (q_goal)
# Choosing a path optimizer
......@@ -35,12 +46,6 @@ ps.addPathOptimizer("RandomShortcut")
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
from hpp.corbaserver.affordance.affordance import AffordanceTool
afftool = AffordanceTool ()
#~ afftool.loadObstacleModel (packageName, "darpa", "planning", r, reduceSizes=[0.05,0.,0.])
afftool.loadObstacleModel ("hpp-rbprm-corba", "darpa", "planning", r)
afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5])
# Choosing RBPRM shooter and path validation methods.
# Note that the standard RRT algorithm is used.
ps.client.problem.selectConfigurationShooter("RbprmShooter")
......@@ -56,11 +61,11 @@ print "computation time for root path ", t
# Playing the computed path
from hpp.gepetto import PathPlayer
pp = PathPlayer (r)
pp = PathPlayer (v)
q_far = q_init [::]
q_far [0:3] = [-2, -3, 0.63];
r(q_far)
q_far [0:3] = [-2, -3, 0.6];
v(q_far)
for i in range(1,10):
rbprmBuilder.client.problem.optimizePath(i)
......@@ -76,6 +81,6 @@ rbprmBuilder2 = Robot ("toto")
ps2 = ProblemSolver( rbprmBuilder2 )
cl.problem.selectProblem("default")
cl.problem.movePathToProblem(8,"rbprm_path",rbprmBuilder.getAllJointNames()[1:])
r2 = Viewer (ps2, viewerClient=r.client)
r2 = Viewer (ps2, viewerClient=v.client)
r2(q_far)
script/scenarios/demos/hyq_slalom_debris.py 0 → 100644
View file @ 2a578be9
from hpp.corbaserver.rbprm.hyq_contact6D import Robot
from hpp.gepetto import Viewer
from tools.display_tools import *
import time
print "Plan guide trajectory ..."
import hyq_slalom_debris_path as tp
print "Done."
import time
pId = tp.ps.numberPaths() -1
fullBody = Robot ()
# Set the bounds for the root
root_bounds = tp.root_bounds
root_bounds[4] -= 0.05
root_bounds[5] += 0.05
fullBody.setJointBounds ("root_joint", root_bounds)
# add the 6 extraDof for velocity and acceleration (see *_path.py script)
fullBody.client.robot.setDimensionExtraConfigSpace(tp.extraDof)
fullBody.client.robot.setExtraConfigSpaceBounds([-tp.vMax,tp.vMax,-tp.vMax,tp.vMax,0,0,-tp.aMax,tp.aMax,-tp.aMax,tp.aMax,0,0])
ps = tp.ProblemSolver( fullBody )
ps.setParameter("Kinodynamic/velocityBound",tp.vMax)
ps.setParameter("Kinodynamic/accelerationBound",tp.aMax)
#load the viewer
v = tp.Viewer (ps,viewerClient=tp.v.client, displayCoM = True)
# load a reference configuration
q_ref = fullBody.referenceConfig[::]+[0]*6
q_init = q_ref[::]
fullBody.setReferenceConfig(q_ref)
fullBody.setCurrentConfig (q_init)
print "Generate limb DB ..."
tStart = time.time()
# generate databases :
nbSamples = 10000
fullBody.addLimb(fullBody.rLegId,fullBody.rleg,fullBody.rfoot,fullBody.offset,fullBody.normal, fullBody.legx, fullBody.legy, nbSamples, "fixedStep06", 0.01)
fullBody.addLimb(fullBody.lLegId,fullBody.lleg,fullBody.lfoot,fullBody.offset,fullBody.normal, fullBody.legx, fullBody.legy, nbSamples, "fixedStep06", 0.01)
fullBody.addLimb(fullBody.rArmId,fullBody.rarm,fullBody.rhand,fullBody.offset,fullBody.normal, fullBody.legx, fullBody.legy, nbSamples, "fixedStep06", 0.01)
fullBody.addLimb(fullBody.lArmId,fullBody.larm,fullBody.lhand,fullBody.offset,fullBody.normal, fullBody.legx, fullBody.legy, nbSamples, "fixedStep06", 0.01)
for limbId in fullBody.limbNames :
fullBody.runLimbSampleAnalysis(limbId, "ReferenceConfiguration", True)
tGenerate = time.time() - tStart
print "Done."
print "Databases generated in : "+str(tGenerate)+" s"
#define initial and final configurations :
configSize = fullBody.getConfigSize() -fullBody.client.robot.getDimensionExtraConfigSpace()
q_init[0:7] = tp.ps.configAtParam(pId,0.01)[0:7] # use this to get the correct orientation
q_goal = q_init[::]; q_goal[0:7] = tp.ps.configAtParam(pId,tp.ps.pathLength(pId))[0:7]
dir_init = tp.ps.configAtParam(pId,0.01)[tp.indexECS:tp.indexECS+3]
acc_init = tp.ps.configAtParam(pId,0.01)[tp.indexECS+3:tp.indexECS+6]
dir_goal = tp.ps.configAtParam(pId,tp.ps.pathLength(pId)-0.01)[tp.indexECS:tp.indexECS+3]
acc_goal = [0,0,0]
robTreshold = 3
# copy extraconfig for start and init configurations
q_init[configSize:configSize+3] = dir_init[::]
q_init[configSize+3:configSize+6] = acc_init[::]
q_goal[configSize:configSize+3] = dir_goal[::]
q_goal[configSize+3:configSize+6] = [0,0,0]
fullBody.setStaticStability(True)
fullBody.setCurrentConfig (q_init)
fullBody.setCurrentConfig (q_goal)
v.addLandmark('hyq/base_link',0.3)
q_init[2] = fullBody.referenceConfig[2]
q_goal[2] = fullBody.referenceConfig[2]
v(q_init)
#q_init = fullBody.generateContacts(q_init, [0,0,1])
#q_goal = fullBody.generateContacts(q_goal, [0,0,1])
# specify the full body configurations as start and goal state of the problem
fullBody.setStartState(q_init,[fullBody.rLegId,fullBody.lArmId,fullBody.rArmId,fullBody.lLegId])
fullBody.setEndState(q_goal,[fullBody.rLegId,fullBody.lArmId,fullBody.rArmId,fullBody.lLegId])
print "Generate contact plan ..."
tStart = time.time()
configs = fullBody.interpolate(0.01,pathId=pId,robustnessTreshold = 2, filterStates = True,testReachability = False)
tInterpolateConfigs = time.time() - tStart
print "Done."
print "Contact plan generated in : "+str(tInterpolateConfigs)+" s"
print "number of configs :", len(configs)
#raw_input("Press Enter to display the contact sequence ...")
#displayContactSequence(v,configs)
script/scenarios/demos/hyq_slalom_debris_path.py 0 → 100644
View file @ 2a578be9
from hpp.corbaserver.rbprm.hyq_abstract import Robot
from hpp.gepetto import Viewer
from hpp.corbaserver import ProblemSolver
import time
vMax = 0.2# linear velocity bound for the root
aMax = 0.1# linear acceleration bound for the root
extraDof = 6
mu=0.5# coefficient of friction
# Creating an instance of the helper class, and loading the robot
# Creating an instance of the helper class, and loading the robot
rbprmBuilder = Robot ()
# Define bounds for the root : bounding box of the scenario
root_bounds = [-1.7,2.5, -0.2, 2, 0.63, 0.63]
rbprmBuilder.setJointBounds ("root_joint",root_bounds)
# The following lines set constraint on the valid configurations:
# a configuration is valid only if all limbs can create a contact with the corresponding afforcances type
rbprmBuilder.setFilter(['hyq_rhleg_rom', 'hyq_lfleg_rom', 'hyq_rfleg_rom','hyq_lhleg_rom'])
rbprmBuilder.setAffordanceFilter('hyq_rhleg_rom', ['Support'])
rbprmBuilder.setAffordanceFilter('hyq_rfleg_rom', ['Support'])
rbprmBuilder.setAffordanceFilter('hyq_lhleg_rom', ['Support'])
rbprmBuilder.setAffordanceFilter('hyq_lfleg_rom', ['Support'])
# We also bound the rotations of the torso. (z, y, x)
rbprmBuilder.boundSO3([-4,4,-0.1,0.1,-0.1,0.1])
# Add 6 extraDOF to the problem, used to store the linear velocity and acceleration of the root
rbprmBuilder.client.robot.setDimensionExtraConfigSpace(extraDof)
# We set the bounds of this extraDof with velocity and acceleration bounds (expect on z axis)
rbprmBuilder.client.robot.setExtraConfigSpaceBounds([-vMax,vMax,-vMax,vMax,0,0,-aMax,aMax,-aMax,aMax,0,0])
indexECS = rbprmBuilder.getConfigSize() - rbprmBuilder.client.robot.getDimensionExtraConfigSpace()
# Creating an instance of HPP problem solver
ps = ProblemSolver( rbprmBuilder )
# define parameters used by various methods :
ps.setParameter("Kinodynamic/velocityBound",vMax)
ps.setParameter("Kinodynamic/accelerationBound",aMax)
ps.setParameter("DynamicPlanner/sizeFootX",0.01)
ps.setParameter("DynamicPlanner/sizeFootY",0.01)
ps.setParameter("DynamicPlanner/friction",0.5)
ps.setParameter("Kinodynamic/forceOrientation",True)
# sample only configuration with null velocity and acceleration :
ps.setParameter("ConfigurationShooter/sampleExtraDOF",False)
ps.setParameter("PathOptimization/RandomShortcut/NumberOfLoops",50)
# initialize the viewer :
from hpp.gepetto import ViewerFactory
vf = ViewerFactory (ps)
# load the module to analyse the environnement and compute the possible contact surfaces
from hpp.corbaserver.affordance.affordance import AffordanceTool
afftool = AffordanceTool ()
afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005])
afftool.loadObstacleModel ("hpp-rbprm-corba", "slalom_debris", "planning", vf,reduceSizes=[0.05,0,0])
v = vf.createViewer(displayArrows = True)
#afftool.visualiseAffordances('Support', v, v.color.lightBrown)
# Setting initial configuration
q_init = rbprmBuilder.getCurrentConfig ();
q_init [0:3] = [-1.5, 0, 0.63]
q_init[3:7] = [0,0,0,1]
q_init[-6] = 0.05
v (q_init)
ps.setInitialConfig (q_init)
# set goal config
rbprmBuilder.setCurrentConfig (q_init)
q_goal = q_init [::]
q_goal[0] = 2.2
v(q_goal)
ps.addGoalConfig (q_goal)
# Choosing RBPRM shooter and path validation methods.
ps.selectConfigurationShooter("RbprmShooter")
ps.addPathOptimizer ("RandomShortcutDynamic")
ps.selectPathValidation("RbprmPathValidation",0.05)
# Choosing kinodynamic methods :
ps.selectSteeringMethod("RBPRMKinodynamic")
ps.selectDistance("Kinodynamic")
ps.selectPathPlanner("DynamicPlanner")
# Solve the planning problem :
t = ps.solve ()
print "Guide planning time : ",t
ps.optimizePath(1)
pid = ps.numberPaths()-1
# display solution :
from hpp.gepetto import PathPlayer
pp = PathPlayer (v)
pp.dt=0.1
#pp.displayVelocityPath(pid)
#v.client.gui.setVisibility("path_2_root","ALWAYS_ON_TOP")
pp.dt=0.01
#pp(0)
# move the robot out of the view before computing the contacts
q_far = q_init[::]
q_far[2] = -2
v(q_far)
script/scenarios/demos/talos_flatGround.py
View file @ 2a578be9
......@@ -32,9 +32,9 @@ tStart = time.time()
# generate databases :
nbSamples = 50000
fullBody.addLimb(fullBody.rLegId,fullBody.rleg,fullBody.rfoot,fullBody.rLegOffset,fullBody.rLegNormal, fullBody.rLegx, fullBody.rLegy, nbSamples, "fixedStep1", 0.01,kinematicConstraintsPath=fullBody.rLegKinematicConstraints)
fullBody.addLimb(fullBody.rLegId,fullBody.rleg,fullBody.rfoot,fullBody.rLegOffset,fullBody.rLegNormal, fullBody.rLegx, fullBody.rLegy, nbSamples, "fixedStep08", 0.01,kinematicConstraintsPath=fullBody.rLegKinematicConstraints)
fullBody.runLimbSampleAnalysis(fullBody.rLegId, "ReferenceConfiguration", True)
fullBody.addLimb(fullBody.lLegId,fullBody.lleg,fullBody.lfoot,fullBody.lLegOffset,fullBody.rLegNormal, fullBody.lLegx, fullBody.lLegy, nbSamples, "fixedStep1", 0.01,kinematicConstraintsPath=fullBody.lLegKinematicConstraints)
fullBody.addLimb(fullBody.lLegId,fullBody.lleg,fullBody.lfoot,fullBody.lLegOffset,fullBody.rLegNormal, fullBody.lLegx, fullBody.lLegy, nbSamples, "fixedStep08", 0.01,kinematicConstraintsPath=fullBody.lLegKinematicConstraints)
fullBody.runLimbSampleAnalysis(fullBody.lLegId, "ReferenceConfiguration", True)
......@@ -84,9 +84,10 @@ tStart = time.time()
configs = fullBody.interpolate(0.01,pathId=pId,robustnessTreshold = 2, filterStates = True)
tInterpolateConfigs = time.time() - tStart
print "Done."
print "Contact plan generated in : "+str(tInterpolateConfigs)+" s"
print "number of configs :", len(configs)
raw_input("Press Enter to display the contact sequence ...")
displayContactSequence(v,configs)
#raw_input("Press Enter to display the contact sequence ...")
#displayContactSequence(v,configs)
script/scenarios/demos/talos_flatGround_path.py
View file @ 2a578be9
from hpp.corbaserver.rbprm.talos_abstract import Robot
from hpp.gepetto import Viewer
from hpp.corbaserver import Client
from hpp.corbaserver import ProblemSolver
import time
......@@ -84,9 +83,10 @@ print "Guide planning time : ",t
# display solution :
from hpp.gepetto import PathPlayer
pp = PathPlayer (v)
pp.dt=0.03
pp.displayVelocityPath(0)
pp.dt=0.1
#pp.displayVelocityPath(0)
v.client.gui.setVisibility("path_0_root","ALWAYS_ON_TOP")
pp.dt=0.01
#pp(0)
# move the robot out of the view before computing the contacts
......
script/tools/disp_bezier.py
View file @ 2a578be9
from spline import bezier, bezier6, polynom, exact_cubic, curve_constraints, spline_deriv_constraint, from_bezier
from hpp_spline import bezier, bezier6, polynom, exact_cubic, curve_constraints, spline_deriv_constraint, from_bezier
from numpy import matrix
from numpy.linalg import norm
......
script/tools/polyBezier.py deleted 100644 → 0
View file @ c61085c9
from spline import bezier, bezier6, polynom, exact_cubic, curve_constraints, spline_deriv_constraint, from_bezier
import inspect
class PolyBezier:
def __init__(self,curves):
if not isinstance(curves,list): # deal with single bezier curve input
curves = [curves]
self.curves=curves
self.times=[]
self.times +=[0]
self.d_curves=[]
self.dd_curves=[]
self.jerk_curves=[]
for i in range(len(curves)):
if not isinstance(curves[i],bezier):
raise TypeError("PolyBezier must be called with a list of bezier curves (or a single bezier curve)")
self.times+=[curves[i].max() + self.times[-1]]
def findInterval(self,t):
if t>self.times[-1] or t<0:
raise ValueError("Parameter is outside of definition range of the curves, t = "+str(t) )
for cit in range(len(self.times)):
if t<=self.times[cit+1]:
return cit
raise ValueError("Error in times intervals for t = "+str(t))
def findIntervalAdjustTime(self,t):
id = self.findInterval(t)
t -= self.times[id]
return id,t
def getBezierAt(self,t):
id = self.findInterval(t)
return self.curves[id]
def __call__(self,t):
id = self.findInterval(t)
tc = t-self.times[id]
return self.curves[id](tc)
def numCurves(self):
return len(self.curves)
def length(self):
return self.times[-1]
def max(self):
return self.length()
def lengthNonZero(self):
length = 0
for c in self.curves:
if c.degree > 0:
length += (c.max() - c.min())
return length
def isInFirst(self,t):
id = self.findInterval(t)