public double[] getYawPitchRoll() { double[] yawPitchRollToReturn = new double[3]; getYawPitchRoll(yawPitchRollToReturn); return yawPitchRollToReturn; }
public void setOrientation(FrameOrientation orientation) { orientation.getYawPitchRoll(tempYawPitchRoll); setYawPitchRoll(tempYawPitchRoll); }
public void getYawPitchRoll(double[] yawPitchRollToPack) { putYoValuesIntoFrameOrientation(); frameOrientation.getYawPitchRoll(yawPitchRollToPack); }
public String toStringAsYawPitchRoll() { double[] yawPitchRoll = getYawPitchRoll(); return "yaw-pitch-roll: (" + yawPitchRoll[0] + ", " + yawPitchRoll[1] + ", " + yawPitchRoll[2] + ")-" + referenceFrame.getName(); }
public String toString() { String ret = ""; FrameOrientation frameOrientation = new FrameOrientation(ReferenceFrame.getWorldFrame(), this.orientation); double[] ypr = frameOrientation.getYawPitchRoll(); ret = location.toString(); ret += ", YawPitchRoll = " + Arrays.toString(ypr) + "\n"; return ret; }
public String toString() { FrameOrientation frameOrientation = new FrameOrientation(ankleReferenceFrame); frameOrientation.changeFrame(ReferenceFrame.getWorldFrame()); double[] ypr = frameOrientation.getYawPitchRoll(); String yawPitchRoll = "YawPitchRoll = " + Arrays.toString(ypr); return "id: " + id + " - pose: " + ankleReferenceFrame + " - trustHeight = " + trustHeight + "\n\tYawPitchRoll= {" + yawPitchRoll + "}"; } }
public String toString() { FrameOrientation frameOrientation = new FrameOrientation(poseReferenceFrame); frameOrientation.changeFrame(ReferenceFrame.getWorldFrame()); double[] ypr = frameOrientation.getYawPitchRoll(); String yawPitchRoll = "YawPitchRoll = " + Arrays.toString(ypr); return "id: " + id + " - pose: " + poseReferenceFrame + "\n\tYawPitchRoll= {" + yawPitchRoll + "}"; }
public String toString() { String ret = ""; FrameOrientation frameOrientation = new FrameOrientation(ReferenceFrame.getWorldFrame(), this.orientation); double[] ypr = frameOrientation.getYawPitchRoll(); ret = location.toString(); ret += ", YawPitchRoll = " + Arrays.toString(ypr) + "\n"; ret += "Predicted Contact Points: "; if (predictedContactPoints != null) { ret += "size = " + predictedContactPoints.size() + "\n"; } else { ret += "null"; } return ret; }
public String toString() { String ret = ""; FrameOrientation frameOrientation = new FrameOrientation(ReferenceFrame.getWorldFrame(), this.orientation); double[] ypr = frameOrientation.getYawPitchRoll(); ret = location.toString(); ret += ", YawPitchRoll = " + Arrays.toString(ypr) + "\n"; ret += "Predicted Contact Points: "; if (predictedContactPoints != null) { ret += "size = " + predictedContactPoints.size() + "\n"; } else { ret += "null"; } ret += trajectoryType.name() + "\n"; if(trajectoryWaypoints != null) { ret += "waypoints = " + trajectoryWaypoints.length + "\n"; } else { ret += "no waypoints" + "\n"; } return ret; }
public void set(FrameOrientation orientation, boolean notifyListeners) { orientation.checkReferenceFrameMatch(getReferenceFrame()); orientation.getYawPitchRoll(tempYawPitchRoll); yaw.set(tempYawPitchRoll[0], notifyListeners); pitch.set(tempYawPitchRoll[1], notifyListeners); roll.set(tempYawPitchRoll[2], notifyListeners); }
public void setAndMatchFrame(YoFrameQuaternion yoFrameQuaternion, boolean notifyListeners) { yoFrameQuaternion.getFrameOrientationIncludingFrame(tempFrameOrientation); tempFrameOrientation.changeFrame(getReferenceFrame()); tempFrameOrientation.getYawPitchRoll(tempYawPitchRoll); yaw.set(tempYawPitchRoll[0], notifyListeners); pitch.set(tempYawPitchRoll[1], notifyListeners); roll.set(tempYawPitchRoll[2], notifyListeners); }
public void setAndMatchFrame(FrameOrientation orientation, boolean notifyListeners) { tempFrameOrientation.setIncludingFrame(orientation); tempFrameOrientation.changeFrame(getReferenceFrame()); tempFrameOrientation.getYawPitchRoll(tempYawPitchRoll); yaw.set(tempYawPitchRoll[0], notifyListeners); pitch.set(tempYawPitchRoll[1], notifyListeners); roll.set(tempYawPitchRoll[2], notifyListeners); }
public void setAndMatchFrame(YoFrameOrientation yoFrameOrientation, boolean notifyListeners) { yoFrameOrientation.getFrameOrientationIncludingFrame(tempFrameOrientation); tempFrameOrientation.changeFrame(getReferenceFrame()); tempFrameOrientation.getYawPitchRoll(tempYawPitchRoll); yaw.set(tempYawPitchRoll[0], notifyListeners); pitch.set(tempYawPitchRoll[1], notifyListeners); roll.set(tempYawPitchRoll[2], notifyListeners); }
public void setTransformToWorld(RigidBodyTransform transformToWorld) { Vector3d translationToWorld = new Vector3d(); transformToWorld.getTranslation(translationToWorld); this.yoFramePoint.set(translationToWorld); FrameOrientation orientation = new FrameOrientation(ReferenceFrame.getWorldFrame(), transformToWorld); double[] yawPitchRoll = orientation.getYawPitchRoll(); yoFrameOrientation.setYawPitchRoll(yawPitchRoll[0], yawPitchRoll[1], yawPitchRoll[2]); }
/** * Sets the orientation of this to the origin of the passed in ReferenceFrame. * * @param referenceFrame */ public void setFromReferenceFrame(ReferenceFrame referenceFrame, boolean notifyListeners) { tempFrameOrientation.setToZero(referenceFrame); tempFrameOrientation.changeFrame(getReferenceFrame()); tempFrameOrientation.getYawPitchRoll(tempYawPitchRoll); yaw.set(tempYawPitchRoll[0], notifyListeners); pitch.set(tempYawPitchRoll[1], notifyListeners); roll.set(tempYawPitchRoll[2], notifyListeners); }
public String toString() { String startingFootstep = ""; int numberOfSteps = this.size(); if (numberOfSteps > 0) { startingFootstep = this.get(0).getLocation().toString(); Quat4d quat4d = this.get(0).getOrientation(); FrameOrientation frameOrientation = new FrameOrientation(ReferenceFrame.getWorldFrame(), quat4d); startingFootstep = startingFootstep + ", ypr= " + Arrays.toString(frameOrientation.getYawPitchRoll()); } if (this.size() == 1) { RobotSide footSide = footstepDataList.get(0).getRobotSide(); String side = "Right Step"; if (footSide.getSideNameFirstLetter().equals("L")) side = "Left Step"; return (side); } else { return ("Starting Footstep: " + startingFootstep + "\n\tExecution Mode: " + this.executionMode + "\n\tTransfer Time: " + defaultTransferTime + "\n\tSwing Time: " + defaultSwingTime + "\n\tSize: " + this.size() + " Footsteps"); } }
private void convertOrientationAndSetOnOutputPort() { transformFromIMUToWorld.setRotationAndZeroTranslation(orientationInputPort.getData()); estimationFrame.getTransformToDesiredFrame(transformFromEstimationFrameToIMUFrame, orientationMeasurementFrame); transformFromEstimationToWorld.multiply(transformFromIMUToWorld, transformFromEstimationFrameToIMUFrame); transformFromEstimationToWorld.getRotation(rotationFromEstimationToWorld); tempOrientationEstimationFrame.setIncludingFrame(ReferenceFrame.getWorldFrame(), rotationFromEstimationToWorld); // Introduce simulated IMU drift tempOrientationEstimationFrame.getYawPitchRoll(estimationFrameYawPitchRoll); estimationFrameYawPitchRoll[0] += imuSimulatedDriftYawAngle.getDoubleValue(); tempOrientationEstimationFrame.setYawPitchRoll(estimationFrameYawPitchRoll); orientationOutputPort.setData(tempOrientationEstimationFrame); }
secondIMUYawPrevValue.set(secondIMUYaw.getDoubleValue()); firstFrameOrientation.getYawPitchRoll(tempYawPitchRoll); tempYawPitchRoll[0] -= firstDriftYaw.getDoubleValue(); tempYawPitchRoll[0] = AngleTools.trimAngleMinusPiToPi(tempYawPitchRoll[0]); firstFrameOrientation.setYawPitchRoll(tempYawPitchRoll); secondFrameOrientation.getYawPitchRoll(tempYawPitchRoll); tempYawPitchRoll[0] -= secondDriftYaw.getDoubleValue(); tempYawPitchRoll[0] = AngleTools.trimAngleMinusPiToPi(tempYawPitchRoll[0]);
private void updateStartAndGoalOffsetErrorRotation() { //Rotation stateEstimatorReferenceFrame.getTransformToDesiredFrame(stateEstimatorTransform_Rotation, worldFrame); stateEstimatorTransform_Rotation.zeroTranslation(); stateEstimatorPose_Rotation.setPose(stateEstimatorTransform_Rotation); stateEstimatorReferenceFrame_Rotation.setPoseAndUpdate(stateEstimatorPose_Rotation); startOffsetErrorPose.changeFrame(stateEstimatorReferenceFrame_Rotation); startOffsetErrorPose.getOrientation(updatedStartOffset_Rotation_quat); startOffsetErrorPose.changeFrame(worldFrame); startOffsetErrorPose.getOrientationIncludingFrame(updatedStartOffset_Rotation); goalOffsetErrorPose.changeFrame(stateEstimatorReferenceFrame_Rotation); goalOffsetErrorPose.getOrientation(updatedGoalOffset_Rotation_quat); goalOffsetErrorPose.changeFrame(worldFrame); goalOffsetErrorPose.getOrientationIncludingFrame(updatedGoalOffset_Rotation); startOffsetTransform_Rotation.setRotationAndZeroTranslation(updatedStartOffset_Rotation_quat); offsetBetweenStartAndGoal_Rotation.setOrientationFromOneToTwo(updatedGoalOffset_Rotation, updatedStartOffset_Rotation); offsetBetweenStartAndGoal_Rotation.getYawPitchRoll(temporaryYawPitchRoll); goalYawRaw.set(temporaryYawPitchRoll[0]); goalYawWithDeadZone.update(); RotationTools.convertYawPitchRollToQuaternion(goalYawWithDeadZone.getDoubleValue(), temporaryYawPitchRoll[1], temporaryYawPitchRoll[2], updatedGoalOffsetWithDeadZone_Rotation_quat); updatedGoalOffsetWithDeadZone_Rotation_quat.mul(updatedStartOffset_Rotation_quat, updatedGoalOffsetWithDeadZone_Rotation_quat); updatedGoalOffsetWithDeadZone_Rotation.set(updatedGoalOffsetWithDeadZone_Rotation_quat); goalOffsetTransform_Rotation.setRotationAndZeroTranslation(updatedGoalOffsetWithDeadZone_Rotation_quat); }
@Override public void doSpecificAction() { feedbackControlCommandList.clear(); desiredOrientation.setToZero(contactableFoot.getFrameAfterParentJoint()); desiredOrientation.changeFrame(worldFrame); desiredOrientation.getYawPitchRoll(tempYawPitchRoll); twistCalculator.getRelativeTwist(footTwist, rootBody, contactableFoot.getRigidBody()); footTwist.changeFrame(contactableFoot.getFrameAfterParentJoint()); toeOffCurrentPitchAngle.set(tempYawPitchRoll[1]); toeOffCurrentPitchVelocity.set(footTwist.getAngularPartY()); desiredPosition.setToZero(contactableFoot.getFrameAfterParentJoint()); desiredPosition.changeFrame(worldFrame); computeDesiredsForFreeMotion(); desiredOrientation.setYawPitchRoll(desiredYawToHold, toeOffDesiredPitchAngle.getDoubleValue(), desiredRollToHold); desiredLinearVelocity.setToZero(worldFrame); desiredAngularVelocity.setIncludingFrame(contactableFoot.getFrameAfterParentJoint(), 0.0, toeOffDesiredPitchVelocity.getDoubleValue(), 0.0); desiredAngularVelocity.changeFrame(worldFrame); desiredLinearAcceleration.setToZero(worldFrame); desiredAngularAcceleration.setIncludingFrame(contactableFoot.getFrameAfterParentJoint(), 0.0, toeOffDesiredPitchAcceleration.getDoubleValue(), 0.0); desiredAngularAcceleration.changeFrame(worldFrame); orientationFeedbackControlCommand.set(desiredOrientation, desiredAngularVelocity, desiredAngularAcceleration); pointFeedbackControlCommand.set(desiredContactPointPosition, desiredLinearVelocity, desiredLinearAcceleration); setupSingleContactPoint(); feedbackControlCommandList.addCommand(orientationFeedbackControlCommand); feedbackControlCommandList.addCommand(pointFeedbackControlCommand); }