public void get(AxisAngle4d axisAngleToPack) { putYoValuesIntoFrameOrientation(); frameOrientation.getAxisAngle(axisAngleToPack); }
private void updateMaxRotationAlphaVariationSpeed() { //Rotation rotationToTravel.setOrientationFromOneToTwo(updatedStartOffset_Rotation, updatedGoalOffsetWithDeadZone_Rotation); rotationToTravel.getAxisAngle(axisAngletoTravel); angleToTravel.set(axisAngletoTravel.getAngle()); rotationalSpeedForGivenAngleToTravel.set(angleToTravel.getDoubleValue() / dt.getDoubleValue()); }
public boolean checkIfErrorIsTooBig(FramePose correctedPelvisPoseInWorldFrame, FramePose iterativeClosestPointInWorldFramePose, boolean isRotationCorrectionEnabled) { correctedPelvisPoseReferenceFrame.setPoseAndUpdate(correctedPelvisPoseInWorldFrame); iterativeClosestPointInWorldFramePose.getOrientationIncludingFrame(iterativeClosestPointOrientation); iterativeClosestPointInWorldFramePose.getPositionIncludingFrame(iterativeClosestPointTranslation); iterativeClosestPointOrientation.changeFrame(correctedPelvisPoseReferenceFrame); iterativeClosestPointTranslation.changeFrame(correctedPelvisPoseReferenceFrame); iterativeClosestPointOrientation.getAxisAngle(axisAngleForError); angleError.set(Math.abs(axisAngleForError.getAngle())); translationErrorX.set(Math.abs(iterativeClosestPointTranslation.getX())); translationErrorY.set(Math.abs(iterativeClosestPointTranslation.getY())); translationErrorZ.set(Math.abs(iterativeClosestPointTranslation.getZ())); if(isRotationCorrectionEnabled && Math.abs(axisAngleForError.getAngle()) > Math.toRadians(maximumErrorAngleInDegrees.getDoubleValue())) return true; if(Math.abs(iterativeClosestPointTranslation.getX()) > maximumErrorTranslation.getDoubleValue()) return true; if(Math.abs(iterativeClosestPointTranslation.getY()) > maximumErrorTranslation.getDoubleValue()) return true; if(Math.abs(iterativeClosestPointTranslation.getZ()) > maximumErrorTranslation.getDoubleValue()) return true; return false; }
public boolean computeIteratively(FramePose desiredPose, Twist desiredTwist, double maxIterations, double epsilon) { for(int i = 0; i < maxIterations; i++) { compute(desiredPose, desiredTwist); desiredPose.getPositionIncludingFrame(tempPoint); tempPoint.changeFrame(localControlFrame); double translationDistance = tempPoint.getDistanceFromOrigin(); desiredPose.getOrientationIncludingFrame(tempOrientation); tempOrientation.changeFrame(localControlFrame); tempOrientation.getAxisAngle(tempAxisAngle); double angle = Math.abs(AngleTools.trimAngleMinusPiToPi(tempAxisAngle.getAngle())); if(translationDistance < epsilon && angle < epsilon) { return true; } } return false; }
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; }
desiredOrientationCopy.setIncludingFrame(desiredOrientation); desiredOrientationCopy.changeFrame(footPolygon.getReferenceFrame()); desiredOrientationCopy.getAxisAngle(desiredAxisAngle); desiredRotationVector.set(desiredAxisAngle.getX(), desiredAxisAngle.getY(), desiredAxisAngle.getZ()); desiredRotationVector.scale(desiredAxisAngle.getAngle());
copyOfFinalAngularVelocity.changeFrame(interpolationFrame); copyOfFinalOrientation.getAxisAngle(tempAxisAngle);