Isaac Sim Pick-and-Place Simulation

Overview

This repo demonstrates how to simulate multi-phase interactions between two robots in a pick-and-place application using Nvidia's Isaac Sim.

Dependencies

* Remark: Isaac Sim is assumed to be installed at path /opt/IsaacSim.

How to run

$ git clone https://github.com/raymondngiam/isaac-sim-pick-and-place-simulation.git
$ cd /opt/IsaacSim
$ ./python.sh <repo-root>/src/standalone.py

Code Walkthrough

Source code: standalone.py

from isaacsim import SimulationApp

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

import carb
from isaacsim.core.api import World
from isaacsim.robot.manipulators.examples.franka.tasks import PickPlace
from isaacsim.robot.wheeled_robots.robots import WheeledRobot
from isaacsim.storage.native import get_assets_root_path
from isaacsim.robot.wheeled_robots.controllers.wheel_base_pose_controller import WheelBasePoseController
from isaacsim.robot.manipulators.examples.franka.controllers import PickPlaceController
from isaacsim.robot.wheeled_robots.controllers.differential_controller import DifferentialController
from isaacsim.core.api.tasks import BaseTask
from isaacsim.core.utils.types import ArticulationAction
import numpy as np

assets_root_path = get_assets_root_path()
if assets_root_path is None:
    carb.log_error("Could not find Isaac Sim assets folder")
    simulation_app.close()
    sys.exit()
else:
    print(f"Isaac Sim assets folder located at {assets_root_path}")

class RobotsPlaying(BaseTask):
    def __init__(
        self,
        name
    ):
        super().__init__(name=name, offset=None)
        self._jetbot_goal_position = np.array([1, 0.3, 0])
        self._jetbot_goal_position2 = np.array([0, 0.3, 0])
        self._task_event = 0
        self._pick_place_task = PickPlace(cube_initial_position=np.array([0.1, 0.3, 0.05]),
                                        target_position=np.array([0.2, 0.3, 0.05/2]))
        return

    def set_up_scene(self, scene):
        super().set_up_scene(scene)
        self._pick_place_task.set_up_scene(scene)
        assets_root_path = get_assets_root_path()
        jetbot_asset_path = assets_root_path + "/Isaac/Robots/NVIDIA/Jetbot/jetbot.usd"
        self._jetbot = scene.add(
            WheeledRobot(
                prim_path="/World/Fancy_Jetbot",
                name="fancy_jetbot",
                wheel_dof_names=["left_wheel_joint", "right_wheel_joint"],
                create_robot=True,
                usd_path=jetbot_asset_path,
                position=np.array([0, 0.3, 0]),
            )
        )
        pick_place_params = self._pick_place_task.get_params()
        self._franka = scene.get_object(pick_place_params["robot_name"]["value"])
        self._franka.set_world_pose(position=np.array([1.0, 0, 0]))
        self._franka.set_default_state(position=np.array([1.0, 0, 0]))
        # Add Franka Controller
        self._franka_controller = PickPlaceController(name="pick_place_controller",
                                                      gripper=self._franka.gripper,
                                                      robot_articulation=self._franka)
        self._jetbot_controller = WheelBasePoseController(name="cool_controller",
                                                          open_loop_wheel_controller=
                                                          DifferentialController(name="simple_control",
                                                                                 wheel_radius=0.03, wheel_base=0.1125))
        return

    def get_observations(self):
        current_jetbot_position, current_jetbot_orientation = self._jetbot.get_world_pose()
        observations= {
            "task_event": self._task_event,
            self._jetbot.name: {
                "position": current_jetbot_position,
                "orientation": current_jetbot_orientation,
                "goal_position": self._jetbot_goal_position if self._task_event==0 else self._jetbot_goal_position2
            }
        }
        # add the subtask's observations as well
        observations.update(self._pick_place_task.get_observations())
        return observations

    def get_params(self):
        pick_place_params = self._pick_place_task.get_params()
        params_representation = pick_place_params
        params_representation["jetbot_name"] = {"value": self._jetbot.name, "modifiable": False}
        params_representation["franka_name"] = pick_place_params["robot_name"]
        params_representation["jetbot_controller"] = {"value": self._jetbot_controller}
        params_representation["franka_controller"] = {"value": self._franka_controller}
        return params_representation

    def pre_step(self, control_index, simulation_time):
        if self._task_event == 0:
            current_jetbot_position, _ = self._jetbot.get_world_pose()
            if np.mean(np.abs(current_jetbot_position[:2] - self._jetbot_goal_position[:2])) < 0.04:
                self._cube_arrive_step_index = control_index
                self._task_event += 1
        elif self._task_event == 1:
            if control_index - self._cube_arrive_step_index == 50:                
                self._task_event += 1
        elif self._task_event == 2:
            current_jetbot_position, _ = self._jetbot.get_world_pose()
            if np.mean(np.abs(current_jetbot_position[:2] - self._jetbot_goal_position2[:2])) < 0.04:
                self._task_event += 1
        elif self._task_event == 3:
            if self._franka_controller.is_done():                
                self._franka_controller.reset()
                self._task_event = 0
        return

    def post_reset(self):
        self._franka.gripper.set_joint_positions(self._franka.gripper.joint_opened_positions)
        self._task_event = 0
        return

def physics_step(step_size):
    current_observations = my_world.get_observations()
    if current_observations["task_event"] == 0:
        _jetbot.apply_wheel_actions(
            _jetbot_controller.forward(
                start_position=current_observations[_jetbot.name]["position"],
                start_orientation=current_observations[_jetbot.name]["orientation"],
                goal_position=current_observations[_jetbot.name]["goal_position"]))
    elif current_observations["task_event"] == 1:
        _jetbot.apply_wheel_actions(ArticulationAction(joint_velocities=[-8, -8]))
    if current_observations["task_event"] == 2:
        _jetbot.apply_wheel_actions(
            _jetbot_controller.forward(
                start_position=current_observations[_jetbot.name]["position"],
                start_orientation=current_observations[_jetbot.name]["orientation"],
                goal_position=current_observations[_jetbot.name]["goal_position"]))
    elif current_observations["task_event"] == 3:
        _jetbot.apply_wheel_actions(ArticulationAction(joint_velocities=[0.0, 0.0]))

        # Pick up the block
        actions = _franka_controller.forward(
            picking_position=current_observations[_cube_name]["position"],
            placing_position=current_observations[_cube_name]["target_position"],
            current_joint_positions=current_observations[_franka.name]["joint_positions"])
        _franka.apply_action(actions)
    return

my_world = World(stage_units_in_meters=1.0)
my_task = RobotsPlaying(name="awesome_task")
my_world.add_task(my_task)
my_world.reset()

task_params = my_world.get_task("awesome_task").get_params()

# We need franka later to apply to it actions
_franka = my_world.scene.get_object(task_params["franka_name"]["value"])
_jetbot = my_world.scene.get_object(task_params["jetbot_name"]["value"])
# We need the cube later on for the pick place controller
_cube_name = task_params["cube_name"]["value"]
_franka_controller = task_params["franka_controller"]["value"]
_jetbot_controller = task_params["jetbot_controller"]["value"]

reset_needed = False
task_completed = False

my_world.add_physics_callback("sim_step", callback_fn=physics_step)

while simulation_app.is_running():
    my_world.step(render=True)
    if my_world.is_stopped() and not reset_needed:
        reset_needed = True
        task_completed = False
    if my_world.is_playing():
        if reset_needed:
            my_world.reset()
            _franka_controller.reset()
            _jetbot_controller.reset()
            reset_needed = False
            task_completed = False
simulation_app.close()

Line 25-112 as highlighted above contains the definition of a RobotPlaying task, which is derived from the base class BaseTask.

Essentially, the RobotPlaying task is responsible for

• setting up the scene for the Franka robot arm and Jetbot wheeled mobile robot in the function set_up_scene().
• exposing the robot instances and controller instances to the World object via the function get_params().
• exposing the sequence control and robot states via the function get_observations().
• control the transition between sequence states via the function pre_step().

The RobotPlaying task instance is inserted into the world scene tasks in lines 141-144.

The overall sequence control of this simulation is basically governed by two sections of the code, namely:

1. physics_step() function defined as a callback function added to the World object via the World.add_physics_callback() function in line 159.
2. pre_step() function in the RobotPlaying task as mentioned before.

Once the simulation_app is running, the program will go into a while loop, executing a World.step() function at each cycle, which in turns will trigger the execution of the physics_step() function.

Without delving too deeply into the implementation of the physics_step() and pre_step() functions, the high level logic of the sequence control is as summarized in the following flowchart:

Support my works

If this project saved you time, helped you learn something new, or just made your day a little easier — consider buying me a coffee. Your support helps me keep building, documenting, and sharing works like this with the community.