/**
* @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);
}
}
