public FrameOrientation getFrameOrientationCopy() { putYoValuesIntoFrameOrientation(); return new FrameOrientation(frameOrientation); }
public static FrameOrientation generateRandomFrameOrientation(Random random, ReferenceFrame referenceFrame) { FrameOrientation randomOrientation = new FrameOrientation(referenceFrame, RandomTools.generateRandomQuaternion(random)); return randomOrientation; }
public OrientationFrame(FrameOrientation orientation) { super("Orientation_" + orientationNumber, orientation.getReferenceFrame(), false, false, false); orientationNumber++; frameOrientation = new FrameOrientation(orientation); this.update(); }
public FrameOrientation getFrameOrientationCopy() { FrameOrientation ret = new FrameOrientation(getReferenceFrame(), pose.getOrientation()); return ret; }
public FrameOrientation getOrientationCopy() { FrameOrientation orientationCopy = new FrameOrientation(getReferenceFrame()); getOrientation(orientationCopy); return orientationCopy; }
public FrameOrientation getOrientationCopy() { FrameOrientation orientationCopy = new FrameOrientation(getReferenceFrame()); getOrientation(orientationCopy); return orientationCopy; }
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 + "}"; }
private void submitFootPose(boolean parallelize, RobotSide robotSide, ReferenceFrame referenceFrame, double x, double y, double z, double yaw, double pitch, double roll) { FramePoint framePosition = new FramePoint(referenceFrame, x, y, z); FrameOrientation frameOrientation = new FrameOrientation(referenceFrame, yaw, pitch, roll); FramePose desiredFootPose = new FramePose(framePosition, frameOrientation); submitFootPose(parallelize, robotSide, desiredFootPose); }
private void submitFootPosition(RobotSide robotSide, FramePoint desiredFootPosition) { FrameOrientation desiredFootOrientation = new FrameOrientation(desiredFootPosition.getReferenceFrame()); FramePose desiredFootPose = new FramePose(desiredFootPosition, desiredFootOrientation); submitFootPose(robotSide, desiredFootPose); }
public static FrameOrientation generateRandomFrameOrientation(Random random, ReferenceFrame referenceFrame, double yawMin, double yawMax, double pitchMin, double pitchMax, double rollMin, double rollMax) { double yaw = RandomTools.generateRandomDouble(random, yawMin, yawMax); double pitch = RandomTools.generateRandomDouble(random, pitchMin, pitchMax); double roll = RandomTools.generateRandomDouble(random, rollMin, rollMax); FrameOrientation randomOrientation = new FrameOrientation(referenceFrame, yaw, pitch, roll); return randomOrientation; }
private void sequenceHandShakePrep() { RobotSide robotSide = RobotSide.RIGHT; boolean mirrorOrientationForRightSide = true; FrameOrientation desiredUpperArmOrientation = new FrameOrientation(fullRobotModel.getChest().getBodyFixedFrame()); FrameOrientation desiredHandOrientation = new FrameOrientation(lowerArmsFrames.get(robotSide)); desiredUpperArmOrientation.setYawPitchRoll(0.0, -shoulderExtensionAngle, -1.2708); desiredHandOrientation.setYawPitchRoll(0.0, Math.PI / 2.0, 0.0); submitHandPose(robotSide, desiredUpperArmOrientation, shoulderExtensionAngle - Math.PI / 2.0, desiredHandOrientation, mirrorOrientationForRightSide); pipeLine.requestNewStage(); }
public static Quat4d getTransformedQuat(Quat4d quat4d, RigidBodyTransform transform3D) { ReferenceFrame ending = ReferenceFrame.constructARootFrame("ending", false, true, true); ReferenceFrame starting = ReferenceFrame.constructFrameWithUnchangingTransformToParent("starting", ending, transform3D, false, true, true); FrameOrientation start = new FrameOrientation(starting, quat4d); start.changeFrame(ending); return start.getQuaternionCopy(); }
public FrameOrientation getFrameOrientationCopy() { FrameOrientation orientation = new FrameOrientation(getReferenceFrame(), yaw.getDoubleValue(), pitch.getDoubleValue(), roll.getDoubleValue()); return orientation; }
public void initialize() { time.set(0.0); FrameOrientation orientationToPack = new FrameOrientation(ReferenceFrame.getWorldFrame()); orientationProvider.getOrientation(orientationToPack); orientationToPack.changeFrame(orientation.getReferenceFrame()); orientation.set(orientationToPack); }
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]); }
@Override protected void setBehaviorInput() { FramePoint point = new FramePoint(ReferenceFrame.getWorldFrame(), initialSphereDetectionBehavior.getBallLocation().getX(), initialSphereDetectionBehavior.getBallLocation().getY(), initialSphereDetectionBehavior.getBallLocation().getZ() + initialSphereDetectionBehavior.getSpehereRadius()); wholeBodyBehavior.setSolutionQualityThreshold(2.01); wholeBodyBehavior.setTrajectoryTime(3); FrameOrientation tmpOr = new FrameOrientation(point.getReferenceFrame(), Math.toRadians(45), Math.toRadians(90), 0); wholeBodyBehavior.setDesiredHandPose(RobotSide.LEFT, point, tmpOr); } };
public void computeChestTrajectoryMessage() { checkIfDataHasBeenSet(); ReferenceFrame chestFrame = fullRobotModelToUseForConversion.getChest().getBodyFixedFrame(); Quat4d desiredQuaternion = new Quat4d(); FrameOrientation desiredOrientation = new FrameOrientation(chestFrame); desiredOrientation.changeFrame(worldFrame); desiredOrientation.getQuaternion(desiredQuaternion); ChestTrajectoryMessage chestTrajectoryMessage = new ChestTrajectoryMessage(trajectoryTime, desiredQuaternion); output.setChestTrajectoryMessage(chestTrajectoryMessage); }
@Override public void getReportedGoalPoseWorldFrame(FramePose framePoseToPack) { ReferenceFrame midFeetZUpFrame = referenceFrames.getMidFeetZUpFrame(); FramePoint goalPosition = new FramePoint(midFeetZUpFrame, constantGoalLocationInMidFeetZUpFrame); goalPosition.changeFrame(ReferenceFrame.getWorldFrame()); FrameOrientation goalOrientation = new FrameOrientation(midFeetZUpFrame); goalOrientation.changeFrame(ReferenceFrame.getWorldFrame()); framePoseToPack.setPosition(goalPosition); framePoseToPack.setOrientation(goalOrientation); }
private FramePose offsetPointFromChestInWorldFrame(double x, double y, double z, double yaw, double pitch, double roll) { FramePoint point1 = new FramePoint(referenceFrames.getChestFrame(), x, y, z); point1.changeFrame(ReferenceFrame.getWorldFrame()); FrameOrientation orient = new FrameOrientation(referenceFrames.getChestFrame(), yaw, pitch, roll); orient.changeFrame(ReferenceFrame.getWorldFrame()); FramePose pose = new FramePose(point1, orient); return pose; }