manipulatorRRT

Plan motion for rigid body tree using bidirectional RRT

Description

The manipulatorRRT object is a single-query planner for manipulator arms that uses the bidirectional rapidly exploring random trees (RRT) algorithm with an optional connect heuristic to potentially increase speed.

The bidirectional RRT planner creates two trees with root nodes at the specified start and goal configurations. To extend each tree, the planner generates a random configuration and, if valid, takes a step from the nearest node based on the MaxConnectionDistance property. After each extension, the planner attempts to connect between the two trees using the new extension and the closest node on the opposite tree. Invalid configurations or connections that collide with the environment are not added to the tree.

For a greedier search, enabling the EnableConnectHeuristic property disables the limit on the MaxConnectionDistance property when connecting between the two trees.

Setting the EnableConnectHueristic property to false limits the extension distance when connecting between the two trees to the value of the MaxConnectionDistance property.

The object uses a rigidBodyTree robot model to generate the random configurations and intermediate states between nodes. Collision objects are specified in the robot model to validate the configurations and check for collisions with the environment or the robot itself.

To plan a path between a start and a goal configuration, use the plan object function. After planning, you can interpolate states along the path using the interpolate object function. To attempt to shorten the path by trimming edges, use the shorten object function.

For more information about the computation complexity, see Planning Complexity.

Creation

Syntax

rrt = manipulatorRRT(robotRBT,{})

rrt = manipulatorRRT(robotRBT,collisionObjects)

Description

rrt = manipulatorRRT(robotRBT,{}) creates a bidirectional RRT planner for the specified rigidBodyTree robot model. The empty cell array indicates that there are no obstacles in the environment.

rrt = manipulatorRRT(robotRBT,collisionObjects) creates a planner for a robot model with collision objects placed in the environment. The planner checks for collisions with these objects.

Properties

MaxConnectionDistance — Maximum length between planned configurations

0.1 (default) | positive scalar

Maximum length between planned configurations, specified as a positive scalar. The object computes the length of the motion as the Euclidean distance between the two joint configurations. Differences between two joint positions for a revolute joint are calculated using the angdiff function. During the extension process, this is the maximum distance a configuration can change.

If the EnableConnectheuristic property is set to true, the object ignores this distance when connecting the two trees during the connect stage.

Data Types: double

ValidationDistance — Distance resolution for validating motion between configurations

0.01 (default) | positive scalar

Distance resolution for validating motion between configurations, specified as a positive scalar. The validation distance determines the number of interpolated nodes between two adjacent nodes in the tree. The object validates each interpolated node by checking for collisions with the robot and the environment.

Data Types: double

MaxIterations — Maximum number of random configurations generated

10000 (default) | positive integer

Maximum number of random configurations generated, specified as a positive integer.

Data Types: double

EnableConnectHeuristic — Directly join trees during connect phase

true or 1 (default) | false or 0

Directly join trees during the connect phase of the planner, specified as a logical 1 (true) or 0 (false). Setting this property to true causes the object to ignore the MaxConnectionDistance property when attempting to connect the two trees together.

Data Types: logical

Object Functions

plan	Plan path using RRT for manipulators
interpolate	Interpolate states along path from RRT
shorten	Trim edges to shorten path from RRT

Examples

Plan Path for Manipulator Robot Using RRT

Use the manipulatorRRT object to plan a path for a rigid body tree robot model in an environment with obstacles. Visualize the planned path with interpolated states.

Load a robot model into the workspace. Use the KUKA LBR iiwa© manipulator arm.

robot = loadrobot("kukaIiwa14","DataFormat","row");

Generate the environment for the robot. Create collision objects and specify their poses relative to the robot base. Visualize the environment.

env = {collisionBox(0.5, 0.5, 0.05) collisionSphere(0.3)};
env{1}.Pose(3, end) = -0.05;
env{2}.Pose(1:3, end) = [0.1 0.2 0.8];

show(robot);
hold on
show(env{1})
show(env{2})

Create the RRT planner for the robot model.

rrt = manipulatorRRT(robot,env);

Specify a start and a goal configuration.

startConfig = [0.08 -0.65 0.05 0.02 0.04 0.49 0.04];
goalConfig =  [2.97 -1.05 0.05 0.02 0.04 0.49 0.04];

Plan the path. Due to the randomness of the RRT algorithm, set the rng seed for repeatability.

rng(0)
path = plan(rrt,startConfig,goalConfig);

Visualize the path. To add more intermediate states, interpolate the path. By default, the interpolate object function uses the value of ValidationDistance property to determine the number of intermediate states. The for loop shows every 20th element of the interpolated path.

interpPath = interpolate(rrt,path);
clf
for i = 1:20:size(interpPath,1)
    show(robot,interpPath(i,:));
    hold on
end
show(env{1})
show(env{2})
hold off

Tips

Planning Complexity

• When planning the motion between nodes in the tree, a set of configurations are generated and validated. This computation time of the planner is proportional to the number of configurations generated. The number of configurations between nodes is controlled by the ratio of the MaxConnectionDistance and ValidationDistance properties. To improve planning time, consider increasing the validation distance or decreasing the max connection distance.

• Validating each configuration has a complexity of O(mn+m²), where m is the number of collision bodies in the rigidBodyTree object and n is the number of collision objects in worldObjects. Using large numbers of meshes to represent your robot or environment increases the time for validating each configuration.

Infinite Joint Limits

• If your rigidBodyTree robot model has joint limits that have infinite range (e.g. revolute joint with limits of [-Inf Inf]), the manipulatorRRT object uses limits of [-1e10 1e10] to perform uniform random sampling in the joint limits.

References

Kuffner, J. J., and S. M. LaValle. “RRT-Connect: An Efficient Approach to Single-Query Path Planning.” In Proceedings 2000 ICRA. Millennium Conference. IEEE International Conference on Robotics and Automation. Symposia Proceedings (Cat. No.00CH37065), 2:995–1001. San Francisco, CA, USA: IEEE, 2000. https://doi:10.1109/ROBOT.2000.844730.

[1]

Extended Capabilities

C/C++ Code Generation
Generate C and C++ code using MATLAB® Coder™.