abstract_path_planner.py 10.2 KB
Newer Older
1
from abc import abstractmethod
Guilhem Saurel's avatar
Guilhem Saurel committed
2
3

from hpp.corbaserver import Client, ProblemSolver, createContext, loadServerPlugin
4
from hpp.corbaserver.affordance.affordance import AffordanceTool
5
from hpp.corbaserver.rbprm import Client as RbprmClient
Guilhem Saurel's avatar
Guilhem Saurel committed
6
7
from hpp.gepetto import PathPlayer, ViewerFactory

8

9
10
class AbstractPathPlanner:

11
12
13
14
15
16
17
18
    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

Guilhem Saurel's avatar
Guilhem Saurel committed
19
    def __init__(self, context=None):
20
21
22
23
        """
        Constructor
        :param context: An optional string that give a name to a corba context instance
        """
24
25
26
27
28
29
30
        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
Guilhem Saurel's avatar
Guilhem Saurel committed
31
        self.mu = 0.5  # friction coefficient between the robot and the environment
32
33
34
35
36
        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 = []
37
38
39
40
41
42
43
44
45
46
47
        self.context = context
        if context:
            createContext(context)
            loadServerPlugin(context, 'rbprm-corba.so')
            loadServerPlugin(context, 'affordance-corba.so')
            self.hpp_client = Client(context=context)
            self.hpp_client.problem.selectProblem(context)
            self.rbprm_client = RbprmClient(context=context)
        else:
            self.hpp_client = None
            self.rbprm_client = None
48
49
50
51

    @abstractmethod
    def load_rbprm(self):
        """
52
53
        Build an rbprmBuilder instance for the correct robot and initialize it's extra config size
        """
54
        pass
55

56
57
58
59
60
61
    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
62

63
    def compute_extra_config_bounds(self):
64
65
66
67
        """
        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
        """
68
        # bounds for the extradof : by default use v_max/a_max on x and y axis and 0 on z axis
Guilhem Saurel's avatar
Guilhem Saurel committed
69
70
71
72
        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
        ]
73

74
75
    def set_joints_bounds(self):
        """
76
77
        Set the root translation and rotation bounds as well as the the extra dofs bounds
        """
78
79
80
        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)
81

82
83
    def set_rom_filters(self):
        """
84
85
86
        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
        """
87
88
89
        self.rbprmBuilder.setFilter(self.used_limbs)
        for limb in self.used_limbs:
            self.rbprmBuilder.setAffordanceFilter(limb, ['Support'])
90

91
92
    def init_problem(self):
        """
93
94
95
96
        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
        """
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
        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=[]):
        """
118
119
120
121
122
123
124
        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
        """
125
        vf = ViewerFactory(self.ps)
126
127
        if self.context:
            self.afftool = AffordanceTool(context=self.context)
Guilhem Saurel's avatar
Guilhem Saurel committed
128
129
            self.afftool.client.affordance.affordance.resetAffordanceConfig(
            )  # FIXME: this should be called in afftool constructor
130
131
        else:
            self.afftool = AffordanceTool()
132
        self.afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005])
Guilhem Saurel's avatar
Guilhem Saurel committed
133
134
135
136
        self.afftool.loadObstacleModel("package://" + env_package + "/urdf/" + env_name + ".urdf",
                                       "planning",
                                       vf,
                                       reduceSizes=reduce_sizes)
137

Guilhem Saurel's avatar
Guilhem Saurel committed
138
        self.v = vf.createViewer(ghost=True, displayArrows=True)
139
140
141
        self.pp = PathPlayer(self.v)
        for aff_type in visualize_affordances:
            self.afftool.visualiseAffordances(aff_type, self.v, self.v.color.lightBrown)
142
143
144

    def init_planner(self, kinodynamic=True, optimize=True):
        """
145
146
147
148
        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)
        """
149
150
151
152
153
154
155
156
157
158
159
160
        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")

Guilhem Saurel's avatar
Guilhem Saurel committed
161
    def solve(self, display_roadmap=False):
162
        """
163
164
165
        Solve the path planning problem.
        q_init and q_goal must have been defined before calling this method
        """
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)
173
174
175
176
        if display_roadmap and self.v.client.gui.getNodeList() is not None:
            t = self.v.solveAndDisplay("roadmap", 5, 0.001)
        else:
            t = self.ps.solve()
177
178
179
180
        print("Guide planning time : ", t)

    def display_path(self, path_id=-1, dt=0.1):
        """
181
182
183
184
        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)
        """
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.displayVelocityPath(path_id)
190

191
192
    def play_path(self, path_id=-1, dt=0.01):
        """
193
194
195
196
        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)
        """
197
198
199
200
201
202
203
204
205
        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):
        """
206
207
        Remove the current robot from the display
        """
208
        self.v.client.gui.setVisibility(self.robot_node_name, "OFF")
209

210
211
    def show_rom(self):
        """
212
213
        Add the current robot to the display
        """
214
        self.v.client.gui.setVisibility(self.robot_node_name, "ON")
215

216
217
218
    @abstractmethod
    def run(self):
        """
219
220
        Must be defined in the child class to run all the methods with the correct arguments.
        """
221
222
        # example of definition:
        """
223
224
225
226
        self.init_problem()
        # define initial and goal position
        self.q_init[:2] = [0, 0]
        self.q_goal[:2] = [1, 0]
Guilhem Saurel's avatar
Guilhem Saurel committed
227

228
229
230
231
232
233
        self.init_viewer("multicontact/ground", visualize_affordances=["Support"])
        self.init_planner()
        self.solve()
        self.display_path()
        self.play_path()
        """
234
        pass