- Add the Codota plugin to your IDE and get smart completions
private void myMethod () {Connection c =
DataSource dataSource;dataSource.getConnection()
- Smart code suggestions by Codota
}
quaternion.getRotationVector(normal); orthogonal = EuclidCoreRandomTools.nextOrthogonalVector3D(random, normal, true); normal.applyTransform(new RigidBodyTransform(new AxisAngle(orthogonal, EPSILON), new Vector3D()));
@Override protected void updateTwistRelativeToParent(Twist twistRelativeToParentToPack) { RigidBodyTransform transformToRoot = originalFrame.getTransformToRoot(); if (previousRotation.containsNaN() || previousTranslation.containsNaN()) { transformToRoot.get(previousRotation, previousTranslation); angularVelocity.setToZero(); linearVelocity.setToZero(); } else { transformToRoot.get(rotation, translation); rotation.multiplyConjugateOther(previousRotation); rotation.getRotationVector(angularVelocity); angularVelocity.scale(1.0 / updateDT); linearVelocity.sub(translation, previousTranslation); linearVelocity.scale(1.0 / updateDT); transformToRoot.get(previousRotation, previousTranslation); transformToRoot.inverseTransform(angularVelocity); transformToRoot.inverseTransform(linearVelocity); } twistRelativeToParentToPack.setIncludingFrame(this, getParent(), this, angularVelocity, linearVelocity); } }
difference.getRotationVector(limitedRotationVector); boolean clipped = limitedRotationVector.clipToMaxLength(dt * maxRateVariable.getValue()); limited.set(clipped);
difference.difference(pose.getOrientation(), quaternionFuture); angularVelocity.setToZero(this); difference.getRotationVector(angularVelocity); angularVelocity.scale(1.0 / integrationDT);
public void update(Vector3DReadOnly inputAngularVelocity, Vector3DReadOnly inputLinearAcceleration, Vector3DReadOnly inputMagneticVector) { if (!hasBeenInitialized.getValue()) { initialize(inputLinearAcceleration, inputMagneticVector); return; } boolean success = computeOrientationError((QuaternionReadOnly) estimatedOrientation, inputLinearAcceleration, inputMagneticVector, quaternionUpdate); if (success) { quaternionUpdate.getRotationVector(totalError); yoErrorTerm.set(totalError); integralTerm.scaleAdd(integralGain.getValue() * updateDT, yoErrorTerm, yoIntegralTerm); yoIntegralTerm.set(integralTerm); angularVelocityTerm.scaleAdd(proportionalGain.getValue(), totalError, inputAngularVelocity); angularVelocityTerm.add(integralTerm); } else { yoErrorTerm.setToZero(); angularVelocityTerm.set(inputAngularVelocity); } rotationUpdate.setAndScale(updateDT, angularVelocityTerm); quaternionUpdate.setRotationVector(rotationUpdate); estimatedOrientation.multiply(quaternionUpdate); if (estimatedAngularVelocity != null) estimatedAngularVelocity.add(inputAngularVelocity, integralTerm); }
difference.getRotationVector(limitedRotationVector); boolean clipped = limitedRotationVector.clipToMaxLength(dt * maxRateVariable.getValue()); limited.set(clipped);
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testComputeAngularVelocityInBodyFrameFromYawPitchRollAnglesRate() throws Exception { double dt = 1.0e-8; for (int i = 0; i < 1000; i++) { double yaw = RandomNumbers.nextDouble(random, Math.PI); double pitch = RandomNumbers.nextDouble(random, Math.PI / 2.0); double roll = RandomNumbers.nextDouble(random, Math.PI); double yawRate = RandomNumbers.nextDouble(random, 1.0); double pitchRate = RandomNumbers.nextDouble(random, 1.0); double rollRate = RandomNumbers.nextDouble(random, 1.0); double previousYaw = yaw - yawRate * dt; double previousPitch = pitch - pitchRate * dt; double previousRoll = roll - rollRate * dt; Quaternion rotation = new Quaternion(); rotation.setYawPitchRoll(yaw, pitch, roll); Quaternion previousRotation = new Quaternion(); previousRotation.setYawPitchRoll(previousYaw, previousPitch, previousRoll); Vector3D expectedAngularVelocity = new Vector3D(); Quaternion difference = new Quaternion(); difference.difference(previousRotation, rotation); difference.getRotationVector(expectedAngularVelocity); expectedAngularVelocity.scale(1.0 / dt); Vector3D actualAngularVelocity = new Vector3D(); RotationTools.computeAngularVelocityInBodyFrameFromYawPitchRollAnglesRate(yaw, pitch, roll, yawRate, pitchRate, rollRate, actualAngularVelocity); EuclidCoreTestTools.assertTuple3DEquals(expectedAngularVelocity, actualAngularVelocity, 1.0e-7); } }
quaternion.getRotationVector(rotationVector); actualTransform.setRotation(rotationVector); EuclidCoreTestTools.assertQuaternionEquals(quaternion, actualTransform.getQuaternion(), EPS);