diff --git a/.vscode/launch.json b/.vscode/launch.json index 233cbfb..957098d 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -4,6 +4,13 @@ // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 "version": "0.2.0", "configurations": [ + { + "type": "java", + "name": "CodeLens (Launch) - BobTrajectoryApp", + "request": "launch", + "mainClass": "com.team319.ui.BobTrajectoryApp", + "projectName": "BobTrajectory" + }, { "type": "java", "name": "CodeLens (Launch) - PlotterFrame", diff --git a/src/main/java/com/team2363/geometry/Pose2d.java b/src/main/java/com/team2363/geometry/Pose2d.java new file mode 100644 index 0000000..66a2fdd --- /dev/null +++ b/src/main/java/com/team2363/geometry/Pose2d.java @@ -0,0 +1,37 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package com.team2363.geometry; + +public class Pose2d { + + private double x; + private double y; + private double rotation; + + public Pose2d(double x, double y, double rotation) { + this.x = x; + this.y = y; + this.rotation = rotation; + } + + public double x() { + return x; + } + + public double y() { + return y; + } + + public double getRotation() { + return rotation; + } + + public double getDistance(Pose2d p1) { + return Math.sqrt((p1.x() - x) * (p1.x() - x) + (p1.y() - y) * (p1.y() - y)); + } +} \ No newline at end of file diff --git a/src/main/java/com/team2363/spline/CartesianCubicSpline.java b/src/main/java/com/team2363/spline/CartesianCubicSpline.java new file mode 100644 index 0000000..7637cd3 --- /dev/null +++ b/src/main/java/com/team2363/spline/CartesianCubicSpline.java @@ -0,0 +1,131 @@ +// /*----------------------------------------------------------------------------*/ +// /* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +// /* Open Source Software - may be modified and shared by FRC teams. The code */ +// /* must be accompanied by the FIRST BSD license file in the root directory of */ +// /* the project. */ +// /*----------------------------------------------------------------------------*/ + +// package com.team2363.spline; + +// import com.team2363.geometry.Pose2d; + +// public class CartesianCubicSpline extends Spline { + +// private double a,b,c; +// private double dydx0, dydx1; +// private double x0, y0, thetaOffset, distance; + +// CartesianCubicSpline(Pose2d p0, Pose2d p1) { + +// x0 = p0.x(); +// y0 = p0.y(); +// thetaOffset = Math.atan2(p1.y() - p0.y(), p1.x() - p0.x()); +// distance = p0.getDistance(p1); + +// dydx0 = Math.tan((p0.getRotation() - thetaOffset) % (2 * Math.PI)); +// dydx1 = Math.tan((p1.getRotation() - thetaOffset) % (2 * Math.PI)); + +// a = ; +// b = ; +// c = ; +// } + +// private static boolean almostEqual(double x, double y) { +// return Math.abs(x - y) < 1E-6; +// } + +// public static boolean reticulateSplines(double x0, double y0, double theta0, double x1, double y1, double theta1, +// Spline result) { +// // System.out.println("Reticulating splines..."); + +// // Transform x to the origin +// if (distance == 0) { +// return false; +// } + +// double theta0_hat = ChezyMath.getDifferenceInAngleRadians(result.theta_offset_, theta0); +// double theta1_hat = ChezyMath.getDifferenceInAngleRadians(result.theta_offset_, theta1); +// // We cannot handle vertical slopes in our rotated, translated basis. +// // This would mean the user wants to end up 90 degrees off of the straight +// // line between p0 and p1. +// if (almostEqual(Math.abs(theta0_hat), Math.PI / 2) || almostEqual(Math.abs(theta1_hat), Math.PI / 2)) { +// return false; +// } +// // We also cannot handle the case that the end angle is facing towards the +// // start angle (total turn > 90 degrees). +// if (Math.abs(ChezyMath.getDifferenceInAngleRadians(theta0_hat, theta1_hat)) >= Math.PI / 2) { +// return false; +// } +// // Turn angles into derivatives (slopes) +// double yp0_hat = Math.tan(theta0_hat); +// double yp1_hat = Math.tan(theta1_hat); + +// // Calculate the cubic spline coefficients +// result.a_ = 0; +// result.b_ = 0; +// result.c_ = 0; + +// return true; +// } + +// public double calculateLength() { + +// final int kNumSamples = 100000; +// double integral = 0; +// double integrand, last_integrand = Math.sqrt(1 + derivativeAt(0) * derivativeAt(0)) / kNumSamples; +// for (double i = 1; i <= kNumSamples; ++i) { +// double t = i / kNumSamples; +// integrand = Math.sqrt(1 + dydx(t) * dydx(t)) / kNumSamples; +// integral += (integrand + last_integrand) / 2; +// last_integrand = integrand; +// } + +// return integral; +// } + +// public double calculateInput(double distance) { +// final int kNumSamples = 100000; +// double arc_length = 0; +// double t = 0; +// double last_arc_length = 0; +// double dydt; +// double integrand, last_integrand = Math.sqrt(1 + dydx(0) * dydx(0)) / kNumSamples; +// distance /= knot_distance_; +// for (double i = 1; i <= kNumSamples; ++i) { +// t = i / kNumSamples; +// integrand = Math.sqrt(1 + dydx(t) * dydx(t)) / kNumSamples; +// arc_length += (integrand + last_integrand) / 2; +// if (arc_length > distance) { +// break; +// } +// last_integrand = integrand; +// last_arc_length = arc_length; +// } + +// // Interpolate between samples. +// double interpolated = t; +// if (arc_length != last_arc_length) { +// interpolated += ((distance - last_arc_length) / (arc_length - last_arc_length) - 1) / (double) kNumSamples; +// } +// return interpolated; +// } + +// public Pose2d getPoint(double t) { +// double x_hat = t * distance; +// double y_hat = (a_ * x_hat + b_) * x_hat * x_hat * x_hat * x_hat + c_ * x_hat * x_hat * x_hat +// + d_ * x_hat * x_hat + e_ * x_hat; + +// double cos_theta = Math.cos(thetaOffset); +// double sin_theta = Math.sin(thetaOffset); + +// double x = x_hat * cos_theta - y_hat * sin_theta + x0; +// double y = x_hat * sin_theta + y_hat * cos_theta + y0; +// double theta = ; + +// return new Pose2d(x, y, theta); +// } + +// private double dydx(double t) { +// return 3 * a * (t * distance) * (t * distance) + 2 * b * (t * distance) + c; +// } +// } \ No newline at end of file diff --git a/src/main/java/com/team2363/spline/CartesianQuinticSpline.java b/src/main/java/com/team2363/spline/CartesianQuinticSpline.java new file mode 100644 index 0000000..2f65044 --- /dev/null +++ b/src/main/java/com/team2363/spline/CartesianQuinticSpline.java @@ -0,0 +1,29 @@ +// /*----------------------------------------------------------------------------*/ +// /* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +// /* Open Source Software - may be modified and shared by FRC teams. The code */ +// /* must be accompanied by the FIRST BSD license file in the root directory of */ +// /* the project. */ +// /*----------------------------------------------------------------------------*/ + +// package com.team2363.spline; + +// import com.team2363.geometry.Pose2d; + +// public class CartesianQuinticSpline extends Spline { + +// CartesianQuinticSpline(Pose2d p0, Pose2d p1) { + +// } + +// public Pose2d getPoint(double t) { +// return new Pose2d(0, 0, 0); +// } + +// public double calculateLength() { +// return 0; +// } + +// public double calculateInput(double distance) { +// return 0; +// } +// } \ No newline at end of file diff --git a/src/main/java/com/team2363/spline/ParametricCubicSpline.java b/src/main/java/com/team2363/spline/ParametricCubicSpline.java new file mode 100644 index 0000000..53ec7b4 --- /dev/null +++ b/src/main/java/com/team2363/spline/ParametricCubicSpline.java @@ -0,0 +1,78 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package com.team2363.spline; + +import com.team2363.geometry.Pose2d; + +public class ParametricCubicSpline extends Spline { + + private double vectorMagnitude; + private double dx0,dx1,dy0,dy1; + private double ax,bx,cx,dx,ay,by,cy,dy; + + public ParametricCubicSpline(Pose2d p0, Pose2d p1) { + + vectorMagnitude = 1.5 * p0.getDistance(p1); + + dx0 = Math.cos(p0.getRotation()) * vectorMagnitude; + dx1 = Math.cos(p1.getRotation()) * vectorMagnitude; + + dy0 = Math.sin(p0.getRotation()) * vectorMagnitude; + dy1 = Math.sin(p1.getRotation()) * vectorMagnitude; + + ax = 2 * p0.x() - 2 * p1.x() + dx0 + dx1; + bx = -2 * dx0 - dx1 - 3 * p0.x() + 3 * p1.x(); + cx = dx0; + dx = p0.x(); + + ay = 2 * p0.y() - 2 * p1.y() + dy0 + dy1; + by = -2 * dy0 - dy1 - 3 * p0.y() + 3 * p1.y(); + cy = dy0; + dy = p0.y(); + } + + public Pose2d getPoint(double t) { + return new Pose2d(x(t), y(t), Math.atan2(dy(t), dx(t))); + } + + public double arcLengthSegment(double t) { + return Math.sqrt(dx(t) * dx(t) + dy(t) * dy(t)); + } + + private double x(double t) { + return ax * t * t * t + bx * t * t + cx * t + dx; + } + + private double y(double t) { + return ay * t * t * t + by * t * t + cy * t + dy; + } + + private double dx(double t) { + return 3 * ax * t * t + 2 * bx * t + cx; + } + + private double dy(double t) { + return 3 * ay * t * t + 2 * by * t + cy; + } + + public double calculateLength() { + + final int kNumSamples = 100000; + double integral = 0; + double integrand, last_integrand = Math.sqrt(dx(0) * dx(0) + dy(0) * dy(0)) / kNumSamples; + + for (double i = 1; i <= kNumSamples; ++i) { + double t = i / kNumSamples; + integrand = Math.sqrt(dx(t) * dx(t) + dy(t) * dy(t)) / kNumSamples; + integral += (integrand + last_integrand) / 2; + last_integrand = integrand; + } + + return integral; + } +} \ No newline at end of file diff --git a/src/main/java/com/team2363/spline/ParametricQuinticSpline.java b/src/main/java/com/team2363/spline/ParametricQuinticSpline.java new file mode 100644 index 0000000..ed9c28a --- /dev/null +++ b/src/main/java/com/team2363/spline/ParametricQuinticSpline.java @@ -0,0 +1,96 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package com.team2363.spline; + +import com.team2363.geometry.Pose2d; +import com.team254.lib.trajectory.Trajectory; +import com.team319.ui.DraggableWaypoint; + +public class ParametricQuinticSpline extends Spline { + + private double vectorMagnitude; + private double x0,x1,dx0,dx1,ddx0,ddx1,y0,y1,dy0,dy1,ddy0,ddy1; + private double ax,bx,cx,dx,ex,fx,ay,by,cy,dy,ey,fy; + + public ParametricQuinticSpline(DraggableWaypoint w0, DraggableWaypoint w1) { + + vectorMagnitude = 1.5 * w0.getDistance(w1); + + x0 = w0.getX(); + x1 = w1.getX(); + dx0 = Math.cos(w0.getHeading()) * vectorMagnitude; + dx1 = Math.cos(w1.getHeading()) * vectorMagnitude; + ddy0 = 0; + ddy1 = 0; + + y0 = w0.getY(); + y1 = w1.getY(); + dy0 = Math.sin(w0.getHeading()) * vectorMagnitude; + dy1 = Math.sin(w1.getHeading()) * vectorMagnitude; + ddy0 = 0; + ddy1 = 0; + + calculateCoefficients(); + } + + public void calculateCoefficients() { + ax = 6 * (x1 - x0) - 3 * (dx0 + dx1) + 0.5 * (ddx1 - ddx0); + bx = 15 * x0 + 8 * dx0 + 1.5 * ddx0 - ddx1 + 7 * dx1 - 15 * x1; + cx = -10 * x0 - 6 * dx0 - 1.5 * ddx0 + 0.5 * ddx1 - 4 * dx1 + 10 * x1; + dx = 0.5 * ddx0; + ex = dx0; + fx = x0; + + ay = 6 * (y1 - y0) - 3 * (dy0 + dy1) + 0.5 * (ddy1 - ddy0);; + by = 15 * y0 + 8 * dy0 + 1.5 * ddy0 - ddy1 + 7 * dy1 - 15 * y1; + cy = -10 * y0 - 6 * dy0 - 1.5 * ddy0 + 0.5 * ddy1 - 4 * dy1 + 10 * y1; + dy = 0.5 * ddy0; + ey = dy0; + fy = y0; + } + + public Pose2d getPoint(double t) { + return new Pose2d(x(t), y(t), Math.atan2(dy(t), dx(t))); + } + + public double arcLengthSegment(double t) { + return Math.sqrt(dx(t) * dx(t) + dy(t) * dy(t)); + } + + private double x(double t) { + return ax * t * t * t * t * t + bx * t * t * t * t + cx * t * t * t + dx * t * t + ex * t + fx; + } + + private double y(double t) { + return ay * t * t * t * t * t + by * t * t * t * t + cy * t * t * t + dy * t * t + ey * t + fy; + } + + private double dx(double t) { + return 5 * ax * t * t * t * t + 4 * bx * t * t * t + 3 * cx * t * t + 2 * dx * t + ex; + } + + private double dy(double t) { + return 5 * ay * t * t * t * t + 4 * by * t * t * t + 3 * cy * t * t + 2 * dy * t + ey; + } + + public double calculateLength() { + + final int kNumSamples = 100000; + double integral = 0; + double integrand, last_integrand = Math.sqrt(dx(0) * dx(0) + dy(0) * dy(0)) / kNumSamples; + + for (double i = 1; i <= kNumSamples; ++i) { + double t = i / kNumSamples; + integrand = Math.sqrt(dx(t) * dx(t) + dy(t) * dy(t)) / kNumSamples; + integral += (integrand + last_integrand) / 2; + last_integrand = integrand; + } + + return integral; + } +} \ No newline at end of file diff --git a/src/main/java/com/team2363/spline/Spline.java b/src/main/java/com/team2363/spline/Spline.java new file mode 100644 index 0000000..cc66848 --- /dev/null +++ b/src/main/java/com/team2363/spline/Spline.java @@ -0,0 +1,12 @@ +package com.team2363.spline; + +import com.team2363.geometry.Pose2d; +import com.team254.lib.trajectory.Trajectory; + +public abstract class Spline { + public abstract Pose2d getPoint(double input); + + public abstract double arcLengthSegment(double t); + + public abstract double calculateLength(); +} \ No newline at end of file diff --git a/src/main/java/com/team2363/spline/SplineGenerator.java b/src/main/java/com/team2363/spline/SplineGenerator.java new file mode 100644 index 0000000..1fa7e23 --- /dev/null +++ b/src/main/java/com/team2363/spline/SplineGenerator.java @@ -0,0 +1,59 @@ +package com.team2363.spline; + +import java.util.List; + +import com.team2363.geometry.Pose2d; +import com.team254.lib.trajectory.Segment; +import com.team254.lib.trajectory.Trajectory; +import com.team319.ui.DraggableWaypoint; + +public class SplineGenerator { + + public static Spline[] getSplines(List waypoints) { + Spline[] splines = new Spline[waypoints.size() - 1]; + for (int i = 0; i < splines.length; ++i) { + splines[i] = new ParametricQuinticSpline(waypoints.get(i), waypoints.get(i + 1)); + } + return splines; + } + + public static void parametrizeSplines(Trajectory traj, Spline[] splines) { + double startPos = 0; + double splineLength = splines[0].calculateLength(); + int currentSpline = 0; + double t = 0; + double dt = 0.00001; + double integral = 0; + double lastIntegral = 0; + double result = 0; + double integrand, lastIntegrand = splines[0].arcLengthSegment(0) * dt; + for (Segment segment : traj.getSegments()) { + if (segment.pos - startPos > splineLength && currentSpline < splines.length - 1) { + startPos += splineLength; + splineLength = splines[currentSpline + 1].calculateLength(); + currentSpline++; + + integral = 0; + lastIntegral = 0; + lastIntegrand = splines[currentSpline].arcLengthSegment(0) * dt; + t = 0; + } + + while (integral < segment.pos - startPos) { + t += dt; + integrand = splines[currentSpline].arcLengthSegment(t) * dt; + integral += (integrand + lastIntegrand) / 2; + if (integral > segment.pos - startPos) { + result = (integral != lastIntegral) ? t + ((segment.pos - startPos - lastIntegral) / (integral - lastIntegral) - 1) * dt : t; + } + lastIntegrand = integrand; + lastIntegral = integral; + } + + Pose2d pose = splines[currentSpline].getPoint(Math.min(result, 1)); + segment.x = pose.x(); + segment.y = pose.y(); + segment.heading = pose.getRotation(); + } + } +} diff --git a/src/main/java/com/team254/lib/trajectory/PathGenerator.java b/src/main/java/com/team254/lib/trajectory/PathGenerator.java index 1ec0ca2..f0593d1 100644 --- a/src/main/java/com/team254/lib/trajectory/PathGenerator.java +++ b/src/main/java/com/team254/lib/trajectory/PathGenerator.java @@ -2,6 +2,9 @@ import java.util.List; +import com.team2363.geometry.Pose2d; +import com.team2363.spline.Spline; +import com.team2363.spline.SplineGenerator; import com.team319.trajectory.RobotConfig; import com.team319.ui.DraggableWaypoint; @@ -28,44 +31,11 @@ public static Trajectory generateTrajectory(List waypoints) { waypoints.get(i).getMaxVelocity())); } - assignHeadings(traj, splines); + SplineGenerator.parametrizeSplines(traj, splines); correctHeading(traj); return traj; } - private static void assignHeadings(Trajectory traj, Spline[] splines) { - // Assign headings based on the splines. - int cur_spline = 0; - double cur_spline_start_pos = 0; - double length_of_splines_finished = 0; - for (int i = 0; i < traj.getNumSegments(); ++i) { - double cur_pos = traj.getSegments().get(i).pos; - - boolean found_spline = false; - while (!found_spline) { - double cur_pos_relative = cur_pos - cur_spline_start_pos; - if (cur_pos_relative <= splines[cur_spline].calculateLength()) { - double percentage = splines[cur_spline].getPercentageForDistance(cur_pos_relative); - traj.getSegments().get(i).heading = splines[cur_spline].angleAt(percentage); - double[] coords = splines[cur_spline].getXandY(percentage); - traj.getSegments().get(i).x = coords[0]; - traj.getSegments().get(i).y = coords[1]; - found_spline = true; - } else if (cur_spline < splines.length - 1) { - length_of_splines_finished += splines[cur_spline].calculateLength(); - cur_spline_start_pos = length_of_splines_finished; - ++cur_spline; - } else { - traj.getSegments().get(i).heading = splines[splines.length - 1].angleAt(1.0); - double[] coords = splines[splines.length - 1].getXandY(1.0); - traj.getSegments().get(i).x = coords[0]; - traj.getSegments().get(i).y = coords[1]; - found_spline = true; - } - } - } - } - private static void correctHeading(Trajectory trajectory) { // Fix headings so they are continuously additive double lastUncorrectedHeading = trajectory.getSegments().get(0).heading; @@ -83,56 +53,43 @@ private static void correctHeading(Trajectory trajectory) { headingDelta = lastUncorrectedHeading - uncorrectedHeading; } - double correctedHeading = lastCorrectedHeading - headingDelta; - currentSegment.heading = correctedHeading; + currentSegment.heading = lastCorrectedHeading - headingDelta; lastUncorrectedHeading = uncorrectedHeading; - lastCorrectedHeading = correctedHeading; + lastCorrectedHeading = currentSegment.heading; } } public static TrajectorySet makeLeftAndRightTrajectories(Trajectory input) { - Trajectory[] output = new Trajectory[2]; - output[0] = input.copy(); - output[1] = input.copy(); - Trajectory left = output[0]; - Trajectory right = output[1]; + Trajectory left = input.copy(); + Trajectory right = input.copy(); for (int i = 0; i < input.getNumSegments(); ++i) { - Segment current = input.getSegments().get(i); - double cos_angle = Math.cos(current.heading); - double sin_angle = Math.sin(current.heading); + Segment currentCenter = input.getSegments().get(i); + double cos_angle = Math.cos(currentCenter.heading); + double sin_angle = Math.sin(currentCenter.heading); - Segment s_left = left.getSegments().get(i); - s_left.x = current.x - RobotConfig.wheelBase / 2 * sin_angle; - s_left.y = current.y + RobotConfig.wheelBase / 2 * cos_angle; - if (i > 0) { - // Get distance between current and last segment - double dist = Math.sqrt((s_left.x - left.getSegments().get(i - 1).x) - * (s_left.x - left.getSegments().get(i - 1).x) - + (s_left.y - left.getSegments().get(i - 1).y) - * (s_left.y - left.getSegments().get(i - 1).y)); - s_left.pos = left.getSegments().get(i - 1).pos + dist; - s_left.vel = dist / s_left.dt; - s_left.acc = (s_left.vel - left.getSegments().get(i - 1).vel) / s_left.dt; - s_left.jerk = (s_left.acc - left.getSegments().get(i - 1).acc) / s_left.dt; - } + Segment currentLeft = left.getSegments().get(i); + currentLeft.x = currentCenter.x + RobotConfig.wheelBase / 2 * sin_angle; + currentLeft.y = currentCenter.y - RobotConfig.wheelBase / 2 * cos_angle; + if (i > 0) calculateSegmentData(currentLeft, left.getSegments().get(i - 1)); - Segment s_right = right.getSegments().get(i); - s_right.x = current.x + RobotConfig.wheelBase / 2 * sin_angle; - s_right.y = current.y - RobotConfig.wheelBase / 2 * cos_angle; - if (i > 0) { - // Get distance between current and last segment - double dist = Math.sqrt((s_right.x - right.getSegments().get(i - 1).x) - * (s_right.x - right.getSegments().get(i - 1).x) - + (s_right.y - right.getSegments().get(i - 1).y) - * (s_right.y - right.getSegments().get(i - 1).y)); - s_right.pos = right.getSegments().get(i - 1).pos + dist; - s_right.vel = dist / s_right.dt; - s_right.acc = (s_right.vel - right.getSegments().get(i - 1).vel) / s_right.dt; - s_right.jerk = (s_right.acc - right.getSegments().get(i - 1).acc) / s_right.dt; - } + Segment currentRight = right.getSegments().get(i); + currentRight.x = currentCenter.x + RobotConfig.wheelBase / 2 * sin_angle; + currentRight.y = currentCenter.y - RobotConfig.wheelBase / 2 * cos_angle; + if (i > 0) calculateSegmentData(currentRight, right.getSegments().get(i - 1)); } - return new TrajectorySet(output[0], input, output[1]); + return new TrajectorySet(left, input, right); + } + + private static void calculateSegmentData(Segment current, Segment previous) { + double dist = Math.sqrt((current.x - previous.x) + * (current.x - previous.x) + + (current.y - previous.y) + * (current.y - previous.y)); + current.pos = previous.pos + dist; + current.vel = dist / current.dt; + current.acc = (current.vel - previous.vel) / current.dt; + current.jerk = (current.acc - previous.acc) / current.dt; } -} +} \ No newline at end of file diff --git a/src/main/java/com/team254/lib/trajectory/Spline.java b/src/main/java/com/team254/lib/trajectory/Spline.java deleted file mode 100644 index b7ac0f4..0000000 --- a/src/main/java/com/team254/lib/trajectory/Spline.java +++ /dev/null @@ -1,224 +0,0 @@ -package com.team254.lib.trajectory; - -import com.team254.lib.util.ChezyMath; -import com.team319.ui.DraggableWaypoint; - -/** - * Do cubic spline interpolation between points. - * - * @author Art Kalb - * @author Jared341 - */ -public class Spline { - - public static class Type { - - private final String value_; - - private Type(String value) { - this.value_ = value; - } - - public String toString() { - return value_; - } - } - - // Cubic spline where positions and first derivatives (angle) constraints will - // be met but second derivatives may be discontinuous. - public static final Type CubicHermite = new Type("CubicHermite"); - - // Quintic spline where positions and first derivatives (angle) constraints - // will be met, and all second derivatives at knots = 0. - public static final Type QuinticHermite = new Type("QuinticHermite"); - - Type type_; - double a_; // ax^5 - double b_; // + bx^4 - double c_; // + cx^3 - double d_; // + dx^2 - double e_; // + ex - // f is always 0 for the spline formulation we support. - - // The offset from the world frame to the spline frame. - // Add these to the output of the spline to obtain world coordinates. - double y_offset_; - double x_offset_; - double knot_distance_; - double theta_offset_; - double arc_length_; - - Spline() { - // All splines should be made via the static interface - arc_length_ = -1; - } - - private static boolean almostEqual(double x, double y) { - return Math.abs(x - y) < 1E-6; - } - - public static boolean reticulateSplines(DraggableWaypoint start, DraggableWaypoint goal, - Spline result, Type type) { - return reticulateSplines(start.getX(), start.getY(), start.getHeading(), goal.getX(), goal.getY(), goal.getHeading(), result, type); - } - - public static boolean reticulateSplines(double x0, double y0, double theta0, double x1, double y1, double theta1, - Spline result, Type type) { -// System.out.println("Reticulating splines..."); - result.type_ = type; - - // Transform x to the origin - result.x_offset_ = x0; - result.y_offset_ = y0; - double x1_hat = Math.sqrt((x1 - x0) * (x1 - x0) + (y1 - y0) * (y1 - y0)); - if (x1_hat == 0) { - return false; - } - result.knot_distance_ = x1_hat; - result.theta_offset_ = Math.atan2(y1 - y0, x1 - x0); - double theta0_hat = ChezyMath.getDifferenceInAngleRadians(result.theta_offset_, theta0); - double theta1_hat = ChezyMath.getDifferenceInAngleRadians(result.theta_offset_, theta1); - // We cannot handle vertical slopes in our rotated, translated basis. - // This would mean the user wants to end up 90 degrees off of the straight - // line between p0 and p1. - if (almostEqual(Math.abs(theta0_hat), Math.PI / 2) || almostEqual(Math.abs(theta1_hat), Math.PI / 2)) { - return false; - } - // We also cannot handle the case that the end angle is facing towards the - // start angle (total turn > 90 degrees). - if (Math.abs(ChezyMath.getDifferenceInAngleRadians(theta0_hat, theta1_hat)) >= Math.PI / 2) { - return false; - } - // Turn angles into derivatives (slopes) - double yp0_hat = Math.tan(theta0_hat); - double yp1_hat = Math.tan(theta1_hat); - - if (type == CubicHermite) { - // Calculate the cubic spline coefficients - result.a_ = 0; - result.b_ = 0; - result.c_ = (yp1_hat + yp0_hat) / (x1_hat * x1_hat); - result.d_ = -(2 * yp0_hat + yp1_hat) / x1_hat; - result.e_ = yp0_hat; - } else if (type == QuinticHermite) { - result.a_ = -(3 * (yp0_hat + yp1_hat)) / (x1_hat * x1_hat * x1_hat * x1_hat); - result.b_ = (8 * yp0_hat + 7 * yp1_hat) / (x1_hat * x1_hat * x1_hat); - result.c_ = -(6 * yp0_hat + 4 * yp1_hat) / (x1_hat * x1_hat); - result.d_ = 0; - result.e_ = yp0_hat; - } - - return true; - } - - public double calculateLength() { - if (arc_length_ >= 0) { - return arc_length_; - } - - final int kNumSamples = 100000; - double arc_length = 0; - double t, dydt; - double integrand, last_integrand = Math.sqrt(1 + derivativeAt(0) * derivativeAt(0)) / kNumSamples; - for (int i = 1; i <= kNumSamples; ++i) { - t = ((double) i) / kNumSamples; - dydt = derivativeAt(t); - integrand = Math.sqrt(1 + dydt * dydt) / kNumSamples; - arc_length += (integrand + last_integrand) / 2; - last_integrand = integrand; - } - arc_length_ = knot_distance_ * arc_length; - return arc_length_; - } - - public double getPercentageForDistance(double distance) { - final int kNumSamples = 100000; - double arc_length = 0; - double t = 0; - double last_arc_length = 0; - double dydt; - double integrand, last_integrand = Math.sqrt(1 + derivativeAt(0) * derivativeAt(0)) / kNumSamples; - distance /= knot_distance_; - for (int i = 1; i <= kNumSamples; ++i) { - t = ((double) i) / kNumSamples; - dydt = derivativeAt(t); - integrand = Math.sqrt(1 + dydt * dydt) / kNumSamples; - arc_length += (integrand + last_integrand) / 2; - if (arc_length > distance) { - break; - } - last_integrand = integrand; - last_arc_length = arc_length; - } - - // Interpolate between samples. - double interpolated = t; - if (arc_length != last_arc_length) { - interpolated += ((distance - last_arc_length) / (arc_length - last_arc_length) - 1) / (double) kNumSamples; - } - return interpolated; - } - - public double[] getXandY(double percentage) { - double[] result = new double[2]; - - percentage = Math.max(Math.min(percentage, 1), 0); - double x_hat = percentage * knot_distance_; - double y_hat = (a_ * x_hat + b_) * x_hat * x_hat * x_hat * x_hat + c_ * x_hat * x_hat * x_hat - + d_ * x_hat * x_hat + e_ * x_hat; - - double cos_theta = Math.cos(theta_offset_); - double sin_theta = Math.sin(theta_offset_); - - result[0] = x_hat * cos_theta - y_hat * sin_theta + x_offset_; - result[1] = x_hat * sin_theta + y_hat * cos_theta + y_offset_; - return result; - } - - public double valueAt(double percentage) { - percentage = Math.max(Math.min(percentage, 1), 0); - double x_hat = percentage * knot_distance_; - double y_hat = (a_ * x_hat + b_) * x_hat * x_hat * x_hat * x_hat + c_ * x_hat * x_hat * x_hat - + d_ * x_hat * x_hat + e_ * x_hat; - - double cos_theta = Math.cos(theta_offset_); - double sin_theta = Math.sin(theta_offset_); - - double value = x_hat * sin_theta + y_hat * cos_theta + y_offset_; - return value; - } - - private double derivativeAt(double percentage) { - percentage = Math.max(Math.min(percentage, 1), 0); - - double x_hat = percentage * knot_distance_; - double yp_hat = (5 * a_ * x_hat + 4 * b_) * x_hat * x_hat * x_hat + 3 * c_ * x_hat * x_hat + 2 * d_ * x_hat - + e_; - - return yp_hat; - } - - private double secondDerivativeAt(double percentage) { - percentage = Math.max(Math.min(percentage, 1), 0); - - double x_hat = percentage * knot_distance_; - double ypp_hat = (20 * a_ * x_hat + 12 * b_) * x_hat * x_hat + 6 * c_ * x_hat + 2 * d_; - - return ypp_hat; - } - - public double angleAt(double percentage) { - double derivative = derivativeAt(percentage); - double angle = Math.atan(derivative) + theta_offset_; - //double angle = ChezyMath.boundAngle0to2PiRadians(Math.atan(derivative) + theta_offset_); - return angle; - } - - public double angleChangeAt(double percentage) { - return ChezyMath.boundAngleNegPiToPiRadians(Math.atan(secondDerivativeAt(percentage))); - } - - public String toString() { - return "a=" + a_ + "; b=" + b_ + "; c=" + c_ + "; d=" + d_ + "; e=" + e_; - } -} diff --git a/src/main/java/com/team254/lib/trajectory/SplineGenerator.java b/src/main/java/com/team254/lib/trajectory/SplineGenerator.java deleted file mode 100644 index a67a84b..0000000 --- a/src/main/java/com/team254/lib/trajectory/SplineGenerator.java +++ /dev/null @@ -1,23 +0,0 @@ -package com.team254.lib.trajectory; - -import java.util.List; - -import com.team319.ui.DraggableWaypoint; - -public class SplineGenerator { - - public static Spline[] getSplines(List waypoints) { - Spline[] splines = new Spline[waypoints.size() - 1]; - double[] spline_lengths = new double[splines.length]; - for (int i = 0; i < splines.length; ++i) { - splines[i] = new Spline(); - if (!Spline.reticulateSplines(waypoints.get(i), waypoints.get(i + 1), splines[i], - Spline.QuinticHermite)) { - System.out.println("COULDN'T RETICULATE SPLINE!!"); - return null; - } - spline_lengths[i] = splines[i].calculateLength(); - } - return splines; - } -} diff --git a/src/main/java/com/team254/lib/trajectory/TrajectoryGenerator.java b/src/main/java/com/team254/lib/trajectory/TrajectoryGenerator.java index f5ab94c..d1bbe35 100644 --- a/src/main/java/com/team254/lib/trajectory/TrajectoryGenerator.java +++ b/src/main/java/com/team254/lib/trajectory/TrajectoryGenerator.java @@ -1,50 +1,75 @@ package com.team254.lib.trajectory; +import java.util.ArrayList; +import java.util.List; + import com.team319.trajectory.RobotConfig; public class TrajectoryGenerator { - public static Trajectory generate(double startVelocity, double startDistance, double finalDistance, double finalVelocity, double maxVelocity) { - Trajectory trajectory = new Trajectory(); - double currentVelocity = startVelocity; - double currentDistance = startDistance; + public static Trajectory generate(double startVelocity, double startPosition, double finalPosition, double finalVelocity, double maxVelocity) { double dt = RobotConfig.dt; - + + List segments = new ArrayList<>(); + double currentVelocity = startVelocity; + double currentPosition = startPosition; + + double nextVelocity = startVelocity + RobotConfig.maxAcceleration * dt; + double nextPosition = startPosition + (startVelocity + nextVelocity) / 2 * dt; + // Add ramp up points - while(isEnoughDistanceRemainingToRampDown(currentVelocity, finalVelocity, finalDistance - currentDistance)) { - currentVelocity += RobotConfig.maxAcceleration * dt; - currentVelocity = Math.min(maxVelocity, currentVelocity); - currentDistance += currentVelocity * dt; - + while(isEnoughDistanceRemainingToRampDown(nextVelocity, finalVelocity, finalPosition - nextPosition)) { + currentVelocity = nextVelocity; + currentPosition = nextPosition; + Segment current = new Segment(); - current.pos = currentDistance; + current.pos = currentPosition; current.vel = currentVelocity; current.acc = RobotConfig.maxAcceleration; current.dt = dt; - trajectory.getSegments().add(current); - } + segments.add(current); + + nextVelocity = currentVelocity + RobotConfig.maxAcceleration * dt; + nextVelocity = Math.min(maxVelocity, nextVelocity); + nextPosition = currentPosition + (currentVelocity + nextVelocity) / 2 * dt; + } + double remainingDistance = finalPosition - currentPosition; + double rampDownAcceleration = (finalVelocity * finalVelocity - currentVelocity * currentVelocity) / 2.0 / remainingDistance; - // Add ramp down points - while(currentVelocity > finalVelocity) { - currentVelocity -= RobotConfig.maxAcceleration * dt; - currentDistance += currentVelocity * dt; + nextVelocity = currentVelocity + rampDownAcceleration * dt; + nextPosition = currentPosition + (currentVelocity + nextVelocity) / 2 * dt; + // add ramp down points + while(rampDownAcceleration != 0 && nextVelocity >= finalVelocity) { + currentVelocity = nextVelocity; + currentPosition = nextPosition; + Segment current = new Segment(); - current.pos = Math.min(currentDistance, finalDistance); + current.pos = currentPosition; current.vel = currentVelocity; - current.acc = -RobotConfig.maxAcceleration; + current.acc = rampDownAcceleration; current.dt = dt; - trajectory.getSegments().add(current); - } + segments.add(current); + + nextVelocity = currentVelocity + rampDownAcceleration * dt; + nextPosition = currentPosition + (currentVelocity + nextVelocity) / 2 * dt; + } + + Trajectory trajectory = new Trajectory(); + trajectory.getSegments().addAll(segments); return trajectory; } private static boolean isEnoughDistanceRemainingToRampDown(double currentVelocity, double finalVelocity, double distanceRemaining) { - double timeToDecellerate = (currentVelocity - finalVelocity) / RobotConfig.maxAcceleration; - double distanceToDecellerate = (currentVelocity + finalVelocity) / 2.0 * timeToDecellerate; + double distanceToDecellerate = getRampDownDistanceNeeded(currentVelocity, finalVelocity); return distanceToDecellerate < distanceRemaining; } -} + + private static double getRampDownDistanceNeeded(double currentVelocity, double finalVelocity) { + double timeToDecellerate = (currentVelocity - finalVelocity) / RobotConfig.maxAcceleration; + return (currentVelocity + finalVelocity) / 2.0 * timeToDecellerate; + } +} \ No newline at end of file diff --git a/src/main/java/com/team319/ui/ClickableSpline.java b/src/main/java/com/team319/ui/ClickableSpline.java index 3921caf..d837561 100644 --- a/src/main/java/com/team319/ui/ClickableSpline.java +++ b/src/main/java/com/team319/ui/ClickableSpline.java @@ -8,7 +8,7 @@ import javax.swing.JOptionPane; -import com.team254.lib.trajectory.Spline; +import com.team2363.spline.Spline; public class ClickableSpline implements MouseListener{ private static final int DIAMETER = 20; @@ -28,8 +28,8 @@ public class ClickableSpline implements MouseListener{ public ClickableSpline(Spline spline, Plotter parentPanel, DraggableWaypoint startPoint, DraggableWaypoint endPoint) { this.spline = spline; this.parentPanel = parentPanel; - this.clickableX = spline.getXandY(0.5)[0]; - this.clickableY = spline.getXandY(0.5)[1]; + this.clickableX = spline.getPoint(0.5).x(); + this.clickableY = spline.getPoint(0.5).y(); this.endPoint = endPoint; this.startPoint = startPoint; parentPanel.addMouseListener(this); @@ -55,9 +55,8 @@ private void drawHighlight(Graphics2D gc) { private void drawSpline(Graphics2D gc) { for (double i = 0; i <= 1; i += 0.005) { - double[] xy = spline.getXandY(i); - gc.fillOval( - Plotter.convertXToPixel(xy[0]) - 2, Plotter.convertYToPixel(xy[1]) - 2, 4, 4); + gc.fillOval( + Plotter.convertXToPixel(spline.getPoint(i).x()) - 2, Plotter.convertYToPixel(spline.getPoint(i).y()) - 2, 4, 4); } } diff --git a/src/main/java/com/team319/ui/DraggableWaypoint.java b/src/main/java/com/team319/ui/DraggableWaypoint.java index 16315c9..460ead6 100644 --- a/src/main/java/com/team319/ui/DraggableWaypoint.java +++ b/src/main/java/com/team319/ui/DraggableWaypoint.java @@ -184,6 +184,10 @@ public void setMaxVelocity(double maxVelocity) { this.maxVelocity = maxVelocity; } + public double getDistance(DraggableWaypoint w1) { + return Math.sqrt((w1.getX() - getX()) * (w1.getX() - getX()) + (w1.getY() - getY()) * (w1.getY() - getY())); + } + /** * @return the isFirst */ diff --git a/src/main/java/com/team319/ui/Plotter.java b/src/main/java/com/team319/ui/Plotter.java index eccdcb7..b79a9b6 100644 --- a/src/main/java/com/team319/ui/Plotter.java +++ b/src/main/java/com/team319/ui/Plotter.java @@ -11,8 +11,8 @@ import javax.imageio.ImageIO; import javax.swing.JPanel; -import com.team254.lib.trajectory.Spline; -import com.team254.lib.trajectory.SplineGenerator; +import com.team2363.spline.Spline; +import com.team2363.spline.SplineGenerator; import com.team319.trajectory.BobPath; public class Plotter extends JPanel {