From edd6fdae21e5511e177f318208afff9db1e055ef Mon Sep 17 00:00:00 2001 From: Zeng Qingcheng <60593302+NeoZng@users.noreply.github.com> Date: Thu, 1 Oct 2026 02:31:34 +0800 Subject: [PATCH 01/11] Fix body-offset Jacobian in Newton task-space action terms Rotate the body offset into the root frame before shifting the translational rows, and keep the angular rows unchanged, as the core task-space action terms do since #7989. --- ...newton-task-space-body-offset-jacobian.rst | 8 +++ .../mdp/actions/newton_task_space_actions.py | 9 ++-- .../envs/test_newton_task_space_actions.py | 54 +++++++++++++++++++ 3 files changed, 68 insertions(+), 3 deletions(-) create mode 100644 source/isaaclab_newton/changelog.d/fix-newton-task-space-body-offset-jacobian.rst create mode 100644 source/isaaclab_newton/test/envs/test_newton_task_space_actions.py diff --git a/source/isaaclab_newton/changelog.d/fix-newton-task-space-body-offset-jacobian.rst b/source/isaaclab_newton/changelog.d/fix-newton-task-space-body-offset-jacobian.rst new file mode 100644 index 000000000000..385b2f585fd0 --- /dev/null +++ b/source/isaaclab_newton/changelog.d/fix-newton-task-space-body-offset-jacobian.rst @@ -0,0 +1,8 @@ +Fixed +^^^^^ + +* Fixed the body-offset Jacobian of + :class:`~isaaclab_newton.envs.mdp.actions.NewtonDifferentialInverseKinematicsAction` and + :class:`~isaaclab_newton.envs.mdp.actions.NewtonOperationalSpaceControllerAction`. The offset is now rotated into + the root frame by the body orientation before shifting the translational rows, and the angular rows are no longer + rotated by the offset rotation, matching the target frame's pose and velocity. diff --git a/source/isaaclab_newton/isaaclab_newton/envs/mdp/actions/newton_task_space_actions.py b/source/isaaclab_newton/isaaclab_newton/envs/mdp/actions/newton_task_space_actions.py index 026b71bf2fc3..0f60e99701ac 100644 --- a/source/isaaclab_newton/isaaclab_newton/envs/mdp/actions/newton_task_space_actions.py +++ b/source/isaaclab_newton/isaaclab_newton/envs/mdp/actions/newton_task_space_actions.py @@ -107,11 +107,14 @@ def _compute_ee_jacobian(self) -> torch.Tensor: self._jacobian_b[:, :3] = torch.bmm(root_rot, jacobian_w[:, :3]) self._jacobian_b[:, 3:] = torch.bmm(root_rot, jacobian_w[:, 3:]) if self._offset_pos is not None: - # v_ee = v_link + w_link x r_link_ee, then rotate into the target frame + # Express the lever arm in root axes; a rigid offset leaves angular velocity unchanged. + body_quat_b = math_utils.quat_mul( + math_utils.quat_inv(data.root_quat_w.torch), data.body_quat_w.torch[:, self._body_idx] + ) + offset_pos_b = math_utils.quat_apply(body_quat_b, self._offset_pos) self._jacobian_b[:, :3] += torch.bmm( - -math_utils.skew_symmetric_matrix(self._offset_pos), self._jacobian_b[:, 3:] + -math_utils.skew_symmetric_matrix(offset_pos_b), self._jacobian_b[:, 3:] ) - self._jacobian_b[:, 3:] = torch.bmm(math_utils.matrix_from_quat(self._offset_rot), self._jacobian_b[:, 3:]) return self._jacobian_b diff --git a/source/isaaclab_newton/test/envs/test_newton_task_space_actions.py b/source/isaaclab_newton/test/envs/test_newton_task_space_actions.py new file mode 100644 index 000000000000..17869d9f2f69 --- /dev/null +++ b/source/isaaclab_newton/test/envs/test_newton_task_space_actions.py @@ -0,0 +1,54 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Kinematics of the target frame shared by the Newton task-space action terms.""" + +import math +from types import SimpleNamespace + +import pytest +import torch +from isaaclab_newton.envs.mdp.actions.newton_task_space_actions import _NewtonTaskSpaceAction + +from isaaclab.utils import math as math_utils + +pytestmark = pytest.mark.unit + +_S = math.sin(math.pi / 4.0) + + +@pytest.mark.parametrize( + "root_quat_w", + [(0.0, 0.0, 0.0, 1.0), (0.0, 0.0, _S, _S), (_S, 0.0, 0.0, _S)], + ids=["identity_root", "yawed_root", "tilted_root"], +) +def test_body_offset_jacobian_uses_offset_in_root_frame(root_quat_w: tuple[float, float, float, float]): + """The offset uses root axes, and a rigid offset rotation leaves angular rows unchanged.""" + root_quat_w = torch.tensor([root_quat_w]) + # One revolute joint about the root z axis through the body origin; the body is yawed 90 degrees from the root. + body_quat_w = math_utils.quat_mul(root_quat_w, torch.tensor([[0.0, 0.0, _S, _S]])) + jacobian_w = torch.zeros(1, 1, 6, 1) + jacobian_w[0, 0, 3:, 0] = math_utils.quat_apply(root_quat_w, torch.tensor([[0.0, 0.0, 1.0]]))[0] + data = SimpleNamespace( + body_link_jacobian_w=SimpleNamespace(torch=jacobian_w), + root_quat_w=SimpleNamespace(torch=root_quat_w), + body_quat_w=SimpleNamespace(torch=body_quat_w.unsqueeze(1)), + ) + term = SimpleNamespace( + _asset=SimpleNamespace(data=data), + _body_idx=0, + _jacobi_body_idx=0, + _jacobi_joint_ids=[0], + _jacobian_b=torch.zeros(1, 6, 1), + _offset_pos=torch.tensor([[1.0, 0.0, 0.0]]), + # a 90 degree rotation about x for the target frame + _offset_rot=torch.tensor([[_S, 0.0, 0.0, _S]]), + ) + + jacobian_b = _NewtonTaskSpaceAction._compute_ee_jacobian(term) + + # The lever arm in root axes is (0, 1, 0), and z x (0, 1, 0) = (-1, 0, 0). + expected = torch.tensor([[[-1.0], [0.0], [0.0], [0.0], [0.0], [1.0]]]) + torch.testing.assert_close(jacobian_b, expected, atol=1e-6, rtol=0.0) From ed588d8b0923e0094b554aa39a203458f027f9bc Mon Sep 17 00:00:00 2001 From: Zeng Qingcheng <60593302+NeoZng@users.noreply.github.com> Date: Thu, 1 Oct 2026 02:48:58 +0800 Subject: [PATCH 02/11] Reuse the inverse root quaternion for the body-offset lever arm --- .../envs/mdp/actions/newton_task_space_actions.py | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/source/isaaclab_newton/isaaclab_newton/envs/mdp/actions/newton_task_space_actions.py b/source/isaaclab_newton/isaaclab_newton/envs/mdp/actions/newton_task_space_actions.py index 0f60e99701ac..6d8d278d184d 100644 --- a/source/isaaclab_newton/isaaclab_newton/envs/mdp/actions/newton_task_space_actions.py +++ b/source/isaaclab_newton/isaaclab_newton/envs/mdp/actions/newton_task_space_actions.py @@ -103,14 +103,13 @@ def _compute_ee_jacobian(self) -> torch.Tensor: """Compute the target-frame Jacobian in the root frame, shape (num_envs, 6, num_joints).""" data = self._asset.data jacobian_w = data.body_link_jacobian_w.torch[:, self._jacobi_body_idx, :, self._jacobi_joint_ids] - root_rot = math_utils.matrix_from_quat(math_utils.quat_inv(data.root_quat_w.torch)) + root_quat_inv = math_utils.quat_inv(data.root_quat_w.torch) + root_rot = math_utils.matrix_from_quat(root_quat_inv) self._jacobian_b[:, :3] = torch.bmm(root_rot, jacobian_w[:, :3]) self._jacobian_b[:, 3:] = torch.bmm(root_rot, jacobian_w[:, 3:]) if self._offset_pos is not None: # Express the lever arm in root axes; a rigid offset leaves angular velocity unchanged. - body_quat_b = math_utils.quat_mul( - math_utils.quat_inv(data.root_quat_w.torch), data.body_quat_w.torch[:, self._body_idx] - ) + body_quat_b = math_utils.quat_mul(root_quat_inv, data.body_quat_w.torch[:, self._body_idx]) offset_pos_b = math_utils.quat_apply(body_quat_b, self._offset_pos) self._jacobian_b[:, :3] += torch.bmm( -math_utils.skew_symmetric_matrix(offset_pos_b), self._jacobian_b[:, 3:] From 374dc2abef16bd2b986f9961057261f2b3a266bc Mon Sep 17 00:00:00 2001 From: Zeng Qingcheng <60593302+NeoZng@users.noreply.github.com> Date: Thu, 1 Oct 2026 03:48:17 +0800 Subject: [PATCH 03/11] Use the towncrier changelog fragment format --- ...st => fix-newton-task-space-body-offset-jacobian.fixed.rst} | 3 --- 1 file changed, 3 deletions(-) rename source/isaaclab_newton/changelog.d/{fix-newton-task-space-body-offset-jacobian.rst => fix-newton-task-space-body-offset-jacobian.fixed.rst} (97%) diff --git a/source/isaaclab_newton/changelog.d/fix-newton-task-space-body-offset-jacobian.rst b/source/isaaclab_newton/changelog.d/fix-newton-task-space-body-offset-jacobian.fixed.rst similarity index 97% rename from source/isaaclab_newton/changelog.d/fix-newton-task-space-body-offset-jacobian.rst rename to source/isaaclab_newton/changelog.d/fix-newton-task-space-body-offset-jacobian.fixed.rst index 385b2f585fd0..5e58e9d884b8 100644 --- a/source/isaaclab_newton/changelog.d/fix-newton-task-space-body-offset-jacobian.rst +++ b/source/isaaclab_newton/changelog.d/fix-newton-task-space-body-offset-jacobian.fixed.rst @@ -1,6 +1,3 @@ -Fixed -^^^^^ - * Fixed the body-offset Jacobian of :class:`~isaaclab_newton.envs.mdp.actions.NewtonDifferentialInverseKinematicsAction` and :class:`~isaaclab_newton.envs.mdp.actions.NewtonOperationalSpaceControllerAction`. The offset is now rotated into From 2d9a3ef5ff2e7334e1445a7eef5689606a641703 Mon Sep 17 00:00:00 2001 From: Zeng Qingcheng <60593302+NeoZng@users.noreply.github.com> Date: Thu, 1 Oct 2026 18:12:14 +0800 Subject: [PATCH 04/11] Reduce the body-offset Jacobian test to the tilted-root case --- ...ix-newton-task-space-body-offset-jacobian.fixed.rst | 4 ++-- .../test/envs/test_newton_task_space_actions.py | 10 +++------- 2 files changed, 5 insertions(+), 9 deletions(-) diff --git a/source/isaaclab_newton/changelog.d/fix-newton-task-space-body-offset-jacobian.fixed.rst b/source/isaaclab_newton/changelog.d/fix-newton-task-space-body-offset-jacobian.fixed.rst index 5e58e9d884b8..b197e161d92b 100644 --- a/source/isaaclab_newton/changelog.d/fix-newton-task-space-body-offset-jacobian.fixed.rst +++ b/source/isaaclab_newton/changelog.d/fix-newton-task-space-body-offset-jacobian.fixed.rst @@ -1,5 +1,5 @@ * Fixed the body-offset Jacobian of :class:`~isaaclab_newton.envs.mdp.actions.NewtonDifferentialInverseKinematicsAction` and :class:`~isaaclab_newton.envs.mdp.actions.NewtonOperationalSpaceControllerAction`. The offset is now rotated into - the root frame by the body orientation before shifting the translational rows, and the angular rows are no longer - rotated by the offset rotation, matching the target frame's pose and velocity. + the root frame by the body orientation relative to the root before shifting the translational rows, and the + angular rows are no longer rotated by the offset rotation, matching the target frame's pose and velocity. diff --git a/source/isaaclab_newton/test/envs/test_newton_task_space_actions.py b/source/isaaclab_newton/test/envs/test_newton_task_space_actions.py index 17869d9f2f69..5037e8a9fbff 100644 --- a/source/isaaclab_newton/test/envs/test_newton_task_space_actions.py +++ b/source/isaaclab_newton/test/envs/test_newton_task_space_actions.py @@ -19,14 +19,10 @@ _S = math.sin(math.pi / 4.0) -@pytest.mark.parametrize( - "root_quat_w", - [(0.0, 0.0, 0.0, 1.0), (0.0, 0.0, _S, _S), (_S, 0.0, 0.0, _S)], - ids=["identity_root", "yawed_root", "tilted_root"], -) -def test_body_offset_jacobian_uses_offset_in_root_frame(root_quat_w: tuple[float, float, float, float]): +def test_body_offset_jacobian_uses_offset_in_root_frame(): """The offset uses root axes, and a rigid offset rotation leaves angular rows unchanged.""" - root_quat_w = torch.tensor([root_quat_w]) + # a tilted root, so that a body-frame or world-frame lever arm gives a different result + root_quat_w = torch.tensor([[_S, 0.0, 0.0, _S]]) # One revolute joint about the root z axis through the body origin; the body is yawed 90 degrees from the root. body_quat_w = math_utils.quat_mul(root_quat_w, torch.tensor([[0.0, 0.0, _S, _S]])) jacobian_w = torch.zeros(1, 1, 6, 1) From 9a3405d5111eb6fabfc5d77c8ea2e4ed2ed2ed4a Mon Sep 17 00:00:00 2001 From: Zeng Qingcheng <60593302+NeoZng@users.noreply.github.com> Date: Fri, 2 Oct 2026 16:53:00 +0800 Subject: [PATCH 05/11] Shift the body-offset Jacobian in world axes before rotating it to the root --- ...on-task-space-body-offset-jacobian.fixed.rst | 6 +++--- .../mdp/actions/newton_task_space_actions.py | 17 +++++++---------- 2 files changed, 10 insertions(+), 13 deletions(-) diff --git a/source/isaaclab_newton/changelog.d/fix-newton-task-space-body-offset-jacobian.fixed.rst b/source/isaaclab_newton/changelog.d/fix-newton-task-space-body-offset-jacobian.fixed.rst index b197e161d92b..f3ec05a5269b 100644 --- a/source/isaaclab_newton/changelog.d/fix-newton-task-space-body-offset-jacobian.fixed.rst +++ b/source/isaaclab_newton/changelog.d/fix-newton-task-space-body-offset-jacobian.fixed.rst @@ -1,5 +1,5 @@ * Fixed the body-offset Jacobian of :class:`~isaaclab_newton.envs.mdp.actions.NewtonDifferentialInverseKinematicsAction` and - :class:`~isaaclab_newton.envs.mdp.actions.NewtonOperationalSpaceControllerAction`. The offset is now rotated into - the root frame by the body orientation relative to the root before shifting the translational rows, and the - angular rows are no longer rotated by the offset rotation, matching the target frame's pose and velocity. + :class:`~isaaclab_newton.envs.mdp.actions.NewtonOperationalSpaceControllerAction`. The translational rows are now + shifted by the offset rotated with the body orientation, and the angular rows are no longer rotated by the offset + rotation, matching the target frame's pose and velocity. diff --git a/source/isaaclab_newton/isaaclab_newton/envs/mdp/actions/newton_task_space_actions.py b/source/isaaclab_newton/isaaclab_newton/envs/mdp/actions/newton_task_space_actions.py index 6d8d278d184d..ddf1a1b95d54 100644 --- a/source/isaaclab_newton/isaaclab_newton/envs/mdp/actions/newton_task_space_actions.py +++ b/source/isaaclab_newton/isaaclab_newton/envs/mdp/actions/newton_task_space_actions.py @@ -103,17 +103,14 @@ def _compute_ee_jacobian(self) -> torch.Tensor: """Compute the target-frame Jacobian in the root frame, shape (num_envs, 6, num_joints).""" data = self._asset.data jacobian_w = data.body_link_jacobian_w.torch[:, self._jacobi_body_idx, :, self._jacobi_joint_ids] - root_quat_inv = math_utils.quat_inv(data.root_quat_w.torch) - root_rot = math_utils.matrix_from_quat(root_quat_inv) - self._jacobian_b[:, :3] = torch.bmm(root_rot, jacobian_w[:, :3]) - self._jacobian_b[:, 3:] = torch.bmm(root_rot, jacobian_w[:, 3:]) + jac_lin_w, jac_ang_w = jacobian_w[:, :3], jacobian_w[:, 3:] if self._offset_pos is not None: - # Express the lever arm in root axes; a rigid offset leaves angular velocity unchanged. - body_quat_b = math_utils.quat_mul(root_quat_inv, data.body_quat_w.torch[:, self._body_idx]) - offset_pos_b = math_utils.quat_apply(body_quat_b, self._offset_pos) - self._jacobian_b[:, :3] += torch.bmm( - -math_utils.skew_symmetric_matrix(offset_pos_b), self._jacobian_b[:, 3:] - ) + # v_offset = v_body + w x r in world axes; a rigid offset leaves angular velocity unchanged. + offset_w = math_utils.quat_apply(data.body_quat_w.torch[:, self._body_idx], self._offset_pos) + jac_lin_w = jac_lin_w + torch.linalg.cross(jac_ang_w, offset_w[:, :, None].expand_as(jac_ang_w), dim=1) + world_to_root = math_utils.matrix_from_quat(math_utils.quat_inv(data.root_quat_w.torch)) + self._jacobian_b[:, :3] = torch.bmm(world_to_root, jac_lin_w) + self._jacobian_b[:, 3:] = torch.bmm(world_to_root, jac_ang_w) return self._jacobian_b From 9a9836d90ff2d76c219b03a0abc097232dab1f15 Mon Sep 17 00:00:00 2001 From: Zeng Qingcheng <60593302+NeoZng@users.noreply.github.com> Date: Fri, 2 Oct 2026 16:53:01 +0800 Subject: [PATCH 06/11] Keep the hand body apart from its Jacobian row in the offset test --- .../test/envs/test_newton_task_space_actions.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/source/isaaclab_newton/test/envs/test_newton_task_space_actions.py b/source/isaaclab_newton/test/envs/test_newton_task_space_actions.py index 5037e8a9fbff..39659a76eed3 100644 --- a/source/isaaclab_newton/test/envs/test_newton_task_space_actions.py +++ b/source/isaaclab_newton/test/envs/test_newton_task_space_actions.py @@ -30,11 +30,11 @@ def test_body_offset_jacobian_uses_offset_in_root_frame(): data = SimpleNamespace( body_link_jacobian_w=SimpleNamespace(torch=jacobian_w), root_quat_w=SimpleNamespace(torch=root_quat_w), - body_quat_w=SimpleNamespace(torch=body_quat_w.unsqueeze(1)), + body_quat_w=SimpleNamespace(torch=torch.stack([root_quat_w, body_quat_w], dim=1)), # [root, hand] ) term = SimpleNamespace( _asset=SimpleNamespace(data=data), - _body_idx=0, + _body_idx=1, # a fixed base has no Jacobian row for its root, so the hand stays Jacobian row 0 _jacobi_body_idx=0, _jacobi_joint_ids=[0], _jacobian_b=torch.zeros(1, 6, 1), From ba216b17ca005b1a62161d6d345ada24bdd1df9c Mon Sep 17 00:00:00 2001 From: Octi Zhang Date: Mon, 5 Oct 2026 14:33:59 -0700 Subject: [PATCH 07/11] Share task-space point-shift math across actions --- ...share-task-space-jacobian-offset.minor.rst | 4 ++++ .../envs/mdp/actions/task_space_actions.py | 8 +++---- source/isaaclab/isaaclab/utils/math.py | 22 +++++++++++++++++++ source/isaaclab/test/utils/test_math.py | 8 +++++++ ...ewton-task-space-body-offset-jacobian.rst} | 3 +++ .../mdp/actions/newton_task_space_actions.py | 7 +++--- .../envs/test_newton_task_space_actions.py | 22 +++++++++++++++++++ 7 files changed, 66 insertions(+), 8 deletions(-) create mode 100644 source/isaaclab/changelog.d/share-task-space-jacobian-offset.minor.rst rename source/isaaclab_newton/changelog.d/{fix-newton-task-space-body-offset-jacobian.fixed.rst => fix-newton-task-space-body-offset-jacobian.rst} (96%) diff --git a/source/isaaclab/changelog.d/share-task-space-jacobian-offset.minor.rst b/source/isaaclab/changelog.d/share-task-space-jacobian-offset.minor.rst new file mode 100644 index 000000000000..15555fa6d268 --- /dev/null +++ b/source/isaaclab/changelog.d/share-task-space-jacobian-offset.minor.rst @@ -0,0 +1,4 @@ +Added +^^^^^ + +* Added :func:`~isaaclab.utils.math.velocity_at_point` to share point-shift math across task-space Jacobians. 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 3b077356cea5..cb89486d5c86 100644 --- a/source/isaaclab/isaaclab/envs/mdp/actions/task_space_actions.py +++ b/source/isaaclab/isaaclab/envs/mdp/actions/task_space_actions.py @@ -261,8 +261,8 @@ def _compute_frame_jacobian(self): self._asset.data.body_quat_w.torch[:, self._body_idx], ) offset_pos_b = math_utils.quat_apply(body_quat_b, self._offset_pos) - self._jacobian_b[:, 0:3, :] += torch.bmm( - -math_utils.skew_symmetric_matrix(offset_pos_b), self._jacobian_b[:, 3:, :] + self._jacobian_b[:, :3] = math_utils.velocity_at_point( + self._jacobian_b[:, :3], self._jacobian_b[:, 3:], offset_pos_b[:, :, None], dim=1 ) return self._jacobian_b @@ -676,8 +676,8 @@ def _compute_ee_jacobian(self): self._asset.data.body_quat_w.torch[:, self._ee_body_idx], ) offset_pos_b = math_utils.quat_apply(body_quat_b, self._offset_pos) - self._jacobian_b[:, 0:3, :] += torch.bmm( - -math_utils.skew_symmetric_matrix(offset_pos_b), self._jacobian_b[:, 3:, :] + self._jacobian_b[:, :3] = math_utils.velocity_at_point( + self._jacobian_b[:, :3], self._jacobian_b[:, 3:], offset_pos_b[:, :, None], dim=1 ) def _compute_ee_pose(self): diff --git a/source/isaaclab/isaaclab/utils/math.py b/source/isaaclab/isaaclab/utils/math.py index eff9e22d576d..9055aa16cf28 100644 --- a/source/isaaclab/isaaclab/utils/math.py +++ b/source/isaaclab/isaaclab/utils/math.py @@ -756,6 +756,28 @@ def skew_symmetric_matrix(vec: torch.Tensor) -> torch.Tensor: return skew_sym_mat +@torch.jit.script +def velocity_at_point( + linear_velocity: torch.Tensor, angular_velocity: torch.Tensor, point_offset: torch.Tensor, dim: int = -1 +) -> torch.Tensor: + """Return linear velocity at a displaced point: ``v_point = v_origin + ω × r``. + + All inputs use the same reference frame. ``dim`` selects the size-3 vector axis; all other dimensions + follow PyTorch broadcasting. This also shifts each column of a batched Jacobian independently. + Angular velocity does not change under a point shift. + + Args: + linear_velocity: Linear velocity at the original point. + angular_velocity: Angular velocity about the original point. + point_offset: Displacement from the original point, expressed in the same frame. + dim: Vector axis. Defaults to the last axis. + + Returns: + Linear velocity at the displaced point. + """ + return linear_velocity + torch.linalg.cross(angular_velocity, point_offset, dim=dim) + + """ Transformations """ diff --git a/source/isaaclab/test/utils/test_math.py b/source/isaaclab/test/utils/test_math.py index 14efba41a888..19269e3d915a 100644 --- a/source/isaaclab/test/utils/test_math.py +++ b/source/isaaclab/test/utils/test_math.py @@ -488,6 +488,14 @@ def test_skew_symmetric_matrix(device, shape): torch.testing.assert_close(vec_rand_resized[:, 0], mat_value[:, 2, 1]) +def test_velocity_at_point(): + """A point displaced perpendicular to an angular velocity gains the expected linear velocity.""" + linear = torch.tensor([[1.0, 2.0, 3.0]]) + angular = torch.tensor([[0.0, 0.0, 2.0]]) + offset = torch.tensor([[0.0, 3.0, 0.0]]) + torch.testing.assert_close(math_utils.velocity_at_point(linear, angular, offset), torch.tensor([[-5.0, 2.0, 3.0]])) + + @pytest.mark.parametrize("device", test_devices()) def test_orthogonalize_perspective_depth(device): """Test for converting perspective depth to orthogonal depth.""" diff --git a/source/isaaclab_newton/changelog.d/fix-newton-task-space-body-offset-jacobian.fixed.rst b/source/isaaclab_newton/changelog.d/fix-newton-task-space-body-offset-jacobian.rst similarity index 96% rename from source/isaaclab_newton/changelog.d/fix-newton-task-space-body-offset-jacobian.fixed.rst rename to source/isaaclab_newton/changelog.d/fix-newton-task-space-body-offset-jacobian.rst index f3ec05a5269b..d1304fdcedb2 100644 --- a/source/isaaclab_newton/changelog.d/fix-newton-task-space-body-offset-jacobian.fixed.rst +++ b/source/isaaclab_newton/changelog.d/fix-newton-task-space-body-offset-jacobian.rst @@ -1,3 +1,6 @@ +Fixed +^^^^^ + * Fixed the body-offset Jacobian of :class:`~isaaclab_newton.envs.mdp.actions.NewtonDifferentialInverseKinematicsAction` and :class:`~isaaclab_newton.envs.mdp.actions.NewtonOperationalSpaceControllerAction`. The translational rows are now diff --git a/source/isaaclab_newton/isaaclab_newton/envs/mdp/actions/newton_task_space_actions.py b/source/isaaclab_newton/isaaclab_newton/envs/mdp/actions/newton_task_space_actions.py index ddf1a1b95d54..062daeaced23 100644 --- a/source/isaaclab_newton/isaaclab_newton/envs/mdp/actions/newton_task_space_actions.py +++ b/source/isaaclab_newton/isaaclab_newton/envs/mdp/actions/newton_task_space_actions.py @@ -103,14 +103,13 @@ def _compute_ee_jacobian(self) -> torch.Tensor: """Compute the target-frame Jacobian in the root frame, shape (num_envs, 6, num_joints).""" data = self._asset.data jacobian_w = data.body_link_jacobian_w.torch[:, self._jacobi_body_idx, :, self._jacobi_joint_ids] - jac_lin_w, jac_ang_w = jacobian_w[:, :3], jacobian_w[:, 3:] + jac_lin_w = jacobian_w[:, :3] if self._offset_pos is not None: - # v_offset = v_body + w x r in world axes; a rigid offset leaves angular velocity unchanged. offset_w = math_utils.quat_apply(data.body_quat_w.torch[:, self._body_idx], self._offset_pos) - jac_lin_w = jac_lin_w + torch.linalg.cross(jac_ang_w, offset_w[:, :, None].expand_as(jac_ang_w), dim=1) + jac_lin_w = math_utils.velocity_at_point(jac_lin_w, jacobian_w[:, 3:], offset_w[:, :, None], dim=1) world_to_root = math_utils.matrix_from_quat(math_utils.quat_inv(data.root_quat_w.torch)) self._jacobian_b[:, :3] = torch.bmm(world_to_root, jac_lin_w) - self._jacobian_b[:, 3:] = torch.bmm(world_to_root, jac_ang_w) + self._jacobian_b[:, 3:] = torch.bmm(world_to_root, jacobian_w[:, 3:]) return self._jacobian_b diff --git a/source/isaaclab_newton/test/envs/test_newton_task_space_actions.py b/source/isaaclab_newton/test/envs/test_newton_task_space_actions.py index 39659a76eed3..2c0c7035a82c 100644 --- a/source/isaaclab_newton/test/envs/test_newton_task_space_actions.py +++ b/source/isaaclab_newton/test/envs/test_newton_task_space_actions.py @@ -5,13 +5,20 @@ """Kinematics of the target frame shared by the Newton task-space action terms.""" +import ast +import inspect import math +import textwrap from types import SimpleNamespace import pytest import torch from isaaclab_newton.envs.mdp.actions.newton_task_space_actions import _NewtonTaskSpaceAction +from isaaclab.envs.mdp.actions.task_space_actions import ( + DifferentialInverseKinematicsAction, + OperationalSpaceControllerAction, +) from isaaclab.utils import math as math_utils pytestmark = pytest.mark.unit @@ -48,3 +55,18 @@ def test_body_offset_jacobian_uses_offset_in_root_frame(): # The lever arm in root axes is (0, 1, 0), and z x (0, 1, 0) = (-1, 0, 0). expected = torch.tensor([[[-1.0], [0.0], [0.0], [0.0], [0.0], [1.0]]]) torch.testing.assert_close(jacobian_b, expected, atol=1e-6, rtol=0.0) + + +def test_task_space_actions_do_not_own_point_shift_math(): + """All task-space actions must use the shared point shift instead of recopying its formula.""" + methods = ( + DifferentialInverseKinematicsAction._compute_frame_jacobian, + OperationalSpaceControllerAction._compute_ee_jacobian, + _NewtonTaskSpaceAction._compute_ee_jacobian, + ) + for method in methods: + tree = ast.parse(textwrap.dedent(inspect.getsource(method))) + calls = [node.func for node in ast.walk(tree) if isinstance(node, ast.Call)] + names = [call.attr for call in calls if isinstance(call, ast.Attribute)] + assert names.count("velocity_at_point") == 1, method.__qualname__ + assert not {"skew_symmetric_matrix", "cross"}.intersection(names), method.__qualname__ From e2cc06195197502cdc14d3addc9bcec86c00379e Mon Sep 17 00:00:00 2001 From: Octi Zhang Date: Mon, 5 Oct 2026 14:41:35 -0700 Subject: [PATCH 08/11] Consolidate task-space offset regression coverage --- .../test/envs/test_newton_task_space_actions.py | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/source/isaaclab_newton/test/envs/test_newton_task_space_actions.py b/source/isaaclab_newton/test/envs/test_newton_task_space_actions.py index 2c0c7035a82c..41440c43d476 100644 --- a/source/isaaclab_newton/test/envs/test_newton_task_space_actions.py +++ b/source/isaaclab_newton/test/envs/test_newton_task_space_actions.py @@ -27,7 +27,7 @@ def test_body_offset_jacobian_uses_offset_in_root_frame(): - """The offset uses root axes, and a rigid offset rotation leaves angular rows unchanged.""" + """The offset uses root axes, keeps angular rows unchanged, and shares the point-shift math.""" # a tilted root, so that a body-frame or world-frame lever arm gives a different result root_quat_w = torch.tensor([[_S, 0.0, 0.0, _S]]) # One revolute joint about the root z axis through the body origin; the body is yawed 90 degrees from the root. @@ -56,9 +56,7 @@ def test_body_offset_jacobian_uses_offset_in_root_frame(): expected = torch.tensor([[[-1.0], [0.0], [0.0], [0.0], [0.0], [1.0]]]) torch.testing.assert_close(jacobian_b, expected, atol=1e-6, rtol=0.0) - -def test_task_space_actions_do_not_own_point_shift_math(): - """All task-space actions must use the shared point shift instead of recopying its formula.""" + # Keep the kinematic operation in math instead of recopying it into an action term. methods = ( DifferentialInverseKinematicsAction._compute_frame_jacobian, OperationalSpaceControllerAction._compute_ee_jacobian, From f2117cff93925e8df7770bce09c77101fa63d5f7 Mon Sep 17 00:00:00 2001 From: Octi Zhang Date: Mon, 5 Oct 2026 15:02:56 -0700 Subject: [PATCH 09/11] Use cross products for task-space offset Jacobians --- ...share-task-space-jacobian-offset.minor.rst | 4 ---- .../speed-up-task-space-offset-jacobian.rst | 4 ++++ .../envs/mdp/actions/task_space_actions.py | 8 ++----- source/isaaclab/isaaclab/utils/math.py | 22 ------------------ source/isaaclab/test/utils/test_math.py | 8 ------- .../mdp/actions/newton_task_space_actions.py | 6 ++--- .../envs/test_newton_task_space_actions.py | 23 +++---------------- 7 files changed, 12 insertions(+), 63 deletions(-) delete mode 100644 source/isaaclab/changelog.d/share-task-space-jacobian-offset.minor.rst create mode 100644 source/isaaclab/changelog.d/speed-up-task-space-offset-jacobian.rst diff --git a/source/isaaclab/changelog.d/share-task-space-jacobian-offset.minor.rst b/source/isaaclab/changelog.d/share-task-space-jacobian-offset.minor.rst deleted file mode 100644 index 15555fa6d268..000000000000 --- a/source/isaaclab/changelog.d/share-task-space-jacobian-offset.minor.rst +++ /dev/null @@ -1,4 +0,0 @@ -Added -^^^^^ - -* Added :func:`~isaaclab.utils.math.velocity_at_point` to share point-shift math across task-space Jacobians. diff --git a/source/isaaclab/changelog.d/speed-up-task-space-offset-jacobian.rst b/source/isaaclab/changelog.d/speed-up-task-space-offset-jacobian.rst new file mode 100644 index 000000000000..d4ffde050722 --- /dev/null +++ b/source/isaaclab/changelog.d/speed-up-task-space-offset-jacobian.rst @@ -0,0 +1,4 @@ +Changed +^^^^^^^ + +* Reduced task-space body-offset Jacobian overhead by using batched cross products instead of skew matrices. 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 cb89486d5c86..ab7b53a109ea 100644 --- a/source/isaaclab/isaaclab/envs/mdp/actions/task_space_actions.py +++ b/source/isaaclab/isaaclab/envs/mdp/actions/task_space_actions.py @@ -261,9 +261,7 @@ def _compute_frame_jacobian(self): self._asset.data.body_quat_w.torch[:, self._body_idx], ) offset_pos_b = math_utils.quat_apply(body_quat_b, self._offset_pos) - self._jacobian_b[:, :3] = math_utils.velocity_at_point( - self._jacobian_b[:, :3], self._jacobian_b[:, 3:], offset_pos_b[:, :, None], dim=1 - ) + self._jacobian_b[:, :3] += torch.linalg.cross(self._jacobian_b[:, 3:], offset_pos_b[:, :, None], dim=1) return self._jacobian_b @@ -676,9 +674,7 @@ def _compute_ee_jacobian(self): self._asset.data.body_quat_w.torch[:, self._ee_body_idx], ) offset_pos_b = math_utils.quat_apply(body_quat_b, self._offset_pos) - self._jacobian_b[:, :3] = math_utils.velocity_at_point( - self._jacobian_b[:, :3], self._jacobian_b[:, 3:], offset_pos_b[:, :, None], dim=1 - ) + self._jacobian_b[:, :3] += torch.linalg.cross(self._jacobian_b[:, 3:], offset_pos_b[:, :, None], dim=1) def _compute_ee_pose(self): """Computes the pose of the ee frame in root frame.""" diff --git a/source/isaaclab/isaaclab/utils/math.py b/source/isaaclab/isaaclab/utils/math.py index 9055aa16cf28..eff9e22d576d 100644 --- a/source/isaaclab/isaaclab/utils/math.py +++ b/source/isaaclab/isaaclab/utils/math.py @@ -756,28 +756,6 @@ def skew_symmetric_matrix(vec: torch.Tensor) -> torch.Tensor: return skew_sym_mat -@torch.jit.script -def velocity_at_point( - linear_velocity: torch.Tensor, angular_velocity: torch.Tensor, point_offset: torch.Tensor, dim: int = -1 -) -> torch.Tensor: - """Return linear velocity at a displaced point: ``v_point = v_origin + ω × r``. - - All inputs use the same reference frame. ``dim`` selects the size-3 vector axis; all other dimensions - follow PyTorch broadcasting. This also shifts each column of a batched Jacobian independently. - Angular velocity does not change under a point shift. - - Args: - linear_velocity: Linear velocity at the original point. - angular_velocity: Angular velocity about the original point. - point_offset: Displacement from the original point, expressed in the same frame. - dim: Vector axis. Defaults to the last axis. - - Returns: - Linear velocity at the displaced point. - """ - return linear_velocity + torch.linalg.cross(angular_velocity, point_offset, dim=dim) - - """ Transformations """ diff --git a/source/isaaclab/test/utils/test_math.py b/source/isaaclab/test/utils/test_math.py index 19269e3d915a..14efba41a888 100644 --- a/source/isaaclab/test/utils/test_math.py +++ b/source/isaaclab/test/utils/test_math.py @@ -488,14 +488,6 @@ def test_skew_symmetric_matrix(device, shape): torch.testing.assert_close(vec_rand_resized[:, 0], mat_value[:, 2, 1]) -def test_velocity_at_point(): - """A point displaced perpendicular to an angular velocity gains the expected linear velocity.""" - linear = torch.tensor([[1.0, 2.0, 3.0]]) - angular = torch.tensor([[0.0, 0.0, 2.0]]) - offset = torch.tensor([[0.0, 3.0, 0.0]]) - torch.testing.assert_close(math_utils.velocity_at_point(linear, angular, offset), torch.tensor([[-5.0, 2.0, 3.0]])) - - @pytest.mark.parametrize("device", test_devices()) def test_orthogonalize_perspective_depth(device): """Test for converting perspective depth to orthogonal depth.""" diff --git a/source/isaaclab_newton/isaaclab_newton/envs/mdp/actions/newton_task_space_actions.py b/source/isaaclab_newton/isaaclab_newton/envs/mdp/actions/newton_task_space_actions.py index 062daeaced23..25c24b6c16fd 100644 --- a/source/isaaclab_newton/isaaclab_newton/envs/mdp/actions/newton_task_space_actions.py +++ b/source/isaaclab_newton/isaaclab_newton/envs/mdp/actions/newton_task_space_actions.py @@ -103,13 +103,13 @@ def _compute_ee_jacobian(self) -> torch.Tensor: """Compute the target-frame Jacobian in the root frame, shape (num_envs, 6, num_joints).""" data = self._asset.data jacobian_w = data.body_link_jacobian_w.torch[:, self._jacobi_body_idx, :, self._jacobi_joint_ids] - jac_lin_w = jacobian_w[:, :3] + jac_lin_w, jac_ang_w = jacobian_w[:, :3], jacobian_w[:, 3:] if self._offset_pos is not None: offset_w = math_utils.quat_apply(data.body_quat_w.torch[:, self._body_idx], self._offset_pos) - jac_lin_w = math_utils.velocity_at_point(jac_lin_w, jacobian_w[:, 3:], offset_w[:, :, None], dim=1) + jac_lin_w = jac_lin_w + torch.linalg.cross(jac_ang_w, offset_w[:, :, None], dim=1) world_to_root = math_utils.matrix_from_quat(math_utils.quat_inv(data.root_quat_w.torch)) self._jacobian_b[:, :3] = torch.bmm(world_to_root, jac_lin_w) - self._jacobian_b[:, 3:] = torch.bmm(world_to_root, jacobian_w[:, 3:]) + self._jacobian_b[:, 3:] = torch.bmm(world_to_root, jac_ang_w) return self._jacobian_b diff --git a/source/isaaclab_newton/test/envs/test_newton_task_space_actions.py b/source/isaaclab_newton/test/envs/test_newton_task_space_actions.py index 41440c43d476..1148554bb3e5 100644 --- a/source/isaaclab_newton/test/envs/test_newton_task_space_actions.py +++ b/source/isaaclab_newton/test/envs/test_newton_task_space_actions.py @@ -5,20 +5,13 @@ """Kinematics of the target frame shared by the Newton task-space action terms.""" -import ast -import inspect import math -import textwrap from types import SimpleNamespace import pytest import torch from isaaclab_newton.envs.mdp.actions.newton_task_space_actions import _NewtonTaskSpaceAction -from isaaclab.envs.mdp.actions.task_space_actions import ( - DifferentialInverseKinematicsAction, - OperationalSpaceControllerAction, -) from isaaclab.utils import math as math_utils pytestmark = pytest.mark.unit @@ -27,7 +20,7 @@ def test_body_offset_jacobian_uses_offset_in_root_frame(): - """The offset uses root axes, keeps angular rows unchanged, and shares the point-shift math.""" + """The offset uses root axes, and a rigid offset rotation leaves angular rows unchanged.""" # a tilted root, so that a body-frame or world-frame lever arm gives a different result root_quat_w = torch.tensor([[_S, 0.0, 0.0, _S]]) # One revolute joint about the root z axis through the body origin; the body is yawed 90 degrees from the root. @@ -56,15 +49,5 @@ def test_body_offset_jacobian_uses_offset_in_root_frame(): expected = torch.tensor([[[-1.0], [0.0], [0.0], [0.0], [0.0], [1.0]]]) torch.testing.assert_close(jacobian_b, expected, atol=1e-6, rtol=0.0) - # Keep the kinematic operation in math instead of recopying it into an action term. - methods = ( - DifferentialInverseKinematicsAction._compute_frame_jacobian, - OperationalSpaceControllerAction._compute_ee_jacobian, - _NewtonTaskSpaceAction._compute_ee_jacobian, - ) - for method in methods: - tree = ast.parse(textwrap.dedent(inspect.getsource(method))) - calls = [node.func for node in ast.walk(tree) if isinstance(node, ast.Call)] - names = [call.attr for call in calls if isinstance(call, ast.Attribute)] - assert names.count("velocity_at_point") == 1, method.__qualname__ - assert not {"skew_symmetric_matrix", "cross"}.intersection(names), method.__qualname__ + # The point-shift identity is one tensor operation, not another Isaac Lab math API. + assert not hasattr(math_utils, "velocity_at_point") From 8f0bf00ec1f338aeb68c3c95d5add0a411dcbbc7 Mon Sep 17 00:00:00 2001 From: Octi Zhang Date: Mon, 5 Oct 2026 15:06:39 -0700 Subject: [PATCH 10/11] Fold Newton offset regression into controller test --- .../controllers/test_newton_controllers.py | 38 +++++++++++-- .../envs/test_newton_task_space_actions.py | 53 ------------------- 2 files changed, 35 insertions(+), 56 deletions(-) delete mode 100644 source/isaaclab_newton/test/envs/test_newton_task_space_actions.py diff --git a/source/isaaclab_newton/test/controllers/test_newton_controllers.py b/source/isaaclab_newton/test/controllers/test_newton_controllers.py index 8eb27ea9dc11..61b3fa32af33 100644 --- a/source/isaaclab_newton/test/controllers/test_newton_controllers.py +++ b/source/isaaclab_newton/test/controllers/test_newton_controllers.py @@ -3,10 +3,13 @@ # # SPDX-License-Identifier: BSD-3-Clause -"""Tests for the Newton model-free controller wrappers against independent control-law references.""" +"""Newton model-free controllers and the task-space Jacobian supplied to them.""" from __future__ import annotations +import math +from types import SimpleNamespace + import pytest import torch from isaaclab_newton.controllers import ( @@ -17,6 +20,9 @@ NewtonOperationalSpaceController, NewtonOperationalSpaceControllerCfg, ) +from isaaclab_newton.envs.mdp.actions.newton_task_space_actions import _NewtonTaskSpaceAction + +from isaaclab.utils import math as math_utils _NUM_ENVS = 3 _NUM_JOINTS = 7 @@ -83,12 +89,38 @@ def test_disabled_input_is_rejected(): def test_differential_ik_dls_matches_reference(): - """Position-only DLS follows ``q + dt * bandwidth * J^T (J J^T + lambda^2 I)^-1 e``.""" + """An offset-frame Jacobian reaches position-only DLS with the correct frame and control law.""" torch.manual_seed(0) bandwidth, damping, dt = 20.0, 0.1, 0.05 cfg = NewtonDifferentialIKControllerCfg(bandwidth=bandwidth, damping=damping, axis_weight=(1, 1, 1, 0, 0, 0)) controller = NewtonDifferentialIKController(cfg, _NUM_ENVS, _NUM_JOINTS, _DEVICE) - jacobian, q = _rand(_NUM_ENVS, 6, _NUM_JOINTS), _rand(_NUM_ENVS, _NUM_JOINTS) + s = math.sqrt(0.5) + root_quat_w = torch.tensor([[s, 0.0, 0.0, s]]).repeat(_NUM_ENVS, 1) + body_quat_w = math_utils.quat_mul(root_quat_w, torch.tensor([[0.0, 0.0, s, s]]).repeat(_NUM_ENVS, 1)) + jacobian_w = _rand(_NUM_ENVS, 1, 6, _NUM_JOINTS) + jacobian_w[:, 0, :3, 0] = 0.0 + jacobian_w[:, 0, 3:, 0] = math_utils.quat_apply(root_quat_w, torch.tensor([[0.0, 0.0, 1.0]])) + data = SimpleNamespace( + body_link_jacobian_w=SimpleNamespace(torch=jacobian_w), + root_quat_w=SimpleNamespace(torch=root_quat_w), + body_quat_w=SimpleNamespace(torch=torch.stack([root_quat_w, body_quat_w], dim=1)), + ) + action = SimpleNamespace( + _asset=SimpleNamespace(data=data), + _body_idx=1, # fixed-root body 1 uses Jacobian row 0 + _jacobi_body_idx=0, + _jacobi_joint_ids=list(range(_NUM_JOINTS)), + _jacobian_b=torch.zeros(_NUM_ENVS, 6, _NUM_JOINTS), + _offset_pos=torch.tensor([[1.0, 0.0, 0.0]]).repeat(_NUM_ENVS, 1), + _offset_rot=torch.tensor([[s, 0.0, 0.0, s]]).repeat(_NUM_ENVS, 1), + ) + jacobian = _NewtonTaskSpaceAction._compute_ee_jacobian(action) + # The body-local x offset is root-local y; a root-z rotation gives linear velocity -x. + expected = torch.tensor([[-1.0, 0.0, 0.0, 0.0, 0.0, 1.0]]).repeat(_NUM_ENVS, 1) + torch.testing.assert_close(jacobian[:, :, 0], expected, atol=1e-6, rtol=0.0) + assert not hasattr(math_utils, "velocity_at_point") + + q = _rand(_NUM_ENVS, _NUM_JOINTS) pos, pos_des = _rand(_NUM_ENVS, 3), _rand(_NUM_ENVS, 3) q_target = controller.compute(_pose(pos), _pose(pos_des), jacobian, q, dt) diff --git a/source/isaaclab_newton/test/envs/test_newton_task_space_actions.py b/source/isaaclab_newton/test/envs/test_newton_task_space_actions.py deleted file mode 100644 index 1148554bb3e5..000000000000 --- a/source/isaaclab_newton/test/envs/test_newton_task_space_actions.py +++ /dev/null @@ -1,53 +0,0 @@ -# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). -# All rights reserved. -# -# SPDX-License-Identifier: BSD-3-Clause - -"""Kinematics of the target frame shared by the Newton task-space action terms.""" - -import math -from types import SimpleNamespace - -import pytest -import torch -from isaaclab_newton.envs.mdp.actions.newton_task_space_actions import _NewtonTaskSpaceAction - -from isaaclab.utils import math as math_utils - -pytestmark = pytest.mark.unit - -_S = math.sin(math.pi / 4.0) - - -def test_body_offset_jacobian_uses_offset_in_root_frame(): - """The offset uses root axes, and a rigid offset rotation leaves angular rows unchanged.""" - # a tilted root, so that a body-frame or world-frame lever arm gives a different result - root_quat_w = torch.tensor([[_S, 0.0, 0.0, _S]]) - # One revolute joint about the root z axis through the body origin; the body is yawed 90 degrees from the root. - body_quat_w = math_utils.quat_mul(root_quat_w, torch.tensor([[0.0, 0.0, _S, _S]])) - jacobian_w = torch.zeros(1, 1, 6, 1) - jacobian_w[0, 0, 3:, 0] = math_utils.quat_apply(root_quat_w, torch.tensor([[0.0, 0.0, 1.0]]))[0] - data = SimpleNamespace( - body_link_jacobian_w=SimpleNamespace(torch=jacobian_w), - root_quat_w=SimpleNamespace(torch=root_quat_w), - body_quat_w=SimpleNamespace(torch=torch.stack([root_quat_w, body_quat_w], dim=1)), # [root, hand] - ) - term = SimpleNamespace( - _asset=SimpleNamespace(data=data), - _body_idx=1, # a fixed base has no Jacobian row for its root, so the hand stays Jacobian row 0 - _jacobi_body_idx=0, - _jacobi_joint_ids=[0], - _jacobian_b=torch.zeros(1, 6, 1), - _offset_pos=torch.tensor([[1.0, 0.0, 0.0]]), - # a 90 degree rotation about x for the target frame - _offset_rot=torch.tensor([[_S, 0.0, 0.0, _S]]), - ) - - jacobian_b = _NewtonTaskSpaceAction._compute_ee_jacobian(term) - - # The lever arm in root axes is (0, 1, 0), and z x (0, 1, 0) = (-1, 0, 0). - expected = torch.tensor([[[-1.0], [0.0], [0.0], [0.0], [0.0], [1.0]]]) - torch.testing.assert_close(jacobian_b, expected, atol=1e-6, rtol=0.0) - - # The point-shift identity is one tensor operation, not another Isaac Lab math API. - assert not hasattr(math_utils, "velocity_at_point") From 1e758c8b0072014f031aecb71dda679f2f34d6c4 Mon Sep 17 00:00:00 2001 From: Octi Zhang Date: Mon, 5 Oct 2026 15:08:20 -0700 Subject: [PATCH 11/11] Adapt offset fix changelogs to towncrier --- ...ian.rst => speed-up-task-space-offset-jacobian.changed.rst} | 3 --- ...st => fix-newton-task-space-body-offset-jacobian.fixed.rst} | 3 --- 2 files changed, 6 deletions(-) rename source/isaaclab/changelog.d/{speed-up-task-space-offset-jacobian.rst => speed-up-task-space-offset-jacobian.changed.rst} (86%) rename source/isaaclab_newton/changelog.d/{fix-newton-task-space-body-offset-jacobian.rst => fix-newton-task-space-body-offset-jacobian.fixed.rst} (96%) diff --git a/source/isaaclab/changelog.d/speed-up-task-space-offset-jacobian.rst b/source/isaaclab/changelog.d/speed-up-task-space-offset-jacobian.changed.rst similarity index 86% rename from source/isaaclab/changelog.d/speed-up-task-space-offset-jacobian.rst rename to source/isaaclab/changelog.d/speed-up-task-space-offset-jacobian.changed.rst index d4ffde050722..a18330ce10a0 100644 --- a/source/isaaclab/changelog.d/speed-up-task-space-offset-jacobian.rst +++ b/source/isaaclab/changelog.d/speed-up-task-space-offset-jacobian.changed.rst @@ -1,4 +1 @@ -Changed -^^^^^^^ - * Reduced task-space body-offset Jacobian overhead by using batched cross products instead of skew matrices. diff --git a/source/isaaclab_newton/changelog.d/fix-newton-task-space-body-offset-jacobian.rst b/source/isaaclab_newton/changelog.d/fix-newton-task-space-body-offset-jacobian.fixed.rst similarity index 96% rename from source/isaaclab_newton/changelog.d/fix-newton-task-space-body-offset-jacobian.rst rename to source/isaaclab_newton/changelog.d/fix-newton-task-space-body-offset-jacobian.fixed.rst index d1304fdcedb2..f3ec05a5269b 100644 --- a/source/isaaclab_newton/changelog.d/fix-newton-task-space-body-offset-jacobian.rst +++ b/source/isaaclab_newton/changelog.d/fix-newton-task-space-body-offset-jacobian.fixed.rst @@ -1,6 +1,3 @@ -Fixed -^^^^^ - * Fixed the body-offset Jacobian of :class:`~isaaclab_newton.envs.mdp.actions.NewtonDifferentialInverseKinematicsAction` and :class:`~isaaclab_newton.envs.mdp.actions.NewtonOperationalSpaceControllerAction`. The translational rows are now