private FloatingJoint getRootJoint(SimulationConstructionSet scs0) {
   Joint firstRootJoint = scs0.getRobots()[0].getRootJoints().get(0);
   if (firstRootJoint instanceof FloatingJoint) return (FloatingJoint) firstRootJoint;
   throw new RuntimeException("first root joint is not a floating joint: " + firstRootJoint);
 }