1 # 5) Look ahead (aka motion planning)
5 The objective of this work is to introduce some key algorithm of motion
6 planning: collision checking, probabilistic roadmaps, visibility PRM,
7 \f$A^*\f$ (a-star), random shortcut.
12 A robot model with simple (static actuated) dynamics,
13 like a UR5 manipulator robot. See Lab 1 for loading the UR5 robot.
16 A collision checking library, provided by Pinocchio.
18 Pinocchio provides collision checking for a large class of 3D objects
19 (sphere, capsule, box, triangle soup) using library FCL. Any 3D object
20 of FCL can be loaded using the C++ interface or the URDF model.
21 Although, only capsules can yet be added through the Python API, and
22 only URDF-loaded meshes and Python-loaded capsules can be easily
23 connected with Gepetto viewer (help is welcome to correct this, it would
26 An example of loading a capsule in Pinocchio and Gepetto viewer is
29 obs = se3.GeometryObject.CreateCapsule(rad, length) # Pinocchio obstacle object
30 obs.name = "obs" # Set object name
31 obs.parentJoint = 0 # Set object parent = 0 = universe
32 obs.placement = se3.SE3(rotate('x', .1) * rotate('y', .1) * rotate('z', .1), np.matrix([.1, .1, .1]).T) # Set object placement wrt parent
33 robot.collision_model.addGeometryObject(obs, robot.model, False) # Add object to collision model
34 robot.visual_model.addGeometryObject(obs, robot.model, False) # Add object to visual model
35 # Also create a geometric object in gepetto viewer, with according name.
36 robot.viewer.gui.addCapsule("world/pinocchio/" + obs.name, rad, length, [1.0, 0.2, 0.2, 1.0])
39 URDF specifications does not allow to define which collision pairs
40 should be tested. By default, Pinocchio does not load any collision
41 pair. A simple strategy is to add all pairs, but often, some meshes of
42 the models induce wrong collision. Then manually remove them by testing
43 valid configurations. To be clean, you can store the valid collision
44 pair in a SRDF file. For UR5:
47 robot.collision_model.addAllCollisionPairs()
48 for idx in [56, 35, 23]:
49 robot.collision_model.removeCollisionPair(robot.collision_model.collisionPairs[idx])
52 Collision checking are done through the following algorithms:
55 se3.updateGeometryPlacements(robot.model, robot.data, robot.collision_model, robot.collision_data, q)
56 se3.computeCollision(robot.collision_model, robot.collision_data, pairId)
57 se3.computeCollisions(robot.collision_model, robot.collision_data, False)
58 # last arg to stop early.
61 Both collision algorithms requires a
62 preliminary update of placement and return `True` if configuration is in
63 collision (`False` otherwise).
65 ## 5.1) Testing collision
67 We need to define a simple function to check whether a configuration is
68 respecting the robot constraints (joint limits and collision, plus any
69 other inequality-defined constraints you might want).
73 Implement the function `check` taking a configuration `q` in
74 argument and return `True` if and only if `q` is acceptable -- The solution
75 only uses the 2 collision algorithms of Pinocchio listed above and
76 standard python otherwise.
78 ## 5.2) Steering method
80 We need to define a local controller, aka a steering method, to define
81 the behavior of the robot when it tries to connect to configuration
82 together. Here we will simply use linear interpolation. More complex
83 controllers (like optimal trajectories) might be preferred if more
84 knowledge about the system is available.
86 In the meantime, we will also need a method to check whether a local
87 path is collision free. We will do that be simply regularly sampling the
88 path with a given step length. Continuous collision detection might be
89 implemented for better and safer performances.
91 Here we propose to implement the steering method and the path validation
92 in a single connected method. More versatile implementation is obtained
93 by defining two different functions.
96 Implement a `connect` function, that takes as argument an
97 initial `q1` and a final `q2` configuration, and return `True` is it is
98 possible to connect both using linear interpolation while avoiding
99 collision. Optionally, the function should also returns the sampling of
100 the path as a list of intermediate configurations -- The solution does
101 not need any new Pinocchio calls.
103 ## 5.3) Nearest neighbors
105 Finally, we need a k-nearest-neighbors algorithms.
109 Implement a function `nearest_neighbors` that takes as
110 argument a new configuration `q`, a list of candidates `qs`, and the number
111 of requested neighbors `k`, and returns the list of the `k` nearest
112 neighbors of `q` in `qs`. Optionally, the distance function that scores how
113 close a configuration `q1` is close to a configuration `q2` might be also
114 provided. If `qs` contains less that `k` elements, simply returns them all
115 -- no new Pinocchio method is needed for the solution.
117 ## 5.2) Probabilistic roadmap
119 Basically, probabilistic roadmaps are build by maintaining a graph of
120 configurations. At each iteration, a new configuration is randomly
121 sampled and connected to the nearest configurations already in the
122 graph. The algorithm stops when both start configuration qstart and goal
123 configuration qgoal can be directly connected to some elements of the
126 We propose here to implement the visibility PRM algorithm. This
127 algorithm also maintains the connected components of the graph. When a
128 new configuration qnew is sampled, we try to connect it to its nearest
129 neighbor in each of the already-sampled connected component.
130 Configuration qnew is added to the graph only if one of the two
131 following conditions is respected:
133 - if qnew cannot be connected to any existing connected component
134 - or if it can be connected to at least two connected component.
136 In the second case, the connected components that can be connected are
139 A graph structure with connected components is
140 [provided here](graph_8py_source.html).
143 Implement a `visibilityPRM` that takes in argument two
144 start and goal configurations `qstart` and `qgoal`, and the number of random
145 sampling that must be achieved before failing. The returns `True` if `qgoal`
146 can be connected to `qstart`. The graph must also be returned -- no fancy
147 Pinocchio algorithm is needed here.
149 The PRM can be visualized in Gepetto-viewer using the function
150 `display_prm` [provided here](prm__display_8py_source.html).
152 ## 5.4) Searching a path in the roadmap
154 \f$A^*\f$ is an algorithm to find the shortest path in a graph (discrete
155 problem). \f$A^*\f$ iterativelly explore the nodes of the graph starting for
156 the given start. One a node gets explored, its exact cost from start
157 (cost to here) is exactly known. Then, all its children are added to the
158 "frontier" of the set of already-explored nodes. Cost from nodes of the
159 frontier to the goal is not (yet) exactly known but is anticipated
160 through a heuristic function (also provided). At next iteration, the
161 algorithm examines a node of the frontier, looking first at the node
162 that is the most likely to be in the shortest path using the heuristic
165 See the fairly complete description of \f$A^*\f$
166 [provided here](http://theory.stanford.edu/~amitp/GameProgramming/AStarComparison.html#the-a-star-algorithm).
169 Implement the \f$A^*\f$ algorithm. The \f$A^*\f$ returns a sequence of
170 node ID from start to goal. We only work here with the existing graph of
171 configuration, meaning that no new nodes a sampled, and no new collision
172 test are computed. Pinocchio is not necessary here. \f$A^*\f$ returns a list
173 containing the indexes of the nodes of the PRM graph that one should
174 cross to reach qgoal from qstart.
178 Being given a list of configurations, a random shortcut simply randomly
179 tries to shortcut some subpart of the list: it randomly selects the
180 start and end of the subpart, and tries to directly connect them (using
181 the above-defined steering method). In case of success, it skips the
182 unnecessary subpart. The algorithm iterates a given number of time.
184 The shortcut can be run on either the list of configuration output by
185 the \f$A^*\f$, or on a sampling of the trajectory connecting the nodes of the
186 \f$A^*\f$. We propose here the second version.
189 Defines a function `sample_path` to uniformly sample that
190 trajectory connecting the nodes selected by \f$A^*\f$: for each edge of the \f$A^*\f$
191 optimal sequence, call `connect` and add the resulting sampling to the
192 previously-computed sample. It takes as argument the PRM graph and the
193 list of indexes computed by \f$A^*\f$ and returns a list of robot
194 configuration starting by qstart and ending by qgoal -- no Pinocchio
195 method is needed here.
197 The sampled path can be displayed in Gepetto-viewer using the function
198 `displayPath` [provided here](prm__display_8py_source.html).
201 Implement the `shortcut` algorithm that tries to randomly
202 connect two configuration of the sampled path. It takes the list of
203 configuration output by `sample_path` and the number of random shortcut
204 it should tries. It returns an optimized list of configurations -- no
205 Pinocchio method is needed here.