5) Look ahead (aka motion planning)

Objective

The objective of this work is to introduce some key algorithm of motion planning: collision checking, probabilistic roadmaps, visibility PRM, \f$A^*\f$ (a-star), random shortcut.

5.0) prerequisites

Prerequisite 1

A robot model with simple (static actuated) dynamics, like a UR5 manipulator robot. See Lab 1 for loading the UR5 robot.

Prerequisite 2

A collision checking library, provided by Pinocchio.

Pinocchio provides collision checking for a large class of 3D objects (sphere, capsule, box, triangle soup) using library FCL. Any 3D object of FCL can be loaded using the C++ interface or the URDF model. Although, only capsules can yet be added through the Python API, and only URDF-loaded meshes and Python-loaded capsules can be easily connected with Gepetto viewer (help is welcome to correct this, it would requires 4h work).

An example of loading a capsule in Pinocchio and Gepetto viewer is below:

obs = se3.GeometryObject.CreateCapsule(rad, length)  # Pinocchio obstacle object
obs.name = "obs"                                     # Set object name
obs.parentJoint = 0                                  # Set object parent = 0 = universe
obs.placement = se3.SE3(rotate('x', .1) * rotate('y', .1) * rotate('z', .1), np.array([.1, .1, .1]))  # Set object placement wrt parent
robot.collision_model.addGeometryObject(obs, robot.model, False) # Add object to collision model
robot.visual_model.addGeometryObject(obs, robot.model, False)    # Add object to visual model
# Also create a geometric object in gepetto viewer, with according name.
robot.viewer.gui.addCapsule("world/pinocchio/" + obs.name, rad, length, [1.0, 0.2, 0.2, 1.0])

URDF specifications does not allow to define which collision pairs should be tested. By default, Pinocchio does not load any collision pair. A simple strategy is to add all pairs, but often, some meshes of the models induce wrong collision. Then manually remove them by testing valid configurations. To be clean, you can store the valid collision pair in a SRDF file. For UR5:

robot.collision_model.addAllCollisionPairs()
for idx in [56, 35, 23]:
    robot.collision_model.removeCollisionPair(robot.collision_model.collisionPairs[idx])

Collision checking are done through the following algorithms:

se3.updateGeometryPlacements(robot.model, robot.data, robot.collision_model, robot.collision_data, q)
se3.computeCollision(robot.collision_model, robot.collision_data, pairId)
se3.computeCollisions(robot.collision_model, robot.collision_data, False)
# last arg to stop early.

Both collision algorithms requires a preliminary update of placement and return True if configuration is in collision (False otherwise).

5.1) Testing collision

We need to define a simple function to check whether a configuration is respecting the robot constraints (joint limits and collision, plus any other inequality-defined constraints you might want).

Question 1

Implement the function check taking a configuration q in argument and return True if and only if q is acceptable – The solution only uses the 2 collision algorithms of Pinocchio listed above and standard python otherwise.

5.2) Steering method

We need to define a local controller, aka a steering method, to define the behavior of the robot when it tries to connect to configuration together. Here we will simply use linear interpolation. More complex controllers (like optimal trajectories) might be preferred if more knowledge about the system is available.

In the meantime, we will also need a method to check whether a local path is collision free. We will do that be simply regularly sampling the path with a given step length. Continuous collision detection might be implemented for better and safer performances.

Here we propose to implement the steering method and the path validation in a single connected method. More versatile implementation is obtained by defining two different functions.

Question 2

Implement a connect function, that takes as argument an initial q1 and a final q2 configuration, and return True is it is possible to connect both using linear interpolation while avoiding collision. Optionally, the function should also returns the sampling of the path as a list of intermediate configurations – The solution does not need any new Pinocchio calls.

5.3) Nearest neighbors

Finally, we need a k-nearest-neighbors algorithms.

Question 3

Implement a function nearest_neighbors that takes as argument a new configuration q, a list of candidates qs, and the number of requested neighbors k, and returns the list of the k nearest neighbors of q in qs. Optionally, the distance function that scores how close a configuration q1 is close to a configuration q2 might be also provided. If qs contains less that k elements, simply returns them all – no new Pinocchio method is needed for the solution.

5.2) Probabilistic roadmap

Basically, probabilistic roadmaps are build by maintaining a graph of configurations. At each iteration, a new configuration is randomly sampled and connected to the nearest configurations already in the graph. The algorithm stops when both start configuration qstart and goal configuration qgoal can be directly connected to some elements of the graph.

We propose here to implement the visibility PRM algorithm. This algorithm also maintains the connected components of the graph. When a new configuration qnew is sampled, we try to connect it to its nearest neighbor in each of the already-sampled connected component. Configuration qnew is added to the graph only if one of the two following conditions is respected:

  • if qnew cannot be connected to any existing connected component

  • or if it can be connected to at least two connected component.

In the second case, the connected components that can be connected are also merged.

A graph structure with connected components is provided here.

Question 4

Implement a visibilityPRM that takes in argument two start and goal configurations qstart and qgoal, and the number of random sampling that must be achieved before failing. The returns True if qgoal can be connected to qstart. The graph must also be returned – no fancy Pinocchio algorithm is needed here.

The PRM can be visualized in Gepetto-viewer using the function display_prm provided here.

5.4) Searching a path in the roadmap

\f$A^\f$ is an algorithm to find the shortest path in a graph (discrete problem). \f$A^\f$ iterativelly explore the nodes of the graph starting for the given start. One a node gets explored, its exact cost from start (cost to here) is exactly known. Then, all its children are added to the “frontier” of the set of already-explored nodes. Cost from nodes of the frontier to the goal is not (yet) exactly known but is anticipated through a heuristic function (also provided). At next iteration, the algorithm examines a node of the frontier, looking first at the node that is the most likely to be in the shortest path using the heuristic distance.

See the fairly complete description of \f$A^*\f$ provided here.

Question 5

Implement the \f$A^\f$ algorithm. The \f$A^\f$ returns a sequence of node ID from start to goal. We only work here with the existing graph of configuration, meaning that no new nodes a sampled, and no new collision test are computed. Pinocchio is not necessary here. \f$A^*\f$ returns a list containing the indexes of the nodes of the PRM graph that one should cross to reach qgoal from qstart.

5.4) Shortcut

Being given a list of configurations, a random shortcut simply randomly tries to shortcut some subpart of the list: it randomly selects the start and end of the subpart, and tries to directly connect them (using the above-defined steering method). In case of success, it skips the unnecessary subpart. The algorithm iterates a given number of time.

The shortcut can be run on either the list of configuration output by the \f$A^\f$, or on a sampling of the trajectory connecting the nodes of the \f$A^\f$. We propose here the second version.

Question 6

Defines a function sample_path to uniformly sample that trajectory connecting the nodes selected by \f$A^\f$: for each edge of the \f$A^\f$ optimal sequence, call connect and add the resulting sampling to the previously-computed sample. It takes as argument the PRM graph and the list of indexes computed by \f$A^*\f$ and returns a list of robot configuration starting by qstart and ending by qgoal – no Pinocchio method is needed here.

The sampled path can be displayed in Gepetto-viewer using the function displayPath provided here.

Question 7

Implement the shortcut algorithm that tries to randomly connect two configuration of the sampled path. It takes the list of configuration output by sample_path and the number of random shortcut it should tries. It returns an optimized list of configurations – no Pinocchio method is needed here.