Enhance Pink IK Solver Robustness and Null-Space Posture Control

[chase6305]

Description

This PR improves the Pink IK solver and null-space posture task for more robust and consistent robot control.

Key changes include:

• Add batched IK with per-target seeds and consistent success reporting.
• Add adaptive damping, backtracking, stagnation detection, and strict convergence checks.
• Correct TCP and root-relative target transformations.
• Synchronize URDF, user-configured, and runtime robot joint limits.
• Fix null-space posture masking, tangent-space dimensions, joint ordering, and model caching.
• Include null-space posture objectives in the Pink QP and step acceptance flow.
• Improve handling of unconstrained task axes and invalid inputs.
• Add focused tests and update solver documentation.

No new runtime dependencies are introduced.

Type of change

• Bug fix (non-breaking change which fixes an issue)
• Enhancement (non-breaking change which improves an existing functionality)
• Documentation update

Screenshots

pink_solver-2026-08-26_09.45.29.mp4

Checklist

• I have run the black . command to format the code base.
• I have made corresponding changes to the documentation.
• Public API changes are reflected in the API docs.
• I have added tests that prove my fix is effective.
• Dependencies have been updated, if applicable.

[greptile-apps]

Greptile Summary

The PR substantially strengthens Pink-based inverse kinematics and null-space posture control.

• Adds batched targets with per-target seeds and consistent success/solution shapes.
• Adds adaptive damping, backtracking, strict convergence, and stagnation handling.
• Corrects TCP/root-relative transforms, joint ordering, and synchronized effective limits.
• Moves posture calculations into tangent space and includes posture objectives in step acceptance.
• Expands focused solver tests and documentation.

Confidence Score: 5/5

The PR appears safe to merge.

No blocking failure remains.

Important Files Changed

Filename	Overview
embodichain/lab/sim/solvers/pink_solver.py	Reworks Pink IK around batched inputs, adaptive step acceptance, corrected transforms, explicit ordering, and synchronized configured/runtime/URDF limits.
embodichain/lab/sim/solvers/null_space_posture_task.py	Corrects posture masking and Jacobians to operate in Pinocchio tangent space while validating mapped joints, frames, and targets.
tests/sim/solvers/test_pink_solver.py	Adds focused coverage for transforms, batching, convergence, posture merit, limit synchronization, and invalid inputs.
docs/source/overview/sim/solvers/pink_solver.md	Updates Pink solver configuration, posture-task usage, input shapes, and result contracts.

Flowchart

%%{init: {'theme': 'neutral'}}%%
flowchart TD
A[Normalize batched targets and seeds] --> B[Convert seed to Pink joint order]
B --> C[Apply effective joint limits]
C --> D[Set root-relative frame target and remove TCP]
D --> E[Evaluate frame and posture merits]
E --> F{Converged?}
F -->|Yes| G[Return successful solution]
F -->|No| H[Solve damped Pink QP]
H --> I[Integrate candidate step]
I --> J{Merit improved?}
J -->|Yes| K[Accept step and reduce damping]
J -->|No| L[Restore state and increase damping]
K --> E
L --> M{Retries or stagnation exhausted?}
M -->|No| H
M -->|Yes| N[Return failure with input seed]

Loading

_{Reviews (6): Last reviewed commit: "fix" | Re-trigger Greptile}

[Copilot]

Pull request overview

This pull request upgrades the Pink-based task-space IK implementation to be more robust and consistent across single-target and batched solves, and integrates a null-space posture objective for improved posture control while satisfying end-effector constraints.

Changes:

• Reworked PinkSolver iteration loop with adaptive damping/backtracking, stagnation detection, stricter convergence checks, and consistent batched (N,) / (N, 1, dof) outputs that preserve per-target seeds on failure.
• Added/updated NullSpacePostureTask to operate in Pinocchio tangent space (nv), with corrected joint masking, selector validation, and model caching.
• Expanded unit tests and refreshed documentation/API docs to reflect the updated solver contract and posture-task usage.

Reviewed changes

Copilot reviewed 6 out of 6 changed files in this pull request and generated 2 comments.

Show a summary per file

File	Description
tests/sim/solvers/test_pink_solver.py	Adds unit-level coverage for batched IK behavior, TCP/root-relative transforms, limit syncing, adaptive control validation, and posture-task integration.
embodichain/lab/sim/solvers/pink_solver.py	Implements the new robust Pink IK solve loop, limit synchronization into the reduced Pinocchio model, TCP/root-relative handling, and posture-task integration.
embodichain/lab/sim/solvers/null_space_posture_task.py	Fixes posture-task math/masking to operate in tangent space, validates selectors, and improves optional-dependency handling.
docs/source/overview/sim/solvers/pink_solver.md	Updates usage examples and documents the new get_ik return contract and posture-task integration.
docs/source/api_reference/public_api.rst	Exposes NullSpacePostureTask and the Pink solver types in the public API reference.

---

💡 Add a code-review agent skill or configure MCP servers for context-aware, tailored reviews. Learn more in the docs.

[yuecideng]

I left four inline comments covering the behavior and validation issues found during the review. The overall direction is good, but the two P1 items should be addressed before merging.

[Copilot]

Pull request overview

Copilot reviewed 7 out of 7 changed files in this pull request and generated no new comments.

Suppressed comments (1)

Previously missed (1) — in code that hasn't changed since the last review.

embodichain/lab/sim/solvers/pink_solver.py:109

• PinkSolverCfg.init_solver relies on **kwargs to accept device, but the abstract SolverCfg.init_solver(self, device, **kwargs) contract (and other solver configs) expose device as an explicit parameter. Making device explicit improves discoverability/type-checking and avoids accidental mistakes where callers forget to pass it as a keyword.

 def init_solver(self, **kwargs: Any) -> PinkSolver:
"""Create a Pink solver and apply the configured TCP.
Args:
**kwargs: Arguments forwarded to :class:`PinkSolver`.

[Copilot]

Pull request overview

Copilot reviewed 7 out of 7 changed files in this pull request and generated 3 comments.

[Copilot]

Pull request overview

Copilot reviewed 7 out of 7 changed files in this pull request and generated 1 comment.

Suppressed comments (1)

Previously missed (1) — in code that hasn't changed since the last review.

embodichain/lab/sim/solvers/null_space_posture_task.py:102

• The posture task objective in the docstring does not match the implemented residual. compute_jacobian() returns a row-masked projector (M·N), so the optimization term is based on M·N(q)·v + M·(q ⊖ q*), not N(q)·v + M·(q ⊖ q*). This mismatch can mislead users about what is being penalized.

 \left\| \mathbf{N}(\mathbf{q}) \mathbf{v} + \mathbf{M} \cdot (\mathbf{q} \ominus \mathbf{q}^*) \right\|_{W_{\text{posture}}}^2

[Copilot]

Pull request overview

Copilot reviewed 7 out of 7 changed files in this pull request and generated 3 comments.

[Copilot]

Pull request overview

Copilot reviewed 7 out of 7 changed files in this pull request and generated no new comments.

Suppressed comments (1)

Previously missed (1) — in code that hasn't changed since the last review.

embodichain/lab/sim/solvers/pink_solver.py:348

• This override raises ValueError for invalid qpos-limit inputs (shape/NaNs/order), whereas BaseSolver.set_qpos_limits returns a bool and logs warnings. Diverging from the base-class contract can surprise callers using BaseSolver polymorphically; consider matching the bool-return semantics (and leaving state unchanged on invalid input) or updating the base interface/callers to consistently handle exceptions.

 raise ValueError(
f"qpos limits must both have shape ({self.dof},), got "
f"{tuple(lower.shape)} and {tuple(upper.shape)}"
)