WPILibC++  2020.3.2
TrajectoryParameterizer.h
1 /*----------------------------------------------------------------------------*/
2 /* Copyright (c) 2019 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 /*
9  * MIT License
10  *
11  * Copyright (c) 2018 Team 254
12  *
13  * Permission is hereby granted, free of charge, to any person obtaining a copy
14  * of this software and associated documentation files (the "Software"), to deal
15  * in the Software without restriction, including without limitation the rights
16  * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
17  * copies of the Software, and to permit persons to whom the Software is
18  * furnished to do so, subject to the following conditions:
19  *
20  * The above copyright notice and this permission notice shall be included in
21  * all copies or substantial portions of the Software.
22  *
23  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
24  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
25  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
26  * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
27  * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
28  * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
29  * SOFTWARE.
30  */
31 
32 #pragma once
33 
34 #include <memory>
35 #include <utility>
36 #include <vector>
37 
38 #include "frc/trajectory/Trajectory.h"
39 #include "frc/trajectory/constraint/TrajectoryConstraint.h"
40 
41 namespace frc {
46  public:
47  using PoseWithCurvature = std::pair<Pose2d, curvature_t>;
48 
69  const std::vector<PoseWithCurvature>& points,
70  const std::vector<std::unique_ptr<TrajectoryConstraint>>& constraints,
71  units::meters_per_second_t startVelocity,
72  units::meters_per_second_t endVelocity,
73  units::meters_per_second_t maxVelocity,
74  units::meters_per_second_squared_t maxAcceleration, bool reversed);
75 
76  private:
77  constexpr static double kEpsilon = 1E-6;
78 
84  struct ConstrainedState {
85  PoseWithCurvature pose = {Pose2d(), curvature_t(0.0)};
86  units::meter_t distance = 0_m;
87  units::meters_per_second_t maxVelocity = 0_mps;
88  units::meters_per_second_squared_t minAcceleration = 0_mps_sq;
89  units::meters_per_second_squared_t maxAcceleration = 0_mps_sq;
90  };
91 
102  static void EnforceAccelerationLimits(
103  bool reverse,
104  const std::vector<std::unique_ptr<TrajectoryConstraint>>& constraints,
105  ConstrainedState* state);
106 };
107 } // namespace frc
frc::TrajectoryParameterizer::TimeParameterizeTrajectory
static Trajectory TimeParameterizeTrajectory(const std::vector< PoseWithCurvature > &points, const std::vector< std::unique_ptr< TrajectoryConstraint >> &constraints, units::meters_per_second_t startVelocity, units::meters_per_second_t endVelocity, units::meters_per_second_t maxVelocity, units::meters_per_second_squared_t maxAcceleration, bool reversed)
Parameterize the trajectory by time.
frc::Pose2d
Represents a 2d pose containing translational and rotational elements.
Definition: Pose2d.h:23
frc::curvature_t
units::unit_t< units::compound_unit< units::radian, units::inverse< units::meter > >> curvature_t
Define a unit for curvature.
Definition: Trajectory.h:27
frc::Trajectory
Represents a time-parameterized trajectory.
Definition: Trajectory.h:34
frc
A class that enforces constraints on the differential drive kinematics.
Definition: SPIAccelerometerSim.h:16
frc::TrajectoryParameterizer
Class used to parameterize a trajectory by time.
Definition: TrajectoryParameterizer.h:45