WPILibC++  2020.3.2-60-g3011ebe
EllipticalRegionConstraint.h
1 /*----------------------------------------------------------------------------*/
2 /* Copyright (c) 2020 FIRST. All Rights Reserved. */
3 /* Open Source Software - may be modified and shared by FRC teams. The code */
4 /* must be accompanied by the FIRST BSD license file in the root directory of */
5 /* the project. */
6 /*----------------------------------------------------------------------------*/
7 
8 #pragma once
9 
10 #include <units/units.h>
11 
12 #include "frc/geometry/Rotation2d.h"
13 #include "frc/geometry/Translation2d.h"
14 #include "frc/trajectory/constraint/TrajectoryConstraint.h"
15 
16 namespace frc {
21  public:
32  EllipticalRegionConstraint(const Translation2d& center, units::meter_t xWidth,
33  units::meter_t yWidth, const Rotation2d& rotation,
34  const TrajectoryConstraint& constraint);
35 
36  units::meters_per_second_t MaxVelocity(
37  const Pose2d& pose, units::curvature_t curvature,
38  units::meters_per_second_t velocity) const override;
39 
40  MinMax MinMaxAcceleration(const Pose2d& pose, units::curvature_t curvature,
41  units::meters_per_second_t speed) const override;
42 
50  bool IsPoseInRegion(const Pose2d& pose) const;
51 
52  private:
53  Translation2d m_center;
54  Translation2d m_radii;
55  const TrajectoryConstraint& m_constraint;
56 };
57 } // namespace frc
frc::EllipticalRegionConstraint::IsPoseInRegion
bool IsPoseInRegion(const Pose2d &pose) const
Returns whether the specified robot pose is within the region that the constraint is enforced in.
frc::TrajectoryConstraint::MinMax
Represents a minimum and maximum acceleration.
Definition: TrajectoryConstraint.h:37
frc::Rotation2d
A rotation in a 2d coordinate frame represented a point on the unit circle (cosine and sine).
Definition: Rotation2d.h:22
frc::Translation2d
Represents a translation in 2d space.
Definition: Translation2d.h:28
frc::TrajectoryConstraint
An interface for defining user-defined velocity and acceleration constraints while generating traject...
Definition: TrajectoryConstraint.h:22
frc::EllipticalRegionConstraint::EllipticalRegionConstraint
EllipticalRegionConstraint(const Translation2d &center, units::meter_t xWidth, units::meter_t yWidth, const Rotation2d &rotation, const TrajectoryConstraint &constraint)
Constructs a new EllipticalRegionConstraint.
frc::Pose2d
Represents a 2d pose containing translational and rotational elements.
Definition: Pose2d.h:23
frc::EllipticalRegionConstraint::MinMaxAcceleration
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,...
frc::EllipticalRegionConstraint
Enforces a particular constraint only within an elliptical region.
Definition: EllipticalRegionConstraint.h:20
frc
A class that enforces constraints on the differential drive kinematics.
Definition: PDPSim.h:16
frc::EllipticalRegionConstraint::MaxVelocity
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.