public static void integrateAngularVelocity(Vector3d angularVelocityToIntegrate, double integrationTime, Quat4d orientationResultToPack) { AxisAngle4d axisAngleResult = axisAngleForIntegrator.get(); integrateAngularVelocity(angularVelocityToIntegrate, integrationTime, axisAngleResult); orientationResultToPack.set(axisAngleResult); }
public static void integrateAngularVelocity(Vector3DReadOnly angularVelocityToIntegrate, double integrationTime, QuaternionBasics orientationResultToPack) { AxisAngle axisAngleResult = axisAngleForIntegrator.get(); integrateAngularVelocity(angularVelocityToIntegrate, integrationTime, axisAngleResult); orientationResultToPack.set(axisAngleResult); }
public static void integrateAngularVelocity(Vector3d angularVelocityToIntegrate, double integrationTime, Matrix3d orientationResultToPack) { AxisAngle4d axisAngleResult = axisAngleForIntegrator.get(); integrateAngularVelocity(angularVelocityToIntegrate, integrationTime, axisAngleResult); orientationResultToPack.set(axisAngleResult); }
public static void integrateAngularVelocity(Vector3DReadOnly angularVelocityToIntegrate, double integrationTime, RotationMatrix orientationResultToPack) { AxisAngle axisAngleResult = axisAngleForIntegrator.get(); integrateAngularVelocity(angularVelocityToIntegrate, integrationTime, axisAngleResult); orientationResultToPack.set(axisAngleResult); }
private void computeDriftSaturated(double time, double alphaSaturation, YoFrameVector angularVelocity, DoubleYoVariable angularVelocityMagnitude, Quat4d driftToPack) { angularVelocity.get(tempAngularVelocityForDrift); double maxTime = Math.signum(time) * PI / angularVelocityMagnitude.getDoubleValue() - 1.0e-5; time = alphaSaturation * maxTime + (1.0 - alphaSaturation) * time; RotationTools.integrateAngularVelocity(tempAngularVelocityForDrift, time, driftToPack); }
private void computeDriftSaturated(double time, double alphaSaturation, YoFrameVector3D angularVelocity, YoDouble angularVelocityMagnitude, Quaternion driftToPack) { tempAngularVelocityForDrift.set(angularVelocity); double maxTime = Math.signum(time) * PI / angularVelocityMagnitude.getDoubleValue() - 1.0e-5; time = alphaSaturation * maxTime + (1.0 - alphaSaturation) * time; RotationTools.integrateAngularVelocity(tempAngularVelocityForDrift, time, driftToPack); }
public static void integrateAngularVelocity(FrameVector3D angularVelocityToIntegrate, double integrationTime, FrameQuaternion orientationResultToPack) { AxisAngle axisAngleResult = axisAngleForIntegrator.get(); integrateAngularVelocity(angularVelocityToIntegrate, integrationTime, axisAngleResult); orientationResultToPack.setIncludingFrame(angularVelocityToIntegrate.getReferenceFrame(), axisAngleResult); }
private void computeDrift(double time, double alphaDecay, YoFrameVector angularVelocity, Quat4d driftToPack) { angularVelocity.get(tempAngularVelocityForDrift); tempAngularVelocity.scale(alphaDecay); RotationTools.integrateAngularVelocity(tempAngularVelocityForDrift, time, driftToPack); }
private void computeDrift(double time, double alphaDecay, YoFrameVector3D angularVelocity, Quaternion driftToPack) { tempAngularVelocityForDrift.set(angularVelocity); tempAngularVelocity.scale(alphaDecay); RotationTools.integrateAngularVelocity(tempAngularVelocityForDrift, time, driftToPack); }
public static void integrateAngularVelocity(FrameVector angularVelocityToIntegrate, double integrationTime, FrameOrientation orientationResultToPack) { AxisAngle4d axisAngleResult = axisAngleForIntegrator.get(); integrateAngularVelocity(angularVelocityToIntegrate.getVector(), integrationTime, axisAngleResult); orientationResultToPack.setIncludingFrame(angularVelocityToIntegrate.getReferenceFrame(), axisAngleResult); }
@ContinuousIntegrationTest(estimatedDuration = 0.4) @Test(timeout = 30000) public void testIntegrateToQuaternion() throws Exception { for (int i = 0; i < 100; i++) { Vector3D expectedAngularVelocity = RandomGeometry.nextVector3D(random); Vector3D actualAngularVelocity = new Vector3D(); Quaternion integrationResultPrevious = new Quaternion(); Quaternion integrationResultCurrent = new Quaternion(); Quaternion integrationResultNext = new Quaternion(); Vector4D qDot = new Vector4D(); QuaternionCalculus quaternionCalculus = new QuaternionCalculus(); double dt = 1.0e-4; for (double t = dt; t <= 1.0; t += dt) { RotationTools.integrateAngularVelocity(expectedAngularVelocity, t - dt, integrationResultPrevious); RotationTools.integrateAngularVelocity(expectedAngularVelocity, t, integrationResultCurrent); RotationTools.integrateAngularVelocity(expectedAngularVelocity, t + dt, integrationResultNext); quaternionCalculus.computeQDotByFiniteDifferenceCentral(integrationResultPrevious, integrationResultNext, dt, qDot); quaternionCalculus.computeAngularVelocityInWorldFrame(integrationResultCurrent, qDot, actualAngularVelocity); assertTrue(expectedAngularVelocity.epsilonEquals(actualAngularVelocity, 1.0e-7)); } } }
RotationTools.integrateAngularVelocity(velIn1, time, m21); RotationTools.integrateAngularVelocity(velIn2, time, m32);
RotationTools.integrateAngularVelocity(angularVelocityVector, dt, integratedAngularVelocity); quaternionFromIntegration.set(orientationFromIntegration); quaternionFromIntegration.multiply(integratedAngularVelocity, quaternionFromIntegration);
RotationTools.integrateAngularVelocity(angularVelocityVector, dt, integratedAngularVelocity); quaternionFromIntegration.set(orientationFromIntegration); quaternionFromIntegration.multiply(integratedAngularVelocity, quaternionFromIntegration);
RotationTools.integrateAngularVelocity(angularVelocityVector, dt, integratedAngularVelocity); quaternionFromIntegration.set(orientationFromIntegration); quaternionFromIntegration.multiply(integratedAngularVelocity, quaternionFromIntegration);
newLinearVelocity.add(linearVelocity); RotationTools.integrateAngularVelocity(angularVelocity, controlDT, newOrientation); newOrientation.preMultiply(orientation); newAngularVelocity.set(desiredAngularAcceleration);