Commit 389c448f by Jordan Ford

Initial commit

parents
.idea/
cmake-build-debug/
cmake-build*/
*.swp
# Use modern CMake.
cmake_minimum_required(VERSION 3.0.0)
# Name the project.
project(Track)
# Use C++14
set(CMAKE_CXX_STANDARD 14)
# Set build locations.
set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/lib)
set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/lib)
set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/bin)
# Build library.
add_subdirectory(src)
# Build tests.
add_subdirectory(test)
# Build examples.
add_subdirectory(examples)
cmake_minimum_required(VERSION 3.0.0)
add_executable(demo demo.cpp)
target_link_libraries(demo Track)
#include <Eigen/Core>
#include <track.h>
#include <iostream>
using namespace track;
using namespace Eigen;
int main() {
Trajectory refTraj;
refTraj.emplace_back(State {0, 0, 0, 1.0, 0.0});
refTraj.emplace_back(State {4, 0, 0, 1.0, 4.0});
Matrix3d qLat = Matrix3d::Identity();
Matrix2d qLon = Matrix2d::Identity();
double rLat = 1;
double rLon = 10000;
double vehicleWheelbase = 3.0;
double initialSteeringAngle = 0.0;
double initialVelocity = 2.0;
double epsTime = 0.1;
Trajectory smoothTraj = trackLQR(refTraj, qLat, rLat, qLon, rLon,
vehicleWheelbase, initialSteeringAngle,
initialVelocity, epsTime);
for(const auto& s : smoothTraj) {
std::cout << s.x << "," << s.y << "," << s.theta
<< "," << s.velocity << "," << s.time << std::endl;
}
return 0;
}
# Use modern CMake.
cmake_minimum_required(VERSION 3.0.0)
find_package(Eigen3 3.3 REQUIRED NO_MODULE)
add_library(Track
include/track.h
src/trackLQR.h
src/trackLQR.cpp
src/lqr.h
src/lqr.cpp
)
target_link_libraries(Track Eigen3::Eigen)
target_include_directories(Track PUBLIC include)
#ifndef TRACK_H_
#define TRACK_H_
#include <Eigen/Core>
#include <vector>
namespace track {
struct State {
double x;
double y;
double theta;
double velocity;
double time;
};
using Trajectory = std::vector<State>;
Trajectory trackLQR(const Trajectory& referenceTrajectory,
const Eigen::Matrix3d& qLat, const double rLat,
const Eigen::Matrix2d& qLon, const double rLon,
const double vehicleWheelbase,
const double initialSteeringAngle = 0.0,
const double initialVelocity = 1.0,
const double epsTime = 0.01);
} // namespace track
#endif // TRACK_H_
#include "lqr.h"
template <class MatT>
Eigen::Matrix<typename MatT::Scalar, MatT::ColsAtCompileTime,
MatT::RowsAtCompileTime>
pseudoinverse(const MatT &mat,
typename MatT::Scalar tolerance = 1e-7) // choose appropriately
{
typedef typename MatT::Scalar Scalar;
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.setZero();
for (unsigned int i = 0; i < singularValues.size(); ++i) {
if (singularValues(i) > tolerance) {
singularValuesInv(i, i) = Scalar{1} / singularValues(i);
} else {
singularValuesInv(i, i) = Scalar{0};
}
}
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) {
// Should check if the problem is solvable. Skip for now.
Eigen::MatrixXd S = B * R.inverse() * B.transpose();
Eigen::MatrixXd W(A.rows() + Q.rows(), A.cols() + S.cols());
Eigen::MatrixXd W0(A.rows() + Q.rows(), A.cols() + S.cols());
W0.topLeftCorner(A.rows(), A.cols()) = A;
W0.topRightCorner(S.rows(), S.cols()) = -S;
W0.bottomLeftCorner(Q.rows(), Q.cols()) = -Q;
W0.bottomRightCorner(A.cols(), A.rows()) = -A.transpose();
bool converged;
do {
converged = true;
W = W0 - 0.5 * (W0 - W0.inverse());
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)) < .01;
}
}
W0 = W;
} while (!converged);
Eigen::MatrixXd W11 = W.topLeftCorner(A.rows(), A.cols());
Eigen::MatrixXd W12 = W.topRightCorner(S.rows(), S.cols());
Eigen::MatrixXd W22 = W.bottomRightCorner(A.cols(), A.rows());
Eigen::MatrixXd W21 = W.bottomLeftCorner(Q.rows(), Q.cols());
Eigen::MatrixXd M(W12.rows() + W22.rows(), W12.cols());
Eigen::MatrixXd N(W11.rows() + W21.rows(), W11.cols());
M.topRows(W12.rows()) = W12;
M.bottomRows(W22.rows()) =
W22 + Eigen::MatrixXd::Identity(W22.rows(), W22.cols());
N.topRows(W11.rows()) =
W11 + Eigen::MatrixXd::Identity(W11.rows(), W11.cols());
N.bottomRows(W21.rows()) = W21;
Eigen::MatrixXd P = pseudoinverse(M) * (-N);
return P;
}
#ifndef TRACK_LQR_H
#define TRACK_LQR_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);
#endif // TRACK_LQR_H
#include "trackLQR.h"
#include "lqr.h"
#include "track.h"
#include <Eigen/Dense>
#include <Eigen/SVD>
#include <cmath>
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,
const double vehicleWheelbase,
const double initialSteeringAngle,
const double initialVelocity,
const double epsTime) {
assert(checkTrajectoryValid(referenceTrajectory));
using namespace Eigen;
double initialTime = referenceTrajectory[0].time;
double finalTime = referenceTrajectory[referenceTrajectory.size()-1].time;
int points = static_cast<int>(finalTime / epsTime);
Trajectory trackTrajectory;
trackTrajectory.reserve(points);
Matrix3d aLat;
Vector3d bLat;
bLat << 0, 0, 1;
Matrix2d aLon;
aLon << 0, 1, 0, 0;
Vector2d bLon;
bLon << 0, -1;
Matrix<double, 1, 1> rLatMatrix;
rLatMatrix << rLat;
Matrix<double, 1, 1> rLonMatrix;
rLonMatrix << rLon;
// Precompute LQR gain for longitudinal control.
Matrix2d pLon = lqr(aLon, bLon, qLon, rLonMatrix);
RowVector2d kLon = (1 / rLon) * bLon.transpose() * pLon;
// Set initial values for state and controls.
State state = referenceTrajectory[0];
double delta = initialSteeringAngle;
double velocity = initialVelocity;
// Iterate to fill in the new trajectory.
for (int i = 0; i <= points; ++i) {
// Record current state before computing the update.
trackTrajectory.push_back(state);
// Look up the goal state within the reference trajectory.
State targetState = indexTrajectory(state.time, referenceTrajectory);
// Compute the positional errors.
double d = computeLateralOffset(state, targetState);
double l = computeLongitudinalOffset(state, targetState);
// Compute lateral state vector.
Vector3d xLat;
xLat << d, targetState.theta - state.theta, delta;
// Compute longitudinal state vector.
Vector2d xLon;
xLon << l, targetState.velocity - state.velocity;
// Re-linearize the model for lateral control.
aLat << 0, state.velocity, 0, 0, 0, -state.velocity / vehicleWheelbase, 0,
0, 0;
// Recompute the lateral control gain.
Matrix3d pLat = lqr(aLat, bLat, qLat, rLatMatrix);
RowVector3d kLat = rLatMatrix.inverse() * bLat.transpose() * pLat;
// Calculate controls.
double uLat = -kLat * xLat;
double uLon = -kLon * xLon;
double deltaDot = uLat;
double vDot = uLon;
delta += deltaDot * epsTime;
velocity += vDot * epsTime;
Vector2d controls;
controls << deltaDot, vDot;
// 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(delta) / vehicleWheelbase * epsTime;
state.velocity += vDot * epsTime;
state.time += epsTime;
}
return trackTrajectory;
}
} // namespace track
#ifndef TRACK_TRACKLQR_H
#define TRACK_TRACKLQR_H
#include "track.h"
namespace track {
/// Linearly interpolate two states. Alpha should be in [0, 1].
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);
/// Lookup the state of a trajectory at a given time.
State indexTrajectory(double time, const Trajectory& trajectory);
/// Decide whether the trajectory input is valid or not.
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);
/// 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);
} // namespace track
#endif // TRACK_TRACKLQR_H
cmake_minimum_required(VERSION 3.0)
find_package(GTest REQUIRED)
find_package(Eigen3 3.3 REQUIRED NO_MODULE)
enable_testing()
add_executable(testTrack testTrack.cpp)
target_include_directories(testTrack PRIVATE ../src)
target_link_libraries(testTrack Track GTest::GTest GTest::Main Eigen3::Eigen)
add_test(NAME testTrack COMMAND testTrack)
#include <gtest/gtest.h>
#include <track.h>
#include <Eigen/Dense>
#include "src/lqr.h"
#include "src/trackLQR.h"
using namespace Eigen;
using namespace track;
/// Sanity check the LQR CARE solver.
TEST(testInternals, lqrIdentity) {
Matrix3d i = Matrix3d::Identity();
MatrixXd K = lqr(i, i, i, i);
ASSERT_TRUE(K.isApprox(Matrix3d::Identity() * 2.41421, 1e-5));
}
/// Check linear interpolation of two doubles.
TEST(testInternals, doubleInterpolation) {
double d0 = 1.0;
double d1 = 2.0;
double d2 = linearlyInterpolate(0.75, d0, d1);
ASSERT_DOUBLE_EQ(d2, 1.75);
}
/// 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 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);
}
/// 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});
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);
}
/// 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});
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);
}
/// 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});
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);
}
/// 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});
// Trajectory too short.
ASSERT_FALSE(checkTrajectoryValid(traj));
traj.emplace_back(State{1.0, 1.0, 1.0, 10.0, 1.0});
// Trajectory is good.
ASSERT_TRUE(checkTrajectoryValid(traj));
// Times not increasing.
traj[0].time = traj[1].time + 1;
ASSERT_FALSE(checkTrajectoryValid(traj));
// Times not strictly increasing.
traj[0].time = traj[1].time;
ASSERT_FALSE(checkTrajectoryValid(traj));
}
// 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);
}
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