[NeoZng]

Description

A task-space action with body_offset needs a Jacobian at the offset point. The core DiffIK and OSC actions received the corrected frame convention in #7989, but the Newton model-free action retained its own formula: it used a body-local lever arm against a root-frame Jacobian and rotated the angular rows by the offset orientation. With a rotated body or root, J @ qdot therefore disagreed with the offset-frame twist.

This PR fixes the Newton action by rotating the offset into world axes, applying Jv_point = Jv_origin + Jw × r, and then rotating the Jacobian into root axes. The angular rows stay unchanged. Core DiffIK and OSC now use the same direct torch.linalg.cross operation in root axes instead of constructing a skew matrix and multiplying it by the angular Jacobian. The two forms are mathematically equivalent; the direct cross product avoids an extra matrix allocation and multiplication. No new math wrapper is needed.

The Newton task-space actions use Newton's model-free controllers, which require a caller-supplied tool-point pose and Jacobian. Newton's separate model-based controllers and IK objectives support tool-site/link offsets, but are not the controller API used here. The Newton action continues to accept both Newton and PhysX asset data through the public link-origin Jacobian contract.

On the official Franka validation from the original fix (rotated fixed root and a hand offset), J @ qdot matched the simulator's offset-frame twist: Newton linear error fell from about 0.27 m/s to 2e-6 m/s and angular error from about 0.95 rad/s to 1e-7 rad/s. PhysX errors were 1e-4 to 4e-4, matching the core term's floor. Only configurations with body_offset are affected; no shipped task currently uses these Newton action terms.

Fixes #8202

Type of change

• Bug fix and task-space offset performance improvement

Release backport

• Backport this pull request to the active release branch after it merges into develop

Validation

• uv run --frozen --extra test python -m pytest source/isaaclab/test/envs/test_diffik_jacobian_aliasing.py -q — 5 passed.
• uv run --frozen --extra test python -m pytest source/isaaclab_newton/test/controllers/test_newton_controllers.py -q — 8 passed; the existing DiffIK case now checks the offset Jacobian.
• uv run --frozen isaaclab -f — passed with the local changelog base set to this PR's upstream base commit.
• 4,096-env, 7-joint Torch GPU microbenchmarks on RTX 5090 (median of 8 paired rounds of 100 calls): Newton world-frame point shift, direct cross 149.6 us/call versus skew matrix 213.5 us/call; core root-frame point shift, direct cross 107.0 us/call versus skew matrix 181.0 us/call. These measure Jacobian computation, not a full simulation step.

Checklist

• I have read and understood the contribution guidelines.
• I have run the pre-commit checks.
• I have added changelog fragments for both touched packages.
• The focused regression tests pass.
• I have made corresponding changes to the documentation.

[greptile-apps]

Confidence Score: 5/5

[Medium risk] Fixes Jacobian calculation for task-space robot control.

The PR appears safe to merge; no actionable issue was identified in the changed code.

Summary

The PR corrects the body-offset Jacobian shared by Newton differential IK and operational-space action terms.

• Rotates the offset into root axes before shifting the linear rows, while leaving angular rows in root axes.
• Adds a focused test for identity, yawed, and tilted root orientations and a changelog fragment.

Diagram

%%{init: {'theme': 'neutral'}}%%
flowchart LR
  A[Body-frame offset] --> B[Rotate by body orientation relative to root]
  B --> C[Root-frame lever arm]
  C --> D[Shift linear Jacobian rows]
  E[Angular Jacobian rows in root axes] --> D
  D --> F[Target-frame Jacobian in root axes]

Loading

_{Reviews (1) · Last reviewed commit: "Fix body-offset Jacobian in Newton task-..."}

[isaaclab-review-bot]

Isaac Lab Review Bot

The body-offset Jacobian correction aligns Newton task-space actions with the root-frame pose convention. One minor hot-path issue remains: the root inverse quaternion is computed twice in each offset-path call.

• Design and architecture: The change preserves the existing action/controller boundary and keeps the Jacobian in root axes, consistent with the pose consumed by both Newton differential IK and operational-space action terms.
• API: No public signatures, configuration fields, registry identifiers, return shapes, dtypes, or device behavior change. _compute_ee_jacobian continues returning the preallocated (num_envs, 6, num_joints) buffer, and the no-offset path is unchanged.
• Compatibility and deprecation: Breaking changes: none identified. Configurations using body_offset receive corrected Jacobian values; the prior body-frame lever arm and angular-row rotation were inconsistent with the class's root-frame target pose. The required isaaclab_newton changelog fragment documents the correction.
• Implementation: The offset is correctly transformed from body axes into root axes before applying the lever-arm contribution, and removing the offset-rotation transform from the angular rows is consistent with rigid-frame velocity. However, quat_inv(data.root_quat_w.torch) is evaluated once for root_rot and again for body_quat_b on the per-step offset path; compute it once and reuse it.
• Style consistency: Naming, comments, and use of math_utils match the surrounding implementation. Reusing a named root inverse quaternion would also make the shared transform input clearer while removing the duplicated operation.
• Test quality: The added unit test provides an independent analytic lever-arm expectation and covers identity, yawed, and tilted root orientations plus a rotated offset. It targets the regression at the owning Newton action boundary without duplicating an existing Newton test in the supplied inventory.

Minor fixes needed. Posted 1 actionable finding inline.

Automated review; human maintainers own approval decisions.

[hujc7]

[P2] Agent suggestion: an equivalent, simpler form shifts to the offset point in world axes with body_quat_w, then rotates to root once. It drops the root-relative quat_mul and the in-place edit of the rotated Jacobian.

Suggested change

	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:
	# 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(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(self._offset_pos), self._jacobian_b[:, 3:]
	-math_utils.skew_symmetric_matrix(offset_pos_b), self._jacobian_b[:, 3:]
	)
	jac_lin_w, jac_ang_w = jacobian_w[:, :3], jacobian_w[:, 3:]
	if self._offset_pos is not None:
	# v_offset = v_body + w x r (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)

It gives the same Jacobian (within 6e-8 on a live Newton env) and your test passes. env.step on Isaac-Reach-Franka (Newton, 4096 envs) drops from 11.19 to 10.85 ms. Non-blocking.

[NeoZng]

Adopted in 9a3405d: the lever arm is applied in world axes with body_quat_w, then the Jacobian is rotated to the root once. Same result on the test and on random inputs; the offset path drops from about 265 to 160 us per call at 4096 envs in a standalone benchmark.

[hujc7]

[P2] Agent suggestion: on a fixed base, _body_idx and _jacobi_body_idx differ by one (newton_task_space_actions.py:56), but this stub sets both to 0. As a result, reading body_quat_w with the Jacobian index still passes. A two-body stub catches it:

-        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]
 ...
-        _body_idx=0,
+        _body_idx=1,  # a fixed base has no Jacobian row for its root, so the hand stays Jacobian row 0

[NeoZng]

Adopted in 9a9836d: the stub now holds [root, hand] with _body_idx=1 and _jacobi_body_idx=0, and reading the orientation with the Jacobian row index now fails the test.

[NeoZng]

@hujc7 thanks, both suggestions are in:

• 9a3405d shifts to the offset point in world axes and rotates to the root once.
• 9a9836d keeps the hand (body 1) apart from its Jacobian row (0) in the test stub, so reading the orientation with the Jacobian row index now fails.

The description now gives the new cost: at 4096 envs the offset path takes about 160 us per call, down from 255 us on develop.

[hujc7]

run-ci

[NeoZng]

@hujc7 the failing standalone demos (headless, Kit) job is the ARL demo aborting with Destroying busy TaskGroup! at Kit shutdown, which #8259 addresses. The demo finished its steps, and this PR does not touch that path. Could you re-run that job? I cannot re-run it from the fork.

[hujc7]

run-ci