Main Content

Pick and Place Using RRT for Manipulators

Open Live Script

Using manipulators to pick and place objects in an environment may require path planning algorithms like the rapidly-exploring random tree planner. The planner explores in the joint-configuration space and searches for a collision-free path between different robot configurations. This example shows how to use the manipulatorRRT object to tune the planner parameters and plan a path between two joint configurations based on a rigidBodyTree robot model of the Franka Emika™ Panda robot. After tuning the planner parameters, the robot manipulator plans a path to move a can from one place to another.

Load Robot Model and Environment

Load the robot and its environment using the exampleHelperLoadPickAndPlaceRRT function. The function outputs three variables:

  • franka — A Franka Emika Panda robot model as a rigidBodyTree object. The model has been modified to remove some adjacent collision meshes that are always in collision and adjust position limits based on feasibility.

  • config — An initial configuration of joint positions for the robot.

  • env — A set collision objects as a cell array that represent the robot's environment. The path planner checks for self-collisions and collisions with this environment.

[franka,config,env] = exampleHelperLoadPickAndPlaceRRT;

Visualize the robot model's collision meshes and the environment objects.

figure("Name","Pick and Place Using RRT",...
    "Units","normalized",...
    "OuterPosition",[0, 0, 1, 1],...
    "Visible","on");
show(franka,config,"Visuals","off","Collisions","on");
hold on
for i = 1:length(env)
    show(env{i});
end

Figure Pick and Place Using RRT contains an axes object. The axes object with xlabel X, ylabel Y contains 40 objects of type patch, line. These objects represent panda_link0, panda_link1, panda_link2, panda_link3, panda_link4, panda_link5, panda_link6, panda_link7, panda_link8, panda_hand, panda_leftfinger, panda_rightfinger, panda_link1_mesh, panda_link2_mesh, panda_link3_mesh, panda_link4_mesh, panda_link5_mesh, panda_link6_mesh, panda_link7_mesh, panda_hand_mesh, panda_leftfinger_mesh, panda_rightfinger_mesh, panda_link0_mesh, panda_link1_coll_mesh, panda_link2_coll_mesh, panda_link3_coll_mesh, panda_link4_coll_mesh, panda_link5_coll_mesh, panda_link6_coll_mesh, panda_link7_coll_mesh, panda_hand_coll_mesh, panda_leftfinger_coll_mesh, panda_rightfinger_coll_mesh, panda_link0_coll_mesh.

Planner

Create the RRT path planner and specify the robot model and the environment. Define some parameters, which are later tuned, and specify the start and goal configuration for the robot.

planner = manipulatorRRT(franka, env);
planner.SkippedSelfCollisions='parent';

planner.MaxConnectionDistance = 0.3;
planner.ValidationDistance = 0.1;

startConfig = config;
goalConfig = [0.2371   -0.0200    0.0542   -2.2272    0.0013    2.2072   -0.9670    0.0400    0.0400];

Plan the path between configurations. The RRT planner should generate a rapidly-exploring tree of random configurations to explore the space and eventually returns a collision-free path through the environment. Before planning, reset the random number generator for repeatable results.

rng('default');
path = plan(planner,startConfig,goalConfig);

To visualize the entire path, interpolate the path into small steps. By default, the interpolate function generates all of the configurations that were checked for feasibility (collision checking) based on the ValidationDistance property. 

interpStates = interpolate(planner, path);

for i = 1:2:size(interpStates,1)
    show(franka, interpStates(i,:),...
        "PreservePlot", false,...
        "Visuals","off",...
        "Collisions","on");
    title("Plan 1: MaxConnectionDistance = 0.3")
    drawnow;
end

Figure Pick and Place Using RRT contains an axes object. The axes object with title Plan 1: MaxConnectionDistance = 0.3, xlabel X, ylabel Y contains 40 objects of type patch, line. These objects represent panda_link0, panda_link1, panda_link2, panda_link3, panda_link4, panda_link5, panda_link6, panda_link7, panda_link8, panda_hand, panda_leftfinger, panda_rightfinger, panda_link1_mesh, panda_link2_mesh, panda_link3_mesh, panda_link4_mesh, panda_link5_mesh, panda_link6_mesh, panda_link7_mesh, panda_hand_mesh, panda_leftfinger_mesh, panda_rightfinger_mesh, panda_link0_mesh, panda_link1_coll_mesh, panda_link2_coll_mesh, panda_link3_coll_mesh, panda_link4_coll_mesh, panda_link5_coll_mesh, panda_link6_coll_mesh, panda_link7_coll_mesh, panda_hand_coll_mesh, panda_leftfinger_coll_mesh, panda_rightfinger_coll_mesh, panda_link0_coll_mesh.

Tuning the Planner

Tune the path planner by modifying the MaxConnectionDistance, ValidationDistance, EnableConnectHeuristic properties on the planner object.

Setting the MaxConnectionDistance property to a larger value causes longer motions in the planned path, but also enables the planner to greedily explore the space. Use tic and toc functions to time the plan function for reference on how these parameters can affect the execution time.

planner.MaxConnectionDistance = 5;
tic
path = plan(planner,startConfig,goalConfig);
toc
Elapsed time is 0.644253 seconds.

Notice the change in the path. The robot arm swings much higher due to the larger connection distance.

interpStates = interpolate(planner, path);

for i = 1:2:size(interpStates, 1)
    show(franka,interpStates(i,:),...
        "PreservePlot",false,...
        "Visuals","off",...
        "Collisions","on");
    title("Plan 2: MaxConnectionDistance = 5")
    drawnow;
end

Figure Pick and Place Using RRT contains an axes object. The axes object with title Plan 2: MaxConnectionDistance = 5, xlabel X, ylabel Y contains 40 objects of type patch, line. These objects represent panda_link0, panda_link1, panda_link2, panda_link3, panda_link4, panda_link5, panda_link6, panda_link7, panda_link8, panda_hand, panda_leftfinger, panda_rightfinger, panda_link1_mesh, panda_link2_mesh, panda_link3_mesh, panda_link4_mesh, panda_link5_mesh, panda_link6_mesh, panda_link7_mesh, panda_hand_mesh, panda_leftfinger_mesh, panda_rightfinger_mesh, panda_link0_mesh, panda_link1_coll_mesh, panda_link2_coll_mesh, panda_link3_coll_mesh, panda_link4_coll_mesh, panda_link5_coll_mesh, panda_link6_coll_mesh, panda_link7_coll_mesh, panda_hand_coll_mesh, panda_leftfinger_coll_mesh, panda_rightfinger_coll_mesh, panda_link0_coll_mesh.

Setting the ValidationDistance to a smaller value enables finer validation of the motion along an edge in the planned path. Increasing the number of configurations to be validated along a path leads to longer planning times. A smaller value is useful in case of a cluttered environment with a lot of collision objects. Because of the small validation distance, interpStates has a larger number of elements. For faster visualization, the for loop skips more states in this step for faster visualization.

planner.MaxConnectionDistance = 0.3;
planner.ValidationDistance = 0.01;

tic
path = plan(planner,startConfig,goalConfig);
toc
Elapsed time is 0.431038 seconds.

interpStates = interpolate(planner,path);
for i = 1:10:size(interpStates,1)
    show(franka, interpStates(i,:),...
        "PreservePlot",false,...
        "Visuals","off",...
        "Collisions","on");
    title("Plan 3: ValidationDistance = 0.01")
    drawnow;
end

Figure Pick and Place Using RRT contains an axes object. The axes object with title Plan 3: ValidationDistance = 0.01, xlabel X, ylabel Y contains 40 objects of type patch, line. These objects represent panda_link0, panda_link1, panda_link2, panda_link3, panda_link4, panda_link5, panda_link6, panda_link7, panda_link8, panda_hand, panda_leftfinger, panda_rightfinger, panda_link1_mesh, panda_link2_mesh, panda_link3_mesh, panda_link4_mesh, panda_link5_mesh, panda_link6_mesh, panda_link7_mesh, panda_hand_mesh, panda_leftfinger_mesh, panda_rightfinger_mesh, panda_link0_mesh, panda_link1_coll_mesh, panda_link2_coll_mesh, panda_link3_coll_mesh, panda_link4_coll_mesh, panda_link5_coll_mesh, panda_link6_coll_mesh, panda_link7_coll_mesh, panda_hand_coll_mesh, panda_leftfinger_coll_mesh, panda_rightfinger_coll_mesh, panda_link0_coll_mesh.

The connect heuristic allows the planner to greedily join the start and goal trees. In places where the environment is less cluttered, this heuristic is useful for shorter planning times. However, a greedy behavior in a cluttered environment leads to wasted connection attempts. Setting the EnableConnectHeuristic to false may give longer planning times and longer paths, but results in a higher success rate of finding a path given the number of iterations.

planner.ValidationDistance = 0.1;
planner.EnableConnectHeuristic = false;

tic
path = plan(planner,startConfig,goalConfig);
toc
Elapsed time is 0.221472 seconds.

interpStates = interpolate(planner,path);
for i = 1:2:size(interpStates,1)
    show(franka, interpStates(i,:), ...
        "PreservePlot",false,...
        "Visuals","off",...
        "Collisions","on");
    title("Plan 4: EnableConnectHeuristic = false")
    drawnow;
end

Figure Pick and Place Using RRT contains an axes object. The axes object with title Plan 4: EnableConnectHeuristic = false, xlabel X, ylabel Y contains 40 objects of type patch, line. These objects represent panda_link0, panda_link1, panda_link2, panda_link3, panda_link4, panda_link5, panda_link6, panda_link7, panda_link8, panda_hand, panda_leftfinger, panda_rightfinger, panda_link1_mesh, panda_link2_mesh, panda_link3_mesh, panda_link4_mesh, panda_link5_mesh, panda_link6_mesh, panda_link7_mesh, panda_hand_mesh, panda_leftfinger_mesh, panda_rightfinger_mesh, panda_link0_mesh, panda_link1_coll_mesh, panda_link2_coll_mesh, panda_link3_coll_mesh, panda_link4_coll_mesh, panda_link5_coll_mesh, panda_link6_coll_mesh, panda_link7_coll_mesh, panda_hand_coll_mesh, panda_leftfinger_coll_mesh, panda_rightfinger_coll_mesh, panda_link0_coll_mesh.

Attach the Can to the End-Effector

After tuning the planner for the desired behavior, follow the pick-and-place workflow where the robot moves an object through the environment. This example attaches a cylinder, or can, to the end-effector of the robot and moves it to a new location. For each configuration, the planner checks for collisions with the cylinder mesh as well.

% Create can as a rigid body
cylinder1 = env{3};
canBody = rigidBody("myCan");
canJoint = rigidBodyJoint("canJoint");

% Get current pose of the robot hand.
startConfig = path(end, :);
endEffectorPose = getTransform(franka,startConfig,"panda_hand");

% Place can into the end effector gripper.
setFixedTransform(canJoint,endEffectorPose\cylinder1.Pose); 

% Add collision geometry to rigid body.
addCollision(canBody,cylinder1,inv(cylinder1.Pose));
canBody.Joint = canJoint;

% Add rigid body to robot model.
addBody(franka,canBody,"panda_hand");

% Remove object from environment.
env(3) = [];

After the can has been attached to the robot arm, specify a goal configuration for placing the object. Then, with the modified robot, create a planning instance with the new robot model. Modify the planner parameters. Plan a path from start to goal. Visualize the path. Notice the can clears the wall. 

goalConfig = [-0.6564 0.2885 -0.3187 -1.5941 0.1103 1.8678 -0.2344 0.04 0.04];
planner=manipulatorRRT(franka, env);
planner.SkippedSelfCollisions='parent';
planner.MaxConnectionDistance = 1;
planner.ValidationDistance = 0.2;
planner.EnableConnectHeuristic = false;
path = plan(planner,startConfig,goalConfig);

interpStates = interpolate(planner,path);

hold off

Figure Pick and Place Using RRT contains an axes object. The axes object with title Plan 4: EnableConnectHeuristic = false, xlabel X, ylabel Y contains 40 objects of type patch, line. These objects represent panda_link0, panda_link1, panda_link2, panda_link3, panda_link4, panda_link5, panda_link6, panda_link7, panda_link8, panda_hand, panda_leftfinger, panda_rightfinger, panda_link1_mesh, panda_link2_mesh, panda_link3_mesh, panda_link4_mesh, panda_link5_mesh, panda_link6_mesh, panda_link7_mesh, panda_hand_mesh, panda_leftfinger_mesh, panda_rightfinger_mesh, panda_link0_mesh, panda_link1_coll_mesh, panda_link2_coll_mesh, panda_link3_coll_mesh, panda_link4_coll_mesh, panda_link5_coll_mesh, panda_link6_coll_mesh, panda_link7_coll_mesh, panda_hand_coll_mesh, panda_leftfinger_coll_mesh, panda_rightfinger_coll_mesh, panda_link0_coll_mesh.

show(franka,config,"Visuals","off","Collisions","on");
hold on
for i = 1:length(env)
    show(env{i});
end

for i = 1:size(interpStates,1)
    show(franka,interpStates(i,:),...
        "PreservePlot", false,...
        "Visuals","off",...
        "Collisions","on");
    title("Plan 5: Place the Can")
    drawnow;
    if i == (size(interpStates,1))
        view([80,7])
    end
end

Figure Pick and Place Using RRT contains an axes object. The axes object with title Plan 5: Place the Can, xlabel X, ylabel Y contains 42 objects of type patch, line. These objects represent panda_link0, panda_link1, panda_link2, panda_link3, panda_link4, panda_link5, panda_link6, panda_link7, panda_link8, panda_hand, panda_leftfinger, panda_rightfinger, myCan, panda_link1_mesh, panda_link2_mesh, panda_link3_mesh, panda_link4_mesh, panda_link5_mesh, panda_link6_mesh, panda_link7_mesh, panda_hand_mesh, panda_leftfinger_mesh, panda_rightfinger_mesh, panda_link0_mesh, panda_link1_coll_mesh, panda_link2_coll_mesh, panda_link3_coll_mesh, panda_link4_coll_mesh, panda_link5_coll_mesh, panda_link6_coll_mesh, panda_link7_coll_mesh, panda_hand_coll_mesh, panda_leftfinger_coll_mesh, panda_rightfinger_coll_mesh, myCan_coll_mesh, panda_link0_coll_mesh.

Shorten the Planned Path
To shorten your path, use the shorten function and specify a number of iterations. A small value for the ValidationDistance property combined with a large number of iterations can result in large computation times.
shortenedPath = shorten(planner,path,20);


interpStates = interpolate(planner,shortenedPath);
for i = 1:size(interpStates,1)
    show(franka, ...
        interpStates(i, :),  ...
        "PreservePlot", false, ...
        "Visuals", "off", ...
        "Collisions", "on");
    drawnow;
    title("Plan 6: Shorten the Path")
    if i > (size(interpStates,1)-2)
        view([80,7])
    end
end
hold off
Figure Pick and Place Using RRT contains an axes object. The axes object with title Plan 6: Shorten the Path, xlabel X, ylabel Y contains 42 objects of type patch, line. These objects represent panda_link0, panda_link1, panda_link2, panda_link3, panda_link4, panda_link5, panda_link6, panda_link7, panda_link8, panda_hand, panda_leftfinger, panda_rightfinger, myCan, panda_link1_mesh, panda_link2_mesh, panda_link3_mesh, panda_link4_mesh, panda_link5_mesh, panda_link6_mesh, panda_link7_mesh, panda_hand_mesh, panda_leftfinger_mesh, panda_rightfinger_mesh, panda_link0_mesh, panda_link1_coll_mesh, panda_link2_coll_mesh, panda_link3_coll_mesh, panda_link4_coll_mesh, panda_link5_coll_mesh, panda_link6_coll_mesh, panda_link7_coll_mesh, panda_hand_coll_mesh, panda_leftfinger_coll_mesh, panda_rightfinger_coll_mesh, myCan_coll_mesh, panda_link0_coll_mesh.

                
See Also
 | 

                
Related Topics