Perform Pick and Place with Collision-Object-Based Obstacle Avoidance in Robot Scenario

In this example, you create a scenario to simulate a manipulator performing pick-and-place operations with obstacle avoidance in a collision-object-based environment. This example demonstrates how to create a scene with collision-object-based static meshes, add a manipulator using the rigid body tree representation, plan manipulator motion with manipulatorRRT, and simulate the pick-and-place workflow in the scenario.

Create Scene with Collision-Object-Based Static Meshes

A robotScenario object consists of static meshes that are, by default, collision-free, but you can optionally set them up as collision meshes to perform motion planning with a collision-object-based planner. Create a robot scenario.

scenario = robotScenario(UpdateRate=10);

Create a static surface structure representing a table by using scenario meshes fitted with collision objects. The surface structure acts as an obstacle in the scenario.

addMesh(scenario,"Box",Position=[0.35 0 0.2],Size=[0.5 0.9 0.05],Color=[0.7 0.7 0.7],Collision="mesh")
addMesh(scenario,"Box",Position=[0 -0.6 0.2],Size=[1.3 0.4 0.235],Color=[0.7 0.7 0.7],Collision="mesh")
addMesh(scenario,"Box",Position=[0.3 -0.25 0.4],Size=[0.8 0.03 0.35],Color=[0.7 0.7 0.7],Collision="mesh")

Place two boxes on the table with collision-object-fitted scenario meshes. The two boxes are static meshes that act as obstacles and cannot attach to a robotPlatform object.

addMesh(scenario,"Box",Position=[0.52 0 0.278],Size=[0.06 0.06 0.1],Color=[1 0.5 0.25],Collision="mesh")
addMesh(scenario,"Box",Position=[0.4 -0.1 0.2758],Size=[0.06 0.06 0.1],Color=[1 0.5 0.25],Collision="mesh")

Visualize the visual meshes and collision meshes in the scenario by specifying the Visuals and Collisions name-value arguments.

visuals = "on";
collisions = "off";
show3D(scenario,Visuals=visuals,Collisions=collisions);
title("Scene with collision object-based static meshes")
view(81,19)
light

Add Manipulator to Scenario

Load a robot manipulator into the workspace by using the exampleHelperLoadFrankaScenario helper function. The function loads a frankaEmikaPanda robot from the robot library using the loadrobot function.

[franka,endEffectorName] = exampleHelperLoadFrankaScenario;

Create a robotPlatform object with the frankaEmikaPanda robot model.

robot = robotPlatform("Manipulator",scenario,RigidBodyTree=franka,ReferenceFrame="ENU");

Obtain and update the initial joint configuration.

initialConfig = homeConfiguration(robot.RigidBodyTree);
initialConfig(7) = 0.65;
initialConfig(6) = 0.75;

Add Box for Manipulation

robotPlatform objects can be static or movable. Because rigidBodyTree-based robotPlatform objects can grab only non-rigidBodyTree-based robotPlatform objects, to perform a pick-and-place operation, you must add a non-rigidBodyTree-based robotPlatform object, a moveable mesh, to the scenario. Create a box on the static surface structure as a scenario mesh fitted with a collision object.

boxToMove = robotPlatform("Box",scenario,Collision="mesh",InitialBasePosition=[0.5 0.15 0.278]);

Update the properties of the movable mesh.

updateMesh(boxToMove,"Cuboid",Collision="mesh",Size=[0.06 0.06 0.1],Color=[0.9 0.0 0.0]);

Set up the scene and move the manipulator robotPlatform to the initial joint configuration.

setup(scenario)
move(robot,"joint",initialConfig)

Visualize the scenario.

ax = show3D(scenario,Visuals=visuals,Collisions=collisions);
title("Scene with static meshes and robot platform")
view(81,19)
light

Plan and Move Manipulator Gripper to Pick-Up Location

You must first position the manipulator gripper close to the movable mesh box to grab it. To accomplish this, plan a manipulator motion between the initial position of the gripper and the current position of the box.

Plan a path using the manipulatorRRT planner, and avoid obstacles using the collision meshes in the scenario. For more details on path planning, see the Pick and Place Using RRT for Manipulators example.

planner = manipulatorRRT(robot.RigidBodyTree,scenario.CollisionMeshes);
planner.MaxConnectionDistance = 0.1;
planner.ValidationDistance = 0.1;
planner.SkippedSelfCollisions = "parent";

Specify a pick-up joint configuration for the manipulator based on the target box location.

pickUpConfig = [0.2371 -0.0200 0.0542 -2.2272 0.0013 2.2072 -0.9670 0.0400 0.0400];

Plan a path as a series of waypoints and interpolate between the waypoints to visualize the intermediate configurations of the planned motion.

rng("default")
path = plan(planner,initialConfig,pickUpConfig);

% Number of interpolations between each adjacent configuration.
numState = 10;
interpStates = interpolate(planner,path,numState);

Move the manipulator based on the planned state, and visualize each state in the scenario.

% Visualize scene
show3D(scenario,FastUpdate=true,Parent=ax,Visuals=visuals,Collisions=collisions);
title("Approaching towards pick-up location")

for idx = 1:size(interpStates,1)
    % Get current joint configuration.
    jointConfig = interpStates(idx,:);

    % Move manipulator with current joint configuration.
    move(robot,"joint",jointConfig)

    % Use fast update to move platform visualization frames.
    show3D(scenario,FastUpdate=true,Parent=ax,Visuals=visuals,Collisions=collisions);

    % Get robot end effector pose. Draw trajectory.
    hold on
    poseNow = scenario.TransformTree.getTransform(robot.ReferenceFrame,robot.Name + "_" + endEffectorName);
    jointSpaceMarker = plot3(poseNow(1,4),poseNow(2,4),poseNow(3,4),"b.",MarkerSize=12);
    drawnow
    hold off

    % Advance scenario simulation time.
    advance(scenario);
end

Pick Up Target Box with Manipulator Gripper

Attach the target box to the rigidBodyTree of the manipulator by using a transform to simulate the pick-up action.

attach(robot,"Box",endEffectorName,ChildToParentTransform=trvec2tform([0.0 0.0 0.1]))

Visualize the scenario.

show3D(scenario,FastUpdate=true,Parent=ax,Visuals=visuals,Collisions=collisions);
title("Picked-up target box")

Plan Manipulator Motion Between Pick-Up and Drop Locations

Upon picking up the target box, the manipulator must proceed to the drop location and place the box. Specify a joint configuration or the manipulator based on the target box drop-off location.

dropConfig = [-0.6564 0.2885 -0.3187 -1.5941 0.1103 1.8678 -0.2344 0.04 0.04];

planner = manipulatorRRT(robot.RigidBodyTree,scenario.CollisionMeshes);
planner.MaxConnectionDistance = 0.1;
planner.ValidationDistance = 0.1;
planner.SkippedSelfCollisions = "parent";

Plan a path as a series of waypoints and interpolate between the waypoints to visualize the intermediate configurations of the planned motion.

path = plan(planner,pickUpConfig,dropConfig);
interpStates = interpolate(planner,path,numState);

Move the manipulator based on the planned state, and visualize each state in the scenario.

% Visualize scene
show3D(scenario,FastUpdate=true,Parent=ax,Visuals=visuals,Collisions=collisions);
title("Approaching towards drop location")

for idx = 1:size(interpStates,1)
    % Get current joint configuration.
    jointConfig = interpStates(idx,:);

    % Move manipulator with current joint configuration.
    move(robot,"joint",jointConfig)

    % Use fast update to move platform visualization frames.
    show3D(scenario,FastUpdate=true,Parent=ax,Visuals=visuals,Collisions=collisions);

    % Get robot end effector pose. Draw trajectory.
    hold on
    poseNow = scenario.TransformTree.getTransform(robot.ReferenceFrame,robot.Name + "_" + endEffectorName);
    jointSpaceMarker = plot3(poseNow(1,4),poseNow(2,4),poseNow(3,4),"r.",MarkerSize=12);
    drawnow
    hold off
    
    % Advance scenario simulation time.
    advance(scenario);
end

Place Target Box at Drop Location

Detach the target box from the rigidBodyTree of the manipulator to place the target box at the drop location.

detach(robot)

Visualize the scenario.

show3D(scenario,FastUpdate=true,Parent=ax,Visuals=visuals,Collisions=collisions);
title("Placed target box at drop location")

Plan Manipulator Motion Between Drop Location and Home Configuration

After placing the target box at drop location, manipulator moves back to the home configuration.

planner = manipulatorRRT(robot.RigidBodyTree,scenario.CollisionMeshes);
planner.MaxConnectionDistance = 0.1;
planner.ValidationDistance = 0.1;
planner.SkippedSelfCollisions = "parent";

Plan a path as a series of waypoints and interpolate between the waypoints to visualize the intermediate configurations of the planned motion.

path = plan(planner,dropConfig,initialConfig);
interpStates = interpolate(planner,path,numState);

Move the manipulator based on the planned state, and visualize each state in the scenario.

% Visualize scene
show3D(scenario,FastUpdate=true,Parent=ax,Visuals=visuals,Collisions=collisions);
title("Approaching towards home configuration")

for idx = 1:size(interpStates,1)
    % Get current joint configuration.
    jointConfig = interpStates(idx,:);

    % Move manipulator with current joint configuration.
    move(robot,"joint",jointConfig)

    % Use fast update to move platform visualization frames.
    show3D(scenario,FastUpdate=true,Parent=ax,Visuals=visuals,Collisions=collisions);

    % Get robot end effector pose. Draw trajectory.
    hold on
    poseNow = scenario.TransformTree.getTransform(robot.ReferenceFrame,robot.Name + "_" + endEffectorName);
    jointSpaceMarker = plot3(poseNow(1,4),poseNow(2,4),poseNow(3,4),"g.",MarkerSize=12);
    drawnow
    hold off
    
    % Advance scenario simulation time.
    advance(scenario);
end