public void setWrench(WrenchReadOnly newWrench) { measurementFrame.checkReferenceFrameMatch(newWrench.getReferenceFrame()); measurementFrame.checkReferenceFrameMatch(newWrench.getBodyFrame()); newWrench.get(wrench); }
/** * 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()); }
/** * Checks that the reference frames used by {@code this} correspond to the given ones. * * @param bodyFrame the query for the body frame. * @param expressedInFrame the query for the "expressed in frame". * @throws ReferenceFrameMismatchException if the reference frames are not the same: * {@code this.getBodyFrame() != bodyFrame} or * {@code this.getReferenceFrame() != expressedInFrame}. */ default void checkReferenceFrameMatch(ReferenceFrame bodyFrame, ReferenceFrame expressedInFrame) throws ReferenceFrameMismatchException { checkBodyFrameMatch(bodyFrame); checkExpressedInFrameMatch(expressedInFrame); }
/** * Checks that all frames, i.e. the body frame and the "expressed in frame", are identical * between the two wrenches. * * @param other the other wrench holding onto the reference frames to compare against the * reference frames held by {@code this}. Not modified. * @throws ReferenceFrameMismatchException if the reference frames are not the same: * {@code this.getBodyFrame() != other.getBodyFrame()} or * {@code this.getReferenceFrame() != other.getReferenceFrame()}. */ default void checkReferenceFrameMatch(WrenchReadOnly other) throws ReferenceFrameMismatchException { checkReferenceFrameMatch(other.getBodyFrame(), other.getReferenceFrame()); }
@Override public ReferenceFrame getBodyFrame() { return referenceWrench.getBodyFrame(); }
/** * 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); }
/** * 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()); }
/** * Computes and packs the joint torque vector that corresponds to the given wrench. * * @param wrench the resulting wrench at the end effector. The wrench should be expressed in * {@code jacobianFrame} and the wrench's {@code bodyFrame} should be the body fixed * frame of the end-effector. * @throws ReferenceFrameMismatchException if the given wrench * {@code wrench.getExpressedInFrame() != this.getJacobianFrame()} or * {@code wrench.getBodyFrame() != this.getEndEffectorFrame()}. */ public void computeJointTorques(WrenchReadOnly wrench, DenseMatrix64F jointTorquesToPack) { // reference frame check wrench.getReferenceFrame().checkReferenceFrameMatch(this.jacobianFrame); // FIXME add the following reference frame check // wrench.getBodyFrame().checkReferenceFrameMatch(getEndEffectorFrame()); wrench.get(tempMatrix); jointTorquesToPack.reshape(1, jacobian.getNumCols()); CommonOps.multTransA(tempMatrix, jacobian, jointTorquesToPack); CommonOps.transpose(jointTorquesToPack); }
/** * 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; }
/** * Computes and packs the joint torque vector that corresponds to the given wrench. * * @param endEffectorWrench the resulting wrench at the end effector. The wrench should be expressed * in {@code jacobianFrame} and the wrench's {@code bodyFrame} should be the body fixed * frame of the end-effector. Not modified. * @param jointTorquesToPack the dense matrix used to store the computed joint torques. Modified. * @throws ReferenceFrameMismatchException if the given wrench * {@code wrench.getExpressedInFrame() != this.getJacobianFrame()} or * {@code wrench.getBodyFrame() != this.getEndEffectorFrame()}. * @throws RuntimeException if either the base or the end-effector has not been provided beforehand. */ public void getJointTorques(WrenchReadOnly endEffectorWrench, DenseMatrix64F jointTorquesToPack) { endEffectorWrench.checkReferenceFrameMatch(getEndEffectorFrame(), jacobianFrame); endEffectorWrench.get(spatialVector); jointTorquesToPack.reshape(1, numberOfDegreesOfFreedom); CommonOps.multTransA(spatialVector, getJacobianMatrix(), jointTorquesToPack); CommonOps.transpose(jointTorquesToPack); }
@Override public ReferenceFrame getReferenceFrame() { return referenceWrench.getReferenceFrame(); }
/** {@inheritDoc} */ @Override default int getJointTau(int rowStart, DenseMatrix64F matrixToPack) { getJointWrench().get(rowStart, matrixToPack); return rowStart + getDegreesOfFreedom(); }
/** {@inheritDoc} */ @Override default int getJointTau(int rowStart, DenseMatrix64F matrixToPack) { getJointWrench().getAngularPart().get(rowStart, matrixToPack); return rowStart + getDegreesOfFreedom(); }
/** * Sets this wrench to {@code other}. * <p> * If {@code other} is expressed in the frame as {@code this}, then this method is equivalent to * {@link #set(WrenchReadOnly)}. * </p> * <p> * If {@code other} is expressed in a different frame than {@code this}, then {@code this} is set * to {@code other} once transformed to be expressed in {@code this.getReferenceFrame()}. * </p> * * @param other the other vector to copy. Not modified. * @throws ReferenceFrameMismatchException if either the body frame or base frame in * {@code other} does not match {@code this}. */ default void setMatchingFrame(WrenchReadOnly other) { other.checkBodyFrameMatch(getBodyFrame()); FixedFrameSpatialForceBasics.super.setMatchingFrame(other); }
/** * Asserts on a per component basis that the two wrenches 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 wrench. Not modified. * @param actual the actual wrench. 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 wrenches are not equal. If only one of the arguments is * equal to {@code null}. */ public static void assertWrenchEquals(String messagePrefix, WrenchReadOnly expected, WrenchReadOnly 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); } }
/** * Checks if the body frame held by {@code this} matches the query {@code bodyFrame}. * * @param bodyFrame the query to compare against the body frame held by {@code this}. Not * modified. * @throws ReferenceFrameMismatchException if the two reference frames are not the same: * {@code this.getBodyFrame() != bodyFrame}. */ default void checkBodyFrameMatch(ReferenceFrame bodyFrame) { if (getBodyFrame() != bodyFrame) throw new ReferenceFrameMismatchException("bodyFrame mismatch: this.bodyFrame = " + getBodyFrame() + ", other bodyFrame = " + bodyFrame); }
/** * 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); }
/** * Checks if the "expressed in frame" held by {@code this} matches the query * {@code expressedInFrame}. * * @param expressedInFrame the query to compare against the "expressed in frame" held by * {@code this}. Not modified. * @throws ReferenceFrameMismatchException if the two reference frames are not the same: * {@code this.getReferenceFrame() != expressedInFrame}. */ default void checkExpressedInFrameMatch(ReferenceFrame expressedInFrame) { if (getReferenceFrame() != expressedInFrame) throw new ReferenceFrameMismatchException("expressedInFrame mismatch: this.expressedInFrame = " + getReferenceFrame() + ", other expressedInFrame = " + expressedInFrame); }
/** * 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()); }
static void compareWrenches(WrenchReadOnly inputWrench, Wrench outputWrench, DenseMatrix64F selectionMatrix) { inputWrench.getBodyFrame().checkReferenceFrameMatch(outputWrench.getBodyFrame()); outputWrench.changeFrame(inputWrench.getReferenceFrame()); inputWrench.getReferenceFrame().checkReferenceFrameMatch(outputWrench.getReferenceFrame()); DenseMatrix64F inputWrenchMatrix = new DenseMatrix64F(Wrench.SIZE, 1); DenseMatrix64F outputWrenchMatrix = new DenseMatrix64F(Wrench.SIZE, 1); DenseMatrix64F selectedValues = new DenseMatrix64F(Wrench.SIZE, 1); inputWrench.get(inputWrenchMatrix); outputWrench.get(outputWrenchMatrix); double epsilon = 1e-4; int taskSize = selectionMatrix.getNumRows(); int colIndex = 0; for (int i = 0; i < taskSize; i++) for (int j = colIndex; j < Wrench.SIZE; j++) if (selectionMatrix.get(i, j) == 1) selectedValues.set(j, 0, 1); // only compare the selected values for (int i = 0; i < Wrench.SIZE; i++) { if (selectedValues.get(i, 0) == 1) Assert.assertEquals(inputWrenchMatrix.get(i, 0), outputWrenchMatrix.get(i, 0), epsilon); } }