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 () ## Video recording pp.dt = 0.02 r.startCapture ("capture","png") #pp(1)
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) r(ps.configAtParam(0,2)) ps.getWaypoints (0) ps.getWaypoints (ps.numberPaths()-1) # plot paths import numpy as np dt = 0.1 nPath = ps.numberPaths()-1 lineNamePrefix = "optimized_path" for t in np.arange(0., cl.problem.pathLength(nPath), dt): lineName = lineNamePrefix+str(t) r.client.gui.addLine(lineName,[cl.problem.configAtParam(nPath, t)[0],cl.problem.configAtParam(nPath, t)[1],0],[cl.problem.configAtParam(nPath, t+dt)[0],cl.problem.configAtParam(nPath, t+dt)[1],0],[0,1,0.3,1]) r.client.gui.addToGroup (lineName, r.sceneName) nPath = nInitialPath
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) ps.getWaypoints (1) plotPath (cl, 0, r, "pathy", 0.1) # time-step should depend on sub-path length ? plotFrame (r, "framy", [0,0,0], 0.5) plotThetaPlane (q1, q2, r, "ThetaPlane") ## DEBUG commands cl.obstacle.getObstaclePosition('decor_base') robot.getJointOuterObjects('base_joint_xyz') robot.isConfigValid(q1)
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")
plt = addPathPlot (cl, x3Path, 'y', 1, plt) plt = addPathPlot (cl, x4Path, 'c', 1, plt) plt = addPathPlot (cl, x5Path, '0.75', 1, plt) plt = addNodePlot (collConstrNodes, 'bo', 'qConstr', plt) plt = addNodePlot (collNodes, 'ro', 'qCol', plt) plt = addNodePlot ([ps.getWaypoints (4)[1], ps.getWaypoints (4)[2]], 'go', 'qCol', plt) plt.show() # will reset plt ##################################################################### ## DEBUG commands robot.isConfigValid(q1) cl.robot.distancesToCollision() from numpy import * argmin(cl.robot.distancesToCollision()[0]) r( ps.configAtParam(0,5) ) ps.optimizePath (0) ps.clearRoadmap () ps.resetGoalConfigs () r.client.gui.removeFromGroup (elementName, r.sceneName) # Compute manually path length with waypoints import math wps = ps.getWaypoints(4) dist = 0 for i in range(0,len(wps)-1): wpi = wps [i] wpii = wps [i+1] dist += math.sqrt( (wpii [0] - wpi [0])**2 + (wpii [1] - wpi [1])**2 ) print dist
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
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 = [-2, 0, 1, 0]; q2 = [2, 0, 1, 0] ps.setInitialConfig (q1); ps.addGoalConfig (q2) cl.obstacle.loadObstacleModel('potential_description','cylinder_obstacle','') ps.createOrientationConstraint ("orConstraint", "base_joint_rz", "", [1,0,0,0], [0,0,1]) # OK ps.setNumericalConstraints ("constraints", ["orConstraint"]) ps.solve () ps.configAtParam(0,2) ps.configAtParam(0,6) ps.configAtParam(0,13) ps.getWaypoints (0)[1] ps.optimizePath(0) ps.pathLength(0) ps.pathLength(1) ps.configAtParam(1,2) ps.configAtParam(1,6) ps.configAtParam(1,13) ps.getWaypoints (1)[1] len(ps.getWaypoints (0)) cl.problem.getIterationNumber()
pp (0) pp (1) ## Video recording r.startCapture ("capture","png") pp(1) r.stopCapture () r.startCapture ("capture","png") pp(2) r.stopCapture () #ffmpeg -r 50 -i capture_0_%d.png -r 25 -vcodec libx264 video.mp4 robot.getJointNames () robot.getJointPosition ("torso_lift_joint") r(ps.configAtParam(0,5)) ps.configAtParam(0,2)[0] ps.configAtParam(0,6)[0] ps.configAtParam(0,13)[0] ps.getWaypoints (0)[1][0] ps.configAtParam(0,2)[1] ps.configAtParam(0,6)[1] ps.configAtParam(0,13)[1] ps.getWaypoints (0)[1][1] ps.configAtParam(1,2)[0] ps.configAtParam(1,6)[0] ps.configAtParam(1,13)[0] ps.getWaypoints (1)[1][0]
# 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 () ps.pathLength(0) cl.problem.getWaypoints (0) ps.configAtParam(0,0.2) ps.pathLength(4) cl.problem.getWaypoints (4) ## Plot whole path in viewer ## import numpy as np nPath = 0 dt = 0.1 for t in np.arange(0., cl.problem.pathLength(nPath), dt): lineName = "g"+str(t) r.client.gui.addLine(lineName,[cl.problem.configAtParam(nPath, t)[0],cl.problem.configAtParam(nPath, t)[1],0.2],[cl.problem.configAtParam(nPath, t+dt)[0],cl.problem.configAtParam(nPath, t+dt)[1],0.2],[0,0.3,1,1]) r.client.gui.addToGroup (lineName, r.sceneName)