Using an operational space controller#

Use controllers.OperationalSpaceController to control end-effector motion and contact force through joint efforts. This example presses against a tilted wall while tracking a pose along the unconstrained directions. See Controllers for the control methods and their requirements.

References for the operational space control:

  1. O Khatib. A unified approach for motion and force control of robot manipulators: The operational space formulation. IEEE Journal of Robotics and Automation, 3(1):43–53, 1987. URL http://dx.doi.org/10.1109/JRA.1987.1087068.

  2. Robot Dynamics Lecture Notes by Marco Hutter (ETH Zurich). URL https://ethz.ch/content/dam/ethz/special-interest/mavt/robotics-n-intelligent-systems/rsl-dam/documents/RobotDynamics2017/RD_HS2017script.pdf

This tutorial uses Isaac Sim PhysX and requires an Isaac Sim installation. The target quaternions use (x, y, z, w) order and orient the end-effector toward the tilted wall. For controller selection and the control-loop walkthrough, see Controllers.

The Code#

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

Code for run_osc.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 operational space controller (OSC) with the simulator.
  8
  9The OSC controller can be configured in different modes. It uses the dynamical quantities such as Jacobians and
 10mass matricescomputed by PhysX.
 11
 12.. code-block:: bash
 13
 14    # Usage
 15    uv run python scripts/tutorials/05_controllers/run_osc.py
 16
 17"""
 18
 19"""Parse the command-line arguments first."""
 20
 21import argparse
 22from typing import TYPE_CHECKING
 23
 24from isaaclab.app import add_launcher_args, launch_simulation
 25
 26# add argparse arguments
 27parser = argparse.ArgumentParser(description="Tutorial on using the operational space controller.")
 28parser.add_argument("--num_envs", type=int, default=128, help="Number of environments to spawn.")
 29# append simulation launcher cli args
 30add_launcher_args(parser)
 31# parse the arguments
 32args_cli = parser.parse_args()
 33
 34"""Rest everything follows."""
 35
 36import torch
 37
 38import isaaclab.sim as sim_utils
 39from isaaclab.assets import Articulation, AssetBaseCfg
 40from isaaclab.controllers import OperationalSpaceController, OperationalSpaceControllerCfg
 41from isaaclab.markers import VisualizationMarkers
 42from isaaclab.markers.config import FRAME_MARKER_CFG
 43from isaaclab.scene import InteractiveSceneCfg
 44from isaaclab.sensors import ContactSensorCfg
 45from isaaclab.utils import clone, configclass, instantiate, replace
 46from isaaclab.utils.math import (
 47    combine_frame_transforms,
 48    matrix_from_quat,
 49    quat_apply_inverse,
 50    quat_inv,
 51    subtract_frame_transforms,
 52)
 53
 54if TYPE_CHECKING:
 55    from isaaclab.scene import InteractiveScene
 56
 57##
 58# Pre-defined configs
 59##
 60from isaaclab_assets import FRANKA_PANDA_HIGH_PD_CFG  # isort:skip
 61
 62
 63@configclass
 64class SceneCfg(InteractiveSceneCfg):
 65    """Configuration for a simple scene with a tilted wall."""
 66
 67    # ground plane
 68    ground = AssetBaseCfg(
 69        prim_path="/World/defaultGroundPlane",
 70        spawn=sim_utils.GroundPlaneCfg(),
 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    # Tilted wall
 79    tilted_wall = AssetBaseCfg(
 80        prim_path="{ENV_REGEX_NS}/TiltedWall",
 81        spawn=sim_utils.CuboidCfg(
 82            size=(2.0, 1.5, 0.01),
 83            collision_props=sim_utils.UsdPhysicsCollisionCfg(),
 84            visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(1.0, 0.0, 0.0), opacity=0.1),
 85            rigid_props=sim_utils.UsdPhysicsRigidBodyCfg(kinematic_enabled=True),
 86            activate_contact_sensors=True,
 87        ),
 88        init_state=AssetBaseCfg.InitialStateCfg(
 89            pos=(0.6 + 0.085, 0.0, 0.3), rot=(0.0, -0.3826834324, 0.0, 0.9238795325)
 90        ),
 91    )
 92
 93    contact_forces = ContactSensorCfg(
 94        prim_path="{ENV_REGEX_NS}/TiltedWall",
 95        update_period=0.0,
 96        history_length=2,
 97        debug_vis=False,
 98    )
 99
100    robot = replace(FRANKA_PANDA_HIGH_PD_CFG, prim_path="{ENV_REGEX_NS}/Robot")
101    robot.spawn.variants["Physics"] = "physx"
102    robot.actuators["panda_arm"].stiffness = 0.0
103    robot.actuators["panda_arm"].damping = 0.0
104    robot.spawn.rigid_props.disable_gravity = True
105
106
107def run_simulator(sim: sim_utils.SimulationContext, scene: "InteractiveScene"):
108    """Runs the simulation loop.
109
110    Args:
111        sim: (SimulationContext) Simulation context.
112        scene: (InteractiveScene) Interactive scene.
113    """
114
115    # Extract scene entities for readability.
116    robot = scene["robot"]
117    contact_forces = scene["contact_forces"]
118
119    # Obtain indices for the end-effector and arm joints
120    ee_frame_name = "panda_leftfinger"
121    arm_joint_names = ["panda_joint.*"]
122    ee_frame_idx = robot.find_bodies(ee_frame_name)[0][0]
123    arm_joint_ids = robot.find_joints(arm_joint_names)[0]
124
125    # Create the OSC
126    osc_cfg = OperationalSpaceControllerCfg(
127        target_types=["pose_abs", "wrench_abs"],
128        impedance_mode="variable_kp",
129        inertial_dynamics_decoupling=True,
130        partial_inertial_dynamics_decoupling=False,
131        gravity_compensation=False,
132        motion_damping_ratio_task=1.0,
133        contact_wrench_stiffness_task=[0.0, 0.0, 0.1, 0.0, 0.0, 0.0],
134        motion_control_axes_task=[1, 1, 0, 1, 1, 1],
135        contact_wrench_control_axes_task=[0, 0, 1, 0, 0, 0],
136        nullspace_control="position",
137    )
138    osc = OperationalSpaceController(osc_cfg, num_envs=scene.num_envs, device=sim.device)
139
140    # Markers
141    frame_marker_cfg = clone(FRAME_MARKER_CFG)
142    frame_marker_cfg.markers["frame"].scale = (0.1, 0.1, 0.1)
143    ee_marker = VisualizationMarkers(replace(frame_marker_cfg, prim_path="/Visuals/ee_current"))
144    goal_marker = VisualizationMarkers(replace(frame_marker_cfg, prim_path="/Visuals/ee_goal"))
145
146    # Define targets for the arm (x,y,z,qx,qy,qz,qw)
147    ee_goal_pose_set_tilted_b = torch.tensor(
148        [
149            [0.6, 0.15, 0.3, 0.92387953, 0.0, 0.38268343, 0.0],
150            [0.6, -0.3, 0.3, 0.92387953, 0.0, 0.38268343, 0.0],
151            [0.8, 0.0, 0.5, 0.92387953, 0.0, 0.38268343, 0.0],
152        ],
153        device=sim.device,
154    )
155    ee_goal_wrench_set_tilted_task = torch.tensor(
156        [
157            [0.0, 0.0, 10.0, 0.0, 0.0, 0.0],
158            [0.0, 0.0, 10.0, 0.0, 0.0, 0.0],
159            [0.0, 0.0, 10.0, 0.0, 0.0, 0.0],
160        ],
161        device=sim.device,
162    )
163    kp_set_task = torch.tensor(
164        [
165            [360.0, 360.0, 360.0, 360.0, 360.0, 360.0],
166            [420.0, 420.0, 420.0, 420.0, 420.0, 420.0],
167            [320.0, 320.0, 320.0, 320.0, 320.0, 320.0],
168        ],
169        device=sim.device,
170    )
171    ee_target_set = torch.cat([ee_goal_pose_set_tilted_b, ee_goal_wrench_set_tilted_task, kp_set_task], dim=-1)
172
173    # Define simulation stepping
174    sim_dt = sim.get_physics_dt()
175
176    # Update existing buffers
177    # Note: We need to update buffers before the first step for the controller.
178    robot.update(dt=sim_dt)
179
180    # Get the center of the robot soft joint limits
181    joint_centers = torch.mean(robot.data.soft_joint_pos_limits.torch[:, arm_joint_ids, :], dim=-1)
182
183    # get the updated states
184    (
185        jacobian_b,
186        mass_matrix,
187        gravity,
188        ee_pose_b,
189        ee_vel_b,
190        root_pose_w,
191        ee_pose_w,
192        ee_force_b,
193        joint_pos,
194        joint_vel,
195    ) = update_states(sim, scene, robot, ee_frame_idx, arm_joint_ids, contact_forces)
196
197    # Track the given target command
198    current_goal_idx = 0  # Current goal index for the arm
199    command = torch.zeros(
200        scene.num_envs, osc.action_dim, device=sim.device
201    )  # Generic target command, which can be pose, position, force, etc.
202    ee_target_pose_b = torch.zeros(scene.num_envs, 7, device=sim.device)  # Target pose in the body frame
203    ee_target_pose_w = torch.zeros(scene.num_envs, 7, device=sim.device)  # Target pose in the world frame (for marker)
204
205    # Set joint efforts to zero
206    zero_joint_efforts = torch.zeros(scene.num_envs, robot.num_joints, device=sim.device)
207    joint_efforts = torch.zeros(scene.num_envs, len(arm_joint_ids), device=sim.device)
208
209    count = 0
210    # Simulation loop
211    while sim.is_running():
212        # reset every 500 steps
213        if count % 500 == 0:
214            # reset joint state to default
215            default_joint_pos = robot.data.default_joint_pos.torch.clone()
216            default_joint_vel = robot.data.default_joint_vel.torch.clone()
217            robot.write_joint_position_to_sim_index(position=default_joint_pos)
218            robot.write_joint_velocity_to_sim_index(velocity=default_joint_vel)
219            robot.set_joint_effort_target_index(target=zero_joint_efforts)  # Set zero torques in the initial step
220            robot.write_data_to_sim()
221            robot.reset()
222            # reset contact sensor
223            contact_forces.reset()
224            # reset target pose
225            robot.update(sim_dt)
226            _, _, _, ee_pose_b, _, _, _, _, _, _ = update_states(
227                sim, scene, robot, ee_frame_idx, arm_joint_ids, contact_forces
228            )  # at reset, the jacobians are not updated to the latest state
229            command, ee_target_pose_b, ee_target_pose_w, current_goal_idx = update_target(
230                sim, scene, osc, root_pose_w, ee_target_set, current_goal_idx
231            )
232            # set the osc command
233            osc.reset()
234            command, task_frame_pose_b = convert_to_task_frame(osc, command=command, ee_target_pose_b=ee_target_pose_b)
235            osc.set_command(command=command, current_ee_pose_b=ee_pose_b, current_task_frame_pose_b=task_frame_pose_b)
236        else:
237            # get the updated states
238            (
239                jacobian_b,
240                mass_matrix,
241                gravity,
242                ee_pose_b,
243                ee_vel_b,
244                root_pose_w,
245                ee_pose_w,
246                ee_force_b,
247                joint_pos,
248                joint_vel,
249            ) = update_states(sim, scene, robot, ee_frame_idx, arm_joint_ids, contact_forces)
250            # compute the joint commands
251            joint_efforts = osc.compute(
252                jacobian_b=jacobian_b,
253                current_ee_pose_b=ee_pose_b,
254                current_ee_vel_b=ee_vel_b,
255                current_ee_force_b=ee_force_b,
256                mass_matrix=mass_matrix,
257                gravity=gravity,
258                current_joint_pos=joint_pos,
259                current_joint_vel=joint_vel,
260                nullspace_joint_pos_target=joint_centers,
261            )
262            # apply actions
263            robot.set_joint_effort_target_index(target=joint_efforts, joint_ids=arm_joint_ids)
264            robot.write_data_to_sim()
265
266        # update marker positions
267        ee_marker.visualize(ee_pose_w[:, 0:3], ee_pose_w[:, 3:7])
268        goal_marker.visualize(ee_target_pose_w[:, 0:3], ee_target_pose_w[:, 3:7])
269
270        # perform step
271        sim.step(render=True)
272        # update robot buffers
273        robot.update(sim_dt)
274        # update buffers
275        scene.update(sim_dt)
276        # update sim-time
277        count += 1
278
279
280# Update robot states
281def update_states(
282    sim: sim_utils.SimulationContext,
283    scene: "InteractiveScene",
284    robot: Articulation,
285    ee_frame_idx: int,
286    arm_joint_ids: list[int],
287    contact_forces,
288):
289    """Update the robot states.
290
291    Args:
292        sim: (SimulationContext) Simulation context.
293        scene: (InteractiveScene) Interactive scene.
294        robot: (Articulation) Robot articulation.
295        ee_frame_idx: (int) End-effector frame index.
296        arm_joint_ids: (list[int]) Arm joint indices.
297        contact_forces: (ContactSensor) Contact sensor.
298
299    Returns:
300        jacobian_b (torch.tensor): Jacobian in the body frame.
301        mass_matrix (torch.tensor): Mass matrix.
302        gravity (torch.tensor): Gravity vector.
303        ee_pose_b (torch.tensor): End-effector pose in the body frame.
304        ee_vel_b (torch.tensor): End-effector velocity in the body frame.
305        root_pose_w (torch.tensor): Root pose in the world frame.
306        ee_pose_w (torch.tensor): End-effector pose in the world frame.
307        ee_force_b (torch.tensor): End-effector force in the body frame.
308        joint_pos (torch.tensor): The joint positions.
309        joint_vel (torch.tensor): The joint velocities.
310    """
311    # obtain dynamics related quantities from simulation
312    ee_jacobi_idx = ee_frame_idx - 1
313    # The J / M / g DoF axis prepends ``num_base_dofs`` floating-base columns
314    # (0 for fixed-base, 6 for floating-base); shift the actuated-joint ids by
315    # ``num_base_dofs`` to address the actuated-joint columns directly.
316    jacobi_joint_ids = [j + robot.num_base_dofs for j in arm_joint_ids]
317    jacobian_w = robot.data.body_link_jacobian_w.torch[:, ee_jacobi_idx, :, jacobi_joint_ids]
318    mass_matrix = robot.data.mass_matrix.torch[:, jacobi_joint_ids, :][:, :, jacobi_joint_ids]
319    gravity = robot.data.gravity_compensation_forces.torch[:, jacobi_joint_ids]
320    # Convert the Jacobian from world to root frame
321    jacobian_b = jacobian_w.clone()
322    root_rot_matrix = matrix_from_quat(quat_inv(robot.data.root_quat_w.torch))
323    jacobian_b[:, :3, :] = torch.bmm(root_rot_matrix, jacobian_b[:, :3, :])
324    jacobian_b[:, 3:, :] = torch.bmm(root_rot_matrix, jacobian_b[:, 3:, :])
325
326    # Compute current pose of the end-effector
327    root_pos_w = robot.data.root_pos_w.torch
328    root_quat_w = robot.data.root_quat_w.torch
329    ee_pos_w = robot.data.body_pos_w.torch[:, ee_frame_idx]
330    ee_quat_w = robot.data.body_quat_w.torch[:, ee_frame_idx]
331    ee_pos_b, ee_quat_b = subtract_frame_transforms(root_pos_w, root_quat_w, ee_pos_w, ee_quat_w)
332    root_pose_w = torch.cat([root_pos_w, root_quat_w], dim=-1)
333    ee_pose_w = torch.cat([ee_pos_w, ee_quat_w], dim=-1)
334    ee_pose_b = torch.cat([ee_pos_b, ee_quat_b], dim=-1)
335
336    # Compute the current velocity of the end-effector
337    ee_vel_w = robot.data.body_vel_w.torch[:, ee_frame_idx, :]  # Extract end-effector velocity in the world frame
338    root_vel_w = robot.data.root_vel_w.torch  # Extract root velocity in the world frame
339    relative_vel_w = ee_vel_w - root_vel_w  # Compute the relative velocity in the world frame
340    ee_lin_vel_b = quat_apply_inverse(robot.data.root_quat_w.torch, relative_vel_w[:, 0:3])  # From world to root frame
341    ee_ang_vel_b = quat_apply_inverse(robot.data.root_quat_w.torch, relative_vel_w[:, 3:6])
342    ee_vel_b = torch.cat([ee_lin_vel_b, ee_ang_vel_b], dim=-1)
343
344    # Calculate the contact force
345    ee_force_w = torch.zeros(scene.num_envs, 3, device=sim.device)
346    sim_dt = sim.get_physics_dt()
347    contact_forces.update(sim_dt)  # update contact sensor
348    # Calculate the contact force by averaging over last four time steps (i.e., to smoothen) and
349    # taking the max of three surfaces as only one should be the contact of interest
350    ee_force_w, _ = torch.max(torch.mean(contact_forces.data.net_normal_forces_w_history, dim=1), dim=1)
351
352    # This is a simplification, only for the sake of testing.
353    ee_force_b = ee_force_w
354
355    # Get joint positions and velocities
356    joint_pos = robot.data.joint_pos.torch[:, arm_joint_ids]
357    joint_vel = robot.data.joint_vel.torch[:, arm_joint_ids]
358
359    return (
360        jacobian_b,
361        mass_matrix,
362        gravity,
363        ee_pose_b,
364        ee_vel_b,
365        root_pose_w,
366        ee_pose_w,
367        ee_force_b,
368        joint_pos,
369        joint_vel,
370    )
371
372
373# Update the target commands
374def update_target(
375    sim: sim_utils.SimulationContext,
376    scene: "InteractiveScene",
377    osc: OperationalSpaceController,
378    root_pose_w: torch.tensor,
379    ee_target_set: torch.tensor,
380    current_goal_idx: int,
381):
382    """Update the targets for the operational space controller.
383
384    Args:
385        sim: (SimulationContext) Simulation context.
386        scene: (InteractiveScene) Interactive scene.
387        osc: (OperationalSpaceController) Operational space controller.
388        root_pose_w: (torch.tensor) Root pose in the world frame.
389        ee_target_set: (torch.tensor) End-effector target set.
390        current_goal_idx: (int) Current goal index.
391
392    Returns:
393        command (torch.tensor): Updated target command.
394        ee_target_pose_b (torch.tensor): Updated target pose in the body frame.
395        ee_target_pose_w (torch.tensor): Updated target pose in the world frame.
396        next_goal_idx (int): Next goal index.
397
398    Raises:
399        ValueError: Undefined target_type.
400    """
401
402    # update the ee desired command
403    command = torch.zeros(scene.num_envs, osc.action_dim, device=sim.device)
404    command[:] = ee_target_set[current_goal_idx]
405
406    # update the ee desired pose
407    ee_target_pose_b = torch.zeros(scene.num_envs, 7, device=sim.device)
408    for target_type in osc.cfg.target_types:
409        if target_type == "pose_abs":
410            ee_target_pose_b[:] = command[:, :7]
411        elif target_type == "wrench_abs":
412            pass  # ee_target_pose_b could stay at the root frame for force control, what matters is ee_target_b
413        else:
414            raise ValueError("Undefined target_type within update_target().")
415
416    # update the target desired pose in world frame (for marker)
417    ee_target_pos_w, ee_target_quat_w = combine_frame_transforms(
418        root_pose_w[:, 0:3], root_pose_w[:, 3:7], ee_target_pose_b[:, 0:3], ee_target_pose_b[:, 3:7]
419    )
420    ee_target_pose_w = torch.cat([ee_target_pos_w, ee_target_quat_w], dim=-1)
421
422    next_goal_idx = (current_goal_idx + 1) % len(ee_target_set)
423
424    return command, ee_target_pose_b, ee_target_pose_w, next_goal_idx
425
426
427# Convert the target commands to the task frame
428def convert_to_task_frame(osc: OperationalSpaceController, command: torch.tensor, ee_target_pose_b: torch.tensor):
429    """Converts the target commands to the task frame.
430
431    Args:
432        osc: OperationalSpaceController object.
433        command: Command to be converted.
434        ee_target_pose_b: Target pose in the body frame.
435
436    Returns:
437        command (torch.tensor): Target command in the task frame.
438        task_frame_pose_b (torch.tensor): Target pose in the task frame.
439
440    Raises:
441        ValueError: Undefined target_type.
442    """
443    command = command.clone()
444    task_frame_pose_b = ee_target_pose_b.clone()
445
446    cmd_idx = 0
447    for target_type in osc.cfg.target_types:
448        if target_type == "pose_abs":
449            command[:, :3], command[:, 3:7] = subtract_frame_transforms(
450                task_frame_pose_b[:, :3], task_frame_pose_b[:, 3:], command[:, :3], command[:, 3:7]
451            )
452            cmd_idx += 7
453        elif target_type == "wrench_abs":
454            # These are already defined in target frame for ee_goal_wrench_set_tilted_task (since it is
455            # easier), so not transforming
456            cmd_idx += 6
457        else:
458            raise ValueError("Undefined target_type within _convert_to_task_frame().")
459
460    return command, task_frame_pose_b
461
462
463def main():
464    """Main function."""
465    # Configure the simulation
466    sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device)
467    # Launch the simulator runtime that the configuration needs
468    with launch_simulation(sim_cfg, args_cli):
469        # Initialize the simulation context
470        sim = sim_utils.SimulationContext(sim_cfg)
471        # Set main camera
472        sim.set_camera_view([2.5, 2.5, 2.5], [0.0, 0.0, 0.0])
473        # Design scene
474        scene_cfg = SceneCfg(num_envs=args_cli.num_envs, env_spacing=2.0)
475        scene = instantiate(scene_cfg)
476        # Play the simulator
477        sim.reset()
478        # Now we are ready!
479        print("[INFO]: Setup complete...")
480        # Run the simulator
481        run_simulator(sim, scene)
482
483
484if __name__ == "__main__":
485    # run the main function
486    main()

Creating an Operational Space Controller#

The OperationalSpaceController class computes the joint efforts/torques for a robot to do simultaneous motion and force control in task space.

The reference frame of this task space could be an arbitrary coordinate frame in Euclidean space. By default, it is the robot’s base frame. However, in certain cases, it could be easier to define target coordinates w.r.t. a different frame. In such cases, the pose of this task reference frame, w.r.t. to the robot’s base frame, should be provided in the set_command method’s current_task_frame_pose_b argument. For example, in this tutorial, it makes sense to define the target commands w.r.t. a frame that is parallel to the wall surface, as the force control direction would be then only nonzero in the z-axis of this frame. The target pose, which is set to have the same orientation as the wall surface, is such a candidate and is used as the task frame in this tutorial. Therefore, all the arguments to the OperationalSpaceControllerCfg should be set with this task reference frame in mind.

For the motion control, the task space targets could be given as absolute (i.e., defined w.r.t. the robot base, target_types: "pose_abs") or relative to the end-effector’s current pose (i.e., target_types: "pose_rel"). For the force control, the task space targets could be given as absolute (i.e., defined w.r.t. the robot base, target_types: "wrench_abs"). If it is desired to apply pose and force control simultaneously, the target_types should be a list such as ["pose_abs", "wrench_abs"] or ["pose_rel", "wrench_abs"].

The axes that the motion and force control will be applied can be specified using the motion_control_axes_task and contact_wrench_control_axes_task arguments, respectively. These lists should consist of 0/1 for all six axes (position and rotation) and be complementary to each other (e.g., for the x-axis, if the motion_control_axes_task is 0, the contact_wrench_control_axes_task should be 1).

For the motion control axes, desired stiffness, and damping ratio values can be specified using the motion_stiffness_task and motion_damping_ratio_task arguments, which can be a scalar (same value for all axes) or a list of six scalars, one value corresponding to each axis. If desired, the stiffness and damping ratio values could be a command parameter (e.g., to learn the values using RL or change them on the go). For this, impedance_mode should be either "variable_kp" to include the stiffness values within the command or "variable" to include both the stiffness and damping ratio values. In these cases, motion_stiffness_limits_task and motion_damping_ratio_limits_task should be set as well, which puts bounds on the stiffness and damping ratio values.

For contact force control, it is possible to apply an open-loop force control by not setting the contact_wrench_stiffness_task, or apply a closed-loop force control (with the feed-forward term) by setting the desired stiffness values using the contact_wrench_stiffness_task argument, which can be a scalar or a list of six scalars. Please note that, currently, only the linear part of the contact wrench (hence the first three elements of the contact_wrench_stiffness_task) is considered in the closed-loop control, as the rotational part cannot be measured with the contact sensors.

For the motion control, inertial_dynamics_decoupling should be set to True to use the robot’s inertia matrix to decouple the desired accelerations in the task space. This is important for the motion control to be accurate, especially for rapid movements. This inertial decoupling accounts for the coupling between all the six motion axes. If desired, the inertial coupling between the translational and rotational axes could be ignored by setting the partial_inertial_dynamics_decoupling to True.

If it is desired to include the gravity compensation in the operational space command, the gravity_compensation should be set to True.

A final consideration regarding the operational space control is what to do with the null-space of redundant robots. The null-space is the subspace of the joint space that does not affect the task space coordinates. If nothing is done to control the null-space, the robot joints will float without moving the end-effector. This might be undesired (e.g., the robot joints might get close to their limits), and one might want to control the robot behaviour within its null-space. One way to do is to set nullspace_control to "position" (by default it is "none") which integrates a null-space PD controller to attract the robot joints to desired targets without affecting the task space. The behaviour of this null-space controller can be defined using the nullspace_stiffness and nullspace_damping_ratio arguments. Please note that theoretical decoupling of the null-space and task space accelerations is only possible when inertial_dynamics_decoupling is set to True and partial_inertial_dynamics_decoupling is set to False.

The included OSC implementation performs the computation in a batched format and uses PyTorch operations.

In this tutorial, we will use "pose_abs" for controlling the motion in all axes except the z-axis and "wrench_abs" for controlling the force in the z-axis. Moreover, we will include the full inertia decoupling in the motion control and not include the gravity compensation, as the gravity is disabled from the robot configuration. We set the impedance mode to "variable_kp" to dynamically change the stiffness values (motion_damping_ratio_task is set to 1: the kd values adapt according to kp values to maintain a critically damped response). Finally, nullspace_control is set to use "position" where the joint set points are provided to be the center of the joint position limits.

    # Create the OSC
    osc_cfg = OperationalSpaceControllerCfg(
        target_types=["pose_abs", "wrench_abs"],
        impedance_mode="variable_kp",
        inertial_dynamics_decoupling=True,
        partial_inertial_dynamics_decoupling=False,
        gravity_compensation=False,
        motion_damping_ratio_task=1.0,
        contact_wrench_stiffness_task=[0.0, 0.0, 0.1, 0.0, 0.0, 0.0],
        motion_control_axes_task=[1, 1, 0, 1, 1, 1],
        contact_wrench_control_axes_task=[0, 0, 1, 0, 0, 0],
        nullspace_control="position",
    )
    osc = OperationalSpaceController(osc_cfg, num_envs=scene.num_envs, device=sim.device)

Updating the states of the robot#

The OSC implementation is a computation-only class. Thus, it expects the user to provide the necessary information about the robot. This includes the robot’s Jacobian matrix, mass/inertia matrix, end-effector pose, velocity, contact force (all in the root frame), and finally, the joint positions and velocities. Moreover, the user should provide gravity compensation vector and null-space joint position targets if required.

# Update robot states
def update_states(
    sim: sim_utils.SimulationContext,
    scene: "InteractiveScene",
    robot: Articulation,
    ee_frame_idx: int,
    arm_joint_ids: list[int],
    contact_forces,
):
    """Update the robot states.

    Args:
        sim: (SimulationContext) Simulation context.
        scene: (InteractiveScene) Interactive scene.
        robot: (Articulation) Robot articulation.
        ee_frame_idx: (int) End-effector frame index.
        arm_joint_ids: (list[int]) Arm joint indices.
        contact_forces: (ContactSensor) Contact sensor.

    Returns:
        jacobian_b (torch.tensor): Jacobian in the body frame.
        mass_matrix (torch.tensor): Mass matrix.
        gravity (torch.tensor): Gravity vector.
        ee_pose_b (torch.tensor): End-effector pose in the body frame.
        ee_vel_b (torch.tensor): End-effector velocity in the body frame.
        root_pose_w (torch.tensor): Root pose in the world frame.
        ee_pose_w (torch.tensor): End-effector pose in the world frame.
        ee_force_b (torch.tensor): End-effector force in the body frame.
        joint_pos (torch.tensor): The joint positions.
        joint_vel (torch.tensor): The joint velocities.
    """
    # obtain dynamics related quantities from simulation
    ee_jacobi_idx = ee_frame_idx - 1
    # The J / M / g DoF axis prepends ``num_base_dofs`` floating-base columns
    # (0 for fixed-base, 6 for floating-base); shift the actuated-joint ids by
    # ``num_base_dofs`` to address the actuated-joint columns directly.
    jacobi_joint_ids = [j + robot.num_base_dofs for j in arm_joint_ids]
    jacobian_w = robot.data.body_link_jacobian_w.torch[:, ee_jacobi_idx, :, jacobi_joint_ids]
    mass_matrix = robot.data.mass_matrix.torch[:, jacobi_joint_ids, :][:, :, jacobi_joint_ids]
    gravity = robot.data.gravity_compensation_forces.torch[:, jacobi_joint_ids]
    # Convert the Jacobian from world to root frame
    jacobian_b = jacobian_w.clone()
    root_rot_matrix = matrix_from_quat(quat_inv(robot.data.root_quat_w.torch))
    jacobian_b[:, :3, :] = torch.bmm(root_rot_matrix, jacobian_b[:, :3, :])
    jacobian_b[:, 3:, :] = torch.bmm(root_rot_matrix, jacobian_b[:, 3:, :])

    # Compute current pose of the end-effector
    root_pos_w = robot.data.root_pos_w.torch
    root_quat_w = robot.data.root_quat_w.torch
    ee_pos_w = robot.data.body_pos_w.torch[:, ee_frame_idx]
    ee_quat_w = robot.data.body_quat_w.torch[:, ee_frame_idx]
    ee_pos_b, ee_quat_b = subtract_frame_transforms(root_pos_w, root_quat_w, ee_pos_w, ee_quat_w)
    root_pose_w = torch.cat([root_pos_w, root_quat_w], dim=-1)
    ee_pose_w = torch.cat([ee_pos_w, ee_quat_w], dim=-1)
    ee_pose_b = torch.cat([ee_pos_b, ee_quat_b], dim=-1)

    # Compute the current velocity of the end-effector
    ee_vel_w = robot.data.body_vel_w.torch[:, ee_frame_idx, :]  # Extract end-effector velocity in the world frame
    root_vel_w = robot.data.root_vel_w.torch  # Extract root velocity in the world frame
    relative_vel_w = ee_vel_w - root_vel_w  # Compute the relative velocity in the world frame
    ee_lin_vel_b = quat_apply_inverse(robot.data.root_quat_w.torch, relative_vel_w[:, 0:3])  # From world to root frame
    ee_ang_vel_b = quat_apply_inverse(robot.data.root_quat_w.torch, relative_vel_w[:, 3:6])
    ee_vel_b = torch.cat([ee_lin_vel_b, ee_ang_vel_b], dim=-1)

    # Calculate the contact force
    ee_force_w = torch.zeros(scene.num_envs, 3, device=sim.device)
    sim_dt = sim.get_physics_dt()
    contact_forces.update(sim_dt)  # update contact sensor
    # Calculate the contact force by averaging over last four time steps (i.e., to smoothen) and
    # taking the max of three surfaces as only one should be the contact of interest
    ee_force_w, _ = torch.max(torch.mean(contact_forces.data.net_normal_forces_w_history, dim=1), dim=1)

    # This is a simplification, only for the sake of testing.
    ee_force_b = ee_force_w

    # Get joint positions and velocities
    joint_pos = robot.data.joint_pos.torch[:, arm_joint_ids]
    joint_vel = robot.data.joint_vel.torch[:, arm_joint_ids]

    return (
        jacobian_b,
        mass_matrix,
        gravity,
        ee_pose_b,
        ee_vel_b,
        root_pose_w,
        ee_pose_w,
        ee_force_b,
        joint_pos,
        joint_vel,
    )


Computing robot command#

The OSC separates the operation of setting the desired command and computing the desired joint efforts. To set the desired command, the user should provide command vector, which includes the target commands (i.e., in the order they appear in the target_types argument of the OSC configuration), and the desired stiffness and damping ratio values if the impedance_mode is set to "variable_kp" or "variable". They should be all in the same coordinate frame as the task frame (e.g., indicated with _task subscript) and concatenated together.

In this tutorial, the desired wrench is already defined w.r.t. the task frame, and the desired pose is transformed to the task frame as the following:

# Convert the target commands to the task frame
def convert_to_task_frame(osc: OperationalSpaceController, command: torch.tensor, ee_target_pose_b: torch.tensor):
    """Converts the target commands to the task frame.

    Args:
        osc: OperationalSpaceController object.
        command: Command to be converted.
        ee_target_pose_b: Target pose in the body frame.

    Returns:
        command (torch.tensor): Target command in the task frame.
        task_frame_pose_b (torch.tensor): Target pose in the task frame.

    Raises:
        ValueError: Undefined target_type.
    """
    command = command.clone()
    task_frame_pose_b = ee_target_pose_b.clone()

    cmd_idx = 0
    for target_type in osc.cfg.target_types:
        if target_type == "pose_abs":
            command[:, :3], command[:, 3:7] = subtract_frame_transforms(
                task_frame_pose_b[:, :3], task_frame_pose_b[:, 3:], command[:, :3], command[:, 3:7]
            )
            cmd_idx += 7
        elif target_type == "wrench_abs":
            # These are already defined in target frame for ee_goal_wrench_set_tilted_task (since it is
            # easier), so not transforming
            cmd_idx += 6
        else:
            raise ValueError("Undefined target_type within _convert_to_task_frame().")

    return command, task_frame_pose_b

The OSC command is set with the command vector in the task frame, the end-effector pose in the base frame, and the task (reference) frame pose in the base frame as the following. This information is needed, as the internal computations are done in the base frame.

            # set the osc command
            osc.reset()
            command, task_frame_pose_b = convert_to_task_frame(osc, command=command, ee_target_pose_b=ee_target_pose_b)
            osc.set_command(command=command, current_ee_pose_b=ee_pose_b, current_task_frame_pose_b=task_frame_pose_b)

The joint effort/torque values are computed using the provided robot states and the desired command as the following:

            # compute the joint commands
            joint_efforts = osc.compute(
                jacobian_b=jacobian_b,
                current_ee_pose_b=ee_pose_b,
                current_ee_vel_b=ee_vel_b,
                current_ee_force_b=ee_force_b,
                mass_matrix=mass_matrix,
                gravity=gravity,
                current_joint_pos=joint_pos,
                current_joint_vel=joint_vel,
                nullspace_joint_pos_target=joint_centers,
            )

The computed joint effort/torque targets can then be applied on the robot.

            # apply actions
            robot.set_joint_effort_target_index(target=joint_efforts, joint_ids=arm_joint_ids)
            robot.write_data_to_sim()

The Code Execution#

You can now run the script and see the result:

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

The script will start a simulation with 128 robots. The robots will be controlled using the OSC. The current and desired end-effector poses should be displayed using frame markers in addition to the red tilted wall. You should see that the robot reaches the desired pose while applying a constant force perpendicular to the wall surface.

result of run_osc.py

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