Add constraints

This commit is contained in:
Prateek Machiraju 2019-06-25 12:31:21 -04:00
parent 6f0f46fead
commit 996aaf2fc2
6 changed files with 159 additions and 8 deletions

View File

@ -127,7 +127,10 @@
<ClInclude Include="src\mathematics\spline\ParametricQuinticHermiteSpline.h" />
<ClInclude Include="src\mathematics\spline\ParametricSpline.h" />
<ClInclude Include="src\mathematics\spline\SplineGenerator.h" />
<ClInclude Include="src\mathematics\trajectory\constraints\AngularAccelerationConstraint.h" />
<ClInclude Include="src\mathematics\trajectory\constraints\CentripetalAccelerationConstraint.h" />
<ClInclude Include="src\mathematics\trajectory\constraints\TimingConstraint.h" />
<ClInclude Include="src\mathematics\trajectory\constraints\VelocityLimitRadiusConstraint.h" />
<ClInclude Include="src\mathematics\trajectory\DistanceTrajectory.h" />
<ClInclude Include="src\mathematics\trajectory\IndexedTrajectory.h" />
<ClInclude Include="src\mathematics\trajectory\TimedTrajectory.h" />

View File

@ -72,5 +72,14 @@
<ClInclude Include="src\mathematics\trajectory\constraints\TimingConstraint.h">
<Filter>Header Files</Filter>
</ClInclude>
<ClInclude Include="src\mathematics\trajectory\constraints\CentripetalAccelerationConstraint.h">
<Filter>Header Files</Filter>
</ClInclude>
<ClInclude Include="src\mathematics\trajectory\constraints\AngularAccelerationConstraint.h">
<Filter>Header Files</Filter>
</ClInclude>
<ClInclude Include="src\mathematics\trajectory\constraints\VelocityLimitRadiusConstraint.h">
<Filter>Header Files</Filter>
</ClInclude>
</ItemGroup>
</Project>

View File

@ -0,0 +1,69 @@
#pragma once
#include "../../geometry/Pose2dWithCurvature.h"
#include "TimingConstraint.h"
namespace frc5190 {
class AngularAccelerationConstraint final
: public TimingConstraint<Pose2dWithCurvature> {
public:
explicit AngularAccelerationConstraint(double max_angular_acceleration)
: max_angular_acceleration_(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()));
}
frc5190::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 frc5190::MinMaxAcceleration{-max_absolute_acceleration,
max_absolute_acceleration};
}
private:
double max_angular_acceleration_;
};
} // namespace frc5190

View File

@ -0,0 +1,32 @@
#pragma once
#include "../../geometry/Pose2dWithCurvature.h"
#include "TimingConstraint.h"
namespace frc5190 {
class CentripetalAccelerationConstraint final
: public TimingConstraint<Pose2dWithCurvature> {
public:
explicit CentripetalAccelerationConstraint(
const double max_centripetal_acceleration)
: max_centripetal_acceleration_(max_centripetal_acceleration) {}
~CentripetalAccelerationConstraint() = default;
double MaxVelocity(const Pose2dWithCurvature& state) const override {
return std::sqrt(
std::abs(max_centripetal_acceleration_ / state.Curvature()));
}
frc5190::MinMaxAcceleration MinMaxAcceleration(
const Pose2dWithCurvature& state, double velocity) const override {
return kNoLimits;
}
private:
double max_centripetal_acceleration_;
};
} // namespace frc5190

View File

@ -3,21 +3,24 @@
#include <limits>
namespace frc5190 {
struct MinMaxAcceleration {
double min_acceleration;
double max_acceleration;
bool IsValid() const { return min_acceleration < max_acceleration; }
};
template <typename S>
class TimingConstraint {
public:
struct MinMaxAcceleration {
double min_acceleration;
double max_acceleration;
bool IsValid() const { return min_acceleration < max_acceleration; }
};
virtual ~TimingConstraint() = default;
static constexpr MinMaxAcceleration kNoLimits =
MinMaxAcceleration{std::numeric_limits<double>::lowest(),
std::numeric_limits<double>::max()};
virtual double MaxVelocity(const S& state) const = 0;
virtual MinMaxAcceleration MinMaxAcceleration(const S& state, double velocity) const = 0;
virtual MinMaxAcceleration MinMaxAcceleration(const S& state,
double velocity) const = 0;
};
} // namespace frc5190

View File

@ -0,0 +1,35 @@
#pragma once
#include "../../geometry/Pose2dWithCurvature.h"
#include "TimingConstraint.h"
namespace frc5190 {
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() = default;
double MaxVelocity(const Pose2dWithCurvature& state) const override {
if (state.Pose().Translation().Distance(point_) < radius_) {
return max_velocity_;
}
return std::numeric_limits<double>::max();
}
frc5190::MinMaxAcceleration MinMaxAcceleration(
const Pose2dWithCurvature& state, double velocity) const override {
return kNoLimits;
}
private:
Translation2d point_;
double radius_;
double max_velocity_;
};
} // namespace frc5190