From 24f3a9773e3f48cd5a879ff48de72195007e94c6 Mon Sep 17 00:00:00 2001 From: Octi Zhang Date: Thu, 17 Sep 2026 04:05:19 -0700 Subject: [PATCH 1/3] Correct operational-space controller test feedback and fixtures --- .../isaaclab/changelog.d/fix-osc-test-feedback.skip | 1 + .../test/controllers/test_operational_space.py | 11 +++++++---- 2 files changed, 8 insertions(+), 4 deletions(-) create mode 100644 source/isaaclab/changelog.d/fix-osc-test-feedback.skip diff --git a/source/isaaclab/changelog.d/fix-osc-test-feedback.skip b/source/isaaclab/changelog.d/fix-osc-test-feedback.skip new file mode 100644 index 000000000000..ef86a60a4f07 --- /dev/null +++ b/source/isaaclab/changelog.d/fix-osc-test-feedback.skip @@ -0,0 +1 @@ +Stabilized operational-space controller tests with consistent link-origin feedback, unit task-frame orientations, and damped nullspace pose steps. diff --git a/source/isaaclab/test/controllers/test_operational_space.py b/source/isaaclab/test/controllers/test_operational_space.py index ab4c46acc2d9..6b19de81ee5a 100644 --- a/source/isaaclab/test/controllers/test_operational_space.py +++ b/source/isaaclab/test/controllers/test_operational_space.py @@ -122,6 +122,8 @@ def sim(): ], device=sim.device, ) + # These orientations also define task frames, whose transforms require unit quaternions. + ee_goal_abs_quad_set_b /= torch.linalg.vector_norm(ee_goal_abs_quad_set_b, dim=-1, keepdim=True) ee_goal_rel_pos_set = torch.tensor( [ [0.2, 0.0, 0.0], @@ -1174,7 +1176,8 @@ def test_franka_pose_abs_with_nullspace_centering(sim): partial_inertial_dynamics_decoupling=False, gravity_compensation=False, motion_stiffness_task=500.0, - motion_damping_ratio_task=1.0, + # Avoid hitting joint limits during the large pose steps while centering the nullspace. + motion_damping_ratio_task=2.0, nullspace_control="position", nullspace_stiffness=1.0, ) @@ -1635,9 +1638,9 @@ def _update_states( ) 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 + # Match the link-origin reference point used by the pose and Jacobian. + ee_vel_w = robot.data.body_link_vel_w.torch[:, ee_frame_idx, :] + root_vel_w = robot.data.root_link_vel_w.torch 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]) From 2dbfee483b9d8630cc61eaab78f77ca15b496504 Mon Sep 17 00:00:00 2001 From: Octi Zhang Date: Thu, 17 Sep 2026 06:48:31 -0700 Subject: [PATCH 2/3] Align operational-space velocity feedback with link Jacobians --- .../changelog.d/fix-osc-link-velocity.rst | 5 ++ .../changelog.d/fix-osc-test-feedback.skip | 1 - .../envs/mdp/actions/task_space_actions.py | 6 +-- .../controllers/test_operational_space.py | 53 +++++++++++++++++-- 4 files changed, 57 insertions(+), 8 deletions(-) create mode 100644 source/isaaclab/changelog.d/fix-osc-link-velocity.rst delete mode 100644 source/isaaclab/changelog.d/fix-osc-test-feedback.skip diff --git a/source/isaaclab/changelog.d/fix-osc-link-velocity.rst b/source/isaaclab/changelog.d/fix-osc-link-velocity.rst new file mode 100644 index 000000000000..87556a701d28 --- /dev/null +++ b/source/isaaclab/changelog.d/fix-osc-link-velocity.rst @@ -0,0 +1,5 @@ +Fixed +^^^^^ + +* Fixed operational-space controller velocity feedback to use the same link origins as the end-effector pose and + Jacobian in the action term and integration tests. diff --git a/source/isaaclab/changelog.d/fix-osc-test-feedback.skip b/source/isaaclab/changelog.d/fix-osc-test-feedback.skip deleted file mode 100644 index ef86a60a4f07..000000000000 --- a/source/isaaclab/changelog.d/fix-osc-test-feedback.skip +++ /dev/null @@ -1 +0,0 @@ -Stabilized operational-space controller tests with consistent link-origin feedback, unit task-frame orientations, and damped nullspace pose steps. diff --git a/source/isaaclab/isaaclab/envs/mdp/actions/task_space_actions.py b/source/isaaclab/isaaclab/envs/mdp/actions/task_space_actions.py index ea9d319938c5..e34343ef3194 100644 --- a/source/isaaclab/isaaclab/envs/mdp/actions/task_space_actions.py +++ b/source/isaaclab/isaaclab/envs/mdp/actions/task_space_actions.py @@ -712,10 +712,10 @@ def _compute_ee_pose(self): def _compute_ee_velocity(self): """Computes the velocity of the ee frame in root frame.""" - # Extract end-effector velocity in the world frame - self._ee_vel_w[:] = self._asset.data.body_vel_w.torch[:, self._ee_body_idx, :] + # Match the link-origin reference point used by the pose and Jacobian. + self._ee_vel_w[:] = self._asset.data.body_link_vel_w.torch[:, self._ee_body_idx, :] # Compute the relative velocity in the world frame - relative_vel_w = self._ee_vel_w - self._asset.data.root_vel_w.torch + relative_vel_w = self._ee_vel_w - self._asset.data.root_link_vel_w.torch # Convert ee velocities from world to root frame root_quat_w = self._asset.data.root_quat_w.torch diff --git a/source/isaaclab/test/controllers/test_operational_space.py b/source/isaaclab/test/controllers/test_operational_space.py index 6b19de81ee5a..80c2344dc0c7 100644 --- a/source/isaaclab/test/controllers/test_operational_space.py +++ b/source/isaaclab/test/controllers/test_operational_space.py @@ -12,6 +12,8 @@ """Rest everything follows.""" +from types import SimpleNamespace + import numpy as np import pytest import torch @@ -31,6 +33,7 @@ ## from isaaclab.envs import ManagerBasedEnv, ManagerBasedEnvCfg from isaaclab.envs.mdp.actions.actions_cfg import OperationalSpaceControllerActionCfg +from isaaclab.envs.mdp.actions.task_space_actions import OperationalSpaceControllerAction from isaaclab.managers import ObservationGroupCfg as ObsGroup from isaaclab.managers import ObservationTermCfg as ObsTerm from isaaclab.managers import SceneEntityCfg @@ -122,8 +125,6 @@ def sim(): ], device=sim.device, ) - # These orientations also define task frames, whose transforms require unit quaternions. - ee_goal_abs_quad_set_b /= torch.linalg.vector_norm(ee_goal_abs_quad_set_b, dim=-1, keepdim=True) ee_goal_rel_pos_set = torch.tensor( [ [0.2, 0.0, 0.0], @@ -1176,8 +1177,7 @@ def test_franka_pose_abs_with_nullspace_centering(sim): partial_inertial_dynamics_decoupling=False, gravity_compensation=False, motion_stiffness_task=500.0, - # Avoid hitting joint limits during the large pose steps while centering the nullspace. - motion_damping_ratio_task=2.0, + motion_damping_ratio_task=1.0, nullspace_control="position", nullspace_stiffness=1.0, ) @@ -1339,6 +1339,51 @@ class _FloatingBaseOscEnvCfg(ManagerBasedEnvCfg): sim: sim_utils.SimulationCfg = sim_utils.SimulationCfg(dt=0.01) +@pytest.mark.isaacsim_ci +@pytest.mark.parametrize("feedback_source", ["test_helper", "action"]) +def test_franka_velocity_feedback_matches_jacobian(sim, feedback_source): + """Both OSC callers must measure velocity at the link origin used by the Jacobian.""" + sim_context, num_envs, robot_cfg, *_ = sim + robot = Articulation(cfg=robot_cfg) + sim_context.reset() + arm_joint_ids, _ = robot.find_joints("panda_joint.*") + ee_frame_idx = robot.find_bodies("panda_hand")[0][0] + + joint_vel = torch.zeros_like(robot.data.default_joint_vel.torch) + joint_vel[:, arm_joint_ids] = torch.linspace(0.1, 0.7, len(arm_joint_ids), device=sim_context.device) + robot.write_joint_state_to_sim_index(position=robot.data.default_joint_pos.torch, velocity=joint_vel) + sim_context.step(render=False) + robot.update(sim_context.get_physics_dt()) + + # Angular motion and the hand's COM offset must expose the reference-point mismatch. + assert not torch.allclose( + robot.data.body_com_vel_w.torch[:, ee_frame_idx, :3], + robot.data.body_link_vel_w.torch[:, ee_frame_idx, :3], + atol=1e-4, + rtol=1e-4, + ) + if feedback_source == "test_helper": + states = _update_states(robot, ee_frame_idx, arm_joint_ids, sim_context, None, num_envs) + jacobian_b, _, _, _, ee_vel_b, _, _, _, _, joint_vel = states + else: + env = SimpleNamespace(scene={"robot": robot}, sim=sim_context, num_envs=num_envs, device=sim_context.device) + action_cfg = OperationalSpaceControllerActionCfg( + asset_name="robot", + joint_names=["panda_joint.*"], + body_name="panda_hand", + controller_cfg=OperationalSpaceControllerCfg(target_types=["pose_abs"]), + ) + action_term = OperationalSpaceControllerAction(action_cfg, env) + action_term._compute_ee_jacobian() + action_term._compute_ee_velocity() + jacobian_b, ee_vel_b = action_term._jacobian_b, action_term._ee_vel_b + joint_vel = robot.data.joint_vel.torch[:, arm_joint_ids] + + # With a stationary fixed base, the link twist must equal J(q) * q_dot. + expected_vel_b = torch.bmm(jacobian_b, joint_vel.unsqueeze(-1)).squeeze(-1) + torch.testing.assert_close(ee_vel_b, expected_vel_b, atol=1e-4, rtol=1e-4) + + @pytest.mark.isaacsim_ci def test_floating_base_osc_action_term_indexing(): """Regression test for #4999 / PR #5107: verify OperationalSpaceControllerAction uses correct From 6d266c8cb98445e3afdeb3c0d35e611a80197430 Mon Sep 17 00:00:00 2001 From: Octi Zhang Date: Thu, 17 Sep 2026 06:56:59 -0700 Subject: [PATCH 3/3] Normalize constructed task frames in OSC tests --- .../controllers/test_operational_space.py | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) diff --git a/source/isaaclab/test/controllers/test_operational_space.py b/source/isaaclab/test/controllers/test_operational_space.py index 80c2344dc0c7..1018d3c34726 100644 --- a/source/isaaclab/test/controllers/test_operational_space.py +++ b/source/isaaclab/test/controllers/test_operational_space.py @@ -864,6 +864,23 @@ def test_franka_hybrid_variable_kp_impedance(sim): ) +@pytest.mark.isaacsim_ci +def test_task_frame_conversion_preserves_absolute_target(): + """A rounded pose command must resolve to the same target through either reference frame.""" + osc_cfg = OperationalSpaceControllerCfg(target_types=["pose_abs"]) + osc = OperationalSpaceController(osc_cfg, num_envs=1, device="cpu") + target_b = torch.tensor([[0.5, -0.4, 0.6, 0.707, 0.0, 0.0, 0.707]]) + command = target_b.clone() + resolved_targets = [] + for frame in ("root", "task"): + converted_command, task_frame_pose_b = _convert_to_task_frame(osc, command, target_b, frame) + osc.set_command(converted_command, current_task_frame_pose_b=task_frame_pose_b) + resolved_targets.append(osc.desired_ee_pose_b.clone()) + + torch.testing.assert_close(resolved_targets[0], resolved_targets[1], atol=1e-6, rtol=0.0) + torch.testing.assert_close(command, target_b, atol=0.0, rtol=0.0) + + @pytest.mark.isaacsim_ci def test_franka_taskframe_pose_abs(sim): """Test absolute pose control in task frame with fixed impedance and inertial dynamics decoupling.""" @@ -1802,6 +1819,8 @@ def _convert_to_task_frame( # Convert target commands from base to the task frame command = command.clone() task_frame_pose_b = ee_target_pose_b.clone() + # Rounded goal quaternions must define a unit rotation when used as a reference frame. + task_frame_pose_b[:, 3:] /= torch.linalg.vector_norm(task_frame_pose_b[:, 3:], dim=-1, keepdim=True) cmd_idx = 0 for target_type in osc.cfg.target_types: