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 strengthens Pink inverse kinematics with batched seeds, adaptive step acceptance, synchronized limits, corrected target transforms, and null-space posture control.

• Adds robust convergence, damping, backtracking, and failure reporting.
• Intersects URDF, configured, and runtime joint limits in Pink joint order.
• Reworks posture errors and projectors in Pinocchio tangent space.
• Updates solver documentation, examples, and focused tests.

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	Adds batched IK, transformed TCP targets, adaptive convergence logic, joint-order conversion, and order-independent synchronization of configured and runtime limits.
embodichain/lab/sim/solvers/null_space_posture_task.py	Moves posture errors and masking into tangent space, validates task inputs, and applies joint selection to both rows and columns of the null-space projector.
tests/sim/solvers/test_pink_solver.py	Adds focused coverage for batching, convergence behavior, transformed targets, null-space control, and joint-limit synchronization in both setter orders.
docs/source/overview/sim/solvers/pink_solver.md	Documents the updated solver construction, input shapes, return contract, TCP semantics, and null-space posture usage.

Flowchart

%%{init: {'theme': 'neutral'}}%%
flowchart TD
    A[Normalize target batch and seeds] --> B[Convert seed to Pink order]
    B --> C[Apply root and TCP target transforms]
    C --> D[Evaluate frame and posture tasks]
    D --> E[Solve damped Pink QP]
    E --> F{Step improves lexicographic merit?}
    F -- Yes --> G[Accept step and reduce damping]
    F -- No --> H[Backtrack and increase damping]
    H --> E
    G --> I{Converged?}
    I -- No --> D
    I -- Yes --> J[Return success and simulator-ordered solution]
    H --> K{Retries exhausted or stagnant?}
    K -- Yes --> L[Return failure and preserve seed]
    K -- No --> E

Loading

_{Reviews (8): Last reviewed commit: "applies the velocity-selection mask" | 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)}"
            )

[Copilot]

Pull request overview

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

Suppressed comments (1)

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

• As above, the null-space projector should also be masked on the columns to prevent null-space posture objectives from producing gradients in non-controlled velocity coordinates (e.g., floating-base DoFs).

        # Compute null space projector: N = I - J^+ * J
        projector = (
            np.eye(J_combined.shape[1]) - np.linalg.pinv(J_combined) @ J_combined
        )
        return self._velocity_mask[:, None] * projector

[Copilot]

Pull request overview

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

Suppressed comments (2)

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

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

• available_joints currently includes joints with nv == 0 (fixed joints) because it only checks idx_v >= root_nv. This can make fixed joints appear selectable even though they will never contribute to the posture mask.

        available_joints = {
            model.names[joint_id]
            for joint_id in range(1, model.njoints)
            if model.joints[joint_id].idx_v >= root_nv
        }

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

• get_ik() and get_fk() are defined in terms of end_link_name/TCP, but variable_input_tasks can currently provide a FrameTask targeting a different frame. That makes TCP removal and FK inconsistent with the controlled frame target.

        self._target_task = self._frame_tasks[0]
        self._frame_task_ids = {id(self._target_task)}