Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-24 01:31:46 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Make trajectory constraints use Rectangle2d and Ellipse2d (#6901)

Browse Source
This commit is contained in:
Tyler Veness
2024-08-01 16:46:23 -07:00
committed by GitHub
parent ddd64aa70c
commit 685c732568
8 changed files with 108 additions and 232 deletions
Download Patch File Download Diff File Expand all files Collapse all files

57
wpimath/src/main/native/include/frc/trajectory/constraint/EllipticalRegionConstraint.h
View File

@@ -7,12 +7,14 @@
#include <concepts>
#include <limits>
#include "frc/geometry/Ellipse2d.h"
#include "frc/geometry/Rotation2d.h"
#include "frc/geometry/Translation2d.h"
#include "frc/trajectory/constraint/TrajectoryConstraint.h"
#include "units/length.h"
namespace frc {
/**
* Enforces a particular constraint only within an elliptical region.
*/
@@ -27,21 +29,31 @@ class EllipticalRegionConstraint : public TrajectoryConstraint {
* @param yWidth The height of the ellipse in which to enforce the constraint.
* @param rotation The rotation to apply to all radii around the origin.
* @param constraint The constraint to enforce when the robot is within the
* region.
* region.
* @deprecated Use constructor taking Ellipse2d instead.
*/
[[deprecated("Use constructor taking Ellipse2d instead.")]]
EllipticalRegionConstraint(const Translation2d& center, units::meter_t xWidth,
units::meter_t yWidth, const Rotation2d& rotation,
const Constraint& constraint)
: m_center(center),
m_radii(xWidth / 2.0, yWidth / 2.0),
m_constraint(constraint) {
m_radii = m_radii.RotateBy(rotation);
}
: m_ellipse{Pose2d{center, rotation}, xWidth / 2.0, yWidth / 2.0},
m_constraint(constraint) {}
/**
* Constructs a new EllipticalRegionConstraint.
*
* @param ellipse The ellipse in which to enforce the constraint.
* @param constraint The constraint to enforce when the robot is within the
* region.
*/
EllipticalRegionConstraint(const Ellipse2d& ellipse,
const Constraint& constraint)
: m_ellipse{ellipse}, m_constraint{constraint} {}
units::meters_per_second_t MaxVelocity(
const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t velocity) const override {
if (IsPoseInRegion(pose)) {
if (m_ellipse.Contains(pose.Translation())) {
return m_constraint.MaxVelocity(pose, curvature, velocity);
} else {
return units::meters_per_second_t{
@@ -51,41 +63,16 @@ class EllipticalRegionConstraint : public TrajectoryConstraint {
MinMax MinMaxAcceleration(const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t speed) const override {
if (IsPoseInRegion(pose)) {
if (m_ellipse.Contains(pose.Translation())) {
return m_constraint.MinMaxAcceleration(pose, curvature, speed);
} else {
return {};
}
}
/**
* Returns whether the specified robot pose is within the region that the
* constraint is enforced in.
*
* @param pose The robot pose.
* @return Whether the robot pose is within the constraint region.
*/
bool IsPoseInRegion(const Pose2d& pose) const {
// The region bounded by the ellipse is given by the equation:
//
// (x−h)²/Rx² + (y−k)²/Ry² ≤ 1
//
// Multiply by Rx²Ry² for efficiency reasons:
//
// (x−h)²Ry² + (y−k)²Rx² ≤ Rx²Ry²
//
// If the inequality is satisfied, then it is inside the ellipse; otherwise
// it is outside the ellipse.
return units::math::pow<2>(pose.X() - m_center.X()) *
units::math::pow<2>(m_radii.Y()) +
units::math::pow<2>(pose.Y() - m_center.Y()) *
units::math::pow<2>(m_radii.X()) <=
units::math::pow<2>(m_radii.X()) * units::math::pow<2>(m_radii.Y());
}
private:
Translation2d m_center;
Translation2d m_radii;
Ellipse2d m_ellipse;
Constraint m_constraint;
};
} // namespace frc

47
wpimath/src/main/native/include/frc/trajectory/constraint/RectangularRegionConstraint.h
View File

@@ -7,11 +7,12 @@
#include <concepts>
#include <limits>
#include "frc/geometry/Rotation2d.h"
#include "frc/geometry/Rectangle2d.h"
#include "frc/geometry/Translation2d.h"
#include "frc/trajectory/constraint/TrajectoryConstraint.h"
namespace frc {
/**
* Enforces a particular constraint only within a rectangular region.
*/
@@ -22,23 +23,34 @@ class RectangularRegionConstraint : public TrajectoryConstraint {
* Constructs a new RectangularRegionConstraint.
*
* @param bottomLeftPoint The bottom left point of the rectangular region in
* which to enforce the constraint.
* which to enforce the constraint.
* @param topRightPoint The top right point of the rectangular region in which
* to enforce the constraint.
* to enforce the constraint.
* @param constraint The constraint to enforce when the robot is within the
* region.
* region.
* @deprecated Use constructor taking Rectangle2d instead.
*/
[[deprecated("Use constructor taking Rectangle2d instead.")]]
RectangularRegionConstraint(const Translation2d& bottomLeftPoint,
const Translation2d& topRightPoint,
const Constraint& constraint)
: m_bottomLeftPoint(bottomLeftPoint),
m_topRightPoint(topRightPoint),
m_constraint(constraint) {}
: m_rectangle{bottomLeftPoint, topRightPoint}, m_constraint(constraint) {}
/**
* Constructs a new RectangularRegionConstraint.
*
* @param rectangle The rectangular region in which to enforce the constraint.
* @param constraint The constraint to enforce when the robot is within the
* region.
*/
RectangularRegionConstraint(const Rectangle2d& rectangle,
const Constraint& constraint)
: m_rectangle{rectangle}, m_constraint{constraint} {}
units::meters_per_second_t MaxVelocity(
const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t velocity) const override {
if (IsPoseInRegion(pose)) {
if (m_rectangle.Contains(pose.Translation())) {
return m_constraint.MaxVelocity(pose, curvature, velocity);
} else {
return units::meters_per_second_t{
@@ -48,29 +60,16 @@ class RectangularRegionConstraint : public TrajectoryConstraint {
MinMax MinMaxAcceleration(const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t speed) const override {
if (IsPoseInRegion(pose)) {
if (m_rectangle.Contains(pose.Translation())) {
return m_constraint.MinMaxAcceleration(pose, curvature, speed);
} else {
return {};
}
}
/**
* Returns whether the specified robot pose is within the region that the
* constraint is enforced in.
*
* @param pose The robot pose.
* @return Whether the robot pose is within the constraint region.
*/
bool IsPoseInRegion(const Pose2d& pose) const {
return pose.X() >= m_bottomLeftPoint.X() &&
pose.X() <= m_topRightPoint.X() &&
pose.Y() >= m_bottomLeftPoint.Y() && pose.Y() <= m_topRightPoint.Y();
}
private:
Translation2d m_bottomLeftPoint;
Translation2d m_topRightPoint;
Rectangle2d m_rectangle;
Constraint m_constraint;
};
} // namespace frc