/** * Sets this wrench to {@code other}. * * @param other the other wrench to copy. Not modified. * @throws ReferenceFrameMismatchException if any of the reference frames in {@code other} do not * match {@code this}. */ default void set(WrenchReadOnly other) { set(other.getBodyFrame(), other.getReferenceFrame(), other.getAngularPart(), other.getLinearPart()); }
/** * Tests if {@code this} and {@code other} represent the same wrench to an {@code epsilon}. * <p> * It is likely that the implementation of this method will change in the future as the * definition of "geometrically-equal" for wrenches might evolve. In the meantime, the current * assumption is that two wrenches are geometrically equal if both their 3D torque and 3D force * are independently geometrically equal, see * {@link Vector3DReadOnly#geometricallyEquals(Vector3DReadOnly, double)}. * </p> * <p> * Note that {@code this.geometricallyEquals(other, epsilon) == true} does not necessarily imply * {@code this.epsilonEquals(other, epsilon)} and vice versa. * </p> * * @param other the other wrench to compare against this. Not modified. * @param epsilon the tolerance to use for the comparison. * @return {@code true} if the two wrenches represent the same physical quantity, {@code false} * otherwise. * @throws ReferenceFrameMismatchException if the reference frames of {@code other} do not * respectively match the reference frames of {@code this}. */ default boolean geometricallyEquals(WrenchReadOnly other, double epsilon) { checkReferenceFrameMatch(other); if (!getAngularPart().geometricallyEquals(getAngularPart(), epsilon)) return false; if (!getLinearPart().geometricallyEquals(getLinearPart(), epsilon)) return false; return true; }
/** * Sets this joint force/torque from the other joint. * * @param other the other to get the force/torque from. Not modified. */ default void setJointWrench(SixDoFJointReadOnly other) { // Cast to frameless object so we don't perform frame checks which would automatically fail. Vector3DReadOnly otherTorque = other.getJointWrench().getAngularPart(); Vector3DReadOnly otherForce = other.getJointWrench().getLinearPart(); setJointTorque(otherTorque); setJointForce(otherForce); }
/** * Gets a representative {@code String} of {@code wrench} given a specific format to use. * <p> * Using the default format {@link #DEFAULT_FORMAT}, this provides a {@code String} as follows: * * <pre> * Wrench exerted on bodyFrame: [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 wrench the object to get the {@code String} of. Not modified. * @return the representative {@code String}. */ public static String getWrenchString(String format, WrenchReadOnly wrench) { if (wrench == null) return "null"; else return getWrenchString(format, wrench.getBodyFrame(), wrench.getReferenceFrame(), wrench.getAngularPart(), wrench.getLinearPart()); }
/** * Sets this joint force/torque from the other joint. * * @param other the other to get the force/torque from. Not modified. */ default void setJointWrench(PlanarJointReadOnly other) { // Cast to frameless object so we don't perform frame checks which would automatically fail. Vector3DReadOnly otherTorque = other.getJointWrench().getAngularPart(); Vector3DReadOnly otherForce = other.getJointWrench().getLinearPart(); setJointTorque(otherTorque); setJointForce(otherForce); }
private final FrameVector3DReadOnly linearPart = newFrameVector3DReadOnly(scaleSupplier, referenceWrench.getLinearPart());
/** * Sets this joint current force/torque to the given wrench. * <p> * As for the other spatial setters, this method's common implementation is to project the given * wrench to this joint's motion subspace. For instance, a {@code RevoluteJoint} that can rotate * around the y-axis will extract the torque around the y-axis from the given wrench to update its * torque. * </p> * * @param jointWrench the new wrench for this joint. Not modified. * @throws ReferenceFrameMismatchException if the given wrench does not have the following frames: * <ul> * <li>{@code bodyFrame} is {@code successor.getBodyFixedFrame()}. * <li>{@code expressedInFrame} is {@code frameAfterJoint}. * </ul> */ default void setJointWrench(WrenchReadOnly jointWrench) { jointWrench.checkBodyFrameMatch(getSuccessor().getBodyFixedFrame()); jointWrench.checkExpressedInFrameMatch(getFrameAfterJoint()); setJointTorque((Vector3DReadOnly) jointWrench.getAngularPart()); setJointForce((Vector3DReadOnly) jointWrench.getLinearPart()); }
public static void compareWrenches(WrenchReadOnly inputWrench, Wrench outputWrench) { inputWrench.getBodyFrame().checkReferenceFrameMatch(outputWrench.getBodyFrame()); outputWrench.changeFrame(inputWrench.getReferenceFrame()); inputWrench.getReferenceFrame().checkReferenceFrameMatch(outputWrench.getReferenceFrame()); double epsilon = 1e-4; EuclidCoreTestTools.assertTuple3DEquals(new Vector3D(inputWrench.getAngularPart()), new Vector3D(outputWrench.getAngularPart()), epsilon); EuclidCoreTestTools.assertTuple3DEquals(new Vector3D(inputWrench.getLinearPart()), new Vector3D(outputWrench.getLinearPart()), epsilon); }