Commit 9f62a1db by Jordan Ford

Made the demo more interesting.

parent a82e8606
......@@ -22,15 +22,22 @@ void plotTrajectory(const Trajectory& traj, const std::string& title) {
int main() {
Trajectory refTraj;
refTraj.emplace_back(State{0, 0, 0, 0, 1.0, 0.0});
refTraj.emplace_back(State{40, 0, 0, 0, 1.0, 40.0});
State initialState{0, .1, 0, 0, 2.0, 0.0};
refTraj.emplace_back(State{10, 0, 0, 0, 1.0, 10.0});
refTraj.emplace_back(State{10, 1, 0, 0, 1.0, 11.0});
refTraj.emplace_back(State{20, 1, 0, 0, 1.0, 21.0});
refTraj.emplace_back(State{20, 2, 0, 0, 1.0, 22.0});
refTraj.emplace_back(State{30, 2, 0, 0, 1.0, 32.0});
refTraj.emplace_back(State{30, 3, 0, 0, 1.0, 33.0});
refTraj.emplace_back(State{40, 3, 0, 0, 1.0, 43.0});
refTraj.emplace_back(State{40, 4, 0, 0, 1.0, 44.0});
State initialState{0, .5, 0, 0, 2.0, 0.0};
Control initialControl{0, 0};
VehicleParams vehicleParams{3.0}; // vehicle wheelbase length
VehicleParams vehicleParams{2.0}; // vehicle wheelbase length
LQRParams lqrParams;
lqrParams.qLat << 1000, 0, 0, // d
lqrParams.qLat << 10000, 0, 0, // d
0, 1, 0, // theta* - theta
0, 0, 1000; // delta
......@@ -45,6 +52,11 @@ int main() {
lqrParams, vehicleParams, 0.01);
plotTrajectory(smoothTraj, "Tracked Trajectory");
plotTrajectory(refTraj, "Tracked Trajectory");
plt::xlim(0, 40);
plt::ylim(-10, 10);
plt::xlabel("X");
plt::ylabel("Y");
plt::show();
return 0;
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment