public YoQuaternionProvider(String namePrefix, ReferenceFrame referenceFrame, YoVariableRegistry registry) { orientation = new YoFrameQuaternion(namePrefix + "Orientation", referenceFrame, registry); }
public YoSE3ConfigurationProvider(String name, ReferenceFrame frame, YoVariableRegistry registry) { position = new YoFramePoint3D(name, frame, registry); orientation = new YoFrameQuaternion(name, frame, registry); }
/** * Creates a new {@code YoFramePose3D}. * * @param namePrefix a unique name string to use as the prefix for child variable names. * @param nameSuffix a string to use as the suffix for child variable names. * @param referenceFrame the reference frame for this pose. * @param registry the registry to register child variables to. */ public YoFramePose3D(String namePrefix, String nameSuffix, ReferenceFrame referenceFrame, YoVariableRegistry registry) { position = new YoFramePoint3D(namePrefix, nameSuffix, referenceFrame, registry); orientation = new YoFrameQuaternion(namePrefix, nameSuffix, referenceFrame, registry); }
private FiniteDifferenceAngularVelocityYoFrameVector(String namePrefix, YoFrameQuaternion orientationToDifferentiate, ReferenceFrame referenceFrame, double dt, YoVariableRegistry registry) { super(namePrefix, referenceFrame, registry); this.dt = dt; orientation = orientationToDifferentiate; orientationPreviousValue = new YoFrameQuaternion(namePrefix + "_previous", referenceFrame, registry); hasBeenCalled = new YoBoolean(namePrefix + "HasBeenCalled", registry); hasBeenCalled.set(false); }
public YoGraphicPolygon(String name, YoFrameConvexPolygon2D convexPolygon2d, String namePrefix, String nameSuffix, YoVariableRegistry registry, boolean useYawPitchRoll, double scale, AppearanceDefinition appearance) { this(name, convexPolygon2d, new YoFramePoint3D(namePrefix, nameSuffix, worldFrame, registry), useYawPitchRoll ? new YoFrameYawPitchRoll(namePrefix, nameSuffix, worldFrame, registry) : null, useYawPitchRoll ? null : new YoFrameQuaternion(namePrefix, nameSuffix, worldFrame, registry), scale, DEFAULT_HEIGHT, appearance); }
public YoGraphicPolygon(String name, YoFrameConvexPolygon2D convexPolygon2d, YoVariableRegistry registry, boolean useYawPitchRoll, double scale, AppearanceDefinition appearance) { this(name, convexPolygon2d, new YoFramePoint3D(name + "Position", worldFrame, registry), useYawPitchRoll ? new YoFrameYawPitchRoll(name + "Orientation", worldFrame, registry) : null, useYawPitchRoll ? null : new YoFrameQuaternion(name + "Orientation", worldFrame, registry), scale, DEFAULT_HEIGHT, appearance); }
public YoGraphicCoordinateSystem(String namePrefix, String nameSuffix, YoVariableRegistry registry, boolean useYawPitchRoll, double scale, AppearanceDefinition arrowAppearance) { this(namePrefix + nameSuffix, new YoFramePoint3D(namePrefix, nameSuffix, worldFrame, registry), useYawPitchRoll ? new YoFrameYawPitchRoll(namePrefix, nameSuffix, worldFrame, registry) : null, useYawPitchRoll ? null : new YoFrameQuaternion(namePrefix, nameSuffix, worldFrame, registry), scale, arrowAppearance); }
private AlphaFilteredYoFrameQuaternion createAlphaFilteredYoFrameQuaternion(DoubleProvider alpha) { YoVariableRegistry registry = new YoVariableRegistry("test"); ReferenceFrame referenceFrame = ReferenceFrame.getWorldFrame(); YoFrameQuaternion unfilteredQuaternion = new YoFrameQuaternion("qMeasured", referenceFrame, registry); AlphaFilteredYoFrameQuaternion q = new AlphaFilteredYoFrameQuaternion("qFiltered", "", unfilteredQuaternion, alpha, registry); return q; }
public ConstantOrientationTrajectoryGenerator(String namePrefix, ReferenceFrame referenceFrame, OrientationProvider orientationProvider, double finalTime, YoVariableRegistry parentRegistry) { MathTools.checkIntervalContains(finalTime, 0.0, Double.POSITIVE_INFINITY); this.registry = new YoVariableRegistry(namePrefix + getClass().getSimpleName()); this.orientationProvider = orientationProvider; this.orientation = new YoFrameQuaternion("orientation", referenceFrame, registry); this.finalTime = new YoDouble("finalTime", registry); this.time = new YoDouble("time", registry); this.finalTime.set(finalTime); parentRegistry.addChild(registry); }
public OrientationInterpolationTrajectoryGenerator(String namePrefix, ReferenceFrame referenceFrame, DoubleProvider trajectoryTimeProvider, OrientationProvider initialOrientationProvider, OrientationProvider finalOrientationProvider, YoVariableRegistry parentRegistry) { this.registry = new YoVariableRegistry(namePrefix + getClass().getSimpleName()); this.trajectoryTime = new YoDouble(namePrefix + "TrajectoryTime", registry); this.currentTime = new YoDouble(namePrefix + "Time", registry); this.parameterPolynomial = new YoPolynomial(namePrefix + "ParameterPolynomial", 6, registry); this.initialOrientation = new YoFrameQuaternion(namePrefix + "InitialOrientation", referenceFrame, registry); this.finalOrientation = new YoFrameQuaternion(namePrefix + "FinalOrientation", referenceFrame, registry); this.continuouslyUpdateFinalOrientation = new YoBoolean(namePrefix + "ContinuouslyUpdate", registry); this.desiredOrientation = new YoFrameQuaternion(namePrefix + "desiredOrientation", referenceFrame, registry); this.desiredAngularVelocity = new YoFrameVector3D(namePrefix + "desiredAngularVelocity", referenceFrame, registry); this.desiredAngularAcceleration = new YoFrameVector3D(namePrefix + "desiredAngularAcceleration", referenceFrame, registry); this.trajectoryTimeProvider = trajectoryTimeProvider; this.initialOrientationProvider = initialOrientationProvider; this.finalOrientationProvider = finalOrientationProvider; tempInitialOrientation = new FrameQuaternion(referenceFrame); tempFinalOrientation = new FrameQuaternion(referenceFrame); parentRegistry.addChild(registry); }
@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)); } } }
private static YoFrameQuaternion findOrientationDesired(SimulationConstructionSet scs, RigidBodyBasics body) { String bodyName = body.getName(); String namespace = "FeedbackControllerToolbox"; YoDouble qx = getDoubleYoVariable(scs, bodyName + "DesiredOrientationQx", namespace); YoDouble qy = getDoubleYoVariable(scs, bodyName + "DesiredOrientationQy", namespace); YoDouble qz = getDoubleYoVariable(scs, bodyName + "DesiredOrientationQz", namespace); YoDouble qs = getDoubleYoVariable(scs, bodyName + "DesiredOrientationQs", namespace); return new YoFrameQuaternion(qx, qy, qz, qs, ReferenceFrame.getWorldFrame()); }
public YoFrameQuaternion findYoFrameQuaternion(String quaternionPrefix, String quaternionSuffix, ReferenceFrame quaternionFrame) { YoDouble qx = (YoDouble) scs.getVariable(createQxName(quaternionPrefix, quaternionSuffix)); YoDouble qy = (YoDouble) scs.getVariable(createQyName(quaternionPrefix, quaternionSuffix)); YoDouble qz = (YoDouble) scs.getVariable(createQzName(quaternionPrefix, quaternionSuffix)); YoDouble qs = (YoDouble) scs.getVariable(createQsName(quaternionPrefix, quaternionSuffix)); if (qx == null || qy == null || qz == null || qs == null) return null; else return new YoFrameQuaternion(qx, qy, qz, qs, quaternionFrame); } }
@Before public void setUp() { RigidBodyTransform transformToParent = new RigidBodyTransform(); // referenceFrame = ReferenceFrame.constructARootFrame("rootFrame"); referenceFrame = ReferenceFrame.constructFrameWithUnchangingTransformToParent("referenceFrame", rootFrame1, transformToParent); positionYoFramePoint = new YoFramePoint3D("prefixTEST", referenceFrame, registry); orientationQuaternion = new YoFrameQuaternion("orientationPrefix", referenceFrame, registry); frame2 = ReferenceFrame.constructFrameWithUnchangingTransformToParent("frame2", rootFrame1, transformToParent); positionMultipleFrames = new YoFramePointInMultipleFrames("positionMultipleFrames", registry, rootFrame1, frame2); orientationMultipleFrames = new YoFrameQuaternionInMultipleFrames("orientationMultipleFrames", registry, rootFrame1, frame2); generator = new ConstantPoseTrajectoryGenerator(positionYoFramePoint, orientationQuaternion); }
public YoIMUMahonyFilter(String imuName, String namePrefix, String nameSuffix, double updateDT, ReferenceFrame sensorFrame, YoVariableRegistry parentRegistry) { this.updateDT = updateDT; this.sensorFrame = sensorFrame; YoVariableRegistry registry = new YoVariableRegistry(imuName + "MahonyFilter"); parentRegistry.addChild(registry); estimatedOrientation = new YoFrameQuaternion(namePrefix, nameSuffix, sensorFrame, registry); estimatedAngularVelocity = new YoFrameVector3D(namePrefix, nameSuffix, sensorFrame, registry); yoErrorTerm = new YoFrameVector3D("ErrorTerm", nameSuffix, sensorFrame, registry); yoIntegralTerm = new YoFrameVector3D("IntegralTerm", nameSuffix, sensorFrame, registry); proportionalGain = new YoDouble("ProportionalGain" + nameSuffix, registry); integralGain = new YoDouble("IntegralGain" + nameSuffix, registry); hasBeenInitialized = new YoBoolean("HasBeenInitialized" + nameSuffix, registry); }
YoFrameQuaternion yoFrameQuaternion = new YoFrameQuaternion("test", worldFrame, registry);
/** * 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()); }
private static YoFramePose3D findYoFramePose3D(String nameSpace, String namePrefix, String nameSuffix, SimulationConstructionSet scs) { YoDouble x = (YoDouble) scs.getVariable(nameSpace, YoFrameVariableNameTools.createXName(namePrefix, nameSuffix)); YoDouble y = (YoDouble) scs.getVariable(nameSpace, YoFrameVariableNameTools.createYName(namePrefix, nameSuffix)); YoDouble z = (YoDouble) scs.getVariable(nameSpace, YoFrameVariableNameTools.createZName(namePrefix, nameSuffix)); YoFramePoint3D position = new YoFramePoint3D(x, y, z, ReferenceFrame.getWorldFrame()); YoDouble qx = (YoDouble) scs.getVariable(nameSpace, YoFrameVariableNameTools.createQxName(namePrefix, nameSuffix)); YoDouble qy = (YoDouble) scs.getVariable(nameSpace, YoFrameVariableNameTools.createQyName(namePrefix, nameSuffix)); YoDouble qz = (YoDouble) scs.getVariable(nameSpace, YoFrameVariableNameTools.createQzName(namePrefix, nameSuffix)); YoDouble qs = (YoDouble) scs.getVariable(nameSpace, YoFrameVariableNameTools.createQsName(namePrefix, nameSuffix)); YoFrameQuaternion orientation = new YoFrameQuaternion(qx, qy, qz, qs, ReferenceFrame.getWorldFrame()); return new YoFramePose3D(position, orientation); }
@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)); } }
@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); }