abstract_path_planner.py 8.49 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
from abc import abstractmethod
from hpp.gepetto import ViewerFactory, PathPlayer
from hpp.corbaserver.affordance.affordance import AffordanceTool
from hpp.corbaserver import ProblemSolver
import time

class AbstractPathPlanner:

  rbprmBuilder = None
  ps = None
  v = None
  afftool = None
  pp = None
14
  extra_dof_bounds = None
15
  robot_node_name = None # name of the robot in the node list of the viewer
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37

  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

38
39
40
41
42
43
44
45
46
47
48
49
  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):
    # 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]

50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
  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()
74
75
    self.set_configurations()
    self.compute_extra_config_bounds()
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
    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)

92
  def init_viewer(self, env_name, env_package = "hpp_environments",reduce_sizes = [0, 0, 0], visualize_affordances = []):
93
94
95
96
    """
    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)
97
98
    :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)
99
100
101
102
103
    :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])
104
    self.afftool.loadObstacleModel (env_package, env_name, "planning", vf, reduceSizes = reduce_sizes)
105
106
107
108
109
110
111
112
113
114
115
116
    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()
117
118
119
120
    try:
      self.pp = PathPlayer(self.v)
    except Exception:
      pass
121
122
123
124
125
    for aff_type in visualize_affordances:
      self.afftool.visualiseAffordances(aff_type, self.v, self.v.color.lightBrown)



126
  def init_planner(self, kinodynamic = True, optimize = True):
127
128
129
    """
    Select the rbprm methods, and the kinodynamic ones if required
    :param kinodynamic: if True, also select the kinodynamic methods
130
    :param optimize: if True, add randomShortcut path optimizer (or randomShortcutDynamic if kinodynamic is also True)
131
132
133
134
135
136
137
    """
    self.ps.selectConfigurationShooter("RbprmShooter")
    self.ps.selectPathValidation("RbprmPathValidation", 0.05)
    if kinodynamic:
      self.ps.selectSteeringMethod("RBPRMKinodynamic")
      self.ps.selectDistance("Kinodynamic")
      self.ps.selectPathPlanner("DynamicPlanner")
138
139
140
141
142
    if optimize:
      if kinodynamic:
        self.ps.addPathOptimizer("RandomShortcutDynamic")
      else:
        self.ps.addPathOptimizer("RandomShortcut")
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171


  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 run(self):
    self.init_problem()
    self.init_viewer()
    self.init_planner()
    self.solve()

  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)
    """
172
173
174
175
176
    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)
177
178
179
180
181
182
183

  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)
    """
184
    self.show_rom()
185
186
187
188
189
    if self.pp is not None:
      if path_id < 0:
        path_id = self.ps.numberPaths()-1
      self.pp.dt = dt
      self.pp(path_id)
190
191
192

  def hide_rom(self):
    """
193
    Remove the current robot from the display
194
    """
195
196
197
198
199
200
201
202
    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")

203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222

  @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[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()
    """
    pass