```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.controller;
006
007import edu.wpi.first.math.geometry.Pose2d;
008import edu.wpi.first.math.kinematics.ChassisSpeeds;
009import edu.wpi.first.math.trajectory.Trajectory;
010
011/**
012 * Ramsete is a nonlinear time-varying feedback controller for unicycle models that drives the model
013 * to a desired pose along a two-dimensional trajectory. Why would we need a nonlinear control law
014 * in addition to the linear ones we have used so far like PID? If we use the original approach with
015 * PID controllers for left and right position and velocity states, the controllers only deal with
016 * the local pose. If the robot deviates from the path, there is no way for the controllers to
017 * correct and the robot may not reach the desired global pose. This is due to multiple endpoints
018 * existing for the robot which have the same encoder path arc lengths.
019 *
020 * <p>Instead of using wheel path arc lengths (which are in the robot's local coordinate frame),
021 * nonlinear controllers like pure pursuit and Ramsete use global pose. The controller uses this
022 * extra information to guide a linear reference tracker like the PID controllers back in by
023 * adjusting the references of the PID controllers.
024 *
025 * <p>The paper "Control of Wheeled Mobile Robots: An Experimental Overview" describes a nonlinear
026 * controller for a wheeled vehicle with unicycle-like kinematics; a global pose consisting of x, y,
027 * and theta; and a desired pose consisting of x_d, y_d, and theta_d. We call it Ramsete because
028 * that's the acronym for the title of the book it came from in Italian ("Robotica Articolata e
029 * Mobile per i SErvizi e le TEcnologie").
030 *
031 * <p>See <a href="https://file.tavsys.net/control/controls-engineering-in-frc.pdf">Controls
032 * Engineering in the FIRST Robotics Competition</a> section on Ramsete unicycle controller for a
033 * derivation and analysis.
034 */
035public class RamseteController {
036  private final double m_b;
037
038  private final double m_zeta;
039
040  private Pose2d m_poseError = new Pose2d();
041  private Pose2d m_poseTolerance = new Pose2d();
042  private boolean m_enabled = true;
043
044  /**
045   * Construct a Ramsete unicycle controller.
046   *
047   * @param b Tuning parameter (b &gt; 0 rad²/m²) for which larger values make convergence more
048   *     aggressive like a proportional term.
049   * @param zeta Tuning parameter (0 rad⁻¹ &lt; zeta &lt; 1 rad⁻¹) for which larger values provide
050   *     more damping in response.
051   */
052  public RamseteController(double b, double zeta) {
053    m_b = b;
054    m_zeta = zeta;
055  }
056
057  /**
058   * Construct a Ramsete unicycle controller. The default arguments for b and zeta of 2.0 rad²/m²
059   * and 0.7 rad⁻¹ have been well-tested to produce desirable results.
060   */
061  public RamseteController() {
062    this(2.0, 0.7);
063  }
064
065  /**
066   * Returns true if the pose error is within tolerance of the reference.
067   *
068   * @return True if the pose error is within tolerance of the reference.
069   */
070  public boolean atReference() {
071    final var eTranslate = m_poseError.getTranslation();
072    final var eRotate = m_poseError.getRotation();
073    final var tolTranslate = m_poseTolerance.getTranslation();
074    final var tolRotate = m_poseTolerance.getRotation();
075    return Math.abs(eTranslate.getX()) < tolTranslate.getX()
076        && Math.abs(eTranslate.getY()) < tolTranslate.getY()
078  }
079
080  /**
081   * Sets the pose error which is considered tolerable for use with atReference().
082   *
083   * @param poseTolerance Pose error which is tolerable.
084   */
085  public void setTolerance(Pose2d poseTolerance) {
086    m_poseTolerance = poseTolerance;
087  }
088
089  /**
090   * Returns the next output of the Ramsete controller.
091   *
092   * <p>The reference pose, linear velocity, and angular velocity should come from a drivetrain
093   * trajectory.
094   *
095   * @param currentPose The current pose.
096   * @param poseRef The desired pose.
097   * @param linearVelocityRefMeters The desired linear velocity in meters per second.
098   * @param angularVelocityRefRadiansPerSecond The desired angular velocity in radians per second.
099   * @return The next controller output.
100   */
101  public ChassisSpeeds calculate(
102      Pose2d currentPose,
103      Pose2d poseRef,
104      double linearVelocityRefMeters,
106    if (!m_enabled) {
107      return new ChassisSpeeds(linearVelocityRefMeters, 0.0, angularVelocityRefRadiansPerSecond);
108    }
109
110    m_poseError = poseRef.relativeTo(currentPose);
111
112    // Aliases for equation readability
113    final double eX = m_poseError.getX();
114    final double eY = m_poseError.getY();
115    final double eTheta = m_poseError.getRotation().getRadians();
116    final double vRef = linearVelocityRefMeters;
117    final double omegaRef = angularVelocityRefRadiansPerSecond;
118
119    // k = 2ζ√(ω_ref² + b v_ref²)
120    double k = 2.0 * m_zeta * Math.sqrt(Math.pow(omegaRef, 2) + m_b * Math.pow(vRef, 2));
121
122    // v_cmd = v_ref cos(e_θ) + k e_x
123    // ω_cmd = ω_ref + k e_θ + b v_ref sinc(e_θ) e_y
124    return new ChassisSpeeds(
125        vRef * m_poseError.getRotation().getCos() + k * eX,
126        0.0,
127        omegaRef + k * eTheta + m_b * vRef * sinc(eTheta) * eY);
128  }
129
130  /**
131   * Returns the next output of the Ramsete controller.
132   *
133   * <p>The reference pose, linear velocity, and angular velocity should come from a drivetrain
134   * trajectory.
135   *
136   * @param currentPose The current pose.
137   * @param desiredState The desired pose, linear velocity, and angular velocity from a trajectory.
138   * @return The next controller output.
139   */
140  public ChassisSpeeds calculate(Pose2d currentPose, Trajectory.State desiredState) {
141    return calculate(
142        currentPose,
143        desiredState.poseMeters,
144        desiredState.velocityMetersPerSecond,
146  }
147
148  /**
149   * Enables and disables the controller for troubleshooting purposes.
150   *
151   * @param enabled If the controller is enabled or not.
152   */
153  public void setEnabled(boolean enabled) {
154    m_enabled = enabled;
155  }
156
157  /**
158   * Returns sin(x) / x.
159   *
160   * @param x Value of which to take sinc(x).
161   */
162  private static double sinc(double x) {
163    if (Math.abs(x) < 1e-9) {
164      return 1.0 - 1.0 / 6.0 * x * x;
165    } else {
166      return Math.sin(x) / x;
167    }
168  }
169}

```