Commit 24e5e8ed by Jordan Ford

Reorganized so I can add other tracking algorithms more easily later.

parent 389c448f
......@@ -7,13 +7,18 @@ add_library(Track
include/track.h
src/trackLQR.h
src/trackLQR.cpp
src/common.h
src/common.cpp
src/lqr.h
src/lqr.cpp
src/lqr/trackLQR.h
src/lqr/trackLQR.cpp
src/lqr/care.h
src/lqr/care.cpp
)
target_link_libraries(Track Eigen3::Eigen)
target_include_directories(Track PUBLIC include)
target_include_directories(Track PRIVATE src)
target_include_directories(Track PRIVATE src/lqr)
#include "common.h"
namespace track {
double linearlyInterpolate(const double alpha, const double d0,
const double d1) {
assert(alpha >= 0 && alpha <= 1.0);
return (1 - alpha) * d0 + alpha * d1;
}
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;
}
State indexTrajectory(double time, const Trajectory& trajectory) {
// Trajectory doesn't start early enough.
if (time < trajectory[0].time) {
State s = trajectory[0];
s.time = time;
return s;
}
// TODO: Speed this up.
for (int i = 0; i < trajectory.size() - 1; ++i) {
const State& s0 = trajectory[i];
const State& s1 = trajectory[i + 1];
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;
return s;
}
bool checkTrajectoryValid(const Trajectory& trajectory) {
if (trajectory.size() < 2) {
return false;
}
for (int i = 1; i < trajectory.size(); ++i) {
if (trajectory[i - 1].time >= trajectory[i].time) {
return false;
}
}
return true;
}
double computeLateralOffset(const State& actualState,
const State& targetState) {
return std::sin(targetState.theta) * (targetState.x - actualState.x) -
std::cos(targetState.theta) * (targetState.y - actualState.y);
}
double computeLongitudinalOffset(const State& actualState,
const State& targetState) {
return -std::cos(targetState.theta) * (targetState.x - actualState.x) -
-std::sin(targetState.theta) * (targetState.y - actualState.y);
}
} // namespace track
\ No newline at end of file
#ifndef TRACK_TRACKLQR_H
#define TRACK_TRACKLQR_H
#ifndef TRACK_COMMON_H
#define TRACK_COMMON_H
#include "track.h"
......@@ -10,22 +10,23 @@ double linearlyInterpolate(const double alpha, const double d0,
const double d1);
/// Linearly interpolate two states. Alpha should be in [0, 1].
State linearlyInterpolate(const double alpha, const State& s0, const State& s1);
State linearlyInterpolate(const double alpha, const State &s0, const State &s1);
/// Lookup the state of a trajectory at a given time.
State indexTrajectory(double time, const Trajectory& trajectory);
State indexTrajectory(double time, const Trajectory &trajectory);
/// Decide whether the trajectory input is valid or not.
bool checkTrajectoryValid(const Trajectory& trajectory);
bool checkTrajectoryValid(const Trajectory &trajectory);
/// Project actualState onto the ray defined by targetState.
/// Return the offset distance of the actualState perpendicular to the ray.
double computeLateralOffset(const State& actualState, const State& targetState);
double computeLateralOffset(const State &actualState, const State &targetState);
/// Project actualState onto the ray defined by targetState.
/// Return the offset distance of the actualState parallel the ray.
double computeLongitudinalOffset(const State& actualState,
const State& targetState);
double computeLongitudinalOffset(const State &actualState,
const State &targetState);
} // namespace track
#endif // TRACK_TRACKLQR_H
} // namespace track
#endif // TRACK_COMMON_H
#include "lqr.h"
#include "care.h"
template <class MatT>
namespace track {
template<class MatT>
Eigen::Matrix<typename MatT::Scalar, MatT::ColsAtCompileTime,
MatT::RowsAtCompileTime>
MatT::RowsAtCompileTime>
pseudoinverse(const MatT &mat,
typename MatT::Scalar tolerance = 1e-7) // choose appropriately
{
......@@ -10,7 +12,7 @@ pseudoinverse(const MatT &mat,
auto svd = mat.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV);
const auto &singularValues = svd.singularValues();
Eigen::Matrix<Scalar, MatT::ColsAtCompileTime, MatT::RowsAtCompileTime>
singularValuesInv(mat.cols(), mat.rows());
singularValuesInv(mat.cols(), mat.rows());
singularValuesInv.setZero();
for (unsigned int i = 0; i < singularValues.size(); ++i) {
if (singularValues(i) > tolerance) {
......@@ -22,8 +24,8 @@ pseudoinverse(const MatT &mat,
return svd.matrixV() * singularValuesInv * svd.matrixU().adjoint();
}
Eigen::MatrixXd lqr(const Eigen::MatrixXd &A, const Eigen::MatrixXd &B,
const Eigen::MatrixXd &Q, const Eigen::MatrixXd &R) {
Eigen::MatrixXd care(const Eigen::MatrixXd &A, const Eigen::MatrixXd &B,
const Eigen::MatrixXd &Q, const Eigen::MatrixXd &R) {
// Should check if the problem is solvable. Skip for now.
Eigen::MatrixXd S = B * R.inverse() * B.transpose();
......@@ -61,11 +63,13 @@ Eigen::MatrixXd lqr(const Eigen::MatrixXd &A, const Eigen::MatrixXd &B,
M.topRows(W12.rows()) = W12;
M.bottomRows(W22.rows()) =
W22 + Eigen::MatrixXd::Identity(W22.rows(), W22.cols());
W22 + Eigen::MatrixXd::Identity(W22.rows(), W22.cols());
N.topRows(W11.rows()) =
W11 + Eigen::MatrixXd::Identity(W11.rows(), W11.cols());
W11 + Eigen::MatrixXd::Identity(W11.rows(), W11.cols());
N.bottomRows(W21.rows()) = W21;
Eigen::MatrixXd P = pseudoinverse(M) * (-N);
return P;
}
} // namespace track
#ifndef TRACK_LQR_H
#define TRACK_LQR_H
#ifndef TRACK_LQR_CARE_H
#define TRACK_LQR_CARE_H
#include <Eigen/Dense>
#include <Eigen/SVD>
/// solves the continuous time, infinite horizon LQR problem.
Eigen::MatrixXd lqr(const Eigen::MatrixXd &A,
const Eigen::MatrixXd &B,
const Eigen::MatrixXd &Q,
const Eigen::MatrixXd &R);
namespace track {
/// solves the continuous time, infinite horizon LQR problem.
Eigen::MatrixXd care(const Eigen::MatrixXd &A,
const Eigen::MatrixXd &B,
const Eigen::MatrixXd &Q,
const Eigen::MatrixXd &R);
#endif // TRACK_LQR_H
} // namespace track
#endif // TRACK_LQR_CARE_H
#include "trackLQR.h"
#include "lqr.h"
#include "track.h"
#include "care.h"
#include "common.h"
#include <Eigen/Dense>
#include <Eigen/SVD>
......@@ -8,75 +9,6 @@
namespace track {
double linearlyInterpolate(const double alpha, const double d0,
const double d1) {
assert(alpha >= 0 && alpha <= 1.0);
return (1 - alpha) * d0 + alpha * d1;
}
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;
}
State indexTrajectory(double time, const Trajectory& trajectory) {
// Trajectory doesn't start early enough.
if (time < trajectory[0].time) {
State s = trajectory[0];
s.time = time;
return s;
}
// TODO: Speed this up.
for (int i = 0; i < trajectory.size() - 1; ++i) {
const State& s0 = trajectory[i];
const State& s1 = trajectory[i + 1];
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;
return s;
}
inline bool checkTrajectoryValid(const Trajectory& trajectory) {
if (trajectory.size() < 2) {
return false;
}
for (int i = 1; i < trajectory.size(); ++i) {
if (trajectory[i - 1].time >= trajectory[i].time) {
return false;
}
}
return true;
}
double computeLateralOffset(const State& actualState,
const State& targetState) {
return std::sin(targetState.theta) * (targetState.x - actualState.x) -
std::cos(targetState.theta) * (targetState.y - actualState.y);
}
double computeLongitudinalOffset(const State& actualState,
const State& targetState) {
return -std::cos(targetState.theta) * (targetState.x - actualState.x) -
-std::sin(targetState.theta) * (targetState.y - actualState.y);
}
Trajectory trackLQR(const Trajectory& referenceTrajectory,
const Eigen::Matrix3d& qLat, const double rLat,
const Eigen::Matrix2d& qLon, const double rLon,
......@@ -110,7 +42,7 @@ Trajectory trackLQR(const Trajectory& referenceTrajectory,
rLonMatrix << rLon;
// Precompute LQR gain for longitudinal control.
Matrix2d pLon = lqr(aLon, bLon, qLon, rLonMatrix);
Matrix2d pLon = track::care(aLon, bLon, qLon, rLonMatrix);
RowVector2d kLon = (1 / rLon) * bLon.transpose() * pLon;
// Set initial values for state and controls.
......@@ -143,7 +75,7 @@ Trajectory trackLQR(const Trajectory& referenceTrajectory,
0, 0;
// Recompute the lateral control gain.
Matrix3d pLat = lqr(aLat, bLat, qLat, rLatMatrix);
Matrix3d pLat = track::care(aLat, bLat, qLat, rLatMatrix);
RowVector3d kLat = rLatMatrix.inverse() * bLat.transpose() * pLat;
// Calculate controls.
......
#ifndef TRACK_TRACKLQR_H
#define TRACK_TRACKLQR_H
#endif // TRACK_TRACKLQR_H
#include <gtest/gtest.h>
#include <track.h>
#include <Eigen/Dense>
#include "src/lqr.h"
#include "src/trackLQR.h"
#include "src/lqr/care.h"
#include "src/lqr/trackLQR.h"
#include "src/common.h"
using namespace Eigen;
using namespace track;
/// Sanity check the LQR CARE solver.
TEST(testInternals, lqrIdentity) {
TEST(testInternals, careIdentity) {
Matrix3d i = Matrix3d::Identity();
MatrixXd K = lqr(i, i, i, i);
MatrixXd K = track::care(i, i, i, i);
ASSERT_TRUE(K.isApprox(Matrix3d::Identity() * 2.41421, 1e-5));
}
......
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