Home research People General Info Seminars Resources Intranet
| Algorithms & Applcations Group | Home | Research | Publications | People | Resources | News
Algorithms & Applications Group
Motion Planning for Constrained Systems

Project Personnel:Shawna Thomas,Nancy Amato

Constrained motion planning problems are problems where a set of constraints are placed on the motion of a robot. These constraints could require that parts of the robot such as the end effector occupy a specified subset of the workspace. They could also require that certain joints of the robot remain in contact with each other (e.g., closed chains). Motion planning with constraints has many applications, including the Stewart Platform, parallel robots, closed molecular chains, animation, reconfigurable robots, and grasping for single and multiple robots.

Constrained problems are particularly challenging because they require that the robot satisfy a problems constraints in addition to an validity conditions associated with the problem. In many problems these constraints require that a part of the robot such as its end effector be located at a single point or along a manifold. In these problems traditional motion planning methods will fail because probability of generating a sample which satisfies these constraints is zero. We address this problem by developing a set of specialized sampling methods that can generate samples which satisfy constraints. These samplers can be used in combination with existing motion planning methods to solve problems with constraints.

In our initial work, we proposed a two-stage kinematics-based probabilistic roadmap (KBPRM) to sample closed configurations using inverse kinematics. We then augmented this method to handle higher DOF problems by iteratively relaxing constraints (IRC) . Later we proposed a reachable-distance based prepresentation of the constrained systems, which enables us to sample directly in the subspace subspace where constraints are satisfied. In our most recent work we proposed reachable-volumes which computes the region of space that joints of a robot can occupy while satisfying constraints, then places the joints in these regions to generate samples that satisfy the constraints. 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.

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.

Movie: a 2-loop linkage (mpg 1.3MB)

The example linkage has two kinematics loops and there are 12 links in total. The movie shows rigid body movement between configurations with same closure type and transformation between closed configurations without translating the linkage.

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.

Movie: Luxo (mpg 1.1MB)

Luxo is lamp made up by a 3-loop linkage and an adjustable head. Our planner can find the path to adjust the lamp from a given start posture to a given end posture.

Movie: a 98-link linkage (mpg 1.1MB)

A single loop with 98 links. The movie shows the transformation between two configurations.

See IRC project webpage for more information about Iterative Relaxation of Constraints (links to Dr. Bayazit's web page).

Instead of randomly sampling in the joint angle space to find closed configurations, we overcome this challenge by precomputing the subspace where closed constraints are satisfied and then directly sampling in this subspace. We represent 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.

A grasping experiment where 2 articulated arms collaborate to grasp an object and transport it from one end of the environment to the other. Each arm is composed of 3 links with variable lengths.

A fixed-base 20 link closed chain in an environment without obstacles. The links have variable lengths. The chain (red) travels from the start (green) to the goal (orange).

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.

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.


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 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:

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:

Initial joint placements.
Step first joint.
Update reachable volumes of neighboring joints.
Step neighboring joints.
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 f 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
Tunnel
Grid
WAM clutter (W-CL)
Rods
Wheeled grasper (Wh-gr)
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
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).

Sampler Success Rate
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
Connection Time

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.
Step first joint.
Update reachable volumes of neighboring joints.
Step neighboring joints.
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:

L-Tunnel (l-tun)
Rods (r)
S-Tunnel (st)
Walls (w)
Maze (m)
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

Sampling Based Motion Planning with Reachable Volumes, Troy McMahon, Ph.D. Thesis, Department of Computer Science and Engineering, Texas A&M University, College Station, TX, Aug 2016.
Ph.D. Thesis(ps, pdf, abstract)

Reachable Volume RRT, Troy McMahon, Shawna L. Thomas, Nancy M. Amato, In Proc. IEEE Int. Conf. Robot. Autom. (ICRA), pp. 2977-2984, Seattle, Washington, May 2015.
Proceedings(ps, pdf, abstract)

Sampling Based Motion Planning with Reachable Volumes: Application to Manipulators and Closed Chain Systems, Troy McMahon, Shawna L. Thomas, Nancy M. Amato, In Proc. IEEE Int. Conf. Intel. Rob. Syst. (IROS), Sep 2014.
Proceedings(pdf, abstract)

Sampling-Based Motion Planning with Reachable Volumes: Theoretical Foundations, Troy McMahon, Shawna L. Thomas, Nancy M. Amato, In Proc. IEEE Int. Conf. Robot. Autom. (ICRA), Hong Kong, China, May 2014.
Proceedings(pdf, abstract)

Sampling Based Motion Planning with Reachable Volumes, Troy McMahon, Shawna L. Thomas, Nancy M. Amato, Technical Report, TR14-003, Parasol Laboratory, Department of Computer Science, Texas A&M University, (replaces TR13-008), Feb 2014.
Technical Report(pdf, abstract)

Reachable Distance Space: Efficient Sampling-Based Planning for Spatially Constrained Systems, Xinyu Tang, Shawna Thomas, Philip Coleman, Nancy M. Amato, International Journal of Robotics Research, 29(7):916-934, Jun 2010.
Journal(pdf, abstract)

Planning with Reachable Distances, Xinyu Tang, Shawna Thomas, Nancy M. Amato, In Proc. Int. Wkshp. on Alg. Found. of Rob. (WAFR), Guanajuato, Mexico, Dec 2008.
Proceedings(ps, pdf, abstract)

Planning with Reachable Distances: Fast Enforcement of Closure Constraints, Xinyu Tang, Shawna Thomas, Nancy M. Amato, In Proc. IEEE Int. Conf. Robot. Autom. (ICRA), pp. 2694-2699, Rome, Italy, Apr 2007.
Proceedings(ps, pdf, abstract)

Iterative Relaxation of Constraints: A Framework for Improving Automated Motion Planning, O. Burchan Bayazit, Dawen Xie, Nancy M. Amato, In Proc. IEEE Int. Conf. Intel. Rob. Syst. (IROS), pp. 586 - 593, Edmonton, Alberta, Canada, Aug 2005.
Proceedings(ps, pdf, abstract)

A Kinematics-Based Probabilistic Roadmap Method for High DOF Closed Chain Systems, Dawen Xie, Nancy M. Amato, In Proc. IEEE Int. Conf. Robot. Autom. (ICRA), pp. 473-478, New Orleans, LA, Apr 2004. Also, Technical Report, TR03-007, Parasol Laboratory, Department of Computer Science, Texas A&M University, Nov 2003.
Proceedings(ps, pdf, abstract) Technical Report(ps, pdf, abstract)

A Kinematics-Based Probabilistic Roadmap Method for Closed Chain Systems, Li Han, Nancy M. Amato, In Proc. Int. Wkshp. on Alg. Found. of Rob. (WAFR), pp. 233-246, Hanover, NH, Mar 2000.
Proceedings(ps, pdf, abstract)

Supported by NSF, Texas Higher Education Coordinating Board

Project Alumni:Burchan Bayazit,Philip Coleman,Li Han,Troy McMahon,Xinyu Tang,Dawen Xie