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