Consistent assembly of constrained systems by optimizing numerical energy

[symnsa]

Description:

Joining bodies via constraints is challenging when using the default quaternion-based parametrization of rigid bodies.
Despite tools like pose2q, complex mechanical systems require managing both the configuration of numerical models and the physical coordinates, adding complexity.

Feature Request:

• Implement an automatic assembly mechanism that minimizes numerical potential energy of 'springs' between constrained bodies and therefore allows to obtain the correct configuration as the configuration with minimal numerical energy.
• Ideally, this numerical energy vanishes when the kinematics are correct. However, when the bodies are misaligned (due to rounding issues in the numerical data), non-zero energy remains, indicating quality of the assembly.

This would align bodies optimally without manual adjustments, simplifying complex system setups and bridging the gap between physical intuition and numerical models.