1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include <concepts>
#include <limits>
#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.
*/
template <std::derived_from<TrajectoryConstraint> Constraint>
class RectangularRegionConstraint : public TrajectoryConstraint {
public:
/**
* Constructs a new RectangularRegionConstraint.
*
* @param bottomLeftPoint The bottom left point of the rectangular region in
* which to enforce the constraint.
* @param topRightPoint The top right point of the rectangular region in which
* to enforce the constraint.
* @param constraint The constraint to enforce when the robot is within the
* region.
* @deprecated Use constructor taking Rectangle2d instead.
*/
[[deprecated("Use constructor taking Rectangle2d instead.")]]
constexpr RectangularRegionConstraint(const Translation2d& bottomLeftPoint,
const Translation2d& topRightPoint,
const 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.
*/
constexpr RectangularRegionConstraint(const Rectangle2d& rectangle,
const Constraint& constraint)
: m_rectangle{rectangle}, m_constraint{constraint} {}
constexpr units::meters_per_second_t MaxVelocity(
const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t velocity) const override {
if (m_rectangle.Contains(pose.Translation())) {
return m_constraint.MaxVelocity(pose, curvature, velocity);
} else {
return units::meters_per_second_t{
std::numeric_limits<double>::infinity()};
}
}
constexpr MinMax MinMaxAcceleration(
const Pose2d& pose, units::curvature_t curvature,
units::meters_per_second_t speed) const override {
if (m_rectangle.Contains(pose.Translation())) {
return m_constraint.MinMaxAcceleration(pose, curvature, speed);
} else {
return {};
}
}
private:
Rectangle2d m_rectangle;
Constraint m_constraint;
};
} // namespace frc