@Override public ReferenceFrame getReferenceFrame() { angularPart.checkReferenceFrameMatch(linearPart); return angularPart.getReferenceFrame(); }
/** * Tests if this vector contains a {@link Double#NaN}. * * @return {@code true} if this vector contains a {@link Double#NaN}, {@code false} otherwise. */ default boolean containsNaN() { return getAngularPart().containsNaN() || getLinearPart().containsNaN(); }
public void update(FrameVector3DReadOnly inputAngularVelocity, FrameVector3DReadOnly inputLinearAcceleration, FrameVector3DReadOnly inputMagneticVector) { inputAngularVelocity.checkReferenceFrameMatch(sensorFrame); inputLinearAcceleration.checkReferenceFrameMatch(sensorFrame); if (inputMagneticVector != null) inputMagneticVector.checkReferenceFrameMatch(sensorFrame); update((Vector3DReadOnly) inputAngularVelocity, (Vector3DReadOnly) inputLinearAcceleration, (Vector3DReadOnly) inputMagneticVector); }
@Override public ReferenceFrame getReferenceFrame() { return referenceVector.getReferenceFrame(); }
/** * Tests on a per component basis if this vector is equal to the given {@code other} to an * {@code epsilon} and both vectors are expressed in the same reference frame. * * @param other the other vector to compare against this. Not modified. * @param epsilon the tolerance to use when comparing each component. * @return {@code true} if the two vectors are equal, {@code false} otherwise. */ default boolean epsilonEquals(SpatialVectorReadOnly other, double epsilon) { if (getReferenceFrame() != other.getReferenceFrame()) return false; if (!getAngularPart().epsilonEquals(other.getAngularPart(), epsilon)) return false; if (!getLinearPart().epsilonEquals(other.getLinearPart(), epsilon)) return false; return true; }
/** {@inheritDoc} */ @Override default void setJointAngularVelocity(Vector3DReadOnly jointAngularVelocity) { setQd(getJointAxis().dot(jointAngularVelocity)); }
if (!getAngularPart().geometricallyEquals(other.getAngularPart(), epsilon)) return false; if (!getLinearPart().geometricallyEquals(other.getLinearPart(), epsilon)) return false; return true;
/** * Tests on a per component basis, if this vector is exactly equal to {@code other} and expressed * in the same reference frame. * * @param other the other vector to compare against this. Not modified. * @return {@code true} if the two vectors are exactly equal component-wise, {@code false} * otherwise. */ default boolean equals(SpatialVectorReadOnly other) { if (other == null) return false; if (getReferenceFrame() != other.getReferenceFrame()) return false; if (!getAngularPart().equals(other.getAngularPart())) return false; if (!getLinearPart().equals(other.getLinearPart())) return false; return true; } }
angularPart.checkReferenceFrameMatch(linearPart);
/** * Redefines this line with a new point, a new direction vector, and a new reference frame. * * @param pointOnLine new point on this line. Not modified. * @param lineDirection new direction of this line. Not modified. */ default void setIncludingFrame(Point3DReadOnly pointOnLine, FrameVector3DReadOnly lineDirection) { setIncludingFrame(lineDirection.getReferenceFrame(), pointOnLine, lineDirection); }
static void assertTrajectoryPointContainsExpectedData(ReferenceFrame expectedFrame, double expectedTime, FrameQuaternionReadOnly expectedOrientation, FrameVector3DReadOnly expectedAngularVelocity, FrameSO3TrajectoryPoint testedFrameSO3TrajectoryPoint, double epsilon) { assertTrue(expectedFrame == testedFrameSO3TrajectoryPoint.getReferenceFrame()); assertEquals(expectedTime, testedFrameSO3TrajectoryPoint.getTime(), epsilon); assertTrue(expectedOrientation.geometricallyEquals(testedFrameSO3TrajectoryPoint.getGeometryObject().getOrientation(), epsilon)); assertTrue(expectedAngularVelocity.epsilonEquals(testedFrameSO3TrajectoryPoint.getGeometryObject().getAngularVelocity(), epsilon)); Quaternion actualOrientation = new Quaternion(); Vector3D actualAngularVelocity = new Vector3D(); testedFrameSO3TrajectoryPoint.getOrientation(actualOrientation); testedFrameSO3TrajectoryPoint.getAngularVelocity(actualAngularVelocity); assertTrue(expectedOrientation.geometricallyEquals(actualOrientation, epsilon)); assertTrue(expectedAngularVelocity.epsilonEquals(actualAngularVelocity, epsilon)); FrameQuaternion actualFrameOrientation = new FrameQuaternion(); FrameVector3D actualFrameAngularVelocity = new FrameVector3D(); testedFrameSO3TrajectoryPoint.getOrientationIncludingFrame(actualFrameOrientation); testedFrameSO3TrajectoryPoint.getAngularVelocityIncludingFrame(actualFrameAngularVelocity); assertTrue(expectedOrientation.geometricallyEquals(actualFrameOrientation, epsilon)); assertTrue(expectedAngularVelocity.epsilonEquals(actualFrameAngularVelocity, epsilon)); actualFrameOrientation = new FrameQuaternion(expectedFrame); actualFrameAngularVelocity = new FrameVector3D(expectedFrame); testedFrameSO3TrajectoryPoint.getOrientation(actualFrameOrientation); testedFrameSO3TrajectoryPoint.getAngularVelocity(actualFrameAngularVelocity); assertTrue(expectedOrientation.geometricallyEquals(actualFrameOrientation, epsilon)); assertTrue(expectedAngularVelocity.epsilonEquals(actualFrameAngularVelocity, epsilon)); }
/** {@inheritDoc} */ @Override default void setJointAngularAcceleration(Vector3DReadOnly jointAngularAcceleration) { setQdd(getJointAxis().dot(jointAngularAcceleration)); }
if (!getAngularPart().geometricallyEquals(getAngularPart(), epsilon)) return false; if (!getLinearPart().geometricallyEquals(getLinearPart(), epsilon)) return false; return true;
/** * Tests on a per component basis, if this spatial inertia matrix is exactly equal to * {@code other} and have the same reference frames. * * @param other the other spatial inertia to compare against this. Not modified. * @return {@code true} if the two spatial inertia matrices are exactly equal component-wise, * {@code false} otherwise. */ default boolean equals(SpatialInertiaReadOnly other) { if (other == null) return false; if (getBodyFrame() != other.getBodyFrame()) return false; if (getReferenceFrame() != other.getReferenceFrame()) return false; if (!getMomentOfInertia().equals(other.getMomentOfInertia())) return false; if (getMass() != other.getMass()) return false; if (!getCenterOfMassOffset().equals(other.getCenterOfMassOffset())) return false; return true; } }
@Override public ReferenceFrame getReferenceFrame() { angularPart.checkReferenceFrameMatch(linearPart); return angularPart.getReferenceFrame(); }
public void setCubicUsingFinalAccelerationButNotFinalPosition(double t0, double tFinal, FramePoint3DReadOnly z0, FrameVector3DReadOnly zd0, FrameVector3DReadOnly zdFinal, FrameVector3DReadOnly zddFinal) { z0.checkReferenceFrameMatch(referenceFrame); zd0.checkReferenceFrameMatch(referenceFrame); zdFinal.checkReferenceFrameMatch(referenceFrame); zddFinal.checkReferenceFrameMatch(referenceFrame); super.setCubicUsingFinalAccelerationButNotFinalPosition(t0, tFinal, z0, zd0, zdFinal, zddFinal); }
/** * Redefines this line with a new point, a new direction vector, and a new reference frame. * * @param pointOnLine new point on this line. Not modified. * @param lineDirection new direction of this line. Not modified. */ default void setIncludingFrame(Point3DReadOnly pointOnLine, FrameVector3DReadOnly lineDirection) { setIncludingFrame(lineDirection.getReferenceFrame(), pointOnLine, lineDirection); }
static void assertTrajectoryPointContainsExpectedData(ReferenceFrame expectedFrame, double expectedTime, FramePoint3DReadOnly expectedPosition, FrameVector3DReadOnly expectedLinearVelocity, FrameEuclideanTrajectoryPoint testedFrameEuclideanTrajectoryPoint, double epsilon) { assertTrue(expectedFrame == testedFrameEuclideanTrajectoryPoint.getReferenceFrame()); assertEquals(expectedTime, testedFrameEuclideanTrajectoryPoint.getTime(), epsilon); assertTrue(expectedPosition.epsilonEquals(testedFrameEuclideanTrajectoryPoint.getGeometryObject().getPosition(), epsilon)); assertTrue(expectedLinearVelocity.epsilonEquals(testedFrameEuclideanTrajectoryPoint.getGeometryObject().getLinearVelocity(), epsilon)); Point3D actualPosition = new Point3D(); Vector3D actualLinearVelocity = new Vector3D(); testedFrameEuclideanTrajectoryPoint.getPosition(actualPosition); testedFrameEuclideanTrajectoryPoint.getLinearVelocity(actualLinearVelocity); assertTrue(expectedPosition.epsilonEquals(actualPosition, epsilon)); assertTrue(expectedLinearVelocity.epsilonEquals(actualLinearVelocity, epsilon)); FramePoint3D actualFramePosition = new FramePoint3D(); FrameVector3D actualFrameLinearVelocity = new FrameVector3D(); testedFrameEuclideanTrajectoryPoint.getPositionIncludingFrame(actualFramePosition); testedFrameEuclideanTrajectoryPoint.getLinearVelocityIncludingFrame(actualFrameLinearVelocity); assertTrue(expectedPosition.epsilonEquals(actualFramePosition, epsilon)); assertTrue(expectedLinearVelocity.epsilonEquals(actualFrameLinearVelocity, epsilon)); actualFramePosition = new FramePoint3D(expectedFrame); actualFrameLinearVelocity = new FrameVector3D(expectedFrame); testedFrameEuclideanTrajectoryPoint.getPosition(actualFramePosition); testedFrameEuclideanTrajectoryPoint.getLinearVelocity(actualFrameLinearVelocity); assertTrue(expectedPosition.epsilonEquals(actualFramePosition, epsilon)); assertTrue(expectedLinearVelocity.epsilonEquals(actualFrameLinearVelocity, epsilon)); }
/** {@inheritDoc} */ @Override default void setJointLinearAcceleration(Vector3DReadOnly jointLinearAcceleration) { setQdd(getJointAxis().dot(jointLinearAcceleration)); }
/** * 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; }