/** * Recursively navigates the subtree that starts at the given {@code rootBody} and integrates * each joint acceleration and velocity to update their respective velocity and configuration. * * @param rootBody the origin of the subtree to integrate the state of. The configuration of each * joint in the subtree is modified. */ public void doubleIntegrateFromAccelerationSubtree(RigidBodyBasics rootBody) { if (rootBody == null) return; List<? extends JointBasics> childrenJoints = rootBody.getChildrenJoints(); for (int i = 0; i < childrenJoints.size(); i++) { JointBasics childJoint = childrenJoints.get(i); doubleIntegrateFromAcceleration(childJoint); doubleIntegrateFromAccelerationSubtree(childJoint.getSuccessor()); } }