/** * Gets a representative {@code String} of {@code spatialAcceleration} given a specific format to * use. * <p> * Using the default format {@link #DEFAULT_FORMAT}, this provides a {@code String} as follows: * * <pre> * Spatial acceleration of bodyFrame, with respect to baseFrame: [angular = ( 0.174, 0.732, -0.222 ), linear = ( 0.174, 0.732, -0.222 )] - expressedInFrame * </pre> * </p> * * @param format the format to use for each number. * @param spatialAcceleration the object to get the {@code String} of. Not modified. * @return the representative {@code String}. */ public static String getSpatialAccelerationString(String format, SpatialAccelerationReadOnly spatialAcceleration) { if (spatialAcceleration == null) return "null"; else return getSpatialAccelerationString(format, spatialAcceleration.getBodyFrame(), spatialAcceleration.getBaseFrame(), spatialAcceleration.getReferenceFrame(), spatialAcceleration.getAngularPart(), spatialAcceleration.getLinearPart()); }
checkReferenceFrameMatch(other); if (!getAngularPart().geometricallyEquals(other.getAngularPart(), epsilon)) return false; if (!getLinearPart().geometricallyEquals(other.getLinearPart(), epsilon)) return false; return true;
if (getReferenceFrame() != getBodyFrame() && getReferenceFrame() != getBaseFrame()) throw new ReferenceFrameMismatchException("This spatial acceleration has to either be expressed in base frame or body frame to use this feature: body frame = " + getBodyFrame() + ", base frame = " + getBaseFrame() + ", expressed in frame = " + getReferenceFrame()); checkExpressedInFrameMatch(bodyFixedPoint.getReferenceFrame()); checkReferenceFrameMatch(bodyTwist); MecanoTools.addCrossToVector(getAngularPart(), bodyFixedPoint, linearAccelerationToPack); // (wDot x p) + w x ((w x p) + v) linearAccelerationToPack.add(getLinearPart()); // vDot + (wDot x p) + w x ((w x p) + v)
FixedFrameTwistBasics finalTwist, Pose3DBasics finalPose) spatialAcceleration.checkReferenceFrameMatch(initialTwist); spatialAcceleration.checkExpressedInFrameMatch(spatialAcceleration.getBodyFrame()); if (finalTwist != initialTwist) finalTwist.checkReferenceFrameMatch(initialTwist); FrameVector3DReadOnly angularAcceleration = spatialAcceleration.getAngularPart(); FrameVector3DReadOnly linearAcceleration = spatialAcceleration.getLinearPart();
/** * Sets this joint current acceleration to the given spatial acceleration. * <p> * As for configuration setters, the common implementation here is to project the given acceleration * onto the motion subspace of this joint such that not all the components of the given spatial * acceleration may necessarily be used. * </p> * * @param jointAcceleration the new acceleration for this joint. Not modified. * @throws ReferenceFrameMismatchException if the given spatial acceleration does not have the * following frames: * <ul> * <li>{@code bodyFrame} is {@code afterJointFrame}. * <li>{@code baseFrame} is {@code beforeJointFrame}. * <li>{@code expressedInFrame} is {@code afterJointFrame}. * </ul> */ default void setJointAcceleration(SpatialAccelerationReadOnly jointAcceleration) { jointAcceleration.checkBodyFrameMatch(getFrameAfterJoint()); jointAcceleration.checkBaseFrameMatch(getFrameBeforeJoint()); jointAcceleration.checkExpressedInFrameMatch(getFrameAfterJoint()); setJointAngularAcceleration((Vector3DReadOnly) jointAcceleration.getAngularPart()); setJointLinearAcceleration((Vector3DReadOnly) jointAcceleration.getLinearPart()); }
getBodyFrame().checkReferenceFrameMatch(getReferenceFrame()); checkReferenceFrameMatch(bodyTwist); linearAccelerationToPack.setIncludingFrame(getLinearPart()); MecanoTools.addCrossToVector(bodyTwist.getAngularPart(), bodyTwist.getLinearPart(), linearAccelerationToPack);
acceleration.checkBodyFrameMatch(getBodyFrame()); acceleration.getBaseFrame().checkIsAStationaryFrame(); acceleration.checkExpressedInFrameMatch(getReferenceFrame()); angularAcceleration = acceleration.getAngularPart(); linearAcceleration = acceleration.getLinearPart();
/** * Sets this joint acceleration from the other joint. * * @param other the other to get the acceleration from. Not modified. */ default void setJointAcceleration(PlanarJointReadOnly other) { // Cast to frameless object so we don't perform frame checks which would automatically fail. Vector3DReadOnly otherAngularAcceleration = other.getJointAcceleration().getAngularPart(); Vector3DReadOnly otherLinearAcceleration = other.getJointAcceleration().getLinearPart(); setJointAngularAcceleration(otherAngularAcceleration); setJointLinearAcceleration(otherLinearAcceleration); }
/** * This method provides a spatial acceleration controller. * @param spatialAccelerationToPack spatial acceleration to return. * @param desiredPose desired pose that we want to achieve. * @param desiredTwist desired twist to damp around. * @param feedForwardAcceleration feed forward acceleration from a reference trajectory. * @param currentTwist current twist of the rigid body. */ public void compute(SpatialAcceleration spatialAccelerationToPack, FramePose3D desiredPose, TwistReadOnly desiredTwist, SpatialAccelerationReadOnly feedForwardAcceleration, TwistReadOnly currentTwist) { checkBodyFrames(desiredTwist, feedForwardAcceleration, currentTwist); checkBaseFrames(desiredTwist, feedForwardAcceleration, currentTwist); checkExpressedInFrames(desiredTwist, feedForwardAcceleration, currentTwist); spatialAccelerationToPack.setToZero(bodyFrame, feedForwardAcceleration.getBaseFrame(), bodyFrame); desiredOrientation.setIncludingFrame(desiredPose.getOrientation()); desiredAngularVelocity.setIncludingFrame(desiredTwist.getAngularPart()); feedForwardAngularAction.setIncludingFrame(feedForwardAcceleration.getAngularPart()); currentAngularVelocity.setIncludingFrame(currentTwist.getAngularPart()); orientationController.compute(angularActionFromOrientationController, desiredOrientation, desiredAngularVelocity, currentAngularVelocity, feedForwardAngularAction); spatialAccelerationToPack.getAngularPart().set(angularActionFromOrientationController); desiredPosition.setIncludingFrame(desiredPose.getPosition()); desiredVelocity.setIncludingFrame(desiredTwist.getLinearPart()); feedForwardLinearAction.setIncludingFrame(feedForwardAcceleration.getLinearPart()); currentVelocity.setIncludingFrame(currentTwist.getLinearPart()); positionController.compute(actionFromPositionController, desiredPosition, desiredVelocity, currentVelocity, feedForwardLinearAction); spatialAccelerationToPack.getLinearPart().set(actionFromPositionController); }
@Override public FrameVector3DReadOnly getLinearAccelerationOfBodyFixedPoint(RigidBodyReadOnly base, RigidBodyReadOnly body, FramePoint3DReadOnly bodyFixedPoint) { SpatialAccelerationReadOnly accelerationToUse = base != null ? getRelativeAcceleration(base, body) : getAccelerationOfBody(body); MovingReferenceFrame bodyFrame = body.getBodyFixedFrame(); ReferenceFrame baseFrame = accelerationToUse.getBaseFrame(); localBodyFixedPoint.setIncludingFrame(bodyFixedPoint); localBodyFixedPoint.changeFrame(bodyFrame); if (areVelocitiesConsidered()) bodyFrame.getTwistRelativeToOther(baseFrame, twistForLinearAcceleration); else twistForLinearAcceleration.setToZero(bodyFrame, baseFrame, bodyFrame); accelerationToUse.getLinearAccelerationAt(twistForLinearAcceleration, localBodyFixedPoint, linearAcceleration); linearAcceleration.changeFrame(baseFrame); return linearAcceleration; }
@Override public ReferenceFrame getReferenceFrame() { return referenceAcceleration.getReferenceFrame(); }
@Override public ReferenceFrame getBodyFrame() { return referenceAcceleration.getBodyFrame(); }
@Override public ReferenceFrame getBaseFrame() { return referenceAcceleration.getBaseFrame(); }
/** {@inheritDoc} */ @Override default int getJointAcceleration(int rowStart, DenseMatrix64F matrixToPack) { getJointAcceleration().getAngularPart().get(rowStart, matrixToPack); return rowStart + getDegreesOfFreedom(); }
/** * Asserts on a per component basis that the two spatial acceleration vectors are equal to an * {@code epsilon}. * <p> * Note: the two arguments are considered to be equal if they are both equal to {@code null}. * </p> * * @param messagePrefix prefix to add to the automated message. * @param expected the expected spatial acceleration vector. Not modified. * @param actual the actual spatial acceleration vector. Not modified. * @param epsilon the tolerance to use. * @param format the format to use for printing each component when an {@code AssertionError} is * thrown. * @throws AssertionError if the two spatial acceleration vectors are not equal. If only one of * the arguments is equal to {@code null}. */ public static void assertSpatialAccelerationEquals(String messagePrefix, SpatialAccelerationReadOnly expected, SpatialAccelerationReadOnly actual, double epsilon, String format) { if (expected == null && actual == null) return; if (!(expected != null && actual != null)) throwNotEqualAssertionError(messagePrefix, expected, actual, format); if (!expected.epsilonEquals(actual, epsilon)) { throwNotEqualAssertionError(messagePrefix, expected, actual, format); } }
/** * Sets this joint acceleration from the other joint. * * @param other the other to get the acceleration from. Not modified. */ default void setJointAcceleration(SixDoFJointReadOnly other) { // Cast to frameless object so we don't perform frame checks which would automatically fail. Vector3DReadOnly otherAngularAcceleration = other.getJointAcceleration().getAngularPart(); Vector3DReadOnly otherLinearAcceleration = other.getJointAcceleration().getLinearPart(); setJointAngularAcceleration(otherAngularAcceleration); setJointLinearAcceleration(otherLinearAcceleration); }
private void checkExpressedInFrames(TwistReadOnly desiredTwist, SpatialAccelerationReadOnly feedForwardAcceleration, TwistReadOnly currentTwist) { checkExpressedInFrames(desiredTwist, currentTwist); feedForwardAcceleration.getReferenceFrame().checkReferenceFrameMatch(bodyFrame); }
private void checkBodyFrames(TwistReadOnly desiredTwist, SpatialAccelerationReadOnly feedForwardAcceleration, TwistReadOnly currentTwist) { checkBodyFrames(desiredTwist, currentTwist); feedForwardAcceleration.getBodyFrame().checkReferenceFrameMatch(bodyFrame); }
private void checkBaseFrames(TwistReadOnly desiredTwist, SpatialAccelerationReadOnly feedForwardAcceleration, TwistReadOnly currentTwist) { checkBaseFrames(desiredTwist, currentTwist); desiredTwist.getBaseFrame().checkReferenceFrameMatch(feedForwardAcceleration.getBaseFrame()); }
checkExpressedInFrameMatch(other.getReferenceFrame()); if (getBodyFrame() == other.getBaseFrame()) setBodyFrame(other.getBodyFrame()); else if (getBaseFrame() == other.getBodyFrame()) setBaseFrame(other.getBaseFrame()); else throw new ReferenceFrameMismatchException("This acceleration and other are incompatible for addition."); getAngularPart().add((Vector3DReadOnly) other.getAngularPart()); getLinearPart().add((Vector3DReadOnly) other.getLinearPart());