for managing the 3D: I use the library JPCT
For the management of 3D physics: I begin by managing the gravity of all 3D objects in motion, and when the object collides I calculate the orientation of the object relative to the object in collision.
For the displacement of the robot: I continuously calculates the angles of the axis, each axis rotates around a pivot. I use direct and indirect kinematics to calculate angles.
kind regards