Commit 40623ca9 by Jordan Ford

Moved some things into common.cpp and common.h.

parent 24e5e8ed
......@@ -71,4 +71,4 @@ double computeLongitudinalOffset(const State& actualState,
-std::sin(targetState.theta) * (targetState.y - actualState.y);
}
} // namespace track
\ No newline at end of file
} // namespace track
......@@ -10,23 +10,22 @@ 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_COMMON_H
} // namespace track
#endif // TRACK_COMMON_H
......@@ -2,17 +2,17 @@
namespace track {
template<class MatT>
template <class MatT>
Eigen::Matrix<typename MatT::Scalar, MatT::ColsAtCompileTime,
MatT::RowsAtCompileTime>
pseudoinverse(const MatT &mat,
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();
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) {
......@@ -24,8 +24,8 @@ pseudoinverse(const MatT &mat,
return svd.matrixV() * singularValuesInv * svd.matrixU().adjoint();
}
Eigen::MatrixXd care(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();
......@@ -63,13 +63,13 @@ Eigen::MatrixXd care(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
} // namespace track
......@@ -7,10 +7,8 @@
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);
Eigen::MatrixXd care(const Eigen::MatrixXd& A, const Eigen::MatrixXd& B,
const Eigen::MatrixXd& Q, const Eigen::MatrixXd& R);
} // namespace track
} // namespace track
#endif // TRACK_LQR_CARE_H
#include "trackLQR.h"
#include "track.h"
#include "care.h"
#include "common.h"
#include "track.h"
#include <Eigen/Dense>
#include <Eigen/SVD>
......@@ -10,19 +10,17 @@
namespace track {
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) {
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;
double finalTime = referenceTrajectory[referenceTrajectory.size() - 1].time;
int points = static_cast<int>(finalTime / epsTime);
Trajectory trackTrajectory;
trackTrajectory.reserve(points);
......
......@@ -4,7 +4,7 @@ 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)
add_executable(tests testLQR.cpp testCommon.cpp)
target_include_directories(tests PRIVATE ../src)
target_link_libraries(tests Track GTest::GTest GTest::Main Eigen3::Eigen)
add_test(NAME tests COMMAND tests)
#include <gtest/gtest.h>
#include <track.h>
#include <Eigen/Dense>
#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, careIdentity) {
Matrix3d i = Matrix3d::Identity();
MatrixXd K = track::care(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;
......
#include <gtest/gtest.h>
#include <track.h>
#include <Eigen/Dense>
#include "src/lqr/care.h"
using namespace Eigen;
using namespace track;
/// Sanity check the LQR CARE solver.
TEST(testInternals, careIdentity) {
Matrix3d i = Matrix3d::Identity();
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