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

Add generateTrajectory overload (#1944)

Browse Source 
Add an overload for the generateTrajectory method that accepts a DifferentialDriveKinematics instance instead of a list of constraints. This instance is used to automatically create a DifferentialDriveKinematicsConstraint behind the scenes, saving the user some verbosity.
This commit is contained in:
Prateek Machiraju
2019-10-18 11:02:42 -04:00
committed by Peter Johnson
parent 63cfa64fb3
commit c10f2003c5
3 changed files with 178 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

86
wpilibj/src/main/java/edu/wpi/first/wpilibj/trajectory/TrajectoryGenerator.java
View File

@@ -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<Pose2d> 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 -&gt; b -&gt; ... -&gt; z
* as defined in the waypoints.
* @return The trajectory.
*/
public static Trajectory generateTrajectory(
Pose2d start,
List<Translation2d> 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<PoseWithCurvature> splinePointsFromSplines(
Spline[] splines) {
// Create the vector of spline points.