Share an immutable compiled kinematic traversal across forward kinematics and obstacle queries

[dellaert]

Context

PR #433 adds obstacle-aware planning through RobotQueryPoints, which maps a selected joint configuration to world-frame query points and, when requested, their analytic Jacobians. This is evaluated repeatedly by the obstacle factors, so the implementation precomputes the robot topology/traversal once and replays a compact traversal in the optimizer's hot loop.

GTDynamics already has a general robot-tree traversal in Robot::forwardKinematics(). The simulator's forward-dynamics path calls that routine before solving for accelerations. However, the existing routine has a different interface and purpose: it operates on gtsam::Values, computes poses and twists for the reachable links, rebuilds a breadth-first traversal per call, and does not directly produce the query-point Jacobians required by the obstacle factors.

Consequently, #433 contains a second implementation of robot-tree traversal. Calling forward dynamics from obstacle evaluation would be incorrect and unnecessarily expensive; calling the current general forward-kinematics routine would still leave the Jacobian propagation problem and would likely add overhead in the optimization loop. Nevertheless, maintaining two independent implementations of the topology and joint-propagation rules creates a long-term risk of behavioral drift.

Timing: This issue is intentionally a follow-up to #433. It is relevant only after #433 has been merged, and should not block or expand that PR.

Proposal

Introduce an immutable, precompiled kinematic traversal abstraction that can be constructed once from a Robot and reused by clients that need to propagate state through the robot tree.

The shared layer should own only the common structural information and traversal rules, for example:

• validated root and parent/child relationships;
• deterministic traversal order;
• references or indices for links and joints;
• the mapping needed to propagate a child pose from its parent and joint value;
• optional filtering to a selected set of joints, links, or descendant subtrees.

Clients would remain responsible for choosing which quantities they compute:

• Robot::forwardKinematics() can use the traversal to produce its existing poses and twists in gtsam::Values;
• RobotQueryPoints can use it to compute only the required link poses, query-point positions, and analytic Jacobians;
• dynamics code can continue to use the general forward-kinematics API and its subsequent acceleration solve.

This should be a reusable kinematics primitive, not an attempt to route collision checking through forward dynamics.

Performance requirements

The refactor must preserve the reason RobotQueryPoints has a specialized implementation:

• no topology search or BFS construction during factor evaluation;
• no gtsam::Values assembly in the obstacle-factor hot path;
• no pose/twist/dynamics quantities computed unless requested;
• no additional heap allocation proportional to the robot size per evaluation, where practical;
• analytic query-point Jacobians remain available;
• the compiled traversal is immutable after construction and safe for concurrent read-only evaluation.

A benchmark should compare the implementation from #433 with the shared-traversal version for representative robot sizes, numbers of query points, and Jacobian-on/Jacobian-off evaluations. The goal should be no meaningful regression in obstacle-factor evaluation time.

Suggested implementation sequence

1. Add tests that compare the existing Robot::forwardKinematics() and RobotQueryPoints pose results for the same configurations.
2. Extract the immutable traversal description without changing public behavior.
3. Migrate RobotQueryPoints to the shared representation while retaining its specialized evaluation buffers and outputs.
4. Migrate Robot::forwardKinematics() to the same traversal if doing so keeps its API and behavior clear.
5. Add equivalence, branched-tree, fixed-link, selected-joint, Jacobian, concurrency, and performance tests.

Acceptance criteria

• There is one canonical implementation of robot-tree topology and traversal ordering.
• Existing forward-kinematics and obstacle-factor APIs retain their behavior.
• Query-point Jacobians pass numerical derivative checks.
• Branched robots, fixed links, nontrivial roots, and selected joint subsets are covered.
• A single compiled traversal can be evaluated concurrently without mutation or shared scratch state.
• Benchmarks demonstrate that the obstacle-query hot path remains approximately as fast as the implementation introduced by Gpmp2 obstacles #433.