/** * Integrates the given {@code joint}'s acceleration and velocity to update its velocity and * configuration. * * @param joint the joint to integrate the state of. The joint configuration is modified. */ public void doubleIntegrateFromAcceleration(JointBasics joint) { if (joint instanceof OneDoFJointBasics) doubleIntegrateFromAcceleration((OneDoFJointBasics) joint); else if (joint instanceof FloatingJointBasics) doubleIntegrateFromAcceleration((FloatingJointBasics) joint); else if (joint instanceof SphericalJointBasics) doubleIntegrateFromAcceleration((SphericalJointBasics) joint); else if (joint instanceof FixedJointBasics) return; else throw new UnsupportedOperationException("Integrator does not support the joint type: " + joint.getClass().getSimpleName()); }
/** * 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()); } }
/** * Iterates through the given {@code joints} and integrates each joint acceleration and velocity * to update their respective velocity and configuration. * * @param joints the list of the joints to integrate the state of. The configuration of each * joint is modified. */ public void doubleIntegrateFromAcceleration(List<? extends JointBasics> joints) { for (int i = 0; i < joints.size(); i++) { JointBasics joint = joints.get(i); doubleIntegrateFromAcceleration(joint); } }