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_maxCentripetalAccelerationMetersPerSecondSq;
019
020  /**
021   * Constructs a centripetal acceleration constraint.
022   *
023   * @param maxCentripetalAccelerationMetersPerSecondSq The max centripetal acceleration.
024   */
025  public CentripetalAccelerationConstraint(double maxCentripetalAccelerationMetersPerSecondSq) {
026    m_maxCentripetalAccelerationMetersPerSecondSq = maxCentripetalAccelerationMetersPerSecondSq;
027  }
028
029  /**
030   * Returns the max velocity given the current pose and curvature.
031   *
032   * @param poseMeters The pose at the current point in the trajectory.
033   * @param curvatureRadPerMeter The curvature at the current point in the trajectory.
034   * @param velocityMetersPerSecond The velocity at the current point in the trajectory before
035   *     constraints are applied.
036   * @return The absolute maximum velocity.
037   */
038  @Override
039  public double getMaxVelocityMetersPerSecond(
040      Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
041    // ac = v²/r
042    // k (curvature) = 1/r
043
044    // therefore, ac = v²k
045    // ac/k = v²
046    // v = √(ac/k)
047
048    return Math.sqrt(
049        m_maxCentripetalAccelerationMetersPerSecondSq / Math.abs(curvatureRadPerMeter));
050  }
051
052  /**
053   * Returns the minimum and maximum allowable acceleration for the trajectory given pose,
054   * curvature, and speed.
055   *
056   * @param poseMeters The pose at the current point in the trajectory.
057   * @param curvatureRadPerMeter The curvature at the current point in the trajectory.
058   * @param velocityMetersPerSecond The speed at the current point in the trajectory.
059   * @return The min and max acceleration bounds.
060   */
061  @Override
062  public MinMax getMinMaxAccelerationMetersPerSecondSq(
063      Pose2d poseMeters, double curvatureRadPerMeter, double velocityMetersPerSecond) {
064    // The acceleration of the robot has no impact on the centripetal acceleration
065    // of the robot.
066    return new MinMax();
067  }
068}