...
 
Commits (3)
......@@ -22,7 +22,6 @@ 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{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};
......
......@@ -49,6 +49,8 @@ Eigen::MatrixXd care(const Eigen::MatrixXd& A, const Eigen::MatrixXd& B,
for (int c = 0; c < W0.cols(); ++c) {
for (int r = 0; r < W0.rows(); ++r) {
converged &= std::fabs(W0(r, c) - W(r, c)) < eps;
// If you trigger this assert, it means your lqr problem is
// ill-conditioned.
assert(!std::isnan(W(r, c)));
}
}
......
......@@ -18,8 +18,8 @@ TEST(testInternals, doubleInterpolation) {
/// Check linear interpolation of two States.
TEST(testInternals, stateInterpolation) {
State s0{1.0, 1.0, 1.0, 1.0, 1.0};
State s1{2.0, 2.0, 2.0, 2.0, 2.0};
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 s2 = linearlyInterpolate(0.75, s0, s1);
ASSERT_DOUBLE_EQ(s2.x, 1.75);
......@@ -32,8 +32,8 @@ TEST(testInternals, stateInterpolation) {
/// Look up state at a given time in a trajectory.
TEST(testInternals, trajectoryLookup) {
Trajectory traj;
traj.emplace_back(State{0.0, 0.0, 0.0, 0.0, 0.0});
traj.emplace_back(State{1.0, 1.0, 1.0, 10.0, 1.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});
State s = indexTrajectory(0.75, traj);
......@@ -47,8 +47,8 @@ TEST(testInternals, trajectoryLookup) {
/// Look up the state at a time before the trajectory start.
TEST(testInternals, trajectoryLookupStart) {
Trajectory traj;
traj.emplace_back(State{0.0, 0.0, 0.0, 0.0, 0.0});
traj.emplace_back(State{1.0, 1.0, 1.0, 10.0, 1.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});
State s = indexTrajectory(-1.0, traj);
......@@ -62,8 +62,8 @@ TEST(testInternals, trajectoryLookupStart) {
/// 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});
traj.emplace_back(State{1.0, 1.0, 1.0, 10.0, 1.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});
State s = indexTrajectory(2.0, traj);
......@@ -77,12 +77,12 @@ TEST(testInternals, trajectoryLookupEnd) {
/// Verify the procedure for checking trajectory validity.
TEST(testInternals, checkTrajectoryValid) {
Trajectory traj;
traj.emplace_back(State{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});
// Trajectory too short.
ASSERT_FALSE(checkTrajectoryValid(traj));
traj.emplace_back(State{1.0, 1.0, 1.0, 10.0, 1.0});
traj.emplace_back(State{1.0, 1.0, 1.0, 0, 10.0, 1.0});
// Trajectory is good.
ASSERT_TRUE(checkTrajectoryValid(traj));
......@@ -98,8 +98,8 @@ TEST(testInternals, checkTrajectoryValid) {
// Check projection of actState onto the ray defined by tgtState.
TEST(testInternals, stateProjections) {
State actState{0.5, 1.0, 10.0, 0.0, 0.0};
State tgtState{0.0, 0.0, 0.0, 0.0, 0.0};
ASSERT_DOUBLE_EQ(computeLateralOffset(actState, tgtState), 1.0);
ASSERT_DOUBLE_EQ(computeLongitudinalOffset(actState, tgtState), 0.5);
State actState{0.5, 1.0, 10.0, 0, 0.0, 0.0};
State tgtState{0.0, 0.0, 0.0, 0, 0.0, 0.0};
ASSERT_DOUBLE_EQ(computeLateralOffset(actState, tgtState), -1.0);
ASSERT_DOUBLE_EQ(computeLongitudinalOffset(actState, tgtState), -0.5);
}