Forward dynamics: given torques, what motion follows

[Onwcan]

Decide first

ADR-0010 currently declines forward dynamics on the grounds that it requires the mass matrix to be factorised and an integrator, which is a simulator's job rather than a controller's.

The ADR therefore keeps massMatrix as the half that belongs in this library.

That reasoning still holds for building a simulator.

There is, however, one controller-oriented use that needs a decision.

Checking whether a planned move stays within a drive's torque envelope only requires inverse dynamics, which already exists.

Predicting the motion that results after torque saturation requires forward dynamics.

Before implementing anything, settle this question:

• Is there a controller use in this library that inverse dynamics cannot serve?
• If the only answer is "so users can simulate", close this issue as not planned and point back to ADR-0010.

If it is built

Implement a single-step solve, not a simulator or integrator.

The intended operation is:

forwardDynamics(q, qd, tau) -> qdd

The implementation should:

• build the mass matrix M
• obtain the bias term using one inverseDynamics call with zero acceleration
• form tau - C qd - g
• solve M qdd = b using Cholesky

For a valid chain, M is symmetric positive definite.

Acceptance criteria

• Add forwardDynamics(q, qd, tau) -> qdd.
• Do not add an integrator.
• Do not add simulation state.
• Do not add a time step.
• The caller owns integration and state progression.
• Use the existing dynamics machinery to form the bias torque rather than duplicating it.
• Solve the mass-matrix system using Cholesky.
• Add a round-trip test: pass an arbitrary qdd through inverseDynamics, then feed the resulting torque into forwardDynamics.
• The recovered acceleration should agree with the original to approximately one ULP.
• Keep the implementation allocation-free.
• Keep the implementation non-throwing.
• Reuse a single Cholesky implementation rather than introducing a third copy.
• Coordinate this with the existing Cholesky tech-debt issue.

References

docs/adr/0010-dynamics-in-the-base-frame.md, Decision 3 and Consequences.