001// Copyright (c) FIRST and other WPILib contributors.
002// Open Source Software; you can modify and/or share it under the terms of
003// the WPILib BSD license file in the root directory of this project.
004
005package edu.wpi.first.math.trajectory.constraint;
006
007import edu.wpi.first.math.geometry.Pose2d;
008import edu.wpi.first.math.kinematics.ChassisSpeeds;
009import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
010
011/**
012 * A class that enforces constraints on the differential drive kinematics. This can be used to
013 * ensure that the trajectory is constructed so that the commanded velocities for both sides of the
014 * drivetrain stay below a certain limit.
015 */
016public class DifferentialDriveKinematicsConstraint implements TrajectoryConstraint {
017  private final double m_maxSpeed;
018  private final DifferentialDriveKinematics m_kinematics;
019
020  /**
021   * Constructs a differential drive dynamics constraint.
022   *
023   * @param kinematics A kinematics component describing the drive geometry.
024   * @param maxSpeed The max speed that a side of the robot can travel at in m/s.
025   */
026  public DifferentialDriveKinematicsConstraint(
027      final DifferentialDriveKinematics kinematics, double maxSpeed) {
028    m_maxSpeed = maxSpeed;
029    m_kinematics = kinematics;
030  }
031
032  /**
033   * Returns the max velocity given the current pose and curvature.
034   *
035   * @param pose The pose at the current point in the trajectory.
036   * @param curvature The curvature at the current point in the trajectory in rad/m.
037   * @param velocity The velocity at the current point in the trajectory before constraints are
038   *     applied in m/s.
039   * @return The absolute maximum velocity.
040   */
041  @Override
042  public double getMaxVelocity(Pose2d pose, double curvature, double velocity) {
043    // Create an object to represent the current chassis speeds.
044    var chassisSpeeds = new ChassisSpeeds(velocity, 0, velocity * curvature);
045
046    // Get the wheel speeds and normalize them to within the max velocity.
047    var wheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds);
048    wheelSpeeds.desaturate(m_maxSpeed);
049
050    // Return the new linear chassis speed.
051    return m_kinematics.toChassisSpeeds(wheelSpeeds).vx;
052  }
053
054  /**
055   * Returns the minimum and maximum allowable acceleration for the trajectory given pose,
056   * curvature, and speed.
057   *
058   * @param pose The pose at the current point in the trajectory.
059   * @param curvature The curvature at the current point in the trajectory in rad/m.
060   * @param velocity The speed at the current point in the trajectory in m/s.
061   * @return The min and max acceleration bounds.
062   */
063  @Override
064  public MinMax getMinMaxAcceleration(Pose2d pose, double curvature, double velocity) {
065    return new MinMax();
066  }
067}