## 3D viewer tools ## #gui.removeFromGroup("path0",r.sceneName) #gui.getNodeList() #gui.createGroup ('sphere_wp0_group') #gui.getGroupNodeList ('sphere_wp0_group') ## IMPORT SCENE AND CONFIGS TO BLENDER ## # I skip the "collada generation part" since I already have them from blender ... blender/urdf_to_blender.py -p /local/mcampana/devel/hpp/videos/ -i /local/mcampana/devel/hpp/src/animals_description/urdf/sphere_mesh.urdf -o robot_blend.py # generate robot loadable by Blender from viewer_display_library import pathToYamlFile, writeEdgeSamples, writePathSamples, writeSkipList len(np.arange(0, ps.pathLength(pathId), 0.02)) pathToYamlFile (cl, r, "frames.yaml ", "robot/base_link", pathId, q2, 0.02) ps.numberNodes() # Plot cones and edges in viewer plotConesRoadmap (cl, r, 'cone_rm_group', "friction_cone2") plotEdgesRoadmap (cl, r, 'edgeGroup', 70, [0,1,0.2,1]) gui.writeNodeFile('cone_rm_group','cones_RM.dae') ## Write EDGES in a file, which will be parsed by a Blender-Python script writeEdgeSamples (cl, 'edges.txt', 70) ## Write PATH samples in a file, which will be parsed by a Blender-Python script
nPointsPlot = 50 offsetOrientedPath = 2 # If remove oriented path computation in ProblemSolver, set to 1 instead of 2 plotFrame (r, "_", [0,0,1.5], 0.5) # First parabolas: vmax = 8m/s, mu = 1.2 cl.problem.setFrictionCoef(1.2); cl.problem.setMaxVelocityLim(4.5) plotCone (q1, cl, r, "cone2", "friction_cone2"); plotCone (q2, cl, r, "cone22", "friction_cone2") ps.clearRoadmap(); solveTime = ps.solve () # 0.312 s pahtId = ps.numberPaths()-offsetOrientedPath # path without orientation stuff samples = plotSampleSubPath (cl, r, pahtId, nPointsPlot, "path0", [0,0,1,1]) plotConeWaypoints (cl, pahtId, r, "wp0", "friction_cone2") plotSpheresWaypoints (cl, pahtId, r, "sphere_wp0", [0,0,1,1], 0.02) print "solve duration: " + str(solveTime) print "path length: " + str(ps.pathLength(pahtId)) print "number of waypoints: " + str(len(ps.getWaypoints (pahtId))) print "number of nodes: " + str(ps.numberNodes ()) cl.problem.getResultValues() # Second parabolas: vmax = 6.5m/s, mu = 0.5 plotCone (q1, cl, r, "cone1", "friction_cone"); plotCone (q2, cl, r, "cone12", "friction_cone") cl.problem.setFrictionCoef(0.5); cl.problem.setMaxVelocityLim(4.5) ps.clearRoadmap(); solveTime = ps.solve () # 0.9689 s pahtId = ps.numberPaths()-offsetOrientedPath samples = plotSampleSubPath (cl, r, pahtId, nPointsPlot, "path2", [1,0,0,1]) plotConeWaypoints (cl, pahtId, r, "wp2", "friction_cone") plotSpheresWaypoints (cl, pahtId, r, "sphere_wp2", [1,0,0,1], 0.02) print "solve duration: " + str(solveTime) print "path length: " + str(ps.pathLength(pahtId))
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.configAtParam(2,0.5)
plotSphere (q2, cl, r, "sphere_q2", [0,1,0,1], 0.02) # same as robot nPointsPlot = 50 offsetOrientedPath = 2 # If remove oriented path computation in ProblemSolver, set to 1 instead of 2 plotFrame (r, "_", [0,0,2.8], 0.5) # First parabola(s): vmax = 6.8m/s, mu = 1.2 cl.problem.setFrictionCoef(1.2); cl.problem.setMaxVelocityLim(6.8) plotCone (q1, cl, r, "cone2", "friction_cone2"); plotCone (q2, cl, r, "cone22", "friction_cone2") ps.clearRoadmap(); solveTime = ps.solve () # 0.085 s pahtId = ps.numberPaths()-offsetOrientedPath # path without orientation stuff samples = plotSampleSubPath (cl, r, pahtId, nPointsPlot, "path0", [0,0,1,1]) plotConeWaypoints (cl, pahtId, r, "wp0", "friction_cone2") plotSpheresWaypoints (cl, pahtId, r, "sphere_wp0", [0,0,1,1], 0.02) print "solve duration: " + str(solveTime) print "path length: " + str(ps.pathLength(pahtId)) print "number of waypoints: " + str(len(ps.getWaypoints (pahtId))) print "number of nodes: " + str(ps.numberNodes ()) cl.problem.getResultValues () # Second parabola(s): vmax = 5.3m/s, mu = 0.5 plotCone (q1, cl, r, "cone1", "friction_cone"); plotCone (q2, cl, r, "cone12", "friction_cone") cl.problem.setFrictionCoef(0.5); cl.problem.setMaxVelocityLim(5.3) ps.clearRoadmap(); solveTime = ps.solve () # 0.738 s pahtId = ps.numberPaths()-offsetOrientedPath samples = plotSampleSubPath (cl, r, pahtId, nPointsPlot, "path2", [0.2,0.8,0.2,1]) plotConeWaypoints (cl, pahtId, r, "wp2", "friction_cone") plotSpheresWaypoints (cl, pahtId, r, "sphere_wp2", [0.2,0.8,0.2,1], 0.02) print "solve duration: " + str(solveTime)
q2 = [6.55, -2.91, 1.605, 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] q2hard = [7.60, -2.41, 0.545, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.8, 0.0, -0.4, -0.55, 0.0, -0.6, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, -2.8, 0.0, 0.1, -0.2, -0.1, 0.4, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, -0.2, 0.6, -0.1, 1.2, -0.4, 0.2, -0.3, 0.0, -0.4, 0.2, 0.7, 0.0] robot.isConfigValid(q1) robot.isConfigValid(q2) # qf should be invalid 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))
q2 = [4, -4, 1, 0] # obstS 1 #q1 = [-4, 4, 0]; q2 = [4, -4, 0] # obstS 1 ps.setInitialConfig(q1) 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.setInitialConfig (q1); ps.addGoalConfig (q2) ps.solve () # PROBLEM !! not finding solution for environment_3d_window with mu=0.5 V0max=6.5 Projectionshooter .... V0 or Vimp too much limiting ?? 'cause V0max=7 gives a too "easy" solution ... samples = plotSampleSubPath (cl, r, 0, 20, "curvy", [0,0.8,0.2,1]) plotConeWaypoints (cl, 0, r, "wp", "friction_cone") #ps.saveRoadmap ('/local/mcampana/devel/hpp/data/PARAB_envir3d_with_window.rdm') r.client.gui.setVisibility('robot/l_bounding_sphere',"OFF") samples = plotSampleSubPath (cl, r, 0, 20, "curvy", [0,0.8,0.2,1]) r(ps.configAtParam(0,0.001)) ps.pathLength(0) ps.getWaypoints (0) ## 3D Plot tools ## q0 = [0, 0, 5, 0, 0, 0, 1, 0, 0, 1, 0]; r(q0) plotFrame (r, "_", [0,0,4], 0.5) plotPath (cl, 0, r, "pathy", 0.1) plotThetaPlane (q1, q2, r, "ThetaPlane2") plotCone (q1, cl, r, 0.5, 0.4, "c1")
""" 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) """ from hpp.gepetto import Viewer, PathPlayer
#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') f.write('number of waypoints: '+str(pathNumberWaypoints)+'\n')
class Agent(Client): robot = None platform = None index = 0 ps = None # we load other agents as ghosts to reduce the computation time while planning ghosts = [] ghost_urdf = '' ghost_package = '' # to avoid confusion, we use start and end instead of init and goal start_config = [] end_config = [] current_config = [] # this is not used for now permitted_plan = [] # this is the plan generated repeat = 0 # to help the selectProblem function # an agent should have a robot and the start and end configuration # to avoid confusion, we use start_config instead of init_config and # end_confi instead of goal_config def __init__(self, robot, start, end): Client.__init__(self) self.repeat = 0 # print 'creating an agent of type ', robotType self.robot = robot self.start_config = start self.end_config = end self.current_config = self.start_config self.__plan_proposed = [] # once all agents are generated, we may register the agents to a platform def registerPlatform(self, platform, index): self.platform = platform self.index = index # this function gives some information about the agent and robot it is managing def printInformation(self): print '-------------------------------------------' print 'name of the robot:\t', self.robot.name print 'configuration size:\t', self.robot.getConfigSize() print 'degree of freedom:\t', self.robot.getNumberDof() print 'mass of the robot:\t', self.robot.getMass() print 'the center of mass:\t', self.robot.getCenterOfMass() config = self.robot.getCurrentConfig() nm = self.robot.getJointNames() print 'there are ', len(nm), 'joint names in total. They are:' for i in range(len(nm)): lower = self.robot.getJointBounds(nm[i])[0] upper = self.robot.getJointBounds(nm[i])[1] print 'joint name: ', nm[ i], '\trank in configuration:', self.robot.rankInConfiguration[ nm[i]], print '\tlower bound: {0:.3f}'.format( lower), '\tupper bound: {0:.3f}'.format(upper) # set up the environment def setEnvironment(self): if self.platform.env != None: self.ps.loadObstacleFromUrdf(self.platform.env.packageName, self.platform.env.urdfName, self.platform.env.name) # self.ps.moveObstacle('airbase_link_0', [0,0, -3, 1,0,0,0]) # load the other agents to the problem solver def loadOtherAgents(self): # print 'There are ', len(self.platform.agents), 'agents' #load ghost agents for a in self.platform.agents: if (a.index != self.index): # if it is not itself then load a ghost agent g = Ghost() self.ps.loadObstacleFromUrdf( g.packageName, g.urdfName, a.robot.name) # it's the robot's name!!! # and then place it at the initial location of the agent # print self.robot.name, ' is now loading ', a.robot.name, ' as a ghost' config = a.current_config spec = self.getMoveSpecification(config) spec[2] = 0.3 self.obstacle.moveObstacle(a.robot.name + 'base_link_0', spec) # load agents from the node def loadOtherAgentsFromNode(self, node): print 'There are ', len(self.platform.agents), 'agents' #load ghost agents for a in self.platform.agents: if (a.index != self.index): # if it is not itself then load a ghost agent g = Ghost() self.ps.loadObstacleFromUrdf( g.packageName, g.urdfName, a.robot.name) # it's the robot's name!!! # and then place it at the initial location of the agent config = node.getAgentCurrentConfig(a.index) spec = self.getMoveSpecification(config) self.obstacle.moveObstacle(a.robot.name + 'base_link_0', spec) print self.robot.name, ' is now loading ', a.robot.name, ' as a ghost', 'it is at ', spec[ 0], spec[1] # note that the default solver does not consider the position of other agents def startDefaultSolver(self): self.repeat += 1 name = self.robot.name self.problem.selectProblem(str(self.index) + ' ' + str(self.repeat)) self.robot = HyQ(name) self.ps = ProblemSolver(self.robot) self.ps.setInitialConfig(self.start_config) self.ps.addGoalConfig(self.end_config) self.ps.selectPathPlanner("VisibilityPrmPlanner") self.ps.addPathOptimizer("RandomShortcut") # initialise a solver from a node, the node contains information about other agents and itself # this method is used when proposing plans while interacting with platform for MAS path planning def startNodeSolver(self, node): self.repeat += 1 name = self.robot.name self.problem.selectProblem(str(self.index) + ' ' + str(self.repeat)) self.robot = HyQ(name) self.ps = ProblemSolver(self.robot) cfg = node.getAgentCurrentConfig(self.index) print 'this iteration, the agent', name, 'starts from ', cfg[0], cfg[1] self.ps.setInitialConfig(cfg) self.ps.addGoalConfig(self.end_config) self.ps.selectPathPlanner("VisibilityPrmPlanner") self.ps.addPathOptimizer("RandomShortcut") # this is only used when the agent takes too long (30 seconds) while planning def terminate_solving(self): self.problem.interruptPathPlanning() # the solve method for problem solver but with a time bound def solve(self): # try catch ------------------- try: t = Timer(30.0, self.terminate_solving) t.start() print 'solved: ', self.ps.solve() t.cancel() except Error as e: print e.msg print '***************\nfailed to plan within limited time\n**************' return -1 # self.repeat += 1 # store the path for two reasons: # 1. store as a default plan # 2. to continue the path def storePath(self, choice=0, segments=8): # always store the first one for now self.__plan_proposed = [] for p in range(int(round(segments * self.ps.pathLength(choice)))): self.__plan_proposed.append( self.ps.configAtParam(choice, p * 1.0 / segments)) # the last configuration is the goal configuration if self.ps.configAtParam( choice, self.ps.pathLength(choice)) == self.end_config: self.__plan_proposed.append(self.end_config) print 'stored; plan length: ', len(self.__plan_proposed) # this is hard colded for now for the airplane example, we should introduce an entry for the environment # class about it. def setBounds(self): self.robot.setJointBounds("base_joint_xy", [-35, 10, -2.6, 4.3]) #the rest are just some helping functions def getConfigOfProposedPlanAtTime(self, index): return self.__plan_proposed[index] def getConfigOfPermittedPlanAtTime(self, index): return self.permitted_plan[index] def getProposedPlanLength(self): return len(self.__plan_proposed) def setPermittedPlan(self, plan): self.permitted_plan = plan def getPermittedPlanLength(self): return len(self.permitted_plan) # export the permitted plan to a specific file in the format # agent X # config 1 # config 2 # etc def exportPermittedPlan(self, filename): f = open(filename, 'a+') f.write('agent ' + str(self.index) + '\n') for p in self.permitted_plan: f.write(str(p)[1:-1] + '\n') f.close() # for the sake of manipulation, we return a copy of it def obtainPermittedPlan(self): return copy.copy(self.permitted_plan) # we will get only a copy of it, not the original one # to remind the difference, we use 'obtain' instead of 'get' def obtainProposedPlan(self): return copy.copy( self.__plan_proposed ) #for some reason, sometimes the value would maybe changed??? # to transfer the specification from 2D to 3D def getMoveSpecification(self, config): x = config[0] y = config[1] th = atan2(config[3], config[2]) # print 'sin = ', self.init_config[3], ' cos = ', self.init_config[2], ' th = ', th return [x, y, 0, cos(th / 2), 0, 0, sin(th / 2)] # the function to compute a plan, exceptions are not handled in this simple demo def computePlan(self, node): self.startNodeSolver(node) self.setBounds() self.setEnvironment() self.loadOtherAgentsFromNode(node) if self.solve() != -1: self.storePath() else: self.__plan_proposed = self.__plan_proposed[node.progress_time::] [node.getAgentCurrentConfig(self.index)] print 'take the previous one and continue the searching' return -1
#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') f.write('number of waypoints: ' + str(pathNumberWaypoints) + '\n')
ps.solve (); ps.resetGoalConfigs () q9 = [0, 0, 0, 1, 0, 0, 0] ps.setInitialConfig (q8); ps.addGoalConfig (q9) ps.solve (); ps.resetGoalConfigs () """ # Only path to do a useless turn ! (Not the case if solved separately) # r( ps.configAtParam(7,2.17) ) = [0.0, 0.0, 0.0, -0.9999559023922103, 0.0, 0.0, -0.00939112724262876] ps.setInitialConfig (q1); ps.addGoalConfig (q5) # do a turn as expected ps.solve () ps.optimizePath (4) # reduced to a point as expected len(ps.getWaypoints (0)) ps.pathLength(5) # = 0 ##############################" # TEST quaternions x0 HRP2 + optimisation = meme resultat (-1) (un ou 2 tours)? # Oui mais il faut imposer alpha = alpha_init et ne pas MàJ H1_ # les valeurs propres de la Hessienne sont bien >0 q1 = [0, 0, 0, 0.9999028455952614, -0.00643910090823595, -0.012362740316661774, 1.3620998722148461e-06] q2 = [0, 0, 0, 0.8258496711518952, -0.2042733013060623, -0.19724860126354768, -0.4871732015735299] ps.setInitialConfig (q1); ps.addGoalConfig (q2) ps.solve (); ps.resetGoalConfigs () q3 = [0, 0, 0, 0.6659085913630002, -0.04107471639409278, 0.06948325706470262, -0.7416540248726288] ps.setInitialConfig (q2); ps.addGoalConfig (q3) ps.solve (); ps.resetGoalConfigs ()
cl.obstacle.loadObstacleModel('robot_2d_description','cylinder_obstacle','') # q = [x, y] # limits in URDF file q1 = [-2, 0]; q2 = [-0.2, 2]; q3 = [0.2, 2]; q4 = [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 (q1); ps.addGoalConfig (q4); ps.solve (); ps.resetGoalConfigs () # pp(3) = p0 final #ps.addPathOptimizer("GradientBased") #ps.addPathOptimizer("Prune") ps.addPathOptimizer("PartialRandomShortcut") ps.optimizePath(3) # pp(4) = p1 final ps.pathLength(3) ps.pathLength(4) ps.getWaypoints (3) ps.getWaypoints (4) # should be [-0.07 0] [0.07 0] if alpha_init=1 """ q1 = [-2, 0]; q2 = [-1, 1] ps.setInitialConfig (q1); ps.addGoalConfig (q2); ps.solve () ps.resetGoalConfigs () q1 = [-1, 1]; q2 = [-1.2, 1.8] ps.setInitialConfig (q1); ps.addGoalConfig (q2); ps.solve () ps.resetGoalConfigs () q1 = [-1.2, 1.8]; q2 = [-0.2, 1.2] ps.setInitialConfig (q1); ps.addGoalConfig (q2); ps.solve ()
r.loadObstacleModel ("puzzle_description","decor_very_easy","decor_very_easy") r(q1) ps.setInitialConfig (q1); ps.addGoalConfig (q2) # Load box obstacle in HPP for collision avoidance cl.obstacle.loadObstacleModel('puzzle_description','decor_very_easy','') #cl.obstacle.loadObstacleModel('puzzle_description','decor_easy','') #cl.obstacle.loadObstacleModel('puzzle_description','decor','') ps.selectPathPlanner ("VisibilityPrmPlanner") # 26min solve time begin=time.time() ps.solve () end=time.time() print "Solving time: "+str(end-begin) ps.pathLength(0) ps.addPathOptimizer("GradientBased") begin=time.time() ps.optimizePath (0) end=time.time() print "Optim time: "+str(end-begin) cl.problem.getIterationNumber () ps.pathLength(1) #vPRM: 24 iter, 25.3s->21s, weighted Cost + comp linear error #RRTc: 30 iter, 33.4s->14.9s, weighted Cost + comp linear error begin=time.time() ps.optimizePath (1) end=time.time() print "Optim time: "+str(end-begin)
#ps.selectPathPlanner ("VisibilityPrmPlanner") begin=time.time() ps.solve () end=time.time() print "Solving time: "+str(end-begin) ps.addPathOptimizer("GradientBased") begin=time.time() ps.optimizePath(0) end=time.time() print "Optim time: "+str(end-begin) cl = robot.client cl.problem.getIterationNumber () cl.problem.getComputationTime () ps.pathLength(0) ps.pathLength(1) begin=time.time() ps.optimizePath(1) end=time.time() print "Optim2 time: "+str(end-begin) cl.problem.getIterationNumber () ps.pathLength(0) ps.pathLength(1) ps.pathLength(2) len(ps.getWaypoints (0)) ps.optimizePath(3)
ps.setInitialConfig(q7) ps.addGoalConfig(q8) ps.solve() ps.resetGoalConfigs() 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)