[codex] Add dynamics integration helpers

[erlendbasso]

Summary

Adds convenience dynamics stepping helpers around forward_dynamics_ab:

• introduces DynamicsState, DynamicsStepInput, IntegrationMethod, and IntegrationOptions
• adds step_dynamics / try_step_dynamics for semi-implicit Euler and RK4-style stepping
• adds SE(3) exponential support for SixDOF body-frame twists ordered [linear; angular]
• reuses a single ForwardDynamicsWorkspace across step acceleration evaluations
• documents the SixDOF RK4 pose convention and validation behavior

Validation

Ran locally:

• cargo fmt --check
• cargo test
• cargo test --benches
• cargo clippy --all-targets --all-features -- -D warnings
• git diff --check

Notes

SixDOF RK4 uses a fourth-order generalized-velocity update and ordered SE(3) stage composition for poses, rather than averaging body-frame stage twists.

[Copilot]

Pull request overview

This PR adds public “dynamics stepping” helpers on MultiBody to integrate configuration/velocity forward in time using either semi-implicit Euler or an RK4-based scheme, building on forward_dynamics_ab. It also adds an SE(3) exponential utility to support integrating SixDOF joints using body-frame twists ordered as [linear; angular], and introduces integration-focused test coverage and documentation.

Changes:

• Introduces DynamicsState, DynamicsStepInput, IntegrationMethod, and IntegrationOptions, plus step_dynamics / try_step_dynamics integration entrypoints.
• Implements SE(3) exponential mapping (exp_se3) for [linear; angular] body-frame twists and uses it for SixDOF pose advancement (including RK4 stage composition).
• Adds integration helper tests and README documentation for the new checked stepping API.

Reviewed changes

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

File	Description
src/multibody.rs	Adds new integration API/types and implements Euler + RK4 stepping on top of forward dynamics.
src/math_functions.rs	Adds exp_se3 to integrate SixDOF poses via SE(3) exponential for [linear; angular] twists.
tests/integration_helpers.rs	Adds tests validating Euler/RK4 stepping behavior and input validation/panic messages.
README.md	Documents the new integration helpers and mentions try_step_dynamics among checked APIs.

---

💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.

[Copilot]

Pull request overview

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