/** * Asserts on a per component basis that the two spatial force 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 expected the expected spatial force vector. Not modified. * @param actual the actual spatial force vector. Not modified. * @param epsilon the tolerance to use. * @throws AssertionError if the two spatial force vectors are not equal. If only one of the * arguments is equal to {@code null}. */ public static void assertSpatialForceEquals(SpatialForceReadOnly expected, SpatialForceReadOnly actual, double epsilon) { assertSpatialForceEquals(null, expected, actual, epsilon); }
/** * Asserts on a per component basis that the two spatial force 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 force vector. Not modified. * @param actual the actual spatial force vector. Not modified. * @param epsilon the tolerance to use. * @throws AssertionError if the two spatial force vectors are not equal. If only one of the * arguments is equal to {@code null}. */ public static void assertSpatialForceEquals(String messagePrefix, SpatialForceReadOnly expected, SpatialForceReadOnly actual, double epsilon) { assertSpatialForceEquals(messagePrefix, expected, actual, epsilon, DEFAULT_FORMAT); }
public static void assertRootJointWrenchZero(Map<RigidBodyBasics, Wrench> externalWrenches, SixDoFJoint rootJoint, double gravityZ, double epsilon) { InverseDynamicsCalculator inverseDynamicsCalculator = new InverseDynamicsCalculator(rootJoint.getPredecessor()); inverseDynamicsCalculator.setGravitionalAcceleration(-gravityZ); for (RigidBodyBasics rigidBody : externalWrenches.keySet()) { Wrench externalWrench = externalWrenches.get(rigidBody); externalWrench.changeFrame(rigidBody.getBodyFixedFrame()); inverseDynamicsCalculator.setExternalWrench(rigidBody, externalWrench); } inverseDynamicsCalculator.compute(); Wrench wrench = new Wrench(); wrench.setIncludingFrame(rootJoint.getJointWrench()); SpatialForce zeroWrench = new SpatialForce(wrench.getReferenceFrame()); MecanoTestTools.assertSpatialForceEquals(wrench, zeroWrench, epsilon); } }
public static void assertWrenchesSumUpToMomentumDot(Collection<Wrench> externalWrenches, SpatialForce desiredCentroidalMomentumRate, double gravityZ, double mass, ReferenceFrame centerOfMassFrame, double epsilon) { SpatialForce totalWrench = new SpatialForce(centerOfMassFrame); Wrench tempWrench = new Wrench(); for (Wrench wrench : externalWrenches) { tempWrench.setIncludingFrame(wrench); tempWrench.changeFrame(centerOfMassFrame); totalWrench.add(tempWrench); } Wrench gravitationalWrench = new Wrench(centerOfMassFrame, centerOfMassFrame); gravitationalWrench.setLinearPartZ(-mass * gravityZ); totalWrench.add(gravitationalWrench); MecanoTestTools.assertSpatialForceEquals(desiredCentroidalMomentumRate, totalWrench, epsilon); }