private void computeBezierQuaternionCurveTerm(int i, Quat4d resultToPack) { double cumulativeBernsteinCoefficient = cumulativeBeziers[i].getDoubleValue(); controlAngularVelocities[i].get(tempAngularVelocity); tempAngularVelocity.scale(cumulativeBernsteinCoefficient); quaternionCalculus.exp(tempAngularVelocity, resultToPack); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testLogAndExpAlgebra() throws Exception { Random random = new Random(651651961L); for (int i = 0; i < 10000; i++) { QuaternionCalculus quaternionCalculus = new QuaternionCalculus(); Quaternion q = RandomGeometry.nextQuaternion(random); Vector4D qLog = new Vector4D(); Quaternion vExp = new Quaternion(); quaternionCalculus.log(q, qLog); Vector3D v = new Vector3D(qLog.getX(),qLog.getY(),qLog.getZ()); quaternionCalculus.exp(v, vExp); assertTrue(Math.abs(q.getX() - vExp.getX()) < 10e-10); assertTrue(Math.abs(q.getY() - vExp.getY()) < 10e-10); assertTrue(Math.abs(q.getZ() - vExp.getZ()) < 10e-10); assertTrue(Math.abs(q.getS() - vExp.getS()) < 10e-10); } }
tempAngularVelocity.scale(trajectoryTime.getDoubleValue()); tempAngularVelocity.scale(1 / 3.0); quaternionCalculus.exp(tempAngularVelocity, tempQuatForControlQuats); tempAngularVelocity.scale(1 / 3.0); tempAngularVelocity.negate(); quaternionCalculus.exp(tempAngularVelocity, tempQuatForControlQuats); tempControlQuaternions[2].mul(tempQuatForControlQuats);
angularVelocity.scale(maximumSixDoFJointAngularVelocity / angularVelocityMagnitude); rotationVectorIntegrated.scale(controlDT, angularVelocity); quaternionCalculus.exp(rotationVectorIntegrated, rotationIntegrated);