Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-24 01:31:46 +00:00
Code Issues Packages Projects Releases Wiki Activity

Refactor TrajectoryGenerator (#1972)

Browse Source
This commit is contained in:
Prateek Machiraju
2019-10-26 12:39:47 -04:00
committed by Peter Johnson
parent 73a30182c3
commit 9440edf2b5
23 changed files with 825 additions and 629 deletions
Download Patch File Download Diff File Expand all files Collapse all files

13
wpilibc/src/main/native/include/frc/spline/Spline.h
View File

@@ -7,6 +7,7 @@
#pragma once
#include <array>
#include <utility>
#include <vector>
@@ -43,6 +44,18 @@ class Spline {
virtual ~Spline() = default;
/**
* Represents a control vector for a spline.
*
* Each element in each array represents the value of the derivative at the
* index. For example, the value of x[2] is the second derivative in the x
* dimension.
*/
struct ControlVector {
std::array<double, (Degree + 1) / 2> x;
std::array<double, (Degree + 1) / 2> y;
};
/**
* Gets the pose and curvature at some point t on the spline.
*

74
wpilibc/src/main/native/include/frc/spline/SplineHelper.h
View File

@@ -7,6 +7,8 @@
#pragma once
#include <array>
#include <utility>
#include <vector>
#include "frc/spline/CubicHermiteSpline.h"
@@ -20,39 +22,79 @@ namespace frc {
class SplineHelper {
public:
/**
* Returns a set of cubic splines corresponding to the provided waypoints. The
* user is free to set the direction of the start and end point. The
* directions for the middle waypoints are determined automatically to ensure
* continuous curvature throughout the path.
* Returns 2 cubic control vectors from a set of exterior waypoints and
* interior translations.
*
* @param start The starting pose.
* @param interiorWaypoints The interior waypoints.
* @param end The ending pose.
* @return 2 cubic control vectors.
*/
static std::array<Spline<3>::ControlVector, 2>
CubicControlVectorsFromWaypoints(
const Pose2d& start, const std::vector<Translation2d>& interiorWaypoints,
const Pose2d& end);
/**
* Returns quintic control vectors from a set of waypoints.
*
* @param waypoints The waypoints
* @return List of control vectors
*/
static std::vector<Spline<5>::ControlVector>
QuinticControlVectorsFromWaypoints(const std::vector<Pose2d>& waypoints);
/**
* Returns a set of cubic splines corresponding to the provided control
* vectors. The user is free to set the direction of the start and end
* point. The directions for the middle waypoints are determined
* automatically to ensure continuous curvature throughout the path.
*
* The derivation for the algorithm used can be found here:
* <https://www.uio.no/studier/emner/matnat/ifi/nedlagte-emner/INF-MAT4350/h08/undervisningsmateriale/chap7alecture.pdf>
*
* @param start The starting waypoint.
* @param waypoints The middle waypoints. This can be left blank if you only
* wish to create a path with two waypoints.
* @param end The ending waypoint.
* @param start The starting control vector.
* @param waypoints The middle waypoints. This can be left blank if you
* only wish to create a path with two waypoints.
* @param end The ending control vector.
*
* @return A vector of cubic hermite splines that interpolate through the
* provided waypoints.
*/
static std::vector<CubicHermiteSpline> CubicSplinesFromWaypoints(
const Pose2d& start, std::vector<Translation2d> waypoints,
const Pose2d& end);
static std::vector<CubicHermiteSpline> CubicSplinesFromControlVectors(
const Spline<3>::ControlVector& start,
std::vector<Translation2d> waypoints,
const Spline<3>::ControlVector& end);
/**
* Returns a set of quintic splines corresponding to the provided waypoints.
* The user is free to set the direction of all waypoints. Continuous
* Returns a set of quintic splines corresponding to the provided control
* vectors. The user is free to set the direction of all waypoints. Continuous
* curvature is guaranteed throughout the path.
*
* @param waypoints The waypoints.
* @param controlVectors The control vectors.
* @return A vector of quintic hermite splines that interpolate through the
* provided waypoints.
*/
static std::vector<QuinticHermiteSpline> QuinticSplinesFromWaypoints(
const std::vector<Pose2d>& waypoints);
static std::vector<QuinticHermiteSpline> QuinticSplinesFromControlVectors(
const std::vector<Spline<5>::ControlVector>& controlVectors);
private:
static Spline<3>::ControlVector CubicControlVector(double scalar,
const Pose2d& point) {
return {
{point.Translation().X().to<double>(), scalar * point.Rotation().Cos()},
{point.Translation().Y().to<double>(),
scalar * point.Rotation().Sin()}};
}
static Spline<5>::ControlVector QuinticControlVector(double scalar,
const Pose2d& point) {
return {{point.Translation().X().to<double>(),
scalar * point.Rotation().Cos(), 0.0},
{point.Translation().Y().to<double>(),
scalar * point.Rotation().Sin(), 0.0}};
}
/**
* Thomas algorithm for solving tridiagonal systems Af = d.
*

140
wpilibc/src/main/native/include/frc/trajectory/TrajectoryConfig.h Normal file
View File

@@ -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

132
wpilibc/src/main/native/include/frc/trajectory/TrajectoryGenerator.h
View File

@@ -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

2
wpilibc/src/main/native/include/frc/trajectory/TrajectoryParameterizer.h
View File

@@ -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,