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.MecanumDriveKinematics; 010 011/** 012 * A class that enforces constraints on the mecanum drive kinematics. This can be used to ensure 013 * that the trajectory is constructed so that the commanded velocities for all 4 wheels of the 014 * drivetrain stay below a certain limit. 015 */ 016public class MecanumDriveKinematicsConstraint implements TrajectoryConstraint { 017 private final double m_maxSpeed; 018 private final MecanumDriveKinematics m_kinematics; 019 020 /** 021 * Constructs a mecanum drive kinematics constraint. 022 * 023 * @param kinematics Mecanum drive kinematics. 024 * @param maxSpeed The max speed that a side of the robot can travel at in m/s. 025 */ 026 public MecanumDriveKinematicsConstraint( 027 final MecanumDriveKinematics 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 // Represents the velocity of the chassis in the x direction 044 var xdVelocity = velocity * pose.getRotation().getCos(); 045 046 // Represents the velocity of the chassis in the y direction 047 var ydVelocity = velocity * pose.getRotation().getSin(); 048 049 // Create an object to represent the current chassis speeds. 050 var chassisSpeeds = new ChassisSpeeds(xdVelocity, ydVelocity, velocity * curvature); 051 052 // Get the wheel speeds and normalize them to within the max velocity. 053 var wheelSpeeds = m_kinematics.toWheelSpeeds(chassisSpeeds).desaturate(m_maxSpeed); 054 055 // Convert normalized wheel speeds back to chassis speeds 056 var normSpeeds = m_kinematics.toChassisSpeeds(wheelSpeeds); 057 058 // Return the new linear chassis speed. 059 return Math.hypot(normSpeeds.vx, normSpeeds.vy); 060 } 061 062 /** 063 * Returns the minimum and maximum allowable acceleration for the trajectory given pose, 064 * curvature, and speed. 065 * 066 * @param pose The pose at the current point in the trajectory. 067 * @param curvature The curvature at the current point in the trajectory in rad/m. 068 * @param velocity The speed at the current point in the trajectory in m/s. 069 * @return The min and max acceleration bounds. 070 */ 071 @Override 072 public MinMax getMinMaxAcceleration(Pose2d pose, double curvature, double velocity) { 073 return new MinMax(); 074 } 075}