public double getOrientationDistance(FramePose framePose) { AxisAngle4d rotationFromThatToThis = new AxisAngle4d(); getAxisAngleRotationToOtherPose(framePose, rotationFromThatToThis); return Math.abs(rotationFromThatToThis.getAngle()); }
public static double getMagnitudeOfAngleOfRotation(RigidBodyTransform rigidBodyTransform) { AxisAngle4d axisAngle4d = new AxisAngle4d(); rigidBodyTransform.getRotation(axisAngle4d); return Math.abs(axisAngle4d.getAngle()); }
/** * This computes: resultToPack = q^power. * @param q is a unit quaternion and describes a orientation. * @param resultToPack is used to store the result. */ public void pow(Quat4d q, double power, Quat4d resultToPack) { axisAngleForPow.set(q); axisAngleForPow.setAngle(power * axisAngleForPow.getAngle()); resultToPack.set(axisAngleForPow); }
public static void trimAxisAngleMinusPiToPi(AxisAngle4d axisAngle4d, AxisAngle4d axisAngleTrimmedToPack) { axisAngleTrimmedToPack.set(axisAngle4d); axisAngleTrimmedToPack.setAngle(AngleTools.trimAngleMinusPiToPi(axisAngleTrimmedToPack.getAngle())); }
/** * 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 assertQuaternionsEqualUsingDifference(Quat4d q1, Quat4d q2, double epsilon) { try { Quat4d qDifference = new Quat4d(); qDifference.mulInverse(q1, q2); AxisAngle4d axisAngle = new AxisAngle4d(); axisAngle.set(qDifference); assertEquals(0.0, axisAngle.getAngle(), epsilon); } catch (AssertionError e) { throw new AssertionError("expected:\n<" + q1 + ">\n but was:\n<" + q2 + ">"); } } }
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()); }
private void updateMaxRotationAlphaVariationSpeed() { //Rotation rotationToTravel.setOrientationFromOneToTwo(updatedStartOffset_Rotation, updatedGoalOffsetWithDeadZone_Rotation); rotationToTravel.getAxisAngle(axisAngletoTravel); angleToTravel.set(axisAngletoTravel.getAngle()); rotationalSpeedForGivenAngleToTravel.set(angleToTravel.getDoubleValue() / dt.getDoubleValue()); }
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()); }
/** * clips max rotational velocity */ private void updateRotationalMaxVelocityClip() { interpolatedRotationCorrectionFrame.getTransformToDesiredFrame(errorBetweenCurrentPositionAndCorrected, worldFrame); errorBetweenCurrentPositionAndCorrected.getRotation(angleToTravelAxis4d); angleToTravel.set(angleToTravelAxis4d.getAngle()); maxRotationAlpha.set((estimatorDT * maxRotationVelocityClip.getDoubleValue() / angleToTravel.getDoubleValue()) + previousRotationClippedAlphaValue.getDoubleValue()); rotationClippedAlphaValue.set(MathTools.clipToMinMax(interpolationRotationAlphaFilter.getDoubleValue(), 0.0, maxRotationAlpha.getDoubleValue())); previousRotationClippedAlphaValue.set(rotationClippedAlphaValue.getDoubleValue()); }
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; }
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 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); }
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; }