diff --git a/wpilibc/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp b/wpilibc/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp index 35585f6042..ed97a904f1 100644 --- a/wpilibc/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp +++ b/wpilibc/src/main/native/cpp/trajectory/TrajectoryGenerator.cpp @@ -75,3 +75,38 @@ Trajectory TrajectoryGenerator::GenerateTrajectory( points, std::move(constraints), startVelocity, endVelocity, maxVelocity, maxAcceleration, reversed); } + +Trajectory TrajectoryGenerator::GenerateTrajectory( + const std::vector& 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) { + std::vector> constraints; + constraints.emplace_back( + std::make_unique( + differentialDriveKinematics, maxVelocity)); + + return GenerateTrajectory(waypoints, std::move(constraints), startVelocity, + endVelocity, maxVelocity, maxAcceleration, + reversed); +} + +Trajectory TrajectoryGenerator::GenerateTrajectory( + const Pose2d& start, const std::vector& 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) { + std::vector> constraints; + constraints.emplace_back( + std::make_unique( + differentialDriveKinematics, maxVelocity)); + + return GenerateTrajectory(start, waypoints, end, std::move(constraints), + startVelocity, endVelocity, maxVelocity, + maxAcceleration, reversed); +} diff --git a/wpilibc/src/main/native/include/frc/trajectory/TrajectoryGenerator.h b/wpilibc/src/main/native/include/frc/trajectory/TrajectoryGenerator.h index b39f6d392f..061b3738fd 100644 --- a/wpilibc/src/main/native/include/frc/trajectory/TrajectoryGenerator.h +++ b/wpilibc/src/main/native/include/frc/trajectory/TrajectoryGenerator.h @@ -13,6 +13,7 @@ #include "frc/spline/SplineParameterizer.h" #include "frc/trajectory/Trajectory.h" +#include "frc/trajectory/constraint/DifferentialDriveKinematicsConstraint.h" #include "frc/trajectory/constraint/TrajectoryConstraint.h" namespace frc { @@ -73,6 +74,62 @@ class TrajectoryGenerator { units::meters_per_second_t maxVelocity, units::meters_per_second_squared_t maxAcceleration, bool reversed); + /** + * 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. + * + * @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. + */ + static Trajectory GenerateTrajectory( + const std::vector& 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); + + /** + * 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. + * + * @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. + */ + static Trajectory GenerateTrajectory( + const Pose2d& start, const std::vector& 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); + private: /** * Generate spline points from a vector of splines by parameterizing the diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGenerator.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGenerator.java index 2ec0fe4b90..364fa5f660 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGenerator.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGenerator.java @@ -14,10 +14,12 @@ import edu.wpi.first.wpilibj.geometry.Pose2d; import edu.wpi.first.wpilibj.geometry.Rotation2d; import edu.wpi.first.wpilibj.geometry.Transform2d; import edu.wpi.first.wpilibj.geometry.Translation2d; +import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics; import edu.wpi.first.wpilibj.spline.PoseWithCurvature; import edu.wpi.first.wpilibj.spline.Spline; import edu.wpi.first.wpilibj.spline.SplineHelper; import edu.wpi.first.wpilibj.spline.SplineParameterizer; +import edu.wpi.first.wpilibj.trajectory.constraint.DifferentialDriveKinematicsConstraint; import edu.wpi.first.wpilibj.trajectory.constraint.TrajectoryConstraint; public final class TrajectoryGenerator { @@ -131,6 +133,90 @@ public final class TrajectoryGenerator { maxAccelerationMetersPerSecondSq, reversed); } + /** + * 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. + * + * @param waypoints A vector of points that the trajectory must go through. + * @param differentialDriveKinematics The DifferentialDriveKinematics object that represents + * your drivetrain. + * @param startVelocityMetersPerSecond The start velocity for the trajectory. + * @param endVelocityMetersPerSecond The end velocity for the trajectory. + * @param maxVelocityMetersPerSecond The max velocity for the trajectory. + * @param maxAccelerationMetersPerSecondSq 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. + */ + public static Trajectory generateTrajectory( + List waypoints, + DifferentialDriveKinematics differentialDriveKinematics, + double startVelocityMetersPerSecond, + double endVelocityMetersPerSecond, + double maxVelocityMetersPerSecond, + double maxAccelerationMetersPerSecondSq, + boolean reversed + ) { + return generateTrajectory( + waypoints, + List.of(new DifferentialDriveKinematicsConstraint(differentialDriveKinematics, + maxVelocityMetersPerSecond)), + startVelocityMetersPerSecond, + endVelocityMetersPerSecond, + maxVelocityMetersPerSecond, + maxAccelerationMetersPerSecondSq, + reversed + ); + } + + /** + * 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. + * + * @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 startVelocityMetersPerSecond The start velocity for the trajectory. + * @param endVelocityMetersPerSecond The end velocity for the trajectory. + * @param maxVelocityMetersPerSecond The max velocity for the trajectory. + * @param maxAccelerationMetersPerSecondSq 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. + */ + public static Trajectory generateTrajectory( + Pose2d start, + List waypoints, + Pose2d end, + DifferentialDriveKinematics differentialDriveKinematics, + double startVelocityMetersPerSecond, + double endVelocityMetersPerSecond, + double maxVelocityMetersPerSecond, + double maxAccelerationMetersPerSecondSq, + boolean reversed + ) { + return generateTrajectory( + start, waypoints, end, + List.of(new DifferentialDriveKinematicsConstraint(differentialDriveKinematics, + maxVelocityMetersPerSecond)), + startVelocityMetersPerSecond, + endVelocityMetersPerSecond, + maxVelocityMetersPerSecond, + maxAccelerationMetersPerSecondSq, + reversed + ); + } + private static List splinePointsFromSplines( Spline[] splines) { // Create the vector of spline points.