Commit 9a124373 by Jordan Ford

Formatting the demo and playing with the weights

parent d51eabca
......@@ -6,27 +6,46 @@
using namespace track;
using namespace Eigen;
void printTrajectory(const Trajectory& traj, const std::string& desc) {
std::cout << std::fixed << std::setprecision(4);
std::cout << desc << std::endl;
std::cout << "==================================" << std::endl;
std::cout << "x, y, theta, delta, velocity, time" << std::endl;
for (const auto& s : traj) {
std::cout << s.x << ", " << s.y << ", " << s.theta << ", " << s.delta << ", "
<< s.velocity << ", " << s.time << std::endl;
}
}
int main() {
Trajectory refTraj;
refTraj.emplace_back(State{0, 0, 0, 0, 1.0, 0.0});
refTraj.emplace_back(State{10, 0, 0, 0, 1.0, 10.0});
refTraj.emplace_back(State{20, 0, 0, 0, 1.0, 20.0});
double epsTime = .02;
State initialState{0, 1, 0, 0, 2.0, 0.0};
State initialState{0, .1, 0, 0, 2.0, 0.0};
Control initialControl{0, 0};
VehicleParams vehicleParams{3.0};
LQRParams lqrParams{100*Matrix3d::Identity(), 100*Matrix2d::Identity(), 1, 1};
VehicleParams vehicleParams{3.0}; // vehicle wheelbase length
LQRParams lqrParams;
lqrParams.qLat << 1000, 0, 0, // d
0, 1, 0, // theta* - theta
0, 0, 1; // delta
lqrParams.rLat = 1; // deltaDot
lqrParams.qLon << 1, 0, // l
0, 1; // v* - v
lqrParams.rLon = 1; // vDot
Trajectory smoothTraj = trackLQR(initialState, initialControl, refTraj,
lqrParams, vehicleParams, epsTime);
lqrParams, vehicleParams, 0.01);
std::cout << std::fixed << std::setprecision(4);
for (const auto& s : smoothTraj) {
std::cout << s.x << "," << s.y << "," << s.theta << "," << s.delta << ","
<< s.velocity << "," << s.time << std::endl;
}
printTrajectory(refTraj, "Reference Trajectory");
printTrajectory(smoothTraj, "Tracked Trajectory");
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