mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-24 01:31:46 +00:00
Refactor TrajectoryGenerator (#1972)
This commit is contained in:
Prateek Machiraju
committed by
Peter Johnson
Peter Johnson
parent
73a30182c3
commit
9440edf2b5
@@ -0,0 +1,140 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) 2019 FIRST. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <memory>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
#include <units/units.h>
|
||||
|
||||
#include "frc/kinematics/DifferentialDriveKinematics.h"
|
||||
#include "frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h"
|
||||
#include "frc/trajectory/constraint/TrajectoryConstraint.h"
|
||||
|
||||
namespace frc {
|
||||
/**
|
||||
* Represents the configuration for generating a trajectory. This class stores
|
||||
* the start velocity, end velocity, max velocity, max acceleration, custom
|
||||
* constraints, and the reversed flag.
|
||||
*
|
||||
* The class must be constructed with a max velocity and max acceleration.
|
||||
* The other parameters (start velocity, end velocity, constraints, reversed)
|
||||
* have been defaulted to reasonable values (0, 0, {}, false). These values can
|
||||
* be changed via the SetXXX methods.
|
||||
*/
|
||||
class TrajectoryConfig {
|
||||
public:
|
||||
/**
|
||||
* Constructs a config object.
|
||||
* @param maxVelocity The max velocity of the trajectory.
|
||||
* @param maxAcceleration The max acceleration of the trajectory.
|
||||
*/
|
||||
TrajectoryConfig(units::meters_per_second_t maxVelocity,
|
||||
units::meters_per_second_squared_t maxAcceleration)
|
||||
: m_maxVelocity(maxVelocity), m_maxAcceleration(maxAcceleration) {}
|
||||
|
||||
TrajectoryConfig(const TrajectoryConfig&) = delete;
|
||||
TrajectoryConfig& operator=(const TrajectoryConfig&) = delete;
|
||||
|
||||
TrajectoryConfig(TrajectoryConfig&&) = default;
|
||||
TrajectoryConfig& operator=(TrajectoryConfig&&) = default;
|
||||
|
||||
/**
|
||||
* Sets the start velocity of the trajectory.
|
||||
* @param startVelocity The start velocity of the trajectory.
|
||||
*/
|
||||
void SetStartVelocity(units::meters_per_second_t startVelocity) {
|
||||
m_startVelocity = startVelocity;
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the end velocity of the trajectory.
|
||||
* @param endVelocity The end velocity of the trajectory.
|
||||
*/
|
||||
void SetEndVelocity(units::meters_per_second_t endVelocity) {
|
||||
m_endVelocity = endVelocity;
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets the reversed flag of the trajectory.
|
||||
* @param reversed Whether the trajectory should be reversed or not.
|
||||
*/
|
||||
void SetReversed(bool reversed) { m_reversed = reversed; }
|
||||
|
||||
/**
|
||||
* Adds a user-defined constraint to the trajectory.
|
||||
* @param constraint The user-defined constraint.
|
||||
*/
|
||||
template <typename Constraint, typename = std::enable_if_t<std::is_base_of_v<
|
||||
TrajectoryConstraint, Constraint>>>
|
||||
void AddConstraint(Constraint constraint) {
|
||||
m_constraints.emplace_back(std::make_unique<Constraint>(constraint));
|
||||
}
|
||||
|
||||
/**
|
||||
* Adds a differential drive kinematics constraint to ensure that
|
||||
* no wheel velocity of a differential drive goes above the max velocity.
|
||||
*
|
||||
* @param kinematics The differential drive kinematics.
|
||||
*/
|
||||
void SetKinematics(const DifferentialDriveKinematics& kinematics) {
|
||||
AddConstraint(
|
||||
DifferentialDriveKinematicsConstraint(kinematics, m_maxVelocity));
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the starting velocity of the trajectory.
|
||||
* @return The starting velocity of the trajectory.
|
||||
*/
|
||||
units::meters_per_second_t StartVelocity() const { return m_startVelocity; }
|
||||
|
||||
/**
|
||||
* Returns the ending velocity of the trajectory.
|
||||
* @return The ending velocity of the trajectory.
|
||||
*/
|
||||
units::meters_per_second_t EndVelocity() const { return m_endVelocity; }
|
||||
|
||||
/**
|
||||
* Returns the maximum velocity of the trajectory.
|
||||
* @return The maximum velocity of the trajectory.
|
||||
*/
|
||||
units::meters_per_second_t MaxVelocity() const { return m_maxVelocity; }
|
||||
|
||||
/**
|
||||
* Returns the maximum acceleration of the trajectory.
|
||||
* @return The maximum acceleration of the trajectory.
|
||||
*/
|
||||
units::meters_per_second_squared_t MaxAcceleration() const {
|
||||
return m_maxAcceleration;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the user-defined constraints of the trajectory.
|
||||
* @return The user-defined constraints of the trajectory.
|
||||
*/
|
||||
const std::vector<std::unique_ptr<TrajectoryConstraint>>& Constraints()
|
||||
const {
|
||||
return m_constraints;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns whether the trajectory is reversed or not.
|
||||
* @return whether the trajectory is reversed or not.
|
||||
*/
|
||||
bool IsReversed() const { return m_reversed; }
|
||||
|
||||
private:
|
||||
units::meters_per_second_t m_startVelocity = 0_mps;
|
||||
units::meters_per_second_t m_endVelocity = 0_mps;
|
||||
units::meters_per_second_t m_maxVelocity;
|
||||
units::meters_per_second_squared_t m_maxAcceleration;
|
||||
std::vector<std::unique_ptr<TrajectoryConstraint>> m_constraints;
|
||||
bool m_reversed = false;
|
||||
};
|
||||
} // namespace frc
|
||||
@@ -13,6 +13,7 @@
|
||||
|
||||
#include "frc/spline/SplineParameterizer.h"
|
||||
#include "frc/trajectory/Trajectory.h"
|
||||
#include "frc/trajectory/TrajectoryConfig.h"
|
||||
#include "frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h"
|
||||
#include "frc/trajectory/constraint/TrajectoryConstraint.h"
|
||||
|
||||
@@ -25,114 +26,63 @@ class TrajectoryGenerator {
|
||||
using PoseWithCurvature = std::pair<Pose2d, curvature_t>;
|
||||
|
||||
/**
|
||||
* Generates a trajectory with the given waypoints and constraints.
|
||||
* Generates a trajectory from the given control vectors and config. This
|
||||
* method uses clamped cubic splines -- a method in which the exterior control
|
||||
* vectors and interior waypoints are provided. The headings are automatically
|
||||
* determined at the interior points to ensure continuous curvature.
|
||||
*
|
||||
* @param waypoints A vector of points that the trajectory must go through.
|
||||
* @param constraints A vector of various velocity and acceleration
|
||||
* constraints.
|
||||
* @param startVelocity The start velocity for the trajectory.
|
||||
* @param endVelocity The end velocity for the trajectory.
|
||||
* @param maxVelocity The max velocity for the trajectory.
|
||||
* @param maxAcceleration The max acceleration for the trajectory.
|
||||
* @param reversed Whether the robot should move backwards. Note that the
|
||||
* robot will still move from a -> b -> ... -> z as defined in the waypoints.
|
||||
*
|
||||
* @return The trajectory.
|
||||
* @param initial The initial control vector.
|
||||
* @param interiorWaypoints The interior waypoints.
|
||||
* @param end The ending control vector.
|
||||
* @param config The configuration for the trajectory.
|
||||
* @return The generated trajectory.
|
||||
*/
|
||||
static Trajectory GenerateTrajectory(
|
||||
const std::vector<Pose2d>& waypoints,
|
||||
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 = false);
|
||||
Spline<3>::ControlVector initial,
|
||||
const std::vector<Translation2d>& interiorWaypoints,
|
||||
Spline<3>::ControlVector end, const TrajectoryConfig& config);
|
||||
|
||||
/**
|
||||
* Generates a trajectory with the given waypoints and constraints.
|
||||
* Generates a trajectory from the given waypoints and config. This method
|
||||
* uses clamped cubic splines -- a method in which the initial pose, final
|
||||
* pose, and interior waypoints are provided. The headings are automatically
|
||||
* determined at the interior points to ensure continuous curvature.
|
||||
*
|
||||
* @param start The starting pose for the trajectory.
|
||||
* @param waypoints The interior waypoints for the trajectory. The headings
|
||||
* will be determined automatically to ensure continuous curvature.
|
||||
* @param end The ending pose for the trajectory.
|
||||
* @param constraints A vector of various velocity and acceleration
|
||||
* constraints.
|
||||
* @param startVelocity The start velocity for the trajectory.
|
||||
* @param endVelocity The end velocity for the trajectory.
|
||||
* @param maxVelocity The max velocity for the trajectory.
|
||||
* @param maxAcceleration The max acceleration for the trajectory.
|
||||
* @param reversed Whether the robot should move backwards. Note that the
|
||||
* robot will still move from a -> b -> ... -> z as defined in the waypoints.
|
||||
*
|
||||
* @return The trajectory.
|
||||
* @param start The starting pose.
|
||||
* @param interiorWaypoints The interior waypoints.
|
||||
* @param end The ending pose.
|
||||
* @param config The configuration for the trajectory.
|
||||
* @return The generated trajectory.
|
||||
*/
|
||||
static Trajectory GenerateTrajectory(
|
||||
const Pose2d& start, const std::vector<Translation2d>& waypoints,
|
||||
const Pose2d& end,
|
||||
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 = false);
|
||||
const Pose2d& start, const std::vector<Translation2d>& interiorWaypoints,
|
||||
const Pose2d& end, const TrajectoryConfig& config);
|
||||
|
||||
/**
|
||||
* Generates a trajectory with the given waypoints and differential drive
|
||||
* constraints. Use this method if you just want a constraint such that none
|
||||
* of the wheels on your differential drive exceed the specified max velocity.
|
||||
* If you desire to impose more constraints, please use the other overloads.
|
||||
* Generates a trajectory from the given quintic control vectors and config.
|
||||
* This method uses quintic hermite splines -- therefore, all points must be
|
||||
* represented by control vectors. Continuous curvature is guaranteed in this
|
||||
* method.
|
||||
*
|
||||
* @param waypoints A vector of points that the trajectory must go through.
|
||||
* @param differentialDriveKinematics The DifferentialDriveKinematics
|
||||
* object that represents your drivetrain.
|
||||
* @param startVelocity The start velocity for the trajectory.
|
||||
* @param endVelocity The end velocity for the trajectory.
|
||||
* @param maxVelocity The max velocity for the trajectory.
|
||||
* @param maxAcceleration The max acceleration for the trajectory.
|
||||
* @param reversed Whether the robot should move backwards. Note that the
|
||||
* robot will still move from a -> b -> ... -> z as defined in the waypoints.
|
||||
*
|
||||
* @return The trajectory.
|
||||
* @param controlVectors List of quintic control vectors.
|
||||
* @param config The configuration for the trajectory.
|
||||
* @return The generated trajectory.
|
||||
*/
|
||||
static Trajectory GenerateTrajectory(
|
||||
const std::vector<Pose2d>& waypoints,
|
||||
const DifferentialDriveKinematics& differentialDriveKinematics,
|
||||
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 = false);
|
||||
std::vector<Spline<5>::ControlVector> controlVectors,
|
||||
const TrajectoryConfig& config);
|
||||
|
||||
/**
|
||||
* Generates a trajectory with the given waypoints and differential drive
|
||||
* constraints. Use this method if you just want a constraint such that none
|
||||
* of the wheels on your differential drive exceed the specified max velocity.
|
||||
* If you desire to impose more constraints, please use the other overloads.
|
||||
* Generates a trajectory from the given waypoints and config. This method
|
||||
* uses quintic hermite splines -- therefore, all points must be represented
|
||||
* by Pose2d objects. Continuous curvature is guaranteed in this method.
|
||||
*
|
||||
* @param start The starting pose for the trajectory.
|
||||
* @param waypoints The interior waypoints for the trajectory. The headings
|
||||
* will be determined automatically to ensure continuous curvature.
|
||||
* @param end The ending pose for the trajectory.
|
||||
* @param differentialDriveKinematics The DifferentialDriveKinematics
|
||||
* object that represents your drivetrain.
|
||||
* @param startVelocity The start velocity for the trajectory.
|
||||
* @param endVelocity The end velocity for the trajectory.
|
||||
* @param maxVelocity The max velocity for the trajectory.
|
||||
* @param maxAcceleration The max acceleration for the trajectory.
|
||||
* @param reversed Whether the robot should move backwards. Note that the
|
||||
* robot will still move from a -> b -> ... -> z as defined in the waypoints.
|
||||
*
|
||||
* @return The trajectory.
|
||||
* @param waypoints List of waypoints..
|
||||
* @param config The configuration for the trajectory.
|
||||
* @return The generated trajectory.
|
||||
*/
|
||||
static Trajectory GenerateTrajectory(
|
||||
const Pose2d& start, const std::vector<Translation2d>& waypoints,
|
||||
const Pose2d& end,
|
||||
const DifferentialDriveKinematics& differentialDriveKinematics,
|
||||
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 = false);
|
||||
static Trajectory GenerateTrajectory(const std::vector<Pose2d>& waypoints,
|
||||
const TrajectoryConfig& config);
|
||||
|
||||
/**
|
||||
* Generate spline points from a vector of splines by parameterizing the
|
||||
|
||||
@@ -67,7 +67,7 @@ class TrajectoryParameterizer {
|
||||
*/
|
||||
static Trajectory TimeParameterizeTrajectory(
|
||||
const std::vector<PoseWithCurvature>& points,
|
||||
std::vector<std::unique_ptr<TrajectoryConstraint>>&& constraints,
|
||||
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,
|
||||
|
||||
Reference in New Issue
Block a user