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") # Solve the planning problem : success = ps.client.problem.prepareSolveStepByStep() if not success: print "planning failed." import sys sys.exit(1) ps.client.problem.finishSolveStepByStep() try: # display solution :
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
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)
from hpp.environments import Buggy robot = Buggy("buggy") robot.setJointBounds ("base_joint_xy", [-5, 16, -4.5, 4.5]) from hpp.corbaserver import ProblemSolver ps = ProblemSolver (robot) from hpp.gepetto import ViewerFactory gui = ViewerFactory (ps) gui.loadObstacleModel ('hpp_environments', "scene", "scene") q_init = robot.getCurrentConfig () q_goal = q_init [::] q_init[0:2] = [-3.7, -4]; gui (q_init) q_goal [0:2] = [15,2] gui (q_goal) ps.setInitialConfig (q_init) ps.addGoalConfig (q_goal) ps.selectSteeringMethod ("ReedsShepp") ps.selectPathPlanner ("DiffusingPlanner") ps.addPathOptimizer ("RandomShortcut") t = ps.solve () print ("solving time", t)
#q_goal[0:3] = [6.5,-1,0.4] # straight line q_goal [0:3] = [3,-4,0.4] # easy goal position #q_goal[0:3]=[-4.5,-4.8,0.4]# harder goal position #set goal velocity (along x,y,z axis) : q_goal[-6:-3]=[0,0,0] vf.loadObstacleModel ("iai_maps", "room", "room") # with displayArrow parameter the viewer will display velocity and acceleration of the center of the robot with 3D arrow. The length scale with the amplitude with a length of 1 for the maximal amplitude v = vf.createViewer(displayArrows = True) ps.setInitialConfig (q_init) ps.addGoalConfig (q_goal) ps.addPathOptimizer ("RandomShortcut") #select kinodynamic methods : ps.selectSteeringMethod("Kinodynamic") ps.selectDistance("Kinodynamic") # the Kinodynamic steering method require a planner that build directionnal roadmap (with oriented edges) as the trajectories cannot always be reversed. ps.selectPathPlanner("BiRRTPlanner") print (ps.solve ()) # display the computed roadmap. Note that the edges are all represented as straight line and may not show the real motion of the robot between the nodes : v.displayRoadmap("rm") #Alternatively, use the following line instead of ps.solve() to display the roadmap as it's computed (note that it slow down a lot the computation) #v.solveAndDisplay('rm',1) # Highlight the solution path used in the roadmap v.displayPathMap('pm',0)
from hpp.gepetto import PathPlayer robot = Buggy("buggy") robot.setJointBounds ("root_joint", [-5, 16, -4.5, 4.5, -1.01, 1.01, -1.01, 1.01]) from hpp.corbaserver import ProblemSolver ps = ProblemSolver (robot) from hpp.gepetto import ViewerFactory gui = ViewerFactory (ps) gui.loadObstacleModel ('hpp_environments', "scene", "scene") q_init = robot.getCurrentConfig () q_goal = q_init [::] q_init[0:2] = [-3.7, -4]; gui (q_init) q_goal [0:2] = [15,2] gui (q_goal) ps.setInitialConfig (q_init) ps.addGoalConfig (q_goal) ps.selectSteeringMethod ("ReedsShepp") ps.selectPathPlanner ("DiffusingPlanner") ps.addPathOptimizer ("RandomShortcut") t = ps.solve () print ("solving time", t)