Overview

Sometimes motion planning problems include constraints such as the grippers must maintain contact with an object being manipulated (closure constraints) or that parts of the robot must (or cannot) occupy certain subsets of the workspace. Such problems are particularly challenging for sampling-based methods since it is difficult to generate samples that satisfy these constraints. In a series of results, we have developed specialized sampling-based planning methods that can efficiently generate samples that satisfy such constraints.

KBPRM - Kinematics-Based PRM

In our initial work, we proposed a two-stage strategy to plan for problems that have closed chain systems, e.g., a two-armed manipulator carrying an object. We next extended the KBPRM method to handle higher DOF problems by iteratively relaxing constraints (IRC).

Reachable Distances

In our next method, instead of randomly sampling in the joint angle space to find constraint satisfying configurations (e.g., closed chains), we precomputed the subspace where the constraints are satisfied and then directly sampled in this subspace. In this way, only the feasible space is sampled, which can greatly increase the probabiltity of obtaining a valid sample and increase efficiency significantly. This was a breakthrough approach, providing the first efficient sampling-based planning approach for arbitrary DOF closed chain systems.

Reachable Volume Sampling

In our next method, we proposed computing reachable volumes, the region of space that joints of a robot can occupy while satisfying constraints, and then restricting sampling to that region. Reachable volumes generalizes reachable distance to applicable to robots with 3-dimensional spherical joints as well as to robots with combinations of spherical, planar and prismatic joints.

Reachable Volumes: RV-Space for Constraint-Satisfying Planning

We introduce a new concept, called reachable volumes, that are a geometric representation of the regions the joints and end effectors of a robot can reach, and use it to define a new planning space called RV-space where all points automatically satisfy robot-specific constraints. Constraints include joint closure constraints and constraints on the position of individual joints (including the end effector).

Samples and paths generated in RV-space naturally conform to constraints, making planning for constrained systems no more difficult than planning for unconstrained systems. Consequently, constrained motion planning problmes that were previously difficult or unsolvable become manageable and in many cases trivial.

This method is applicable to high dof linkages, closed chains and tree-like robots with spherical, prismatic and planar articulated joints (and combinations thereof). Reachable volumes can be used to guide robot design and assist in robot operation by allowing the operator to see what portions of the robot can reach.

Reachable Volumes Illustrations

Reachable Volumes can handle both unconstrained and constrained systems that include a variety of joint types. In this video we define reachable volumes and show how they are computed. We also show visualizations of the reachable volumes for a number of example robots.

Movie: Sampling Based Motion Planning with Reachable Volumes: Theoretical Foundations

RV-space encodes the intrinsic constraints of the robot. It is independent of the environment. Just as with traditional configuration space (C-space), collision checking must be preformed separately. Below, we provide visualizations of the reachable volumes for several different linkages. In each example, the linkage is displayed in red, and the reachable volume for each joint is shaded in a different color.

Example Visualizations

Reachable volumes of a 4 link chain with spherical joints: The first joint can reach anywhere on the pink shell. The second joint can reach anywhere within the yellow sphere (including the region inside the pink shell). The third joint can reach anywhere within the mint sphere. The end effector can reach anywhere inside of the green sphere.

Reachable volumes of a closed chain with 4 links of equal length: The first and third joints can reach anywhere on the green shell. The second joint can reach anywhere within the blue sphere (including the region inside of the green shell).

Reachable volumes of a 4 link fixed-base grasper with spherical joints with constraints on the end effector: The reachable volume of the base (teal) given constraints on the end effectors to grasp a cubic object (blue and green).

Reachable-Volume–Guided Sampling

We also develop a family of samplers that use reachable volumes to generate samples for a wide variety of motion planning problems including end effector constraints, closure constraints, and constraints on joint positions with as many as 256 dof. The following video summarizes how reachable volume sampling works for different example environments:

Movie: Sampling Based Motion Planning with Reachable Volumes: Application to Manipulators and Closed Chain Systems

Reachable Volume sampling iteratively places joints in their available reachable volume until all joints have been placed. Before any joints are placed, all joints have their orignial/full reachable volume. Once a joint is placed, this implies an additional constraint to the system which restricts the available reachable volume of other joints (e.g., neighboring joints). To place a joint, we first compute its updated reachable volume based on any previously placed joints. We then randomly select a joint placement from this reduced set.

For example, consider the following 6 link fixed-base grasper with constraints placed on the 2 end effectors (red and green regions in the top left image).

Generating a sample for a 6 link fixed-base grasper whose end effectors are constrained to be located in the red and green regions (top left image). Reachable volumes sampling generates samples by iteratively placing joints in their available reachable volume until all joints have been placed (bottom right image).

Sampling proceeds as follows (right to left, top to bottom in image):

  1. Sample placement for the second end effector in its reachable volume (green region).
  2. Sample placement for the joint neighboring the second end effector in is available reachable volume (dark green region).
  3. Sample placement for the first end effector in its available reachable volume (red region).
  4. Sample placement for the joint neighboring the first end effector in its available reachable volume (orange region).
  5. Sample placement for the joint at the branch in its available reachable volume (yellow region).
  6. Sample placement for the joint neighboring the base (purple region).

At each placement, the unplaced reachable volumes become restricted.

Reachable Volume Local Planning

We also present a reachable volume-based local planner. Just like the sampler, it guarantees that the local plan will satisfy constraints. The local planner works by interatively stepping joints in RV-space and updating affected available reachable volumes.

Reachable Volume Local Planning Steps

Initial joint placements
Initial joint placements
Step first joint
Step first joint
Update reachable volumes of neighboring joints
Update reachable volumes of neighboring joints
Step neighboring joints
Step neighboring joints
Final joint placements
Final joint placements

Reachable Volume Distance Metric

The reachable volume distance metric captures the distance traversed during reachable volume stepping (and local planning). It is a weighted sum of the Euclidean distance between the robot base placements in workspace and the Euclidean distance between the joints placements in RV-space.

The reachable volume distance between two samples (black and gray) is the sum of the distances between their joint placements in RV-space.

Reachable Volume Performance in Practice

We consider a variety of environments and study robots with 19 to 262 dof:

Walls environment
Walls
Tunnel environment
Tunnel
Grid environment
Grid
WAM clutter environment
WAM Clutter
Rods environment
Rods
Wheeled grasper environment
Wheeled grasper (Wh-gr)
Loop-tree environment
Loop-tree (Lp-tr)

We allow each method to attempt 2000 samples. Reachable volume sampling may be slower than other methods, but it is more successfull in generating valid samples in unconstrained systems.

Sampler success rate for 2000 samples
Sampler Success Rate
Node generation time for 2000 samples
Node Generation Time

For constrained systems (e.g., closed chains), reachable volume sampling is often the only method able to generate samples in the allotted time (20 hours).

Closed-chain sampler success rate
Sampler Success Rate
Closed-chain node generation time
Node Generation Time

We also demonstrate different combionations of local planners and distance metrics in the context of PRM planning with reachable volume samples in the walls environment. We show both a 22 dof unconstrained chain (w-22) and a 22 dof closed chain (w-cc). We compare reachable volume local planning (rv) and straightline local planning (sl). For reachable volume local planning, we use either scaled Euclidean (se) or reachable volume distance (rv) for selecting the k nearest neighbors for connection.

Number of edges
Number of Edges
Connection time
Connection Time

Reachable Volume RRT (RVRRT)

We present a novel method for stepping reachable volume samples to generate nearby samples that are also in their reachable volumes. We use this stepping method in an RV-Expand function to grow an RRT. It guarantees that the steps will satisfy constraints. It works by interatively stepping joints in RV-space and updating affected available reachable volumes:

Initial joint placements
Initial joint placements
Step first joint
Step first joint
Update reachable volumes of neighboring joints
Update reachable volumes of neighboring joints
Step neighboring joints
Step neighboring joints
Final joint placements
Final joint placements

This reachable volume RRT (RVRRT) can be applied to high dof problems that RRTs have previously been unable to solve. It can also be applied to constrained systems where it generates paths that are guaranteed to adhere to the problem’s constraints.

Reachable Volume RRT in Practice

We consider a variety of environments and study robots with 22 to 134 dof:

RVRRT L-Tunnel environment
L-Tunnel (l-tun)
RVRRT Rods environment
Rods (r)
RVRRT S-Tunnel environment
S-Tunnel (st)
RVRRT Walls environment
Walls (w)
RVRRT Maze environment
Maze (m)
RVRRT Arm Cord environment
Arm Cord (crd)

We compare RVRRT to RRT and DDRRT. The different variants of RVRRT correspond to differen input parameters (see our publications for more details). For unconstrained systems, RVRRTs require less computation time and fewer nodes to solve the query, and in some cases are capable of solving difficult problems that other methods cannot. (Each method was allowed to run a maximum of 8 hours). The running time of RVRRT is generally higher than RRT in low dof problems but comparable to RRT and DDRRT in higher dof problems.

Number of Nodes Required

Running Time

For constrained systems (e.g., closed chains), RVRRT is often the only method able to solve the problem.

Number of Nodes Required

Running Time

DRV: Directed Reachable Volumes

Reachable volumes have been highly successful in solving constrained problems and problems with many degrees of freedom, but they cannot handle rotational joints. We develop directed reachable volumes (DRVs) which reparameterizes traditional configuration space into a new planning space called DRV-space. DRV-space makes planning the motion of a Fetch robot (composed of several revolute joints) to extract binders and place them in a bin feasible.

Rotational joints often appear in industrial robots because they weigh less and are more cost effective than spherical joints. Motion planning for manipulators with rotational joints is challenging because the actuation range for each link is constrained by the placement and orientation of other links.

Examples of robots that employ revolute joints: a SCARA fixed base manipulator and a mobile KUKA Youbot.

DRV-space encodes the oriented volume of workspace that individual joints can access in the context of how other joints are placed. DRVs extend the concept of reachable volumes (RVs) to handle rotational joints in addition to spherical and prismatic joints. DRVs are also able to handle constraints on the orientation as well as the position of a robot’s joints and end-effectors. (RVs only handle positional constraints.)

DRVs (blue and yellow regions) for the Kuka Youbot’s joints.

We demonstrate DRVs in the context of sampling based motion planning to solve pick-and-place problems with the KUKA Youbot. We study 2 different versions of the manipulator: the standard 5 dof arm (K8) and a 7 dof arm with 2 additional links (K10). We also vary the pick position from the front of the cubbard (f), the middle of the cubbard (m) and the back of the cubbard (b). DRVs are able to solve all variants while uniform random sampling cannot.

Pick and place problem requires the Youbot (K8 shown) to position its arm inside a cubbard for a grasping maneuver and then move to a drop-off container.

Average time (log scale) to solve the pick-and-place problem. Starts indicate methods unable to generate samples in the allotted time.

Reachable Distance Sampling

Motion planning for closed-chain systems is particularly difficult due to additional constraints, so-called closure constraints , placed on the system. In fact, the probability of randomly selecting a set of joint angles that satisfy the closure constraints is zero. Reachable distance sampling overcomes this challenge by precomputing the subspace where the closure constraints are satisfied and then directly sampling in this subspace. This method represents the chain as a hierarchy of sub-chains by recursively breaking down the problem into smaller subproblems. Each sub-chain in the hierarchy may be partitioned into other, smaller sub-chains forming a closed loop. For any sub-chain, we can compute the attainable distance (reachable distance or length) between its two endpoints recursively. Instead of randomly sampling in the joint angle space, we recursively sample in the reachable distance space. This provides distinct advantages over traditional approaches:

  • Joint angles are quickly and easily calculated using basic trigonometry relationships instead of using more expensive inverse kinematics solvers
  • Configurations are guaranteed to satisfy closure constraints

Our method can be used to significantly improve the performance of sampling-based planners for closed-chain systems such as Probabilistic Roadmap Methods (PRM) and Rapidly-Exploring Randomized Trees. We provide the necessary motion planning primitives (namely sampling and local planning) to implement most sampling-based motion planners. Our experimental results show that our method is fast and efficient in practice making sampling configurations with closure constraints comparable to sampling open chain configurations that ignore closure constraints entirely. It is easy to implement and general. It is also extends to more distance-related constraints besides the ones demonstrated here.

KBPRM: Kinematics-Based PRM

In our initial work, we proposed a two-stage kinematics-based probabilistic roadmap (KBPRM) planner for closed chains. In the first stage, we build a (small) kinematic roadmap that deals solely with the robot’s kinematics and utilizes both forward and inverse kinematics in its construction. In the second stage, the environment is populated with copies of the kinematic roadmap, and rigid body connections are made between nodes with the same closure type. Both stages employ PRM planners to construct their roadmaps. Our experimental results indicate that the use of kinematics to guide the generation and connection of closure configurations is very beneficial, both reducing the computation costs and improving the connectivity of the resulting roadmap as compared to previous purely randomized approaches.

The efficiency of KBPRM depends critically on how the linkage is partitioned into open chains. The original method assumed the Partition was provided as input to the problem. In the extended work, we proposed a fully automated method for partitioning an arbitrary linkage into open chains and for determining which should be positioned using the inverse kinematic solver. Even so, the size (number of links) of the closed loops that can be handled by this method is limited because the inverse solver can only be applied to small chains. To handle high DOF closed loops, we show how we can use the Iterative Relaxation of Constraints (IRC) strategy to efficiently handle large loops while still only using inverse kinematics for small chains.

Updated: