whole body IK solver

[yhnsu]

Description

This pull request introduces a new whole‑body inverse kinematics solver, WholeBodyIKSolver, and integrates it into the existing simulation framework alongside the existing PinocchioSolver and SRSSolver.

Summary of changes

• Add WholeBodyIKSolverCfg and WholeBodyIKSolver that build a reduced Pinocchio model keeping torso + single arm joints active, use Damped Least Squares (DLS) in task space, and add null‑space secondary objectives (joint regularization, smoothness, leg / torso stability) to resolve redundancy in a physically reasonable way.
• Align configuration and public API with existing solvers by re‑using end_link_name, root_link_name, tcp, joint_names fields from SolverCfg / PinocchioSolverCfg, keeping get_ik(target_xpos, qpos_seed, qvel_seed=None, return_all_solutions=False, **kwargs) and get_fk(qpos, **kwargs) signatures compatible with other solvers, and integrating via Robot.init_solver and Robot.compute_ik/compute_fk using control parts such as "left_arm_body" / "right_arm_body".
• Wire the solver into the W1 robot configuration by extending DexforceW1Cfg._build_default_solver_cfg to define "left_arm_body" / "right_arm_body" solvers using WholeBodyIKSolverCfg, and relying on control parts to supply joint_names (torso + corresponding arm) so configuration stays consistent with other solvers.
• Add examples and documentation: examples/sim/solvers/whole_body_ik_solver.py showing how to call robot.compute_fk / robot.compute_ik with whole‑body IK; scripts/visualize_whole_body_ik.py / scripts/visualize_whole_body_ik_robot.py for interactive visualization; a new conceptual overview in docs/source/overview/sim/solvers/whole_body_ik_solver.md; and a short comparison note docs/source/overview/sim/solvers/whole_body_ik_vs_pinocchio.md explaining how this solver differs from PinocchioSolver.

Motivation and rationale

The existing PinocchioSolver is designed as a single‑chain DLS solver for manipulators where the number of joints is close to the 6‑DOF task space (non‑redundant or only lightly redundant). Its core update is (\Delta q = J^\dagger e), where (J \in \mathbb{R}^{6 \times n}) is the Jacobian and (e \in \mathbb{R}^6) is the pose error. This works well for arms, but is not sufficient for whole‑body IK on W1:

• a single arm plus torso uses 11 joints (ANKLE, KNEE, BUTTOCK, WAIST, LEFT_J1..LEFT_J7), controlling a 6‑D end‑effector pose;
• the Jacobian is therefore (6 \times 11), with a 5‑dimensional null space;
• if we only use (\Delta q = J^\dagger e), the extra 5 DOFs are effectively unconstrained and can drift, leading to unnatural torso motion and unstable poses.

WholeBodyIKSolver addresses this by building a reduced whole‑body model (torso + arm) via buildReducedRobot, computing a primary DLS step in task space, and adding a null‑space secondary step:

[
\Delta q = J^\dagger e - (I - J^\dagger J),\nabla E_{\text{sec}},
]

where E_sec encodes joint regularization (keep joints near 0 or another neutral posture), smoothness (stay close to the seed configuration), and leg / torso stability (via LegCostCfg over linear combinations of joints).

This design keeps the external API aligned with PinocchioSolver, but separates the implementation so that existing users of PinocchioSolver are not affected by whole‑body specific behavior or parameters, and whole‑body IK users have clear, explicit controls for redundancy resolution (standing posture, smoothness, etc.).

Why a new class instead of extending PinocchioSolver?

Conceptually, the null‑space and leg‑stability logic could be implemented inside PinocchioSolver, but this would significantly blur its responsibilities: it would have to support both “simple arm DLS” and “whole‑body redundant IK” modes in a single class, with many conditional code paths; configuration semantics would diverge (some parameters only meaningful for whole‑body IK), making the solver harder to understand and tune; enabling new behavior by default risks changing the behaviour of existing arms‑only applications.

By introducing WholeBodyIKSolver as a separate class that reuses the same configuration surface (end_link_name, root_link_name, tcp, joint_names) and plugs into Robot in the same way as other solvers, we keep the public API unified while allowing the internal implementation to specialize for redundant whole‑body control.