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]