@Test void relativeOdometryUpdate() { }
public static Pose relativeOdometryUpdate(Pose fieldPose, Pose robotPoseDelta) { double dtheta = robotPoseDelta.heading; double sineTerm, cosTerm; if (approxEquals(dtheta, 0)) { sineTerm = 1.0 - dtheta * dtheta / 6.0; cosTerm = dtheta / 2.0; } else { sineTerm = Math.sin(dtheta) / dtheta; cosTerm = (1 - Math.cos(dtheta)) / dtheta; } Point fieldPositionDelta = new Point( sineTerm * robotPoseDelta.x - cosTerm * robotPoseDelta.y, cosTerm * robotPoseDelta.x + sineTerm * robotPoseDelta.y ); Pose fieldPoseDelta = new Pose(fieldPositionDelta.rotated(fieldPose.heading), robotPoseDelta.heading); return fieldPose.add(fieldPoseDelta); }
MathUtil { public static Pose relativeOdometryUpdate(Pose fieldPose, Pose robotPoseDelta) { double dtheta = robotPoseDelta.heading; double sineTerm, cosTerm; if (approxEquals(dtheta, 0)) { sineTerm = 1.0 - dtheta * dtheta / 6.0; cosTerm = dtheta / 2.0; } else { sineTerm = Math.sin(dtheta) / dtheta; cosTerm = (1 - Math.cos(dtheta)) / dtheta; } Point fieldPositionDelta = new Point( sineTerm * robotPoseDelta.x - cosTerm * robotPoseDelta.y, cosTerm * robotPoseDelta.x + sineTerm * robotPoseDelta.y ); Pose fieldPoseDelta = new Pose(fieldPositionDelta.rotated(fieldPose.heading), robotPoseDelta.heading); return fieldPose.add(fieldPoseDelta); } }
MathUtil { public static Pose relativeOdometryUpdate(Pose fieldPose, Pose robotPoseDelta) { double dtheta = robotPoseDelta.heading; double sineTerm, cosTerm; if (approxEquals(dtheta, 0)) { sineTerm = 1.0 - dtheta * dtheta / 6.0; cosTerm = dtheta / 2.0; } else { sineTerm = Math.sin(dtheta) / dtheta; cosTerm = (1 - Math.cos(dtheta)) / dtheta; } Point fieldPositionDelta = new Point( sineTerm * robotPoseDelta.x - cosTerm * robotPoseDelta.y, cosTerm * robotPoseDelta.x + sineTerm * robotPoseDelta.y ); Pose fieldPoseDelta = new Pose(fieldPositionDelta.rotated(fieldPose.heading), robotPoseDelta.heading); return fieldPose.add(fieldPoseDelta); }  }
MathUtil { public static Pose relativeOdometryUpdate(Pose fieldPose, Pose robotPoseDelta) { double dtheta = robotPoseDelta.heading; double sineTerm, cosTerm; if (approxEquals(dtheta, 0)) { sineTerm = 1.0 - dtheta * dtheta / 6.0; cosTerm = dtheta / 2.0; } else { sineTerm = Math.sin(dtheta) / dtheta; cosTerm = (1 - Math.cos(dtheta)) / dtheta; } Point fieldPositionDelta = new Point( sineTerm * robotPoseDelta.x - cosTerm * robotPoseDelta.y, cosTerm * robotPoseDelta.x + sineTerm * robotPoseDelta.y ); Pose fieldPoseDelta = new Pose(fieldPositionDelta.rotated(fieldPose.heading), robotPoseDelta.heading); return fieldPose.add(fieldPoseDelta); }  static double angleWrap(double angle); static double clamp(double d); static double clampAbove(double d, double threshold); static double cutOffBelow(double d, double threshold); static double powRetainingSign(double d, double power); static double deadZone(double d, double thresh); static Pose relativeOdometryUpdate(Pose fieldPose, Pose robotPoseDelta); static boolean approxEquals(double d1, double d2); static Point lineSegmentCircleIntersection(Point ul1, Point ul2, Point o, double radius); static boolean between(double r1, double r2, double val, double threshold); }
MathUtil { public static Pose relativeOdometryUpdate(Pose fieldPose, Pose robotPoseDelta) { double dtheta = robotPoseDelta.heading; double sineTerm, cosTerm; if (approxEquals(dtheta, 0)) { sineTerm = 1.0 - dtheta * dtheta / 6.0; cosTerm = dtheta / 2.0; } else { sineTerm = Math.sin(dtheta) / dtheta; cosTerm = (1 - Math.cos(dtheta)) / dtheta; } Point fieldPositionDelta = new Point( sineTerm * robotPoseDelta.x - cosTerm * robotPoseDelta.y, cosTerm * robotPoseDelta.x + sineTerm * robotPoseDelta.y ); Pose fieldPoseDelta = new Pose(fieldPositionDelta.rotated(fieldPose.heading), robotPoseDelta.heading); return fieldPose.add(fieldPoseDelta); }  static double angleWrap(double angle); static double clamp(double d); static double clampAbove(double d, double threshold); static double cutOffBelow(double d, double threshold); static double powRetainingSign(double d, double power); static double deadZone(double d, double thresh); static Pose relativeOdometryUpdate(Pose fieldPose, Pose robotPoseDelta); static boolean approxEquals(double d1, double d2); static Point lineSegmentCircleIntersection(Point ul1, Point ul2, Point o, double radius); static boolean between(double r1, double r2, double val, double threshold); static final double EPSILON; }