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; 008 009/** 010 * A constraint on the maximum absolute centripetal acceleration allowed when traversing a 011 * trajectory. The centripetal acceleration of a robot is defined as the velocity squared divided by 012 * the radius of curvature. 013 * 014 * <p>Effectively, limiting the maximum centripetal acceleration will cause the robot to slow down 015 * around tight turns, making it easier to track trajectories with sharp turns. 016 */ 017public class CentripetalAccelerationConstraint implements TrajectoryConstraint { 018 private final double m_maxCentripetalAcceleration; 019 020 /** 021 * Constructs a centripetal acceleration constraint. 022 * 023 * @param maxCentripetalAcceleration The max centripetal acceleration in m/s². 024 */ 025 public CentripetalAccelerationConstraint(double maxCentripetalAcceleration) { 026 m_maxCentripetalAcceleration = maxCentripetalAcceleration; 027 } 028 029 /** 030 * Returns the max velocity given the current pose and curvature. 031 * 032 * @param pose The pose at the current point in the trajectory. 033 * @param curvature The curvature at the current point in the trajectory in rad/m. 034 * @param velocity The velocity at the current point in the trajectory before constraints are 035 * applied in m/s. 036 * @return The absolute maximum velocity. 037 */ 038 @Override 039 public double getMaxVelocity(Pose2d pose, double curvature, double velocity) { 040 // ac = v²/r 041 // k (curvature) = 1/r 042 043 // therefore, ac = v²k 044 // ac/k = v² 045 // v = √(ac/k) 046 047 return Math.sqrt(m_maxCentripetalAcceleration / Math.abs(curvature)); 048 } 049 050 /** 051 * Returns the minimum and maximum allowable acceleration for the trajectory given pose, 052 * curvature, and speed. 053 * 054 * @param pose The pose at the current point in the trajectory. 055 * @param curvature The curvature at the current point in the trajectory in rad/m. 056 * @param velocity The speed at the current point in the trajectory in m/s. 057 * @return The min and max acceleration bounds. 058 */ 059 @Override 060 public MinMax getMinMaxAcceleration(Pose2d pose, double curvature, double velocity) { 061 // The acceleration of the robot has no impact on the centripetal acceleration 062 // of the robot. 063 return new MinMax(); 064 } 065}