/** * 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()); }