plan
Syntax
Description
plans a path between the specified start and goal configurations using the manipulator
rapidly exploring random trees (RRT) planner path
= plan(rrt
,startConfig
,goalConfig
)rrt
.
plans a path between the specified start and a goal region as a path
= plan(rrt
,startConfig
,goalRegion
)workspaceGoalRegion
object
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);
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
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
Specify Multiple Workspace Goal Regions
Load a KINOVA® Gen3 robot and create a workspace goal region for the end-effector link of the robot.
rbt = loadrobot("kinovagen3",DataFormat="row"); eename = "EndEffector_Link"; wgr1 = workspaceGoalRegion(eename); wgr1.ReferencePose = trvec2tform([0.5 0 0.2]); wgr1.Bounds(1:2,:) = [-0.2 0.2; -0.1 0.1]; wgr1.Bounds(4,:) = [-pi pi];
Create another workspace goal region with the same bounds as the first goal region but offset the goal region by 0.4
meters on the z-axis.
wgr2 = wgr1; wgr2.ReferencePose = wgr1.ReferencePose*trvec2tform([0 0 0.4]);
Show the robot and the workspace goal regions.
show(rbt,homeConfiguration(rbt),Frames="off"); hold on show(wgr1); show(wgr2); title("Robot with Two Workspace Goal Regions") axis equal
Create the RRT planner with an empty environment.
env = {};
planner = manipulatorRRT(rbt,env,skip="parent");
Plan a paths to the workspace goal regions. Note that the RRT planner samples goal configurations uniformly from the two goal regions.
rng(60) for i = 1:4 plannedpath = plan(planner,homeConfiguration(rbt),{wgr1,wgr2}); show(rbt,plannedpath(end,:),Frames="off"); end axis equal title("Goal Configurations in Workspace Goal Regions") hold off
Input Arguments
rrt
— Manipulator RRT planner
manipulatorRRT
object
Manipulator RRT planner, specified as a manipulatorRRT
object. This planner is for a specific rigid body tree robot
model stored as a rigidBodyTree
object.
startConfig
— Initial robot configuration
n-element vector of joint positions
Initial robot configuration, specified as an n-element vector of
joint positions for the rigidBodyTree
object stored in the RRT
planner rrt
. n is the number of nonfixed joints
in the robot model.
Data Types: double
goalConfig
— Desired robot configuration
n-element vector of joint positions
Desired robot configuration, specified as an n-element vector of
joint positions for the rigidBodyTree
object stored in the RRT
planner rrt
. n is the number of nonfixed joints
in the robot model.
Data Types: double
goalRegion
— Workspace goal region
workspaceGoalRegion
object | cell array of workspaceGoalRegion
objects
Workspace goal region, specified as a workspaceGoalRegion
object or a cell array of
workspaceGoalRegion
objects. Goal configurations are sampled
uniformly from the provided goal regions provided in a cell-array.
The workspaceGoalRegion
objects define the bounds on the
end-effector pose and the sample
object function returns random poses to add to the RRT tree. Set the WorkspaceGoalRegionBias property to change the probability of sampling a
state within the goal region.
Output Arguments
path
— Planned path in joint space
r-by-n matrix of joint configurations
Planned path in joint space, returned as an
r-by-n matrix of joint configurations, where
r is the number of configurations in the path, and
n is the number of nonfixed joints in the rigidBodyTree
robot model.
Data Types: double
solnInfo
— Solution information from planner
structure
Solution information from planner, returned as a structure with these fields:
IsPathFound
— A logical indicating if a path was foundExitFlag
— An integer indicating why the planner terminated:1
— Goal configuration reached2
— Maximum number of iterations reached
Data Types: struct
Extended Capabilities
C/C++ Code Generation
Generate C and C++ code using MATLAB® Coder™.
Version History
Introduced in R2020bR2024b: 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: 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.
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:
How to Get Best Site Performance
Select the China site (in Chinese or English) for best site performance. Other MathWorks country sites are not optimized for visits from your location.
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
- United Kingdom (English)