How to use

getX

method

in

javax.vecmath.AxisAngle4d

/**
* Checks the validity of a {@link AxisAngle4d}
* @param axisAngleToCheck
* @return null if the axisAngle4d is valid, or the error message.
*/
public static ObjectErrorType validateAxisAngle4d(AxisAngle4d axisAngleToCheck)
{
 if (axisAngleToCheck == null)
   return ObjectErrorType.NULL;
 if (isNaN(axisAngleToCheck.getX()) || isNaN(axisAngleToCheck.getY()) || isNaN(axisAngleToCheck.getZ()) || isNaN(axisAngleToCheck.getAngle()))
   return ObjectErrorType.CONTAINS_NaN;
 if (isInfinite(axisAngleToCheck.getX()) || isInfinite(axisAngleToCheck.getY()) || isInfinite(axisAngleToCheck.getZ())
    || isInfinite(axisAngleToCheck.getAngle()))
   return ObjectErrorType.CONTAINS_INFINITY;
 return null;
}

/**
* Rotates the given {@code tupleOriginal} tuple by a axis-angle (ux, uy, uz, angle) and stores the result in the tuple {@code tupleTransformed}.
* 
* @param axisAngle the axis-angle used to rotate the tuple.
* @param tupleOriginal the original tuple. Not modified.
* @param tupleTransformed the tuple in which the transformed {@code original} is stored. Modified.
*/
public static void rotateTuple3d(AxisAngle4d axisAngle, Tuple3d tupleOriginal, Tuple3d tupleTransformed)
{
 double uNorm = Math.sqrt(axisAngle.getX() * axisAngle.getX() + axisAngle.getY() * axisAngle.getY() + axisAngle.getZ() * axisAngle.getZ());
 if (uNorm < EPSILON)
 {
   return;
 }
 else
 {
   double halfTheta = 0.5 * axisAngle.getAngle();
   double cosHalfTheta = Math.cos(halfTheta);
   double sinHalfTheta = Math.sin(halfTheta) / uNorm;
   rotateTuple3d(axisAngle.getX() * sinHalfTheta, axisAngle.getY() * sinHalfTheta, axisAngle.getZ() * sinHalfTheta, cosHalfTheta, tupleOriginal, tupleTransformed);
 }
}

/**
* Convert AxisAngle representation to rotation matrix and store as
* rotational component of this transform.
*
* @param axisAngle
*/
public void setRotation(AxisAngle4d axisAngle)
{
 setRotationWithAxisAngle(axisAngle.getX(), axisAngle.getY(), axisAngle.getZ(), axisAngle.getAngle());
}

public static void convertQuaternionToRotationVector(Quat4d quaternion, Vector3d rotationVectorToPack)
{
 AxisAngle4d axisAngle = axisAngleForRotationVectorConvertor.get();
 axisAngle.set(quaternion);
 rotationVectorToPack.set(axisAngle.getX(), axisAngle.getY(), axisAngle.getZ());
 rotationVectorToPack.scale(axisAngle.getAngle());
}

public void setOrientation(AxisAngle4d orientation)
{
 rotationAngleDecoupled = orientation.getAngle();
 rotationAxisDecoupled = new Point3D(orientation.getX(), orientation.getY(), orientation.getZ());
 setOrientation(rotationAxisDecoupled, rotationAngleDecoupled);
}

/**
* This computes: resultToPack = log(q)
* @param q is a unit quaternion and describes a orientation.
* @param resultToPack is used to store the result and is a pure quaternion (w = 0.0).
*/
public void log(Quat4d q, Quat4d resultToPack)
{
 axisAngleForLog.set(q);
 resultToPack.set(axisAngleForLog.getX(), axisAngleForLog.getY(), axisAngleForLog.getZ(), 0.0);
 resultToPack.scale(axisAngleForLog.getAngle());
}

public double getAxisAngleRotationToOtherPose(FramePose otherPose, FrameVector rotationAxisToPack)
{
 AxisAngle4d rotationAxisAngle = new AxisAngle4d();
 getAxisAngleRotationToOtherPose(otherPose, rotationAxisAngle);
 rotationAxisToPack.setIncludingFrame(this.referenceFrame, rotationAxisAngle.getX(), rotationAxisAngle.getY(), rotationAxisAngle.getZ());
 return rotationAxisAngle.getAngle();
}

/**
* This computes: resultToPack = log(q)
* @param q is a unit quaternion and describes a orientation.
* @param resultToPack is used to store the result.
*/
public void log(Quat4d q, Vector3d resultToPack)
{
 axisAngleForLog.set(q);
 resultToPack.set(axisAngleForLog.getX(), axisAngleForLog.getY(), axisAngleForLog.getZ());
 resultToPack.scale(axisAngleForLog.getAngle());
}

  public static boolean axisAngleEpsilonEqualsIgnoreFlippedAxes(AxisAngle4d original, AxisAngle4d result, double epsilon)
  {
   if (original.epsilonEquals(result, epsilon))
   {
     return true;
   }
   else
   {
     AxisAngle4d originalNegated = originalAxisAngleForEpsilonEquals.get();
     originalNegated.set(original);
     originalNegated.setAngle(originalNegated.getAngle() * -1.0);
     originalNegated.setX(originalNegated.getX() * -1.0);
     originalNegated.setY(originalNegated.getY() * -1.0);
     originalNegated.setZ(originalNegated.getZ() * -1.0);

     return originalNegated.epsilonEquals(result, epsilon);
   }
  }
}

private DenseMatrix64F getSpatialErrorEndEffectorDesired()
{
 transformEndEffectorToDesired.getTranslation(errorTranslationVector);
 transformEndEffectorToDesired.getRotation(errorQuat);
 errorAxisAngle.set(errorQuat);
 errorAngularVector.set(errorAxisAngle.getX(), errorAxisAngle.getY(), errorAxisAngle.getZ());
 errorAngularVector.scale(errorAxisAngle.getAngle());
 MatrixTools.setDenseMatrixFromTuple3d(spatialError, errorAngularVector, 0, 0);
 MatrixTools.setDenseMatrixFromTuple3d(spatialError, errorTranslationVector, 3, 0);
 return spatialError;
}

private void updateOrientationVisualization()
{
 desiredSpatialAcceleration.getAngularPart(desiredAngularAcceleration);
 yoDesiredAngularAcceleration.setAndMatchFrame(desiredAngularAcceleration);
 tempOrientation.setToZero(accelerationControlModule.getTrackingFrame());
 yoCurrentOrientation.setAndMatchFrame(tempOrientation);
 accelerationControlModule.getEndEffectorCurrentAngularVelocity(tempAngularVelocity);
 yoCurrentAngularVelocity.setAndMatchFrame(tempAngularVelocity);
 yoDesiredOrientation.get(tempAxisAngle);
 yoDesiredRotationVector.set(tempAxisAngle.getX(), tempAxisAngle.getY(), tempAxisAngle.getZ());
 yoDesiredRotationVector.scale(tempAxisAngle.getAngle());
 yoCurrentOrientation.get(tempAxisAngle);
 yoCurrentRotationVector.set(tempAxisAngle.getX(), tempAxisAngle.getY(), tempAxisAngle.getZ());
 yoCurrentRotationVector.scale(tempAxisAngle.getAngle());
}

public FrameVector computeDesiredAngularVelocity(FrameOrientation desiredOrientation, ReferenceFrame controlFrame, MutableDouble errorMagnitude)
{
 FrameVector ret = new FrameVector();
 errorFrameOrientation.setIncludingFrame(desiredOrientation);
 errorFrameOrientation.changeFrame(controlFrame);
 errorFrameOrientation.getAxisAngle(errorAxisAngle);
 errorRotation.set(errorAxisAngle.getX(), errorAxisAngle.getY(), errorAxisAngle.getZ());
 errorRotation.scale(errorAxisAngle.getAngle());
 if (errorMagnitude != null)
   errorMagnitude.setValue(errorRotation.length());
 errorRotation.scale(1.0 / updateDT);
 ret.setIncludingFrame(controlFrame, errorRotation);
 return ret;
}

private void computeProportionalTerm(FrameOrientation desiredOrientation)
{
 desiredOrientation.changeFrame(bodyFrame);
 desiredOrientation.getQuaternion(errorQuaternion);
 errorAngleAxis.set(errorQuaternion);
 errorAngleAxis.setAngle(AngleTools.trimAngleMinusPiToPi(errorAngleAxis.getAngle()));
 // Limit the maximum position error considered for control action
 double maximumError = gains.getMaximumProportionalError();
 if (errorAngleAxis.getAngle() > maximumError)
 {
   errorAngleAxis.setAngle(Math.signum(errorAngleAxis.getAngle()) * maximumError);
 }
 proportionalTerm.set(errorAngleAxis.getX(), errorAngleAxis.getY(), errorAngleAxis.getZ());
 proportionalTerm.scale(errorAngleAxis.getAngle());
 rotationErrorInBody.set(proportionalTerm);
 proportionalGainMatrix.transform(proportionalTerm.getVector());
}

public FrameVector computeDesiredAngularVelocity(FrameOrientation desiredOrientation, ReferenceFrame controlFrame, MutableDouble errorMagnitude)
{
 FrameVector ret = new FrameVector();
 errorFrameOrientation.setIncludingFrame(desiredOrientation);
 errorFrameOrientation.changeFrame(controlFrame);
 errorFrameOrientation.getAxisAngle(errorAxisAngle);
 errorRotation.set(errorAxisAngle.getX(), errorAxisAngle.getY(), errorAxisAngle.getZ());
 errorRotation.scale(AngleTools.trimAngleMinusPiToPi(errorAxisAngle.getAngle()));
 if (errorMagnitude != null)
   errorMagnitude.setValue(errorRotation.length());
 errorRotation.scale(1.0 / updateDT);
 ret.setIncludingFrame(controlFrame, errorRotation);
 return ret;
}

  public void update(Matrix3d rotationMatrix)
  {
   if (!hasBeenCalled.getBooleanValue())
   {
     orientationPreviousValue.set(rotationMatrix);
     hasBeenCalled.set(true);
   }

   if (rotationMatrix != currentOrientationMatrix)
     currentOrientationMatrix.set(rotationMatrix);
   orientationPreviousValue.get(previousOrientationMatrix);
   deltaOrientationMatrix.mulTransposeRight(currentOrientationMatrix, previousOrientationMatrix);
   deltaAxisAngle.set(deltaOrientationMatrix);

   set(deltaAxisAngle.getX(), deltaAxisAngle.getY(), deltaAxisAngle.getZ());
   scale(deltaAxisAngle.getAngle() / dt);

   orientationPreviousValue.set(currentOrientationMatrix);
  }
}

public double getNormRotationError(FrameOrientation desiredOrientation)
{
 tempOrientation.setIncludingFrame(desiredOrientation);
 tempOrientation.changeFrame(localControlFrame);
 tempOrientation.getAxisAngle(errorAxisAngle);
 errorRotationVector.set(errorAxisAngle.getX(), errorAxisAngle.getY(), errorAxisAngle.getZ());
 errorRotationVector.scale(errorAxisAngle.getAngle());
 DenseMatrix64F selectionMatrix = inverseJacobianSolver.getSelectionMatrix();
 tempSpatialError.reshape(SpatialMotionVector.SIZE, 1);
 tempSubspaceError.reshape(selectionMatrix.getNumRows(), 1);
 MatrixTools.insertTuple3dIntoEJMLVector(errorRotationVector, tempSpatialError, 0);
 CommonOps.mult(selectionMatrix, tempSpatialError, tempSubspaceError);
 return NormOps.normP2(tempSubspaceError);
}

private void computeError(RigidBodyTransform desiredTransform)
{
 jacobian.compute();
 jacobian.getEndEffectorFrame().getTransformToDesiredFrame(actualTransform, jacobian.getBaseFrame());
 errorTransform.invert(desiredTransform);
 errorTransform.multiply(actualTransform);
 errorTransform.getRotation(errorRotationMatrix);
 RotationTools.convertMatrixToAxisAngle(errorRotationMatrix, errorAxisAngle);
 axis.set(errorAxisAngle.getX(), errorAxisAngle.getY(), errorAxisAngle.getZ());
 errorRotationVector.set(axis);
 errorRotationVector.scale(errorAxisAngle.getAngle());
 errorRotationVector.scale(0.2);
 
 errorTransform.getTranslation(errorTranslationVector);
 
 MatrixTools.setDenseMatrixFromTuple3d(spatialError, errorRotationVector, 0, 0);
 MatrixTools.setDenseMatrixFromTuple3d(spatialError, errorTranslationVector, 3, 0);
}

public Twist computeDesiredTwist(FrameOrientation desiredOrientation, RigidBody endEffector, ReferenceFrame controlFrame, DenseMatrix64F selectionMatrix, MutableDouble errorMagnitude)
{
 errorFrameOrientation.setIncludingFrame(desiredOrientation);
 errorFrameOrientation.changeFrame(controlFrame);
 errorFrameOrientation.getAxisAngle(errorAxisAngle);
 errorRotation.set(errorAxisAngle.getX(), errorAxisAngle.getY(), errorAxisAngle.getZ());
 errorRotation.scale(AngleTools.trimAngleMinusPiToPi(errorAxisAngle.getAngle()));
 ReferenceFrame endEffectorFrame = endEffector.getBodyFixedFrame();
 Twist desiredTwist = new Twist();
 desiredTwist.set(endEffectorFrame, elevatorFrame, controlFrame, new Vector3d(), errorRotation);
 desiredTwist.getMatrix(spatialError, 0);
 subspaceError.reshape(selectionMatrix.getNumRows(), 1);
 CommonOps.mult(selectionMatrix, spatialError, subspaceError);
 errorMagnitude.setValue(NormOps.normP2(subspaceError));
 desiredTwist.scale(1.0 / updateDT);
 return desiredTwist;
}

public Twist computeDesiredTwist(FramePose desiredPose, RigidBody endEffector, ReferenceFrame controlFrame, DenseMatrix64F selectionMatrix, MutableDouble errorMagnitude)
{
 errorFramePose.setIncludingFrame(desiredPose);
 errorFramePose.changeFrame(controlFrame);
 errorFramePose.getPosition(errorPosition);
 errorFramePose.getOrientation(errorAxisAngle);
 errorRotation.set(errorAxisAngle.getX(), errorAxisAngle.getY(), errorAxisAngle.getZ());
 errorRotation.scale(AngleTools.trimAngleMinusPiToPi(errorAxisAngle.getAngle()));
 ReferenceFrame endEffectorFrame = endEffector.getBodyFixedFrame();
 Twist desiredTwist = new Twist();
 desiredTwist.set(endEffectorFrame, elevatorFrame, controlFrame, errorPosition, errorRotation);
 desiredTwist.getMatrix(spatialError, 0);
 subspaceError.reshape(selectionMatrix.getNumRows(), 1);
 CommonOps.mult(selectionMatrix, spatialError, subspaceError);
 errorMagnitude.setValue(NormOps.normP2(subspaceError));
 desiredTwist.scale(1.0 / updateDT);
 return desiredTwist;
}

public static void rigidBodyTransformToOpenCVTR(RigidBodyTransform transform, Mat tvec, Mat rvec)
{
 Point3d translation = new Point3d();
 transform.getTranslation(translation);
 AxisAngle4d axisAngle = new AxisAngle4d();
 transform.getRotation(axisAngle);
 Vector3d rotVector = new Vector3d(axisAngle.getX(), axisAngle.getY(), axisAngle.getZ());
 rotVector.normalize();
 rotVector.scale(axisAngle.getAngle());
 tvec.put(0, 0, translation.getX());
 tvec.put(1, 0, translation.getY());
 tvec.put(2, 0, translation.getZ());
 rvec.put(0, 0, rotVector.getX());
 rvec.put(1, 0, rotVector.getY());
 rvec.put(2, 0, rotVector.getZ());
}