/** * @param p0 The starting pose of the spline * @param p1 The ending pose of the spline */ public QuinticSpline(Pose2d p0, Pose2d p1) { double scale = 1.2 * p0.getTranslation().distance(p1.getTranslation()); x0 = p0.getTranslation().x(); x1 = p1.getTranslation().x(); dx0 = p0.getRotation().cos() * scale; dx1 = p1.getRotation().cos() * scale; ddx0 = 0; ddx1 = 0; y0 = p0.getTranslation().y(); y1 = p1.getTranslation().y(); dy0 = p0.getRotation().sin() * scale; dy1 = p1.getRotation().sin() * scale; ddy0 = 0; ddy1 = 0; computeCoefficients(); }
private static void getSegmentArc(Spline s, ref List <Pose2dWithCurvature> rv, double t0, double t1, double maxDx, double maxDy, double maxDTheta) { Translation2d p0 = s.getPoint(t0); Translation2d p1 = s.getPoint(t1); Rotation2d r0 = s.getHeading(t0); Rotation2d r1 = s.getHeading(t1); Pose2d transformation = new Pose2d(new Translation2d(p0, p1).rotateBy(r0.inverse()), r1.rotateBy(r0.inverse())); Twist2d twist = Pose2d.log(transformation); if (twist.dy > maxDy || twist.dx > maxDx || twist.dtheta > maxDTheta) { getSegmentArc(s, ref rv, t0, (t0 + t1) / 2, maxDx, maxDy, maxDTheta); getSegmentArc(s, ref rv, (t0 + t1) / 2, t1, maxDx, maxDy, maxDTheta); } else { rv.Add(s.getPose2dWithCurvature(t1)); } }
public Form1() { InitializeComponent(); //Create a list of waypoints List <Pose2d> waypoints = new List <Pose2d>(3); waypoints.Add(new Pose2d(new Translation2d(0, 0), Rotation2d.fromDegrees(0))); waypoints.Add(new Pose2d(new Translation2d(100, 20), Rotation2d.fromDegrees(0))); waypoints.Add(new Pose2d(new Translation2d(254.8, 50.87), Rotation2d.fromDegrees(45))); // For a reversed trajectory List <Pose2d> waypoints_maybe_flipped = waypoints; Pose2d flip = Pose2d.fromRotation(new Rotation2d(-1, 0, false)); if (false) { waypoints_maybe_flipped = new List <Pose2d>(waypoints.Count); for (int i = 0; i < waypoints.Count; ++i) { waypoints_maybe_flipped.Add(waypoints[i].transformBy(flip)); } } //Create a list of splines between each pair of waypoints List <QuinticSpline> splines = new List <QuinticSpline>(waypoints.Count - 1); for (int i = 1; i < waypoints.Count; ++i) { splines.Add(new QuinticSpline(waypoints[i - 1], waypoints[i])); } //Doesnt do much for simple curves //Optimize spline based on curvature QuinticSpline.optimizeSpline(ref splines); Console.WriteLine("Spline Unsegmented"); foreach (QuinticSpline S in splines) { for (double t = 0; t <= 1; t += 1 / 100.0) { Console.WriteLine(S.getPoint(t).x() + " , " + S.getPoint(t).y() + " , " + S.getHeading(t).getDegrees()); } } Console.WriteLine("Spline Combined and Segmented"); //Create the untimed trajectory (Splines into segment generator) UntimedTrajectory trajectory = new UntimedTrajectory(SegmentedSplineGenerator.parameterizeSplines(splines)); for (int i = 0; i < trajectory.length(); i++) { double x = trajectory.getState(i).getTranslation().x(); double y = trajectory.getState(i).getTranslation().y(); double theta = trajectory.getState(i).getRotation().getDegrees(); double curvature = trajectory.getState(i).getCurvature(); double dCurvatureDs = trajectory.getState(i).getDCurvatureDs(); Console.WriteLine(i + " , " + x + " , " + y + " , " + theta + " , " + curvature + " , " + dCurvatureDs); } //For a reversed trajectory if (false) { List <Pose2dWithCurvature> flipped = new List <Pose2dWithCurvature>(trajectory.length()); for (int i = 0; i < trajectory.length(); ++i) { flipped.Add(new Pose2dWithCurvature(trajectory.getState(i).getPose().transformBy(flip), -trajectory.getState(i).getCurvature(), trajectory.getState(i).getDCurvatureDs())); } trajectory = new UntimedTrajectory(flipped); } Console.WriteLine("Trajectory"); TrajectoryContainer final_trajectory = TrajectoryGenerator.parameterizeTrajectory(false, trajectory, 2.0, 0.0, 0.0, 120.0, 120.0, 24.0, 20); final_trajectory.setDefaultVelocity(72.0 / 150.0); for (int i = 0; i < final_trajectory.length(); i++) { double x = final_trajectory.getState(i).get_state().getTranslation().x(); double y = final_trajectory.getState(i).get_state().getTranslation().y(); double position = Math.Sqrt(x * x + y * y); double theta = final_trajectory.getState(i).get_state().getRotation().getDegrees(); double time = final_trajectory.getState(i).get_t(); double velocity = final_trajectory.getState(i).get_velocity(); double accel = final_trajectory.getState(i).get_acceleration(); Console.WriteLine(time + " , " + position + " , " + velocity + " , " + accel); } }