Commit cac349a7 by Jordan Ford

MAJOR BUG FIXED. Switched State from a POD to a wrapper for Eigen::Vector6d. In…

MAJOR BUG FIXED. Switched State from a POD to a wrapper for Eigen::Vector6d. In the process, I found that I was not linearly interpolating delta when I looked up the target state. MUCH better tracking now.
parent fc4a1434
...@@ -9,16 +9,30 @@ using namespace Eigen; ...@@ -9,16 +9,30 @@ using namespace Eigen;
namespace plt = matplotlibcpp; namespace plt = matplotlibcpp;
void plotTrajectory(const Trajectory& traj, const std::string& title) { void plotStates(const Trajectory& traj, const std::string& title) {
std::vector<double> x, y; std::vector<double> x, y;
for(const auto& s : traj) { for(const auto& s : traj) {
x.push_back(s.x); x.push_back(s.x());
y.push_back(s.y); y.push_back(s.y());
} }
plt::plot(x, y); plt::plot(x, y);
plt::title(title); plt::title(title);
} }
void plotControls(const Trajectory& traj, const std::string& title) {
std::vector<double> theta, delta, velocity, time;
for(const auto& s : traj) {
theta.push_back(s.theta());
delta.push_back(s.delta());
velocity.push_back(s.velocity());
time.push_back(s.time());
}
plt::plot(time, theta);
plt::plot(time, delta);
plt::plot(time, velocity);
plt::title(title);
}
int main() { int main() {
Trajectory refTraj; Trajectory refTraj;
refTraj.emplace_back(State{0, 0, 0, 0, 1.0, 0.0}); refTraj.emplace_back(State{0, 0, 0, 0, 1.0, 0.0});
...@@ -38,26 +52,29 @@ int main() { ...@@ -38,26 +52,29 @@ int main() {
LQRParams lqrParams; LQRParams lqrParams;
lqrParams.qLat << 10000, 0, 0, // d lqrParams.qLat << 10000, 0, 0, // d
0, 1, 0, // theta* - theta 0, 1, 0, // theta* - theta
0, 0, 1000; // delta 0, 0, 1000; // delta
//
lqrParams.rLat = 1; // deltaDot lqrParams.rLat = 1; // deltaDot
//
lqrParams.qLon << 1, 0, // l lqrParams.qLon << 1, 0, // l
0, 1; // v* - v 0, 1; // v* - v
//
lqrParams.rLon = 1; // vDot lqrParams.rLon = 1; // vDot
Trajectory smoothTraj = trackLQR(initialState, initialControl, refTraj, Trajectory smoothTraj = trackLQR(initialState, initialControl, refTraj,
lqrParams, vehicleParams, 0.01); lqrParams, vehicleParams, 0.01);
plotTrajectory(smoothTraj, "Tracked Trajectory"); plotStates(smoothTraj, "Tracked Trajectory");
plotTrajectory(refTraj, "Tracked Trajectory"); plotStates(refTraj, "Tracked Trajectory");
plt::xlim(0, 40); plt::xlim(0, 40);
plt::ylim(-10, 10); plt::ylim(-10, 10);
plt::xlabel("X"); plt::xlabel("X");
plt::ylabel("Y"); plt::ylabel("Y");
plt::show(); plt::show();
plotControls(smoothTraj, "LQR Smoothed Trajectory Controls");
plt::show();
return 0; return 0;
} }
examples/plots/demo.png

18.6 KB | W: | H:

examples/plots/demo.png

16 KB | W: | H:

examples/plots/demo.png
examples/plots/demo.png
examples/plots/demo.png
examples/plots/demo.png
  • 2-up
  • Swipe
  • Onion skin
...@@ -6,13 +6,33 @@ ...@@ -6,13 +6,33 @@
namespace track { namespace track {
struct State { class State {
double x; public:
double y; inline double& x() { return state(X); }
double theta; inline double& y() { return state(Y); }
double delta; inline double& theta() { return state(THETA); }
double velocity; inline double& delta() { return state(DELTA); }
double time; inline double& velocity() { return state(VELOCITY); }
inline double& time() { return state(TIME); }
inline Eigen::Matrix<double, 6, 1>& vec() { return state; }
inline const double& x() const { return state(X); }
inline const double& y() const { return state(Y); }
inline const double& theta() const { return state(THETA); }
inline const double& delta() const { return state(DELTA); }
inline const double& velocity() const { return state(VELOCITY); }
inline const double& time() const { return state(TIME); }
inline const Eigen::Matrix<double, 6, 1>& vec() const { return state; };
State() : state(Eigen::Matrix<double, 6, 1>::Zero()){}
explicit State(const Eigen::Matrix<double, 6, 1>& vec) : state(vec) {}
State(double x, double y, double theta, double delta, double velocity, double time) {
state << x, y, theta, delta, velocity, time;
}
private:
enum Layout {X=0, Y=1, THETA=2, DELTA=3, VELOCITY=4, TIME=5};
Eigen::Matrix<double, 6, 1> state;
}; };
struct Control { struct Control {
......
...@@ -12,20 +12,14 @@ State linearlyInterpolate(const double alpha, const State& s0, ...@@ -12,20 +12,14 @@ State linearlyInterpolate(const double alpha, const State& s0,
const State& s1) { const State& s1) {
assert(alpha >= 0 && alpha <= 1.0); assert(alpha >= 0 && alpha <= 1.0);
State s; return State(s0.vec()*(1-alpha) + s1.vec()*alpha);
s.x = linearlyInterpolate(alpha, s0.x, s1.x);
s.y = linearlyInterpolate(alpha, s0.y, s1.y);
s.theta = linearlyInterpolate(alpha, s0.theta, s1.theta);
s.velocity = linearlyInterpolate(alpha, s0.velocity, s1.velocity);
s.time = linearlyInterpolate(alpha, s0.time, s1.time);
return s;
} }
State indexTrajectory(double time, const Trajectory& trajectory) { State indexTrajectory(double time, const Trajectory& trajectory) {
// Trajectory doesn't start early enough. // Trajectory doesn't start early enough.
if (time < trajectory[0].time) { if (time < trajectory[0].time()) {
State s = trajectory[0]; State s = trajectory[0];
s.time = time; s.time() = time;
return s; return s;
} }
...@@ -34,15 +28,15 @@ State indexTrajectory(double time, const Trajectory& trajectory) { ...@@ -34,15 +28,15 @@ State indexTrajectory(double time, const Trajectory& trajectory) {
const State& s0 = trajectory[i]; const State& s0 = trajectory[i];
const State& s1 = trajectory[i + 1]; const State& s1 = trajectory[i + 1];
if (s1.time > time) { if (s1.time() > time) {
double alpha = (time - s0.time) / (s1.time - s0.time); double alpha = (time - s0.time()) / (s1.time() - s0.time());
return linearlyInterpolate(alpha, s0, s1); return linearlyInterpolate(alpha, s0, s1);
} }
} }
// Trajectory doesn't go long enough. // Trajectory doesn't go long enough.
State s = trajectory[trajectory.size() - 1]; State s = trajectory[trajectory.size() - 1];
s.time = time; s.time() = time;
return s; return s;
} }
...@@ -52,7 +46,7 @@ bool checkTrajectoryValid(const Trajectory& trajectory) { ...@@ -52,7 +46,7 @@ bool checkTrajectoryValid(const Trajectory& trajectory) {
} }
for (int i = 1; i < trajectory.size(); ++i) { for (int i = 1; i < trajectory.size(); ++i) {
if (trajectory[i - 1].time >= trajectory[i].time) { if (trajectory[i - 1].time() >= trajectory[i].time()) {
return false; return false;
} }
} }
...@@ -61,14 +55,14 @@ bool checkTrajectoryValid(const Trajectory& trajectory) { ...@@ -61,14 +55,14 @@ bool checkTrajectoryValid(const Trajectory& trajectory) {
double computeLateralOffset(const State& actualState, double computeLateralOffset(const State& actualState,
const State& targetState) { const State& targetState) {
return std::sin(targetState.theta) * (actualState.x - targetState.x) - return std::sin(targetState.theta()) * (actualState.x() - targetState.x()) -
std::cos(targetState.theta) * (actualState.y - targetState.y); std::cos(targetState.theta()) * (actualState.y() - targetState.y());
} }
double computeLongitudinalOffset(const State& actualState, double computeLongitudinalOffset(const State& actualState,
const State& targetState) { const State& targetState) {
return -std::cos(targetState.theta) * (actualState.x - targetState.x) - return -std::cos(targetState.theta()) * (actualState.x() - targetState.x()) -
std::sin(targetState.theta) * (actualState.y - targetState.y); std::sin(targetState.theta()) * (actualState.y() - targetState.y());
} }
} // namespace track } // namespace track
...@@ -9,6 +9,18 @@ ...@@ -9,6 +9,18 @@
namespace track { namespace track {
State dynamicsBicycleModel(const State& state, const Control& control, const VehicleParams& params) {
State dState;
dState.x() = state.velocity() * std::cos(state.theta());
dState.y() = state.velocity() * std::sin(state.theta());
dState.theta() = std::tan(state.delta()) / params.wheelbase;
dState.delta() = control.deltaDot;
dState.velocity() += control.vDot;
dState.time() = 1;
return dState;
}
Trajectory trackLQR(const State& initialState, Trajectory trackLQR(const State& initialState,
const Control& initialControl, const Control& initialControl,
const Trajectory& referenceTrajectory, const Trajectory& referenceTrajectory,
...@@ -17,12 +29,12 @@ Trajectory trackLQR(const State& initialState, ...@@ -17,12 +29,12 @@ Trajectory trackLQR(const State& initialState,
const double epsTime) { const double epsTime) {
assert(checkTrajectoryValid(referenceTrajectory)); assert(checkTrajectoryValid(referenceTrajectory));
assert(referenceTrajectory[0].time == initialState.time); assert(referenceTrajectory[0].time() == initialState.time());
using namespace Eigen; using namespace Eigen;
double initialTime = referenceTrajectory[0].time; double initialTime = referenceTrajectory[0].time();
double finalTime = referenceTrajectory[referenceTrajectory.size() - 1].time; double finalTime = referenceTrajectory[referenceTrajectory.size() - 1].time();
int points = static_cast<int>((finalTime - initialTime) / epsTime); int points = static_cast<int>((finalTime - initialTime) / epsTime);
Trajectory trackTrajectory; Trajectory trackTrajectory;
trackTrajectory.reserve(points); trackTrajectory.reserve(points);
...@@ -54,7 +66,7 @@ Trajectory trackLQR(const State& initialState, ...@@ -54,7 +66,7 @@ Trajectory trackLQR(const State& initialState,
trackTrajectory.push_back(state); trackTrajectory.push_back(state);
// Look up the goal state within the reference trajectory. // Look up the goal state within the reference trajectory.
State targetState = indexTrajectory(state.time, referenceTrajectory); State targetState = indexTrajectory(state.time(), referenceTrajectory);
// Compute the positional errors. // Compute the positional errors.
double d = computeLateralOffset(state, targetState); double d = computeLateralOffset(state, targetState);
...@@ -62,14 +74,14 @@ Trajectory trackLQR(const State& initialState, ...@@ -62,14 +74,14 @@ Trajectory trackLQR(const State& initialState,
// Compute lateral state vector. // Compute lateral state vector.
Vector3d xLat; Vector3d xLat;
xLat << d, targetState.theta - state.theta, state.delta; xLat << d, targetState.theta() - state.theta(), state.delta();
// Compute longitudinal state vector. // Compute longitudinal state vector.
Vector2d xLon; Vector2d xLon;
xLon << l, targetState.velocity - state.velocity; xLon << l, targetState.velocity() - state.velocity();
// Re-linearize the model for lateral control. // Re-linearize the model for lateral control.
aLat << 0, state.velocity, 0, 0, 0, -state.velocity / vehicleParams.wheelbase, 0, aLat << 0, state.velocity(), 0, 0, 0, -state.velocity() / vehicleParams.wheelbase, 0,
0, 0; 0, 0;
// Recompute the lateral control gain. // Recompute the lateral control gain.
...@@ -82,15 +94,19 @@ Trajectory trackLQR(const State& initialState, ...@@ -82,15 +94,19 @@ Trajectory trackLQR(const State& initialState,
controls.vDot = -kLon * xLon; controls.vDot = -kLon * xLon;
// TODO: Use RK4 to forward integrate. // TODO: Use RK4 to forward integrate.
/*
state.x += state.velocity * (std::cos(state.theta)) * epsTime; state.x += state.velocity * (std::cos(state.theta)) * epsTime;
state.y += state.velocity * (std::sin(state.theta)) * epsTime; state.y += state.velocity * (std::sin(state.theta)) * epsTime;
state.theta += (std::tan(state.delta) / vehicleParams.wheelbase) * epsTime; state.theta += (std::tan(state.delta) / vehicleParams.wheelbase) * epsTime;
state.delta = controls.deltaDot * epsTime; state.delta = controls.deltaDot * epsTime;
state.velocity += controls.vDot * epsTime; state.velocity += controls.vDot * epsTime;
state.time += epsTime; state.time += epsTime;
*/
State dState = dynamicsBicycleModel(state, controls, vehicleParams);
state.vec() += dState.vec()*epsTime;
} }
return trackTrajectory; return trackTrajectory;
} }
} // namespace track } // namespace track
...@@ -18,15 +18,16 @@ TEST(testInternals, doubleInterpolation) { ...@@ -18,15 +18,16 @@ TEST(testInternals, doubleInterpolation) {
/// Check linear interpolation of two States. /// Check linear interpolation of two States.
TEST(testInternals, stateInterpolation) { TEST(testInternals, stateInterpolation) {
State s0{1.0, 1.0, 1.0, 0, 1.0, 1.0}; State s0{1.0, 1.0, 1.0, 1.0, 1.0, 1.0};
State s1{2.0, 2.0, 2.0, 0, 2.0, 2.0}; State s1{2.0, 2.0, 2.0, 2.0, 2.0, 2.0};
State s2 = linearlyInterpolate(0.75, s0, s1); State s2 = linearlyInterpolate(0.75, s0, s1);
ASSERT_DOUBLE_EQ(s2.x, 1.75); ASSERT_DOUBLE_EQ(s2.x(), 1.75);
ASSERT_DOUBLE_EQ(s2.y, 1.75); ASSERT_DOUBLE_EQ(s2.y(), 1.75);
ASSERT_DOUBLE_EQ(s2.theta, 1.75); ASSERT_DOUBLE_EQ(s2.theta(), 1.75);
ASSERT_DOUBLE_EQ(s2.velocity, 1.75); ASSERT_DOUBLE_EQ(s2.delta(), 1.75);
ASSERT_DOUBLE_EQ(s2.time, 1.75); ASSERT_DOUBLE_EQ(s2.velocity(), 1.75);
ASSERT_DOUBLE_EQ(s2.time(), 1.75);
} }
/// Look up state at a given time in a trajectory. /// Look up state at a given time in a trajectory.
...@@ -37,11 +38,12 @@ TEST(testInternals, trajectoryLookup) { ...@@ -37,11 +38,12 @@ TEST(testInternals, trajectoryLookup) {
State s = indexTrajectory(0.75, traj); State s = indexTrajectory(0.75, traj);
ASSERT_DOUBLE_EQ(s.x, 0.75); ASSERT_DOUBLE_EQ(s.x(), 0.75);
ASSERT_DOUBLE_EQ(s.y, 0.75); ASSERT_DOUBLE_EQ(s.y(), 0.75);
ASSERT_DOUBLE_EQ(s.theta, 0.75); ASSERT_DOUBLE_EQ(s.theta(), 0.75);
ASSERT_DOUBLE_EQ(s.velocity, 7.5); ASSERT_DOUBLE_EQ(s.delta(), 0.0);
ASSERT_DOUBLE_EQ(s.time, 0.75); ASSERT_DOUBLE_EQ(s.velocity(), 7.5);
ASSERT_DOUBLE_EQ(s.time(), 0.75);
} }
/// Look up the state at a time before the trajectory start. /// Look up the state at a time before the trajectory start.
...@@ -52,26 +54,28 @@ TEST(testInternals, trajectoryLookupStart) { ...@@ -52,26 +54,28 @@ TEST(testInternals, trajectoryLookupStart) {
State s = indexTrajectory(-1.0, traj); State s = indexTrajectory(-1.0, traj);
ASSERT_DOUBLE_EQ(s.x, 0.0); ASSERT_DOUBLE_EQ(s.x(), 0.0);
ASSERT_DOUBLE_EQ(s.y, 0.0); ASSERT_DOUBLE_EQ(s.y(), 0.0);
ASSERT_DOUBLE_EQ(s.theta, 0.0); ASSERT_DOUBLE_EQ(s.theta(), 0.0);
ASSERT_DOUBLE_EQ(s.velocity, 0.0); ASSERT_DOUBLE_EQ(s.delta(), 0.0);
ASSERT_DOUBLE_EQ(s.time, -1.0); ASSERT_DOUBLE_EQ(s.velocity(), 0.0);
ASSERT_DOUBLE_EQ(s.time(), -1.0);
} }
/// Look up the state at a time after the trajectory end. /// Look up the state at a time after the trajectory end.
TEST(testInternals, trajectoryLookupEnd) { TEST(testInternals, trajectoryLookupEnd) {
Trajectory traj; Trajectory traj;
traj.emplace_back(State{0.0, 0.0, 0.0, 0, 0.0, 0.0}); traj.emplace_back(State{0.0, 0.0, 0.0, 0, 0.0, 0.0});
traj.emplace_back(State{1.0, 1.0, 1.0, 0, 10.0, 1.0}); traj.emplace_back(State{1.0, 1.0, 1.0, 1.0, 10.0, 1.0});
State s = indexTrajectory(2.0, traj); State s = indexTrajectory(2.0, traj);
ASSERT_DOUBLE_EQ(s.x, 1.0); ASSERT_DOUBLE_EQ(s.x(), 1.0);
ASSERT_DOUBLE_EQ(s.y, 1.0); ASSERT_DOUBLE_EQ(s.y(), 1.0);
ASSERT_DOUBLE_EQ(s.theta, 1.0); ASSERT_DOUBLE_EQ(s.theta(), 1.0);
ASSERT_DOUBLE_EQ(s.velocity, 10.0); ASSERT_DOUBLE_EQ(s.delta(), 1.0);
ASSERT_DOUBLE_EQ(s.time, 2.0); ASSERT_DOUBLE_EQ(s.velocity(), 10.0);
ASSERT_DOUBLE_EQ(s.time(), 2.0);
} }
/// Verify the procedure for checking trajectory validity. /// Verify the procedure for checking trajectory validity.
...@@ -88,11 +92,11 @@ TEST(testInternals, checkTrajectoryValid) { ...@@ -88,11 +92,11 @@ TEST(testInternals, checkTrajectoryValid) {
ASSERT_TRUE(checkTrajectoryValid(traj)); ASSERT_TRUE(checkTrajectoryValid(traj));
// Times not increasing. // Times not increasing.
traj[0].time = traj[1].time + 1; traj[0].time() = traj[1].time() + 1;
ASSERT_FALSE(checkTrajectoryValid(traj)); ASSERT_FALSE(checkTrajectoryValid(traj));
// Times not strictly increasing. // Times not strictly increasing.
traj[0].time = traj[1].time; traj[0].time() = traj[1].time();
ASSERT_FALSE(checkTrajectoryValid(traj)); ASSERT_FALSE(checkTrajectoryValid(traj));
} }
......
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