I want to implement a physics simulator for large floating base rigid body systems from scratch. The Rigid Body Dynamics Systems (RBD) should typically have the following characteristics:
- About ~50 degrees of freedom (DoF) plus the 6 DoF of the floating base (e.g., a legged robot, a humanoid robot, etc.).
- It will always have tree structure (no parallel robots)
- The simulation needs to be fast.
- There will be contact forces (e.g., during walking, grasping or manipulation)
What would be the best physics formulation in terms of numerical accuracy and computational complexity? Lagrange appears too slow, but Newton Euler is often hard to comprehend.