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); }