Actin®  Version 5.1.0
Software for Robotics Simulation and Control
Path Planning with Actin

Abstract

Actin is a general-purpose inverse kinematics algorithm suited for almost any type of robots, including serial-chained robots, redundant robots, and multi-armed robots. Many conventional path planning methods involve searching the configuration (joint) space for a collision-free path that connects a start configuration to a goal configuration. One of the more popular single-query path planning methods is Rapidly-exploring Random Tree (RRT). Here, we combine the RRT approach with fast-converging inverse kinematics solutions of Actin to create a generalized single-query planning algorithm that can search for collision-free path to a goal location in the work (end-effector) space.

Background

The Rapidly-exploring Random Tree (RRT) was introduced in [1] as an efficient data structure and sampling scheme to quickly search high-dimensional spaces. [2] creates the RRT-Connect method by incrementally building two RRTs rooted at the start and goal configurations. Each of the two trees explores space around itself while also trying to advance towards the other tree via a greedy heuristic. [3] proposes an approach of biasing the tree exploration towards the goal in the end-effector space using a heuristic workspace metric that maps the pose of the end-effector to a scalar value representing proximity to the goal. Taking a different approach, [4] applies the Jacobian Transpose method as the biasing approach to RRT and creates the Jacobian Transpose-directed Rapidly-exploring Random Tree (JT-RRT) method that combines the configuration exploration of RRT with a workspace goal bias of JT to produce collision-free path without the need for inverse kinematics.

Rapidly-exploring Random Tree

The basic RRT construction algorithm is given in Figure 1. Each step of EXTEND operation attempts to extend the RRT towards a randomly-selected configuration and is illustrated in Figure 2. When a problem does not involve differential constraints, the need for incremental motions is less important and the path planning can be solved more quickly with a heuristic that attempts to move over a longer distance. The Connect heuristic (Figure 3) is a greedy function that iterates the EXTEND operation until q or an obstacle is reached.

PathPlanning_image1.png
The basic RRT construction algorithm [2]

PathPlanning_image2.png
The EXTEND operation [2]

PathPlanning_image3.png
The Connect heuristic [2]

In [4], the Connect algorithm is modified to work with a local planner utilizing the Jacobian Transpose approach that attempts to connect a configuration node in RRT to the goal in the workspace. It was shown that the JT-RRT algorithm, in many circumstances, performed better than a method presented in [3] in that it converged faster and succeeded more frequently with fewer numbers of node creations and collision checks. However, in the experiments presented in [4] to illustrate the effectiveness of the JT-RRT algorithm, the end-effector was only required to be within 0.15 meter of the goal for the threshold to be satisfied. The accuracy of 0.15 meter is typically not adequate for most real-world tasks.

PathPlanning_image4.png
The JT-RRT algorithm [4]

Approach

One problem with the Jacobian Transpose controller is that, even though it is fast to calculate, it converges slowly. In addition, a straightforward implementation of the Jacobian Transpose method may result in the robot oscillating around the goal position without ever reaching it. This may be why the threshold of 0.15 meter was used in the experiments with the JT-RRT algorithm.

Our approach is very similar to the JT-RRT method with minor differences in details and the obvious difference being the use of Actin controller in place of the Jacobian Transpose controller. Actin controller is based on the work in [5] and incorporates optimizations for kinematically redundant robots and validity checks for joint limits, collisions, and singularities for all types of robots.

The followings show pseudo code for the Actin-RRT algorithm. First, the tree is initialized with the starting (initial) configuration. For each iteration, a random number is drawn and if it’s less than pg, ConnectRandom is called. Otherwise, ConnectGoal is called. If ConnectGoal succeeds, it creates a path from initial configuration to the goal configuration and returns true. However, if the maximum iterations (N) is exceeded, it returns false.

PathPlanning_image5.png
Pseudo code for Actin-RRT

In ConnectRandom, first, a valid random configuration is generated. A configuration is valid if it lies within joint limits and collision-free space. Then, a configuration in the tree nearest (or near within some tolerance) to the random configuration is determined. Next, the tree is extended by a small step from the near configuration to the random configuration and the new configuration is checked for collision with NewConfig call. This step is repeated until a collision is detected or the random configuration is reached. The last collision-free configuration is added to the tree.

PathPlanning_image6.png
Pseudo code for ConnectRandom

In ConnectGoal, a configuration nearest (or near within some tolerance) to the goal is chosen. The metric to measure proximity is defined in the workspace (for example, the distance of the end-effector to the goal). Then, MoveOneStepToGoal is called repeatedly until it fails or the goal is reached. MoveOneStepToGoal uses Actin control to incrementally move the end-effector from the current position to the goal position while checking for joint limits, collision, and singularity. If MoveOneStepToGoal is successful, the new configuration is added to the tree. If the goal is reached, ConnectGoal returns true. If MoveOneStepToGoal fails, the near configuration is marked is invalid so it will not be chosen again because it would again fail.

PathPlanning_image7.png
Pseudo code for ConnectGoal

Path Optimization

As with most randomized approaches, the generated path is often jagged and typically contains many unnecessary zigzags due to the random steps taken by the planner. It needs to be smoothed and/or optimized first before it can be used by the robot. Here, the Shortcut and Adaptive Shortcut algorithms [6] have been adapted and employed to shorten the collision-free path generated by the Actin-RRT planner.

PathPlanning_image8.png
Pseudo code for Shortcut
PathPlanning_image9.png
Pseudo code for IterativeShortcut

In Shortcut, a path P = [q1, q2, …, qn] consists of collision-free straight-line paths (in configuration space) connecting successive configurations q1 to qn. P is broken up recursively into two sub-paths P1 = [q1, q2, …, qn/2] and P2 = [qn/2, qn/2+1, …, qn]. Each sub-path is checked whether it can be replaced by a simple straight-line between the start and end configurations. Shortcut is called recursively until the sub-path is a simple straight line between two configurations. Once Shortcut is completed, the path is then fed back to Shortcut again until no improvement can be achieved, i.e. no configuration can be removed. Figure 5 illustrates the effect of IterativeShortcut algorithm. Path P = [q1, q2, …, q7] is now shortened to path P’ = [q1, q4, q7] .

PathPlanning_image10.png
IterativeShortcut is applied to path P resulting in path P'

Although IterativeShortcut can eliminate many unnecessary configurations along the path, it can still leave the path far from optimized (shortest). Note in Figure 5 that while q2, q3, q5, and q6 are removed from the path, q4 still lies to far from where it needs to be if the shortest collision-free path from q1 to q7 is desired. The AdaptiveShortcut helps address this problem by scanning through the path and replacing a node (configuration) with two configurations around it. The two additional configurations ql and qr are determined by bisecting the line segments (qi-1, qi) and (qi, qi+1), respectively. If the path from ql to qr is collision-free, then qi is replaced with ql and qr. If not, we bisect again between previous ql and qi and qi and previous qr. We keep bisecting the line segments until a collision-free path between ql and qr is found or ql and qr are very close to qi. Finally, the path is then run through IterativeShortcut again to smooth out any unnecessary configuration.

PathPlanning_image11.png
Pseudo code for AdaptiveShortcut

Figure 6 shows how AdaptiveShortcut works on the earlier path. In the first bisection step, ql1 and qr1 do not result in a collision-free path. We then bisect again, in which ql2 and qr2 possess a collision-free path and replace q4. ql2 is then removed in IterativeShortcut. Note that the new path P” = [q1, qr2, q7] is shorter than P’ = [q1, q4, q7].

PathPlanning_image12.png
Path P', after one iteration through AdaptiveShortcut, becomes P''

Performance Enhancements

To help speed up Actin-RRT calculations, a few enhancements have been added to the algorithm.

Predefined Positions

The predefined positions are optional and provided to the algorithm by the user. These are positions at which the robot is supposedly collision-free. At the start of the calculation, these positions are first tested for validity and added to the tree as connected nodes if they are collision-free. This provides a non-empty initial tree that can help speed up Actin-RRT calculations. The set of predefined positions is usually dependent on the workspace within which the robot operates and therefore different for each workspace.

Solution Caches

Oftentimes, the robot is asked to move among a set of defined locations. Because RRT algorithm is classified as a one-shot path planning one, it has to compute a solution every time, even if it’s to a location that has already been computed. Depending on the complexity of the workspace and where the goal and the starting configurations are, this process can take a long time. If a solution to a goal location has already been computed, it’s natural that this solution should somehow be utilized to speed up Actin-RRT calculations.

Implementation

The Actin-RRT algorithm is implemented in C++ using the Actin toolkits and Boost Graph Library (BGL). BGL is a header-only library that provides general-purpose graph classes that conform to a generic interface. It also includes many graph algorithms such as breadth first search, Dijskstra’s shortest path, Bellman-Ford shortest path, A-star, etc. BGL is used to implement the tree structure of RRT.

Important classes for path planning with Actin
Class Description
EcRrtPathPlanner This class is the highest level interface in the pathPlanning library. It's used to calculate a collision-free path to either the desired end effector pose or joint positions. It's designed to run in its own thread while calculating a collision-free path so as not to block the execution of the thread that calls it.
EcRrtUtility This is a helper class that is used in EcRrtPathPlanner for checking for collisions and also generating random nodes. This class can be used for manipulators with no constrained joints. However, if a manipulator has special constraints, a derived class can override the EcRrtUtility::generateRandomNode method to take care of those constraints.
EcPathPlannerHelper This class represents a solution cache that contains a set of joint positions corresponding to a end effector pose and a set of previous collision-free paths to get to that position.
EcPathPlannerHelperManager This is simply a map of string keys and solution caches. After a collision-free path has been calculated, the user can put a solution cache in EcPathPlannerHelperManager with a unique string key and later if the same desired end effector pose is used, the cache for this pose can be retrieved and reused.
EcWorkerThreadPool A helper class that facilitates running path planning calculations in a separate thrad so it can be non-blocking.

The EcRrtPathPlanner class

EcRrtPathPlanner is the main user-facing class. The user configures path planning parameters, sets the current state and the goal, and then calculates a collision-free path with this class. The following table lists some important methods for

EcRrtPathPlanner Method Description
EcRrtPathPlanner::setPathCalculationParams Set the initial (current) state of the manipulator and the end effector goal.
EcRrtPathPlanner::setJointGoalPathCalculationParams Set the initial (current) state of the manipulator and the joint goal.
EcRrtPathPlanner::calculate Start the calculation. Must be called after either setPathCalculationParams or setJointGoalPathCalculationParams is called.
EcRrtPathPlanner::getResults Get the results of path planning calculation. The results contain, among other things, the collision-free path, which is a set of joint positions, and the time it took to calculate the path.

Path planning calculation normally takes a long time to run (a few seconds to minutes) and thus it is typical to run EcRrtPathPlanner::calculate method in another thread. The user can create a thread using thread commands of their choice or can use the EcWorkerThreadPool class to accomplish this. The following code snippet shows how to use EcRrtPathPlanner class both in a single thread (blocking) and in conjuction with EcWorkerThreadPool class in another thread (non-blocking).

class Callback
{
public:
Callback(EcRrtPathPlanner* pPlanner) : m_pPlanner(pPlanner) {}
void pathCalcCompletionCallback
(
EcBoolean success
)
{
if(success)
{
// get the results
m_pPlanner->getResults(results);
// do something with the results. In this case, just printing the joint positions along the path
BOOST_FOREACH(EcTreeNode const& node, results.path)
{
const EcXmlRealVector& jointPos = node.positionState().jointPositions();
for(EcSizeT ii=0; ii<jointPos.size(); ++ii)
{
std::cout << jointPos[ii] << " ";
}
std::cout << std::endl;
}
}
}
private:
EcRrtPathPlanner* m_pPlanner;
};
// load the simulation from a file or populate the simulation some other ways
// create a path planner object
planner.startCalculationThread(); // Start the calculation thread
planner.setStatedSystem(sim.statedSystem()); // Set the stated system
EcU32 manipIndex = 0;
planner.setManipulatorIndex(manipIndex); // Tell path planner that we want to plan the path for the first manipulator in the system
const EcPositionController& posController = sim.positionControlSystem().positionControllers()[manipIndex];
planner.setPositionController(posController, 0); // Set the position controller as well as telling it to use the first end effector set
const EcPositionState& initialState = sim.statedSystem().state().positionStates()[manipIndex];
// copy the actual placement as a goal then we can change the goal to wherever we want the manipulator to move to
// set the goal
planner.setPathCalculationParams(initialState, goal);
// path planning in the same thread
EcString reasonForFailure;
if(planner.calculate(reasonForFailure))
{
// get the results
planner.getResults(results);
// do something with the results. In this case, just printing the joint positions along the path
BOOST_FOREACH(EcTreeNode const& node, results.path)
{
const EcXmlRealVector& jointPos = node.positionState().jointPositions();
for(EcSizeT ii=0; ii<jointPos.size(); ++ii)
{
std::cout << jointPos[ii] << " ";
}
std::cout << std::endl;
}
}
else
{
std::cout << "Path planning failed because " << reasonForFailure << std::endl;
}
// path planning in a different thread
// create thread pool with just one thread
EcWorkerThreadPool threadPool(1);
// sleep a bit to allow new thread to be created and run before calling startCalculation
// or we could have created the thread pool at the very beginning
EcSLEEPMS(500);
// set the callback
Callback callback(&planner);
threadPool.setCalculationCompletionCallback(&planner, boost::bind(&Callback::pathCalcCompletionCallback, &callback, _1));
// start the calculation
threadPool.startCalculation(&planner);

The EcRrtUtility class

For most manipulators, the provided EcRrtUtility class is sufficient. In EcRrtUtility::generateRandomNode method, a node is generated by uniformly randomize each joint value within its own joint limits. However, if the manipulator contains some mechanical couplings among joints or if the user wants to constrain the generated path in some fashion, he can then derive a class from EcRrtUtility and override the generateRandomNode method.

For example, if the robot's second joint is mechanically coupled with the first joint (i.e. q[1] is always equal to q[0]), then we cannot simply generate random joint values for the second joint. The following code snippet illustrates how we cope with this constraint by forcing q[1] to always equal q[0] while letting randomization on all other joints.

EcBoolean MyRrtUtility::generateRandomNode
(
EcTreeNode& node
)
{
if(!m_pStatedSystem)
{
return EcFalse;
}
const EcIndividualManipulator& manip = m_pStatedSystem->system().manipulators()[m_ManipIndex];
const EcU32 jointDof = manip.jointDof();
// randomize all joints except joint 1
node.positionState().setCoordSysXForm(m_BasePosition);
jointPos.resize(jointDof);
for(EcU32 ii=0; ii<jointDof; ++ii)
{
if(ii!=1)
{
const EcManipulatorLink* link = manip.linkByIndex(ii);
const EcJointActuator& joint = link->jointActuator();
jointPos[ii] = EcRandom::uniformRealRandom(joint.lowerLimit(), joint.upperLimit());
}
}
// now force q1 to be q0
jointPos[1] = jointPos[0];
return EcTrue;
}
// now use it in the path planner
MyRrtUtility util;
planner.setRrtUtility(util);
Custom RRT Utility

References

[1] S.M. LaValle. Rapidly-exploring random trees: A new tool for path planning. TR-98-11, Computer Science Dept., Iowa State University. Oct. 1998
[2] J.J. Kuffner and S.M. LaValle. RRT-Connect: An efficient approach to single-query path planning. Proc. Of 2000 IEEE Int. Conf. on Robotics and Automation. San Francisco, CA. April 2000
[3] D. Bertram, J.J. Kuffner, R. Dillmann, and T. Asfour. An integrated approach to inverse kinematics and path planning for redundant manipulators. Proc. Of 2006 IEEE Int. Conf. on Robotics and Automation. Orlando, FL. May 2006
[4] M. Vande Weghe, D. Ferguson, S.S. Srinivasa. Randomized path planning for redundant manipulators without inverse kinematics. Proc. Of 2007 IEEE-RAS Int. Conf. on Humanoid Robots. Pittsburg, PA. Nov 2007
[5] J.D. English and A.A. Maciejewski. On the implementation of velocity control for kinematically redundant manipulators. IEEE Transactions on Systems, Man, and Cybernetics. Vol. 30, No. 3. May 2000
[6] D. Hsu. Randomized single-query motion planning in expansive spaces. Ph.D. Dissertation, Stanford University. May 2000.
Created by Energid Technologies www.energid.com
Copyright © 2016 Energid. All trademarks mentioned in this document are property of their respective owners.