Tutorial 9: Pick and Place Example#

Learning Objectives#

This is the final manipulator tutorial in a series of four tutorials. This tutorial tie everything together by showing how to use the UR10e robot and the 2F-140 gripper to follow a target and pick up a block. We will be using the robot imported in Tutorial 6: Setup a Manipulator, tuned in Tutorial 7: Configure a Manipulator, and the robot configuration file generated in Tutorial 8: Generate Robot Configuration File.

In this tutorial, we will be using the Lula kinematics solver to follow a target and the RMPFlow to pick up a block.

Note

All the files created in this tutorial are available at standalone_examples/api/isaacsim.robot.manipulators/ur10e for verification.

30 Minutes Tutorial

Prerequisites#

Note

If you have not completed the previous tutorial, you can find the prebuilt asset in the content browser at Isaac Sim/Samples/Rigging/Manipulator/configure_manipulator/ur10e/ur/ur_gripper.usd.

Gripper Control Example#

The script below uses the Parallel Gripper class to control the gripper joints and the Manipulator class to control the robot joints. Steps 0 to 400 close the gripper slowly. Steps 400 to 800, open the gripper slowly, and then reset the gripper position to 0.

Note

The provided script can be run using:

./python.sh standalone_examples/api/isaacsim.robot.manipulators/ur10e/gripper_control.py
gripper_control.py
from isaacsim import SimulationApp

simulation_app = SimulationApp({"headless": False})

import argparse

import numpy as np
from isaacsim.core.api import World
from isaacsim.core.utils.stage import add_reference_to_stage
from isaacsim.core.utils.types import ArticulationAction
from isaacsim.robot.manipulators import SingleManipulator
from isaacsim.robot.manipulators.grippers import ParallelGripper
from isaacsim.storage.native import get_assets_root_path

parser = argparse.ArgumentParser()
parser.add_argument("--test", default=False, action="store_true", help="Run in test mode")
args, unknown = parser.parse_known_args()

my_world = World(stage_units_in_meters=1.0)
assets_root_path = get_assets_root_path()
if assets_root_path is None:
    raise Exception("Could not find Isaac Sim assets folder")
asset_path = assets_root_path + "/Isaac/Samples/Rigging/Manipulator/configure_manipulator/ur10e/ur/ur_gripper.usd"
add_reference_to_stage(usd_path=asset_path, prim_path="/ur")
# define the gripper
gripper = ParallelGripper(
    # We chose the following values while inspecting the articulation
    end_effector_prim_path="/ur/ee_link/robotiq_arg2f_base_link",
    joint_prim_names=["finger_joint"],
    joint_opened_positions=np.array([0]),
    joint_closed_positions=np.array([40]),
    action_deltas=np.array([-40]),
    use_mimic_joints=True,
)
# define the manipulator
my_ur10 = my_world.scene.add(
    SingleManipulator(
        prim_path="/ur",
        name="ur10_robot",
        end_effector_prim_path="/ur/ee_link/robotiq_arg2f_base_link",
        gripper=gripper,
    )
)

my_world.scene.add_default_ground_plane()
my_world.reset()

i = 0
reset_needed = False
while simulation_app.is_running():
    my_world.step(render=True)
    if my_world.is_stopped() and not reset_needed:
        reset_needed = True
    if my_world.is_playing():
        if reset_needed:
            my_world.reset()
            reset_needed = False
        i += 1
        gripper_positions = my_ur10.gripper.get_joint_positions()
        if i < 400:
            # close the gripper slowly
            my_ur10.gripper.apply_action(ArticulationAction(joint_positions=[gripper_positions[0] + 0.1]))
        if i > 400:
            # open the gripper slowly
            my_ur10.gripper.apply_action(ArticulationAction(joint_positions=[gripper_positions[0] - 0.1]))
        if i == 800:
            i = 0
    if args.test is True:
        break

simulation_app.close()
../_images/isim_5.0_full_tut_gui_ur10e_gripper_control.webp

Follow Target Example using Lula Kinematics Solver#

Create a follow target task using the Lula kinematics solver, where you can specify the target position using a cube and the robot will move its end effector to the target position. The inverse kinematics solver will use the Lula robot descriptor created in the Tutorial 8: Generate Robot Configuration File tutorial.

The generated robot descriptor file is available at source/standalone_examples/api/isaacsim.robot.manipulators/ur10e/rmpflow/robot_descriptor.yaml.

Note

The provided script can be run using:

./python.sh standalone_examples/api/isaacsim.robot.manipulators/ur10e/follow_target_example.py

Move the cube to the target location and run the script to see the robot move its end effector to the target location.

../_images/isim_5.0_full_tut_gui_ur10e_follow_target.webp

The ik_solver.py script initializes the KinematicsSolver class and the LulaKinematicsSolver class.

controllers/ik_solver.py
import os
from typing import Optional

from isaacsim.core.prims import Articulation
from isaacsim.robot_motion.motion_generation import ArticulationKinematicsSolver, LulaKinematicsSolver


class KinematicsSolver(ArticulationKinematicsSolver):
    def __init__(self, robot_articulation: Articulation, end_effector_frame_name: Optional[str] = None) -> None:
        # TODO: change the config path
        self._kinematics = LulaKinematicsSolver(
            robot_description_path=os.path.join(os.path.dirname(__file__), "../rmpflow/robot_descriptor.yaml"),
            urdf_path=os.path.join(os.path.dirname(__file__), "../rmpflow/ur10e.urdf"),
        )
        if end_effector_frame_name is None:
            end_effector_frame_name = "ee_link_robotiq_arg2f_base_link"
        ArticulationKinematicsSolver.__init__(self, robot_articulation, self._kinematics, end_effector_frame_name)
        return

The follow_target.py script initializes the FollowTarget class and sets up the manipulator and parallel_gripper objects.

tasks/follow_target.py
from typing import Optional

import isaacsim.core.api.tasks as tasks
import numpy as np
from isaacsim.core.utils.stage import add_reference_to_stage
from isaacsim.robot.manipulators.grippers import ParallelGripper
from isaacsim.robot.manipulators.manipulators import SingleManipulator
from isaacsim.storage.native import get_assets_root_path


# Inheriting from the base class Follow Target
class FollowTarget(tasks.FollowTarget):
    def __init__(
        self,
        name: str = "ur10e_follow_target",
        target_prim_path: Optional[str] = None,
        target_name: Optional[str] = None,
        target_position: Optional[np.ndarray] = None,
        target_orientation: Optional[np.ndarray] = None,
        offset: Optional[np.ndarray] = None,
    ) -> None:
        tasks.FollowTarget.__init__(
            self,
            name=name,
            target_prim_path=target_prim_path,
            target_name=target_name,
            target_position=target_position,
            target_orientation=target_orientation,
            offset=offset,
        )
        return

    def set_robot(self) -> SingleManipulator:

        assets_root_path = get_assets_root_path()
        if assets_root_path is None:
            raise Exception("Could not find Isaac Sim assets folder")
        asset_path = (
            assets_root_path + "/Isaac/Samples/Rigging/Manipulator/configure_manipulator/ur10e/ur/ur_gripper.usd"
        )
        add_reference_to_stage(usd_path=asset_path, prim_path="/ur")
        # define the gripper
        gripper = ParallelGripper(
            # We chose the following values while inspecting the articulation
            end_effector_prim_path="/ur/ee_link/robotiq_arg2f_base_link",
            joint_prim_names=["finger_joint"],
            joint_opened_positions=np.array([0]),
            joint_closed_positions=np.array([40]),
            action_deltas=np.array([-40]),
            use_mimic_joints=True,
        )
        # define the manipulator
        manipulator = SingleManipulator(
            prim_path="/ur",
            name="ur10_robot",
            end_effector_prim_path="/ur/ee_link/robotiq_arg2f_base_link",
            gripper=gripper,
        )
        return manipulator

The follow_target_example.py script initializes the FollowTarget task and the KinematicsSolver created in the previous step with a target location for the cube to be followed by the end effector.

follow_target_example.py
import argparse

parser = argparse.ArgumentParser()
parser.add_argument("--test", action="store_true", help="Run in test mode.")
args, unknown = parser.parse_known_args()

from isaacsim import SimulationApp

simulation_app = SimulationApp({"headless": False})

import numpy as np
from controller.ik_solver import KinematicsSolver
from isaacsim.core.api import World
from tasks.follow_target import FollowTarget

my_world = World(stage_units_in_meters=1.0)
# Initialize the Follow Target task with a target location for the cube to be followed by the end effector
my_task = FollowTarget(name="ur10e_follow_target", target_position=np.array([0.5, 0, 0.5]))
my_world.add_task(my_task)
my_world.reset()
task_params = my_world.get_task("ur10e_follow_target").get_params()
target_name = task_params["target_name"]["value"]
ur10e_name = task_params["robot_name"]["value"]
my_ur10e = my_world.scene.get_object(ur10e_name)

# initialize the ik solver
ik_solver = KinematicsSolver(my_ur10e)
articulation_controller = my_ur10e.get_articulation_controller()

# run the simulation
i = 0
while simulation_app.is_running() and (not args.test or i < 100):
    my_world.step(render=True)
    if my_world.is_playing():
        if my_world.current_time_step_index == 0:
            my_world.reset()

        observations = my_world.get_observations()
        actions, succ = ik_solver.compute_inverse_kinematics(
            target_position=observations[target_name]["position"],
            target_orientation=observations[target_name]["orientation"],
        )
        if succ:
            articulation_controller.apply_action(actions)
        else:
            print("IK did not converge to a solution.  No action is being taken.")
    i += 1
simulation_app.close()

RMP Flow Configuration#

Use RMPFlow to control the end effector. See RMPflow for more details about RMPFlow.

The ur10e_rmpflow_common.yaml file is available at source/standalone_examples/api/isaacsim.robot.manipulators/ur10e/rmpflow/ur10e_rmpflow_common.yaml, it specifies various parameters for the RMPFlow controller.

rmpflow/ur10e_rmpflow_common.yaml
 1joint_limit_buffers: [.01, .01, .01, .01, .01, .01]
 2rmp_params:
 3    cspace_target_rmp:
 4        metric_scalar: 50.
 5        position_gain: 100.
 6        damping_gain: 50.
 7        robust_position_term_thresh: .5
 8        inertia: 1.
 9    cspace_trajectory_rmp:
10        p_gain: 100.
11        d_gain: 10.
12        ff_gain: .25
13        weight: 50.
14    cspace_affine_rmp:
15        final_handover_time_std_dev: .25
16        weight: 2000.
17    joint_limit_rmp:
18        metric_scalar: 1000.
19        metric_length_scale: .01
20        metric_exploder_eps: 1e-3
21        metric_velocity_gate_length_scale: .01
22        accel_damper_gain: 200.
23        accel_potential_gain: 1.
24        accel_potential_exploder_length_scale: .1
25        accel_potential_exploder_eps: 1e-2
26    joint_velocity_cap_rmp:
27        max_velocity: 1.
28        velocity_damping_region: .3
29        damping_gain: 1000.0
30        metric_weight: 100.
31    target_rmp:
32        accel_p_gain: 30.
33        accel_d_gain: 85.
34        accel_norm_eps: .075
35        metric_alpha_length_scale: .05
36        min_metric_alpha: .01
37        max_metric_scalar: 10000
38        min_metric_scalar: 2500
39        proximity_metric_boost_scalar: 20.
40        proximity_metric_boost_length_scale: .02
41        xi_estimator_gate_std_dev: 20000.
42        accept_user_weights: false
43    axis_target_rmp:
44        accel_p_gain: 210.
45        accel_d_gain: 60.
46        metric_scalar: 10
47        proximity_metric_boost_scalar: 3000.
48        proximity_metric_boost_length_scale: .08
49        xi_estimator_gate_std_dev: 20000.
50        accept_user_weights: false
51    collision_rmp:
52        damping_gain: 50.
53        damping_std_dev: .04
54        damping_robustness_eps: 1e-2
55        damping_velocity_gate_length_scale: .01
56        repulsion_gain: 800.
57        repulsion_std_dev: .01
58        metric_modulation_radius: .5
59        metric_scalar: 10000.
60        metric_exploder_std_dev: .02
61        metric_exploder_eps: .001
62    damping_rmp:
63        accel_d_gain: 30.
64        metric_scalar: 50.
65        inertia: 100.
66canonical_resolve:
67    max_acceleration_norm: 50.
68    projection_tolerance: .01
69    verbose: false
70body_cylinders:
71    - name: base
72    pt1: [0,0,.10]
73    pt2: [0,0,0.]
74    radius: .2
75body_collision_controllers:
76    - name: ee_link_robotiq_arg2f_base_link
77    radius: .05

Follow Target Example using RMP Flow#

Create an RMP flow controller to move the robot end effector to the target location.

Note

The provided script can be run using:

./python.sh standalone_examples/api/isaacsim.robot.manipulators/ur10e/follow_target_example_rmpflow.py
../_images/isim_5.0_full_tut_gui_ur10e_follow_target_rmp.webp

The rmpflow.py initializes Lula motion generation policy using the ur10e_rmpflow_common.yaml file above, the ur10e.urdf and the robot descriptor file created in Tutorial 8: Generate Robot Configuration File.

controllers/rmpflow.py
import os

import isaacsim.robot_motion.motion_generation as mg
from isaacsim.core.prims import Articulation


class RMPFlowController(mg.MotionPolicyController):
    def __init__(self, name: str, robot_articulation: Articulation, physics_dt: float = 1.0 / 60.0) -> None:

        self.rmpflow = mg.lula.motion_policies.RmpFlow(
            robot_description_path=os.path.join(os.path.dirname(__file__), "../rmpflow/robot_descriptor.yaml"),
            rmpflow_config_path=os.path.join(os.path.dirname(__file__), "../rmpflow/ur10e_rmpflow_common.yaml"),
            urdf_path=os.path.join(os.path.dirname(__file__), "../rmpflow/ur10e.urdf"),
            end_effector_frame_name="ee_link_robotiq_arg2f_base_link",
            maximum_substep_size=0.00334,
        )

        self.articulation_rmp = mg.ArticulationMotionPolicy(robot_articulation, self.rmpflow, physics_dt)

        mg.MotionPolicyController.__init__(self, name=name, articulation_motion_policy=self.articulation_rmp)
        self._default_position, self._default_orientation = (
            self._articulation_motion_policy._robot_articulation.get_world_pose()
        )
        self._motion_policy.set_robot_base_pose(
            robot_position=self._default_position, robot_orientation=self._default_orientation
        )
        return

    def reset(self):
        mg.MotionPolicyController.reset(self)
        self._motion_policy.set_robot_base_pose(
            robot_position=self._default_position, robot_orientation=self._default_orientation
        )

The follow_target_example_rmpflow.py script initializes the FollowTarget task and the RMPFlowController created in the previous step with a target location for the cube to be followed by the end effector.

follow_target_example_rmpflow.py
import argparse

parser = argparse.ArgumentParser()
parser.add_argument("--test", action="store_true", help="Run in test mode.")
args, unknown = parser.parse_known_args()

from isaacsim import SimulationApp

simulation_app = SimulationApp({"headless": False})

import numpy as np
from controller.rmpflow import RMPFlowController
from isaacsim.core.api import World
from tasks.follow_target import FollowTarget

my_world = World(stage_units_in_meters=1.0)
# Initialize the Follow Target task with a target location for the cube to be followed by the end effector
my_task = FollowTarget(name="ur10e_follow_target", target_position=np.array([0.5, 0, 0.5]))
my_world.add_task(my_task)
my_world.reset()
task_params = my_world.get_task("ur10e_follow_target").get_params()
target_name = task_params["target_name"]["value"]
ur10e_name = task_params["robot_name"]["value"]
my_ur10e = my_world.scene.get_object(ur10e_name)
articulation_controller = my_ur10e.get_articulation_controller()


# initialize the ik solver
my_controller = RMPFlowController(name="target_follower_controller", robot_articulation=my_ur10e)
my_controller.reset()

# run the simulation
i = 0
while simulation_app.is_running() and (not args.test or i < 100):
    my_world.step(render=True)
    if my_world.is_playing():
        if my_world.current_time_step_index == 0:
            my_world.reset()
            my_controller.reset()

        observations = my_world.get_observations()
        actions = my_controller.forward(
            target_end_effector_position=observations[target_name]["position"],
            target_end_effector_orientation=observations[target_name]["orientation"],
        )
        articulation_controller.apply_action(actions)
    i += 1
simulation_app.close()

Basic Pick and Place Task using RMP Flow#

Use the RMPFlow controller to pick up a block and place it in a target location.

Note

The provided script can be run using:

./python.sh standalone_examples/api/isaacsim.robot.manipulators/ur10e/pick_up_example.py
../_images/isim_5.0_full_tut_gui_ur10e_pick_place_rmp.webp

The controllers/pick_place.py script creates a PickPlace controller that will pick up a block and place it in a target location.

controllers/pick_place.py
import isaacsim.robot.manipulators.controllers as manipulators_controllers
from isaacsim.core.prims import SingleArticulation
from isaacsim.robot.manipulators.grippers import ParallelGripper

from .rmpflow import RMPFlowController


class PickPlaceController(manipulators_controllers.PickPlaceController):
    def __init__(
        self, name: str, gripper: ParallelGripper, robot_articulation: SingleArticulation, events_dt=None
    ) -> None:
        if events_dt is None:
            events_dt = [0.005, 0.002, 1, 0.05, 0.0008, 0.005, 0.0008, 0.1, 0.0008, 0.008]
        manipulators_controllers.PickPlaceController.__init__(
            self,
            name=name,
            cspace_controller=RMPFlowController(
                name=name + "_cspace_controller", robot_articulation=robot_articulation
            ),
            gripper=gripper,
            events_dt=events_dt,
            end_effector_initial_height=0.6,
        )
        return

The tasks/pick_place.py script creates a PickPlace task that sets up the UR10e manipulator and the gripper to pick up a block and place it in a target location.

tasks/pick_place.py
from typing import Optional

import isaacsim.core.api.tasks as tasks
import numpy as np
from isaacsim.core.utils.stage import add_reference_to_stage
from isaacsim.robot.manipulators.grippers import ParallelGripper
from isaacsim.robot.manipulators.manipulators import SingleManipulator
from isaacsim.storage.native import get_assets_root_path


class PickPlace(tasks.PickPlace):
    def __init__(
        self,
        name: str = "ur10e_pick_place",
        cube_initial_position: Optional[np.ndarray] = None,
        cube_initial_orientation: Optional[np.ndarray] = None,
        target_position: Optional[np.ndarray] = None,
        offset: Optional[np.ndarray] = None,
        cube_size: Optional[np.ndarray] = np.array([0.0515, 0.0515, 0.0515]),
    ) -> None:
        tasks.PickPlace.__init__(
            self,
            name=name,
            cube_initial_position=cube_initial_position,
            cube_initial_orientation=cube_initial_orientation,
            target_position=target_position,
            cube_size=cube_size,
            offset=offset,
        )
        return

    def set_robot(self) -> SingleManipulator:
        assets_root_path = get_assets_root_path()
        if assets_root_path is None:
            raise Exception("Could not find Isaac Sim assets folder")
        asset_path = (
            assets_root_path + "/Isaac/Samples/Rigging/Manipulator/configure_manipulator/ur10e/ur/ur_gripper.usd"
        )
        add_reference_to_stage(usd_path=asset_path, prim_path="/ur")
        # define the gripper
        gripper = ParallelGripper(
            # We chose the following values while inspecting the articulation
            end_effector_prim_path="/ur/ee_link/robotiq_arg2f_base_link",
            joint_prim_names=["finger_joint"],
            joint_opened_positions=np.array([0]),
            joint_closed_positions=np.array([40]),
            action_deltas=np.array([-40]),
            use_mimic_joints=True,
        )
        # define the manipulator
        manipulator = SingleManipulator(
            prim_path="/ur",
            name="ur10_robot",
            end_effector_prim_path="/ur/ee_link/robotiq_arg2f_base_link",
            gripper=gripper,
        )
        return manipulator

The pick_place_example.py script puts everything together and runs the simulation.

Important

Make sure to tune the end_effector_offset parameter to get the best results, this is the offset between the end effector link on the robot and optimal grasp position for the claw.

pick_place_example.py
import argparse

parser = argparse.ArgumentParser()
parser.add_argument("--test", action="store_true", help="Run in headless mode.")
args, unknown = parser.parse_known_args()

from isaacsim import SimulationApp

simulation_app = SimulationApp({"headless": False})
import numpy as np
from controller.pick_place import PickPlaceController
from isaacsim.core.api import World
from tasks.pick_place import PickPlace

my_world = World(stage_units_in_meters=1.0, physics_dt=1 / 200, rendering_dt=20 / 200)

target_position = np.array([-0.3, 0.6, 0])
target_position[2] = 0.0515 / 2.0
my_task = PickPlace(name="ur10e_pick_place", target_position=target_position, cube_size=np.array([0.1, 0.0515, 0.1]))
my_world.add_task(my_task)
my_world.reset()
task_params = my_world.get_task("ur10e_pick_place").get_params()
ur10e_name = task_params["robot_name"]["value"]
my_ur10e = my_world.scene.get_object(ur10e_name)
# initialize the controller

my_controller = PickPlaceController(name="controller", robot_articulation=my_ur10e, gripper=my_ur10e.gripper)
task_params = my_world.get_task("ur10e_pick_place").get_params()
articulation_controller = my_ur10e.get_articulation_controller()

reset_needed = False

i = 0
while simulation_app.is_running() and (not args.test or i < 100):
    my_world.step(render=True)
    if my_world.is_playing():
        if reset_needed:
            my_world.reset()
            reset_needed = False
            my_controller.reset()
        if my_world.current_time_step_index == 0:
            my_controller.reset()

        observations = my_world.get_observations()
        # forward the observation values to the controller to get the actions
        actions = my_controller.forward(
            picking_position=observations[task_params["cube_name"]["value"]]["position"],
            placing_position=observations[task_params["cube_name"]["value"]]["target_position"],
            current_joint_positions=observations[task_params["robot_name"]["value"]]["joint_positions"],
            # This offset needs tuning as well
            end_effector_offset=np.array([0, 0, 0.20]),
        )
        if my_controller.is_done():
            print("done picking and placing")
        articulation_controller.apply_action(actions)
    if my_world.is_stopped():
        reset_needed = True
    i += 1


simulation_app.close()

Advanced Pick and Place Task using RMPFlow and Foundation Pose#

In the pick and place example above, you used the RMPFlow controller to pick up a block and place it in a target location. However there are some limitations to this approach.

  • The robot gets the cube pose directly from the simulator observation, which does not translate to the real world.

  • The class set up is limited to the cube, and in real life different objects have different shapes and sizes, and different grasping strategies.

To address these limitations, see Isaac Manipulator tutorials for more advanced pick and place tasks.

Summary#

In this tutorial, you learned how to use the Lula kinematics solver to follow a target and the RMPFlow to pick up a block.