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_viewer(self, env_name, env_package="hpp_environments", reduce_sizes=[0, 0, 0], visualize_affordances=[], min_area=None): """ 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 :param min_area: list of couple [affordanceType, size]. If provided set the minimal area for each affordance """ vf = ViewerFactory(self.ps) if self.context: self.afftool = AffordanceTool(context=self.context) self.afftool.client.affordance.affordance.resetAffordanceConfig( ) # FIXME: this should be called in afftool constructor else: self.afftool = AffordanceTool() self.afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005]) if min_area is not None: for (aff_type, min_size) in min_area: self.afftool.setMinimumArea(aff_type, min_size) 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)
#~ q_init [0:3] = [0, -0.63, 0.6]; rbprmBuilder.setCurrentConfig (q_init); r (q_init) #~ q_init [3:7] = [ 0.98877108, 0. , 0.14943813, 0. ] q_goal = q_init[::] #~ q_goal [3:7] = [ 0.98877108, 0. , 0.14943813, 0. ] q_goal[0:3] = [0, 0, 0.648702] r(q_goal) #~ q_goal [0:3] = [1.2, -0.65, 1.1]; r (q_goal) #~ ps.addPathOptimizer("GradientBased") ps.addPathOptimizer("RandomShortcut") ps.setInitialConfig(q_init) ps.addGoalConfig(q_goal) from hpp.corbaserver.affordance.affordance import AffordanceTool afftool = AffordanceTool() afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005]) afftool.loadObstacleModel(packageName, "stair_bauzil", "planning", r) #~ afftool.analyseAll() #~ afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5]) #~ afftool.visualiseAffordances('Lean', r, [0, 0, 0.9]) ps.client.problem.selectConFigurationShooter("RbprmShooter") ps.client.problem.selectPathValidation("RbprmPathValidation", 0.05) #~ ps.solve () t = ps.solve() print(t) if isinstance(t, list): t = t[0] * 3600000 + t[1] * 60000 + t[2] * 1000 + t[3] f = open('log.txt', 'a')
q_init[0:3] = [1.1035428951774233, -1.060021251429404, 0.68] r(q_init) rbprmBuilder.setCurrentConfig(q_init) r(q_init) #~ q_goal[0:3] = [1.2,-1,0.5]; r(q_goal) #~ q_goal[0:3] = [0.9, -1.1, 0.6]; r(q_goal) q_goal[0:3] = [1.5, -1.1, 0.57] r(q_goal) #~ q_goal[0:3] = [0.36, -1, 0.9]; r(q_goal) ps.addPathOptimizer("RandomShortcut") ps.setInitialConfig(q_init) ps.addGoalConfig(q_goal) from hpp.corbaserver.affordance.affordance import AffordanceTool afftool = AffordanceTool() afftool.setAffordanceConfig('Support', [0.2, 0.03, 0.00005]) afftool.setAffordanceConfig('Lean', [0.5, 0.05, 0.00005]) afftool.loadObstacleModel(packageName, "polaris", "planning", r) #~ afftool.analyseAll() #~ afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5]) #~ afftool.visualiseAffordances('Lean', r, [1, 0, 0]) ps.client.problem.selectConFigurationShooter("RbprmShooter") ps.client.problem.selectPathValidation("RbprmPathValidation", 0.01) t = ps.solve() print(t) if isinstance(t, list): t = t[0] * 3600000 + t[1] * 60000 + t[2] * 1000 + t[3] f = open('log.txt', 'a') f.write("path computation " + str(t) + "\n")
#~ q_init [0:3] = [0, -0.63, 0.6]; rbprmBuilder.setCurrentConfig (q_init); r (q_init) #~ q_init [3:7] = [ 0.98877108, 0. , 0.14943813, 0. ] q_goal = q_init[::] #~ q_goal [3:7] = [ 0.98877108, 0. , 0.14943813, 0. ] q_goal[0:3] = [-0.05, -0.02, 0.5] r(q_goal) #~ q_goal [0:3] = [1.2, -0.65, 1.1]; r (q_goal) #~ ps.addPathOptimizer("GradientBased") ps.addPathOptimizer("RandomShortcut") ps.setInitialConfig(q_init) ps.addGoalConfig(q_goal) from hpp.corbaserver.affordance.affordance import AffordanceTool afftool = AffordanceTool() afftool.setAffordanceConfig('Support', [0.000005, 0.000005, 0.00005]) afftool.setAffordanceConfig('Lean', [0.00005, 0.000005, 0.00005]) afftool.loadObstacleModel(packageName, "grasp", "planning", r) #~ afftool.analyseAll() afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5]) afftool.visualiseAffordances('Lean', r, [0, 0, 0.9]) ps.client.problem.selectConFigurationShooter("RbprmShooter") ps.client.problem.selectPathValidation("RbprmPathValidation", 0.05) #~ ps.solve () t = ps.solve() print t if isinstance(t, list): t = t[0] * 3600000 + t[1] * 60000 + t[2] * 1000 + t[3]
#ps.setParameter("Kinodynamic/synchronizeVerticalAxis",False) #ps.setParameter("Kinodynamic/forceYawOrientation",True) ps.setParameter("DynamicPlanner/sizeFootX",0.01) ps.setParameter("DynamicPlanner/sizeFootY",0.01) ps.setParameter("DynamicPlanner/friction",mu) # sample only configuration with null velocity and acceleration : ps.setParameter("ConfigurationShooter/sampleExtraDOF",False) ps.setParameter("PathOptimization/RandomShortcut/NumberOfLoops",100) # initialize the viewer : from hpp.gepetto import ViewerFactory vf = ViewerFactory (ps) # load the module to analyse the environnement and compute the possible contact surfaces from hpp.corbaserver.affordance.affordance import AffordanceTool afftool = AffordanceTool () afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005]) afftool.setMinimumArea('Support',0.03) afftool.loadObstacleModel ("hpp_environments", "ori/modular_palet_flat", "planning", vf,reduceSizes=[0.11,0,0]) #afftool.loadObstacleModel ("hpp_environments", "ori/modular_palet_low", "planning", vf) try : v = vf.createViewer(displayArrows = True) except Exception: print("No viewer started !") class FakeViewer(): def __init__(self): return def __call__(self,q): return v = FakeViewer()
rbprmBuilder.setCurrentConfig(q_init) r(q_init) #~ q_0 [0:3] = [-0.2, 0, 0.3]; r (q_0) q_goal[0:3] = [0.13, 0.05, 0.8] r(q_goal) #~ q_init [0:6] = [0.0, -2.2, 2.0, 0.7316888688738209, 0.0, 0.6816387600233341]; #~ rbprmBuilder.setCurrentConfig (q_init); r (q_init) #~ ps.addPathOptimizer("GradientBased") ps.addPathOptimizer("RandomShortcut") ps.setInitialConfig(q_init) ps.addGoalConfig(q_goal) from hpp.corbaserver.affordance.affordance import AffordanceTool afftool = AffordanceTool() #~ afftool.setAffordanceConfig('Support', [0.2, 0.03, 0.00005]) #~ afftool.setAffordanceConfig('Lean', [0.5, 0.05, 0.00005]) r.loadObstacleModel(packageName, "scene_wall", "planning") afftool.analyseAll() afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5]) afftool.visualiseAffordances('Lean', r, [1, 0, 0]) ps.client.problem.selectConFigurationShooter("RbprmShooter") ps.client.problem.selectPathValidation("RbprmPathValidation", 0.01) t = ps.solve() if isinstance(t, list): t = t[0] * 3600000 + t[1] * 60000 + t[2] * 1000 + t[3] f = open('log.txt', 'a') f.write("path computation " + str(t) + "\n") f.close()
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
# Creating an instance of HPP problem solver and the viewer from hpp.corbaserver.rbprm.problem_solver import ProblemSolver ps = ProblemSolver(rbprmBuilder) ps.client.problem.setParameter("aMax", omniORB.any.to_any(aMax)) ps.client.problem.setParameter("aMaxZ", omniORB.any.to_any(10.)) ps.client.problem.setParameter("vMax", omniORB.any.to_any(vMax)) ps.client.problem.setParameter("orientedPath", omniORB.any.to_any(0.)) ps.client.problem.setParameter("friction", omniORB.any.to_any(MU)) ps.client.problem.setParameter("sizeFootX", omniORB.any.to_any(0.24)) ps.client.problem.setParameter("sizeFootY", omniORB.any.to_any(0.14)) r = Viewer(ps, displayArrows=True) from hpp.corbaserver.affordance.affordance import AffordanceTool afftool = AffordanceTool() afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.005]) afftool.loadObstacleModel(packageName, "darpa_no_mid", "planning", r, reduceSizes=[0.14, 0]) #r.loadObstacleModel (packageName, "ground", "planning") afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5]) r.addLandmark(r.sceneName, 1) # Setting initial and goal configurations q_init = rbprmBuilder.getCurrentConfig() q_init[3:7] = [1, 0, 0, 0] q_init[0:3] = [-0.95, 0, 0.53] r(q_init)
r = Viewer (ps) # Setting initial configuration to show on the viewer q_init = robot.getCurrentConfig () q_init [0:3] = [-2, 0, 0.63] robot.setCurrentConfig (q_init) r (q_init) # Set coulour variables for different affordance types SupportColour = [0.0, 0.95, 0.80] LeanColour = [0.9, 0.5, 0] # Import the affordance helper class to extract useful surface # objects from the environment, and create an instance of affordanceTool from hpp.corbaserver.affordance.affordance import AffordanceTool afftool = AffordanceTool () # Set the affordance configuration. The configuration vector has size 3 # and comprises the error margin, the angle margin for neighbouring triangles # and the minimum area, in that order. # If no configuration is set, a default configuration is used. afftool.setAffordanceConfig('Support', [0.3, 0.3, 0.05]) afftool.setAffordanceConfig('Lean', [0.1, 0.3, 0.05]) # Load obstacle models and visualise affordances. When loading an obstacle, # the affordance analysis is done automatically. afftool.loadObstacleModel ("hpp-affordance-corba", "darpa", "planning", r) afftool.visualiseAffordances('Support', r, SupportColour) # If affordance configuration is changed, the affordance analysis must # be relaunched.
[-1, 1, -1, 1, -0.5, 0.5, 0, 0, 0, 0, 0, 0]) indexECS = rbprmBuilder.getConfigSize( ) - rbprmBuilder.client.basic.robot.getDimensionExtraConfigSpace() # Creating an instance of HPP problem solver and the viewer ps = ProblemSolver(rbprmBuilder) ps.client.problem.setParameter("aMax", aMax) ps.client.problem.setParameter("vMax", vMax) ps.client.problem.setParameter("sizeFootX", omniORB.any.to_any(0.24)) ps.client.problem.setParameter("sizeFootY", omniORB.any.to_any(0.14)) ps.client.problem.setParameter("friction", mu) r = Viewer(ps) from hpp.corbaserver.affordance.affordance import AffordanceTool afftool = AffordanceTool() afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005]) afftool.loadObstacleModel(ENV_PACKAGE_NAME, ENV_NAME, ENV_PREFIX, r) #afftool.loadObstacleModel (ENV_PACKAGE_NAME, ENV_NAME, ENV_PREFIX, r) #r.loadObstacleModel (packageName, "ground", "planning") afftool.visualiseAffordances('Support', r, r.color.lightBrown) #r.addLandmark(r.sceneName,1) # Setting initial and goal configurations q_init = rbprmBuilder.getCurrentConfig() q_init[3:7] = [1, 0, 0, 0] q_init[0:3] = [0.3, 0, 0.58] r(q_init) q_init[8] = 0.8 #q_init[3:7] = [0.7071,0,0,0.7071] #q_init [0:3] = [1, 1, 0.65]
]) rbprmBuilder.client.basic.robot.setDimensionExtraConfigSpace(extraDof) rbprmBuilder.client.basic.robot.setExtraConfigSpaceBounds( [-vMax, vMax, -vMax, vMax, 0, 0, 0, 0, 0, 0, 0, 0]) #~ from hpp.corbaserver.rbprm. import ProblemSolver from hpp.corbaserver.rbprm.problem_solver import ProblemSolver ps = ProblemSolver(rbprmBuilder) ps.client.problem.setParameter("aMax", aMax) ps.client.problem.setParameter("vMax", vMax) ps.client.problem.setParameter("tryJump", 1) r = Viewer(ps) from hpp.corbaserver.affordance.affordance import AffordanceTool afftool = AffordanceTool() afftool.loadObstacleModel(packageName, "ground_jump_easy", "planning", r) afftool.visualiseAffordances('Support', r, r.color.brown) r.addLandmark(r.sceneName, 1) q_init = rbprmBuilder.getCurrentConfig() #q_init[(len(q_init)-3):]=[0,0,1] # set normal for init / goal config #q_init [0:3] = [-3, 0, 0.79]; rbprmBuilder.setCurrentConfig (q_init); r (q_init) q_init[0:3] = [-3, 0, 0.7] rbprmBuilder.setCurrentConfig(q_init) r(q_init) q_goal = q_init[::] #q_goal [0:3] = [-2, 0, 0.9]; r (q_goal) # premiere passerelle q_goal[0:3] = [0.7, 0, 0.7] r(q_goal) # pont #q_goal = [-0.594272,0,0.724869,1,0,0,0,3.26968,0,1.85351,0,0,0]
ps.setParameter("Kinodynamic/velocityBound", vMax) ps.setParameter("Kinodynamic/accelerationBound", aMax) ps.setParameter("DynamicPlanner/sizeFootX", 0.2) ps.setParameter("DynamicPlanner/sizeFootY", 0.12) ps.setParameter("DynamicPlanner/friction", mu) ps.setParameter("Kinodynamic/forceYawOrientation", True) # sample only configuration with null velocity and acceleration : ps.setParameter("ConfigurationShooter/sampleExtraDOF", False) ps.setParameter("PathOptimization/RandomShortcut/NumberOfLoops", 500) # initialize the viewer : from hpp.gepetto import ViewerFactory vf = ViewerFactory(ps) # load the module to analyse the environnement and compute the possible contact surfaces from hpp.corbaserver.affordance.affordance import AffordanceTool afftool = AffordanceTool() afftool.setAffordanceConfig('Support', [0.5, 0.03, 5.]) afftool.loadObstacleModel("hpp_environments", "multicontact/maze_hard", "planning", vf) #load the viewer try: v = vf.createViewer(displayArrows=True) except Exception: print("No viewer started !") class FakeViewer(): def __init__(self): return def __call__(self, q): return
r = Viewer(ps) # Setting initial configuration to show on the viewer q_init = robot.getCurrentConfig() q_init[0:3] = [-2, 0, 0.63] robot.setCurrentConfig(q_init) r(q_init) # Set coulour variables for different affordance types SupportColour = [0.0, 0.95, 0.80] LeanColour = [0.9, 0.5, 0] # Import the affordance helper class to extract useful surface # objects from the environment, and create an instance of affordanceTool from hpp.corbaserver.affordance.affordance import AffordanceTool afftool = AffordanceTool() afftool.setAffordanceConfig('Support', [0.3, 0.3, 0.05]) afftool.setAffordanceConfig('Lean', [0.1, 0.3, 0.05]) # Load obstacle models and visualise affordances. When loading an obstacle, # the affordance analysis is done automatically. afftool.loadObstacleModel("hpp-affordance-corba", "darpa", "planning", r) afftool.loadObstacleModel("hpp-affordance-corba", "box", "box1", r) afftool.visualiseAffordances('Support', r, SupportColour) # If an object is translated or rotated, the affordance analysis must # be relaunched. # First, delete the already created affordance objects (this function also # deletes them from viewer) afftool.deleteAffordances(r, 'box1/base_link_0')
q_init = rbprmBuilder.getCurrentConfig() q_init[0:3] = [-5, 0, 0.63] rbprmBuilder.setCurrentConfig(q_init) r(q_init) q_goal = q_init[::] q_goal[0:3] = [-2.5, 0, 0.63] #pass the hole #~ q_goal [0:3] = [5, 0, 0.63]; r (q_goal) #pass the bridge ps.addPathOptimizer("RandomShortcut") ps.setInitialConfig(q_init) ps.addGoalConfig(q_goal) from hpp.corbaserver.affordance.affordance import AffordanceTool afftool = AffordanceTool() afftool.loadObstacleModel(packageName, "groundcrouch", "planning", r) #~ afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5]) # Choosing RBPRM shooter and path validation methods. # Note that the standard RRT algorithm is used. ps.client.problem.selectConFigurationShooter("RbprmShooter") ps.client.problem.selectPathValidation("RbprmPathValidation", 0.05) t = ps.solve() if isinstance(t, list): t = t[0] * 3600000 + t[1] * 60000 + t[2] * 1000 + t[3] from hpp.gepetto import PathPlayer pp = PathPlayer(rbprmBuilder.client.basic, r)
ps.setParameter("Kinodynamic/velocityBound", vMax) ps.setParameter("Kinodynamic/accelerationBound", aMax) ps.setParameter("DynamicPlanner/sizeFootX", 0.2) ps.setParameter("DynamicPlanner/sizeFootY", 0.12) ps.setParameter("DynamicPlanner/friction", 0.5) ps.setParameter("Kinodynamic/forceYawOrientation", True) # sample only configuration with null velocity and acceleration : ps.setParameter("ConfigurationShooter/sampleExtraDOF", False) # initialize the viewer : from hpp.gepetto import ViewerFactory vf = ViewerFactory(ps) # load the module to analyse the environnement and compute the possible contact surfaces from hpp.corbaserver.affordance.affordance import AffordanceTool afftool = AffordanceTool() afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005]) afftool.loadObstacleModel("hpp_environments", "multicontact/ground", "planning", vf) #load the viewer try: v = vf.createViewer(displayArrows=True) except Exception: print "No viewer started !" class FakeViewer(): sceneName = "" def __init__(self): return
# force the orientation of the trunk to match the direction of the motion ps.setParameter("Kinodynamic/forceYawOrientation", False) ps.setParameter("DynamicPlanner/sizeFootX", 0.2) ps.setParameter("DynamicPlanner/sizeFootY", 0.12) ps.setParameter("DynamicPlanner/friction", mu) # sample only configuration with null velocity and acceleration : ps.setParameter("ConfigurationShooter/sampleExtraDOF", False) ps.setParameter("PathOptimization/RandomShortcut/NumberOfLoops", 100) # initialize the viewer : from hpp.gepetto import ViewerFactory vf = ViewerFactory(ps) # load the module to analyse the environnement and compute the possible contact surfaces from hpp.corbaserver.affordance.affordance import AffordanceTool afftool = AffordanceTool() afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005]) afftool.loadObstacleModel("hpp_environments", "multicontact/bauzil_stairs", "planning", vf, reduceSizes=[0.18, 0., 0.]) v = vf.createViewer(displayArrows=True) #afftool.visualiseAffordances('Support', v, v.color.lightBrown) v.addLandmark(v.sceneName, 1) # Setting initial configuration q_init = rbprmBuilder.getCurrentConfig() q_init[0:3] = [0.05, 1.2, 0.98] q_init[-6:-3] = [0, 0, 0] v(q_init)
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver ps = ProblemSolver( rbprmBuilder ) r = Viewer (ps) # Setting initial and goal configurations q_init = rbprmBuilder.getCurrentConfig (); q_init [0:3] = [-2, 0, 0.63]; rbprmBuilder.setCurrentConfig (q_init); r (q_init) q_goal = q_init [::] q_goal [0:3] = [3, 0, 0.63]; r (q_goal) ps.setInitialConfig (q_init) ps.addGoalConfig (q_goal) import random from hpp.corbaserver.affordance.affordance import AffordanceTool afftool = AffordanceTool () afftool.loadObstacleModel (packageName, "darpa", "planning", r) afftool.visualiseAffordances('Support', r, 'planning', [0.25, 0.5, 0.5]) # Choosing RBPRM shooter and path validation methods. # Note that the standard RRT algorithm is used. ps.client.problem.selectConFigurationShooter("RbprmShooter") ps.client.problem.selectPathValidation("RbprmPathValidation",0.05) # Solve the problem t = ps.solve () # Playing the computed path from hpp.gepetto import PathPlayer pp = PathPlayer (rbprmBuilder.client.basic, r) pp (0)
# Creating an instance of HPP problem solver and the viewer from hpp.corbaserver.rbprm.problem_solver import ProblemSolver ps = ProblemSolver( rbprmBuilder ) r = Viewer (ps) # Setting initial and goal configurations q_init = rbprmBuilder.getCurrentConfig (); q_init [0:3] = [-2, 0, 0.63]; rbprmBuilder.setCurrentConfig (q_init); r (q_init) q_goal = q_init [::] q_goal [0:3] = [3, 0, 0.63]; r (q_goal) ps.setInitialConfig (q_init) ps.addGoalConfig (q_goal) from hpp.corbaserver.affordance.affordance import AffordanceTool afftool = AffordanceTool () afftool.loadObstacleModel (packageName, "darpa", "planning", r) afftool.loadObstacleModel ("hpp-ompl-benchmark", "cubicles_robot", "robo", r) afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5]) afftool.deleteAffordances(r,'robo/base_link_0') ps.moveObstacle('robo/base_link_0', [0,-0.75,0, 0.5,0.5,0.5,0.5]) r.computeObjectPosition() afftool.analyseObject('robo/base_link_0') afftool.visualiseAffordances('Support', r, [0.75, 0.75, 0.1], 'robo/base_link_0') # Choosing RBPRM shooter and path validation methods. # Note that the standard RRT algorithm is used. ps.client.problem.selectConFigurationShooter("RbprmShooter") ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
# Setting initial and goal configurations q_init = rbprmBuilder.getCurrentConfig() q_init[0:3] = [0, 0, 0.63] rbprmBuilder.setCurrentConfig(q_init) r(q_init) q_goal = q_init[::] q_goal[0:3] = [6, 0, 0.63] r(q_goal) # Choosing a path optimizer ps.addPathOptimizer("RandomShortcut") ps.setInitialConfig(q_init) ps.addGoalConfig(q_goal) from hpp.corbaserver.affordance.affordance import AffordanceTool afftool = AffordanceTool() afftool.loadObstacleModel(packageName, "plane_hole", "planning", r) #~ afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5]) # Choosing RBPRM shooter and path validation methods. # Note that the standard RRT algorithm is used. ps.client.problem.selectConFigurationShooter("RbprmShooter") ps.client.problem.selectPathValidation("RbprmPathValidation", 0.05) # Solve the problem t = ps.solve() if isinstance(t, list): t = t[0] * 3600000 + t[1] * 60000 + t[2] * 1000 + t[3] # Playing the computed path from hpp.gepetto import PathPlayer
q_init [0:3] = [0.1, -0.82, 0.648702]; rbprmBuilder.setCurrentConfig (q_init); r (q_init) #~ q_init [0:3] = [0, -0.63, 0.6]; rbprmBuilder.setCurrentConfig (q_init); r (q_init) #~ q_init [3:7] = [ 0.98877108, 0. , 0.14943813, 0. ] q_goal = q_init [::] q_goal [3:7] = [ 0.98877108, 0. , 0.14943813, 0. ] q_goal [0:3] = [1.49, -0.65, 1.25]; r (q_goal) #~ q_goal [0:3] = [1.2, -0.65, 1.1]; r (q_goal) #~ ps.addPathOptimizer("GradientBased") ps.addPathOptimizer("RandomShortcut") ps.setInitialConfig (q_init) ps.addGoalConfig (q_goal) from hpp.corbaserver.affordance.affordance import AffordanceTool afftool = AffordanceTool () afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005]) afftool.loadObstacleModel (packageName, "stair_bauzil", "planning", r) #~ afftool.analyseAll() afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5]) #~ afftool.visualiseAffordances('Lean', r, [0, 0, 0.9]) ps.client.problem.selectConFigurationShooter("RbprmShooter") ps.client.problem.selectPathValidation("RbprmPathValidation",0.05) #~ ps.solve () t = ps.solve () print t; if isinstance(t, list): t = t[0]* 3600000 + t[1] * 60000 + t[2] * 1000 + t[3] f = open('log.txt', 'a')
ps.setParameter("Kinodynamic/accelerationBound", aMax) ps.setParameter("DynamicPlanner/sizeFootX", 0.01) ps.setParameter("DynamicPlanner/sizeFootY", 0.01) ps.setParameter("DynamicPlanner/friction", 0.5) ps.setParameter("Kinodynamic/forceYawOrientation", True) # sample only configuration with null velocity and acceleration : ps.setParameter("ConfigurationShooter/sampleExtraDOF", False) ps.setParameter("PathOptimization/RandomShortcut/NumberOfLoops", 50) # initialize the viewer : from hpp.gepetto import ViewerFactory vf = ViewerFactory(ps) # load the module to analyse the environnement and compute the possible contact surfaces from hpp.corbaserver.affordance.affordance import AffordanceTool afftool = AffordanceTool() afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005]) afftool.loadObstacleModel("hpp_environments", "multicontact/slalom_debris", "planning", vf, reduceSizes=[0.05, 0, 0]) v = vf.createViewer(displayArrows=True) #afftool.visualiseAffordances('Support', v, v.color.lightBrown) # Setting initial configuration q_init = rbprmBuilder.getCurrentConfig() q_init[0:3] = [-1.5, 0, 0.63] q_init[3:7] = [0, 0, 0, 1] q_init[-6] = 0.05 v(q_init)
q_init [0:3] = [-1, 0.05, 0.4]; rbprmBuilder.setCurrentConfig (q_init); r (q_init) #~ q_0 [0:3] = [-0.2, 0, 0.3]; r (q_0) q_goal [0:3] = [0.13, 0.05, 0.8]; r (q_goal) #~ q_init [0:6] = [0.0, -2.2, 2.0, 0.7316888688738209, 0.0, 0.6816387600233341]; #~ rbprmBuilder.setCurrentConfig (q_init); r (q_init) #~ ps.addPathOptimizer("GradientBased") ps.addPathOptimizer("RandomShortcut") ps.setInitialConfig (q_init) ps.addGoalConfig (q_goal) from hpp.corbaserver.affordance.affordance import AffordanceTool afftool = AffordanceTool () #~ afftool.setAffordanceConfig('Support', [0.2, 0.03, 0.00005]) #~ afftool.setAffordanceConfig('Lean', [0.5, 0.05, 0.00005]) r.loadObstacleModel (packageName, "scene_wall", "planning") afftool.analyseAll() afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5]) afftool.visualiseAffordances('Lean', r, [1, 0, 0]) ps.client.problem.selectConFigurationShooter("RbprmShooter") ps.client.problem.selectPathValidation("RbprmPathValidation",0.01) t = ps.solve () if isinstance(t, list): t = t[0]* 3600000 + t[1] * 60000 + t[2] * 1000 + t[3] f = open('log.txt', 'a') f.write("path computation " + str(t) + "\n") f.close()
#ps.setParameter("Kinodynamic/forceAllOrientation",True) ps.setParameter("DynamicPlanner/sizeFootX",0.1) ps.setParameter("DynamicPlanner/sizeFootY",0.1) ps.setParameter("DynamicPlanner/friction",mu) # sample only configuration with null velocity and acceleration : ps.setParameter("ConfigurationShooter/sampleExtraDOF",False) ps.setParameter("PathOptimization/RandomShortcut/NumberOfLoops",500) ps.setParameter("Kinodynamic/verticalAccelerationBound",aMaxZ) # initialize the viewer : from hpp.gepetto import ViewerFactory vf = ViewerFactory (ps) # load the module to analyse the environnement and compute the possible contact surfaces from hpp.corbaserver.affordance.affordance import AffordanceTool afftool = AffordanceTool () afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005]) afftool.loadObstacleModel ("hpp_environments", "ori/slalom2", "planning", vf,reduceSizes=[0.08,0,0]) v = vf.createViewer(displayArrows = True) afftool.visualiseAffordances('Support', v, v.color.lightBrown) #v.addLandmark(v.sceneName,1) # Setting initial configuration q_init = rbprmBuilder.getCurrentConfig (); q_init [0:3] = [-0.3,-0.4,0.47] q_init[-6:-3] = [0,0,0] v (q_init) ps.setInitialConfig (q_init) # set goal config rbprmBuilder.setCurrentConfig (q_init) q_goal = q_init [::]
#~ q_init [0:3] = [0, -0.63, 0.6]; rbprmBuilder.setCurrentConfig (q_init); r (q_init) #~ q_init [3:7] = [ 0.98877108, 0. , 0.14943813, 0. ] q_goal = q_init[::] q_goal[3:7] = [0.98877108, 0., 0.14943813, 0.] #~ q_goal [0:3] = [1.49, -0.65, 1.25]; r (q_goal) q_goal[0:3] = [2.1, -0.82, 1.0] r(q_goal) #~ ps.addPathOptimizer("GradientBased") ps.addPathOptimizer("RandomShortcut") ps.setInitialConfig(q_init) ps.addGoalConfig(q_goal) from hpp.corbaserver.affordance.affordance import AffordanceTool afftool = AffordanceTool() afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005]) afftool.setAffordanceConfig('Lean', [0.5, 0.03, 0.00005]) #~ afftool.loadObstacleModel (packageName, "twister", "planning", r) #~ afftool.analyseAll() #~ afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5]) #~ afftool.visualiseAffordances('Lean', r, [0, 0, 0.9]) #~ ps.client.problem.selectConFigurationShooter("RbprmShooter") #~ ps.client.problem.selectPathValidation("RbprmPathValidation",0.05) #~ ps.solve () #~ t = ps.solve () #~ #~ print t; #~ if isinstance(t, list): #~ t = t[0]* 3600000 + t[1] * 60000 + t[2] * 1000 + t[3]
rbprmBuilder.client.robot.setDimensionExtraConfigSpace(extraDof) # We set the bounds of this extraDof with velocity and acceleration bounds (expect on z axis) extraDofBounds = [ -vMax, vMax, -vMax, vMax, -10., 10., -aMax, aMax, -aMax, aMax, -10., 10. ] rbprmBuilder.client.robot.setExtraConfigSpaceBounds(extraDofBounds) indexECS = rbprmBuilder.getConfigSize( ) - rbprmBuilder.client.robot.getDimensionExtraConfigSpace() from hpp.corbaserver.problem_solver import ProblemSolver ps = ProblemSolver(rbprmBuilder) from hpp.gepetto import ViewerFactory vf = ViewerFactory(ps) from hpp.corbaserver.affordance.affordance import AffordanceTool afftool = AffordanceTool() afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005]) afftool.loadObstacleModel(packageName, "multicontact/slalom_debris", "planning", vf, reduceSizes=[0., 0., 0.]) v = vf.createViewer(displayArrows=True) afftool.visualiseAffordances('Support', v, [0.25, 0.5, 0.5]) v.addLandmark(v.sceneName, 1) # force the orientation of the trunk to match the direction of the motion ps.setParameter("Kinodynamic/forceYawOrientation", True) ps.setParameter("Kinodynamic/synchronizeVerticalAxis", True) ps.setParameter("Kinodynamic/verticalAccelerationBound", 10.) ps.setParameter("DynamicPlanner/sizeFootX", 0.2)
) - rbprmBuilder.client.basic.robot.getDimensionExtraConfigSpace() # Creating an instance of HPP problem solver and the viewer from hpp.corbaserver.rbprm.problem_solver import ProblemSolver ps = ProblemSolver(rbprmBuilder) ps.client.problem.setParameter("aMax", omniORB.any.to_any(aMax)) ps.client.problem.setParameter("vMax", omniORB.any.to_any(vMax)) ps.client.problem.setParameter("orientedPath", omniORB.any.to_any(1.)) ps.client.problem.setParameter("friction", omniORB.any.to_any(MU)) ps.client.problem.setParameter("sizeFootX", omniORB.any.to_any(0.24)) ps.client.problem.setParameter("sizeFootY", omniORB.any.to_any(0.14)) r = Viewer(ps) from hpp.corbaserver.affordance.affordance import AffordanceTool afftool = AffordanceTool() afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.2]) afftool.loadObstacleModel(packageName, ENV_NAME, ENV_PREFIX, r, reduceSizes=[0.2, 0, 0]) #r.loadObstacleModel (packageName, "ground", "planning") #afftool.visualiseAffordances('Support', r,r.color.lightYellow) r.addLandmark(r.sceneName, 1) # Setting initial and goal configurations q_init = rbprmBuilder.getCurrentConfig() q_init[3:7] = [1, 0, 0, 0] q_init[0:3] = [-0.9, 1, 0.6] r(q_init)
# Creating an instance of HPP problem solver and the viewer from hpp.corbaserver.rbprm.problem_solver import ProblemSolver ps = ProblemSolver( rbprmBuilder ) ps.client.problem.setParameter("aMax",omniORB.any.to_any(aMax)) ps.client.problem.setParameter("vMax",omniORB.any.to_any(vMax)) ps.client.problem.setParameter("orientedPath",omniORB.any.to_any(0.)) ps.client.problem.setParameter("friction",omniORB.any.to_any(MU)) ps.client.problem.setParameter("sizeFootX",omniORB.any.to_any(0.24)) ps.client.problem.setParameter("sizeFootY",omniORB.any.to_any(0.14)) r = Viewer (ps,displayArrows = True) from hpp.corbaserver.affordance.affordance import AffordanceTool afftool = AffordanceTool () afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.2]) afftool.loadObstacleModel (packageName, ENV_NAME, ENV_PREFIX, r,reduceSizes=[0.17,0,0]) #r.loadObstacleModel (packageName, "ground", "planning") afftool.visualiseAffordances('Support', r,r.color.lightYellow) r.addLandmark(r.sceneName,1) # Setting initial and goal configurations q_init = rbprmBuilder.getCurrentConfig (); q_init[3:7] = [1,0,0,0] q_init [0:3] = [0.05, 0.86, 0.61]; r (q_init) rbprmBuilder.setCurrentConfig (q_init) q_goal = q_init [::]
) - rbprmBuilder.client.basic.robot.getDimensionExtraConfigSpace() # Creating an instance of HPP problem solver and the viewer from hpp.corbaserver.rbprm.problem_solver import ProblemSolver ps = ProblemSolver(rbprmBuilder) ps.client.problem.setParameter("aMax", omniORB.any.to_any(aMax)) ps.client.problem.setParameter("vMax", omniORB.any.to_any(vMax)) ps.client.problem.setParameter("orientedPath", omniORB.any.to_any(0.)) ps.client.problem.setParameter("friction", omniORB.any.to_any(MU)) ps.client.problem.setParameter("sizeFootX", omniORB.any.to_any(0.24)) ps.client.problem.setParameter("sizeFootY", omniORB.any.to_any(0.14)) r = Viewer(ps, displayArrows=True) from hpp.corbaserver.affordance.affordance import AffordanceTool afftool = AffordanceTool() afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005]) afftool.loadObstacleModel(packageName, ENV_NAME, ENV_PREFIX, r) #r.loadObstacleModel (packageName, "ground", "planning") #afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5]) r.addLandmark(r.sceneName, 1) # Setting initial and goal configurations q_init = rbprmBuilder.getCurrentConfig() q_init[3:7] = [1, 0, 0, 0] q_init[0:3] = [-0.9, 0.2, 0.58] r(q_init) q_init[-6] = 0.05 rbprmBuilder.setCurrentConfig(q_init) q_goal = q_init[::]
# define parameters used by various methods : ps.setParameter("Kinodynamic/velocityBound",vMax) ps.setParameter("Kinodynamic/accelerationBound",aMax) ps.setParameter("DynamicPlanner/sizeFootX",0.2) ps.setParameter("DynamicPlanner/sizeFootY",0.12) ps.setParameter("DynamicPlanner/friction",0.5) # sample only configuration with null velocity and acceleration : ps.setParameter("ConfigurationShooter/sampleExtraDOF",False) # initialize the viewer : from hpp.gepetto import ViewerFactory vf = ViewerFactory (ps) # load the module to analyse the environnement and compute the possible contact surfaces from hpp.corbaserver.affordance.affordance import AffordanceTool afftool = AffordanceTool () afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005]) afftool.loadObstacleModel ("hpp_environments", "multicontact/plateforme_surfaces", "planning", vf,reduceSizes=[0.18,0,0]) v = vf.createViewer(displayArrows = True) afftool.visualiseAffordances('Support', v, v.color.lightBrown) # Setting initial configuration q_init = rbprmBuilder.getCurrentConfig (); q_init[3:7] = [0,0,0,1] q_init [0:3] = [0.16, 0.25, 1.14] v (q_init) ps.setInitialConfig (q_init) # set goal config rbprmBuilder.setCurrentConfig (q_init) q_goal = q_init [::] q_goal[0] = 1.09
r = Viewer(ps) # Setting initial configuration to show on the viewer q_init = robot.getCurrentConfig() q_init[0:3] = [-2, 0, 0.63] robot.setCurrentConfig(q_init) r(q_init) # Set coulour variables for different affordance types SupportColour = [0.0, 0.95, 0.80] LeanColour = [0.9, 0.5, 0] # Import the affordance helper class to extract useful surface # objects from the environment, and create an instance of affordanceTool from hpp.corbaserver.affordance.affordance import AffordanceTool afftool = AffordanceTool() # Set the affordance configuration. The configuration vector has size 3 # and comprises the error margin, the angle margin for neighbouring triangles # and the minimum area, in that order. # If no configuration is set, a default configuration is used. afftool.setAffordanceConfig('Support', [0.3, 0.3, 0.05]) afftool.setAffordanceConfig('Lean', [0.1, 0.3, 0.05]) # Load obstacle models and visualise affordances. When loading an obstacle, # the affordance analysis is done automatically. afftool.loadObstacleModel("hpp-affordance-corba", "darpa", "planning", r) afftool.visualiseAffordances('Support', r, SupportColour) # If affordance configuration is changed, the affordance analysis must # be relaunched.
[-1, 1, -1, 1, -0.5, 0.5, 0, 0, 0, 0, 0, 0]) indexECS = rbprmBuilder.getConfigSize( ) - rbprmBuilder.client.basic.robot.getDimensionExtraConfigSpace() # Creating an instance of HPP problem solver and the viewer ps = ProblemSolver(rbprmBuilder) ps.client.problem.setParameter("aMax", aMax) ps.client.problem.setParameter("vMax", vMax) ps.client.problem.setParameter("sizeFootX", omniORB.any.to_any(0.24)) ps.client.problem.setParameter("sizeFootY", omniORB.any.to_any(0.14)) ps.client.problem.setParameter("friction", mu) r = Viewer(ps) from hpp.corbaserver.affordance.affordance import AffordanceTool afftool = AffordanceTool() afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005]) afftool.loadObstacleModel(ENV_PACKAGE_NAME, ENV_NAME, ENV_PREFIX, r, reduceSizes=[0.14, 0.]) #afftool.loadObstacleModel (ENV_PACKAGE_NAME, ENV_NAME, ENV_PREFIX, r) #r.loadObstacleModel (packageName, "ground", "planning") #afftool.visualiseAffordances('Support', r, r.color.lightBrown) #r.addLandmark(r.sceneName,1) # Setting initial and goal configurations q_init = rbprmBuilder.getCurrentConfig() q_init[3:7] = [1, 0, 0, 0] q_init[0:3] = [0.3, 0, 0.58]
# define parameters used by various methods : ps.setParameter("Kinodynamic/velocityBound",vMax) ps.setParameter("Kinodynamic/accelerationBound",aMax) ps.setParameter("DynamicPlanner/sizeFootX",0.2) ps.setParameter("DynamicPlanner/sizeFootY",0.12) ps.setParameter("DynamicPlanner/friction",mu) # sample only configuration with null velocity and acceleration : ps.setParameter("ConfigurationShooter/sampleExtraDOF",False) # initialize the viewer : from hpp.gepetto import ViewerFactory vf = ViewerFactory (ps) # load the module to analyse the environnement and compute the possible contact surfaces from hpp.corbaserver.affordance.affordance import AffordanceTool afftool = AffordanceTool () afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005]) afftool.loadObstacleModel ("hpp_environments", "multicontact/plateforme_not_flat", "planning", vf, reduceSizes=[0.1,0,0]) try : v = vf.createViewer(displayArrows = True) except Exception: print "No viewer started !" class FakeViewer(): def __init__(self): return def __call__(self,q): return v = FakeViewer() #afftool.visualiseAffordances('Support', v, v.color.lightBrown)
]) # We also bound the rotations of the torso. rbprmBuilder.boundSO3([-0.4, 0.4, -3, 3, -3, 3]) rbprmBuilder.client.basic.robot.setDimensionExtraConfigSpace(extraDof) rbprmBuilder.client.basic.robot.setExtraConfigSpaceBounds( [-vMax, vMax, -vMax, vMax, 0, 0, 0, 0, 0, 0, 0, 0]) # Creating an instance of HPP problem solver and the viewer from hpp.corbaserver.rbprm.problem_solver import ProblemSolver ps = ProblemSolver(rbprmBuilder) ps.client.problem.setParameter("aMax", aMax) ps.client.problem.setParameter("vMax", vMax) r = Viewer(ps) from hpp.corbaserver.affordance.affordance import AffordanceTool afftool = AffordanceTool() afftool.loadObstacleModel(packageName, "darpa", "planning", r) #r.loadObstacleModel (packageName, "ground", "planning") afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5]) r.addLandmark(r.sceneName, 1) # Setting initial and goal configurations q_init = rbprmBuilder.getCurrentConfig() q_init[0:3] = [-2, 0, 0.63] rbprmBuilder.setCurrentConfig(q_init) r(q_init) q_goal = q_init[::] q_goal[0:3] = [3, 0, 0.63] #q_goal[0:3]=[3,-4,0.4]#position easy q_goal[7:10] = [0, 0, 0] #velocity r(q_goal)
ps = ProblemSolver( rbprmBuilder ) r = Viewer (ps) # Setting initial and goal configurations q_init = rbprmBuilder.getCurrentConfig (); q_init [0:3] = [-2, 0, 0.63]; rbprmBuilder.setCurrentConfig (q_init); r (q_init) q_goal = q_init [::] q_goal [0:3] = [3, 0, 0.63]; r (q_goal) # Choosing a path optimizer ps.addPathOptimizer("RandomShortcut") ps.setInitialConfig (q_init) ps.addGoalConfig (q_goal) from hpp.corbaserver.affordance.affordance import AffordanceTool afftool = AffordanceTool () afftool.loadObstacleModel (packageName, "darpa", "planning", r) #~ afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5]) # Choosing RBPRM shooter and path validation methods. # Note that the standard RRT algorithm is used. ps.client.problem.selectConFigurationShooter("RbprmShooter") ps.client.problem.selectPathValidation("RbprmPathValidation",0.05) # Solve the problem t = ps.solve () if isinstance(t, list): t = t[0]* 3600000 + t[1] * 60000 + t[2] * 1000 + t[3] # Playing the computed path from hpp.gepetto import PathPlayer
]) # We also bound the rotations of the torso. (z, y, x) rbprmBuilder.boundSO3([-0.1, 0.1, -0.65, 0.65, -0.2, 0.2]) rbprmBuilder.client.basic.robot.setDimensionExtraConfigSpace(extraDof) rbprmBuilder.client.basic.robot.setExtraConfigSpaceBounds( [-vMax, vMax, -1, 1, -2, 2, 0, 0, 0, 0, 0, 0]) # Creating an instance of HPP problem solver and the viewer from hpp.corbaserver.rbprm.problem_solver import ProblemSolver ps = ProblemSolver(rbprmBuilder) ps.client.problem.setParameter("aMax", aMax) ps.client.problem.setParameter("vMax", vMax) r = Viewer(ps) from hpp.corbaserver.affordance.affordance import AffordanceTool afftool = AffordanceTool() afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005]) afftool.loadObstacleModel(packageName, "downSlope", "planning", r) #r.loadObstacleModel (packageName, "ground", "planning") afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5]) r.addLandmark(r.sceneName, 1) # Setting initial and goal configurations q_init = rbprmBuilder.getCurrentConfig() q_init[3:7] = [0.9659, 0, 0.2588, 0] q_init[0:3] = [-1.25, 1, 1.7] r(q_init) #q_init[3:7] = [0.7071,0,0,0.7071] #q_init [0:3] = [1, 1, 0.65] rbprmBuilder.setCurrentConfig(q_init)
q_init = rbprmBuilder.getCurrentConfig (); #~ q_init[0:3] = [0.36, -0.32, 1.01]; r(q_init) q_init[0:3] = [0.36, 0, 1.01]; r(q_init) rbprmBuilder.setCurrentConfig (q_init); r (q_init) #~ q_goal[0:3] = [1.2,-1,0.5]; r(q_goal) #~ q_goal[0:3] = [0.9, -1.1, 0.6]; r(q_goal) #~ q_goal[0:3] = [1.5, -1.1, 0.5]; r(q_goal) q_goal[0:3] = [0.36, -1, 0.9]; r(q_goal) ps.addPathOptimizer("RandomShortcut") ps.setInitialConfig (q_init) ps.addGoalConfig (q_goal) from hpp.corbaserver.affordance.affordance import AffordanceTool afftool = AffordanceTool () afftool.setAffordanceConfig('Support', [0.2, 0.03, 0.00005]) afftool.setAffordanceConfig('Lean', [0.5, 0.05, 0.00005]) afftool.loadObstacleModel (packageName, "polaris", "planning", r) #~ afftool.analyseAll() #~ afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5]) #~ afftool.visualiseAffordances('Lean', r, [1, 0, 0]) ps.client.problem.selectConFigurationShooter("RbprmShooter") ps.client.problem.selectPathValidation("RbprmPathValidation",0.01) t = ps.solve () print t; if isinstance(t, list): t = t[0]* 3600000 + t[1] * 60000 + t[2] * 1000 + t[3] f = open('log.txt', 'a') f.write("path computation " + str(t) + "\n")