/** * Integrates the given {@code joint}'s velocity to update its configuration. * * @param joint the joint to integrate the state of. The joint configuration is modified. */ public void integrateFromVelocity(JointBasics joint) { if (joint instanceof OneDoFJointBasics) integrateFromVelocity((OneDoFJointBasics) joint); else if (joint instanceof FloatingJointBasics) integrateFromVelocity((FloatingJointBasics) joint); else if (joint instanceof SphericalJointBasics) integrateFromVelocity((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 velocity to update their respective configuration. * <p> * If the acceleration of each joint is available, it is preferable to use * {@link #doubleIntegrateFromAccelerationSubtree(RigidBodyBasics)}. * </p> * * @param rootBody the origin of the subtree to integrate the state of. The configuration of each * joint in the subtree is modified. */ public void integrateFromVelocitySubtree(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); integrateFromVelocity(childJoint); integrateFromVelocitySubtree(childJoint.getSuccessor()); } }
/** * Iterates through the given {@code joints} and integrates each joint velocity to update their * respective configuration. * <p> * If the acceleration of each joint is available, it is preferable to use * {@link #doubleIntegrateFromAcceleration(List)}. * </p> * * @param joints the list of the joints to integrate the state of. The configuration of each * joint is modified. */ public void integrateFromVelocity(List<? extends JointBasics> joints) { for (int i = 0; i < joints.size(); i++) { JointBasics joint = joints.get(i); integrateFromVelocity(joint); } }
integrator.integrateFromVelocity(joints); joints.get(0).getPredecessor().updateFramesRecursively(); jointFramesToFDFrames.keySet().forEach(MovingReferenceFrame::update);
integrator.integrateFromVelocity(joints); joints.get(0).getPredecessor().updateFramesRecursively(); jointFramesToFDFrames.keySet().forEach(MovingReferenceFrame::update);
integrator.integrateFromVelocity(joints); joints.get(0).getPredecessor().updateFramesRecursively(); jointFramesToFDFrames.keySet().forEach(MovingReferenceFrame::update);
integrator.integrateFromVelocity(joints); joints.get(0).getPredecessor().updateFramesRecursively(); jointFramesToFDFrames.keySet().forEach(MovingReferenceFrame::update);
integrator.integrateFromVelocity(joints); joints.get(0).getPredecessor().updateFramesRecursively();
floatingJointInFuture.setJointTwist(floatingJoint); floatingJointInFuture.setJointAcceleration(floatingJoint); integrator.integrateFromVelocity(floatingJointInFuture);
floatingJointInFuture.setJointTwist(floatingJoint); floatingJointInFuture.setJointAcceleration(floatingJoint); integrator.integrateFromVelocity(floatingJointInFuture);
double dt = 1e-8; MultiBodySystemStateIntegrator integrator = new MultiBodySystemStateIntegrator(dt); integrator.integrateFromVelocity(randomFloatingChain.getRevoluteJoints()); randomFloatingChain.getElevator().updateFramesRecursively(); point2.changeFrame(base.getBodyFixedFrame());