Plan motion for rigid body tree using bidirectional RRT
Since R2020b
expand all in page
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.
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
example
rrt = manipulatorRRT( creates a bidirectional RRT planner for the specified rigidBodyTree robot model. The empty cell array indicates that there are no obstacles in the environment.robotRBT,{})
rrt = manipulatorRRT( creates a planner for a robot model with collision objects placed in the environment. The planner checks for collisions with these objects.robotRBT,collisionObjects)
example
rrt = manipulatorRRT(___,Map=map) specifies a 3-D occupancy map map to represent the environment. This requires Navigation Toolbox™.
rrt = manipulatorRRT(___, specifies properties using one or more name-value arguments in addition to any input arguments from the previous syntaxes.Name=Value)
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 either "parent", "adjacent" or as 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 onshow(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);clffor i = 1:20:size(interpPath,1) show(robot,interpPath(i,:)); hold onendshow(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 BoundsgoalRegion.Bounds(2, :) = [-0.2 0.2]; % Y BoundsgoalRegion.Bounds(4, :) = [-pi/2 pi/2]; % Rotation about the Z-axisYou 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 onshow(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) drawnowendhold 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 onshow(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;endhold 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 onshow(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
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+m2), 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]), themanipulatorRRTobject 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
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); tocend
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.
See Also
Objects
- rigidBodyTree | interactiveRigidBodyTree | analyticalInverseKinematics | occupancyMap3D (Navigation Toolbox)
Functions
- plan | interpolate | shorten
Topics
- Pick and Place Using RRT for Manipulators
- Pick-and-Place Workflow Using RRT Planner and Stateflow for MATLAB
MATLAB Command
You clicked a link that corresponds to this MATLAB command:
Run the command by entering it in the MATLAB Command Window. Web browsers do not support MATLAB commands.
Select a Web Site
Choose a web site to get translated content where available and see local events and offers. Based on your location, we recommend that you select: .
You can also select a web site from the following list:
Americas
- América Latina (Español)
- Canada (English)
- United States (English)
Europe
- Belgium (English)
- Denmark (English)
- Deutschland (Deutsch)
- España (Español)
- Finland (English)
- France (Français)
- Ireland (English)
- Italia (Italiano)
- Luxembourg (English)
- Netherlands (English)
- Norway (English)
- Österreich (Deutsch)
- Portugal (English)
- Sweden (English)
- Switzerland
- Deutsch
- English
- Français
- United Kingdom (English)
Asia Pacific
- Australia (English)
- India (English)
- New Zealand (English)
- 中国
- 日本 (日本語)
- 한국 (한국어)
Contact your local office
