Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-28 02:11:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Copy child constraint in region constraints (#2831)

Browse Source
This commit is contained in:
Prateek Machiraju
2020-11-14 15:03:26 -05:00
committed by GitHub
parent cfac22b4c0
commit 66b57f0323
4 changed files with 67 additions and 110 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -7,6 +7,8 @@
#pragma once
#include <limits>
#include "frc/geometry/Rotation2d.h"
#include "frc/geometry/Translation2d.h"
#include "frc/trajectory/constraint/TrajectoryConstraint.h"
@@ -16,6 +18,8 @@ namespace frc {
/**
* Enforces a particular constraint only within an elliptical region.
*/
template <typename Constraint, typename = std::enable_if_t<std::is_base_of_v<
TrajectoryConstraint, Constraint>>>
class EllipticalRegionConstraint : public TrajectoryConstraint {
public:
/**
@@ -30,14 +34,32 @@ class EllipticalRegionConstraint : public TrajectoryConstraint {
*/
EllipticalRegionConstraint(const Translation2d& center, units::meter_t xWidth,
units::meter_t yWidth, const Rotation2d& rotation,
const TrajectoryConstraint& constraint);
const Constraint& constraint)
: m_center(center),
m_radii(xWidth / 2.0, yWidth / 2.0),
m_constraint(constraint) {
m_radii = m_radii.RotateBy(rotation);
}
units::meters_per_second_t MaxVelocity(
const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t velocity) const override;
units::meters_per_second_t velocity) const override {
if (IsPoseInRegion(pose)) {
return m_constraint.MaxVelocity(pose, curvature, velocity);
} else {
return units::meters_per_second_t(
std::numeric_limits<double>::infinity());
}
}
MinMax MinMaxAcceleration(const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t speed) const override;
units::meters_per_second_t speed) const override {
if (IsPoseInRegion(pose)) {
return m_constraint.MinMaxAcceleration(pose, curvature, speed);
} else {
return {};
}
}
/**
* Returns whether the specified robot pose is within the region that the
@@ -46,11 +68,22 @@ class EllipticalRegionConstraint : public TrajectoryConstraint {
* @param pose The robot pose.
* @return Whether the robot pose is within the constraint region.
*/
bool IsPoseInRegion(const Pose2d& pose) const;
bool IsPoseInRegion(const Pose2d& pose) const {
// The region (disk) bounded by the ellipse is given by the equation:
// ((x-h)^2)/Rx^2) + ((y-k)^2)/Ry^2) <= 1
// If the inequality is satisfied, then it is inside the ellipse; otherwise
// it is outside the ellipse.
// Both sides have been multiplied by Rx^2 * Ry^2 for efficiency reasons.
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;
const TrajectoryConstraint& m_constraint;
Constraint m_constraint;
};
} // namespace frc

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

@@ -7,6 +7,8 @@
#pragma once
#include <limits>
#include "frc/geometry/Rotation2d.h"
#include "frc/geometry/Translation2d.h"
#include "frc/trajectory/constraint/TrajectoryConstraint.h"
@@ -15,6 +17,8 @@ namespace frc {
/**
* Enforces a particular constraint only within a rectangular region.
*/
template <typename Constraint, typename = std::enable_if_t<std::is_base_of_v<
TrajectoryConstraint, Constraint>>>
class RectangularRegionConstraint : public TrajectoryConstraint {
public:
/**
@@ -29,14 +33,30 @@ class RectangularRegionConstraint : public TrajectoryConstraint {
*/
RectangularRegionConstraint(const Translation2d& bottomLeftPoint,
const Translation2d& topRightPoint,
const TrajectoryConstraint& constraint);
const Constraint& constraint)
: m_bottomLeftPoint(bottomLeftPoint),
m_topRightPoint(topRightPoint),
m_constraint(constraint) {}
units::meters_per_second_t MaxVelocity(
const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t velocity) const override;
units::meters_per_second_t velocity) const override {
if (IsPoseInRegion(pose)) {
return m_constraint.MaxVelocity(pose, curvature, velocity);
} else {
return units::meters_per_second_t(
std::numeric_limits<double>::infinity());
}
}
MinMax MinMaxAcceleration(const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t speed) const override;
units::meters_per_second_t speed) const override {
if (IsPoseInRegion(pose)) {
return m_constraint.MinMaxAcceleration(pose, curvature, speed);
} else {
return {};
}
}
/**
* Returns whether the specified robot pose is within the region that the
@@ -45,11 +65,15 @@ class RectangularRegionConstraint : public TrajectoryConstraint {
* @param pose The robot pose.
* @return Whether the robot pose is within the constraint region.
*/
bool IsPoseInRegion(const Pose2d& pose) const;
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;
const TrajectoryConstraint& m_constraint;
Constraint m_constraint;
};
} // namespace frc