manipulatorRRT

Plan motion for rigid body tree using bidirectional RRT

Since R2020b

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.

Image showing the extension of the two branching trees from start and end goal. When the EnableConnectHeurist is true, the connection steps are not limited by the max connection distance.

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

Image showing the extension of the two branching trees from start and end goal. When the EnableConnectHeurist is false, the connection steps are limited by the max connection distance.

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.

To specify a region to sample end-effector poses near the goal configuration, create a workspaceGoalRegion object and specify it as the goalRegion input to the plan object function. To change the probability of sampling additional goal configurations, specify the WorkspaceGoalRegionBias property.

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

Creation

Syntax

rrt = manipulatorRRT(robotRBT,{})

rrt = manipulatorRRT(robotRBT,collisionObjects)

rrt = manipulatorRRT(___,Map=map)

rrt = manipulatorRRT(___,Name=Value)

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.

example

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.

rrt = manipulatorRRT(___,Map=map) specifies a 3-D occupancy map map to represent the environment. This requires Navigation Toolbox™.

example

rrt = manipulatorRRT(___,Name=Value) specifies properties using one or more name-value arguments in addition to any input arguments from the previous syntaxes.

Input Arguments

expand all

`map` — 3-D occupancy map representing environment
`occupancyMap3D` object

3-D occupancy map representing the environment, specified as a occupancyMap3D (Navigation Toolbox) object. Note that the manipulatorRRT object does not deep copy the specified map and instead holds the handle to the specified map.

Specifying this input argument requires Navigation Toolbox.

Properties

expand all

`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. During the extension process, this is the maximum distance a configuration can change.

When revolute joints have infinite limits, differences between two joint positions are calculated using the angdiff function.

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

`WorkspaceGoalRegionBias` — Probability to sample additional goal state from workspace goal region
0.50 (default) | positive value in the range [0,1)

Probability to sample a goal state from the workspace goal region, specified as a positive value in the range [0,1). The bias defines the probability to add additional goal states to the tree from the workspaceGoalRegion object. When this value is set to zero, the workspaceGoalRegion object still samples a single goal for the planner to plan to.

Increasing this value increases the likelihood of reaching a goal state in the goal region, but may lead to longer planning times because each new goal state adds additional complexity for planning.

Dependency

You must use the goalRegion input when calling the plan object function.

Data Types: double

`IgnoreSelfCollision` — Ignore self collisions during planning
`0` or `false` (default) | `1` or `true`

Ignore self collisions during planning, specified as a logical. If this property is set to true, the plan function skips checking between bodies for collisions and only compares the bodies to the environment. By not checking for self-collisions, you may improve the speed of the planning phase.

Data Types: logical

`SkippedSelfCollisions` — Body pairs skipped for checking self-collisions
`"parent"` (default) | `"adjacent"` | p-by-2 cell array of character vectors

Body pairs skipped for checking self-collisions, specified as "parent", "adjacent", or a p-by-2 cell array of character vectors:

"parent" — Skip collision checking between child and parent bodies. See Skip Self-Collision Checking Between Parent and Adjacent Bodies for more information.
"adjacent" — Skip collision checking between bodies on adjacent indices. See Skip Self-Collision Checking Between Parent and Adjacent Bodies for more information.
p-by-2 cell array of character vectors — Skip collision checking between specific body pairs. Each row specifies a pair of body names between which to skip self-collision checking. p is the number of body pairs to skip for self-collision checking. For more information, see Skip Self-Collision Checking Between Specific Body Pairs.

Data Types: char | string

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

collapse all

Plan Path for Manipulator Robot Using RRT

Open Live Script

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);
rrt.SkippedSelfCollisions = "parent";

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

Plan Path To Workspace Goal Region

Open Live Script

Specify a goal region in your workspace and plan a path within those bounds. The workspaceGoalRegion object defines the bounds on the xyz-position and zyx Euler orientation of the robot end effector. The manipulatorRRT object plans a path based on that goal region and samples random poses within the bounds.

Load an existing robot model as a rigidBodyTree object.

robot = loadrobot("kinovaGen3", "DataFormat", "row");
ax = show(robot);

Create Path Planner

Create a rapidly-exploring random tree (RRT) path planner for the robot. This example uses an empty environment, but this workflow also works well with cluttered environments. You can add collision objects to the environment like the collisionBox or collisionMesh object.

planner = manipulatorRRT(robot,{});
planner.SkippedSelfCollisions="parent";

Define Goal Region

Create a workspace goal region using the end-effector body name of the robot.

Define the goal region parameters for your workspace. The goal region includes a reference pose, xyz-position bounds, and orientation limits on the zyx Euler angles. This example specifies bounds on the xy-plane in meters and allows rotation about the z-axis in radians.

goalRegion = workspaceGoalRegion(robot.BodyNames{end}); 
goalRegion.ReferencePose = trvec2tform([0.5 0.5 0.2]);
goalRegion.Bounds(1, :) = [-0.2 0.2];    % X Bounds
goalRegion.Bounds(2, :) = [-0.2 0.2];    % Y Bounds
goalRegion.Bounds(4, :) = [-pi/2 pi/2];  % Rotation about the Z-axis

You can also apply a fixed offset to all poses sampled within the region. This offset can account for grasping tools or variations in dimensions within your workspace. For this example, apply a fixed transformation that places the end effector 5 cm above the workspace.

goalRegion.EndEffectorOffsetPose = trvec2tform([0 0 0.05]);
hold on
show(goalRegion);

Plan Path To Goal Region

Plan a path to the goal region from the robot's home configuration. Due to the randomness in the RRT algorithm, this example sets the rng seed to ensure repeatable results.

rng(0)
path = plan(planner,homeConfiguration(robot),goalRegion);

Show the robot executing the path. To visualize a more realistic path, interpolate points between path configurations.

interpConfigurations = interpolate(planner,path,5);

for i = 1:size(interpConfigurations,1)
    show(robot,interpConfigurations(i,:),"PreservePlot",false);
    set(ax,'ZLim',[-0.05 0.75],'YLim',[-0.05 1],'XLim',[-0.05 1],...
        'CameraViewAngle',5)
  
    drawnow
end
hold off

Adjust End-Effector Pose

Notice that the robot arm approaches the workspace from the bottom. To flip the orientation of the final position, add a pi rotation to the Y-axis for the reference pose.

goalRegion.EndEffectorOffsetPose = ... 
    goalRegion.EndEffectorOffsetPose*eul2tform([0 pi 0],"ZYX");

Replan the path and visualize the robot motion again. The robot now approaches from the top.

hold on
show(goalRegion);
path = plan(planner,homeConfiguration(robot),goalRegion);

interpConfigurations = interpolate(planner,path,5);

for i = 1 : size(interpConfigurations,1)
    show(robot, interpConfigurations(i, :),"PreservePlot",false);
    set(ax,'ZLim',[-0.05 0.75],'YLim',[-0.05 1],'XLim',[-0.05 1])
    drawnow;
end
hold off

Plan Path Through 3-D Occupancy Map

Open Live Script

Load the Kinova Gen 3 robot.

rbt = loadrobot("kinovagen3",DataFormat="row");

Create a 3-D occupancy map and set the coordinate at [0.4 0.0 0.4] to occupied.

map = occupancyMap3D(10);
map.setOccupancy([0.4 0.0 0.4],1);

Display the robot in the map.

show(map);
hold on
show(rbt);
axis("equal")
xlim([-1,1.0])
ylim([-1,1.0])
zlim([-0.5,1.2])

Define a start configuration.

startconfig = [2.2131,-1.3950,0.1618,0.2053,-0.1624,1.1684,-2.1886];

Define a goal configuration that is the same as the start configuration except for the first joint.

goalconfig = startconfig;
goalconfig(1) = 3.4;

Create the manipulator RRT planner for the robot and specify the map as the environment using the Map argument.

planner = manipulatorRRT(rbt,{},Map=map);
planner.ValidationDistance=0.1;
planner.MaxConnectionDistance=0.2;
planner.SkippedSelfCollisions="parent";

Plan a path between the start and goal configuration. Then interpolate between the paths.

plannedpath = plan(planner,startconfig,goalconfig);
interpoalatedpath = interpolate(planner,plannedpath);

Animate the robot following the path.

rc=rateControl(10);
view([pi/3,pi/2,pi/4]);
for i = 1:size(interpoalatedpath,1)
    show(rbt,interpoalatedpath(i,:),FastUpdate=true,PreservePlot=false);
    waitfor(rc);
end

Figure contains an axes object. The axes object with title Occupancy Map, xlabel X [meters], ylabel Y [meters] contains 26 objects of type patch, line.

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

[1] 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.

Extended Capabilities

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

Version History

Introduced in R2020b

expand all

R2024b: Specify multiple workspace goal regions

The plan object function now supports specifying multiple workspace goal regions as a cell array using the goalRegion argument.

R2023b: Specify each body pair to skip for self-collision checking

You can now select which body pairs to skip in self-collision checks by specifying the SkippedSelfCollisions name-value argument as a cell array of body pairs. For more information, see the SkippedSelfCollisions property.

R2023b: Improved Performance of Object Functions

The object functions of the manipulatorRRT object have improved performance in MATLAB^® R2023b interpreted execution compared to R2023a. For example, this code is about 6 times faster than in the previous release:

function timingtest
    rng(10);
    kuka=loadrobot("kukaIiwa14",DataFormat="row");
    env={collisionBox(0.5,0.5,0.05),...
        collisionSphere(0.3)};
    env{1}.Pose=trvec2tform([0,0,-0.05]);
    env{2}.Pose=trvec2tform([0.1,0.2,0.8]);
    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];
    planner=manipulatorRRT(kuka,env);
    planner.SkippedSelfCollisions="parent";
    tic;
    plannedplath=plan(planner,startConfig,goalConfig);
    shortenedpath=shorten(planner,plannedplath,10);
    toc
end

The approximate execution times are:

R2023a: 14.0 s

R2023b: 2.5 s

This code was timed on a Windows 10 system with a 3.60 GHz Intel^® W-2133 CPU with 64 GB of RAM.

R2023a: Specify environments using 3-D occupancy maps

If you have the Navigation Toolbox, you can specify the planning environment as a 3-D occupancy map using the occupancyMap3D (Navigation Toolbox) object.

R2022b: Alter rigid body tree self-collision checking behavior change and new default self-collision checking behavior

You can now specify self-collision checking behavior for a rigid body tree robot model by using the SkippedSelfCollisions property. Specify SkippedSelfCollisions as "parent" or "adjacent":

"parent" — Collision checking ignores self-collisions between parent and child rigid bodies.
"adjacent" — Collision checking ignores self -collisions between rigid bodies of adjacent indices.

As of R2022b, the default behavior of collision checking is to ignore self-collisions between parent and child rigid bodies. In previous releases, the default behavior of self-collision checking was to ignore self-collisions between adjacent rigid bodies. To instead ignore self-collisions between rigid bodies of adjacent indices, specify SkippedSelfCollisions as "adjacent".

See Skip Self-Collision Checking Between Parent and Adjacent Bodies for more information.

manipulatorRRT

Description

Creation

Syntax

Description

Input Arguments

`map` — 3-D occupancy map representing environment
`occupancyMap3D` object

Properties

`MaxConnectionDistance` — Maximum length between planned configurations
`0.1` (default) | positive scalar

`ValidationDistance` — Distance resolution for validating motion between configurations
`0.01` (default) | positive scalar

`MaxIterations` — Maximum number of random configurations generated
`10000` (default) | positive integer

`EnableConnectHeuristic` — Directly join trees during connect phase
`true` or `1` (default) | `false` or `0`

`WorkspaceGoalRegionBias` — Probability to sample additional goal state from workspace goal region
0.50 (default) | positive value in the range [0,1)

Dependency

`IgnoreSelfCollision` — Ignore self collisions during planning
`0` or `false` (default) | `1` or `true`

`SkippedSelfCollisions` — Body pairs skipped for checking self-collisions
`"parent"` (default) | `"adjacent"` | p-by-2 cell array of character vectors

Object Functions

Examples

Plan Path for Manipulator Robot Using RRT

Plan Path To Workspace Goal Region

Plan Path Through 3-D Occupancy Map

Tips

References

Extended Capabilities

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

Version History

R2024b: Specify multiple workspace goal regions

R2023b: Specify each body pair to skip for self-collision checking

R2023b: Improved Performance of Object Functions

R2023a: Specify environments using 3-D occupancy maps

R2022b: Alter rigid body tree self-collision checking behavior change and new default self-collision checking behavior

See Also

Objects

Functions

Topics

manipulatorRRT

Description

Creation

Syntax

Description

Input Arguments

map — 3-D occupancy map representing environment occupancyMap3D object

Properties

MaxConnectionDistance — Maximum length between planned configurations 0.1 (default) | positive scalar

ValidationDistance — Distance resolution for validating motion between configurations 0.01 (default) | positive scalar

MaxIterations — Maximum number of random configurations generated 10000 (default) | positive integer

EnableConnectHeuristic — Directly join trees during connect phase true or 1 (default) | false or 0

WorkspaceGoalRegionBias — Probability to sample additional goal state from workspace goal region 0.50 (default) | positive value in the range [0,1)

Dependency

IgnoreSelfCollision — Ignore self collisions during planning 0 or false (default) | 1 or true

SkippedSelfCollisions — Body pairs skipped for checking self-collisions "parent" (default) | "adjacent" | p-by-2 cell array of character vectors

Object Functions

Examples

Plan Path for Manipulator Robot Using RRT

Plan Path To Workspace Goal Region

Plan Path Through 3-D Occupancy Map

Tips

References

Extended Capabilities

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

Version History

R2024b: Specify multiple workspace goal regions

R2023b: Specify each body pair to skip for self-collision checking

R2023b: Improved Performance of Object Functions

R2023a: Specify environments using 3-D occupancy maps

R2022b: Alter rigid body tree self-collision checking behavior change and new default self-collision checking behavior

See Also

Objects

Functions

Topics

`map` — 3-D occupancy map representing environment
`occupancyMap3D` object

`MaxConnectionDistance` — Maximum length between planned configurations
`0.1` (default) | positive scalar

`ValidationDistance` — Distance resolution for validating motion between configurations
`0.01` (default) | positive scalar

`MaxIterations` — Maximum number of random configurations generated
`10000` (default) | positive integer

`EnableConnectHeuristic` — Directly join trees during connect phase
`true` or `1` (default) | `false` or `0`

`WorkspaceGoalRegionBias` — Probability to sample additional goal state from workspace goal region
0.50 (default) | positive value in the range [0,1)

`IgnoreSelfCollision` — Ignore self collisions during planning
`0` or `false` (default) | `1` or `true`

`SkippedSelfCollisions` — Body pairs skipped for checking self-collisions
`"parent"` (default) | `"adjacent"` | p-by-2 cell array of character vectors

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