private static List<OverheadPath> calculatePaths(FramePose2D startPose, FramePoint2D endPoint, double headingOffset, double noTranslationTolerance) { startPose.checkReferenceFrameMatch(endPoint); double heading = AngleTools.calculateHeading(startPose, endPoint, headingOffset, noTranslationTolerance); FrameOrientation2D intermediateOrientation = new FrameOrientation2D(startPose.getReferenceFrame(), heading); TurningOverheadPath turningPath = new TurningOverheadPath(startPose, intermediateOrientation); FramePose2D intermediatePose = turningPath.getPoseAtS(1.0); StraightLineOverheadPath straightPath = new StraightLineOverheadPath(intermediatePose, endPoint); List<OverheadPath> paths = new ArrayList<OverheadPath>(); paths.add(turningPath); paths.add(straightPath); return paths; }
private static List<OverheadPath> calculatePaths(FramePose2d startPose, FramePoint2d endPoint, double headingOffset, double noTranslationTolerance) { startPose.checkReferenceFrameMatch(endPoint); double heading = AngleTools.calculateHeading(startPose, endPoint, headingOffset, noTranslationTolerance); FrameOrientation2d intermediateOrientation = new FrameOrientation2d(startPose.getReferenceFrame(), heading); TurningOverheadPath turningPath = new TurningOverheadPath(startPose, intermediateOrientation); FramePose2d intermediatePose = turningPath.getPoseAtS(1.0); StraightLineOverheadPath straightPath = new StraightLineOverheadPath(intermediatePose, endPoint); List<OverheadPath> paths = new ArrayList<OverheadPath>(); paths.add(turningPath); paths.add(straightPath); return paths; }
private void addTurnInPlaceToFacePoint(ArrayList<FramePose2D> footstepList, FramePose2D robotPose, FramePoint2D goalPoint) { double turningAngle = AngleTools.calculateHeading(robotPose, goalPoint, -robotPose.getYaw(), 0.0); FramePoint2D pointToTurnAbout = new FramePoint2D(robotPose.getPosition()); addTurnInPlace(footstepList, turningAngle, pointToTurnAbout); }
private void addTurnInPlaceToFacePoint(ArrayList<FramePose2d> footstepList, FramePose2d robotPose, FramePoint2d goalPoint) { double turningAngle = AngleTools.calculateHeading(robotPose, goalPoint, -robotPose.getYaw(), 0.0); FramePoint2d pointToTurnAbout = new FramePoint2d(); robotPose.getPosition(pointToTurnAbout); addTurnInPlace(footstepList, turningAngle, pointToTurnAbout); }
private static List<OverheadPath> calculatePaths(FramePose2d startPose, FramePose2d endPose, double headingOffset, double noTranslationTolerance) { startPose.checkReferenceFrameMatch(endPose); FramePoint2d endPosition = new FramePoint2d(); endPose.getPosition(endPosition); double heading = AngleTools.calculateHeading(startPose, endPosition, headingOffset, noTranslationTolerance); FrameOrientation2d intermediateOrientation = new FrameOrientation2d(startPose.getReferenceFrame(), heading); TurningOverheadPath turningPath = new TurningOverheadPath(startPose, intermediateOrientation); FramePose2d intermediatePose = turningPath.getPoseAtS(1.0); StraightLineOverheadPath straightPath = new StraightLineOverheadPath(intermediatePose, endPosition); intermediatePose = straightPath.getPoseAtS(1.0); FrameOrientation2d endOrientation = new FrameOrientation2d(); endPose.getOrientation(endOrientation); TurningOverheadPath endTurningPath = new TurningOverheadPath(intermediatePose, endOrientation); List<OverheadPath> paths = new ArrayList<OverheadPath>(); paths.add(turningPath); paths.add(straightPath); paths.add(endTurningPath); return paths; }
private static List<OverheadPath> calculatePaths(FramePose2D startPose, FramePose2D endPose, double headingOffset, double noTranslationTolerance) { startPose.checkReferenceFrameMatch(endPose); FramePoint2D endPosition = new FramePoint2D(endPose.getPosition()); double heading = AngleTools.calculateHeading(startPose, endPosition, headingOffset, noTranslationTolerance); FrameOrientation2D intermediateOrientation = new FrameOrientation2D(startPose.getReferenceFrame(), heading); TurningOverheadPath turningPath = new TurningOverheadPath(startPose, intermediateOrientation); FramePose2D intermediatePose = turningPath.getPoseAtS(1.0); StraightLineOverheadPath straightPath = new StraightLineOverheadPath(intermediatePose, endPosition); intermediatePose = straightPath.getPoseAtS(1.0); FrameOrientation2D endOrientation = new FrameOrientation2D(endPose.getOrientation()); TurningOverheadPath endTurningPath = new TurningOverheadPath(intermediatePose, endOrientation); List<OverheadPath> paths = new ArrayList<OverheadPath>(); paths.add(turningPath); paths.add(straightPath); paths.add(endTurningPath); return paths; }
double heading = Math.toDegrees(AngleTools.calculateHeading(start, end, 0.0, 0.0)); assertEquals(heading, 45.0, 1e-7); heading = Math.toDegrees(AngleTools.calculateHeading(start, end, 0.0, 0.0)); assertEquals(heading, 0.0, 1e-7); heading = Math.toDegrees(AngleTools.calculateHeading(start, end, 0.0, 0.0)); assertEquals(heading, 0.0, 1e-7); heading = Math.toDegrees(AngleTools.calculateHeading(start, end, 0.0, 0.0)); assertEquals(heading, 90.0, 1e-7); heading = Math.toDegrees(AngleTools.calculateHeading(start, end, 0.0, 0.0)); assertEquals(heading, -90.0, 1e-7); heading = Math.toDegrees(AngleTools.calculateHeading(start, end, 0.0, 0.0)); assertEquals(heading, -135.0, 1e-7); heading = Math.toDegrees(AngleTools.calculateHeading(start, end, 0.0, 0.0)); assertEquals(heading, -180.0, 1e-7); heading = Math.toDegrees(AngleTools.calculateHeading(start, end, 0.0, 0.0)); assertEquals(heading, -45, 1e-7); heading = Math.toDegrees(AngleTools.calculateHeading(start, end, 0.0, 0.0)); assertEquals(heading, 135.0, 1e-7);
double midYaw = AngleTools.calculateHeading(startPose, midPoint, 0.0, 1e-14);