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;
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;
for(const auto& s : traj) {
x.push_back(s.x);
y.push_back(s.y);
x.push_back(s.x());
y.push_back(s.y());
}
plt::plot(x, y);
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() {
Trajectory refTraj;
refTraj.emplace_back(State{0, 0, 0, 0, 1.0, 0.0});
......@@ -40,24 +54,27 @@ int main() {
lqrParams.qLat << 10000, 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, 0.01);
plotTrajectory(smoothTraj, "Tracked Trajectory");
plotTrajectory(refTraj, "Tracked Trajectory");
plotStates(smoothTraj, "Tracked Trajectory");
plotStates(refTraj, "Tracked Trajectory");
plt::xlim(0, 40);
plt::ylim(-10, 10);
plt::xlabel("X");
plt::ylabel("Y");
plt::show();
plotControls(smoothTraj, "LQR Smoothed Trajectory Controls");
plt::show();
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 @@
namespace track {
struct State {
double x;
double y;
double theta;
double delta;
double velocity;
double time;
class State {
public:
inline double& x() { return state(X); }
inline double& y() { return state(Y); }
inline double& theta() { return state(THETA); }
inline double& delta() { return state(DELTA); }
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 {
......
......@@ -12,20 +12,14 @@ State linearlyInterpolate(const double alpha, const State& s0,
const State& s1) {
assert(alpha >= 0 && alpha <= 1.0);
State s;
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;
return State(s0.vec()*(1-alpha) + s1.vec()*alpha);
}
State indexTrajectory(double time, const Trajectory& trajectory) {
// Trajectory doesn't start early enough.
if (time < trajectory[0].time) {
if (time < trajectory[0].time()) {
State s = trajectory[0];
s.time = time;
s.time() = time;
return s;
}
......@@ -34,15 +28,15 @@ State indexTrajectory(double time, const Trajectory& trajectory) {
const State& s0 = trajectory[i];
const State& s1 = trajectory[i + 1];
if (s1.time > time) {
double alpha = (time - s0.time) / (s1.time - s0.time);
if (s1.time() > time) {
double alpha = (time - s0.time()) / (s1.time() - s0.time());
return linearlyInterpolate(alpha, s0, s1);
}
}
// Trajectory doesn't go long enough.
State s = trajectory[trajectory.size() - 1];
s.time = time;
s.time() = time;
return s;
}
......@@ -52,7 +46,7 @@ bool checkTrajectoryValid(const Trajectory& trajectory) {
}
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;
}
}
......@@ -61,14 +55,14 @@ bool checkTrajectoryValid(const Trajectory& trajectory) {
double computeLateralOffset(const State& actualState,
const State& targetState) {
return std::sin(targetState.theta) * (actualState.x - targetState.x) -
std::cos(targetState.theta) * (actualState.y - targetState.y);
return std::sin(targetState.theta()) * (actualState.x() - targetState.x()) -
std::cos(targetState.theta()) * (actualState.y() - targetState.y());
}
double computeLongitudinalOffset(const State& actualState,
const State& targetState) {
return -std::cos(targetState.theta) * (actualState.x - targetState.x) -
std::sin(targetState.theta) * (actualState.y - targetState.y);
return -std::cos(targetState.theta()) * (actualState.x() - targetState.x()) -
std::sin(targetState.theta()) * (actualState.y() - targetState.y());
}
} // namespace track
......@@ -9,6 +9,18 @@
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,
const Control& initialControl,
const Trajectory& referenceTrajectory,
......@@ -17,12 +29,12 @@ Trajectory trackLQR(const State& initialState,
const double epsTime) {
assert(checkTrajectoryValid(referenceTrajectory));
assert(referenceTrajectory[0].time == initialState.time);
assert(referenceTrajectory[0].time() == initialState.time());
using namespace Eigen;
double initialTime = referenceTrajectory[0].time;
double finalTime = referenceTrajectory[referenceTrajectory.size() - 1].time;
double initialTime = referenceTrajectory[0].time();
double finalTime = referenceTrajectory[referenceTrajectory.size() - 1].time();
int points = static_cast<int>((finalTime - initialTime) / epsTime);
Trajectory trackTrajectory;
trackTrajectory.reserve(points);
......@@ -54,7 +66,7 @@ Trajectory trackLQR(const State& initialState,
trackTrajectory.push_back(state);
// 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.
double d = computeLateralOffset(state, targetState);
......@@ -62,14 +74,14 @@ Trajectory trackLQR(const State& initialState,
// Compute lateral state vector.
Vector3d xLat;
xLat << d, targetState.theta - state.theta, state.delta;
xLat << d, targetState.theta() - state.theta(), state.delta();
// Compute longitudinal state vector.
Vector2d xLon;
xLon << l, targetState.velocity - state.velocity;
xLon << l, targetState.velocity() - state.velocity();
// 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;
// Recompute the lateral control gain.
......@@ -82,15 +94,19 @@ Trajectory trackLQR(const State& initialState,
controls.vDot = -kLon * xLon;
// TODO: Use RK4 to forward integrate.
/*
state.x += state.velocity * (std::cos(state.theta)) * epsTime;
state.y += state.velocity * (std::sin(state.theta)) * epsTime;
state.theta += (std::tan(state.delta) / vehicleParams.wheelbase) * epsTime;
state.delta = controls.deltaDot * epsTime;
state.velocity += controls.vDot * epsTime;
state.time += epsTime;
*/
State dState = dynamicsBicycleModel(state, controls, vehicleParams);
state.vec() += dState.vec()*epsTime;
}
return trackTrajectory;
}
} // namespace track
......@@ -18,15 +18,16 @@ TEST(testInternals, doubleInterpolation) {
/// Check linear interpolation of two States.
TEST(testInternals, stateInterpolation) {
State s0{1.0, 1.0, 1.0, 0, 1.0, 1.0};
State s1{2.0, 2.0, 2.0, 0, 2.0, 2.0};
State s0{1.0, 1.0, 1.0, 1.0, 1.0, 1.0};
State s1{2.0, 2.0, 2.0, 2.0, 2.0, 2.0};
State s2 = linearlyInterpolate(0.75, s0, s1);
ASSERT_DOUBLE_EQ(s2.x, 1.75);
ASSERT_DOUBLE_EQ(s2.y, 1.75);
ASSERT_DOUBLE_EQ(s2.theta, 1.75);
ASSERT_DOUBLE_EQ(s2.velocity, 1.75);
ASSERT_DOUBLE_EQ(s2.time, 1.75);
ASSERT_DOUBLE_EQ(s2.x(), 1.75);
ASSERT_DOUBLE_EQ(s2.y(), 1.75);
ASSERT_DOUBLE_EQ(s2.theta(), 1.75);
ASSERT_DOUBLE_EQ(s2.delta(), 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.
......@@ -37,11 +38,12 @@ TEST(testInternals, trajectoryLookup) {
State s = indexTrajectory(0.75, traj);
ASSERT_DOUBLE_EQ(s.x, 0.75);
ASSERT_DOUBLE_EQ(s.y, 0.75);
ASSERT_DOUBLE_EQ(s.theta, 0.75);
ASSERT_DOUBLE_EQ(s.velocity, 7.5);
ASSERT_DOUBLE_EQ(s.time, 0.75);
ASSERT_DOUBLE_EQ(s.x(), 0.75);
ASSERT_DOUBLE_EQ(s.y(), 0.75);
ASSERT_DOUBLE_EQ(s.theta(), 0.75);
ASSERT_DOUBLE_EQ(s.delta(), 0.0);
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.
......@@ -52,26 +54,28 @@ TEST(testInternals, trajectoryLookupStart) {
State s = indexTrajectory(-1.0, traj);
ASSERT_DOUBLE_EQ(s.x, 0.0);
ASSERT_DOUBLE_EQ(s.y, 0.0);
ASSERT_DOUBLE_EQ(s.theta, 0.0);
ASSERT_DOUBLE_EQ(s.velocity, 0.0);
ASSERT_DOUBLE_EQ(s.time, -1.0);
ASSERT_DOUBLE_EQ(s.x(), 0.0);
ASSERT_DOUBLE_EQ(s.y(), 0.0);
ASSERT_DOUBLE_EQ(s.theta(), 0.0);
ASSERT_DOUBLE_EQ(s.delta(), 0.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.
TEST(testInternals, trajectoryLookupEnd) {
Trajectory traj;
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);
ASSERT_DOUBLE_EQ(s.x, 1.0);
ASSERT_DOUBLE_EQ(s.y, 1.0);
ASSERT_DOUBLE_EQ(s.theta, 1.0);
ASSERT_DOUBLE_EQ(s.velocity, 10.0);
ASSERT_DOUBLE_EQ(s.time, 2.0);
ASSERT_DOUBLE_EQ(s.x(), 1.0);
ASSERT_DOUBLE_EQ(s.y(), 1.0);
ASSERT_DOUBLE_EQ(s.theta(), 1.0);
ASSERT_DOUBLE_EQ(s.delta(), 1.0);
ASSERT_DOUBLE_EQ(s.velocity(), 10.0);
ASSERT_DOUBLE_EQ(s.time(), 2.0);
}
/// Verify the procedure for checking trajectory validity.
......@@ -88,11 +92,11 @@ TEST(testInternals, checkTrajectoryValid) {
ASSERT_TRUE(checkTrajectoryValid(traj));
// Times not increasing.
traj[0].time = traj[1].time + 1;
traj[0].time() = traj[1].time() + 1;
ASSERT_FALSE(checkTrajectoryValid(traj));
// Times not strictly increasing.
traj[0].time = traj[1].time;
traj[0].time() = traj[1].time();
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