abstract_path_planner.py 9.1 KB
 Pierre Fernbach committed Feb 27, 2020 1 2 3 4 5 6 ``````from abc import abstractmethod from hpp.gepetto import ViewerFactory, PathPlayer from hpp.corbaserver.affordance.affordance import AffordanceTool from hpp.corbaserver import ProblemSolver import time `````` Pierre Fernbach committed Mar 06, 2020 7 `````` `````` Pierre Fernbach committed Feb 27, 2020 8 9 ``````class AbstractPathPlanner: `````` Pierre Fernbach committed Mar 06, 2020 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 `````` 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): """ `````` Pierre Fernbach committed Feb 27, 2020 36 37 `````` Build an rbprmBuilder instance for the correct robot and initialize it's extra config size """ `````` Pierre Fernbach committed Mar 06, 2020 38 `````` pass `````` Pierre Fernbach committed Feb 27, 2020 39 `````` `````` Pierre Fernbach committed Mar 06, 2020 40 41 42 43 44 45 `````` 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 `````` Pierre Fernbach committed Mar 03, 2020 46 `````` `````` Pierre Fernbach committed Mar 06, 2020 47 48 49 50 51 52 `````` def compute_extra_config_bounds(self): # 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 ] `````` Pierre Fernbach committed Mar 03, 2020 53 `````` `````` Pierre Fernbach committed Mar 06, 2020 54 55 `````` def set_joints_bounds(self): """ `````` Pierre Fernbach committed Feb 27, 2020 56 57 `````` Set the root translation and rotation bounds as well as the the extra dofs bounds """ `````` Pierre Fernbach committed Mar 06, 2020 58 59 60 `````` 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) `````` Pierre Fernbach committed Feb 27, 2020 61 `````` `````` Pierre Fernbach committed Mar 06, 2020 62 63 `````` def set_rom_filters(self): """ `````` Pierre Fernbach committed Feb 27, 2020 64 65 66 `````` 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 """ `````` Pierre Fernbach committed Mar 06, 2020 67 68 69 `````` self.rbprmBuilder.setFilter(self.used_limbs) for limb in self.used_limbs: self.rbprmBuilder.setAffordanceFilter(limb, ['Support']) `````` Pierre Fernbach committed Feb 27, 2020 70 `````` `````` Pierre Fernbach committed Mar 06, 2020 71 72 `````` def init_problem(self): """ `````` Pierre Fernbach committed Feb 27, 2020 73 74 75 76 `````` 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 """ `````` Pierre Fernbach committed Mar 06, 2020 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 `````` 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=[]): """ `````` Pierre Fernbach committed Feb 27, 2020 98 99 100 `````` 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) `````` Pierre Fernbach committed Mar 03, 2020 101 102 `````` :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) `````` Pierre Fernbach committed Feb 27, 2020 103 104 `````` :param visualize_affordances: list of affordances type to visualize, default to none """ `````` Pierre Fernbach committed Mar 06, 2020 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 `````` vf = ViewerFactory(self.ps) self.afftool = AffordanceTool() self.afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005]) self.afftool.loadObstacleModel(env_package, env_name, "planning", vf, reduceSizes=reduce_sizes) try: self.v = vf.createViewer(displayArrows=True) except Exception: print("No viewer started !") class FakeViewer(): def __init__(self): return def __call__(self, q): return def addLandmark(self, a, b): return self.v = FakeViewer() try: self.pp = PathPlayer(self.v) except Exception: pass 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): """ `````` Pierre Fernbach committed Feb 27, 2020 134 135 `````` Select the rbprm methods, and the kinodynamic ones if required :param kinodynamic: if True, also select the kinodynamic methods `````` Pierre Fernbach committed Mar 04, 2020 136 `````` :param optimize: if True, add randomShortcut path optimizer (or randomShortcutDynamic if kinodynamic is also True) `````` Pierre Fernbach committed Feb 27, 2020 137 `````` """ `````` Pierre Fernbach committed Mar 06, 2020 138 139 140 141 142 143 144 145 146 147 148 149 150 151 `````` 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): """ `````` Pierre Fernbach committed Feb 27, 2020 152 153 154 `````` Solve the path planning problem. q_init and q_goal must have been defined before calling this method """ `````` Pierre Fernbach committed Mar 06, 2020 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 `````` 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 run(self): self.init_problem() self.init_viewer() self.init_planner() self.solve() def display_path(self, path_id=-1, dt=0.1): """ `````` Pierre Fernbach committed Feb 27, 2020 173 174 175 176 `````` 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) """ `````` Pierre Fernbach committed Mar 06, 2020 177 178 179 180 181 `````` 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) `````` Pierre Fernbach committed Feb 27, 2020 182 `````` `````` Pierre Fernbach committed Mar 06, 2020 183 184 `````` def play_path(self, path_id=-1, dt=0.01): """ `````` Pierre Fernbach committed Feb 27, 2020 185 186 187 188 `````` 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) """ `````` Pierre Fernbach committed Mar 06, 2020 189 190 191 192 193 194 195 196 197 `````` 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): """ `````` Pierre Fernbach committed Mar 05, 2020 198 `````` Remove the current robot from the display `````` Pierre Fernbach committed Feb 27, 2020 199 `````` """ `````` Pierre Fernbach committed Mar 06, 2020 200 `````` self.v.client.gui.setVisibility(self.robot_node_name, "OFF") `````` Pierre Fernbach committed Mar 05, 2020 201 `````` `````` Pierre Fernbach committed Mar 06, 2020 202 203 `````` def show_rom(self): """ `````` Pierre Fernbach committed Mar 05, 2020 204 205 `````` Add the current robot to the display """ `````` Pierre Fernbach committed Mar 06, 2020 206 `````` self.v.client.gui.setVisibility(self.robot_node_name, "ON") `````` Pierre Fernbach committed Feb 27, 2020 207 `````` `````` Pierre Fernbach committed Mar 06, 2020 208 209 210 `````` @abstractmethod def run(self): """ `````` Pierre Fernbach committed Feb 27, 2020 211 212 `````` Must be defined in the child class to run all the methods with the correct arguments. """ `````` Pierre Fernbach committed Mar 06, 2020 213 214 `````` # example of definition: """ `````` Pierre Fernbach committed Feb 27, 2020 215 216 217 218 219 220 221 222 223 224 225 `````` self.init_problem() # define initial and goal position self.q_init[0:2] = [0, 0] self.q_goal[0:2] = [1, 0] self.init_viewer("multicontact/ground", visualize_affordances=["Support"]) self.init_planner() self.solve() self.display_path() self.play_path() """ `````` Pierre Fernbach committed Mar 06, 2020 226 `` pass``