split all impls from declarations
This commit is contained in:
parent
bf0e158d24
commit
ff9e83f445
73
src/main/cpp/fl/mathematics/control/PurePursuitTracker.cpp
Normal file
73
src/main/cpp/fl/mathematics/control/PurePursuitTracker.cpp
Normal 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
|
23
src/main/cpp/fl/mathematics/control/RamseteTracker.cpp
Normal file
23
src/main/cpp/fl/mathematics/control/RamseteTracker.cpp
Normal 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
|
51
src/main/cpp/fl/mathematics/control/TrajectoryTracker.cpp
Normal file
51
src/main/cpp/fl/mathematics/control/TrajectoryTracker.cpp
Normal 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(); }
|
||||
}
|
|
@ -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);
|
||||
}
|
||||
|
|
|
@ -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
|
||||
|
|
31
src/main/cpp/fl/mathematics/geometry/Twist2d.cpp
Normal file
31
src/main/cpp/fl/mathematics/geometry/Twist2d.cpp
Normal 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_);
|
||||
}
|
||||
}
|
|
@ -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
|
13
src/main/cpp/fl/mathematics/spline/ParametricSpline.cpp
Normal file
13
src/main/cpp/fl/mathematics/spline/ParametricSpline.cpp
Normal 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
|
54
src/main/cpp/fl/mathematics/spline/SplineGenerator.cpp
Normal file
54
src/main/cpp/fl/mathematics/spline/SplineGenerator.cpp
Normal 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
|
|
@ -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;
|
||||
}
|
||||
}
|
|
@ -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
|
|
@ -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
|
|
@ -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
|
|
@ -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<Pose2dWithCurvature>& 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<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};
|
||||
}
|
||||
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<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{}});
|
||||
}
|
||||
const Pose2d& robot_pose) const;
|
||||
};
|
||||
} // namespace fl
|
|
@ -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<Pose2dWithCurvature>& iterator,
|
||||
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};
|
||||
}
|
||||
const Pose2d& robot_pose) const override;
|
||||
|
||||
private:
|
||||
static double Sinc(const double theta) {
|
||||
|
|
|
@ -4,6 +4,7 @@
|
|||
|
||||
#include <units.h>
|
||||
#include <memory>
|
||||
#include "fl/mathematics/geometry/Pose2dWithCurvature.h"
|
||||
|
||||
namespace fl {
|
||||
|
||||
|
@ -21,53 +22,14 @@ struct TrajectoryTrackerOutput {
|
|||
|
||||
class TrajectoryTracker {
|
||||
public:
|
||||
void Reset(const TimedTrajectory<Pose2dWithCurvature>& trajectory) {
|
||||
iterator_ = static_cast<TimedIterator<Pose2dWithCurvature>*>(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<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;
|
||||
}
|
||||
void Reset(const TimedTrajectory<Pose2dWithCurvature>& trajectory);
|
||||
TrajectoryTrackerOutput NextState(const Pose2d& current_pose, const units::second_t current_time);
|
||||
TrajectorySamplePoint<TimedEntry<Pose2dWithCurvature>> ReferencePoint() const;
|
||||
bool IsFinished() const;
|
||||
|
||||
virtual TrajectoryTrackerVelocityOutput CalculateState(const TimedIterator<Pose2dWithCurvature>& iterator,
|
||||
const Pose2d& robot_pose) const = 0;
|
||||
|
||||
TrajectorySamplePoint<TimedEntry<Pose2dWithCurvature>> ReferencePoint() const {
|
||||
return iterator_->CurrentState();
|
||||
}
|
||||
|
||||
bool IsFinished() const { return iterator_->IsDone(); }
|
||||
|
||||
private:
|
||||
TimedIterator<Pose2dWithCurvature>* iterator_ = nullptr;
|
||||
std::unique_ptr<TrajectoryTrackerVelocityOutput> previous_velocity_;
|
||||
|
|
|
@ -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;
|
||||
|
||||
|
|
|
@ -11,45 +11,32 @@ class Translation2d final : public VaryInterpolatable<Translation2d> {
|
|||
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_;
|
||||
|
|
|
@ -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_;
|
||||
|
|
|
@ -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 <typename T>
|
||||
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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -14,52 +14,14 @@ class SplineGenerator {
|
|||
static std::vector<Pose2dWithCurvature> ParameterizeSpline(const std::shared_ptr<ParametricSpline>& 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<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;
|
||||
}
|
||||
const double t1 = 1.0);
|
||||
|
||||
static std::vector<Pose2dWithCurvature> 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;
|
||||
}
|
||||
const double max_dtheta);
|
||||
|
||||
static void 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));
|
||||
}
|
||||
}
|
||||
const double max_dx, const double max_dy, const double max_dtheta);
|
||||
};
|
||||
} // namespace fl
|
||||
|
|
|
@ -1,8 +1,8 @@
|
|||
#pragma once
|
||||
|
||||
#include <memory>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
|
||||
namespace fl {
|
||||
|
||||
|
@ -18,7 +18,6 @@ struct TrajectorySamplePoint {
|
|||
int index_floor;
|
||||
int index_ceil;
|
||||
|
||||
public:
|
||||
explicit TrajectorySamplePoint(TrajectoryPoint<S> point)
|
||||
: state(point.state), index_floor(point.index), index_ceil(point.index) {}
|
||||
|
||||
|
|
|
@ -17,45 +17,13 @@ class TrajectoryGenerator {
|
|||
|
||||
public:
|
||||
static TimedTrajectory<Pose2dWithCurvature> 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);
|
||||
}
|
||||
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);
|
||||
|
||||
static IndexedTrajectory<Pose2dWithCurvature> 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;
|
||||
}
|
||||
const std::vector<Pose2d>& waypoints, const double max_dx, const double max_dy,
|
||||
const double max_dtheta);
|
||||
|
||||
template <typename S>
|
||||
struct ConstrainedPose {
|
||||
|
|
|
@ -7,56 +7,10 @@ namespace fl {
|
|||
|
||||
class AngularAccelerationConstraint final : public TimingConstraint<Pose2dWithCurvature> {
|
||||
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_;
|
||||
|
|
|
@ -7,19 +7,10 @@ namespace fl {
|
|||
|
||||
class CentripetalAccelerationConstraint final : public TimingConstraint<Pose2dWithCurvature> {
|
||||
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_;
|
||||
|
|
|
@ -7,22 +7,10 @@ namespace fl {
|
|||
|
||||
class VelocityLimitRadiusConstraint : public TimingConstraint<Pose2dWithCurvature> {
|
||||
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<double>::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_;
|
||||
|
|
Loading…
Reference in New Issue
Block a user