...
 
Commits (2)
.idea/
cmake-build-debug/
build/
cmake-build*/
*.swp
*.xlsx#
# Install
## Dependencies
sudo apt-get install python-matplotlib python-numpy python2.7-dev cmake
Install [Eigen](http://eigen.tuxfamily.org/index.php?title=Main_Page).
## Build
git clone git@jrdnfrd.com:motion-planning/Track.git
cd Track
mkdir build
cd build
cmake ..
make
## Run Demo
./bin/demo
cmake_minimum_required(VERSION 3.0.0)
find_package(PythonLibs 2.7)
add_executable(demo demo.cpp)
target_link_libraries(demo Track)
target_link_libraries(demo Track ${PYTHON_LIBRARIES})
target_include_directories(demo PRIVATE ${PYTHON_INCLUDE_DIRS})
#include "matplotlibcpp.h"
#include <track.h>
#include <Eigen/Core>
#include <iomanip>
......@@ -6,27 +7,46 @@
using namespace track;
using namespace Eigen;
namespace plt = matplotlibcpp;
void plotTrajectory(const Trajectory& traj, const std::string& title) {
std::vector<double> x, y;
for(const auto& s : traj) {
x.push_back(s.x);
y.push_back(s.y);
}
plt::plot(x, y);
plt::title(title);
}
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;
//refTraj.emplace_back(State{10, 0, 0, 0, 1.0, 10.0});
refTraj.emplace_back(State{40, 0, 0, 0, 1.0, 40.0});
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, 1000; // 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;
}
plotTrajectory(smoothTraj, "Tracked Trajectory");
plt::show();
return 0;
}