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