Run autoformat
This commit is contained in:
parent
3dff114fd5
commit
bdc8053235
|
@ -18,8 +18,8 @@ AlwaysBreakAfterDefinitionReturnType: None
|
|||
AlwaysBreakAfterReturnType: None
|
||||
AlwaysBreakBeforeMultilineStrings: true
|
||||
AlwaysBreakTemplateDeclarations: true
|
||||
BinPackArguments: true
|
||||
BinPackParameters: true
|
||||
BinPackArguments: false
|
||||
BinPackParameters: false
|
||||
BraceWrapping:
|
||||
AfterClass: false
|
||||
AfterControlStatement: false
|
||||
|
|
|
@ -82,8 +82,8 @@ class Pose2d final : public VaryInterpolatable<Pose2d> {
|
|||
pose.translation_ *
|
||||
Rotation2d{half_theta_by_tan_of_half_dtheta, -half_dtheta, false};
|
||||
|
||||
return Twist2d{translation_part.X(), translation_part.Y(),
|
||||
pose.rotation_.Radians()};
|
||||
return Twist2d{
|
||||
translation_part.X(), translation_part.Y(), pose.rotation_.Radians()};
|
||||
}
|
||||
|
||||
static Pose2d FromTwist(const Twist2d& twist) {
|
||||
|
|
|
@ -38,7 +38,8 @@ class Rotation2d final {
|
|||
|
||||
Rotation2d operator+(const Rotation2d& other) const {
|
||||
return Rotation2d{Cos() * other.Cos() - Sin() * other.Sin(),
|
||||
Cos() * other.Sin() + Sin() * other.Cos(), true};
|
||||
Cos() * other.Sin() + Sin() * other.Cos(),
|
||||
true};
|
||||
}
|
||||
|
||||
// Accessors
|
||||
|
|
|
@ -45,9 +45,11 @@ class DistanceTrajectory : public Trajectory<double, S> {
|
|||
return TrajectorySamplePoint<S>(s, i, i);
|
||||
}
|
||||
return TrajectorySamplePoint<S>(
|
||||
prev_s.Interpolate(s, (interpolant - distances_[i - 1]) /
|
||||
(distances_[i] - distances_[i - 1])),
|
||||
i - 1, i);
|
||||
prev_s.Interpolate(s,
|
||||
(interpolant - distances_[i - 1]) /
|
||||
(distances_[i] - distances_[i - 1])),
|
||||
i - 1,
|
||||
i);
|
||||
}
|
||||
}
|
||||
throw - 1;
|
||||
|
|
|
@ -7,18 +7,16 @@ namespace frc5190 {
|
|||
template <typename S>
|
||||
class TimedEntry final : public VaryInterpolatable<TimedEntry<S>> {
|
||||
public:
|
||||
TimedEntry(const S& state, const double t, const double velocity,
|
||||
TimedEntry(const S& state,
|
||||
const double t,
|
||||
const double velocity,
|
||||
const double acceleration)
|
||||
: state_(state),
|
||||
t_(t),
|
||||
velocity_(velocity),
|
||||
acceleration_(acceleration) {}
|
||||
|
||||
TimedEntry()
|
||||
: t_(0),
|
||||
velocity_(0),
|
||||
acceleration_(0) {
|
||||
}
|
||||
TimedEntry() : t_(0), velocity_(0), acceleration_(0) {}
|
||||
|
||||
TimedEntry<S> Interpolate(const TimedEntry<S>& end_value,
|
||||
double t) const override {
|
||||
|
@ -38,7 +36,9 @@ class TimedEntry final : public VaryInterpolatable<TimedEntry<S>> {
|
|||
return TimedEntry{
|
||||
state_.Interpolate(end_value.state_,
|
||||
new_s / state_.Distance(end_value.state_)),
|
||||
new_t, new_v, acceleration_};
|
||||
new_t,
|
||||
new_v,
|
||||
acceleration_};
|
||||
}
|
||||
|
||||
double Distance(const TimedEntry<S>& other) const override {
|
||||
|
@ -100,7 +100,8 @@ class TimedTrajectory : public Trajectory<double, TimedEntry<S>> {
|
|||
prev_s.state.Interpolate(s.state,
|
||||
(interpolant - prev_s.state.T()) /
|
||||
(s.state.T() - prev_s.state.T())),
|
||||
i - 1, i);
|
||||
i - 1,
|
||||
i);
|
||||
}
|
||||
}
|
||||
throw - 1;
|
||||
|
|
|
@ -13,24 +13,20 @@ struct TrajectoryPoint {
|
|||
|
||||
template <typename S>
|
||||
struct TrajectorySamplePoint {
|
||||
|
||||
S state;
|
||||
int index_floor;
|
||||
int index_ceil;
|
||||
|
||||
public:
|
||||
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) {}
|
||||
|
||||
TrajectorySamplePoint(S state, int index_floor, int index_ceil)
|
||||
: state(std::move(state)), index_floor(index_floor), index_ceil(index_ceil) {}
|
||||
: state(std::move(state)),
|
||||
index_floor(index_floor),
|
||||
index_ceil(index_ceil) {}
|
||||
|
||||
TrajectorySamplePoint()
|
||||
: index_floor(0),
|
||||
index_ceil(0) {
|
||||
}
|
||||
TrajectorySamplePoint() : index_floor(0), index_ceil(0) {}
|
||||
};
|
||||
|
||||
template <typename U, typename S>
|
||||
|
|
|
@ -1,6 +1,5 @@
|
|||
#pragma once
|
||||
|
||||
#include <array>
|
||||
#include "../geometry/Pose2dWithCurvature.h"
|
||||
#include "../spline/ParametricQuinticHermiteSpline.h"
|
||||
#include "../spline/SplineGenerator.h"
|
||||
|
@ -16,8 +15,11 @@ class TrajectoryGenerator {
|
|||
static TimedTrajectory<Pose2dWithCurvature> GenerateTrajectory(
|
||||
std::vector<Pose2d> waypoints,
|
||||
const std::vector<TimingConstraint<Pose2dWithCurvature>*>& constraints,
|
||||
const double start_velocity, const double end_velocity,
|
||||
const double max_velocity, const double max_acceleration, bool reversed) {
|
||||
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)};
|
||||
|
||||
|
@ -35,19 +37,27 @@ class TrajectoryGenerator {
|
|||
if (reversed) {
|
||||
for (auto& point : points) {
|
||||
point = Pose2dWithCurvature{point.Pose().TransformBy(flipped_position),
|
||||
-point.Curvature(), point.Dkds()};
|
||||
-point.Curvature(),
|
||||
point.Dkds()};
|
||||
}
|
||||
}
|
||||
|
||||
return TimeParameterizeTrajectory(
|
||||
DistanceTrajectory<Pose2dWithCurvature>(points), constraints,
|
||||
start_velocity, end_velocity, max_velocity, max_acceleration, 0.051,
|
||||
DistanceTrajectory<Pose2dWithCurvature>(points),
|
||||
constraints,
|
||||
start_velocity,
|
||||
end_velocity,
|
||||
max_velocity,
|
||||
max_acceleration,
|
||||
0.051,
|
||||
reversed);
|
||||
}
|
||||
|
||||
static IndexedTrajectory<Pose2dWithCurvature> TrajectoryFromSplineWaypoints(
|
||||
const std::vector<Pose2d>& waypoints, const double max_dx,
|
||||
const double max_dy, const double max_dtheta) {
|
||||
const std::vector<Pose2d>& waypoints,
|
||||
const double max_dx,
|
||||
const double max_dy,
|
||||
const double max_dtheta) {
|
||||
auto size = static_cast<int>(waypoints.size());
|
||||
std::vector<ParametricSpline*> splines(size - 1);
|
||||
for (int i = 1; i < waypoints.size(); ++i) {
|
||||
|
@ -55,8 +65,8 @@ class TrajectoryGenerator {
|
|||
new ParametricQuinticHermiteSpline(waypoints[i - 1], waypoints[i]);
|
||||
}
|
||||
auto trajectory = IndexedTrajectory<Pose2dWithCurvature>(
|
||||
SplineGenerator::ParameterizeSplines(splines, max_dx, max_dy,
|
||||
max_dtheta));
|
||||
SplineGenerator::ParameterizeSplines(
|
||||
splines, max_dx, max_dy, max_dtheta));
|
||||
|
||||
for (auto ptr : splines) {
|
||||
delete ptr;
|
||||
|
@ -76,7 +86,8 @@ class TrajectoryGenerator {
|
|||
|
||||
template <typename S>
|
||||
static void EnforceAccelerationLimits(
|
||||
bool reverse, std::vector<TimingConstraint<S>*> constraints,
|
||||
bool reverse,
|
||||
std::vector<TimingConstraint<S>*> constraints,
|
||||
ConstrainedPose<S>* constrained_pose) {
|
||||
for (const auto& constraint : constraints) {
|
||||
auto min_max_accel = constraint->MinMaxAcceleration(
|
||||
|
@ -101,8 +112,12 @@ class TrajectoryGenerator {
|
|||
static TimedTrajectory<S> TimeParameterizeTrajectory(
|
||||
DistanceTrajectory<S> distance_trajectory,
|
||||
std::vector<TimingConstraint<Pose2dWithCurvature>*> constraints,
|
||||
double start_velocity, double end_velocity, double max_velocity,
|
||||
double max_acceleration, double step_size, bool reversed) {
|
||||
double start_velocity,
|
||||
double end_velocity,
|
||||
double max_velocity,
|
||||
double max_acceleration,
|
||||
double step_size,
|
||||
bool reversed) {
|
||||
const auto num_states = static_cast<int>(
|
||||
std::ceil(distance_trajectory.LastInterpolant() / step_size + 1));
|
||||
|
||||
|
@ -126,8 +141,8 @@ class TrajectoryGenerator {
|
|||
|
||||
std::vector<ConstrainedPose<S>> constrained_poses(num_states);
|
||||
|
||||
auto _predecessor = ConstrainedPose<S>{states[0], 0.0, start_velocity,
|
||||
-max_acceleration, max_acceleration};
|
||||
auto _predecessor = ConstrainedPose<S>{
|
||||
states[0], 0.0, start_velocity, -max_acceleration, max_acceleration};
|
||||
ConstrainedPose<S>* predecessor = &_predecessor;
|
||||
|
||||
for (int i = 0; i < states.size(); ++i) {
|
||||
|
@ -196,7 +211,9 @@ class TrajectoryGenerator {
|
|||
auto _successor =
|
||||
ConstrainedPose<S>{states[states.size() - 1],
|
||||
constrained_poses[states.size() - 1].distance,
|
||||
end_velocity, -max_acceleration, max_acceleration};
|
||||
end_velocity,
|
||||
-max_acceleration,
|
||||
max_acceleration};
|
||||
ConstrainedPose<S>* successor = &_successor;
|
||||
|
||||
for (int i = states.size() - 1; i >= 0; --i) {
|
||||
|
@ -268,9 +285,10 @@ class TrajectoryGenerator {
|
|||
|
||||
v = constrained_pose.max_velocity;
|
||||
s = constrained_pose.distance;
|
||||
timed_states[i] =
|
||||
TimedEntry<S>{constrained_pose.state, t, reversed ? -v : v,
|
||||
reversed ? -accel : accel};
|
||||
timed_states[i] = TimedEntry<S>{constrained_pose.state,
|
||||
t,
|
||||
reversed ? -v : v,
|
||||
reversed ? -accel : accel};
|
||||
|
||||
t += dt;
|
||||
}
|
||||
|
|
|
@ -19,17 +19,17 @@ class TrajectoryIterator {
|
|||
}
|
||||
|
||||
TrajectorySamplePoint<S> Advance(U amount) {
|
||||
progress_ =
|
||||
Clamp(Addition(progress_, amount), trajectory_->FirstInterpolant(),
|
||||
trajectory_->LastInterpolant());
|
||||
progress_ = Clamp(Addition(progress_, amount),
|
||||
trajectory_->FirstInterpolant(),
|
||||
trajectory_->LastInterpolant());
|
||||
sample_ = trajectory_->Sample(progress_);
|
||||
return sample_;
|
||||
}
|
||||
|
||||
TrajectorySamplePoint<S> Preview(U amount) {
|
||||
auto progress =
|
||||
Clamp(Addition(progress_, amount), trajectory_->FirstInterpolant(),
|
||||
trajectory_->LastInterpolant());
|
||||
auto progress = Clamp(Addition(progress_, amount),
|
||||
trajectory_->FirstInterpolant(),
|
||||
trajectory_->LastInterpolant());
|
||||
return trajectory_->Sample(progress);
|
||||
}
|
||||
|
||||
|
|
|
@ -13,7 +13,6 @@ class AngularAccelerationConstraint final
|
|||
|
||||
~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.
|
||||
|
|
|
@ -7,7 +7,6 @@ namespace frc5190 {
|
|||
|
||||
class CentripetalAccelerationConstraint final
|
||||
: public TimingConstraint<Pose2dWithCurvature> {
|
||||
|
||||
public:
|
||||
explicit CentripetalAccelerationConstraint(
|
||||
const double max_centripetal_acceleration)
|
||||
|
|
|
@ -8,7 +8,8 @@ namespace frc5190 {
|
|||
class VelocityLimitRadiusConstraint
|
||||
: public TimingConstraint<Pose2dWithCurvature> {
|
||||
public:
|
||||
VelocityLimitRadiusConstraint(const Translation2d& point, const double radius,
|
||||
VelocityLimitRadiusConstraint(const Translation2d& point,
|
||||
const double radius,
|
||||
const double max_velocity)
|
||||
: point_(point), radius_(radius), max_velocity_(max_velocity) {}
|
||||
|
||||
|
|
|
@ -9,7 +9,8 @@ class Interpolatable {
|
|||
virtual ~Interpolatable() = default;
|
||||
virtual T Interpolate(const T& end_value, double t) const = 0;
|
||||
|
||||
static constexpr double Lerp(const double start_value, const double end_value,
|
||||
static constexpr double Lerp(const double start_value,
|
||||
const double end_value,
|
||||
const double t) {
|
||||
return start_value + (end_value - start_value) * Clamp(t, 0.0, 1.0);
|
||||
}
|
||||
|
|
|
@ -1,6 +1,5 @@
|
|||
#include "pch.h"
|
||||
#include "../FalconLibraryCPP/src/FalconLibrary.h"
|
||||
#include "../FalconLibraryCPP/src/mathematics/trajectory/constraints/CentripetalAccelerationConstraint.h"
|
||||
|
||||
constexpr double kTestEpsilon = 1E-6;
|
||||
|
||||
|
@ -8,25 +7,30 @@ class TrajectoryTest : public ::testing::Test {
|
|||
public:
|
||||
TrajectoryTest() {}
|
||||
|
||||
void Run(frc5190::Pose2d initial, frc5190::Pose2d final,
|
||||
double max_velocity = 3, double max_acceleration = 2,
|
||||
void Run(frc5190::Pose2d initial,
|
||||
frc5190::Pose2d final,
|
||||
double max_velocity = 3,
|
||||
double max_acceleration = 2,
|
||||
bool backwards = false) {
|
||||
auto trajectory = frc5190::TrajectoryGenerator::GenerateTrajectory(
|
||||
std::vector<frc5190::Pose2d>{initial, final},
|
||||
std::vector<frc5190::TimingConstraint<frc5190::Pose2dWithCurvature>*>{
|
||||
new frc5190::CentripetalAccelerationConstraint{100.0}},
|
||||
0.0, 0.0, max_velocity, max_acceleration, backwards);
|
||||
std::vector<frc5190::TimingConstraint<frc5190::Pose2dWithCurvature>*>{},
|
||||
0.0,
|
||||
0.0,
|
||||
max_velocity,
|
||||
max_acceleration,
|
||||
backwards);
|
||||
|
||||
auto pose = trajectory.Sample(0.0).state.State().Pose();
|
||||
|
||||
EXPECT_FALSE(false);
|
||||
|
||||
EXPECT_NEAR(pose.Translation().X(), initial.Translation().X(),
|
||||
kTestEpsilon);
|
||||
EXPECT_NEAR(pose.Translation().Y(), initial.Translation().Y(),
|
||||
kTestEpsilon);
|
||||
EXPECT_NEAR(pose.Rotation().Radians(), initial.Rotation().Radians(),
|
||||
kTestEpsilon);
|
||||
EXPECT_NEAR(
|
||||
pose.Translation().X(), initial.Translation().X(), kTestEpsilon);
|
||||
EXPECT_NEAR(
|
||||
pose.Translation().Y(), initial.Translation().Y(), kTestEpsilon);
|
||||
EXPECT_NEAR(
|
||||
pose.Rotation().Radians(), initial.Rotation().Radians(), kTestEpsilon);
|
||||
|
||||
const auto iterator = trajectory.Iterator();
|
||||
|
||||
|
@ -51,8 +55,8 @@ class TrajectoryTest : public ::testing::Test {
|
|||
|
||||
EXPECT_NEAR(pose1.Translation().X(), final.Translation().X(), kTestEpsilon);
|
||||
EXPECT_NEAR(pose1.Translation().Y(), final.Translation().Y(), kTestEpsilon);
|
||||
EXPECT_NEAR(pose1.Rotation().Radians(), final.Rotation().Radians(),
|
||||
kTestEpsilon);
|
||||
EXPECT_NEAR(
|
||||
pose1.Rotation().Radians(), final.Rotation().Radians(), kTestEpsilon);
|
||||
}
|
||||
};
|
||||
|
||||
|
|
Loading…
Reference in New Issue
Block a user