Support TSR chains for multi-link pose constraints

[siddhss5]

Motivation

The tsr library has a TSRChain class that couples multiple TSRs across different links of the kinematic chain. pycbirrt currently only accepts list[TSR], all of which constrain the end-effector. There is no reason the planner shouldn't support the full CBiRRT paper formulation.

Canonical use cases:

• Constrained transport: end-effector must hold a mug upright (EE TSR) while avoiding a shelf region (elbow TSR)
• Door opening: gripper at door handle TSR, elbow at hinge-clearance TSR, both enforced simultaneously
• Bimanual: two end-effectors each with their own TSR, projected together

What needs to change

1. RobotModel interface

Add FK for arbitrary links, not just the end-effector:

def fk_link(self, q: np.ndarray, link_name: str) -> np.ndarray: ...

2. IKSolver interface

Add support for solving IK under multiple simultaneous TSR constraints (one per link). The current solve / solve_valid only targets the EE.

3. _project_to_constraint in planner.py

Currently projects onto the worst-violating single TSR. For chains:

• Compute FK for each constrained link
• Accumulate violations across all TSRs in the chain
• Project using a Jacobian that maps all constrained links jointly

4. Planning API

Accept TSRChain (or list[TSRChain]) alongside list[TSR]:

planner.plan(
    goal_tsrs: list[TSR | TSRChain] | None = None,
    constraint_tsrs: list[TSR | TSRChain] | None = None,
    ...
)

A bare TSR is a degenerate chain of length 1 (EE only) — backward compatible.

Complexity note

The main difficulty is the IK solver: solving IK under multi-link TSR constraints requires either:

• A constrained IK formulation (e.g., nullspace projection)
• Iterative projection using per-link Jacobians

The MuJoCo backend currently uses a simple EE IK. This will need extension or a fallback to numerical projection.

Acceptance criteria

• RobotModel protocol exposes fk_link(q, link_name) -> np.ndarray
• _project_to_constraint handles TSRChain by projecting all links jointly
• plan() accepts TSRChain in goal_tsrs and constraint_tsrs
• At least one test with a 2-TSR chain (EE + one intermediate link)
• MuJoCo backend updated or documented as "chain projection falls back to numerical"