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}