Adding Multiple Robots#

Learning Objectives#

This tutorial integrates two different types of robots into the same simulation: a mobile robot (Jetbot) and a manipulator (Franka), working together to accomplish a task. The Jetbot pushes a cube towards the Franka, which then picks it up. After this tutorial, you will have experience building multi-robot simulations with object interaction.

15-20 Minute Tutorial

Getting Started#

Prerequisites

Begin with the source code open from the previous tutorial, Adding a Manipulator Robot.

Note

Pressing STOP, then PLAY in this workflow might not reset the world properly. Use the RESET button instead.

Creating the Scene#

Begin by adding the Jetbot, Franka Panda, and the Cube from the previous tutorials to the scene.

 1import isaacsim.core.experimental.utils.stage as stage_utils
 2from isaacsim.core.experimental.prims import Articulation, XformPrim, GeomPrim, RigidPrim
 3from isaacsim.core.experimental.objects import Cube
 4from isaacsim.core.experimental.materials import PreviewSurfaceMaterial
 5from isaacsim.examples.base.base_sample_experimental import BaseSample
 6from isaacsim.storage.native import get_assets_root_path
 7import numpy as np
 8
 9
10class HelloWorld(BaseSample):
11    def __init__(self) -> None:
12        super().__init__()
13
14    def setup_scene(self):
15        assets_root_path = get_assets_root_path()
16
17        # Add ground plane
18        stage_utils.add_reference_to_stage(
19            usd_path=assets_root_path + "/Isaac/Environments/Grid/default_environment.usd",
20            path="/World/ground",
21        )
22
23        # Add Jetbot mobile robot
24        stage_utils.add_reference_to_stage(
25            usd_path=assets_root_path + "/Isaac/Robots/NVIDIA/Jetbot/jetbot.usd",
26            path="/World/Jetbot",
27        )
28
29        # Add a cube in front of Jetbot for it to push
30        visual_material = PreviewSurfaceMaterial("/World/Materials/red")
31        visual_material.set_input_values("diffuseColor", [1.0, 0.0, 0.0])
32        cube_shape = Cube(
33            paths="/World/Cube",
34            positions=np.array([[0.15, 0.0, 0.025]]),  # In front of Jetbot
35            sizes=[1.0],
36            scales=np.array([[0.05, 0.05, 0.05]]),
37            reset_xform_op_properties=True,
38        )
39        GeomPrim(paths=cube_shape.paths, apply_collision_apis=True)
40        RigidPrim(paths=cube_shape.paths)
41        cube_shape.apply_visual_materials(visual_material)
42
43        # Add Franka manipulator at a position the Jetbot will push the cube to
44        stage_utils.add_reference_to_stage(
45            usd_path=assets_root_path + "/Isaac/Robots/FrankaRobotics/FrankaPanda/franka.usd",
46            path="/World/Franka",
47        )
48
49        # Position Franka so the cube will be pushed into its workspace
50        franka_xform = XformPrim("/World/Franka")
51        franka_xform.set_world_poses(positions=np.array([[0.8, -0.5, 0.0]]))
52
53    async def setup_post_load(self):
54        # Create Articulation handles for both robots
55        self._jetbot = Articulation("/World/Jetbot")
56        self._franka = Articulation("/World/Franka")
57
58        # Print robot info
59        print(f"Jetbot DOFs: {self._jetbot.num_dofs}, names: {self._jetbot.dof_names}")
60        print(f"Franka DOFs: {self._franka.num_dofs}, names: {self._franka.dof_names}")

Click the LOAD button to see both robots and the cube in the scene.

Controlling Multiple Robots#

Now add physics callbacks to control both robots simultaneously. The Jetbot will push the cube forward while the Franka prepares to receive it.

 1import isaacsim.core.experimental.utils.stage as stage_utils
 2from isaacsim.core.experimental.prims import Articulation, XformPrim, GeomPrim, RigidPrim
 3from isaacsim.core.experimental.objects import Cube
 4from isaacsim.core.experimental.materials import PreviewSurfaceMaterial
 5from isaacsim.examples.base.base_sample_experimental import BaseSample
 6from isaacsim.core.simulation_manager import SimulationManager
 7from isaacsim.storage.native import get_assets_root_path
 8import numpy as np
 9
10
11class HelloWorld(BaseSample):
12    def __init__(self) -> None:
13        super().__init__()
14        self._physics_callback_id = None
15        self._step_counter = 0
16
17    def setup_scene(self):
18        assets_root_path = get_assets_root_path()
19
20        # Add ground plane
21        stage_utils.add_reference_to_stage(
22            usd_path=assets_root_path + "/Isaac/Environments/Grid/default_environment.usd",
23            path="/World/ground",
24        )
25
26        # Add Jetbot mobile robot
27        stage_utils.add_reference_to_stage(
28            usd_path=assets_root_path + "/Isaac/Robots/NVIDIA/Jetbot/jetbot.usd",
29            path="/World/Jetbot",
30        )
31
32        # Add a cube in front of Jetbot for it to push
33        visual_material = PreviewSurfaceMaterial("/World/Materials/red")
34        visual_material.set_input_values("diffuseColor", [1.0, 0.0, 0.0])
35        cube_shape = Cube(
36            paths="/World/Cube",
37            positions=np.array([[0.15, 0.0, 0.025]]),
38            sizes=[1.0],
39            scales=np.array([[0.05, 0.05, 0.05]]),
40            reset_xform_op_properties=True,
41        )
42        GeomPrim(paths=cube_shape.paths, apply_collision_apis=True)
43        RigidPrim(paths=cube_shape.paths)
44        cube_shape.apply_visual_materials(visual_material)
45
46        # Add Franka manipulator
47        stage_utils.add_reference_to_stage(
48            usd_path=assets_root_path + "/Isaac/Robots/FrankaRobotics/FrankaPanda/franka.usd",
49            path="/World/Franka",
50        )
51
52        # Position Franka forward and to the right of Jetbot's path
53        franka_xform = XformPrim("/World/Franka")
54        franka_xform.set_world_poses(positions=np.array([[0.8, -0.5, 0.0]]))
55
56    async def setup_post_load(self):
57        # Create Articulation handles
58        self._jetbot = Articulation("/World/Jetbot")
59        self._franka = Articulation("/World/Franka")
60        self._cube = RigidPrim("/World/Cube")
61        self._step_counter = 0
62
63        # Register physics callback
64        from isaacsim.core.simulation_manager.impl.isaac_events import IsaacEvents
65        self._physics_callback_id = SimulationManager.register_callback(
66            self.physics_step, IsaacEvents.POST_PHYSICS_STEP
67        )
68
69    def physics_step(self, dt, context):
70        self._step_counter += 1
71        if self._step_counter < 300:
72            # Drive Jetbot forward to push the cube
73            self._jetbot.set_dof_velocity_targets([[10.0, 10.0]])
74        else:
75            # Stop the Jetbot after pushing
76            self._jetbot.set_dof_velocity_targets([[0.0, 0.0]])
77
78    def physics_cleanup(self):
79        if self._physics_callback_id is not None:
80            SimulationManager.deregister_callback(self._physics_callback_id)
81            self._physics_callback_id = None

Watch as the Jetbot pushes the cube towards the Franka!

../_images/core_api_tutorials_5_1.webp

Adding State Machine Logic#

Create a state machine to coordinate the robots: first the Jetbot pushes the cube towards Franka, then backs up to give space, and finally Franka executes a full pick-and-place sequence using the FrankaExperimental class for IK-based end-effector control:

  1import isaacsim.core.experimental.utils.stage as stage_utils
  2from isaacsim.core.experimental.prims import Articulation, XformPrim, GeomPrim, RigidPrim
  3from isaacsim.core.experimental.objects import Cube
  4from isaacsim.core.experimental.materials import PreviewSurfaceMaterial
  5from isaacsim.examples.base.base_sample_experimental import BaseSample
  6from isaacsim.core.simulation_manager import SimulationManager
  7from isaacsim.robot.manipulators.examples.franka import FrankaExperimental
  8from isaacsim.storage.native import get_assets_root_path
  9import numpy as np
 10
 11
 12class HelloWorld(BaseSample):
 13    def __init__(self) -> None:
 14        super().__init__()
 15        self._physics_callback_id = None
 16        self._state = 0
 17
 18    def setup_scene(self):
 19        assets_root_path = get_assets_root_path()
 20
 21        # Add ground plane
 22        stage_utils.add_reference_to_stage(
 23            usd_path=assets_root_path + "/Isaac/Environments/Grid/default_environment.usd",
 24            path="/World/ground",
 25        )
 26
 27        # Add Jetbot at origin
 28        stage_utils.add_reference_to_stage(
 29            usd_path=assets_root_path + "/Isaac/Robots/NVIDIA/Jetbot/jetbot.usd",
 30            path="/World/Jetbot",
 31        )
 32
 33        # Add cube in front of Jetbot
 34        visual_material = PreviewSurfaceMaterial("/World/Materials/blue")
 35        visual_material.set_input_values("diffuseColor", [0.0, 0.0, 1.0])
 36        cube_shape = Cube(
 37            paths="/World/Cube",
 38            positions=np.array([[0.15, 0.0, 0.0258]]),
 39            sizes=[1.0],
 40            scales=np.array([[0.05, 0.05, 0.05]]),
 41            reset_xform_op_properties=True,
 42        )
 43        GeomPrim(paths=cube_shape.paths, apply_collision_apis=True)
 44        RigidPrim(paths=cube_shape.paths)
 45        cube_shape.apply_visual_materials(visual_material)
 46
 47        # Add Franka using FrankaExperimental for IK and gripper control
 48        self._franka = FrankaExperimental(robot_path="/World/Franka", create_robot=True)
 49        franka_xform = XformPrim("/World/Franka")
 50        franka_xform.set_world_poses(positions=[[0.8, -0.3, 0.0]])
 51
 52    async def setup_post_load(self):
 53        self._jetbot = Articulation("/World/Jetbot")
 54        self._cube = RigidPrim("/World/Cube")
 55        self._cube_goal = np.array([1.2, 0.0, 0.0])  # Target: Franka reaches from the side
 56        self._step_counter = 0
 57        self._pick_phase = 0
 58
 59        from isaacsim.core.simulation_manager.impl.isaac_events import IsaacEvents
 60        self._physics_callback_id = SimulationManager.register_callback(
 61            self.physics_step, IsaacEvents.POST_PHYSICS_STEP
 62        )
 63        self._state = 0
 64
 65    def physics_step(self, dt, context):
 66        if self._state == 0:
 67            # Jetbot pushes cube to Franka
 68            cube_pos = self._cube.get_world_poses()[0].numpy()[0]
 69            if np.linalg.norm(cube_pos[:2] - self._cube_goal[:2]) > 0.05:
 70                self._jetbot.set_dof_velocity_targets([[10.0, 10.0]])
 71            else:
 72                self._jetbot.set_dof_velocity_targets([[0.0, 0.0]])
 73                print("Cube delivered! Backing up...")
 74                self._state = 1
 75                self._step_counter = 0
 76
 77        elif self._state == 1:
 78            # Jetbot backs up
 79            self._jetbot.set_dof_velocity_targets([[-8.0, -8.0]])
 80            self._step_counter += 1
 81            if self._step_counter > 100:
 82                self._jetbot.set_dof_velocity_targets(np.array([[0.0, 0.0]]))
 83                print("Franka starting pick-and-place...")
 84                self._state = 2
 85                self._step_counter = 0
 86                self._franka.open_gripper()
 87
 88        elif self._state == 2:
 89            # Franka pick-and-place sequence using step counter
 90            cube_pos = self._cube.get_world_poses()[0].numpy()[0]
 91            down_orient = self._franka.get_downward_orientation()
 92            self._step_counter += 1
 93
 94            if self._pick_phase == 0:
 95                # Move above cube (wait 120 steps)
 96                self._franka.set_end_effector_pose(np.array([[cube_pos[0], cube_pos[1], cube_pos[2]+0.2]]), down_orient)
 97                if self._step_counter > 120:
 98                    self._pick_phase = 1
 99                    self._step_counter = 0
100            elif self._pick_phase == 1:
101                # Lower to cube (wait 100 steps)
102                self._franka.set_end_effector_pose(
103                    np.array([[cube_pos[0], cube_pos[1], cube_pos[2] + 0.1]]), down_orient)
104                if self._step_counter > 100:
105                    self._franka.close_gripper()
106                    self._pick_phase = 2
107                    self._step_counter = 0
108            elif self._pick_phase == 2:
109                # Close the gripper (wait 50 steps)
110                self._franka.close_gripper()
111                if self._step_counter > 50:
112                    self._pick_phase = 3
113                    self._step_counter = 0
114            elif self._pick_phase == 3:
115                # Lift cube (wait 100 steps)
116                self._franka.set_end_effector_pose(
117                    np.array([[cube_pos[0], cube_pos[1], cube_pos[2] + 0.25]]), down_orient)
118                if self._step_counter > 100:
119                    self._pick_phase = 4
120                    self._step_counter = 0
121            elif self._pick_phase == 4:
122                # Move to target (wait 150 steps)
123                self._franka.set_end_effector_pose(
124                    np.array([[0.3, 0.3, 0.15]]), down_orient)
125                if self._step_counter > 150:
126                    self._franka.open_gripper()
127                    self._pick_phase = 5
128                    self._step_counter = 0
129            elif self._pick_phase == 5:
130                # Lift the arm (wait 150 steps)
131                self._franka.set_end_effector_pose(
132                    np.array([[cube_pos[0], cube_pos[1], cube_pos[2] + 0.5]]), down_orient)
133                if self._step_counter > 150:
134                    self._step_counter = 0
135
136    async def setup_post_reset(self):
137        self._state = 0
138        self._step_counter = 0
139        self._pick_phase = 0
140        self._franka.reset_to_default_pose()
141
142    def physics_cleanup(self):
143        if self._physics_callback_id is not None:
144            SimulationManager.deregister_callback(self._physics_callback_id)
145            self._physics_callback_id = None
../_images/core_api_tutorials_5_2.webp

Summary#

This tutorial covered the following topics:

  1. Adding multiple robots and objects (cube) to the scene

  2. Using Cube, GeomPrim, and RigidPrim to create pushable objects

  3. Using the Articulation class to control different robot types

  4. Having a mobile robot (Jetbot) push objects towards a manipulator (Franka)

  5. Building state machine logic to coordinate pushing, backing up, and picking

  6. Using FrankaExperimental for IK-based end-effector control and gripper operations

Next Steps#

Continue on to the next tutorial in our Essential Tutorials series, Multiple Robot Scenarios, to learn how to add multiple tasks and manage them.