r(q1)
import numpy as np
"""
ps.addPathOptimizer("Prune")
ps.optimizePath (0)
ps.numberPaths()
ps.pathLength(ps.numberPaths()-1)
len(ps.getWaypoints (ps.numberPaths()-1))
"""
ps.clearPathOptimizers()
cl.problem.setAlphaInit (0.05)
ps.addPathOptimizer("GradientBased")
ps.optimizePath (0)
ps.numberPaths()
ps.pathLength(ps.numberPaths()-1)
tGB = cl.problem.getTimeGB ()
timeValuesGB = cl.problem.getTimeValues ()
gainValuesGB = cl.problem.getGainValues ()
newGainValuesGB = ((1-np.array(gainValuesGB))*100).tolist() #percentage of initial length-value
ps.clearPathOptimizers()
ps.addPathOptimizer('RandomShortcut')
ps.optimizePath (0)
ps.pathLength(ps.numberPaths()-1)
ps.clearPathOptimizers()
ps.addPathOptimizer('PartialShortcut')
ps.optimizePath (0)
#ps.createOrientationConstraint ("orConstraint", "base_joint_rz", "", [1,0,0,0], [0,0,1])
#ps.setNumericalConstraints ("constraints", ["orConstraint"])
ps.selectPathPlanner ("VisibilityPrmPlanner")
ps.selectPathValidation ("Dichotomy", 0.)
ps.solve ()
ps.pathLength(0)
len(ps.getWaypoints (0))
import numpy as np
ps.addPathOptimizer("Prune")
ps.optimizePath (0)
ps.numberPaths()
ps.pathLength(ps.numberPaths()-1)
len(ps.getWaypoints (ps.numberPaths()-1))
ps.clearPathOptimizers()
#cl.problem.setAlphaInit (0.05)
ps.addPathOptimizer("GradientBased")
ps.optimizePath (1)
ps.numberPaths()
ps.pathLength(ps.numberPaths()-1)
tGB = cl.problem.getTimeGB ()
timeValuesGB = cl.problem.getTimeValues ()
gainValuesGB = cl.problem.getGainValues ()
newGainValuesGB = ((1-np.array(gainValuesGB))*100).tolist() #percentage of initial length-value
ps.clearPathOptimizers()
import datetime as dt
totalSolveTime = dt.timedelta (0)
totalOptimTime = dt.timedelta (0)
totalNumberNodes = 0
N = 20
for i in range (N):
ps.clearPathOptimizers()
ps.clearRoadmap ()
ps.resetGoalConfigs ()
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
t1 = dt.datetime.now ()
ps.solve ()
t2 = dt.datetime.now ()
ps.addPathOptimizer ("SplineGradientBased_bezier3")
ps.optimizePath (ps.numberPaths() - 1)
t3 = dt.datetime.now ()
totalSolveTime += t2 - t1
totalOptimTime += t3 - t2
print "Solve:", t2-t1
print "Optim:", t3-t2
n = len (ps.client.problem.nodes ())
totalNumberNodes += n
print ("Number nodes: " + str(n))
print ("Average solve time: " + str ((totalSolveTime.seconds+1e-6*totalSolveTime.microseconds)/float (N)))
print ("Average optim time: " + str ((totalOptimTime.seconds+1e-6*totalOptimTime.microseconds)/float (N)))
print ("Average number nodes: " + str (totalNumberNodes/float (N)))
# display solution :
from hpp.gepetto import PathPlayer
pp = PathPlayer(v)
pp.dt = 0.1
pp.displayVelocityPath(0)
#v.client.gui.setVisibility("path_0_root","ALWAYS_ON_TOP")
pp.dt = 0.01
#pp(0)
except Exception:
pass
# move the robot out of the view before computing the contacts
q_far = q_init[::]
q_far[2] = -2
pId = ps.numberPaths() - 1
from hpp.corbaserver import Client
#~ #DEMO code to play root path and final contact plan
cl = Client()
cl.problem.selectProblem("rbprm_path")
rbprmBuilder2 = Robot("toto")
ps2 = ProblemSolver(rbprmBuilder2)
cl.problem.selectProblem("default")
cl.problem.movePathToProblem(pId, "rbprm_path",
rbprmBuilder.getAllJointNames()[1:])
r2 = Viewer(ps2, viewerClient=v.client)
r2(q_far)
#~ v(q_far)
ps.setInitialConfig(q8)
ps.addGoalConfig(q9)
ps.solve()
ps.resetGoalConfigs()
ps.setInitialConfig(q9)
ps.addGoalConfig(q10)
ps.solve()
ps.resetGoalConfigs()
ps.setInitialConfig(q1)
ps.addGoalConfig(q10)
ps.solve()
print ps.pathLength(9)
ps.addPathOptimizer("GradientBased")
ps.optimizePath(9)
ps.numberPaths()
print ps.pathLength(ps.numberPaths() - 1)
"""
# pp(9) = p0 final
ps.optimizePath(9) # pp(10) = p1 final
ps.pathLength(9)
ps.pathLength(10)
ps.addPathOptimizer('RandomShortcut')
ps.optimizePath (9)
ps.pathLength(10)
ps.clearPathOptimizers()
ps.addPathOptimizer("GradientBased")
qf = [1, -3, 3, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.2, 1.0, -0.4, -1.0, 0.0, -0.2, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, -1.5, -0.2, 0.1, -0.3, 0.1, 0.1, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, -0.2, 0.6, -0.453786, 0.872665, -0.418879, 0.2, -0.4, 0.0, -0.453786, 0.1, 0.7, 0.0]
robot.isConfigValid(qf)
ps.setInitialConfig (q1); ps.addGoalConfig (q2); ps.solve ()
ps.solve ()
ps.pathLength(0)
ps.addPathOptimizer('RandomShortcut')
ps.optimizePath (0)
ps.pathLength(1)
ps.clearPathOptimizers()
ps.addPathOptimizer("GradientBased")
ps.optimizePath (0)
ps.numberPaths()
ps.pathLength(ps.numberPaths()-1)
pp(ps.numberPaths()-1)
r(ps.configAtParam(0,2))
ps.getWaypoints (0)
# Add light to scene
lightName = "li4"
r.client.gui.addLight (lightName, r.windowId, 0.01, [0.4,0.4,0.4,0.5])
r.client.gui.addToGroup (lightName, r.sceneName)
r.client.gui.applyConfiguration (lightName, [1,0,0,1,0,0,0])
r.client.gui.refresh ()
r(Q[0])
for i in range(0, len(Q)):
r(Q[i])
time.sleep (0.5)
#robot.isConfigValid(Q[0])
for i in range(0, len(Q)-1):
ps.setInitialConfig (Q[i]); ps.addGoalConfig (Q[i+1]); ps.solve (); ps.resetGoalConfigs ()
ps.setInitialConfig (Q[0]); ps.addGoalConfig (Q[len(Q)-1]); ps.solve ();
nInitialPath = ps.numberPaths()-1 #8
ps.pathLength(nInitialPath)
#ps.addPathOptimizer('RandomShortcut') #9
#ps.optimizePath (nInitialPath)
#ps.pathLength(ps.numberPaths()-1)
#ps.clearPathOptimizers()
ps.addPathOptimizer("GradientBased")
ps.optimizePath (nInitialPath)
ps.numberPaths()
ps.pathLength(ps.numberPaths()-1)
pp(ps.numberPaths()-1)
#ps.readRoadmap ('/local/mcampana/devel/hpp/data/ur5-sphere-PRM.rdm')
#ps.readRoadmap ('/local/mcampana/devel/hpp/data/ur5-sphere-RRT.rdm')
ps.selectPathPlanner ("VisibilityPrmPlanner")
#ps.selectPathValidation ("Dichotomy", 0.)
ps.solve ()
ps.pathLength(0)
len(ps.getWaypoints (0))
#ps.saveRoadmap ('/local/mcampana/devel/hpp/data/ur5-sphere-PRM.rdm')
ps.addPathOptimizer("Prune") # NO CHANGE WITH PRM+DISCR
ps.optimizePath (0)
ps.numberPaths()
ps.pathLength(ps.numberPaths()-1)
len(ps.getWaypoints (ps.numberPaths()-1))
ps.clearPathOptimizers()
cl.problem.setAlphaInit (0.3)
ps.addPathOptimizer("GradientBased")
ps.optimizePath (0)
ps.numberPaths()
ps.pathLength(ps.numberPaths()-1)
tGB = cl.problem.getTimeGB ()
timeValuesGB = cl.problem.getTimeValues ()
gainValuesGB = cl.problem.getGainValues ()
newGainValuesGB = ((1-np.array(gainValuesGB))*100).tolist() #percentage of initial length-value
ps.clearPathOptimizers()
mu = 1.2
#vmax = 6.5; mu = 0.5
#cl.problem.setFrictionCoef(mu); cl.problem.setMaxVelocityLim(vmax)
toSeconds = np.array([60 * 60, 60, 1, 1e-3])
offsetOrientedPath = 2 # If remove oriented path computation in ProblemSolver, set to 1 instead of 2
imax = 3
f = open('results.txt', 'a')
# Assuming that seed in modified directly in HPP (e.g. in core::PathPlanner::solve or ProblemSolver constr)
for i in range(0, imax):
print i
ps.clearRoadmap()
solveTimeVector = ps.solve()
solveTime = np.array(solveTimeVector).dot(toSeconds)
pathId = ps.numberPaths() - offsetOrientedPath
pathLength = ps.pathLength(pathId)
pathNumberWaypoints = len(ps.getWaypoints(pathId))
roadmapNumberNodes = ps.numberNodes()
#TODO: number collisions (checked ???)
#TODO: number parabola that has failed (because of which constraint ??)
#ps.addPathOptimizer("Prune")
#ps.optimizePath (pathId)
#prunePathId = ps.numberPaths()-1
# Write important results #
f.write('Try number: ' + str(i) + '\n')
f.write('with parameters: vmax=' + str(vmax) + ' and mu=' + str(mu) + '\n')
f.write('solve duration: ' + str(solveTime) + '\n')
f.write('path length: ' + str(pathLength) + '\n')
ps.selectPathValidation ("Progressive", 0.025)
import datetime as dt
totalTime = dt.timedelta (0)
totalNumberNodes = 0
for i in range (args.N):
ps.client.problem.clearRoadmap ()
ps.resetGoalConfigs ()
ps.setInitialConfig (q1proj)
ps.addGoalConfig (q2proj)
t1 = dt.datetime.now ()
ps.solve ()
t2 = dt.datetime.now ()
totalTime += t2 - t1
print((t2-t1))
n = len (ps.client.problem.nodes ())
totalNumberNodes += n
print(("Number nodes: " + str(n)))
print(("Average time: " + str ((totalTime.seconds+1e-6*totalTime.microseconds)/float (args.N))))
print(("Average number nodes: " + str (totalNumberNodes/float (args.N))))
#check if there are collisions, discontinuities or wrong configurations in each path
p = PathChecker(ps, q1proj, q2proj)
dtime = 0.001
for i in range (ps.numberPaths()):
print("\n---Path {}---".format(i))
p.check_path(i, dtime)
vf = ViewerFactory (ps)
# Choosing RBPRM shooter and path validation methods.
ps.selectConfigurationShooter("RbprmShooter")
ps.selectPathValidation("RbprmPathValidation", 0.05)
ps.addPathOptimizer("RandomShortcutDynamic")
# Choosing kinodynamic methods :
ps.selectSteeringMethod("RBPRMKinodynamic")
ps.selectDistance("Kinodynamic")
ps.selectPathPlanner("DynamicPlanner")
t = ps.solve()
print("Guide planning time : ", t)
#v.solveAndDisplay("rm",10,0.01)
for i in range(20):
print("Optimize path, " + str(i + 1) + "/20 ... ")
ps.optimizePath(ps.numberPaths() - 1)
pathId = ps.numberPaths() - 1
# display solution :
from hpp.gepetto import PathPlayer
pp = PathPlayer(v)
pp.dt = 0.01
pp.displayPath(pathId)
#v.client.gui.setVisibility("path_"+str(pathId)+"_root","ALWAYS_ON_TOP")
pp.dt = 0.01
#pp(pathId)
#v.client.gui.writeNodeFile("path_"+str(pathId)+"_root","guide_path_maze_hard.obj")
# move the robot out of the view before computing the contacts
q_far = q_init[::]
ps.addPathOptimizer('RandomShortcut')
ps.optimizePath (0)
ps.pathLength(1)
plotPointBodyCurvPath (r, cl, robot, dt, 1, jointName, [0.28,0,0], 'pathPointRS_'+jointName, [0.1,0.1,0.9,1])
#ps.clearPathOptimizers()
#ps.addPathOptimizer('PartialShortcut')
#ps.optimizePath (0)
#ps.pathLength(2)
ps.clearPathOptimizers()
ps.addPathOptimizer("GradientBased")
ps.optimizePath (0)
#ps.numberPaths()
ps.pathLength(ps.numberPaths()-1)
plotPointBodyCurvPath (r, cl, robot, dt, ps.numberPaths ()-1, jointName, [0.28,0,0], 'pathPointGB_'+jointName, [0.2,0.9,0.2,1])
qColl = [-1.09063,-2.10431,0.382719,0.12038,0.535622,-1.95976]
qFree = [-1.09063,-2.10431,0.382716,0.120381,0.535621,-1.95977] # dist 1e-6 to nearest obst...
# when qColl is in collision but without FCL contacts (precision probably not enough in debug)
qColl = [-1.24166,-2.30012,2.57358,-1.95423,1.23529,1.18378]
qFree = [-1.24166,-2.30012,2.57358,-1.95423,1.23529,1.18378]
pp(ps.numberPaths()-1)
ps.solve ()
samples = plotSampleSubPath (cl, r, 0, 20, "curvy", [0,0.2,1,1])
#r.client.gui.removeFromGroup ("ThetaPlane", r.sceneName)
q1 = cl.robot.projectOnObstacle (q11, 0.001); q2 = cl.robot.projectOnObstacle (q22, 0.001)
robot.isConfigValid(q1); robot.isConfigValid(q2)
ps.setInitialConfig (q1); ps.addGoalConfig (q2)
cl.problem.setFrictionCoef(0.5); cl.problem.setMaxVelocityLim(8)
ps.solve ()
samples = plotSampleSubPath (cl, r, 0, 20, "curvy", [0,0.2,1,1])
samples = plotSampleSubPath (cl, r, ps.numberPaths()-2, 20, "curvy1", [0.2,0.2,0.8,1])
samples = plotSampleSubPath (cl, r, ps.numberPaths()-2, 20, "curvy2", [0.2,0.8,0.2,1])
plotCone (q1, cl, r, "cone1", "friction_cone"); plotCone (q2, cl, r, "cone12", "friction_cone")
plotCone (q1, cl, r, "cone2", "friction_cone2"); plotCone (q2, cl, r, "cone22", "friction_cone2")
plotConeWaypoints (cl, 0, r, "wp", "friction_cone")
r.client.gui.setVisibility('robot/l_bounding_sphere',"OFF")
r(ps.configAtParam(0,0.001))
ps.pathLength(0)
ps.getWaypoints (0)
ps.clearRoadmap()
ps.selectPathPlanner ("VisibilityPrmPlanner")
#ps.selectPathValidation ("Dichotomy", 0.)
ps.solve ()
ps.pathLength(0)
ps.saveRoadmap ('/local/mcampana/devel/hpp/data/ur5-PRM1.rdm')
ps.addPathOptimizer('RandomShortcut')
ps.optimizePath (0)
ps.pathLength(1)
ps.clearPathOptimizers()
ps.addPathOptimizer("GradientBased")
ps.optimizePath (0)
ps.numberPaths()
ps.pathLength(ps.numberPaths()-1)
pp(ps.numberPaths()-1)
r(q2)
len(ps.getWaypoints (0))
# Add light to scene
lightName = "li"
r.client.gui.addLight (lightName, r.windowId, 0.1, [0.8,0.8,0.8,0.7])
r.client.gui.addToGroup (lightName, r.sceneName)
r.client.gui.applyConfiguration (lightName, [-25,0,0,1,0,0,0])
r.client.gui.refresh ()
q8 = [xStone-1.5, yEmu+0.8, zEmu, 0.707106781, 0, 0, -0.707106781]
q9 = [xStone-2, yEmu+0, zEmu, 1, 0, 0, 0]
r(q1)
robot.isConfigValid(q1)
ps.setInitialConfig (q1); ps.addGoalConfig (q2); ps.solve (); ps.resetGoalConfigs ()
ps.setInitialConfig (q2); ps.addGoalConfig (q3); ps.solve (); ps.resetGoalConfigs ()
ps.setInitialConfig (q3); ps.addGoalConfig (q4); ps.solve (); ps.resetGoalConfigs ()
ps.setInitialConfig (q4); ps.addGoalConfig (q5); ps.solve (); ps.resetGoalConfigs ()
ps.setInitialConfig (q5); ps.addGoalConfig (q6); ps.solve (); ps.resetGoalConfigs ()
ps.setInitialConfig (q6); ps.addGoalConfig (q7); ps.solve (); ps.resetGoalConfigs ()
ps.setInitialConfig (q7); ps.addGoalConfig (q8); ps.solve (); ps.resetGoalConfigs ()
ps.setInitialConfig (q8); ps.addGoalConfig (q9); ps.solve (); ps.resetGoalConfigs ()
ps.setInitialConfig (q1); ps.addGoalConfig (q9); ps.solve (); ps.resetGoalConfigs ()
nInitialPath = ps.numberPaths()-1 #8
ps.pathLength(nInitialPath)
#ps.addPathOptimizer('RandomShortcut') #9
#ps.optimizePath (nInitialPath)
#ps.pathLength(ps.numberPaths()-1)
#ps.clearPathOptimizers()
ps.addPathOptimizer("GradientBased")
ps.optimizePath (nInitialPath)
ps.numberPaths()
ps.pathLength(ps.numberPaths()-1)
pp(ps.numberPaths()-1)
ps.setInitialConfig (q1); ps.addGoalConfig (q4); ps.solve (); #3
"""
q1 = [-2, 0]; q2 = [-1, 1]; q3 = [-1.2, 1.8]; q4 = [-0.2, 1.2]; q5 = [0.5, 1.9]
q6 = [2, 1.5]; q7 = [1, 0.5]; q8 = [2, 0]
ps.setInitialConfig (q1); ps.addGoalConfig (q2); ps.solve (); ps.resetGoalConfigs ()
ps.setInitialConfig (q2); ps.addGoalConfig (q3); ps.solve (); ps.resetGoalConfigs ()
ps.setInitialConfig (q3); ps.addGoalConfig (q4); ps.solve (); ps.resetGoalConfigs ()
ps.setInitialConfig (q4); ps.addGoalConfig (q5); ps.solve (); ps.resetGoalConfigs ()
ps.setInitialConfig (q5); ps.addGoalConfig (q6); ps.solve (); ps.resetGoalConfigs ()
ps.setInitialConfig (q6); ps.addGoalConfig (q7); ps.solve (); ps.resetGoalConfigs ()
ps.setInitialConfig (q7); ps.addGoalConfig (q8); ps.solve (); ps.resetGoalConfigs ()
ps.setInitialConfig (q1); ps.addGoalConfig (q8); ps.solve (); # 7
nInitialPath = ps.numberPaths()-1 #8
ps.pathLength(nInitialPath)
ps.addPathOptimizer("GradientBased")
#ps.addPathOptimizer("Prune")
#ps.addPathOptimizer("PartialRandomShortcut")
ps.optimizePath(nInitialPath)
ps.pathLength(ps.numberPaths()-1)
ps.getWaypoints (nInitialPath)
ps.getWaypoints (ps.numberPaths()-1)
"""
ps.addGoalConfig(q2)
cl.obstacle.loadObstacleModel('potential_description', 'obstacles_concaves',
'obstacles_concaves')
#ps.createOrientationConstraint ("orConstraint", "base_joint_rz", "", [1,0,0,0], [0,0,1])
#ps.setNumericalConstraints ("constraints", ["orConstraint"])
ps.selectPathPlanner("VisibilityPrmPlanner")
#ps.selectPathValidation ("Dichotomy", 0.)
ps.solve()
ps.pathLength(0)
ps.addPathOptimizer("GradientBased")
ps.optimizePath(0)
ps.numberPaths()
ps.pathLength(ps.numberPaths() - 1)
import matplotlib.pyplot as plt
from mutable_trajectory_plot import planarPlot, addNodePlot
from parseLog import parseCollConstrPoints
num_log = 31891
contactPoints = parseCollConstrPoints(num_log, '77: contact point = (')
plt = planarPlot(cl, 0, ps.numberPaths() - 1, plt, 1.5, 5)
plt = addNodePlot(contactPoints, 'ko', '', 5.5, plt)
plt.show()
ps.addPathOptimizer('RandomShortcut')
ps.optimizePath(0)
ps.pathLength(1)
# Choosing RBPRM shooter and path validation methods.
ps.selectConfigurationShooter("RbprmShooter")
ps.selectPathValidation("RbprmPathValidation",0.05)
# Choosing kinodynamic methods :
ps.selectSteeringMethod("RBPRMKinodynamic")
ps.selectDistance("Kinodynamic")
ps.selectPathPlanner("DynamicPlanner")
ps.addPathOptimizer ("RandomShortcutDynamic")
#ps.addPathOptimizer ("RandomShortcut")
# Solve the planning problem :
t = ps.solve ()
print("Guide planning time : ",t)
#v.solveAndDisplay('rm',2,0.001)
#v.client.gui.removeFromGroup("rm",v.sceneName)
pid = ps.numberPaths()-1
for i in range(5):
print("Optimization pass ",i)
ps.optimizePath(pid)
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_1_root","ALWAYS_ON_TOP")
pp.dt = 0.01
pp(pid)
class AbstractPathPlanner:
rbprmBuilder = None
ps = None
v = None
afftool = None
pp = None
extra_dof_bounds = None
robot_node_name = None # name of the robot in the node list of the viewer
def __init__(self):
self.v_max = -1 # bounds on the linear velocity for the root, negative values mean unused
self.a_max = -1 # bounds on the linear acceleration for the root, negative values mean unused
self.root_translation_bounds = [
0
] * 6 # bounds on the root translation position (-x, +x, -y, +y, -z, +z)
self.root_rotation_bounds = [
-3.14, 3.14, -0.01, 0.01, -0.01, 0.01
] # bounds on the rotation of the root (-z, z, -y, y, -x, x)
# The rotation bounds are only used during the random sampling, they are not enforced along the path
self.extra_dof = 6 # number of extra config appended after the joints configuration, 6 to store linear root velocity and acceleration
self.mu = 0.5 # friction coefficient between the robot and the environment
self.used_limbs = [
] # names of the limbs that must be in contact during all the motion
self.size_foot_x = 0 # size of the feet along the x axis
self.size_foot_y = 0 # size of the feet along the y axis
self.q_init = []
self.q_goal = []
@abstractmethod
def load_rbprm(self):
"""
Build an rbprmBuilder instance for the correct robot and initialize it's extra config size
"""
pass
def set_configurations(self):
self.rbprmBuilder.client.robot.setDimensionExtraConfigSpace(
self.extra_dof)
self.q_init = self.rbprmBuilder.getCurrentConfig()
self.q_goal = self.rbprmBuilder.getCurrentConfig()
self.q_init[2] = self.rbprmBuilder.ref_height
self.q_goal[2] = self.rbprmBuilder.ref_height
def compute_extra_config_bounds(self):
"""
Compute extra dof bounds from the current values of v_max and a_max
By default, set symmetrical bounds on x and y axis and bounds z axis values to 0
"""
# bounds for the extradof : by default use v_max/a_max on x and y axis and 0 on z axis
self.extra_dof_bounds = [
-self.v_max, self.v_max, -self.v_max, self.v_max, 0, 0,
-self.a_max, self.a_max, -self.a_max, self.a_max, 0, 0
]
def set_joints_bounds(self):
"""
Set the root translation and rotation bounds as well as the the extra dofs bounds
"""
self.rbprmBuilder.setJointBounds("root_joint",
self.root_translation_bounds)
self.rbprmBuilder.boundSO3(self.root_rotation_bounds)
self.rbprmBuilder.client.robot.setExtraConfigSpaceBounds(
self.extra_dof_bounds)
def set_rom_filters(self):
"""
Define which ROM must be in collision at all time and with which kind of affordances
By default it set all the roms in used_limbs to be in contact with 'support' affordances
"""
self.rbprmBuilder.setFilter(self.used_limbs)
for limb in self.used_limbs:
self.rbprmBuilder.setAffordanceFilter(limb, ['Support'])
def init_problem(self):
"""
Load the robot, set the bounds and the ROM filters and then
Create a ProblemSolver instance and set the default parameters.
The values of v_max, a_max, mu, size_foot_x and size_foot_y must be defined before calling this method
"""
self.load_rbprm()
self.set_configurations()
self.compute_extra_config_bounds()
self.set_joints_bounds()
self.set_rom_filters()
self.ps = ProblemSolver(self.rbprmBuilder)
# define parameters used by various methods :
if self.v_max >= 0:
self.ps.setParameter("Kinodynamic/velocityBound", self.v_max)
if self.a_max >= 0:
self.ps.setParameter("Kinodynamic/accelerationBound", self.a_max)
if self.size_foot_x > 0:
self.ps.setParameter("DynamicPlanner/sizeFootX", self.size_foot_x)
if self.size_foot_y > 0:
self.ps.setParameter("DynamicPlanner/sizeFootY", self.size_foot_y)
self.ps.setParameter("DynamicPlanner/friction", 0.5)
# sample only configuration with null velocity and acceleration :
self.ps.setParameter("ConfigurationShooter/sampleExtraDOF", False)
def init_viewer(self,
env_name,
env_package="hpp_environments",
reduce_sizes=[0, 0, 0],
visualize_affordances=[]):
"""
Build an instance of hpp-gepetto-viewer from the current problemSolver
:param env_name: name of the urdf describing the environment
:param env_package: name of the package containing this urdf (default to hpp_environments)
:param reduce_sizes: Distance used to reduce the affordances plan toward the center of the plane
(in order to avoid putting contacts closes to the edges of the surface)
:param visualize_affordances: list of affordances type to visualize, default to none
"""
vf = ViewerFactory(self.ps)
self.afftool = AffordanceTool()
self.afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005])
self.afftool.loadObstacleModel("package://" + env_package + "/urdf/" +
env_name + ".urdf",
"planning",
vf,
reduceSizes=reduce_sizes)
self.v = vf.createViewer(ghost=True, displayArrows=True)
self.pp = PathPlayer(self.v)
for aff_type in visualize_affordances:
self.afftool.visualiseAffordances(aff_type, self.v,
self.v.color.lightBrown)
def init_planner(self, kinodynamic=True, optimize=True):
"""
Select the rbprm methods, and the kinodynamic ones if required
:param kinodynamic: if True, also select the kinodynamic methods
:param optimize: if True, add randomShortcut path optimizer (or randomShortcutDynamic if kinodynamic is also True)
"""
self.ps.selectConfigurationShooter("RbprmShooter")
self.ps.selectPathValidation("RbprmPathValidation", 0.05)
if kinodynamic:
self.ps.selectSteeringMethod("RBPRMKinodynamic")
self.ps.selectDistance("Kinodynamic")
self.ps.selectPathPlanner("DynamicPlanner")
if optimize:
if kinodynamic:
self.ps.addPathOptimizer("RandomShortcutDynamic")
else:
self.ps.addPathOptimizer("RandomShortcut")
def solve(self):
"""
Solve the path planning problem.
q_init and q_goal must have been defined before calling this method
"""
if len(self.q_init) != self.rbprmBuilder.getConfigSize():
raise ValueError(
"Initial configuration vector do not have the right size")
if len(self.q_goal) != self.rbprmBuilder.getConfigSize():
raise ValueError(
"Goal configuration vector do not have the right size")
self.ps.setInitialConfig(self.q_init)
self.ps.addGoalConfig(self.q_goal)
self.v(self.q_init)
t = self.ps.solve()
print("Guide planning time : ", t)
def display_path(self, path_id=-1, dt=0.1):
"""
Display the path in the viewer, if no path specified display the last one
:param path_id: the Id of the path specified, default to the most recent one
:param dt: discretization step used to display the path (default to 0.1)
"""
if self.pp is not None:
if path_id < 0:
path_id = self.ps.numberPaths() - 1
self.pp.dt = dt
self.pp.displayVelocityPath(path_id)
def play_path(self, path_id=-1, dt=0.01):
"""
play the path in the viewer, if no path specified display the last one
:param path_id: the Id of the path specified, default to the most recent one
:param dt: discretization step used to display the path (default to 0.01)
"""
self.show_rom()
if self.pp is not None:
if path_id < 0:
path_id = self.ps.numberPaths() - 1
self.pp.dt = dt
self.pp(path_id)
def hide_rom(self):
"""
Remove the current robot from the display
"""
self.v.client.gui.setVisibility(self.robot_node_name, "OFF")
def show_rom(self):
"""
Add the current robot to the display
"""
self.v.client.gui.setVisibility(self.robot_node_name, "ON")
@abstractmethod
def run(self):
"""
Must be defined in the child class to run all the methods with the correct arguments.
"""
# example of definition:
"""
self.init_problem()
# define initial and goal position
self.q_init[:2] = [0, 0]
self.q_goal[:2] = [1, 0]
self.init_viewer("multicontact/ground", visualize_affordances=["Support"])
self.init_planner()
self.solve()
self.display_path()
self.play_path()
"""
pass
vmax = 8; mu = 1.2
#vmax = 6.5; mu = 0.5
#cl.problem.setFrictionCoef(mu); cl.problem.setMaxVelocityLim(vmax)
toSeconds = np.array ([60*60,60,1,1e-3])
offsetOrientedPath = 2 # If remove oriented path computation in ProblemSolver, set to 1 instead of 2
imax=3;
f = open('results.txt','a')
# Assuming that seed in modified directly in HPP (e.g. in core::PathPlanner::solve or ProblemSolver constr)
for i in range(0, imax):
print i
ps.clearRoadmap ()
solveTimeVector = ps.solve ()
solveTime = np.array (solveTimeVector).dot (toSeconds)
pathId = ps.numberPaths()-offsetOrientedPath
pathLength = ps.pathLength (pathId)
pathNumberWaypoints = len(ps.getWaypoints (pathId))
roadmapNumberNodes = ps.numberNodes ()
#TODO: number collisions (checked ???)
#TODO: number parabola that has failed (because of which constraint ??)
#ps.addPathOptimizer("Prune")
#ps.optimizePath (pathId)
#prunePathId = ps.numberPaths()-1
# Write important results #
f.write('Try number: '+str(i)+'\n')
f.write('with parameters: vmax='+str(vmax)+' and mu='+str(mu)+'\n')
f.write('solve duration: '+str(solveTime)+'\n')
f.write('path length: '+str(pathLength)+'\n')
q_init[0:2] = [-1.2,Y_VALUE]
q_init[-6] = vInit
v(q_init)
q_goal = q_init[::]
q_goal[0] = -0.55
q_goal[2] = Z_VALUE_PALET + 0.03
q_goal[3:7] = [ 0, -0.0499792, 0, 0.9987503 ]
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
# Solve the planning problem :
t = ps.solve()
print("Guide planning done in "+str(t))
pidBegin = ps.numberPaths()-1
## middle part on palet
q_init = q_goal[::]
q_goal[3:7] = [ 0, 0.0499792, 0, 0.9987503 ]
q_goal[0] = 0.55
ps.resetGoalConfigs()
ps.setInitialConfig (q_init)
ps.addGoalConfig (q_goal)
# Solve the planning problem :
t = ps.solve()
print("Guide planning done in "+str(t))
pidMiddle = ps.numberPaths()-1
ps.solve()
ps.resetGoalConfigs()
ps.setInitialConfig(q11)
ps.addGoalConfig(q12)
ps.solve()
ps.resetGoalConfigs()
ps.setInitialConfig(q1)
ps.addGoalConfig(q12)
ps.solve()
# 11
cl.problem.pathLength(11)
len(ps.getWaypoints(11))
ps.addPathOptimizer("Prune")
ps.optimizePath(11)
ps.numberPaths()
ps.pathLength(ps.numberPaths() - 1)
len(ps.getWaypoints(ps.numberPaths() - 1))
ps.clearPathOptimizers()
ps.addPathOptimizer("GradientBased")
ps.optimizePath(11)
ps.numberPaths()
ps.pathLength(ps.numberPaths() - 1)
tGB = cl.problem.getTimeGB()
timeValuesGB = cl.problem.getTimeValues()
gainValuesGB = cl.problem.getGainValues()
newGainValuesGB = ((1 - np.array(gainValuesGB)) *
100).tolist() #percentage of initial length-value
ps.clearPathOptimizers()
q1 = cl.robot.projectOnObstacle (q11, 0.001); q2 = cl.robot.projectOnObstacle (q22, 0.001)
robot.isConfigValid(q1); robot.isConfigValid(q2)
r(q1)
ps.setInitialConfig (q1); ps.addGoalConfig (q2)
offsetOrientedPath = 2 # If remove oriented path computation in ProblemSolver, set to 1 instead of 2
#plotFrame (r, 'frame_group', [0,0,0], 0.6)
# First parabolas: vmax = 7m/s, mu = 1.2
cl.problem.setFrictionCoef(1.2); cl.problem.setMaxVelocityLim(7)
ps.clearRoadmap();
solveTime = ps.solve () # 299 nodes
pathId = ps.numberPaths()-offsetOrientedPath # path without orientation stuff
pathSamples = plotSampleSubPath (cl, r, pathId, 70, "path0", [0,0,1,1])
plotCone (q1, cl, r, "cone_first", "friction_cone_SG2"); plotCone (q2, cl, r, "cone_second", "friction_cone_SG2")
plotConeWaypoints (cl, pathId, r, "cone_wp_group", "friction_cone_WP2")
gui.writeNodeFile('cone_wp_group','cones_path.dae')
gui.writeNodeFile('cone_first','cone_start.dae')
gui.writeNodeFile('cone_second','cone_goal.dae')
# Second parabolas: vmax = 6.5m/s, mu = 0.5 # DO NOT SOLVE FIRST PATH BEFORE
cl.problem.setFrictionCoef(0.5); cl.problem.setMaxVelocityLim(6.5)
ps.clearRoadmap();
solveTime = ps.solve () # 4216 nodes .... 37848 edges
pathId = ps.numberPaths()-offsetOrientedPath
q8 = [8.5, 0.5, 0, 0.707106781, 0, 0, -0.707106781, 0.0, 0.0, 0.0, 0.0, -1.8, 1.0, -1.0, -0.85, 0.0, -0.65, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, -1.9, 0.0, -0.6, -0.3, 0.7, -0.4, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, 0.1, -0.15, -0.1, 0.3, -0.418879, 0.0, 0.0, 0.3, -0.8, 0.3, 0.0, 0.0]
q9 = [8, 0, 0, 1, 0, 0, 0, 0.0, 0.0, 0.0, 0.0, -1.8, 1.0, -1.0, -0.85, 0.0, -0.65, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, -1.9, 0.0, -0.6, -0.3, 0.7, -0.4, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, 0.1, -0.15, -0.1, 0.3, -0.418879, 0.0, 0.0, 0.3, -0.8, 0.3, 0.0, 0.0]
ps.setInitialConfig (q1); ps.addGoalConfig (q2); ps.solve (); ps.resetGoalConfigs ()
ps.setInitialConfig (q2); ps.addGoalConfig (q3); ps.solve (); ps.resetGoalConfigs ()
ps.setInitialConfig (q3); ps.addGoalConfig (q4); ps.solve (); ps.resetGoalConfigs ()
ps.setInitialConfig (q4); ps.addGoalConfig (q5); ps.solve (); ps.resetGoalConfigs ()
ps.setInitialConfig (q5); ps.addGoalConfig (q6); ps.solve (); ps.resetGoalConfigs ()
ps.setInitialConfig (q6); ps.addGoalConfig (q7); ps.solve (); ps.resetGoalConfigs ()
ps.setInitialConfig (q7); ps.addGoalConfig (q8); ps.solve (); ps.resetGoalConfigs ()
ps.setInitialConfig (q8); ps.addGoalConfig (q9); ps.solve (); ps.resetGoalConfigs ()
ps.pathLength(ps.numberPaths()-1)
ps.addPathOptimizer('RandomShortcut')
ps.optimizePath (ps.numberPaths()-1)
ps.pathLength(ps.numberPaths()-1)
ps.clearPathOptimizers()
ps.addPathOptimizer("GradientBased")
ps.optimizePath (0)
ps.numberPaths()
ps.pathLength(ps.numberPaths()-1)
pp(ps.numberPaths()-1)
qf = [1, -3, 3, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.2, 1.0, -0.4, -1.0, 0.0, -0.2, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, -1.5, -0.2, 0.1, -0.3, 0.1, 0.1, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, -0.2, 0.6, -0.453786, 0.872665, -0.418879, 0.2, -0.4, 0.0, -0.453786, 0.1, 0.7, 0.0]
