/** * Integrates the given {@code twist} to update the given {@code poseToIntegrate}. * <p> * Pseudo-code: * * <pre> * poseToIntegrate += dt * twist * </pre> * </p> * * @param twist the angular and linear velocity to integrate. Not modified. * @param poseToIntegrate the pose to update. Modified. * @throws ReferenceFrameMismatchException if the given {@code twist} is not expressed in the * frame {@code twist.getBodyFrame()}. */ public void integrate(TwistReadOnly twist, Pose3DBasics poseToIntegrate) { integrate(twist, poseToIntegrate, poseToIntegrate); }
/** * 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()); }
/** * Integrates the given {@code linearAcceleration} and {@code linearVelocityToIntegrate} to * estimate the {@code positionToIntegrate} and {@code linearVelocityToIntegrate}. The given * acceleration and velocity is expected to be expressed in the local frame described by the * given orientation. * <p> * Pseudo-code: * * <pre> * positionToIntegrate += orientation * (0.5 * dt * dt * linearAcceleration + dt * linearVelocityToIntegrate) * linearVelocityToIntegrate += dt * linearAcceleration * </pre> * </p> * * @param orientation the orientation describing the frame in which the acceleration and velocity * are expressed. If equal to {@code null}, the orientation is assumed to be identity. * Not modified. * @param linearAcceleration the linear acceleration to integrate. Not modified. * @param linearVelocityToIntegrate the linear velocity to integrate and update. Modified. * @param positionToIntegrate the position to update. Modified. */ public void doubleIntegrate(Orientation3DReadOnly orientation, Vector3DReadOnly linearAcceleration, Vector3DBasics linearVelocityToIntegrate, Tuple3DBasics positionToIntegrate) { doubleIntegrate(orientation, linearAcceleration, linearVelocityToIntegrate, positionToIntegrate, linearVelocityToIntegrate, positionToIntegrate); }
/** * 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()); } }
int numberOfJoints = 20; double updateDT = 1.0e-8; MultiBodySystemStateIntegrator integrator = new MultiBodySystemStateIntegrator(updateDT); integrator.integrateFromVelocity(joints); joints.get(0).getPredecessor().updateFramesRecursively(); jointFramesToFDFrames.keySet().forEach(MovingReferenceFrame::update);
private boolean exponentialCoordinatesOK() { Twist twist = new Twist(jacobian.getEndEffectorFrame(), jacobian.getBaseFrame(), jacobian.getJacobianFrame(), error); Pose3D poseCheck = new Pose3D(); new MultiBodySystemStateIntegrator(1.0).integrate(twist, poseCheck); RigidBodyTransform transformCheck = new RigidBodyTransform(poseCheck.getOrientation(), poseCheck.getPosition()); return transformCheck.epsilonEquals(errorTransform, 1e-5); }
/** * 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()); } }
/** * 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(OneDoFJointBasics joint) { double initialQ = joint.getQ(); double initialQd = joint.getQd(); double qdd = joint.getQdd(); joint.setQ(doubleIntegratePosition(qdd, initialQd, initialQ)); joint.setQd(integrateVelocity(qdd, initialQd)); }
/** * 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()); }
/** * 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(OneDoFJointBasics joint) { double initialQ = joint.getQ(); double qd = joint.getQd(); joint.setQ(integratePosition(qd, initialQ)); }
int numberOfJoints = 20; double updateDT = 1.0e-8; MultiBodySystemStateIntegrator integrator = new MultiBodySystemStateIntegrator(updateDT); integrator.integrateFromVelocity(joints); joints.get(0).getPredecessor().updateFramesRecursively(); jointFramesToFDFrames.keySet().forEach(MovingReferenceFrame::update);
/** * 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); } }
int numberOfJoints = 20; double updateDT = 1.0e-8; MultiBodySystemStateIntegrator integrator = new MultiBodySystemStateIntegrator(updateDT); integrator.integrateFromVelocity(joints); joints.get(0).getPredecessor().updateFramesRecursively(); jointFramesToFDFrames.keySet().forEach(MovingReferenceFrame::update);
/** * Integrates the given {@code angularVelocity} to update the given * {@code orientationToIntegrate}. The given velocity is expected to be expressed in the local * frame described by the given orientation. * <p> * Pseudo-code: * * <pre> * orientationToIntegrate += dt * angularVelocity * </pre> * </p> * * @param angularVelocity the angular velocity to integrate. Not modified. * @param orientationToIntegrate the orientation to update. Modified. */ public void integrate(Vector3DReadOnly angularVelocity, Orientation3DBasics orientationToIntegrate) { integrate(angularVelocity, orientationToIntegrate, orientationToIntegrate); }
/** * 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); } }
/** * Integrates the given {@code angularAcceleration} and {@code angularVelocityToIntegrate} to * update the given {@code orientationToIntegrate} and {@code angularVelocityToIntegrate}. The * given acceleration and velocity are expected to be expressed in the local frame described by * the given orientation. * <p> * Pseudo-code: * * <pre> * orientationToIntegrate += 0.5 * dt * dt * angularAcceleration + dt * angularVelocityToIntegrate * angularVelocityToIntegrate += dt * spatialAcceleration * </pre> * </p> * * @param angularAcceleration the angular acceleration to integrate. Not modified. * @param angularVelocityToIntegrate the angular velocity to integrate and update. Modified. * @param orientationToIntegrate the orientation to update. Modified. */ public void doubleIntegrate(Vector3DReadOnly angularAcceleration, Vector3DBasics angularVelocityToIntegrate, Orientation3DBasics orientationToIntegrate) { doubleIntegrate(angularAcceleration, angularVelocityToIntegrate, orientationToIntegrate, angularVelocityToIntegrate, orientationToIntegrate); }
int numberOfJoints = 20; double updateDT = 1.0e-8; MultiBodySystemStateIntegrator integrator = new MultiBodySystemStateIntegrator(updateDT); integrator.integrateFromVelocity(joints); joints.get(0).getPredecessor().updateFramesRecursively(); jointFramesToFDFrames.keySet().forEach(MovingReferenceFrame::update);
/** * Integrates the given {@code linearVelocity} to estimate the {@code positionToIntegrate}. The * given velocity is expected to be expressed in the local frame described by the given * orientation. * <p> * Pseudo-code: * * <pre> * positionToIntegrate += orientation * dt * linearVelocity * </pre> * </p> * * @param orientation the orientation describing the frame in which the velocity is expressed. If * equal to {@code null}, the orientation is assumed to be identity. Not modified. * @param linearVelocity the linear velocity to integrate. Not modified. * @param positionToIntegrate the position to update. Modified. */ public void integrate(Orientation3DReadOnly orientation, Vector3DReadOnly linearVelocity, Tuple3DBasics positionToIntegrate) { integrate(orientation, linearVelocity, positionToIntegrate, positionToIntegrate); }
/** * Integrates the given {@code spatialAcceleration} and {@code twistToIntegrate} to estimate the * {@code poseToIntegrate} and {@code twistToIntegrate}. * <p> * Pseudo-code: * * <pre> * poseToIntegrate += 0.5 * dt * dt * spatialAcceleration + dt * twistToIntegrate * twistToIntegrate += dt * spatialAcceleration * </pre> * </p> * * @param spatialAcceleration the angular and linear acceleration to integrate. Not modified. * @param twistToIntegrate the angular and linear velocity to integrate and update. Modified. * @param poseToIntegrate the pose to update. Modified. * @throws ReferenceFrameMismatchException if the given {@code spatialAcceleration} is not * expressed in the frame {@code spatialAcceleration.getBodyFrame()}. * @throws ReferenceFrameMismatchException if the given {@code spatialAcceleration}, * {@code initialTwist}, and {@code finalTwist} do not have the same reference frame. */ public void doubleIntegrate(SpatialAccelerationReadOnly spatialAcceleration, FixedFrameTwistBasics twistToIntegrate, Pose3DBasics poseToIntegrate) { doubleIntegrate(spatialAcceleration, twistToIntegrate, poseToIntegrate, twistToIntegrate, poseToIntegrate); }
int numberOfJoints = 20; double updateDT = 1.0e-8; MultiBodySystemStateIntegrator integrator = new MultiBodySystemStateIntegrator(updateDT); integrator.integrateFromVelocity(joints); joints.get(0).getPredecessor().updateFramesRecursively();