public YoQuaternionProvider(String namePrefix, ReferenceFrame referenceFrame, YoVariableRegistry registry) { orientation = new YoFrameQuaternion(namePrefix + "Orientation", referenceFrame, registry); }
@Override public boolean containsNaN() { return super.containsNaN(); }
@Override public void setOrientation(QuaternionReadOnly orientation) { this.orientation.set(orientation); }
private void updateInitialOrientation() { initialOrientationProvider.getOrientation(tempInitialOrientation); tempInitialOrientation.changeFrame(initialOrientation.getReferenceFrame()); initialOrientation.set(tempInitialOrientation); initialOrientation.checkIfUnitary(); }
/** * Creates a copy of {@code this} by finding the duplicated {@code YoVariable}s in the given * {@link YoVariableRegistry}. * <p> * This method does not duplicate {@code YoVariable}s. Assuming the given registry is a duplicate * of the registry that was used to create {@code this}, this method searches for the duplicated * {@code YoVariable}s and use them to duplicate {@code this}. * </p> * * @param newRegistry YoVariableRegistry to duplicate {@code this} to. * @return the duplicate of {@code this}. */ public YoFrameQuaternion duplicate(YoVariableRegistry newRegistry) { YoDouble x = (YoDouble) newRegistry.getVariable(getYoQx().getFullNameWithNameSpace()); YoDouble y = (YoDouble) newRegistry.getVariable(getYoQy().getFullNameWithNameSpace()); YoDouble z = (YoDouble) newRegistry.getVariable(getYoQz().getFullNameWithNameSpace()); YoDouble s = (YoDouble) newRegistry.getVariable(getYoQs().getFullNameWithNameSpace()); return new YoFrameQuaternion(x, y, z, s, getReferenceFrame()); }
YoFrameQuaternion yoFrameQuaternion = new YoFrameQuaternion("test", worldFrame, registry); yoFrameQuaternion.set(quat4dExpected); Quaternion quat4dActual = new Quaternion(yoFrameQuaternion); assertTrue(RotationTools.quaternionEpsilonEquals(quat4dExpected, quat4dActual, EPS)); yoFrameQuaternion.set(axisAngle4dExpected); AxisAngle axisAngle4dActual = new AxisAngle(yoFrameQuaternion); assertTrue(RotationTools.axisAngleEpsilonEqualsIgnoreFlippedAxes(axisAngle4dExpected, axisAngle4dActual, EPS)); yoFrameQuaternion.set(matrix3dExpected); matrix3dActual.set(yoFrameQuaternion); assertTrue(matrix3dActual.epsilonEquals(matrix3dExpected, EPS)); yoFrameQuaternion.set(frameOrientationExpected); FrameQuaternion frameOrientationActual = new FrameQuaternion(yoFrameQuaternion); assertTrue(frameOrientationActual.epsilonEquals(frameOrientationExpected, EPS)); yoFrameQuaternion.setYawPitchRoll(yawPitchRollExpected[0], yawPitchRollExpected[1], yawPitchRollExpected[2]); double[] yawPitchRollActual = new double[3]; yoFrameQuaternion.getYawPitchRoll(yawPitchRollActual);
Random random = new Random(3454); ReferenceFrame sensorFrame = EuclidFrameRandomTools.nextReferenceFrame(random); YoFrameQuaternion actualOrientation = new YoFrameQuaternion("q_act_", sensorFrame, registry); mahonyFilter.setInputs(inputAngularVelocity, inputLinearAcceleration, inputMagneticVector); mahonyFilter.setGains(0.5, 0.0); mahonyFilter.getEstimatedOrientation().set(EuclidCoreRandomTools.nextQuaternion(random)); actualOrientation.set(EuclidCoreRandomTools.nextQuaternion(random)); Vector3D zUp = new Vector3D(0.0, 0.0, 1.0); actualOrientation.inverseTransform(zUp); inputLinearAcceleration.set(zUp); inputLinearAcceleration.scale(20.0); actualOrientation.inverseTransform(xForward); inputMagneticVector.set(xForward); inputMagneticVector.scale(0.5); double currentError = actualOrientation.distance(mahonyFilter.getEstimatedOrientation());
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout=300000) public void testInterpolate() { Random random = new Random(1776L); YoVariableRegistry registry = new YoVariableRegistry("blop"); FrameQuaternion initialFrameOrientation = EuclidFrameRandomTools.nextFrameQuaternion(random, worldFrame); FrameQuaternion finalFrameOrientation = EuclidFrameRandomTools.nextFrameQuaternion(random, worldFrame); FrameQuaternion interpolatedFrameOrientation = new FrameQuaternion(worldFrame); YoFrameQuaternion initialYoFrameQuaternion = new YoFrameQuaternion("init", worldFrame, registry); initialYoFrameQuaternion.set(initialFrameOrientation); YoFrameQuaternion finalYoFrameQuaternion = new YoFrameQuaternion("final", worldFrame, registry); finalYoFrameQuaternion.set(finalFrameOrientation); YoFrameQuaternion interpolatedYoFrameQuaternion = new YoFrameQuaternion("interpolated", worldFrame, registry); FrameQuaternion temp = new FrameQuaternion(); for (double alpha = -0.1; alpha <= 1.1; alpha += 0.05) { interpolatedFrameOrientation.interpolate(initialFrameOrientation, finalFrameOrientation, alpha); interpolatedYoFrameQuaternion.interpolate(initialYoFrameQuaternion, finalYoFrameQuaternion, alpha); temp.setIncludingFrame(interpolatedYoFrameQuaternion); assertTrue(interpolatedFrameOrientation.epsilonEquals(temp, EPS)); } } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout=300000) public void testMultiplication() { Random random = new Random(1776L); YoVariableRegistry registry = new YoVariableRegistry("blop"); YoFrameQuaternion yoFrameQuaternion = new YoFrameQuaternion("test", worldFrame, registry); Quaternion quat4dActual = new Quaternion(), quat4dExpected = new Quaternion(); Quaternion quat4dA, quat4dB; FrameQuaternion frameOrientation = new FrameQuaternion(worldFrame); for (int i = 0; i < 1000; i++) { quat4dA = RandomGeometry.nextQuaternion(random); quat4dB = RandomGeometry.nextQuaternion(random); quat4dExpected.multiply(quat4dA, quat4dB); yoFrameQuaternion.set(quat4dA); yoFrameQuaternion.multiply(quat4dB); quat4dActual.set(yoFrameQuaternion); assertTrue(RotationTools.quaternionEpsilonEquals(quat4dExpected, quat4dActual, EPS)); yoFrameQuaternion.set(quat4dA); frameOrientation.set(quat4dB); yoFrameQuaternion.multiply(frameOrientation); quat4dActual.set(yoFrameQuaternion); assertTrue(RotationTools.quaternionEpsilonEquals(quat4dExpected, quat4dActual, EPS)); } }
YoFrameQuaternion quaternionForVerification = new YoFrameQuaternion("quaternionForVerification", worldFrame, registry); YoFrameVector3D angularVelocityForVerification = new YoFrameVector3D("angularVelocityForVerification", worldFrame, registry); assertTrue(quaternionForVerification.epsilonEquals(orientation, 1e-10)); assertTrue(angularVelocityForVerification.epsilonEquals(angularVelocity, 1e-10)); quaternionForVerification.setYawPitchRoll(0.2, 0.6, 1.1); angularVelocityForVerification.set(7.1, 2.2, 3.33); assertFalse(quaternionForVerification.epsilonEquals(orientation, 1e-7)); assertFalse(angularVelocityForVerification.epsilonEquals(angularVelocity, 1e-7)); assertTrue(quaternionForVerification.epsilonEquals(orientation, 1e-10)); assertTrue(angularVelocityForVerification.epsilonEquals(angularVelocity, 1e-10));
YoFrameQuaternion yoFrameQuaternion = new YoFrameQuaternion("test", worldFrame, registry); try yoFrameQuaternion.set(frameOrientation); try yoFrameQuaternion.set(frameOrientation);
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout=300000) public void testInitialization() { YoVariableRegistry registry = new YoVariableRegistry("blop"); YoFrameQuaternion yoFrameQuaternion = new YoFrameQuaternion("test", worldFrame, registry); yoFrameQuaternion.checkReferenceFrameMatch(worldFrame); Quaternion quat4dActual = new Quaternion(yoFrameQuaternion); Quaternion quat4dExpected = new Quaternion(0.0, 0.0, 0.0, 1.0); assertTrue(quat4dActual.epsilonEquals(quat4dExpected, EPS)); AxisAngle axisAngle4dActual = new AxisAngle(yoFrameQuaternion); AxisAngle axisAngle4dExpected = new AxisAngle(1.0, 0.0, 0.0, 0.0); assertTrue(axisAngle4dActual.epsilonEquals(axisAngle4dExpected, EPS)); RotationMatrix matrix3dActual = new RotationMatrix(yoFrameQuaternion); RotationMatrix matrix3dExpected = new RotationMatrix(); matrix3dExpected.setIdentity(); assertTrue(matrix3dActual.epsilonEquals(matrix3dExpected, EPS)); FrameQuaternion frameOrientationActual = new FrameQuaternion(yoFrameQuaternion); FrameQuaternion frameOrientationExpected = new FrameQuaternion(worldFrame); assertTrue(frameOrientationActual.epsilonEquals(frameOrientationExpected, EPS)); double[] yawPitchRollActual = new double[3]; yoFrameQuaternion.getYawPitchRoll(yawPitchRollActual); double[] yawPitchRollExpected = new double[3]; assertArrayEquals(yawPitchRollExpected, yawPitchRollActual, EPS); }
public void setInitialOrientation(FrameQuaternionReadOnly initialOrientation) { this.initialOrientation.setMatchingFrame(initialOrientation); }
FrameQuaternion chestOrientation = new FrameQuaternion(chestClone.getBodyFixedFrame()); chestOrientation.changeFrame(ReferenceFrame.getWorldFrame()); currentDesiredOrientation.set(chestOrientation); currentDesiredOrientation.set(desiredOrientation); if (!currentDesiredOrientation.containsNaN() && !previousDesiredOrientation.containsNaN()) previousDesiredOrientation.set(currentDesiredOrientation);
public void initialize() { time.set(0.0); FrameQuaternion orientationToPack = new FrameQuaternion(ReferenceFrame.getWorldFrame()); orientationProvider.getOrientation(orientationToPack); orientationToPack.changeFrame(orientation.getReferenceFrame()); orientation.set(orientationToPack); }
public AlphaFilteredYoFrameQuaternion(String namePrefix, String nameSuffix, YoFrameQuaternion unfilteredQuaternion, DoubleProvider alpha, YoVariableRegistry registry) { this(namePrefix, nameSuffix, unfilteredQuaternion, alpha, unfilteredQuaternion.getReferenceFrame(), registry); }
@Override public void setOrientationToNaN() { orientation.setToNaN(); }
public void setYawPitchRoll(double yaw, double pitch, double roll) { if (isUsingYawPitchRoll()) yoFrameYawPitchRoll.setYawPitchRoll(yaw, pitch, roll); else yoFrameQuaternion.setYawPitchRoll(yaw, pitch, roll); }
protected YoGraphicAbstractShape(String name, YoFramePoint3D framePoint, YoFrameYawPitchRoll frameYawPitchRoll, YoFrameQuaternion frameQuaternion, double scale) { super(name); framePoint.checkReferenceFrameMatch(worldFrame); if ((frameYawPitchRoll == null && frameQuaternion == null) || (frameYawPitchRoll != null && frameQuaternion != null)) throw new IllegalArgumentException("Can only describe the orientation of this shape with either yaw-pitch-roll or quaternion."); if (frameYawPitchRoll != null) frameYawPitchRoll.checkReferenceFrameMatch(worldFrame); if (frameQuaternion != null) frameQuaternion.checkReferenceFrameMatch(worldFrame); yoFramePoint = framePoint; yoFrameYawPitchRoll = frameYawPitchRoll; yoFrameQuaternion = frameQuaternion; this.scale = scale; }
Random random = new Random(3454); ReferenceFrame sensorFrame = EuclidFrameRandomTools.nextReferenceFrame(random); YoFrameQuaternion actualOrientation = new YoFrameQuaternion("q_act_", sensorFrame, registry); actualOrientation.set(EuclidCoreRandomTools.nextQuaternion(random)); Vector3D zUp = new Vector3D(0.0, 0.0, 1.0); actualOrientation.inverseTransform(zUp); inputLinearAcceleration.set(zUp); inputLinearAcceleration.scale(20.0); actualOrientation.inverseTransform(xForward); inputMagneticVector.set(xForward); inputMagneticVector.scale(0.5); currentError = actualOrientation.distance(mahonyFilter.getEstimatedOrientation()); assertTrue(currentError <= previousError); previousError = currentError;