Repository navigation
Include selected link offsets in world-frame Jacobians - #29
Merged
neka-nat merged 1 commit intoOct 17, 2026
Merged
Conversation
Seed both reverse Jacobian recursions with the selected link offset followed by the tool transform. Verify the MJCF fixed-tip derivative and independent world screw columns for mixed joints and intermediate links.
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
Forward kinematics reports the selected link offset followed by the optional tool transform, but both Jacobian recursions started at the tool alone. This gives incorrect linear and angular columns when the link offset is nonidentity.
For example, an MJCF hinge about z with a fixed tip at (1, 0, 0) has tip position (cos(theta), sin(theta), 0). At theta=0 the current linear Jacobian is zero instead of (0, 1, 0).
Initialize the reverse transform with
selected_link.offset.matrix() @ tool.matrix()in both Jacobian paths. Intermediate link offsets describe their reported poses rather than child-joint transforms, so only the selected link offset enters this seed. World-expressed angular axes and zero columns for downstream joints are preserved.Six regression methods cover the real MJCF fixed tip, mixed revolute/prismatic chains, each selected link, world angular velocity, equivalent link/tool composition, and identity-offset controls. Expected poses and screw columns are computed independently with SciPy SO(3), including nonidentity root, joint, link and tool transforms.
Validation on Windows CPython 3.12.13 with NumPy 1.26.4 and SciPy 1.15.3: all 15 existing tests pass; before the fix, five new methods fail assertions and the identity control passes, with no errors or skips; after the fix, all 21 tests pass. The original tests and subtest events are unchanged. The project-declared Flake8 check and new-test lint pass before and after. The full declared Mypy command exits 1 in both versions with the same 16 diagnostics in unchanged MJCF/URDF parsers; complete stdout/stderr match, and neither changed file introduces a type error. Both default isolated sdist-to-wheel builds pass with complete package payload verification. The upstream Linux CI matrix remains for CI.