Add constraints
This commit is contained in:
parent
6f0f46fead
commit
996aaf2fc2
|
@ -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" />
|
||||
|
|
|
@ -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>
|
|
@ -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
|
|
@ -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
|
|
@ -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
|
||||
|
|
|
@ -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
|
Loading…
Reference in New Issue
Block a user