/** * 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()); }
/** * 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); }
/** * 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; }