Skip to content

Commit e7143e7

Browse files
committed
Restore ramsete doc, add deprecation warning
1 parent fc4a1c9 commit e7143e7

File tree

3 files changed

+99
-1
lines changed

3 files changed

+99
-1
lines changed

source/docs/software/advanced-controls/trajectories/index.rst

+1
Original file line numberDiff line numberDiff line change
@@ -12,5 +12,6 @@ This section describes WPILib support for generating parameterized spline trajec
1212
manipulating-trajectories
1313
transforming-trajectories
1414
ltv
15+
ramsete
1516
holonomic
1617
troubleshooting
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,98 @@
1+
# Ramsete Controller
2+
3+
.. warning:: Ramsete Controller has been deprecated. Use :doc:`LTV Unicycle Controller <ltv>` which has more intuitive tuning.
4+
5+
The Ramsete Controller is a trajectory tracker that is built in to WPILib. This tracker can be used to accurately track trajectories with correction for minor disturbances.
6+
7+
## Constructing the Ramsete Controller Object
8+
The Ramsete controller should be initialized with two gains, namely ``b`` and ``zeta``. Larger values of ``b`` make convergence more aggressive like a proportional term whereas larger values of ``zeta`` provide more damping in the response. These controller gains only dictate how the controller will output adjusted velocities. It does NOT affect the actual velocity tracking of the robot. This means that these controller gains are generally robot-agnostic.
9+
10+
.. note:: Gains of ``2.0`` and ``0.7`` for ``b`` and ``zeta`` have been tested repeatedly to produce desirable results when all units were in meters. As such, a zero-argument constructor for ``RamseteController`` exists with gains defaulted to these values.
11+
12+
.. tab-set-code::
13+
```java
14+
// Using the default constructor of RamseteController. Here
15+
// the gains are initialized to 2.0 and 0.7.
16+
RamseteController controller1 = new RamseteController();
17+
// Using the secondary constructor of RamseteController where
18+
// the user can choose any other gains.
19+
RamseteController controller2 = new RamseteController(2.1, 0.8);
20+
```
21+
22+
```c++
23+
// Using the default constructor of RamseteController. Here
24+
// the gains are initialized to 2.0 and 0.7.
25+
frc::RamseteController controller1;
26+
// Using the secondary constructor of RamseteController where
27+
// the user can choose any other gains.
28+
frc::RamseteController controller2{2.1, 0.8};
29+
```
30+
31+
```python
32+
from wpimath.controller import RamseteController
33+
# Using the default constructor of RamseteController. Here
34+
# the gains are initialized to 2.0 and 0.7.
35+
controller1 = RamseteController()
36+
# Using the secondary constructor of RamseteController where
37+
# the user can choose any other gains.
38+
controller2 = RamseteController(2.1, 0.8)
39+
```
40+
41+
## Getting Adjusted Velocities
42+
The Ramsete controller returns "adjusted velocities" so that the when the robot tracks these velocities, it accurately reaches the goal point. The controller should be updated periodically with the new goal. The goal comprises of a desired pose, desired linear velocity, and desired angular velocity. Furthermore, the current position of the robot should also be updated periodically. The controller uses these four arguments to return the adjusted linear and angular velocity. Users should command their robot to these linear and angular velocities to achieve optimal trajectory tracking.
43+
44+
.. note:: The "goal pose" represents the position that the robot should be at a particular timestep when tracking the trajectory. It does NOT represent the final endpoint of the trajectory.
45+
46+
The controller can be updated using the ``Calculate`` (C++) / ``calculate`` (Java/Python) method. There are two overloads for this method. Both of these overloads accept the current robot position as the first parameter. For the other parameters, one of these overloads takes in the goal as three separate parameters (pose, linear velocity, and angular velocity) whereas the other overload accepts a ``Trajectory.State`` object, which contains information about the goal pose. For its ease, users should use the latter method when tracking trajectories.
47+
48+
.. tab-set-code::
49+
50+
```java
51+
Trajectory.State goal = trajectory.sample(3.4); // sample the trajectory at 3.4 seconds from the beginning
52+
ChassisSpeeds adjustedSpeeds = controller.calculate(currentRobotPose, goal);
53+
```
54+
55+
```c++
56+
const Trajectory::State goal = trajectory.Sample(3.4_s); // sample the trajectory at 3.4 seconds from the beginning
57+
ChassisSpeeds adjustedSpeeds = controller.Calculate(currentRobotPose, goal);
58+
```
59+
60+
```python
61+
goal = trajectory.sample(3.4) # sample the trajectory at 3.4 seconds from the beginning
62+
adjustedSpeeds = controller.calculate(currentRobotPose, goal)
63+
```
64+
65+
These calculations should be performed at every loop iteration, with an updated robot position and goal.
66+
67+
## Using the Adjusted Velocities
68+
The adjusted velocities are of type ``ChassisSpeeds``, which contains a ``vx`` (linear velocity in the forward direction), a ``vy`` (linear velocity in the sideways direction), and an ``omega`` (angular velocity around the center of the robot frame). Because the Ramsete controller is a controller for non-holonomic robots (robots which cannot move sideways), the adjusted speeds object has a ``vy`` of zero.
69+
70+
The returned adjusted speeds can be converted to usable speeds using the kinematics classes for your drivetrain type. For example, the adjusted velocities can be converted to left and right velocities for a differential drive using a ``DifferentialDriveKinematics`` object.
71+
72+
.. tab-set-code::
73+
74+
```java
75+
ChassisSpeeds adjustedSpeeds = controller.calculate(currentRobotPose, goal);
76+
DifferentialDriveWheelSpeeds wheelSpeeds = kinematics.toWheelSpeeds(adjustedSpeeds);
77+
double left = wheelSpeeds.leftMetersPerSecond;
78+
double right = wheelSpeeds.rightMetersPerSecond;
79+
```
80+
81+
```c++
82+
ChassisSpeeds adjustedSpeeds = controller.Calculate(currentRobotPose, goal);
83+
DifferentialDriveWheelSpeeds wheelSpeeds = kinematics.ToWheelSpeeds(adjustedSpeeds);
84+
auto [left, right] = kinematics.ToWheelSpeeds(adjustedSpeeds);
85+
```
86+
87+
```python
88+
adjustedSpeeds = controller.calculate(currentRobotPose, goal)
89+
wheelSpeeds = kinematics.toWheelSpeeds(adjustedSpeeds)
90+
left = wheelSpeeds.left
91+
right = wheelSpeeds.right
92+
```
93+
94+
Because these new left and right velocities are still speeds and not voltages, two PID Controllers, one for each side may be used to track these velocities. Either the WPILib PIDController ([C++](https://github.wpilib.org/allwpilib/docs/release/cpp/classfrc_1_1_p_i_d_controller.html), [Java](https://github.wpilib.org/allwpilib/docs/release/java/edu/wpi/first/math/controller/PIDController.html), :external:py:class:`Python <wpimath.controller.PIDController>`) can be used, or the Velocity PID feature on smart motor controllers such as the TalonSRX and the SPARK MAX can be used.
95+
96+
## Ramsete in the Command-Based Framework
97+
For the sake of ease for users, a ``RamseteCommand`` class is built in to WPILib. For a full tutorial on implementing a path-following autonomous using RamseteCommand, see :ref:`docs/software/pathplanning/trajectory-tutorial/index:Trajectory Tutorial`.
98+

source/redirects.txt

-1
Original file line numberDiff line numberDiff line change
@@ -309,4 +309,3 @@
309309
"docs/software/wpilib-tools/choreo/index.rst" "docs/software/pathplanning/choreo/index.rst"
310310
"docs/networking/networking-introduction/om5p-ac-radio-modification.rst" "docs/zero-to-robot/step-3/radio-programming.rst"
311311
"docs/software/driverstation/programming-radios-for-fms-offseason.rst" "docs/zero-to-robot/step-3/radio-programming.rst"
312-
"docs/software/advanced-controls/trajectories/ramsete.rst" "docs/software/advanced-controls/trajectories/ltv.rst"

0 commit comments

Comments
 (0)