WPILibC++ 2025.2.1
Loading...
Searching...
No Matches
EllipticalRegionConstraint.h
Go to the documentation of this file.
1// Copyright (c) FIRST and other WPILib contributors.
2// Open Source Software; you can modify and/or share it under the terms of
3// the WPILib BSD license file in the root directory of this project.
4
5#pragma once
6
7#include <concepts>
8#include <limits>
9
14#include "units/length.h"
15
16namespace frc {
17
18/**
19 * Enforces a particular constraint only within an elliptical region.
20 */
21template <std::derived_from<TrajectoryConstraint> Constraint>
23 public:
24 /**
25 * Constructs a new EllipticalRegionConstraint.
26 *
27 * @param center The center of the ellipse in which to enforce the constraint.
28 * @param xWidth The width of the ellipse in which to enforce the constraint.
29 * @param yWidth The height of the ellipse in which to enforce the constraint.
30 * @param rotation The rotation to apply to all radii around the origin.
31 * @param constraint The constraint to enforce when the robot is within the
32 * region.
33 * @deprecated Use constructor taking Ellipse2d instead.
34 */
35 [[deprecated("Use constructor taking Ellipse2d instead.")]]
37 units::meter_t xWidth,
38 units::meter_t yWidth,
39 const Rotation2d& rotation,
40 const Constraint& constraint)
41 : m_ellipse{Pose2d{center, rotation}, xWidth / 2.0, yWidth / 2.0},
42 m_constraint(constraint) {}
43
44 /**
45 * Constructs a new EllipticalRegionConstraint.
46 *
47 * @param ellipse The ellipse in which to enforce the constraint.
48 * @param constraint The constraint to enforce when the robot is within the
49 * region.
50 */
51 constexpr EllipticalRegionConstraint(const Ellipse2d& ellipse,
52 const Constraint& constraint)
53 : m_ellipse{ellipse}, m_constraint{constraint} {}
54
55 constexpr units::meters_per_second_t MaxVelocity(
56 const Pose2d& pose, units::curvature_t curvature,
57 units::meters_per_second_t velocity) const override {
58 if (m_ellipse.Contains(pose.Translation())) {
59 return m_constraint.MaxVelocity(pose, curvature, velocity);
60 } else {
61 return units::meters_per_second_t{
62 std::numeric_limits<double>::infinity()};
63 }
64 }
65
67 const Pose2d& pose, units::curvature_t curvature,
68 units::meters_per_second_t speed) const override {
69 if (m_ellipse.Contains(pose.Translation())) {
70 return m_constraint.MinMaxAcceleration(pose, curvature, speed);
71 } else {
72 return {};
73 }
74 }
75
76 private:
77 Ellipse2d m_ellipse;
78 Constraint m_constraint;
79};
80
81} // namespace frc
Represents a 2d ellipse space containing translational, rotational, and scaling components.
Definition Ellipse2d.h:26
constexpr bool Contains(const Translation2d &point) const
Checks if a point is contained within this ellipse.
Definition Ellipse2d.h:146
Enforces a particular constraint only within an elliptical region.
Definition EllipticalRegionConstraint.h:22
constexpr EllipticalRegionConstraint(const Ellipse2d &ellipse, const Constraint &constraint)
Constructs a new EllipticalRegionConstraint.
Definition EllipticalRegionConstraint.h:51
constexpr EllipticalRegionConstraint(const Translation2d &center, units::meter_t xWidth, units::meter_t yWidth, const Rotation2d &rotation, const Constraint &constraint)
Constructs a new EllipticalRegionConstraint.
Definition EllipticalRegionConstraint.h:36
constexpr units::meters_per_second_t MaxVelocity(const Pose2d &pose, units::curvature_t curvature, units::meters_per_second_t velocity) const override
Returns the max velocity given the current pose and curvature.
Definition EllipticalRegionConstraint.h:55
constexpr MinMax MinMaxAcceleration(const Pose2d &pose, units::curvature_t curvature, units::meters_per_second_t speed) const override
Returns the minimum and maximum allowable acceleration for the trajectory given pose,...
Definition EllipticalRegionConstraint.h:66
Represents a 2D pose containing translational and rotational elements.
Definition Pose2d.h:28
constexpr const Translation2d & Translation() const
Returns the underlying translation.
Definition Pose2d.h:107
A rotation in a 2D coordinate frame represented by a point on the unit circle (cosine and sine).
Definition Rotation2d.h:31
An interface for defining user-defined velocity and acceleration constraints while generating traject...
Definition TrajectoryConstraint.h:21
Represents a translation in 2D space.
Definition Translation2d.h:29
Definition CAN.h:11
Represents a minimum and maximum acceleration.
Definition TrajectoryConstraint.h:37