else: raise RuntimeError ("Failed to apply constraint.") if not(robot.isConfigValid(q1proj)): raise RuntimeError ("Projected config non valid.") res = ps.applyConstraints (q2) if res [0]: q2proj = res [1] else: raise RuntimeError ("Failed to apply constraint.") if not(robot.isConfigValid(q2proj)): raise RuntimeError ("Projected config non valid.") ps.setInitialConfig (q1proj); ps.addGoalConfig (q2proj) ps.loadObstacleFromUrdf ("iai_maps", "kitchen_area", "") # environment #RRT-connect: path of 36s with 68 waypoints (2600 var to optimize) #ps.selectPathPlanner ("VisibilityPrmPlanner") ps.solve () ps.optimizePath(0) pp = PathPlayer (robot.client, r) pp (0) pp (1) len(ps.getWaypoints (0))
from hpp.gepetto import ViewerFactory, PathPlayer robot = Robot('ur5') ps = ProblemSolver(robot) vf = ViewerFactory(ps) vf.loadObstacleModel("hpp_practicals", "ur_benchmark/obstacles", "obstacles") vf.loadObstacleModel("hpp_practicals", "ur_benchmark/table", "table") vf.loadObstacleModel("hpp_practicals", "ur_benchmark/wall", "wall") v = vf.createViewer() q1 = [0, -1.57, 1.57, 0, 0, 0] q2 = [0.2, -1.57, -1.8, 0, 0.8, 0] q3 = [1.57, -1.57, -1.8, 0, 0.8, 0] v(q2) v(q3) ps.setInitialConfig(q2) ps.addGoalConfig(q3) from motion_planner import MotionPlanner m = MotionPlanner(robot, ps) pathId = m.solveBiRRT(maxIter=1000) pp = PathPlayer(v) #pp (pathId)
q_goal [rank] = -0.5 rank = robot.rankInConfiguration ['r_shoulder_lift_joint'] q_goal [rank] = 0.5 rank = robot.rankInConfiguration ['r_elbow_flex_joint'] q_goal [rank] = -0.5 r (q_goal) ## CONSTRAINTS ## # "torso_lift_joint" : do not constraint z axis because different for q_init and q_goal # constraint torso to be at position [-3.25, -4.0, 0.790675] in workspace frame #ps.createPositionConstraint ("posConstraint", "torso_lift_joint", "", [0,0,0], [-3.25, -4.0, 0.790675], [1,1,0]) #ps.setNumericalConstraints ("constraints", ["posConstraint"]) ps.loadObstacleFromUrdf ("iai_maps", "kitchen_area", "") # environment ps.setInitialConfig (q_init); ps.addGoalConfig (q_goal) #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 ()
qt = [0, 0, 0, math.sqrt (1/3.), math.sqrt (1/3.), 0.0, math.sqrt (1/3.)] qt2 = [0, 0, 0, math.sqrt (1 - 0.95**2), 0.95, 0.0, 0.0] Q.append (qt) Q.append (qt2) Q.append ( [0, 0, 0, 0.99, 0.14003571, 0.013, 0.011]) # to remove 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)
q_init = robot.getCurrentConfig () q_goal = q_init [::] q_init [0:2] = [-3.2, -4] rank = robot.rankInConfiguration ['torso_lift_joint'] q_init [rank] = 0.2 r (q_init) q_goal [0:2] = [-3.2, -4] rank = robot.rankInConfiguration ['l_shoulder_lift_joint'] q_goal [rank] = 0.5 rank = robot.rankInConfiguration ['l_elbow_flex_joint'] q_goal [rank] = -0.5 rank = robot.rankInConfiguration ['r_shoulder_lift_joint'] q_goal [rank] = 0.5 rank = robot.rankInConfiguration ['r_elbow_flex_joint'] q_goal [rank] = -0.5 r (q_goal) ps.loadObstacleFromUrdf ("iai_maps", "kitchen_area", "") ps.setInitialConfig (q_init) ps.addGoalConfig (q_goal) ps.selectPathPlanner ("PRM") ps.solve () from hpp_ros import PathPlayer pp = PathPlayer (robot.client, r) pp (0) pp (1)
q2=[0,-2.5,0.64870180180254433111, 0.7101853756232854, 0, 0, -0.7040147244559684, 0,0,0,0,0.26179900000000000393, 0.1745299999999999907,0,-0.52359900000000003661,0,0,0.1745319999999999927, -0.1745319999999999927,0.1745319999999999927,-0.1745319999999999927, 0.1745319999999999927,-0.1745319999999999927,0.26179900000000000393, -0.1745299999999999907,0,-0.52359900000000003661,0,0,0.1745319999999999927, -0.1745319999999999927,0.1745319999999999927,-0.1745319999999999927, 0.1745319999999999927,-0.1745319999999999927,0,0,-0.45378600000000002268, 0.87266500000000002402,-0.41887900000000000134,0,0,0,-0.45378600000000002268, 0.87266500000000002402,-0.41887900000000000134,0] #q2=[0,-2.5,0.64870180180254433111, 1,0,0,0, 0,0,0,0,0.26179900000000000393, 0.1745299999999999907,0,-0.52359900000000003661,0,0,0.1745319999999999927, -0.1745319999999999927,0.1745319999999999927,-0.1745319999999999927, 0.1745319999999999927,-0.1745319999999999927,0.26179900000000000393, -0.1745299999999999907,0,-0.52359900000000003661,0,0,0.1745319999999999927, -0.1745319999999999927,0.1745319999999999927,-0.1745319999999999927, 0.1745319999999999927,-0.1745319999999999927,0,0,-0.45378600000000002268, 0.87266500000000002402,-0.41887900000000000134,0,0,0,-0.45378600000000002268, 0.87266500000000002402,-0.41887900000000000134,0] publisher = ScenePublisher(robot) publisher(q2) time.sleep(1) publisher(q1) ## Add constraints solver = ProblemSolver (robot) print "diffusing planner (general RRT implementation)" solver.selectPathPlanner("DiffusingPlanner") solver.loadObstacleFromUrdf("hpp_ros","wall") solver.setInitialConfig (q1) solver.addGoalConfig (q2) #pc.invokeIrreduciblePlanner() #mpc = MPClient() #pc = mpc.precomputation #cnames = pc.getNumericalConstraints (robot.getInitialConfig()) #solver.setNumericalConstraints ("stability-constraints", cnames) ## add obstacles print "solve" start_time = float(time.time()) solver.solve () end_time = float(time.time()) seconds = end_time - start_time hours = seconds/3600 print("--- %s seconds ---" % seconds) print("--- %s hours ---" % hours)
if res [0]: q2proj = res [1] else: raise RuntimeError ("Failed to apply constraint.") ps.selectPathValidation ("Progressive", 0.025) import datetime as dt totalTime = dt.timedelta (0) totalNumberNodes = 0 N = 20 for i in range (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 (N))) print ("Average number nodes: " + str (totalNumberNodes/float (N))) from hpp.gepetto import ViewerFactory vf = ViewerFactory (ps) r = vf.createViewer()
ps = ProblemSolver (robot) cl = robot.client 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 (); #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")
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] q4 = [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] # Load obstacles in HPP cl.obstacle.loadObstacleModel('gravity_description','gravity_decor','') # do not move (position in urdf) cl.obstacle.loadObstacleModel('gravity_description','emu','') # loaded as an obstacle for now position_emu= (0,-1,0.2,1,0,0,0) cl.obstacle.moveObstacle ('emu_base', position_emu) # Problem ps.setInitialConfig (q3) ps.addGoalConfig (q2) # q4 or q2 begin=time.time() ps.solve () end=time.time() print "Solving time: "+str(end-begin) # Display spaceship environment and Emu guy r.addObject('spaceShip','obstacle_base') # display r.moveObject('spaceShip',(0,0,0,1,0,0,0)) r.addObject('emu','emu_base') # display position_emu= (0,-1,0.2,1,0,0,0) r.moveObject('emu',position_emu) r(q3) # Nodes from the roadmap
q_goal = q_init [::] q_init [0:2] = [-3.7, -4]; vf (q_init) q_goal [0:2] = [15,2] vf (q_goal) q = q_init[:] q[0:2] = [2,0] vf(q) #~ ps.loadObstacleFromUrdf ("iai_maps", "kitchen_area", "") ps.setInitialConfig (q_init) # ps.addGoalConfig (q_goal) ps.addGoalConfig (q) ps.selectPathValidation ("Discretized", 0.02) ps.selectSteeringMethod ("Quadcopter") ps.client.problem.selectConFigurationShooter("Quadcopter") # ps.addPathOptimizer ("RandomShortcut") # t = ps.solve () # print ("solving time", t) # gui = vf.createViewer() # gui.client.gui.setWireFrameMode("scene", "WIREFRAME") # pp = PathPlayer (robot.client, gui)
# Load obstacles in HPP cl.obstacle.loadObstacleModel('gravity_description','gravity_decor','') # do not move (position in urdf) cl.obstacle.loadObstacleModel('gravity_description','emu','') # loaded as an obstacle for now position_emu= (0,-1,0.2,1,0,0,0) cl.obstacle.moveObstacle ('emu_base', position_emu) q1 = [-2, 0.01, 0.705, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -1.8, 0.8, -1.0, -1.0, 0.0, 0.2, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, 0.7, -0.8, 0.1, -0.523599, 0.1, -0.3, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, 0.6, 0.1, -0.453786, 0.872665, -0.418879, 0.2, -0.4, 0.0, -0.453786, 0.1, 0.7, 0.0] q2 = [2, 0.01, 0.705, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -1.8, 0.8, -1.0, -1.0, 0.0, 0.2, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, 0.7, -0.8, 0.1, -0.523599, 0.1, -0.3, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, 0.6, 0.1, -0.453786, 0.872665, -0.418879, 0.2, -0.4, 0.0, -0.453786, 0.1, 0.7, 0.0] qconf = [0, 0.01, 0.705, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -1.8, 0.8, -1.0, -1.0, 0.0, 0.2, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, 0.7, -0.8, 0.1, -0.523599, 0.1, -0.3, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, 0.6, 0.1, -0.453786, 0.872665, -0.418879, 0.2, -0.4, 0.0, -0.453786, 0.1, 0.7, 0.0] # Problem ps.setInitialConfig (q3) ps.addGoalConfig (q2) ps.solve () # 0-1 ps.setInitialConfig (q2) ps.resetGoalConfigs () ps.addGoalConfig (q2to4) ps.solve () # 2-3 ps.setInitialConfig (q2to4) cl.problem.resetGoalConfigs () ps.addGoalConfig (q4) ps.solve () # 4-5 p(1) #to play the solution begin=time.time() ps.solve () end=time.time() print "Solving time: "+str(end-begin)
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
from hpp.corbaserver import Client import time robot = Robot ('puzzle_robot') # object5 robot.setJointBounds('base_joint_xyz', [-0.1, 0.1, -0.1, 0.1, -0.1, 0.1]) ps = ProblemSolver (robot) r = Viewer (ps) cl = robot.client pp = PathPlayer (cl, r) # Patchwork of path q1 = [0, 0, 0, 1, 0, 0, 0] q2 = [0, 0, 0, 0.707106781, 0, 0, 0.707106781] # equivalent to : [0, 0, 0, -0.707106781, 0, 0, -0.707106781] q7 ps.setInitialConfig (q1); ps.addGoalConfig (q2) ps.solve (); ps.resetGoalConfigs () q3 = [0, 0, 0, 0, 0, 0, 1] # equivalent to : [0, 0, 0, 0, 0, 0, -1] q8 ps.setInitialConfig (q2); ps.addGoalConfig (q3) ps.solve (); ps.resetGoalConfigs () q4 = [0, 0, 0, -0.707106781, 0, 0, 0.707106781] # equivalent to : [0, 0, 0, 0.707106781, 0, 0, -0.707106781] q9 ps.setInitialConfig (q3); ps.addGoalConfig (q4) ps.solve (); ps.resetGoalConfigs () q5 = [0, 0, 0, -1, 0, 0, 0] # equivalent to : [0, 0, 0, 1, 0, 0, 0] q1, q10 ps.setInitialConfig (q4); ps.addGoalConfig (q5)
#qG = robot.shootRandomConfig() qI = robot.getInitialConfig() qG = robot.getGoalConfig() #qI = q1 #qG = q2 #solver.loadObstacleFromUrdf("hpp-motion-prior","floor","") solver.loadObstacleFromUrdf("hpp-motion-prior","floor_obstacle","") #solver.loadObstacleFromUrdf("hpp-motion-prior","wall","") ## Add constraints #qI[1]=1.0 solver.setInitialConfig (qI) solver.addGoalConfig (qG) #publisher(q) #print q #sys.exit(0) qqinit = solver.getInitialConfig()[0:3] qqgoal = solver.getGoalConfigs()[0][0:3] print "planning from",qqinit,"to",qqgoal #print "Irreducible Planner" #solver.selectPathPlanner("PRM") #solver.selectPathPlanner("DiffusingPlanner") print "solve" start_time = float(time.time()) solver.selectPathPlanner("IrreduciblePlanner")
robot = Robot('ur5') ps = ProblemSolver(robot) vf = ViewerFactory(ps) vf.loadObstacleModel("hpp_practicals", "ur_benchmark/obstacles", "obstacles") vf.loadObstacleModel("hpp_practicals", "ur_benchmark/table", "table") vf.loadObstacleModel("hpp_practicals", "ur_benchmark/wall", "wall") r = vf.createViewer() q1 = [0, -1.57, 1.57, 0, 0, 0] q2 = [0.2, -1.57, -1.8, 0, 0.8, 0] q3 = [1.57, -1.57, -1.8, 0, 0.8, 0] r(q2) r(q1) ps.setInitialConfig(q2) ps.addGoalConfig(q1) from motion_planner import MotionPlanner m = MotionPlanner(robot, ps) pathId = m.solveBiRRT(maxIter=1000) ps.solve() pp = PathPlayer(robot.client, r) pid = ps.numberPaths() - 1 if pid < 0: raise RuntimeError("No path in vector") pp(pid)
ps = ProblemSolver (robot) cl = robot.client # Configs : [x, y, z, qw, qx, qy, qz, nx, ny, nz, theta] q1 = [0, 0, 0, 1, 0, 0, 0, 0, 0, 1, 0] q2 = [1, 1, 0, 1, 0, 0, 0, 0, 0, 1, 0] from hpp.gepetto import Viewer, PathPlayer r = Viewer (ps) pp = PathPlayer (robot.client, r) #r.loadObstacleModel ("animals_description","cave","cave") #cl.obstacle.loadObstacleModel('animals_description','cave','cave') r(q2) ps.setInitialConfig (q1); ps.addGoalConfig (q2); ps.solve () # verif orientation: r(ps.getWaypoints (0)[0]) # ref r(ps.getWaypoints (1)[0]) # should be good... index = cl.robot.getConfigSize () - 4 q = q1[::] plotStraightLine ([q[index], q[index+1], q[index+2]], q, r, "normale") plotCone (q1, cl, r, 0.5, 0.2, "cone1") plotCone (q2, cl, r, 0.5, 0.2, "cone2") r( ps.configAtParam(0,2) ) ps.pathLength(0) ps.getWaypoints (0)
# Difficult init config q1 = [1.45, 1.05, -0.8, 1.0, 0.0, 0.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] 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)
kRange = 5 robot = Robot ('simple_robot') robot.setJointBounds('base_joint_xy', [-kRange, kRange, -kRange, kRange]) ps = ProblemSolver (robot) cl = robot.client Viewer.withFloor = True r = Viewer (ps) pp = PathPlayer (cl, r) # q = [x, y, z, theta] # (z not considered since planar) q1 = [-4, 4, 1, 0]; q2 = [4, -4, 1, 0] # obstS 1 #q1 = [2.4, -4.6, 1.0, 0.0]; q2 = [-0.4, 4.6, 1.0, 0.0] # obstS 2 ps.setInitialConfig (q1) ps.addGoalConfig (q2) # Load box obstacle in HPP for collision avoidance # #cl.obstacle.loadObstacleModel('potential_description','cylinder_obstacle','') cl.obstacle.loadObstacleModel('potential_description','obstacles_concaves','') r.loadObstacleModel ("potential_description","obstacles_concaves","obstacles_concaves") # in viewer ! ps.createOrientationConstraint ("orConstraint", "base_joint_rz", "", [0.7071067812 ,0,0,0.7071067812], [0,0,1]) # OK T = [[0,0,0,0],[0,0,0,0],[0,0,0,0],[0,0,0,1]] ps.setNumericalConstraints ("constraints", ["orConstraint"]) ps.solve () begin=time.time() cl.problem.optimizePath(0) end=time.time()
balanceConstraints = [constraintName + "/relative-com", constraintName + "/relative-orientation", constraintName + "/relative-position", constraintName + "/orientation-left-foot", constraintName + "/position-left-foot"] # 3}}} ps.setNumericalConstraints ("balance", balanceConstraints); ps.setInitialConfig (q_init) r = Viewer (ps) r(q_init) ### Walk forward if walk_forward: ps.addGoalConfig (q_goal) ps.solve () print "Solved forward" ### Walk sideway q_goal_side = q_init[:] q_goal_side [0:2] = [0, 0.95] if walk_sideway: ps.resetGoalConfigs () ps.addGoalConfig (q_goal_side) ps.solve () print "Solved sideway" ### Walk in oblique direction q_goal_od = q_init[:] q_goal_od [0:2] = [1.05, -0.95]
# q = [x, y] # limits in URDF file q1 = [-7, 3] q2 = [-8, 2] q3 = [-6, 2] q4 = [-7, 1] q5 = [-5, -1] q6 = [-8, -1] q7 = [-4, 1] q8 = [-2, -1] q9 = [0.2, 1.6] q10 = [1, -1] q11 = [4, 1] q12 = [5, -1] 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)
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
from hpp.corbaserver.practicals.ur5 import Robot from hpp.corbaserver import ProblemSolver from hpp.gepetto import ViewerFactory, PathPlayer robot = Robot ('ur5') ps = ProblemSolver (robot) vf = ViewerFactory (ps) vf.loadObstacleModel ("hpp_practicals","ur_benchmark/obstacles","obstacles") vf.loadObstacleModel ("hpp_practicals","ur_benchmark/table","table") vf.loadObstacleModel ("hpp_practicals","ur_benchmark/wall","wall") v = vf.createViewer () q1 = [0, -1.57, 1.57, 0, 0, 0]; q2 = [0.2, -1.57, -1.8, 0, 0.8, 0] q3 = [1.57, -1.57, -1.8, 0, 0.8, 0] v (q2) v (q3) ps.setInitialConfig (q2) ps.addGoalConfig (q3) from motion_planner import MotionPlanner m = MotionPlanner (robot, ps) pathId = m.solveBiRRT (maxIter = 1000) pp = PathPlayer (v) #pp (pathId)
ps = ProblemSolver (robot) cl = robot.client # Configs : [x, y, z, q1, q2, q3, q4, dir.x, dir.y, dir.z, theta] q11 = [6.2, 0.5, 0.5, 0, 0, 0, 1, 0, 0, 1, 0]; q22 = [-4.4, -1.5, 6.5, 0, 0, 0, 1, 0, 0, 1, 0] from hpp.gepetto import Viewer, PathPlayer r = Viewer (ps); gui = r.client.gui pp = PathPlayer (robot.client, r) r.loadObstacleModel ("animals_description","scene_jump_harder","scene_jump_harder") 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")
robot.setJointBounds('base_joint_xyz', [xStone-2, xStone+2, yEmu-1, yEmu+2, zEmu-0.5, zEmu+0.5]) # List of configs q1 = [xStone+2, yEmu+0, zEmu, 1.0, 0.0, 0.0, 0.0] q2 = [xStone+1.5, yEmu+0.8, zEmu, 0.707106781, 0, 0, 0.707106781] q3 = [xStone+1, yEmu+1.4, zEmu, 0, 0, 0, 1] q4 = [xStone+0.5, yEmu+1.7, zEmu, -0.707106781, 0, 0, 0.707106781] #q5 = [xStone+0, yEmu+2, zEmu, -1, 0, 0, 0] q5 = [xStone+0, yEmu+2, zEmu, -0.99, 0.14003571, 0.013, 0.011] q6 = [xStone-0.5, yEmu+1.7, zEmu, -0.707106781, 0, 0, -0.707106781] q7 = [xStone-1, yEmu+1.4, zEmu, 0, 0, 0, -1] 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)
res = ps.applyConstraints(q2) if res[0]: q2proj = res[1] else: raise RuntimeError("Failed to apply constraint.") ps.selectPathOptimizer("None") import datetime as dt totalTime = dt.timedelta(0) totalNumberNodes = 0 for i in range(20): 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) / 20.)) print("Average number nodes: " + str(totalNumberNodes / 20.)) pp = PathPlayer(cl, r) pp(1)
import matplotlib.pyplot as plt sys.path.append('/local/mcampana/devel/hpp/src/test-hpp/script') kRange = 5 robot = Robot('potential') robot.setJointBounds('base_joint_xy', [-kRange, kRange, -kRange, kRange]) ps = ProblemSolver(robot) cl = robot.client # q = [x, y, theta] # (z not considered since planar) q1 = [-4, 4, 1, 0] 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()
print "Test on a circle, alpha = ", alpha q_goal = q_init[::] q_goal[0:3] = [radius * np.sin(alpha), -radius * np.cos(alpha), 1.] # set final orientation to be along the circle : vx = np.matrix([1, 0, 0]).T v_goal = np.matrix([q_goal[0], q_goal[1], 0]).T quat = Quaternion.FromTwoVectors(vx, v_goal) q_goal[3:7] = quat.coeffs().T.tolist()[0] # set final velocity to fix the orientation : q_goal[-6] = vGoal * np.sin(alpha) q_goal[-5] = -vGoal * np.cos(alpha) v(q_goal) print "initial root position : ", q_init print "final root position : ", q_goal ps.setInitialConfig(q_init) ps.addGoalConfig(q_goal) # write problem in files : f = open(statusFilename, "w") f.write("q_init= " + str(q_init) + "\n") f.write("q_goal= " + str(q_goal) + "\n") f.close() # 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")
#cl.obstacle.loadObstacleModel('robot_2d_description','environment_2d','') #cl.obstacle.loadObstacleModel('robot_2d_description','environment_2d_harder','') from hpp.gepetto import Viewer, PathPlayer r = Viewer (ps) pp = PathPlayer (cl, r) #r.loadObstacleModel ("robot_2d_description","environment_2d","environment_2d") r.loadObstacleModel ("robot_2d_description","environment_2d_harder","environment_2d_harder") #r.loadObstacleModel ("iai_maps", "labyrinth", "labyrinth") #r.loadObstacleModel ("iai_maps", "labyrinth2", "labyrinth2") # q = [x, y, dir.x, dir.y] # limits in URDF file #q1 = [-1.3, 0.2]; q2 = [0.2, 0.2]; q3 = [5, -2.8]; q4 = [9, 2.2]; q5 = [12.4, -5.8]; #q1 = [-1.3, 0.2, 0]; q2 = [0.2, 0.2, 0]; q3 = [5, -2.8, 0]; q4 = [9, 2.2, 0]; q5 = [12.4, -5.8, 0]; q1 = [-1.3, 0.2, 0, 1]; q5 = [12.4, -5.8, 0, 1]; ps.setInitialConfig (q1); ps.addGoalConfig (q5); ps.solve () # with sub-paths q1 = [-1.3, 0.2, 0, 1]; q2 = [0.2, 0.2, 0, 1]; q3 = [5, -2.8, 0, 1]; q4 = [9, 2.2, 0, 1]; ps.setInitialConfig (q1); ps.addGoalConfig (q2); ps.solve () # pp(0) ps.resetGoalConfigs () ps.setInitialConfig (q2); ps.addGoalConfig (q3); ps.solve () # pp(1) ps.resetGoalConfigs () ps.setInitialConfig (q3); ps.addGoalConfig (q4); ps.solve () # pp(2) ps.resetGoalConfigs () ps.setInitialConfig (q4); ps.addGoalConfig (q5); ps.solve () # pp(3) ps.resetGoalConfigs () ps.setInitialConfig (q1); ps.addGoalConfig (q5); ps.solve () # pp(4) ps.resetGoalConfigs ()