Import a Manipulator#

Learning Objectives#

This tutorial shows how to import a new manipulator into NVIDIA Isaac Sim, create a controller for it, and perform a pick and place task.

After this tutorial, you will be able to create a pick and place task as well as its controller for a new manipulator in NVIDIA Isaac Sim. In this tutorial we will use the COBOTTA pro 900 robot from denso.

Note that all the files created in this tutorial are available at standalone_examples/api/isaacsim.robot.manipulators/cobotta_900 for verification.

30-35 Minutes Tutorial

Getting Started#

Prerequisites

  • Review the Core API Hello World and GUI Tutorial series Tutorials prior to beginning this tutorial.

Inspect the Articulation#

  1. Start NVIDIA Isaac Sim and open the Isaac Sim Asset Browser, go to Robots > Denso > COBOTTA pro 900 and drag the cobotta_pro_900.usd file into the stage.

  2. Optionally, you may also download the usd file by right clicking on the file in the Asset Browser and selecting Download.

Let’s make sure that the joints are responsive for position control.

  1. Create a new stage through File > New. A defaultLight is added to the new stage as seen.

  2. Create XForm under World and rename it to denso.

  3. Add the robot USD as a reference to the new denso xform.

  4. Add a physics scene to the stage.

  5. Open the Physics Inspector through Tools -> Physics -> Physics Inspector and then on the Physics Tool bar.

  6. Select the COBOTTA robot in the stage and click on the refresh button in the Physics Inspector extension window.

  7. Familiarize yourself with the dofs in the articulation under DOF View. Notice that each of the dofs has a default target position as 0 and that the gripper has 6 joints. Additionally take a note of the gripper dof names to use with the Parallel Gripper class provided in NVIDIA Isaac Sim, to simplify the pick and place controller we will use 2 joints only for the gripper control. (specifically we will use “finger_joint” and “right_outer_knuckle_joint” to grip the object and we will set dof 7 and 8 to 0.628 so that they are out of the way when the other joints grip the cube). Inspect these dofs further to understand the gripper better.

  8. Try changing the target position with the slider and double check that the dof position reaches the target specified.

../_images/isim_4.5_full_tut_gui_physics_inspector.png

Note

To know more about the Physics Inspector usage, go to Physics Inspector

Controlling the Gripper#

Next we start a simple script where we open and close the gripper repeatedly in order to verify that we can load the USD file and do simple control with it.

  1. Create a new Python file under a new folder COBOTTA_SCRIPTS_PATH/gripper_control.py (Note: this example uses the standalone workflow).

     1from isaacsim import SimulationApp
     2
     3simulation_app = SimulationApp({"headless": False})
     4
     5from isaacsim.core.api import World
     6from isaacsim.robot.manipulators.manipulators import SingleManipulator
     7from isaacsim.robot.manipulators.grippers import ParallelGripper
     8from isaacsim.core.utils.stage import add_reference_to_stage
     9from isaacsim.core.utils.types import ArticulationAction
    10import numpy as np
    11
    12my_world = World(stage_units_in_meters=1.0)
    13
    14# use Isaac Sim provided asset
    15asset_path = assets_root_path + "/Isaac/Robots/Denso/cobotta_pro_900.usd"
    16
    17#TODO: change this to your own path if you downloaded the asset
    18# asset_path = "/home/user_name/cobotta_pro_900/cobotta_pro_900/cobotta_pro_900.usd"
    19
    20add_reference_to_stage(usd_path=asset_path, prim_path="/World/cobotta")
    21#define the gripper
    22gripper = ParallelGripper(
    23    #We chose the following values while inspecting the articulation
    24    end_effector_prim_path="/World/cobotta/onrobot_rg6_base_link",
    25    joint_prim_names=["finger_joint", "right_outer_knuckle_joint"],
    26    joint_opened_positions=np.array([0, 0]),
    27    joint_closed_positions=np.array([0.628, -0.628]),
    28    action_deltas=np.array([-0.628, 0.628]),
    29)
    30#define the manipulator
    31my_denso = my_world.scene.add(SingleManipulator(prim_path="/World/cobotta", name="cobotta_robot",
    32                                                end_effector_prim_name="onrobot_rg6_base_link", gripper=gripper))
    33#set the default positions of the other gripper joints to be opened so
    34#that its out of the way of the joints we want to control when gripping an object for instance.
    35joints_default_positions = np.zeros(12)
    36joints_default_positions[7] = 0.628
    37joints_default_positions[8] = 0.628
    38my_denso.set_joints_default_state(positions=joints_default_positions)
    39my_world.scene.add_default_ground_plane()
    40my_world.reset()
    41
    42i = 0
    43while simulation_app.is_running():
    44    my_world.step(render=True)
    45    if my_world.is_playing():
    46        if my_world.current_time_step_index == 0:
    47            my_world.reset()
    48        i += 1
    49        gripper_positions = my_denso.gripper.get_joint_positions()
    50        if i < 500:
    51            #close the gripper slowly
    52            my_denso.gripper.apply_action(
    53                ArticulationAction(joint_positions=[gripper_positions[0] + 0.1, gripper_positions[1] - 0.1]))
    54        if i > 500:
    55            #open the gripper slowly
    56            my_denso.gripper.apply_action(
    57                ArticulationAction(joint_positions=[gripper_positions[0] - 0.1, gripper_positions[1] + 0.1]))
    58        if i == 1000:
    59            i = 0
    60
    61simulation_app.close()
    
  2. Run it using ./python.sh standalone_examples/api/isaacsim.robot.manipulators/cobotta_900/gripper_control.py.

../_images/isaac_sim_advanced_add_new_manipulator_8.gif

Adding a Follow Target Task#

Next we add a follow target task with a target cube for the cobotta robot to follow with its end effector.

  1. Create a new Python file under a new folder COBOTTA_SCRIPTS_PATH/tasks/follow_target.py to define the task class there.

     1from isaacsim.robot.manipulators.manipulators import SingleManipulator
     2from isaacsim.robot.manipulators.grippers import ParallelGripper
     3from isaacsim.core.utils.stage import add_reference_to_stage
     4import isaacsim.core.api.tasks as tasks
     5from typing import Optional
     6import numpy as np
     7
     8
     9# Inheriting from the base class Follow Target
    10class FollowTarget(tasks.FollowTarget):
    11    def __init__(
    12        self,
    13        name: str = "denso_follow_target",
    14        target_prim_path: Optional[str] = None,
    15        target_name: Optional[str] = None,
    16        target_position: Optional[np.ndarray] = None,
    17        target_orientation: Optional[np.ndarray] = None,
    18        offset: Optional[np.ndarray] = None,
    19    ) -> None:
    20        tasks.FollowTarget.__init__(
    21            self,
    22            name=name,
    23            target_prim_path=target_prim_path,
    24            target_name=target_name,
    25            target_position=target_position,
    26            target_orientation=target_orientation,
    27            offset=offset,
    28        )
    29        return
    30
    31    def set_robot(self) -> SingleManipulator:
    32        #TODO: change this to the robot USD file.
    33        asset_path = "/home/user_name/cobotta_pro_900/cobotta_pro_900/cobotta_pro_900.usd"
    34        add_reference_to_stage(usd_path=asset_path, prim_path="/World/cobotta")
    35        gripper = ParallelGripper(
    36            end_effector_prim_path="/World/cobotta/onrobot_rg6_base_link",
    37            joint_prim_names=["finger_joint", "right_outer_knuckle_joint"],
    38            joint_opened_positions=np.array([0, 0]),
    39            joint_closed_positions=np.array([0.628, -0.628]),
    40            action_deltas=np.array([-0.628, 0.628]))
    41        manipulator = SingleManipulator(prim_path="/World/cobotta",
    42                                        name="cobotta_robot",
    43                                        end_effector_prim_name="onrobot_rg6_base_link",
    44                                        gripper=gripper)
    45        joints_default_positions = np.zeros(12)
    46        joints_default_positions[7] = 0.628
    47        joints_default_positions[8] = 0.628
    48        manipulator.set_joints_default_state(positions=joints_default_positions)
    49        return manipulator
    
  2. Create a new Python file at COBOTTA_SCRIPTS_PATH/follow_target_example.py to run the follow target example.

     1from isaacsim import SimulationApp
     2
     3simulation_app = SimulationApp({"headless": False})
     4
     5from isaacsim.core.api import World
     6from tasks.follow_target import FollowTarget
     7import numpy as np
     8
     9my_world = World(stage_units_in_meters=1.0)
    10#Initialize the Follow Target task with a target location for the cube to be followed by the end effector
    11my_task = FollowTarget(name="denso_follow_target", target_position=np.array([0.5, 0, 0.5]))
    12my_world.add_task(my_task)
    13my_world.reset()
    14task_params = my_world.get_task("denso_follow_target").get_params()
    15target_name = task_params["target_name"]["value"]
    16denso_name = task_params["robot_name"]["value"]
    17my_denso = my_world.scene.get_object(denso_name)
    18articulation_controller = my_denso.get_articulation_controller()
    19while simulation_app.is_running():
    20    my_world.step(render=True)
    21    if my_world.is_playing():
    22        if my_world.current_time_step_index == 0:
    23            my_world.reset()
    24        observations = my_world.get_observations()
    25        print(observations)
    26simulation_app.close()
    
    ../_images/isaac_sim_advanced_add_new_manipulator_9.png
  3. Run it using ./python.sh standalone_examples/api/isaacsim.robot.manipulators/cobotta_900/follow_target_example.py.

  4. Next we try to follow the target with a plain Inverse Kinemetics solver provided in NVIDIA Isaac Sim. We need to point the solver to the URDF and the robot description files, so let’s create new YAML file at COBOTTA_SCRIPTS_PATH/rmpflow/robot_descriptor.yaml.

  5. Copy the following into the robot description file

     1api_version: 1.0
     2cspace:
     3    - joint_1
     4    - joint_2
     5    - joint_3
     6    - joint_4
     7    - joint_5
     8    - joint_6
     9root_link: world
    10default_q: [
    11    0.00, 0.00, 0.00, 0.00, 0.00, 0.00
    12]
    13cspace_to_urdf_rules: []
    14composite_task_spaces: []
    
  6. Create a new Python file at COBOTTA_SCRIPTS_PATH/ik_solver.py to define the IK solver class there.

    The URDF is available locally at <Isaac Sim Folder>/extscache/isaacsim.asset.importer.urdf-<Version Number>/data/urdf/robots/cobotta_pro_900.

     1from isaacsim.robot_motion.motion_generation import ArticulationKinematicsSolver, LulaKinematicsSolver
     2from isaacsim.core.prims import Articulation
     3from typing import Optional
     4
     5
     6class KinematicsSolver(ArticulationKinematicsSolver):
     7    def __init__(self, robot_articulation: Articulation, end_effector_frame_name: Optional[str] = None) -> None:
     8        #TODO: change the config path
     9        self._kinematics = LulaKinematicsSolver(robot_description_path="COBOTTA_SCRIPTS_PATH/rmpflow/robot_descriptor.yaml",
    10                                                urdf_path="/home/user_name/cobotta_pro_900/cobotta_pro_900.urdf")
    11        if end_effector_frame_name is None:
    12            end_effector_frame_name = "onrobot_rg6_base_link"
    13        ArticulationKinematicsSolver.__init__(self, robot_articulation, self._kinematics, end_effector_frame_name)
    14        return
    
  7. Modify the COBOTTA_SCRIPTS_PATH/follow_target_example.py script to follow the target using an IK solver.

     1from isaacsim import SimulationApp
     2
     3simulation_app = SimulationApp({"headless": False})
     4
     5from isaacsim.core.api import World
     6from tasks.follow_target import FollowTarget
     7import numpy as np
     8from ik_solver import KinematicsSolver
     9import carb
    10
    11my_world = World(stage_units_in_meters=1.0)
    12#Initialize the Follow Target task with a target location for the cube to be followed by the end effector
    13my_task = FollowTarget(name="denso_follow_target", target_position=np.array([0.5, 0, 0.5]))
    14my_world.add_task(my_task)
    15my_world.reset()
    16task_params = my_world.get_task("denso_follow_target").get_params()
    17target_name = task_params["target_name"]["value"]
    18denso_name = task_params["robot_name"]["value"]
    19my_denso = my_world.scene.get_object(denso_name)
    20#initialize the controller
    21my_controller = KinematicsSolver(my_denso)
    22articulation_controller = my_denso.get_articulation_controller()
    23while simulation_app.is_running():
    24    my_world.step(render=True)
    25    if my_world.is_playing():
    26        if my_world.current_time_step_index == 0:
    27            my_world.reset()
    28        observations = my_world.get_observations()
    29        actions, succ = my_controller.compute_inverse_kinematics(
    30            target_position=observations[target_name]["position"],
    31            target_orientation=observations[target_name]["orientation"],
    32        )
    33        if succ:
    34            articulation_controller.apply_action(actions)
    35        else:
    36            carb.log_warn("IK did not converge to a solution.  No action is being taken.")
    37simulation_app.close()
    
    ../_images/isaac_sim_advanced_add_new_manipulator_10.gif
  8. Run it using ./python.sh standalone_examples/api/isaacsim.robot.manipulators/cobotta_900/follow_target_example.py.

Note

The robot doesn’t have self collision enabled for tutorial purposes only so the cobotta robot links might overlap in some scenarios.

Using RMPflow to Control the Robot#

  1. Next we try using RMPFlow to follow the target so let’s create a new YAML file at COBOTTA_SCRIPTS_PATH/rmpflow/denso_rmpflow_common.yaml and copy the following into it.

     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,.333]
    73      pt2: [0,0,0.]
    74      radius: .05
    75body_collision_controllers:
    76    - name: onrobot_rg6_base_link
    77      radius: .05
    
  2. Create a new Python file under a new folder COBOTTA_SCRIPTS_PATH/controllers/rmpflow.py to define the RMPFlow Controller class there.

    The URDF is available locally at <Isaac Sim Folder>/extscache/isaacsim.asset.importer.urdf-<Version Number>/data/urdf/robots/cobotta_pro_900.

     1import isaacsim.robot_motion.motion_generation as mg
     2from isaacsim.core.prims import Articulation
     3
     4
     5class RMPFlowController(mg.MotionPolicyController):
     6    def __init__(self, name: str, robot_articulation: Articulation, physics_dt: float = 1.0 / 60.0) -> None:
     7        # TODO: change the follow paths
     8        self.rmpflow = mg.lula.motion_policies.RmpFlow(robot_description_path="COBOTTA_SCRIPTS_PATH/rmpflow/robot_descriptor.yaml",
     9                                                        rmpflow_config_path="COBOTTA_SCRIPTS_PATH/rmpflow/denso_rmpflow_common.yaml",
    10                                                        urdf_path="/home/user_name/cobotta_pro_900/cobotta_pro_900.urdf",
    11                                                        end_effector_frame_name="onrobot_rg6_base_link",
    12                                                        maximum_substep_size=0.00334)
    13
    14        self.articulation_rmp = mg.ArticulationMotionPolicy(robot_articulation, self.rmpflow, physics_dt)
    15
    16        mg.MotionPolicyController.__init__(self, name=name, articulation_motion_policy=self.articulation_rmp)
    17        self._default_position, self._default_orientation = (
    18            self._articulation_motion_policy._robot_articulation.get_world_pose()
    19        )
    20        self._motion_policy.set_robot_base_pose(
    21            robot_position=self._default_position, robot_orientation=self._default_orientation
    22        )
    23        return
    24
    25    def reset(self):
    26        mg.MotionPolicyController.reset(self)
    27        self._motion_policy.set_robot_base_pose(
    28            robot_position=self._default_position, robot_orientation=self._default_orientation
    29        )
    
  3. Modify the COBOTTA_SCRIPTS_PATH/follow_target_example.py script to follow the target using an IK solver.

     1from isaacsim import SimulationApp
     2
     3simulation_app = SimulationApp({"headless": False})
     4
     5from isaacsim.core.api import World
     6from tasks.follow_target import FollowTarget
     7import numpy as np
     8from controllers.rmpflow import RMPFlowController
     9
    10my_world = World(stage_units_in_meters=1.0)
    11#Initialize the Follow Target task with a target location for the cube to be followed by the end effector
    12my_task = FollowTarget(name="denso_follow_target", target_position=np.array([0.5, 0, 0.5]))
    13my_world.add_task(my_task)
    14my_world.reset()
    15task_params = my_world.get_task("denso_follow_target").get_params()
    16target_name = task_params["target_name"]["value"]
    17denso_name = task_params["robot_name"]["value"]
    18my_denso = my_world.scene.get_object(denso_name)
    19#initialize the controller
    20my_controller = RMPFlowController(name="target_follower_controller", robot_articulation=my_denso)
    21my_controller.reset()
    22articulation_controller = my_denso.get_articulation_controller()
    23while simulation_app.is_running():
    24    my_world.step(render=True)
    25    if my_world.is_playing():
    26        if my_world.current_time_step_index == 0:
    27            my_world.reset()
    28            my_controller.reset()
    29        observations = my_world.get_observations()
    30        actions = my_controller.forward(
    31            target_end_effector_position=observations[target_name]["position"],
    32            target_end_effector_orientation=observations[target_name]["orientation"],
    33        )
    34        articulation_controller.apply_action(actions)
    35simulation_app.close()
    
  4. Run it using ./python.sh standalone_examples/api/isaacsim.robot.manipulators/cobotta_900/follow_target_example.py.

Adding a PickPlace Task#

Next we add a pick and place task with a cube for the cobotta robot to pick and place at a target goal location.

  1. Create a new Python file at COBOTTA_SCRIPTS_PATH/tasks/pick_place.py to define the task class there.

     1from isaacsim.robot.manipulators.manipulators import SingleManipulator
     2from isaacsim.robot.manipulators.grippers import ParallelGripper
     3from isaacsim.core.utils.stage import add_reference_to_stage
     4import isaacsim.core.api.tasks as tasks
     5from typing import Optional
     6import numpy as np
     7
     8
     9class PickPlace(tasks.PickPlace):
    10    def __init__(
    11        self,
    12        name: str = "denso_pick_place",
    13        cube_initial_position: Optional[np.ndarray] = None,
    14        cube_initial_orientation: Optional[np.ndarray] = None,
    15        target_position: Optional[np.ndarray] = None,
    16        offset: Optional[np.ndarray] = None,
    17    ) -> None:
    18        tasks.PickPlace.__init__(
    19            self,
    20            name=name,
    21            cube_initial_position=cube_initial_position,
    22            cube_initial_orientation=cube_initial_orientation,
    23            target_position=target_position,
    24            cube_size=np.array([0.0515, 0.0515, 0.0515]),
    25            offset=offset,
    26        )
    27        return
    28
    29    def set_robot(self) -> SingleManipulator:
    30        #TODO: change the asset path here
    31        asset_path = "/home/user_name/cobotta_pro_900/cobotta_pro_900/cobotta_pro_900.usd"
    32        add_reference_to_stage(usd_path=asset_path, prim_path="/World/cobotta")
    33        gripper = ParallelGripper(
    34            end_effector_prim_path="/World/cobotta/onrobot_rg6_base_link",
    35            joint_prim_names=["finger_joint", "right_outer_knuckle_joint"],
    36            joint_opened_positions=np.array([0, 0]),
    37            joint_closed_positions=np.array([0.628, -0.628]),
    38            action_deltas=np.array([-0.2, 0.2]) )
    39        manipulator = SingleManipulator(prim_path="/World/cobotta",
    40                                        name="cobotta_robot",
    41                                        end_effector_prim_name="onrobot_rg6_base_link",
    42                                        gripper=gripper)
    43        joints_default_positions = np.zeros(12)
    44        joints_default_positions[7] = 0.628
    45        joints_default_positions[8] = 0.628
    46        manipulator.set_joints_default_state(positions=joints_default_positions)
    47        return manipulator
    
  2. Create a new Python file at COBOTTA_SCRIPTS_PATH/pick_up_example.py to run the follow target example.

     1from isaacsim import SimulationApp
     2
     3simulation_app = SimulationApp({"headless": False})
     4
     5from isaacsim.core.api import World
     6import numpy as np
     7from tasks.pick_place import PickPlace
     8
     9my_world = World(stage_units_in_meters=1.0)
    10
    11
    12target_position = np.array([-0.3, 0.6, 0])
    13target_position[2] = 0.0515 / 2.0
    14my_task = PickPlace(name="denso_pick_place", target_position=target_position)
    15my_world.add_task(my_task)
    16my_world.reset()
    17task_params = my_world.get_task("denso_pick_place").get_params()
    18denso_name = task_params["robot_name"]["value"]
    19my_denso = my_world.scene.get_object(denso_name)
    20articulation_controller = my_denso.get_articulation_controller()
    21while simulation_app.is_running():
    22    my_world.step(render=True)
    23    if my_world.is_playing():
    24        if my_world.current_time_step_index == 0:
    25            my_world.reset()
    26        observations = my_world.get_observations()
    27        print(observations)
    28simulation_app.close()
    
    ../_images/isaac_sim_advanced_add_new_manipulator_11.png
  3. Run it using ./python.sh standalone_examples/api/isaacsim.robot.manipulators/cobotta_900/pick_up_example.py.

Adding a PickPlace Controller#

  1. Next we try to pick and place the cube with a simple PickPlaceController provided in NVIDIA Isaac Sim. Create a new Python file at COBOTTA_SCRIPTS_PATH/controllers/pick_place.py and copy the following into it.

     1import isaacsim.robot.manipulators.controllers as manipulators_controllers
     2from isaacsim.robot.manipulators.grippers import ParallelGripper
     3from .rmpflow import RMPFlowController
     4from isaacsim.core.prims import Articulation
     5
     6
     7class PickPlaceController(manipulators_controllers.PickPlaceController):
     8    def __init__(
     9        self,
    10        name: str,
    11        gripper: ParallelGripper,
    12        robot_articulation: Articulation,
    13        events_dt=None
    14    ) -> None:
    15        if events_dt is None:
    16            #These values needs to be tuned in general, you checkout each event in execution and slow it down or speed
    17            #it up depends on how smooth the movements are
    18            events_dt = [0.005, 0.002, 1, 0.05, 0.0008, 0.005, 0.0008, 0.1, 0.0008, 0.008]
    19        manipulators_controllers.PickPlaceController.__init__(
    20            self,
    21            name=name,
    22            cspace_controller=RMPFlowController(
    23                name=name + "_cspace_controller", robot_articulation=robot_articulation
    24            ),
    25            gripper=gripper,
    26            events_dt=events_dt,
    27            #This value can be changed
    28            start_picking_height=0.6
    29        )
    30        return
    
  2. Modify the Python file at COBOTTA_SCRIPTS_PATH/pick_up_example.py to run the pick and place routine using COBOTTA.

     1from isaacsim import SimulationApp
     2
     3simulation_app = SimulationApp({"headless": False})
     4
     5from isaacsim.core.api import World
     6import numpy as np
     7from tasks.pick_place import PickPlace
     8from controllers.pick_place import PickPlaceController
     9
    10my_world = World(stage_units_in_meters=1.0)
    11
    12
    13target_position = np.array([-0.3, 0.6, 0])
    14target_position[2] = 0.0515 / 2.0
    15my_task = PickPlace(name="denso_pick_place", target_position=target_position)
    16
    17my_world.add_task(my_task)
    18my_world.reset()
    19my_denso = my_world.scene.get_object("cobotta_robot")
    20#initialize the controller
    21my_controller = PickPlaceController(name="controller", robot_articulation=my_denso, gripper=my_denso.gripper)
    22task_params = my_world.get_task("denso_pick_place").get_params()
    23articulation_controller = my_denso.get_articulation_controller()
    24i = 0
    25while simulation_app.is_running():
    26    my_world.step(render=True)
    27    if my_world.is_playing():
    28        if my_world.current_time_step_index == 0:
    29            my_world.reset()
    30            my_controller.reset()
    31        observations = my_world.get_observations()
    32        #forward the observation values to the controller to get the actions
    33        actions = my_controller.forward(
    34            picking_position=observations[task_params["cube_name"]["value"]]["position"],
    35            placing_position=observations[task_params["cube_name"]["value"]]["target_position"],
    36            current_joint_positions=observations[task_params["robot_name"]["value"]]["joint_positions"],
    37            # This offset needs tuning as well
    38            end_effector_offset=np.array([0, 0, 0.25]),
    39        )
    40        if my_controller.is_done():
    41            print("done picking and placing")
    42        articulation_controller.apply_action(actions)
    43simulation_app.close()
    
    ../_images/isaac_sim_advanced_add_new_manipulator_12.gif
  3. Run it using ./python.sh standalone_examples/api/isaacsim.robot.manipulators/cobotta_900/pick_up_example.py.

Summary#

This tutorial covered the following topics:

  1. Inspecting the robot USD using the Articulation Inspector.

  2. Tuning the robot gains using the Gain Tuner.

  3. Adding follow target as well as pick and place tasks.

  4. Adding controllers to solve the tasks specified.

  5. Adding simple rmpflow config files.

Further Learning#

  1. Next learn more on how to create the rmpflow config files as well as the detailed explanation of most of the fields at Configuring RMPflow for a New Manipulator