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; }
/** * Computes the angular velocity for an interpolation between two orientations using the SLERP method. * @param startOrientation the starting orientation * @param endOrientation the final orientation * @param alphaDot the interpolation rate * @return the angular velocity of the interpolated frame, w.r.t. the startOrientation, expressed in the frame in which the orientations were expressed */ public void computeAngularVelocity(YoFrameVector3D angularVelocityToPack, YoFrameQuaternion startOrientation, YoFrameQuaternion endOrientation, double alphaDot) { angularVelocityToPack.checkReferenceFrameMatch(startOrientation); startOrientation.checkReferenceFrameMatch(endOrientation); startRotationQuaternion.set(startOrientation); endRotationQuaternion.set(endOrientation); computeAngularVelocity(angularVelocity, startRotationQuaternion, endRotationQuaternion, alphaDot); angularVelocityToPack.set(angularVelocity); }
public YoIMUMahonyFilter(String imuName, String nameSuffix, double updateDT, ReferenceFrame sensorFrame, YoFrameQuaternion estimatedOrientation, YoFrameVector3D estimatedAngularVelocity, YoVariableRegistry parentRegistry) { this.updateDT = updateDT; this.sensorFrame = sensorFrame; YoVariableRegistry registry = new YoVariableRegistry(imuName + "MahonyFilter"); parentRegistry.addChild(registry); estimatedOrientation.checkReferenceFrameMatch(sensorFrame); if (estimatedAngularVelocity != null) estimatedAngularVelocity.checkReferenceFrameMatch(sensorFrame); this.estimatedOrientation = estimatedOrientation; this.estimatedAngularVelocity = estimatedAngularVelocity; 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); }
@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); }