Hi Verena, I'm a few years late, but hopefully this will help somebody in the future.
To plot each rigid body of your system, you could use my plotframe function on File Exchange. When each coordinate system moves, you could update its position with the UpdateFrame name-value argument, so that it animates.
