split all impls from declarations

This commit is contained in:
Prateek Machiraju 2019-06-29 14:29:44 -04:00
parent bf0e158d24
commit ff9e83f445
27 changed files with 615 additions and 416 deletions

View File

@ -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<Pose2dWithCurvature>& 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<double>(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<Pose2dWithCurvature>& 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

View File

@ -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<Pose2dWithCurvature>& iterator, const Pose2d& robot_pose) const {
const TimedEntry<Pose2dWithCurvature> reference_state = iterator.CurrentState().state;
const Pose2d error = reference_state.State().Pose().InFrameOfReferenceOf(robot_pose);
const auto vd = units::unit_cast<double>(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

View File

@ -0,0 +1,51 @@
#include "fl/mathematics/control/TrajectoryTracker.h"
namespace fl {
void TrajectoryTracker::Reset(const TimedTrajectory<Pose2dWithCurvature>& trajectory) {
iterator_ = static_cast<TimedIterator<Pose2dWithCurvature>*>(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<double>(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<double>(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<TimedEntry<Pose2dWithCurvature>> TrajectoryTracker::ReferencePoint() const {
return iterator_->CurrentState();
}
bool TrajectoryTracker::IsFinished() const { return iterator_->IsDone(); }
}

View File

@ -25,6 +25,27 @@ Rotation2d Rotation2d::FromDegrees(const double degrees) {
return Rotation2d(Deg2Rad(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 { bool Rotation2d::IsParallel(const Rotation2d& other) const {
return EpsilonEquals((*this - other).Radians(), 0.0); return EpsilonEquals((*this - other).Radians(), 0.0);
} }

View File

@ -7,6 +7,10 @@ Translation2d::Translation2d(const double x, const double y) : x_(x), y_(y) {}
Translation2d::Translation2d(const double length, const Rotation2d& rotation) Translation2d::Translation2d(const double length, const Rotation2d& rotation)
: x_(length * rotation.Cos()), y_(length * rotation.Sin()) {} : 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 { Translation2d Translation2d::Interpolate(const Translation2d& end_value, const double t) const {
if (t <= 0) { if (t <= 0) {
return *this; 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)}; 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 } // namespace fl

View File

@ -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_);
}
}

View File

@ -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

View File

@ -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

View File

@ -0,0 +1,54 @@
#include "fl/mathematics/spline/SplineGenerator.h"
namespace fl {
std::vector<Pose2dWithCurvature> SplineGenerator::ParameterizeSpline(
const std::shared_ptr<ParametricSpline>& 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<Pose2dWithCurvature>(static_cast<int>(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<Pose2dWithCurvature> SplineGenerator::ParameterizeSplines(
std::vector<std::shared_ptr<ParametricSpline>> splines, const double max_dx, const double max_dy,
const double max_dtheta) {
auto rv = std::vector<Pose2dWithCurvature>();
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<ParametricSpline>& spline,
std::vector<Pose2dWithCurvature>* 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

View File

@ -0,0 +1,48 @@
#include "fl/mathematics/trajectory/TrajectoryGenerator.h"
namespace fl {
TimedTrajectory<Pose2dWithCurvature> TrajectoryGenerator::GenerateTrajectory(std::vector<Pose2d> 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<Pose2dWithCurvature>(points), constraints,
start_velocity, end_velocity, max_velocity, max_acceleration, 0.051,
reversed);
}
IndexedTrajectory<Pose2dWithCurvature> TrajectoryGenerator::TrajectoryFromSplineWaypoints(
const std::vector<Pose2d>& waypoints, const double max_dx, const double max_dy, const double max_dtheta) {
std::vector<std::shared_ptr<ParametricSpline>> splines(waypoints.size() - 1);
for (auto i = 1; i < waypoints.size(); ++i) {
splines[i - 1] = std::make_shared<ParametricQuinticHermiteSpline>(waypoints[i - 1], waypoints[i]);
}
auto trajectory = IndexedTrajectory<Pose2dWithCurvature>(
SplineGenerator::ParameterizeSplines(splines, max_dx, max_dy, max_dtheta));
return trajectory;
}
}

View File

@ -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

View File

@ -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

View File

@ -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<double>::max();
}
fl::MinMaxAcceleration VelocityLimitRadiusConstraint::MinMaxAcceleration(const Pose2dWithCurvature& state,
double velocity) const {
return kNoLimits;
}
} // namespace fl

View File

@ -7,37 +7,12 @@
namespace fl { namespace fl {
class PurePursuitTracker : public TrajectoryTracker { class PurePursuitTracker : public TrajectoryTracker {
public: public:
PurePursuitTracker(const double lat, const units::second_t lookahead_time, const double min_lookahead_distance) PurePursuitTracker(const double lat, const units::second_t lookahead_time,
: lat_(lat), lookahead_time_(lookahead_time), min_lookahead_distance_(min_lookahead_distance) {} const double min_lookahead_distance);
TrajectoryTrackerVelocityOutput CalculateState(const TimedIterator<Pose2dWithCurvature>& iterator, TrajectoryTrackerVelocityOutput CalculateState(const TimedIterator<Pose2dWithCurvature>& iterator,
const Pose2d& robot_pose) const override { const Pose2d& robot_pose) const
const auto reference_point = iterator.CurrentState(); override;
// 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<double>(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};
}
private: private:
double lat_; double lat_;
@ -45,39 +20,6 @@ class PurePursuitTracker : public TrajectoryTracker {
double min_lookahead_distance_; double min_lookahead_distance_;
Pose2d CalculateLookaheadPose(const TimedIterator<Pose2dWithCurvature>& iterator, Pose2d CalculateLookaheadPose(const TimedIterator<Pose2dWithCurvature>& iterator,
const Pose2d& robot_pose) const { 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 } // namespace fl

View File

@ -5,24 +5,10 @@
namespace fl { namespace fl {
class RamseteTracker : public TrajectoryTracker { class RamseteTracker : public TrajectoryTracker {
public: public:
RamseteTracker(const double beta, const double zeta) : beta_(beta), zeta_(zeta) {} RamseteTracker(const double beta, const double zeta);
TrajectoryTrackerVelocityOutput CalculateState(const TimedIterator<Pose2dWithCurvature>& iterator, TrajectoryTrackerVelocityOutput CalculateState(const TimedIterator<Pose2dWithCurvature>& iterator,
const Pose2d& robot_pose) const override { const Pose2d& robot_pose) const override;
const TimedEntry<Pose2dWithCurvature> reference_state = iterator.CurrentState().state;
const Pose2d error = reference_state.State().Pose().InFrameOfReferenceOf(robot_pose);
const auto vd = units::unit_cast<double>(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};
}
private: private:
static double Sinc(const double theta) { static double Sinc(const double theta) {

View File

@ -4,6 +4,7 @@
#include <units.h> #include <units.h>
#include <memory> #include <memory>
#include "fl/mathematics/geometry/Pose2dWithCurvature.h"
namespace fl { namespace fl {
@ -21,53 +22,14 @@ struct TrajectoryTrackerOutput {
class TrajectoryTracker { class TrajectoryTracker {
public: public:
void Reset(const TimedTrajectory<Pose2dWithCurvature>& trajectory) { void Reset(const TimedTrajectory<Pose2dWithCurvature>& trajectory);
iterator_ = static_cast<TimedIterator<Pose2dWithCurvature>*>(trajectory.Iterator().get()); TrajectoryTrackerOutput NextState(const Pose2d& current_pose, const units::second_t current_time);
previous_velocity_ = nullptr; TrajectorySamplePoint<TimedEntry<Pose2dWithCurvature>> ReferencePoint() const;
previous_time_ = units::second_t(-1); bool IsFinished() const;
}
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<double>(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<double>(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;
}
virtual TrajectoryTrackerVelocityOutput CalculateState(const TimedIterator<Pose2dWithCurvature>& iterator, virtual TrajectoryTrackerVelocityOutput CalculateState(const TimedIterator<Pose2dWithCurvature>& iterator,
const Pose2d& robot_pose) const = 0; const Pose2d& robot_pose) const = 0;
TrajectorySamplePoint<TimedEntry<Pose2dWithCurvature>> ReferencePoint() const {
return iterator_->CurrentState();
}
bool IsFinished() const { return iterator_->IsDone(); }
private: private:
TimedIterator<Pose2dWithCurvature>* iterator_ = nullptr; TimedIterator<Pose2dWithCurvature>* iterator_ = nullptr;
std::unique_ptr<TrajectoryTrackerVelocityOutput> previous_velocity_; std::unique_ptr<TrajectoryTrackerVelocityOutput> previous_velocity_;

View File

@ -8,26 +8,23 @@ class Rotation2d final {
public: public:
// Constructors // Constructors
Rotation2d(); Rotation2d();
Rotation2d(const double x, const double y, const bool normalize); Rotation2d(double x, double y, bool normalize);
explicit Rotation2d(const double value); explicit Rotation2d(const double value);
static Rotation2d FromDegrees(const double degrees); static Rotation2d FromDegrees(double degrees);
// Operator Overloads // Operator Overloads
inline Rotation2d operator-(const Rotation2d& other) const { return *this + -other; } Rotation2d operator-(const Rotation2d& other) const;
inline Rotation2d operator-() const { return Rotation2d(-value_); } Rotation2d operator-() const;
inline Rotation2d operator+(const Rotation2d& other) const { Rotation2d operator+(const Rotation2d& other) const;
return Rotation2d{Cos() * other.Cos() - Sin() * other.Sin(), Cos() * other.Sin() + Sin() * other.Cos(),
true};
}
// Accessors // Accessors
inline double Radians() const { return value_; } double Radians() const;
inline double Degrees() const { return Rad2Deg(value_); } double Degrees() const;
inline double Cos() const { return cos_; } double Cos() const;
inline double Sin() const { return sin_; } double Sin() const;
inline double Tan() const { return sin_ / cos_; } double Tan() const;
bool IsParallel(const Rotation2d& other) const; bool IsParallel(const Rotation2d& other) const;

View File

@ -11,45 +11,32 @@ class Translation2d final : public VaryInterpolatable<Translation2d> {
public: public:
// Constructors // Constructors
Translation2d(); Translation2d();
Translation2d(const double x, const double y); Translation2d(double x, double y);
Translation2d(const double length, const Rotation2d& rotation); Translation2d(double length, const Rotation2d& rotation);
// Overriden Methods // Overriden Methods
inline double Distance(const Translation2d& other) const override { double Distance(const Translation2d& other) const override;
return std::hypot(other.X() - X(), other.Y() - Y());
}
Translation2d Interpolate(const Translation2d& end_value, const double t) const override; Translation2d Interpolate(const Translation2d& end_value, const double t) const override;
// Operator Overloads // Operator Overloads
inline Translation2d operator+(const Translation2d& other) const { Translation2d operator+(const Translation2d& other) const;
return Translation2d{X() + other.X(), Y() + other.Y()};
}
inline Translation2d operator-(const Translation2d& other) const { Translation2d operator-(const Translation2d& other) const;
return Translation2d{X() - other.X(), Y() - other.Y()};
}
inline Translation2d operator*(const double scalar) const { Translation2d operator*(double scalar) const;
return Translation2d{X() * scalar, Y() * scalar};
}
inline Translation2d operator*(const Rotation2d& rotation) const { Translation2d operator*(const Rotation2d& rotation) const;
return Translation2d{x_ * rotation.Cos() - y_ * rotation.Sin(),
x_ * rotation.Sin() + y_ * rotation.Cos()};
}
inline Translation2d operator/(const double scalar) const { Translation2d operator/(double scalar) const;
return Translation2d{X() / scalar, Y() / scalar};
}
inline Translation2d operator-() const { return Translation2d{-X(), -Y()}; } Translation2d operator-() const;
// Accessors // Accessors
inline double X() const { return x_; } double X() const;
inline double Y() const { return y_; } double Y() const;
inline double Norm() const { return std::hypot(x_, y_); } double Norm() const;
private: private:
double x_; double x_;

View File

@ -6,21 +6,18 @@ namespace fl {
class Twist2d { class Twist2d {
public: public:
// Constructors // Constructors
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(const double dx, const double dy, const double dtheta);
// Operator Overloads // Operator Overloads
Twist2d operator*(const double scalar) const { return {dx_ * scalar, dy_ * scalar, dtheta_ * scalar}; } Twist2d operator*(const double scalar) const;
// Accessors // Accessors
double Dx() const { return dx_; } double Dx() const;
double Dy() const { return dy_; } double Dy() const;
double Dtheta() const { return dtheta_; } double Dtheta() const;
double Norm() const { double Norm() const;
if (dy_ == 0.0) return std::abs(dx_);
return std::hypot(dx_, dy_);
}
private: private:
double dx_; double dx_;

View File

@ -5,65 +5,16 @@
namespace fl { namespace fl {
class ParametricQuinticHermiteSpline final : public ParametricSpline { class ParametricQuinticHermiteSpline final : public ParametricSpline {
public: public:
ParametricQuinticHermiteSpline(const Pose2d& start, const Pose2d& end) { ParametricQuinticHermiteSpline(const Pose2d& start, const Pose2d& end);
const auto scale_factor = 1.2 * start.Translation().Distance(end.Translation());
x0_ = start.Translation().X(); const Pose2d& StartPose() const;
x1_ = end.Translation().X(); const Pose2d& EndPose() const;
dx0_ = scale_factor * start.Rotation().Cos();
dx1_ = scale_factor * end.Rotation().Cos();
ddx0_ = 0.;
ddx1_ = 0.;
y0_ = start.Translation().Y(); Translation2d Point(const double t) const override;
y1_ = end.Translation().Y(); Rotation2d Heading(const double t) const override;
dy0_ = scale_factor * start.Rotation().Sin(); double Curvature(const double t) const override;
dy1_ = scale_factor * end.Rotation().Sin(); double DCurvature(const double t) const override;
ddy0_ = 0.; double Velocity(const double t) const override;
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)); }
private: private:
double x0_, x1_, dx0_, dx1_, ddx0_, ddx1_; double x0_, x1_, dx0_, dx1_, ddx0_, ddx1_;
@ -75,33 +26,11 @@ class ParametricQuinticHermiteSpline final : public ParametricSpline {
Pose2d start_; Pose2d start_;
Pose2d end_; Pose2d end_;
double Dx(const double t) const { 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) + double Dy(const double t) const;
2.0 * dx_ * t + ex_; double Ddx(const double t) const;
} double Ddy(const double t) const;
double Dddx(const double t) const;
double Dy(const double t) const { double Dddy(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 <typename T>
static constexpr double BoundRadians(const T radians) {
while (radians >= kPi) radians -= 2 * kPi;
while (radians < kPi) radians += 2 * kPi;
return radians;
}
}; };
} // namespace fl } // namespace fl

View File

@ -4,17 +4,17 @@
namespace fl { namespace fl {
class ParametricSpline { class ParametricSpline {
public: public:
virtual ~ParametricSpline() = default;
virtual Translation2d Point(double t) const = 0; virtual Translation2d Point(double t) const = 0;
virtual Rotation2d Heading(double t) const = 0; virtual Rotation2d Heading(double t) const = 0;
virtual double Curvature(double t) const = 0; virtual double Curvature(double t) const = 0;
virtual double DCurvature(double t) const = 0; virtual double DCurvature(double t) const = 0;
virtual double Velocity(double t) const = 0; virtual double Velocity(double t) const = 0;
Pose2dWithCurvature PoseWithCurvature(const double t) const { Pose2dWithCurvature PoseWithCurvature(const double t) const;
return Pose2dWithCurvature{Pose(t), Curvature(t), DCurvature(t) / Velocity(t)};
}
private: private:
Pose2d Pose(const double t) const { return Pose2d{Point(t), Heading(t)}; } Pose2d Pose(const double t) const;
}; };
} // namespace fl } // namespace fl

View File

@ -14,52 +14,14 @@ class SplineGenerator {
static std::vector<Pose2dWithCurvature> ParameterizeSpline(const std::shared_ptr<ParametricSpline>& spline, static std::vector<Pose2dWithCurvature> ParameterizeSpline(const std::shared_ptr<ParametricSpline>& spline,
const double max_dx, const double max_dy, const double max_dx, const double max_dy,
const double max_dtheta, const double t0 = 0.0, const double max_dtheta, const double t0 = 0.0,
const double t1 = 1.0) { const double t1 = 1.0);
const auto dt = t1 - t0;
auto rv = std::vector<Pose2dWithCurvature>(static_cast<int>(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;
}
static std::vector<Pose2dWithCurvature> ParameterizeSplines( static std::vector<Pose2dWithCurvature> ParameterizeSplines(
std::vector<std::shared_ptr<ParametricSpline>> splines, const double max_dx, const double max_dy, std::vector<std::shared_ptr<ParametricSpline>> splines, const double max_dx, const double max_dy,
const double max_dtheta) { const double max_dtheta);
auto rv = std::vector<Pose2dWithCurvature>();
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;
}
static void GetSegmentArc(const std::shared_ptr<ParametricSpline>& spline, static void GetSegmentArc(const std::shared_ptr<ParametricSpline>& spline,
std::vector<Pose2dWithCurvature>* rv, const double t0, const double t1, std::vector<Pose2dWithCurvature>* rv, const double t0, const double t1,
const double max_dx, const double max_dy, const double max_dtheta) { 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 } // namespace fl

View File

@ -1,8 +1,8 @@
#pragma once #pragma once
#include <memory>
#include <utility> #include <utility>
#include <vector> #include <vector>
#include <memory>
namespace fl { namespace fl {
@ -18,7 +18,6 @@ struct TrajectorySamplePoint {
int index_floor; int index_floor;
int index_ceil; int index_ceil;
public:
explicit TrajectorySamplePoint(TrajectoryPoint<S> point) explicit TrajectorySamplePoint(TrajectoryPoint<S> point)
: state(point.state), index_floor(point.index), index_ceil(point.index) {} : state(point.state), index_floor(point.index), index_ceil(point.index) {}

View File

@ -17,45 +17,13 @@ class TrajectoryGenerator {
public: public:
static TimedTrajectory<Pose2dWithCurvature> GenerateTrajectory( static TimedTrajectory<Pose2dWithCurvature> GenerateTrajectory(
std::vector<Pose2d> waypoints, const Constraints& constraints, const double start_velocity, std::vector<Pose2d> waypoints, const Constraints& constraints, const double start_velocity,
const double end_velocity, const double max_velocity, const double max_acceleration, const double end_velocity, const double max_velocity, const double max_acceleration,
const bool reversed) { 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<Pose2dWithCurvature>(points), constraints,
start_velocity, end_velocity, max_velocity, max_acceleration, 0.051,
reversed);
}
static IndexedTrajectory<Pose2dWithCurvature> TrajectoryFromSplineWaypoints( static IndexedTrajectory<Pose2dWithCurvature> TrajectoryFromSplineWaypoints(
const std::vector<Pose2d>& waypoints, const double max_dx, const double max_dy, const std::vector<Pose2d>& waypoints, const double max_dx, const double max_dy,
const double max_dtheta) { const double max_dtheta);
std::vector<std::shared_ptr<ParametricSpline>> splines(waypoints.size() - 1);
for (auto i = 1; i < waypoints.size(); ++i) {
splines[i - 1] = std::make_shared<ParametricQuinticHermiteSpline>(waypoints[i - 1], waypoints[i]);
}
auto trajectory = IndexedTrajectory<Pose2dWithCurvature>(
SplineGenerator::ParameterizeSplines(splines, max_dx, max_dy, max_dtheta));
return trajectory;
}
template <typename S> template <typename S>
struct ConstrainedPose { struct ConstrainedPose {

View File

@ -7,56 +7,10 @@ namespace fl {
class AngularAccelerationConstraint final : public TimingConstraint<Pose2dWithCurvature> { class AngularAccelerationConstraint final : public TimingConstraint<Pose2dWithCurvature> {
public: public:
explicit AngularAccelerationConstraint(double max_angular_acceleration) explicit AngularAccelerationConstraint(double max_angular_acceleration);
: max_angular_acceleration_(max_angular_acceleration) {}
~AngularAccelerationConstraint() = default; double MaxVelocity(const Pose2dWithCurvature& state) const override;
fl::MinMaxAcceleration MinMaxAcceleration(const Pose2dWithCurvature& state, double velocity) const override;
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};
}
private: private:
double max_angular_acceleration_; double max_angular_acceleration_;

View File

@ -7,19 +7,10 @@ namespace fl {
class CentripetalAccelerationConstraint final : public TimingConstraint<Pose2dWithCurvature> { class CentripetalAccelerationConstraint final : public TimingConstraint<Pose2dWithCurvature> {
public: public:
explicit CentripetalAccelerationConstraint(const double max_centripetal_acceleration) explicit CentripetalAccelerationConstraint(const double max_centripetal_acceleration);
: max_centripetal_acceleration_(max_centripetal_acceleration) {}
~CentripetalAccelerationConstraint() = default; double MaxVelocity(const Pose2dWithCurvature& state) const override;
fl::MinMaxAcceleration MinMaxAcceleration(const Pose2dWithCurvature& state, double velocity) const override;
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;
}
private: private:
double max_centripetal_acceleration_; double max_centripetal_acceleration_;

View File

@ -7,22 +7,10 @@ namespace fl {
class VelocityLimitRadiusConstraint : public TimingConstraint<Pose2dWithCurvature> { class VelocityLimitRadiusConstraint : public TimingConstraint<Pose2dWithCurvature> {
public: public:
VelocityLimitRadiusConstraint(const Translation2d& point, const double radius, const double max_velocity) VelocityLimitRadiusConstraint(const Translation2d& point, const double radius, const double max_velocity);
: point_(point), radius_(radius), max_velocity_(max_velocity) {}
~VelocityLimitRadiusConstraint() = default; double MaxVelocity(const Pose2dWithCurvature& state) const override;
fl::MinMaxAcceleration MinMaxAcceleration(const Pose2dWithCurvature& state, double velocity) const override;
double MaxVelocity(const Pose2dWithCurvature& state) const override {
if (state.Pose().Translation().Distance(point_) < radius_) {
return max_velocity_;
}
return std::numeric_limits<double>::max();
}
fl::MinMaxAcceleration MinMaxAcceleration(const Pose2dWithCurvature& state,
double velocity) const override {
return kNoLimits;
}
private: private:
Translation2d point_; Translation2d point_;