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:
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.
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.
Press Ctrl+C in the terminal to stop the simulation.