From ff9e83f445127f6706af3039848801086be87a78 Mon Sep 17 00:00:00 2001 From: Prateek Machiraju Date: Sat, 29 Jun 2019 14:29:44 -0400 Subject: [PATCH] split all impls from declarations --- .../control/PurePursuitTracker.cpp | 73 ++++++++++++++ .../fl/mathematics/control/RamseteTracker.cpp | 23 +++++ .../mathematics/control/TrajectoryTracker.cpp | 51 ++++++++++ .../fl/mathematics/geometry/Rotation2d.cpp | 21 ++++ .../fl/mathematics/geometry/Translation2d.cpp | 35 +++++++ .../cpp/fl/mathematics/geometry/Twist2d.cpp | 31 ++++++ .../spline/ParametricQuinticHermiteSpline.cpp | 99 +++++++++++++++++++ .../mathematics/spline/ParametricSpline.cpp | 13 +++ .../fl/mathematics/spline/SplineGenerator.cpp | 54 ++++++++++ .../trajectory/TrajectoryGenerator.cpp | 48 +++++++++ .../AngularAccelerationConstraint.cpp | 52 ++++++++++ .../CentripetalAccelerationConstraint.cpp | 17 ++++ .../VelocityLimitRadiusConstraint.cpp | 20 ++++ .../mathematics/control/PurePursuitTracker.h | 68 +------------ .../fl/mathematics/control/RamseteTracker.h | 18 +--- .../mathematics/control/TrajectoryTracker.h | 48 +-------- .../fl/mathematics/geometry/Rotation2d.h | 25 +++-- .../fl/mathematics/geometry/Translation2d.h | 37 +++---- .../include/fl/mathematics/geometry/Twist2d.h | 17 ++-- .../spline/ParametricQuinticHermiteSpline.h | 99 +++---------------- .../fl/mathematics/spline/ParametricSpline.h | 8 +- .../fl/mathematics/spline/SplineGenerator.h | 44 +-------- .../fl/mathematics/trajectory/Trajectory.h | 3 +- .../trajectory/TrajectoryGenerator.h | 42 +------- .../AngularAccelerationConstraint.h | 52 +--------- .../CentripetalAccelerationConstraint.h | 15 +-- .../VelocityLimitRadiusConstraint.h | 18 +--- 27 files changed, 615 insertions(+), 416 deletions(-) create mode 100644 src/main/cpp/fl/mathematics/control/PurePursuitTracker.cpp create mode 100644 src/main/cpp/fl/mathematics/control/RamseteTracker.cpp create mode 100644 src/main/cpp/fl/mathematics/control/TrajectoryTracker.cpp create mode 100644 src/main/cpp/fl/mathematics/geometry/Twist2d.cpp create mode 100644 src/main/cpp/fl/mathematics/spline/ParametricQuinticHermiteSpline.cpp create mode 100644 src/main/cpp/fl/mathematics/spline/ParametricSpline.cpp create mode 100644 src/main/cpp/fl/mathematics/spline/SplineGenerator.cpp create mode 100644 src/main/cpp/fl/mathematics/trajectory/TrajectoryGenerator.cpp create mode 100644 src/main/cpp/fl/mathematics/trajectory/constraints/AngularAccelerationConstraint.cpp create mode 100644 src/main/cpp/fl/mathematics/trajectory/constraints/CentripetalAccelerationConstraint.cpp create mode 100644 src/main/cpp/fl/mathematics/trajectory/constraints/VelocityLimitRadiusConstraint.cpp diff --git a/src/main/cpp/fl/mathematics/control/PurePursuitTracker.cpp b/src/main/cpp/fl/mathematics/control/PurePursuitTracker.cpp new file mode 100644 index 0000000..f8269b3 --- /dev/null +++ b/src/main/cpp/fl/mathematics/control/PurePursuitTracker.cpp @@ -0,0 +1,73 @@ +#include "fl/mathematics/control/PurePursuitTracker.h" + +namespace fl { + +PurePursuitTracker::PurePursuitTracker(const double lat, const units::second_t lookahead_time, + const double min_lookahead_distance) + : lat_(lat), lookahead_time_(lookahead_time), min_lookahead_distance_(min_lookahead_distance) {} + +TrajectoryTrackerVelocityOutput PurePursuitTracker::CalculateState( + const TimedIterator& iterator, const Pose2d& robot_pose) const { + const auto reference_point = iterator.CurrentState(); + + // Compute the lookahead state. + const auto lookahead_state = CalculateLookaheadPose(iterator, robot_pose); + + // Find the appropriate lookahead point. + const auto lookahead_transform = lookahead_state.InFrameOfReferenceOf(robot_pose); + + // Calculate latitude error. + const auto x_error = + reference_point.state.State().Pose().InFrameOfReferenceOf(robot_pose).Translation().X(); + + // Calculate the velocity at the reference point. + const double vd = units::unit_cast(reference_point.state.Velocity()); + + // Calculate the distance from the robot to the lookahead. + const double l = lookahead_transform.Translation().Norm(); + + // Calculate the curvature of the arc that connects the robot and the lookahead point. + const double curvature = 2 * lookahead_transform.Translation().Y() / std::pow(l, 2); + + // Adjust the linear velocity to compensate for the robot lagging behind. + const double adjusted_linear_velocity = vd * lookahead_transform.Rotation().Cos() + lat_ * x_error; + + return {adjusted_linear_velocity, adjusted_linear_velocity * curvature}; +} + +Pose2d PurePursuitTracker::CalculateLookaheadPose(const TimedIterator& iterator, + const Pose2d& robot_pose) const { + auto lookahead_pose_by_time = iterator.Preview(lookahead_time_).state.State().Pose(); + + // The lookahead point is farther from the robot than the minimum lookahead distance. + // Therefore we can use this point. + if (lookahead_pose_by_time.InFrameOfReferenceOf(robot_pose).Translation().Norm() >= + min_lookahead_distance_) { + return lookahead_pose_by_time; + } + + auto lookahead_pose_by_distance = iterator.CurrentState().state.State().Pose(); + auto previewed_time = units::second_t(0.0); + + // Run the loop until a distance that is greater than the minimum lookahead distance is found or until + // we run out of "trajectory" to search. If this happens, we will simply extend the end of the trajectory. + while (iterator.RemainingProgress() > previewed_time) { + previewed_time += 0.02_s; + lookahead_pose_by_distance = iterator.Preview(previewed_time).state.State().Pose(); + + const auto lookahead_distance = + lookahead_pose_by_distance.InFrameOfReferenceOf(robot_pose).Translation().Norm(); + + if (lookahead_distance > min_lookahead_distance_) { + return lookahead_pose_by_distance; + } + } + + const auto remaining = min_lookahead_distance_ - + (lookahead_pose_by_distance.InFrameOfReferenceOf(robot_pose)).Translation().Norm(); + + // Extend the trajectory + return lookahead_pose_by_distance.TransformBy( + Pose2d{Translation2d{remaining * (iterator.Reversed() ? -1.0 : 1.0), 0.0}, Rotation2d{}}); +} +} // namespace fl diff --git a/src/main/cpp/fl/mathematics/control/RamseteTracker.cpp b/src/main/cpp/fl/mathematics/control/RamseteTracker.cpp new file mode 100644 index 0000000..bb17869 --- /dev/null +++ b/src/main/cpp/fl/mathematics/control/RamseteTracker.cpp @@ -0,0 +1,23 @@ +#include "fl/mathematics/control/RamseteTracker.h" + +namespace fl { + +RamseteTracker::RamseteTracker(const double beta, const double zeta) : beta_(beta), zeta_(zeta) {} + +TrajectoryTrackerVelocityOutput RamseteTracker::CalculateState( + const TimedIterator& iterator, const Pose2d& robot_pose) const { + const TimedEntry reference_state = iterator.CurrentState().state; + const Pose2d error = reference_state.State().Pose().InFrameOfReferenceOf(robot_pose); + + const auto vd = units::unit_cast(reference_state.Velocity()); + const auto wd = vd * reference_state.State().Curvature(); + + const auto k1 = 2 * zeta_ * std::sqrt(wd * wd + beta_ * vd * vd); + const auto angle_error = error.Rotation().Radians(); + + const auto linear = vd * error.Rotation().Cos() + k1 * error.Translation().X(); + const auto angular = wd + beta_ * vd * Sinc(angle_error) * error.Translation().Y() + k1 * angle_error; + + return {linear, angular}; +} +} // namespace fl diff --git a/src/main/cpp/fl/mathematics/control/TrajectoryTracker.cpp b/src/main/cpp/fl/mathematics/control/TrajectoryTracker.cpp new file mode 100644 index 0000000..feb46aa --- /dev/null +++ b/src/main/cpp/fl/mathematics/control/TrajectoryTracker.cpp @@ -0,0 +1,51 @@ +#include "fl/mathematics/control/TrajectoryTracker.h" + +namespace fl { + +void TrajectoryTracker::Reset(const TimedTrajectory& trajectory) { + iterator_ = static_cast*>(trajectory.Iterator().get()); + previous_velocity_ = nullptr; + previous_time_ = units::second_t(-1); +} + +TrajectoryTrackerOutput TrajectoryTracker::NextState(const Pose2d& current_pose, + const units::second_t current_time) { + if (iterator_ == nullptr) throw std::exception("Iterator was nullptr."); + auto& iterator = *iterator_; + + const auto dt = (units::unit_cast(previous_time_) < 0.0) + ? units::second_t{0.0} + : current_time - previous_time_; + previous_time_ = current_time; + + iterator.Advance(dt); + + const auto velocity = CalculateState(iterator, current_pose); + + const auto linear_velocity = velocity.linear_velocity; + const auto angular_velocity = velocity.angular_velocity; + + if (previous_velocity_ == nullptr || dt <= units::second_t()) { + previous_velocity_.reset(new TrajectoryTrackerVelocityOutput{linear_velocity, angular_velocity}); + return {linear_velocity, 0.0, angular_velocity, 0.0}; + } + + const auto _dt = units::unit_cast(dt); + + const TrajectoryTrackerOutput output{ + linear_velocity, (linear_velocity - previous_velocity_->linear_velocity) / _dt, angular_velocity, + (angular_velocity - previous_velocity_->angular_velocity) / _dt + }; + + previous_velocity_->linear_velocity = linear_velocity; + previous_velocity_->angular_velocity = angular_velocity; + + return output; +} + +TrajectorySamplePoint> TrajectoryTracker::ReferencePoint() const { + return iterator_->CurrentState(); +} + +bool TrajectoryTracker::IsFinished() const { return iterator_->IsDone(); } +} diff --git a/src/main/cpp/fl/mathematics/geometry/Rotation2d.cpp b/src/main/cpp/fl/mathematics/geometry/Rotation2d.cpp index dbd4b07..dc219e0 100644 --- a/src/main/cpp/fl/mathematics/geometry/Rotation2d.cpp +++ b/src/main/cpp/fl/mathematics/geometry/Rotation2d.cpp @@ -25,6 +25,27 @@ Rotation2d Rotation2d::FromDegrees(const double degrees) { return Rotation2d(Deg2Rad(degrees)); } +Rotation2d Rotation2d::operator-(const Rotation2d& other) const { return *this + -other; } + +Rotation2d Rotation2d::operator-() const { return Rotation2d(-value_); } + +Rotation2d Rotation2d::operator+(const Rotation2d& other) const { + return Rotation2d{ + Cos() * other.Cos() - Sin() * other.Sin(), Cos() * other.Sin() + Sin() * other.Cos(), + true + }; +} + +double Rotation2d::Radians() const { return value_; } + +double Rotation2d::Degrees() const { return Rad2Deg(value_); } + +double Rotation2d::Cos() const { return cos_; } + +double Rotation2d::Sin() const { return sin_; } + +double Rotation2d::Tan() const { return sin_ / cos_; } + bool Rotation2d::IsParallel(const Rotation2d& other) const { return EpsilonEquals((*this - other).Radians(), 0.0); } diff --git a/src/main/cpp/fl/mathematics/geometry/Translation2d.cpp b/src/main/cpp/fl/mathematics/geometry/Translation2d.cpp index 8bb5d1f..170d15a 100644 --- a/src/main/cpp/fl/mathematics/geometry/Translation2d.cpp +++ b/src/main/cpp/fl/mathematics/geometry/Translation2d.cpp @@ -7,6 +7,10 @@ Translation2d::Translation2d(const double x, const double y) : x_(x), y_(y) {} Translation2d::Translation2d(const double length, const Rotation2d& rotation) : x_(length * rotation.Cos()), y_(length * rotation.Sin()) {} +double Translation2d::Distance(const Translation2d& other) const { + return std::hypot(other.X() - X(), other.Y() - Y()); +} + Translation2d Translation2d::Interpolate(const Translation2d& end_value, const double t) const { if (t <= 0) { return *this; @@ -17,4 +21,35 @@ Translation2d Translation2d::Interpolate(const Translation2d& end_value, const d return Translation2d{Lerp(X(), end_value.X(), t), Lerp(Y(), end_value.Y(), t)}; } +Translation2d Translation2d::operator+(const Translation2d& other) const { + return Translation2d{X() + other.X(), Y() + other.Y()}; +} + +Translation2d Translation2d::operator-(const Translation2d& other) const { + return Translation2d{X() - other.X(), Y() - other.Y()}; +} + +Translation2d Translation2d::operator*(const double scalar) const { + return Translation2d{X() * scalar, Y() * scalar}; +} + +Translation2d Translation2d::operator*(const Rotation2d& rotation) const { + return Translation2d{ + x_ * rotation.Cos() - y_ * rotation.Sin(), + x_ * rotation.Sin() + y_ * rotation.Cos() + }; +} + +Translation2d Translation2d::operator/(const double scalar) const { + return Translation2d{X() / scalar, Y() / scalar}; +} + +Translation2d Translation2d::operator-() const { return Translation2d{-X(), -Y()}; } + +double Translation2d::X() const { return x_; } + +double Translation2d::Y() const { return y_; } + +double Translation2d::Norm() const { return std::hypot(x_, y_); } + } // namespace fl diff --git a/src/main/cpp/fl/mathematics/geometry/Twist2d.cpp b/src/main/cpp/fl/mathematics/geometry/Twist2d.cpp new file mode 100644 index 0000000..f26368c --- /dev/null +++ b/src/main/cpp/fl/mathematics/geometry/Twist2d.cpp @@ -0,0 +1,31 @@ +#include "fl/mathematics/geometry/Twist2d.h" + +namespace fl { + +Twist2d::Twist2d() + : dx_(0.0), + dy_(0.0), + dtheta_(0.0) { +} + +Twist2d::Twist2d(const double dx, const double dy, const double dtheta) + : dx_(dx), + dy_(dy), + dtheta_(dtheta) { +} + +Twist2d Twist2d::operator*(const double scalar) const { + return {dx_ * scalar, dy_ * scalar, dtheta_ * scalar}; +} + +double Twist2d::Dx() const { return dx_; } + +double Twist2d::Dy() const { return dy_; } + +double Twist2d::Dtheta() const { return dtheta_; } + +double Twist2d::Norm() const { + if (dy_ == 0.0) return std::abs(dx_); + return std::hypot(dx_, dy_); +} +} diff --git a/src/main/cpp/fl/mathematics/spline/ParametricQuinticHermiteSpline.cpp b/src/main/cpp/fl/mathematics/spline/ParametricQuinticHermiteSpline.cpp new file mode 100644 index 0000000..bc75a46 --- /dev/null +++ b/src/main/cpp/fl/mathematics/spline/ParametricQuinticHermiteSpline.cpp @@ -0,0 +1,99 @@ +#include "fl/mathematics/spline/ParametricQuinticHermiteSpline.h" + +namespace fl { +ParametricQuinticHermiteSpline::ParametricQuinticHermiteSpline(const Pose2d& start, const Pose2d& end) { + const auto scale_factor = 1.2 * start.Translation().Distance(end.Translation()); + + x0_ = start.Translation().X(); + x1_ = end.Translation().X(); + dx0_ = scale_factor * start.Rotation().Cos(); + dx1_ = scale_factor * end.Rotation().Cos(); + ddx0_ = 0.; + ddx1_ = 0.; + + y0_ = start.Translation().Y(); + y1_ = end.Translation().Y(); + dy0_ = scale_factor * start.Rotation().Sin(); + dy1_ = scale_factor * end.Rotation().Sin(); + ddy0_ = 0.; + ddy1_ = 0.; + + ax_ = -6 * x0_ - 3 * dx0_ - 0.5 * ddx0_ + 0.5 * ddx1_ - 3 * dx1_ + 6 * x1_; + bx_ = 15 * x0_ + 8 * dx0_ + 1.5 * ddx0_ - ddx1_ + 7 * dx1_ - 15 * x1_; + cx_ = -10 * x0_ - 6 * dx0_ - 1.5 * ddx0_ + 0.5 * ddx1_ - 4 * dx1_ + 10 * x1_; + dx_ = 0.5 * ddx0_; + ex_ = dx0_; + fx_ = x0_; + + ay_ = -6 * y0_ - 3 * dy0_ - 0.5 * ddy0_ + 0.5 * ddy1_ - 3 * dy1_ + 6 * y1_; + by_ = 15 * y0_ + 8 * dy0_ + 1.5 * ddy0_ - ddy1_ + 7 * dy1_ - 15 * y1_; + cy_ = -10 * y0_ - 6 * dy0_ - 1.5 * ddy0_ + 0.5 * ddy1_ - 4 * dy1_ + 10 * y1_; + dy_ = 0.5 * ddy0_; + ey_ = dy0_; + fy_ = y0_; + + start_ = start; + end_ = end; +} + +const Pose2d& ParametricQuinticHermiteSpline::StartPose() const { + return start_; +} + +const Pose2d& ParametricQuinticHermiteSpline::EndPose() const { + return end_; +} + +Translation2d ParametricQuinticHermiteSpline::Point(const double t) const { + return Translation2d{ax_ * std::pow(t, 5) + bx_ * std::pow(t, 4) + cx_ * std::pow(t, 3) + + dx_ * std::pow(t, 2) + ex_ * t + fx_, + ay_ * std::pow(t, 5) + by_ * std::pow(t, 4) + cy_ * std::pow(t, 3) + + dy_ * std::pow(t, 2) + ey_ * t + fy_}; +} + +Rotation2d ParametricQuinticHermiteSpline::Heading(const double t) const { + return {Dx(t), Dy(t), true}; +} + +double ParametricQuinticHermiteSpline::Curvature(const double t) const { + return (Dx(t) * Ddy(t) - Ddx(t) * Dy(t)) / ((Dx(t) * Dx(t) + Dy(t) * Dy(t)) * Velocity(t)); +} + +double ParametricQuinticHermiteSpline::DCurvature(const double t) const { + const auto dx_2dy2 = Dx(t) * Dx(t) + Dy(t) * Dy(t); + const auto num = (Dx(t) * Dddy(t) - Dddx(t) * Dy(t)) * dx_2dy2 - + 3.0 * (Dx(t) * Ddy(t) - Ddx(t) * Dy(t)) * (Dx(t) * Ddx(t) + Dy(t) * Ddy(t)); + return num / (dx_2dy2 * dx_2dy2 * std::sqrt(dx_2dy2)); +} + +double ParametricQuinticHermiteSpline::Velocity(const double t) const { + return std::hypot(Dx(t), Dy(t)); +} + +double ParametricQuinticHermiteSpline::Dx(const double t) const { + return 5.0 * ax_ * std::pow(t, 4) + 4.0 * bx_ * std::pow(t, 3) + 3.0 * cx_ * std::pow(t, 2) + + 2.0 * dx_ * t + ex_; +} + +double ParametricQuinticHermiteSpline::Dy(const double t) const { + return 5.0 * ay_ * std::pow(t, 4) + 4.0 * by_ * std::pow(t, 3) + 3.0 * cy_ * std::pow(t, 2) + + 2.0 * dy_ * t + ey_; +} + +double ParametricQuinticHermiteSpline::Ddx(const double t) const { + return 20.0 * ax_ * std::pow(t, 3) + 12.0 * bx_ * std::pow(t, 2) + 6.0 * cx_ * t + 2 * dx_; +} + +double ParametricQuinticHermiteSpline::Ddy(const double t) const { + return 20.0 * ay_ * std::pow(t, 3) + 12.0 * by_ * std::pow(t, 2) + 6.0 * cy_ * t + 2 * dy_; +} + +double ParametricQuinticHermiteSpline::Dddx(const double t) const { + return 60.0 * ax_ * std::pow(t, 2) + 24.0 * bx_ * t + 6 * cx_; +} + +double ParametricQuinticHermiteSpline::Dddy(const double t) const { + return 60.0 * ay_ * std::pow(t, 2) + 24.0 * by_ * t + 6 * cy_; +} + +} // namespace fl \ No newline at end of file diff --git a/src/main/cpp/fl/mathematics/spline/ParametricSpline.cpp b/src/main/cpp/fl/mathematics/spline/ParametricSpline.cpp new file mode 100644 index 0000000..8085542 --- /dev/null +++ b/src/main/cpp/fl/mathematics/spline/ParametricSpline.cpp @@ -0,0 +1,13 @@ +#include "fl/mathematics/spline/ParametricSpline.h" + +namespace fl { + +Pose2dWithCurvature ParametricSpline::PoseWithCurvature(const double t) const { + return Pose2dWithCurvature{Pose(t), Curvature(t), DCurvature(t) / Velocity(t)}; +} + +Pose2d ParametricSpline::Pose(const double t) const { + return Pose2d{Point(t), Heading(t)}; +} + +} // namespace fl diff --git a/src/main/cpp/fl/mathematics/spline/SplineGenerator.cpp b/src/main/cpp/fl/mathematics/spline/SplineGenerator.cpp new file mode 100644 index 0000000..dadec01 --- /dev/null +++ b/src/main/cpp/fl/mathematics/spline/SplineGenerator.cpp @@ -0,0 +1,54 @@ +#include "fl/mathematics/spline/SplineGenerator.h" + +namespace fl { +std::vector SplineGenerator::ParameterizeSpline( + const std::shared_ptr& spline, const double max_dx, const double max_dy, + const double max_dtheta, const double t0, const double t1) { + const auto dt = t1 - t0; + auto rv = std::vector(static_cast(kMinSampleSize / dt)); + + rv.push_back(spline->PoseWithCurvature(0)); + + for (double t = 0; t < t1; t += dt / kMinSampleSize) { + GetSegmentArc(spline, &rv, t, t + dt / kMinSampleSize, max_dx, max_dy, max_dtheta); + } + + return rv; +} + +std::vector SplineGenerator::ParameterizeSplines( + std::vector> splines, const double max_dx, const double max_dy, + const double max_dtheta) { + auto rv = std::vector(); + if (splines.empty()) return rv; + + rv.push_back(splines[0]->PoseWithCurvature(0.0)); + for (const auto& spline : splines) { + auto samples = ParameterizeSpline(spline, max_dx, max_dy, max_dtheta); + samples.erase(samples.begin()); + rv.insert(rv.end(), samples.begin(), samples.end()); + } + + return rv; +} + +void SplineGenerator::GetSegmentArc(const std::shared_ptr& spline, + std::vector* rv, const double t0, const double t1, + const double max_dx, const double max_dy, const double max_dtheta) { + const auto p0 = spline->Point(t0); + const auto p1 = spline->Point(t1); + const auto r0 = spline->Heading(t0); + const auto r1 = spline->Heading(t1); + + const auto transformation = Pose2d{(p1 - p0) * -r0, r1 + -r0}; + const auto twist = Pose2d::ToTwist(transformation); + + if (twist.Dy() > max_dy || twist.Dx() > max_dx || twist.Dtheta() > max_dtheta) { + GetSegmentArc(spline, rv, t0, (t0 + t1) / 2, max_dx, max_dy, max_dtheta); + GetSegmentArc(spline, rv, (t0 + t1) / 2, t1, max_dx, max_dy, max_dtheta); + } else { + rv->push_back(spline->PoseWithCurvature(t1)); + } +} + +} // namespace fl \ No newline at end of file diff --git a/src/main/cpp/fl/mathematics/trajectory/TrajectoryGenerator.cpp b/src/main/cpp/fl/mathematics/trajectory/TrajectoryGenerator.cpp new file mode 100644 index 0000000..67a9f88 --- /dev/null +++ b/src/main/cpp/fl/mathematics/trajectory/TrajectoryGenerator.cpp @@ -0,0 +1,48 @@ +#include "fl/mathematics/trajectory/TrajectoryGenerator.h" + +namespace fl { +TimedTrajectory TrajectoryGenerator::GenerateTrajectory(std::vector waypoints, + const Constraints& constraints, + const double + start_velocity, + const double end_velocity, + const double max_velocity, + const double + max_acceleration, + const bool reversed) { + const auto flipped_position = Pose2d{Translation2d{}, Rotation2d::FromDegrees(180.0)}; + + if (reversed) { + for (auto& waypoint : waypoints) { + waypoint = waypoint.TransformBy(flipped_position); + } + } + + const auto indexed_trajectory = TrajectoryFromSplineWaypoints(waypoints, 2.0, 0.05, 0.1); + + auto points = indexed_trajectory.Points(); + + if (reversed) { + for (auto& point : points) { + point = + Pose2dWithCurvature{point.Pose().TransformBy(flipped_position), -point.Curvature(), point.Dkds()}; + } + } + + return TimeParameterizeTrajectory(DistanceTrajectory(points), constraints, + start_velocity, end_velocity, max_velocity, max_acceleration, 0.051, + reversed); +} + +IndexedTrajectory TrajectoryGenerator::TrajectoryFromSplineWaypoints( + const std::vector& waypoints, const double max_dx, const double max_dy, const double max_dtheta) { + std::vector> splines(waypoints.size() - 1); + for (auto i = 1; i < waypoints.size(); ++i) { + splines[i - 1] = std::make_shared(waypoints[i - 1], waypoints[i]); + } + auto trajectory = IndexedTrajectory( + SplineGenerator::ParameterizeSplines(splines, max_dx, max_dy, max_dtheta)); + + return trajectory; +} +} diff --git a/src/main/cpp/fl/mathematics/trajectory/constraints/AngularAccelerationConstraint.cpp b/src/main/cpp/fl/mathematics/trajectory/constraints/AngularAccelerationConstraint.cpp new file mode 100644 index 0000000..6fcabf6 --- /dev/null +++ b/src/main/cpp/fl/mathematics/trajectory/constraints/AngularAccelerationConstraint.cpp @@ -0,0 +1,52 @@ +#include "fl/mathematics/trajectory/constraints/AngularAccelerationConstraint.h" + +namespace fl { + +AngularAccelerationConstraint::AngularAccelerationConstraint(double max_angular_acceleration) + : max_angular_acceleration_(max_angular_acceleration) {} + +double AngularAccelerationConstraint::MaxVelocity(const Pose2dWithCurvature& state) const { + /** + * We don't want v^2 * dk/ds alone to go over the max angular acceleration. + * v^2 * dk/ds = maxAngularAcceleration when linear acceleration = 0. + * v = sqrt(maxAngularAcceleration / dk/ds) + */ + + return std::sqrt(max_angular_acceleration_ / std::abs(state.Dkds())); +} + +fl::MinMaxAcceleration AngularAccelerationConstraint::MinMaxAcceleration(const Pose2dWithCurvature& state, + const double velocity) const { + /** + * We want to limit the acceleration such that we never go above the + * + * Angular acceleration = dw/dt WHERE w = omega = angular velocity + * w = v * k WHERE v = linear velocity, k = + * curvature + * + * dw/dt = d/dt (v * k) + * + * By chain rule, + * dw/dt = dv/dt * k + v * dk/dt [1] + * + * We don't have dk/dt, but we do have dk/ds and ds/dt + * dk/ds * ds/dt = dk/dt [2] + * + * Substituting [2] in [1], we get + * dw/dt = acceleration * curvature + velocity * velocity * d_curvature + * WHERE acceleration = dv/dt, velocity = ds/dt, d_curvature = dk/dt and + * curvature = k + * + * We now want to find the linear acceleration such that the angular + * acceleration (dw/dt) never goes above the specified amount. + * + * acceleration * curvature = dw/dt - (velocity * velocity * d_curvature) + * acceleration = (dw/dt - (velocity * velocity * d_curvature)) / curvature + */ + + const auto max_absolute_acceleration = + std::abs((max_angular_acceleration_ - (velocity * velocity * state.Dkds())) / state.Curvature()); + + return fl::MinMaxAcceleration{-max_absolute_acceleration, max_absolute_acceleration}; +} +} // namespace fl diff --git a/src/main/cpp/fl/mathematics/trajectory/constraints/CentripetalAccelerationConstraint.cpp b/src/main/cpp/fl/mathematics/trajectory/constraints/CentripetalAccelerationConstraint.cpp new file mode 100644 index 0000000..6f28bae --- /dev/null +++ b/src/main/cpp/fl/mathematics/trajectory/constraints/CentripetalAccelerationConstraint.cpp @@ -0,0 +1,17 @@ +#include "fl/mathematics/trajectory/constraints/CentripetalAccelerationConstraint.h" + +namespace fl { + +CentripetalAccelerationConstraint::CentripetalAccelerationConstraint( + const double max_centripetal_acceleration) + : max_centripetal_acceleration_(max_centripetal_acceleration) {} + +double CentripetalAccelerationConstraint::MaxVelocity(const Pose2dWithCurvature& state) const { + return std::sqrt(std::abs(max_centripetal_acceleration_ / state.Curvature())); +} + +fl::MinMaxAcceleration CentripetalAccelerationConstraint::MinMaxAcceleration(const Pose2dWithCurvature& state, + double velocity) const { + return kNoLimits; +} +} // namespace fl diff --git a/src/main/cpp/fl/mathematics/trajectory/constraints/VelocityLimitRadiusConstraint.cpp b/src/main/cpp/fl/mathematics/trajectory/constraints/VelocityLimitRadiusConstraint.cpp new file mode 100644 index 0000000..6cf45f9 --- /dev/null +++ b/src/main/cpp/fl/mathematics/trajectory/constraints/VelocityLimitRadiusConstraint.cpp @@ -0,0 +1,20 @@ +#include "fl/mathematics/trajectory/constraints/VelocityLimitRadiusConstraint.h" + +namespace fl { + +VelocityLimitRadiusConstraint::VelocityLimitRadiusConstraint(const Translation2d& point, const double radius, + const double max_velocity) + : point_(point), radius_(radius), max_velocity_(max_velocity) {} + +double VelocityLimitRadiusConstraint::MaxVelocity(const Pose2dWithCurvature& state) const { + if (state.Pose().Translation().Distance(point_) < radius_) { + return max_velocity_; + } + return std::numeric_limits::max(); +} + +fl::MinMaxAcceleration VelocityLimitRadiusConstraint::MinMaxAcceleration(const Pose2dWithCurvature& state, + double velocity) const { + return kNoLimits; +} +} // namespace fl diff --git a/src/main/include/fl/mathematics/control/PurePursuitTracker.h b/src/main/include/fl/mathematics/control/PurePursuitTracker.h index 4c52a5d..190ef2a 100644 --- a/src/main/include/fl/mathematics/control/PurePursuitTracker.h +++ b/src/main/include/fl/mathematics/control/PurePursuitTracker.h @@ -7,37 +7,12 @@ namespace fl { class PurePursuitTracker : public TrajectoryTracker { public: - PurePursuitTracker(const double lat, const units::second_t lookahead_time, const double min_lookahead_distance) - : lat_(lat), lookahead_time_(lookahead_time), min_lookahead_distance_(min_lookahead_distance) {} + PurePursuitTracker(const double lat, const units::second_t lookahead_time, + const double min_lookahead_distance); TrajectoryTrackerVelocityOutput CalculateState(const TimedIterator& iterator, - const Pose2d& robot_pose) const override { - const auto reference_point = iterator.CurrentState(); - - // Compute the lookahead state. - const auto lookahead_state = CalculateLookaheadPose(iterator, robot_pose); - - // Find the appropriate lookahead point. - const auto lookahead_transform = lookahead_state.InFrameOfReferenceOf(robot_pose); - - // Calculate latitude error. - const auto x_error = - reference_point.state.State().Pose().InFrameOfReferenceOf(robot_pose).Translation().X(); - - // Calculate the velocity at the reference point. - const double vd = units::unit_cast(reference_point.state.Velocity()); - - // Calculate the distance from the robot to the lookahead. - const double l = lookahead_transform.Translation().Norm(); - - // Calculate the curvature of the arc that connects the robot and the lookahead point. - const double curvature = 2 * lookahead_transform.Translation().Y() / std::pow(l, 2); - - // Adjust the linear velocity to compensate for the robot lagging behind. - const double adjusted_linear_velocity = vd * lookahead_transform.Rotation().Cos() + lat_ * x_error; - - return {adjusted_linear_velocity, adjusted_linear_velocity * curvature}; - } + const Pose2d& robot_pose) const + override; private: double lat_; @@ -45,39 +20,6 @@ class PurePursuitTracker : public TrajectoryTracker { double min_lookahead_distance_; Pose2d CalculateLookaheadPose(const TimedIterator& iterator, - const Pose2d& robot_pose) const { - auto lookahead_pose_by_time = iterator.Preview(lookahead_time_).state.State().Pose(); - - // The lookahead point is farther from the robot than the minimum lookahead distance. - // Therefore we can use this point. - if (lookahead_pose_by_time.InFrameOfReferenceOf(robot_pose).Translation().Norm() >= - min_lookahead_distance_) { - return lookahead_pose_by_time; - } - - auto lookahead_pose_by_distance = iterator.CurrentState().state.State().Pose(); - auto previewed_time = units::second_t(0.0); - - // Run the loop until a distance that is greater than the minimum lookahead distance is found or until - // we run out of "trajectory" to search. If this happens, we will simply extend the end of the trajectory. - while (iterator.RemainingProgress() > previewed_time) { - previewed_time += 0.02_s; - lookahead_pose_by_distance = iterator.Preview(previewed_time).state.State().Pose(); - - const auto lookahead_distance = - lookahead_pose_by_distance.InFrameOfReferenceOf(robot_pose).Translation().Norm(); - - if (lookahead_distance > min_lookahead_distance_) { - return lookahead_pose_by_distance; - } - } - - const auto remaining = min_lookahead_distance_ - - (lookahead_pose_by_distance.InFrameOfReferenceOf(robot_pose)).Translation().Norm(); - - // Extend the trajectory - return lookahead_pose_by_distance.TransformBy( - Pose2d{Translation2d{remaining * (iterator.Reversed() ? -1.0 : 1.0), 0.0}, Rotation2d{}}); - } + const Pose2d& robot_pose) const; }; } // namespace fl \ No newline at end of file diff --git a/src/main/include/fl/mathematics/control/RamseteTracker.h b/src/main/include/fl/mathematics/control/RamseteTracker.h index fe2079f..86cdaf1 100644 --- a/src/main/include/fl/mathematics/control/RamseteTracker.h +++ b/src/main/include/fl/mathematics/control/RamseteTracker.h @@ -5,24 +5,10 @@ namespace fl { class RamseteTracker : public TrajectoryTracker { public: - RamseteTracker(const double beta, const double zeta) : beta_(beta), zeta_(zeta) {} + RamseteTracker(const double beta, const double zeta); TrajectoryTrackerVelocityOutput CalculateState(const TimedIterator& iterator, - const Pose2d& robot_pose) const override { - const TimedEntry reference_state = iterator.CurrentState().state; - const Pose2d error = reference_state.State().Pose().InFrameOfReferenceOf(robot_pose); - - const auto vd = units::unit_cast(reference_state.Velocity()); - const auto wd = vd * reference_state.State().Curvature(); - - const auto k1 = 2 * zeta_ * std::sqrt(wd * wd + beta_ * vd * vd); - const auto angle_error = error.Rotation().Radians(); - - const auto linear = vd * error.Rotation().Cos() + k1 * error.Translation().X(); - const auto angular = wd + beta_ * vd * Sinc(angle_error) * error.Translation().Y() + k1 * angle_error; - - return {linear, angular}; - } + const Pose2d& robot_pose) const override; private: static double Sinc(const double theta) { diff --git a/src/main/include/fl/mathematics/control/TrajectoryTracker.h b/src/main/include/fl/mathematics/control/TrajectoryTracker.h index c16f918..5e540c5 100644 --- a/src/main/include/fl/mathematics/control/TrajectoryTracker.h +++ b/src/main/include/fl/mathematics/control/TrajectoryTracker.h @@ -4,6 +4,7 @@ #include #include +#include "fl/mathematics/geometry/Pose2dWithCurvature.h" namespace fl { @@ -21,53 +22,14 @@ struct TrajectoryTrackerOutput { class TrajectoryTracker { public: - void Reset(const TimedTrajectory& trajectory) { - iterator_ = static_cast*>(trajectory.Iterator().get()); - previous_velocity_ = nullptr; - previous_time_ = units::second_t(-1); - } - - TrajectoryTrackerOutput NextState(const Pose2d& current_pose, const units::second_t current_time) { - if (iterator_ == nullptr) throw std::exception("Iterator was nullptr."); - auto& iterator = *iterator_; - - const auto dt = (units::unit_cast(previous_time_) < 0.0) ? units::second_t{0.0} - : current_time - previous_time_; - previous_time_ = current_time; - - iterator.Advance(dt); - - const auto velocity = CalculateState(iterator, current_pose); - - const auto linear_velocity = velocity.linear_velocity; - const auto angular_velocity = velocity.angular_velocity; - - if (previous_velocity_ == nullptr || dt <= units::second_t()) { - previous_velocity_.reset(new TrajectoryTrackerVelocityOutput{linear_velocity, angular_velocity}); - return {linear_velocity, 0.0, angular_velocity, 0.0}; - } - - const auto _dt = units::unit_cast(dt); - - const TrajectoryTrackerOutput output{ - linear_velocity, (linear_velocity - previous_velocity_->linear_velocity) / _dt, angular_velocity, - (angular_velocity - previous_velocity_->angular_velocity) / _dt}; - - previous_velocity_->linear_velocity = linear_velocity; - previous_velocity_->angular_velocity = angular_velocity; - - return output; - } + void Reset(const TimedTrajectory& trajectory); + TrajectoryTrackerOutput NextState(const Pose2d& current_pose, const units::second_t current_time); + TrajectorySamplePoint> ReferencePoint() const; + bool IsFinished() const; virtual TrajectoryTrackerVelocityOutput CalculateState(const TimedIterator& iterator, const Pose2d& robot_pose) const = 0; - TrajectorySamplePoint> ReferencePoint() const { - return iterator_->CurrentState(); - } - - bool IsFinished() const { return iterator_->IsDone(); } - private: TimedIterator* iterator_ = nullptr; std::unique_ptr previous_velocity_; diff --git a/src/main/include/fl/mathematics/geometry/Rotation2d.h b/src/main/include/fl/mathematics/geometry/Rotation2d.h index 1eb7404..34d5d23 100644 --- a/src/main/include/fl/mathematics/geometry/Rotation2d.h +++ b/src/main/include/fl/mathematics/geometry/Rotation2d.h @@ -8,26 +8,23 @@ class Rotation2d final { public: // Constructors Rotation2d(); - Rotation2d(const double x, const double y, const bool normalize); + Rotation2d(double x, double y, bool normalize); explicit Rotation2d(const double value); - - static Rotation2d FromDegrees(const double degrees); + + static Rotation2d FromDegrees(double degrees); // Operator Overloads - inline Rotation2d operator-(const Rotation2d& other) const { return *this + -other; } - inline Rotation2d operator-() const { return Rotation2d(-value_); } + Rotation2d operator-(const Rotation2d& other) const; + Rotation2d operator-() const; - inline Rotation2d operator+(const Rotation2d& other) const { - return Rotation2d{Cos() * other.Cos() - Sin() * other.Sin(), Cos() * other.Sin() + Sin() * other.Cos(), - true}; - } + Rotation2d operator+(const Rotation2d& other) const; // Accessors - inline double Radians() const { return value_; } - inline double Degrees() const { return Rad2Deg(value_); } - inline double Cos() const { return cos_; } - inline double Sin() const { return sin_; } - inline double Tan() const { return sin_ / cos_; } + double Radians() const; + double Degrees() const; + double Cos() const; + double Sin() const; + double Tan() const; bool IsParallel(const Rotation2d& other) const; diff --git a/src/main/include/fl/mathematics/geometry/Translation2d.h b/src/main/include/fl/mathematics/geometry/Translation2d.h index e250f03..014cee3 100644 --- a/src/main/include/fl/mathematics/geometry/Translation2d.h +++ b/src/main/include/fl/mathematics/geometry/Translation2d.h @@ -11,45 +11,32 @@ class Translation2d final : public VaryInterpolatable { public: // Constructors Translation2d(); - Translation2d(const double x, const double y); - Translation2d(const double length, const Rotation2d& rotation); + Translation2d(double x, double y); + Translation2d(double length, const Rotation2d& rotation); // Overriden Methods - inline double Distance(const Translation2d& other) const override { - return std::hypot(other.X() - X(), other.Y() - Y()); - } + double Distance(const Translation2d& other) const override; Translation2d Interpolate(const Translation2d& end_value, const double t) const override; // Operator Overloads - inline Translation2d operator+(const Translation2d& other) const { - return Translation2d{X() + other.X(), Y() + other.Y()}; - } + Translation2d operator+(const Translation2d& other) const; - inline Translation2d operator-(const Translation2d& other) const { - return Translation2d{X() - other.X(), Y() - other.Y()}; - } + Translation2d operator-(const Translation2d& other) const; - inline Translation2d operator*(const double scalar) const { - return Translation2d{X() * scalar, Y() * scalar}; - } + Translation2d operator*(double scalar) const; - inline Translation2d operator*(const Rotation2d& rotation) const { - return Translation2d{x_ * rotation.Cos() - y_ * rotation.Sin(), - x_ * rotation.Sin() + y_ * rotation.Cos()}; - } + Translation2d operator*(const Rotation2d& rotation) const; - inline Translation2d operator/(const double scalar) const { - return Translation2d{X() / scalar, Y() / scalar}; - } + Translation2d operator/(double scalar) const; - inline Translation2d operator-() const { return Translation2d{-X(), -Y()}; } + Translation2d operator-() const; // Accessors - inline double X() const { return x_; } - inline double Y() const { return y_; } + double X() const; + double Y() const; - inline double Norm() const { return std::hypot(x_, y_); } + double Norm() const; private: double x_; diff --git a/src/main/include/fl/mathematics/geometry/Twist2d.h b/src/main/include/fl/mathematics/geometry/Twist2d.h index f336fdd..753b4fe 100644 --- a/src/main/include/fl/mathematics/geometry/Twist2d.h +++ b/src/main/include/fl/mathematics/geometry/Twist2d.h @@ -6,21 +6,18 @@ namespace fl { class Twist2d { public: // Constructors - Twist2d() : dx_(0.0), dy_(0.0), dtheta_(0.0) {} - Twist2d(const double dx, const double dy, const double dtheta) : dx_(dx), dy_(dy), dtheta_(dtheta) {} + Twist2d(); + Twist2d(const double dx, const double dy, const double dtheta); // Operator Overloads - Twist2d operator*(const double scalar) const { return {dx_ * scalar, dy_ * scalar, dtheta_ * scalar}; } + Twist2d operator*(const double scalar) const; // Accessors - double Dx() const { return dx_; } - double Dy() const { return dy_; } - double Dtheta() const { return dtheta_; } + double Dx() const; + double Dy() const; + double Dtheta() const; - double Norm() const { - if (dy_ == 0.0) return std::abs(dx_); - return std::hypot(dx_, dy_); - } + double Norm() const; private: double dx_; diff --git a/src/main/include/fl/mathematics/spline/ParametricQuinticHermiteSpline.h b/src/main/include/fl/mathematics/spline/ParametricQuinticHermiteSpline.h index 07f8c05..74fae20 100644 --- a/src/main/include/fl/mathematics/spline/ParametricQuinticHermiteSpline.h +++ b/src/main/include/fl/mathematics/spline/ParametricQuinticHermiteSpline.h @@ -5,65 +5,16 @@ namespace fl { class ParametricQuinticHermiteSpline final : public ParametricSpline { public: - ParametricQuinticHermiteSpline(const Pose2d& start, const Pose2d& end) { - const auto scale_factor = 1.2 * start.Translation().Distance(end.Translation()); + ParametricQuinticHermiteSpline(const Pose2d& start, const Pose2d& end); - x0_ = start.Translation().X(); - x1_ = end.Translation().X(); - dx0_ = scale_factor * start.Rotation().Cos(); - dx1_ = scale_factor * end.Rotation().Cos(); - ddx0_ = 0.; - ddx1_ = 0.; + const Pose2d& StartPose() const; + const Pose2d& EndPose() const; - y0_ = start.Translation().Y(); - y1_ = end.Translation().Y(); - dy0_ = scale_factor * start.Rotation().Sin(); - dy1_ = scale_factor * end.Rotation().Sin(); - ddy0_ = 0.; - ddy1_ = 0.; - - ax_ = -6 * x0_ - 3 * dx0_ - 0.5 * ddx0_ + 0.5 * ddx1_ - 3 * dx1_ + 6 * x1_; - bx_ = 15 * x0_ + 8 * dx0_ + 1.5 * ddx0_ - ddx1_ + 7 * dx1_ - 15 * x1_; - cx_ = -10 * x0_ - 6 * dx0_ - 1.5 * ddx0_ + 0.5 * ddx1_ - 4 * dx1_ + 10 * x1_; - dx_ = 0.5 * ddx0_; - ex_ = dx0_; - fx_ = x0_; - - ay_ = -6 * y0_ - 3 * dy0_ - 0.5 * ddy0_ + 0.5 * ddy1_ - 3 * dy1_ + 6 * y1_; - by_ = 15 * y0_ + 8 * dy0_ + 1.5 * ddy0_ - ddy1_ + 7 * dy1_ - 15 * y1_; - cy_ = -10 * y0_ - 6 * dy0_ - 1.5 * ddy0_ + 0.5 * ddy1_ - 4 * dy1_ + 10 * y1_; - dy_ = 0.5 * ddy0_; - ey_ = dy0_; - fy_ = y0_; - - start_ = start; - end_ = end; - } - - const Pose2d& StartPose() const { return start_; } - const Pose2d& EndPose() const { return end_; } - - Translation2d Point(const double t) const override { - return Translation2d{ax_ * std::pow(t, 5) + bx_ * std::pow(t, 4) + cx_ * std::pow(t, 3) + - dx_ * std::pow(t, 2) + ex_ * t + fx_, - ay_ * std::pow(t, 5) + by_ * std::pow(t, 4) + cy_ * std::pow(t, 3) + - dy_ * std::pow(t, 2) + ey_ * t + fy_}; - } - - Rotation2d Heading(const double t) const override { return {Dx(t), Dy(t), true}; } - - double Curvature(const double t) const override { - return (Dx(t) * Ddy(t) - Ddx(t) * Dy(t)) / ((Dx(t) * Dx(t) + Dy(t) * Dy(t)) * Velocity(t)); - } - - double DCurvature(const double t) const override { - const auto dx_2dy2 = Dx(t) * Dx(t) + Dy(t) * Dy(t); - const auto num = (Dx(t) * Dddy(t) - Dddx(t) * Dy(t)) * dx_2dy2 - - 3.0 * (Dx(t) * Ddy(t) - Ddx(t) * Dy(t)) * (Dx(t) * Ddx(t) + Dy(t) * Ddy(t)); - return num / (dx_2dy2 * dx_2dy2 * std::sqrt(dx_2dy2)); - } - - double Velocity(const double t) const override { return std::hypot(Dx(t), Dy(t)); } + Translation2d Point(const double t) const override; + Rotation2d Heading(const double t) const override; + double Curvature(const double t) const override; + double DCurvature(const double t) const override; + double Velocity(const double t) const override; private: double x0_, x1_, dx0_, dx1_, ddx0_, ddx1_; @@ -75,33 +26,11 @@ class ParametricQuinticHermiteSpline final : public ParametricSpline { Pose2d start_; Pose2d end_; - double Dx(const double t) const { - return 5.0 * ax_ * std::pow(t, 4) + 4.0 * bx_ * std::pow(t, 3) + 3.0 * cx_ * std::pow(t, 2) + - 2.0 * dx_ * t + ex_; - } - - double Dy(const double t) const { - return 5.0 * ay_ * std::pow(t, 4) + 4.0 * by_ * std::pow(t, 3) + 3.0 * cy_ * std::pow(t, 2) + - 2.0 * dy_ * t + ey_; - } - - double Ddx(const double t) const { - return 20.0 * ax_ * std::pow(t, 3) + 12.0 * bx_ * std::pow(t, 2) + 6.0 * cx_ * t + 2 * dx_; - } - - double Ddy(const double t) const { - return 20.0 * ay_ * std::pow(t, 3) + 12.0 * by_ * std::pow(t, 2) + 6.0 * cy_ * t + 2 * dy_; - } - - double Dddx(const double t) const { return 60.0 * ax_ * std::pow(t, 2) + 24.0 * bx_ * t + 6 * cx_; } - - double Dddy(const double t) const { return 60.0 * ay_ * std::pow(t, 2) + 24.0 * by_ * t + 6 * cy_; } - - template - static constexpr double BoundRadians(const T radians) { - while (radians >= kPi) radians -= 2 * kPi; - while (radians < kPi) radians += 2 * kPi; - return radians; - } + double Dx(const double t) const; + double Dy(const double t) const; + double Ddx(const double t) const; + double Ddy(const double t) const; + double Dddx(const double t) const; + double Dddy(const double t) const; }; } // namespace fl diff --git a/src/main/include/fl/mathematics/spline/ParametricSpline.h b/src/main/include/fl/mathematics/spline/ParametricSpline.h index 589d2d2..90ace00 100644 --- a/src/main/include/fl/mathematics/spline/ParametricSpline.h +++ b/src/main/include/fl/mathematics/spline/ParametricSpline.h @@ -4,17 +4,17 @@ namespace fl { class ParametricSpline { public: + virtual ~ParametricSpline() = default; + virtual Translation2d Point(double t) const = 0; virtual Rotation2d Heading(double t) const = 0; virtual double Curvature(double t) const = 0; virtual double DCurvature(double t) const = 0; virtual double Velocity(double t) const = 0; - Pose2dWithCurvature PoseWithCurvature(const double t) const { - return Pose2dWithCurvature{Pose(t), Curvature(t), DCurvature(t) / Velocity(t)}; - } + Pose2dWithCurvature PoseWithCurvature(const double t) const; private: - Pose2d Pose(const double t) const { return Pose2d{Point(t), Heading(t)}; } + Pose2d Pose(const double t) const; }; } // namespace fl diff --git a/src/main/include/fl/mathematics/spline/SplineGenerator.h b/src/main/include/fl/mathematics/spline/SplineGenerator.h index c046b3b..263b6fb 100644 --- a/src/main/include/fl/mathematics/spline/SplineGenerator.h +++ b/src/main/include/fl/mathematics/spline/SplineGenerator.h @@ -14,52 +14,14 @@ class SplineGenerator { static std::vector ParameterizeSpline(const std::shared_ptr& spline, const double max_dx, const double max_dy, const double max_dtheta, const double t0 = 0.0, - const double t1 = 1.0) { - const auto dt = t1 - t0; - auto rv = std::vector(static_cast(kMinSampleSize / dt)); - - rv.push_back(spline->PoseWithCurvature(0)); - - for (double t = 0; t < t1; t += dt / kMinSampleSize) { - GetSegmentArc(spline, &rv, t, t + dt / kMinSampleSize, max_dx, max_dy, max_dtheta); - } - - return rv; - } + const double t1 = 1.0); static std::vector ParameterizeSplines( std::vector> splines, const double max_dx, const double max_dy, - const double max_dtheta) { - auto rv = std::vector(); - if (splines.empty()) return rv; - - rv.push_back(splines[0]->PoseWithCurvature(0.0)); - for (const auto& spline : splines) { - auto samples = ParameterizeSpline(spline, max_dx, max_dy, max_dtheta); - samples.erase(samples.begin()); - rv.insert(rv.end(), samples.begin(), samples.end()); - } - - return rv; - } + const double max_dtheta); static void GetSegmentArc(const std::shared_ptr& spline, std::vector* rv, const double t0, const double t1, - const double max_dx, const double max_dy, const double max_dtheta) { - const auto p0 = spline->Point(t0); - const auto p1 = spline->Point(t1); - const auto r0 = spline->Heading(t0); - const auto r1 = spline->Heading(t1); - - const auto transformation = Pose2d{(p1 - p0) * -r0, r1 + -r0}; - const auto twist = Pose2d::ToTwist(transformation); - - if (twist.Dy() > max_dy || twist.Dx() > max_dx || twist.Dtheta() > max_dtheta) { - GetSegmentArc(spline, rv, t0, (t0 + t1) / 2, max_dx, max_dy, max_dtheta); - GetSegmentArc(spline, rv, (t0 + t1) / 2, t1, max_dx, max_dy, max_dtheta); - } else { - rv->push_back(spline->PoseWithCurvature(t1)); - } - } + const double max_dx, const double max_dy, const double max_dtheta); }; } // namespace fl diff --git a/src/main/include/fl/mathematics/trajectory/Trajectory.h b/src/main/include/fl/mathematics/trajectory/Trajectory.h index ce7b869..f116436 100644 --- a/src/main/include/fl/mathematics/trajectory/Trajectory.h +++ b/src/main/include/fl/mathematics/trajectory/Trajectory.h @@ -1,8 +1,8 @@ #pragma once +#include #include #include -#include namespace fl { @@ -18,7 +18,6 @@ struct TrajectorySamplePoint { int index_floor; int index_ceil; - public: explicit TrajectorySamplePoint(TrajectoryPoint point) : state(point.state), index_floor(point.index), index_ceil(point.index) {} diff --git a/src/main/include/fl/mathematics/trajectory/TrajectoryGenerator.h b/src/main/include/fl/mathematics/trajectory/TrajectoryGenerator.h index d1e2cd9..0a25396 100644 --- a/src/main/include/fl/mathematics/trajectory/TrajectoryGenerator.h +++ b/src/main/include/fl/mathematics/trajectory/TrajectoryGenerator.h @@ -17,45 +17,13 @@ class TrajectoryGenerator { public: static TimedTrajectory GenerateTrajectory( - std::vector waypoints, const Constraints& constraints, const double start_velocity, - const double end_velocity, const double max_velocity, const double max_acceleration, - const bool reversed) { - const auto flipped_position = Pose2d{Translation2d{}, Rotation2d::FromDegrees(180.0)}; - - if (reversed) { - for (auto& waypoint : waypoints) { - waypoint = waypoint.TransformBy(flipped_position); - } - } - - const auto indexed_trajectory = TrajectoryFromSplineWaypoints(waypoints, 2.0, 0.05, 0.1); - - auto points = indexed_trajectory.Points(); - - if (reversed) { - for (auto& point : points) { - point = - Pose2dWithCurvature{point.Pose().TransformBy(flipped_position), -point.Curvature(), point.Dkds()}; - } - } - - return TimeParameterizeTrajectory(DistanceTrajectory(points), constraints, - start_velocity, end_velocity, max_velocity, max_acceleration, 0.051, - reversed); - } + std::vector waypoints, const Constraints& constraints, const double start_velocity, + const double end_velocity, const double max_velocity, const double max_acceleration, + const bool reversed); static IndexedTrajectory TrajectoryFromSplineWaypoints( - const std::vector& waypoints, const double max_dx, const double max_dy, - const double max_dtheta) { - std::vector> splines(waypoints.size() - 1); - for (auto i = 1; i < waypoints.size(); ++i) { - splines[i - 1] = std::make_shared(waypoints[i - 1], waypoints[i]); - } - auto trajectory = IndexedTrajectory( - SplineGenerator::ParameterizeSplines(splines, max_dx, max_dy, max_dtheta)); - - return trajectory; - } + const std::vector& waypoints, const double max_dx, const double max_dy, + const double max_dtheta); template struct ConstrainedPose { diff --git a/src/main/include/fl/mathematics/trajectory/constraints/AngularAccelerationConstraint.h b/src/main/include/fl/mathematics/trajectory/constraints/AngularAccelerationConstraint.h index 030c8ab..719b5ac 100644 --- a/src/main/include/fl/mathematics/trajectory/constraints/AngularAccelerationConstraint.h +++ b/src/main/include/fl/mathematics/trajectory/constraints/AngularAccelerationConstraint.h @@ -7,56 +7,10 @@ namespace fl { class AngularAccelerationConstraint final : public TimingConstraint { public: - explicit AngularAccelerationConstraint(double max_angular_acceleration) - : max_angular_acceleration_(max_angular_acceleration) {} + explicit AngularAccelerationConstraint(double max_angular_acceleration); - ~AngularAccelerationConstraint() = default; - - double MaxVelocity(const Pose2dWithCurvature& state) const override { - /** - * We don't want v^2 * dk/ds alone to go over the max angular acceleration. - * v^2 * dk/ds = maxAngularAcceleration when linear acceleration = 0. - * v = sqrt(maxAngularAcceleration / dk/ds) - */ - - return std::sqrt(max_angular_acceleration_ / std::abs(state.Dkds())); - } - - fl::MinMaxAcceleration MinMaxAcceleration(const Pose2dWithCurvature& state, - double velocity) const override { - /** - * We want to limit the acceleration such that we never go above the - * specified angular acceleration. - * - * Angular acceleration = dw/dt WHERE w = omega = angular velocity - * w = v * k WHERE v = linear velocity, k = - * curvature - * - * dw/dt = d/dt (v * k) - * - * By chain rule, - * dw/dt = dv/dt * k + v * dk/dt [1] - * - * We don't have dk/dt, but we do have dk/ds and ds/dt - * dk/ds * ds/dt = dk/dt [2] - * - * Substituting [2] in [1], we get - * dw/dt = acceleration * curvature + velocity * velocity * d_curvature - * WHERE acceleration = dv/dt, velocity = ds/dt, d_curvature = dk/dt and - * curvature = k - * - * We now want to find the linear acceleration such that the angular - * acceleration (dw/dt) never goes above the specified amount. - * - * acceleration * curvature = dw/dt - (velocity * velocity * d_curvature) - * acceleration = (dw/dt - (velocity * velocity * d_curvature)) / curvature - */ - - const auto max_absolute_acceleration = - std::abs((max_angular_acceleration_ - (velocity * velocity * state.Dkds())) / state.Curvature()); - - return fl::MinMaxAcceleration{-max_absolute_acceleration, max_absolute_acceleration}; - } + double MaxVelocity(const Pose2dWithCurvature& state) const override; + fl::MinMaxAcceleration MinMaxAcceleration(const Pose2dWithCurvature& state, double velocity) const override; private: double max_angular_acceleration_; diff --git a/src/main/include/fl/mathematics/trajectory/constraints/CentripetalAccelerationConstraint.h b/src/main/include/fl/mathematics/trajectory/constraints/CentripetalAccelerationConstraint.h index 789b81e..15421ce 100644 --- a/src/main/include/fl/mathematics/trajectory/constraints/CentripetalAccelerationConstraint.h +++ b/src/main/include/fl/mathematics/trajectory/constraints/CentripetalAccelerationConstraint.h @@ -7,19 +7,10 @@ namespace fl { class CentripetalAccelerationConstraint final : public TimingConstraint { public: - explicit CentripetalAccelerationConstraint(const double max_centripetal_acceleration) - : max_centripetal_acceleration_(max_centripetal_acceleration) {} + explicit CentripetalAccelerationConstraint(const double max_centripetal_acceleration); - ~CentripetalAccelerationConstraint() = default; - - double MaxVelocity(const Pose2dWithCurvature& state) const override { - return std::sqrt(std::abs(max_centripetal_acceleration_ / state.Curvature())); - } - - fl::MinMaxAcceleration MinMaxAcceleration(const Pose2dWithCurvature& state, - double velocity) const override { - return kNoLimits; - } + double MaxVelocity(const Pose2dWithCurvature& state) const override; + fl::MinMaxAcceleration MinMaxAcceleration(const Pose2dWithCurvature& state, double velocity) const override; private: double max_centripetal_acceleration_; diff --git a/src/main/include/fl/mathematics/trajectory/constraints/VelocityLimitRadiusConstraint.h b/src/main/include/fl/mathematics/trajectory/constraints/VelocityLimitRadiusConstraint.h index 8f5219e..2c30491 100644 --- a/src/main/include/fl/mathematics/trajectory/constraints/VelocityLimitRadiusConstraint.h +++ b/src/main/include/fl/mathematics/trajectory/constraints/VelocityLimitRadiusConstraint.h @@ -7,22 +7,10 @@ namespace fl { class VelocityLimitRadiusConstraint : public TimingConstraint { public: - VelocityLimitRadiusConstraint(const Translation2d& point, const double radius, const double max_velocity) - : point_(point), radius_(radius), max_velocity_(max_velocity) {} + VelocityLimitRadiusConstraint(const Translation2d& point, const double radius, const double max_velocity); - ~VelocityLimitRadiusConstraint() = default; - - double MaxVelocity(const Pose2dWithCurvature& state) const override { - if (state.Pose().Translation().Distance(point_) < radius_) { - return max_velocity_; - } - return std::numeric_limits::max(); - } - - fl::MinMaxAcceleration MinMaxAcceleration(const Pose2dWithCurvature& state, - double velocity) const override { - return kNoLimits; - } + double MaxVelocity(const Pose2dWithCurvature& state) const override; + fl::MinMaxAcceleration MinMaxAcceleration(const Pose2dWithCurvature& state, double velocity) const override; private: Translation2d point_;