Using a task-space controller

Using a task-space controller#

In the previous tutorials, we have joint-space controllers to control the robot. However, in many cases, it is more intuitive to control the robot using a task-space controller. For example, if we want to teleoperate the robot, it is easier to specify the desired end-effector pose rather than the desired joint positions.

In this tutorial, we will learn how to use a task-space controller to control the robot. We will use the controllers.DifferentialIKController class to track a desired end-effector pose command.

This tutorial uses Isaac Sim PhysX and requires an Isaac Sim installation.

The Code#

The tutorial corresponds to the run_diff_ik.py script in the scripts/tutorials/05_controllers directory.

Code for run_diff_ik.py
  1# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md).
  2# All rights reserved.
  3#
  4# SPDX-License-Identifier: BSD-3-Clause
  5
  6"""
  7This script demonstrates how to use the differential inverse kinematics controller with the simulator.
  8
  9The differential IK controller can be configured in different modes. It uses the Jacobians computed by
 10PhysX. This helps perform parallelized computation of the inverse kinematics.
 11
 12The Franka high-PD preset uses the same USD and effort limits as the base preset, with stiffer gains
 13for tracking IK targets. Other tasks can retain their calibrated actuator gains.
 14
 15.. code-block:: bash
 16
 17    # Usage
 18    uv run python scripts/tutorials/05_controllers/run_diff_ik.py
 19
 20"""
 21
 22"""Parse the command-line arguments first."""
 23
 24import argparse
 25from typing import TYPE_CHECKING
 26
 27from isaaclab.app import add_launcher_args, launch_simulation
 28
 29# add argparse arguments
 30parser = argparse.ArgumentParser(description="Tutorial on using the differential IK controller.")
 31parser.add_argument("--robot", type=str, default="franka_panda", help="Name of the robot.")
 32parser.add_argument("--num_envs", type=int, default=128, help="Number of environments to spawn.")
 33# append simulation launcher cli args
 34add_launcher_args(parser)
 35# parse the arguments
 36args_cli = parser.parse_args()
 37
 38"""Rest everything follows."""
 39
 40import torch
 41
 42import isaaclab.sim as sim_utils
 43from isaaclab.assets import AssetBaseCfg
 44from isaaclab.controllers import DifferentialIKController, DifferentialIKControllerCfg
 45from isaaclab.managers import SceneEntityCfg
 46from isaaclab.markers import VisualizationMarkers
 47from isaaclab.markers.config import FRAME_MARKER_CFG
 48from isaaclab.scene import InteractiveSceneCfg
 49from isaaclab.utils import clone, configclass, instantiate, replace
 50from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR
 51from isaaclab.utils.math import subtract_frame_transforms
 52
 53if TYPE_CHECKING:
 54    from isaaclab.scene import InteractiveScene
 55
 56##
 57# Pre-defined configs
 58##
 59from isaaclab_assets import FRANKA_PANDA_HIGH_PD_CFG, UR10_CFG  # isort:skip
 60
 61
 62@configclass
 63class TableTopSceneCfg(InteractiveSceneCfg):
 64    """Configuration for a cart-pole scene."""
 65
 66    # ground plane
 67    ground = AssetBaseCfg(
 68        prim_path="/World/defaultGroundPlane",
 69        spawn=sim_utils.GroundPlaneCfg(),
 70        init_state=AssetBaseCfg.InitialStateCfg(pos=(0.0, 0.0, -1.05)),
 71    )
 72
 73    # lights
 74    dome_light = AssetBaseCfg(
 75        prim_path="/World/Light", spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75))
 76    )
 77
 78    # mount
 79    table = AssetBaseCfg(
 80        prim_path="{ENV_REGEX_NS}/Table",
 81        spawn=sim_utils.UsdFileCfg(
 82            usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Mounts/Stand/stand_instanceable.usd", scale=(2.0, 2.0, 2.0)
 83        ),
 84    )
 85
 86    # articulation
 87    if args_cli.robot == "franka_panda":
 88        robot = replace(FRANKA_PANDA_HIGH_PD_CFG, prim_path="{ENV_REGEX_NS}/Robot")
 89        robot.spawn.variants["Physics"] = "physx"
 90    elif args_cli.robot == "ur10":
 91        robot = replace(UR10_CFG, prim_path="{ENV_REGEX_NS}/Robot")
 92    else:
 93        raise ValueError(f"Robot {args_cli.robot} is not supported. Valid: franka_panda, ur10")
 94
 95
 96def run_simulator(sim: sim_utils.SimulationContext, scene: "InteractiveScene"):
 97    """Runs the simulation loop."""
 98    # Extract scene entities
 99    # note: we only do this here for readability.
100    robot = scene["robot"]
101
102    # Create controller
103    diff_ik_cfg = DifferentialIKControllerCfg(command_type="pose", use_relative_mode=False, ik_method="dls")
104    diff_ik_controller = DifferentialIKController(diff_ik_cfg, num_envs=scene.num_envs, device=sim.device)
105
106    # Markers
107    frame_marker_cfg = clone(FRAME_MARKER_CFG)
108    frame_marker_cfg.markers["frame"].scale = (0.1, 0.1, 0.1)
109    ee_marker = VisualizationMarkers(replace(frame_marker_cfg, prim_path="/Visuals/ee_current"))
110    goal_marker = VisualizationMarkers(replace(frame_marker_cfg, prim_path="/Visuals/ee_goal"))
111
112    # Define goals for the arm (x,y,z,qx,qy,qz,qw)
113    ee_goals = [
114        [0.5, 0.5, 0.7, 0, 0.707, 0, 0.707],
115        [0.5, -0.4, 0.6, 0.707, 0, 0, 0.707],
116        [0.5, 0, 0.5, 1.0, 0.0, 0.0, 0.0],
117    ]
118    ee_goals = torch.tensor(ee_goals, device=sim.device)
119    # Track the given command
120    current_goal_idx = 0
121    # Create buffers to store actions
122    ik_commands = torch.zeros(scene.num_envs, diff_ik_controller.action_dim, device=robot.device)
123    ik_commands[:] = ee_goals[current_goal_idx]
124
125    # Specify robot-specific parameters
126    if args_cli.robot == "franka_panda":
127        robot_entity_cfg = SceneEntityCfg("robot", joint_names=["panda_joint.*"], body_names=["panda_hand"])
128    elif args_cli.robot == "ur10":
129        robot_entity_cfg = SceneEntityCfg("robot", joint_names=[".*"], body_names=["ee_link"])
130    else:
131        raise ValueError(f"Robot {args_cli.robot} is not supported. Valid: franka_panda, ur10")
132    # Resolving the scene entities
133    robot_entity_cfg.resolve(scene)
134    # Obtain the frame index of the end-effector
135    # For a fixed base robot, the frame index is one less than the body index. This is because
136    # the root body is not included in the returned Jacobians.
137    if robot.is_fixed_base:
138        ee_jacobi_idx = robot_entity_cfg.body_ids[0] - 1
139    else:
140        ee_jacobi_idx = robot_entity_cfg.body_ids[0]
141
142    # Define simulation stepping
143    sim_dt = sim.get_physics_dt()
144    count = 0
145    # Simulation loop
146    while sim.is_running():
147        # reset
148        if count % 150 == 0:
149            # reset time
150            count = 0
151            # reset joint state
152            joint_pos = robot.data.default_joint_pos.torch.clone()
153            joint_vel = robot.data.default_joint_vel.torch.clone()
154            robot.write_joint_position_to_sim_index(position=joint_pos)
155            robot.write_joint_velocity_to_sim_index(velocity=joint_vel)
156            robot.reset()
157            # reset actions
158            ik_commands[:] = ee_goals[current_goal_idx]
159            joint_pos_des = joint_pos[:, robot_entity_cfg.joint_ids].clone()
160            # reset controller
161            diff_ik_controller.reset()
162            diff_ik_controller.set_command(ik_commands)
163            # change goal
164            current_goal_idx = (current_goal_idx + 1) % len(ee_goals)
165        else:
166            # obtain quantities from simulation. The Jacobian DoF axis prepends
167            # ``num_base_dofs`` floating-base columns (0 for fixed-base, 6 for
168            # floating-base); shift the actuated-joint ids accordingly.
169            jacobi_joint_ids = [j + robot.num_base_dofs for j in robot_entity_cfg.joint_ids]
170            jacobian = robot.data.body_link_jacobian_w.torch[:, ee_jacobi_idx, :, jacobi_joint_ids]
171            ee_pose_w = robot.data.body_pose_w.torch[:, robot_entity_cfg.body_ids[0]]
172            root_pose_w = robot.data.root_pose_w.torch
173            joint_pos = robot.data.joint_pos.torch[:, robot_entity_cfg.joint_ids]
174            # compute frame in root frame
175            ee_pos_b, ee_quat_b = subtract_frame_transforms(
176                root_pose_w[:, 0:3], root_pose_w[:, 3:7], ee_pose_w[:, 0:3], ee_pose_w[:, 3:7]
177            )
178            # compute the joint commands
179            joint_pos_des = diff_ik_controller.compute(ee_pos_b, ee_quat_b, jacobian, joint_pos)
180
181        # apply actions
182        robot.set_joint_position_target_index(target=joint_pos_des, joint_ids=robot_entity_cfg.joint_ids)
183        scene.write_data_to_sim()
184        # perform step
185        sim.step()
186        # update sim-time
187        count += 1
188        # update buffers
189        scene.update(sim_dt)
190
191        # obtain quantities from simulation
192        ee_pose_w = robot.data.body_state_w.torch[:, robot_entity_cfg.body_ids[0], 0:7]
193        # update marker positions
194        ee_marker.visualize(ee_pose_w[:, 0:3], ee_pose_w[:, 3:7])
195        goal_marker.visualize(ik_commands[:, 0:3] + scene.env_origins, ik_commands[:, 3:7])
196
197
198def main():
199    """Main function."""
200    # Configure the simulation
201    sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device)
202    # Launch the simulator runtime that the configuration needs
203    with launch_simulation(sim_cfg, args_cli):
204        # Initialize the simulation context
205        sim = sim_utils.SimulationContext(sim_cfg)
206        # Set main camera
207        sim.set_camera_view([2.5, 2.5, 2.5], [0.0, 0.0, 0.0])
208        # Design scene
209        scene_cfg = TableTopSceneCfg(num_envs=args_cli.num_envs, env_spacing=2.0)
210        scene = instantiate(scene_cfg)
211        # Play the simulator
212        sim.reset()
213        # Now we are ready!
214        print("[INFO]: Setup complete...")
215        # Run the simulator
216        run_simulator(sim, scene)
217
218
219if __name__ == "__main__":
220    # run the main function
221    main()

The Code Explained#

While using any task-space controller, it is important to ensure that the provided quantities are in the correct frames. When parallelizing environment instances, they are all existing in the same unique simulation world frame. However, typically, we want each environment itself to have its own local frame. This is accessible through the scene.InteractiveScene.env_origins attribute.

In our APIs, we use the following notation for frames:

  • The simulation world frame (denoted as w), which is the frame of the entire simulation.

  • The local environment frame (denoted as e), which is the frame of the local environment.

  • The robot’s base frame (denoted as b), which is the frame of the robot’s base link.

Since the asset instances are not “aware” of the local environment frame, they return their states in the simulation world frame. Thus, we need to convert the obtained quantities to the local environment frame. This is done by subtracting the local environment origin from the obtained quantities.

Creating an IK controller#

The DifferentialIKController class computes the desired joint positions for a robot to reach a desired end-effector pose. The included implementation performs the computation in a batched format and uses PyTorch operations. It supports different types of inverse kinematics solvers, including the damped least-squares method and the pseudo-inverse method. These solvers can be specified using the ik_method argument. Additionally, the controller can handle commands as both relative and absolute poses.

In this tutorial, we will use the damped least-squares method to compute the desired joint positions. Additionally, since we want to track desired end-effector poses, we will use the absolute pose command mode.

    # Create controller
    diff_ik_cfg = DifferentialIKControllerCfg(command_type="pose", use_relative_mode=False, ik_method="dls")
    diff_ik_controller = DifferentialIKController(diff_ik_cfg, num_envs=scene.num_envs, device=sim.device)

Obtaining the robot’s joint and body indices#

The IK controller implementation is a computation-only class. Thus, it expects the user to provide the necessary information about the robot. This includes the robot’s joint positions, current end-effector pose, and the Jacobian matrix.

While the attribute assets.ArticulationData.joint_pos provides the joint positions, we only want the joint positions of the robot’s arm, and not the gripper. Similarly, while the attribute assets.ArticulationData.body_state_w provides the state of all the robot’s bodies, we only want the state of the robot’s end-effector. Thus, we need to index into these arrays to obtain the desired quantities.

For this, the articulation class provides the methods find_joints() and find_bodies(). These methods take in the names of the joints and bodies and return their corresponding indices.

While you may directly use these methods to obtain the indices, we recommend using the SceneEntityCfg class to resolve the indices. This class is used in various places in the APIs to extract certain information from a scene entity. Internally, it calls the above methods to obtain the indices. However, it also performs some additional checks to ensure that the provided names are valid. Thus, it is a safer option to use this class.

    # Specify robot-specific parameters
    if args_cli.robot == "franka_panda":
        robot_entity_cfg = SceneEntityCfg("robot", joint_names=["panda_joint.*"], body_names=["panda_hand"])
    elif args_cli.robot == "ur10":
        robot_entity_cfg = SceneEntityCfg("robot", joint_names=[".*"], body_names=["ee_link"])
    else:
        raise ValueError(f"Robot {args_cli.robot} is not supported. Valid: franka_panda, ur10")
    # Resolving the scene entities
    robot_entity_cfg.resolve(scene)
    # Obtain the frame index of the end-effector
    # For a fixed base robot, the frame index is one less than the body index. This is because
    # the root body is not included in the returned Jacobians.
    if robot.is_fixed_base:
        ee_jacobi_idx = robot_entity_cfg.body_ids[0] - 1
    else:
        ee_jacobi_idx = robot_entity_cfg.body_ids[0]

Computing robot command#

The IK controller separates the operation of setting the desired command and computing the desired joint positions. This is done to allow for the user to run the IK controller at a different frequency than the robot’s control frequency.

The set_command() method takes in the desired end-effector pose as a single batched array. The pose is specified in the robot’s base frame.

            # reset controller
            diff_ik_controller.reset()
            diff_ik_controller.set_command(ik_commands)

We can then compute the desired joint positions using the compute() method. The method takes in the current end-effector pose (in base frame), Jacobian, and current joint positions. We read the Jacobian matrix from the robot’s data, which uses its value computed from the physics engine.

            # obtain quantities from simulation. The Jacobian DoF axis prepends
            # ``num_base_dofs`` floating-base columns (0 for fixed-base, 6 for
            # floating-base); shift the actuated-joint ids accordingly.
            jacobi_joint_ids = [j + robot.num_base_dofs for j in robot_entity_cfg.joint_ids]
            jacobian = robot.data.body_link_jacobian_w.torch[:, ee_jacobi_idx, :, jacobi_joint_ids]
            ee_pose_w = robot.data.body_pose_w.torch[:, robot_entity_cfg.body_ids[0]]
            root_pose_w = robot.data.root_pose_w.torch
            joint_pos = robot.data.joint_pos.torch[:, robot_entity_cfg.joint_ids]
            # compute frame in root frame
            ee_pos_b, ee_quat_b = subtract_frame_transforms(
                root_pose_w[:, 0:3], root_pose_w[:, 3:7], ee_pose_w[:, 0:3], ee_pose_w[:, 3:7]
            )
            # compute the joint commands
            joint_pos_des = diff_ik_controller.compute(ee_pos_b, ee_quat_b, jacobian, joint_pos)

The computed joint position targets can then be applied on the robot, as done in the previous tutorials.

        # apply actions
        robot.set_joint_position_target_index(target=joint_pos_des, joint_ids=robot_entity_cfg.joint_ids)
        scene.write_data_to_sim()

The Code Execution#

Now that we have gone through the code, let’s run the script and see the result:

uv run isaaclab -p scripts/tutorials/05_controllers/run_diff_ik.py --robot franka_panda --num_envs 128 --viz kit

./isaaclab.sh -p scripts/tutorials/05_controllers/run_diff_ik.py --robot franka_panda --num_envs 128 --viz kit

The script will start a simulation with 128 robots. The robots will be controlled using the IK controller. The current and desired end-effector poses should be displayed using frame markers. When the robot reaches the desired pose, the command should cycle through to the next pose specified in the script.

result of run_diff_ik.py

Press Ctrl+C in the terminal to stop the simulation.