diff --git a/controls-engineering-in-frc.bib b/controls-engineering-in-frc.bib index 61e5a75f..103d05d3 100644 --- a/controls-engineering-in-frc.bib +++ b/controls-engineering-in-frc.bib @@ -21,13 +21,6 @@ @online{bib:banana_distribution year = {2008} } -@online{bib:ctrl_wheeled_mobile_robots, - author = {Luca and Oriolo and Vendittelli}, - title = {Control of Wheeled Mobile Robots: An Experimental Overview}, - url = {https://web.archive.org/web/20240709180729/https://www.diag.uniroma1.it/~labrob/pub/papers/RAMSETE_Chap_LNCIS270.pdf}, - urldate = {2024-07-10} -} - @online{bib:first-hand_the_ut, author = {Jeffrey Uhlmann}, title = {First-Hand:The Unscented Transform}, diff --git a/estimation-and-localization/pose-estimation.tex b/estimation-and-localization/pose-estimation.tex index c9a90f19..6b5b48a9 100644 --- a/estimation-and-localization/pose-estimation.tex +++ b/estimation-and-localization/pose-estimation.tex @@ -1,7 +1,6 @@ \chapterimage{pose-estimation.jpg}{Road next to Stevenson Academic building at UCSC} \chapter{Pose estimation} -\label{ch:pose_estimation} Pose is defined as the position and orientation of an \gls{agent} (a system with a controller). The plant usually includes the pose in its state vector. diff --git a/modern-control-theory/nonlinear-control.tex b/modern-control-theory/nonlinear-control.tex index f0882c00..85d0aa5c 100644 --- a/modern-control-theory/nonlinear-control.tex +++ b/modern-control-theory/nonlinear-control.tex @@ -15,6 +15,5 @@ \chapter{Nonlinear control} \input{\chapterpath/pendulum} \input{\chapterpath/holonomic-drivetrains} \input{\chapterpath/differential-drive} -\input{\chapterpath/ramsete-unicycle-controller} \input{\chapterpath/ltv-unicycle-controller} \input{\chapterpath/further-reading} diff --git a/modern-control-theory/nonlinear-control/differential-drive.tex b/modern-control-theory/nonlinear-control/differential-drive.tex index 15b0ca59..42dd9af3 100644 --- a/modern-control-theory/nonlinear-control/differential-drive.tex +++ b/modern-control-theory/nonlinear-control/differential-drive.tex @@ -75,7 +75,6 @@ \subsection{Velocity subspace state-space model} \end{bmatrix} \end{align*} \begin{theorem}[Differential drive velocity state-space model] - \label{thm:diff_drive_velocity_state-space_model} \begin{equation*} \dot{\mat{x}} = \mat{A} \mat{x} + \mat{B} \mat{u} \end{equation*} diff --git a/modern-control-theory/nonlinear-control/ltv-unicycle-controller.tex b/modern-control-theory/nonlinear-control/ltv-unicycle-controller.tex index c9c22f3b..b1e7b136 100644 --- a/modern-control-theory/nonlinear-control/ltv-unicycle-controller.tex +++ b/modern-control-theory/nonlinear-control/ltv-unicycle-controller.tex @@ -1,8 +1,9 @@ \section{Linear time-varying unicycle controller} One can also create a linear time-varying controller with a cascaded control -architecture like Ramsete. This section will derive a locally optimal -replacement for Ramsete. +architecture, where the outer layer consumes a pose command and produces +unicycle velocity commands, and the inner layer consumes unicycle velocity +commands and produces wheel motor voltages. The change in global pose for a unicycle is defined by the following three equations. diff --git a/modern-control-theory/nonlinear-control/ramsete-unicycle-controller.tex b/modern-control-theory/nonlinear-control/ramsete-unicycle-controller.tex deleted file mode 100644 index 12ae145c..00000000 --- a/modern-control-theory/nonlinear-control/ramsete-unicycle-controller.tex +++ /dev/null @@ -1,292 +0,0 @@ -\section{Ramsete unicycle controller} - -Ramsete is a nonlinear time-varying feedback controller for unicycle -\glspl{model} that drives the \gls{model} to a desired \gls{pose} along a -two-dimensional trajectory. Why would we need a nonlinear control law in -addition to the linear ones we have used so far? If we use the original approach -with an LQR \gls{controller} for left and right position and velocity -\glspl{state}, the \gls{controller} only deals with the local \gls{pose}. If the -robot deviates from the path, there is no way for the \gls{controller} to -correct and the robot may not reach the desired global \gls{pose}. This is due -to multiple endpoints existing for the robot which have the same encoder path -arc lengths. - -Instead of using wheel path arc lengths (which are in the robot's local -coordinate frame), nonlinear controllers like pure pursuit and Ramsete use -global pose. The \gls{controller} uses this extra information to guide a linear -\glslink{tracking}{reference tracker} like an LQR \gls{controller} back in by -adjusting the \glspl{reference} of the LQR \gls{controller}. - -The paper \textit{Control of Wheeled Mobile Robots: An Experimental Overview} -describes a nonlinear controller for a wheeled vehicle with unicycle-like -kinematics; a global \gls{pose} consisting of $x$, $y$, and $\theta$; and a -desired \gls{pose} consisting of $x_d$, $y_d$, and $\theta_d$ -\cite{bib:ctrl_wheeled_mobile_robots}. We'll call it Ramsete because that's the -acronym for the title of the book it came from in Italian (``Robotica Articolata -e Mobile per i SErvizi e le TEcnologie"). - -\subsection{Velocity and turning rate command derivation} - -The \gls{state} tracking \gls{error} $\mat{e}$ in the vehicle's coordinate frame -is defined as -\begin{equation*} - \begin{bmatrix} - e_x \\ - e_y \\ - e_\theta - \end{bmatrix} = - \begin{bmatrix} - \cos\theta & \sin\theta & 0 \\ - -\sin\theta & \cos\theta & 0 \\ - 0 & 0 & 1 - \end{bmatrix} - \begin{bmatrix} - x_d - x \\ - y_d - y \\ - \theta_d - \theta - \end{bmatrix} -\end{equation*} - -where $e_x$ is the tracking \gls{error} in $x$, $e_y$ is the tracking -\gls{error} in $y$, and $e_\theta$ is the tracking \gls{error} in $\theta$. -The $3 \times 3$ matrix is a rotation matrix that transforms the \gls{error} in -the \gls{pose} (represented by $x_d - x$, $y_d - y$, and $\theta_d - \theta$) -from the global coordinate frame into the vehicle's coordinate frame. - -We will use the following control laws $u_1$ and $u_2$ for velocity and turning -rate respectively. -\begin{equation} - \begin{aligned} - u_1 &= -k_1 e_x \\ - u_2 &= -k_3 e_\theta - k_2 v_d \sinc(e_\theta) e_y - \end{aligned} - \label{eq:ramsete_u} -\end{equation} - -where $k_1$, $k_2$, and $k_3$ are time-varying gains and $\sinc(e_\theta)$ is -defined as $\frac{\sin{e_\theta}}{e_\theta}$. This choice of control law may -seem arbitrary, and that's because it is. The only requirement on our choice is -that there exist an associated Lyapunov candidate function that proves the -control law is globally asymptotically stable. We'll provide a sketch of a proof -in theorem \ref{thm:ramsete_lyapunov_stability}. - -Our velocity and turning rate commands for the vehicle will use the following -nonlinear transformation of the inputs. -\begin{align*} - v &= v_d\cos e_\theta - u_1 \\ - \omega &= \omega_d - u_2 - \intertext{Substituting the control laws $u_1$ and $u_2$ into these equations - gives} - v &= v_d\cos{e_\theta} + k_1 e_x \\ - \omega &= k_3 e_\theta + \omega_d + k_2 v_d \sinc(e_\theta) e_y -\end{align*} -\begin{theorem} - \label{thm:ramsete_lyapunov_stability} - - Assuming that $v_d$ and $\omega_d$ are bounded with bounded derivatives, and - that $v_d(t) \rightarrow 0$ or $\omega_d(t) \rightarrow 0$ when - $t \rightarrow \infty$, the control laws in equation \eqref{eq:ramsete_u} - globally asymptotically stabilize the origin $\mat{e} = 0$. - - Proof: - - To prove convergence, the paper previously mentioned uses the following - Lyapunov function. - \begin{equation*} - V = \frac{k_2}{2}(e_x^2 + e_y^2) + \frac{e_\theta^2}{2} - \end{equation*} - - where $k_2$ is a tuning constant, $e_x$ is the tracking error in $x$, $e_y$ is - the tracking error in $y$, and $e_\theta$ is the tracking error in $\theta$. - - The time derivative along the solutions of the closed-loop \gls{system} is - nonincreasing since - \begin{equation*} - \dot{V} = -k_1 k_2 e_x^2 - k_3 e_\theta^2 \leq 0 - \end{equation*} - - Thus, $\lVert e(t) \rVert$ is bounded, $\dot{V}(t)$ is uniformly continuous, - and $V(t)$ tends to some limit value. Using the Barbalat lemma, $\dot{V}(t)$ - tends to zero \cite{bib:ctrl_wheeled_mobile_robots}. -\end{theorem} - -\subsection{Nonlinear controller equations} - -Let $k_2 = b$ and -$k = k_1(v_d, \omega_d) = k_3(v_d, \omega_d) = 2\zeta\sqrt{\omega_d^2 + bv_d^2}$. -\begin{theorem}[Ramsete unicycle controller] - \begin{align} - \begin{bmatrix} - e_x \\ - e_y \\ - e_\theta - \end{bmatrix} &= - \begin{bmatrix} - \cos\theta & \sin\theta & 0 \\ - -\sin\theta & \cos\theta & 0 \\ - 0 & 0 & 1 - \end{bmatrix} - \begin{bmatrix} - x_d - x \\ - y_d - y \\ - \theta_d - \theta - \end{bmatrix} \\ - v &= v_d \cos{e_\theta} + k e_x \label{eq:ramsete_eq1} \\ - \omega &= \omega_d + k e_\theta + b v_d \sinc(e_\theta) e_y - \label{eq:ramsete_eq2} \\ - k &= 2\zeta\sqrt{\omega_d^2 + bv_d^2} \label{eq:ramsete_eq3} \\ - \sinc(e_\theta) &= \frac{\sin{e_\theta}}{e_\theta} - \end{align} - \begin{figurekey} - \begin{tabular}{llll} - $v$ & velocity command & $v_d$ & desired velocity \\ - $\omega$ & turning rate command & $\omega_d$ & desired turning rate \\ - $x$ & actual $x$ position in global coordinate frame & $x_d$ & - desired $x$ position \\ - $y$ & actual $y$ position in global coordinate frame & $y_d$ & - desired $y$ position \\ - $\theta$ & actual angle in global coordinate frame & $\theta_d$ & - desired angle - \end{tabular} - \end{figurekey} - - $b$ and $\zeta$ are tuning parameters where - $b > 0~\frac{\text{rad}^2}{\text{m}^2}$ and - $\zeta \in (0, 1)~\text{rad}^{-1}$. Larger values of $b$ make convergence more - aggressive (like a proportional term), and larger values of $\zeta$ provide - more damping. -\end{theorem} - -$v$ and $\omega$ should be the \glspl{reference} for a drivetrain -\gls{reference} tracker. A good choice would be using equations -\eqref{eq:diff_vl} and \eqref{eq:diff_vr} to convert $v$ and $\omega$ to left -and right velocities, then applying LQR to the model in theorem -\ref{thm:diff_drive_velocity_state-space_model}. - -$x$, $y$, and $\theta$ are obtained via a \gls{pose} estimator (see chapter -\ref{ch:pose_estimation} for how to implement one). The desired velocity, -turning rate, and \gls{pose} can be varied over time according to a desired -trajectory. - -Figures \ref{fig:ramsete_traj_xy} and \ref{fig:ramsete_traj_response} show the -tracking performance of Ramsete for a given trajectory. -\begin{bookfigure} - \begin{minisvg}{2}{build/\chapterpath/ramsete_traj_xy} - \caption{Ramsete nonlinear controller x-y plot} - \label{fig:ramsete_traj_xy} - \end{minisvg} - \hfill - \begin{minisvg}{2}{build/\chapterpath/ramsete_traj_response} - \caption{Ramsete nonlinear controller response} - \label{fig:ramsete_traj_response} - \end{minisvg} -\end{bookfigure} - -\subsection{Dimensional analysis} - -$[v]$ denotes the dimension of $v$. $A$ means angle units, $L$ means length -units, and $T$ means time units. - -\subsubsection{Units of $\sinc(e_\theta)$} - -First, we'll find the units of $\sinc(e_\theta)$ for later use. -\begin{align*} - \sinc(e_\theta) &= \frac{\sin(e_\theta)}{e_\theta} \\ - [\sinc(e_\theta)] &= \frac{1}{A} \\ - [\sinc(e_\theta)] &= A^{-1} -\end{align*} - -\subsubsection{Ramsete velocity command equation} - -Start from equation \eqref{eq:ramsete_eq1}. -\begin{align*} - v &= v_d \cos e_\theta + k e_x \\ - [v] &= [v_d] [\cos e_\theta] + [k] [e_x] \\ - L T^{-1} &= L T^{-1} \cdot 1 + [k] L \\ - L T^{-1} &= L T^{-1} + [k] L \\ - L T^{-1} &= [k] L \\ - T^{-1} &= [k] \\ - [k] &= T^{-1} -\end{align*} - -Therefore, the units of $k$ are $T^{-1}$. - -\subsubsection{Ramsete angular velocity command equation} - -Start from equation \eqref{eq:ramsete_eq2}. -\begin{align} - \omega &= \omega_d + ke_\theta + bv_d \sinc(e_\theta) e_y \nonumber \\ - [\omega] &= [\omega_d] + [k][e_\theta] + [b][v_d] [\sinc(e_\theta)] [e_y] - \nonumber \\ - [\omega] &= [\omega_d] + [k][e_\theta] + [b][v_d] [\sinc(e_\theta)] [e_y] - \nonumber \\ - A T^{-1} &= A T^{-1} + [k] A + [b] L T^{-1} \cdot A^{-1} \cdot L \nonumber \\ - A T^{-1} &= A T^{-1} + [k] A + [b] A^{-1} L^{2} T \nonumber \\ - A T^{-1} &= [k] A + [b] A^{-1} L^{2} T - \label{eq:ramsete_eq2_intermediate_units} -\end{align} - -First, we'll find the units of $k$. - -The left-hand side and first term of equation -\eqref{eq:ramsete_eq2_intermediate_units} must have equivalent units. -\begin{align*} - A T^{-1} &= [k] A \\ - T^{-1} &= [k] \\ - [k] &= T^{-1} -\end{align*} - -This is consistent with the result from the Ramsete velocity command equation. - -Next, we'll find the units of $b$. - -The left-hand side and second term of equation -\eqref{eq:ramsete_eq2_intermediate_units} must have equivalent units. -\begin{align*} - A T^{-1} &= [b] A^{-1} L^{2} T^{-1} \\ - A^{2} L^{-2} &= [b] \\ - [b] &= A^{2} L^{-2} -\end{align*} - -\subsubsection{Ramsete $k$ equation} - -Start from equation \eqref{eq:ramsete_eq3}. -\begin{align} - k &= 2\zeta \sqrt{\omega_d^{2} + bv_d^{2}} \nonumber \\ - [k] &= [\zeta] \sqrt{[\omega_d]^{2} + [b][v_d]^{2}} \nonumber \\ - T^{-1} &= [\zeta] \sqrt{(A T^{-1})^{2} + [b] (L T^{-1})^{2}} \nonumber \\ - T^{-1} &= [\zeta] \sqrt{A^{2} T^{-2} + [b] L^{2} T^{-2}} - \label{eq:ramsete_eq3_intermediate_units} -\end{align} - -First, we'll find the units of $b$. - -The additive terms under the square root must have equivalent units. -\begin{align*} - (A T^{-1})^{2} &= [b] (L T^{-1})^{2} \\ - A^{2} T^{-2} &= [b] L^{2} T^{-2} \\ - A^{2} L^{-2} &= [b] \\ - [b] &= A^{2} L^{-2} -\end{align*} - -This is consistent with the result from the angular velocity command equation, -so we can use it when determining the units of $\zeta$. - -Next, we'll find the units of $\zeta$. - -Substitute $[b]$ into equation \eqref{eq:ramsete_eq3_intermediate_units}. -\begin{align*} - T^{-1} &= [\zeta] \sqrt{A^{2} T^{-2} + [b] L^{2} T^{-2}} \\ - T^{-1} &= [\zeta] \sqrt{A^{2} T^{-2} + A^{2} L^{-2} \cdot L^{2} T^{-2}} \\ - T^{-1} &= [\zeta] \sqrt{A^{2} T^{-2} + A^{2} T^{-2}} \\ - T^{-1} &= [\zeta] \sqrt{A^{2} T^{-2}} \\ - T^{-1} &= [\zeta] A T^{-1} \\ - A^{-1} &= [\zeta] \\ - [\zeta] &= A^{-1} -\end{align*} - -\subsubsection{Units of tunable parameters $b$ and $\zeta$} -\begin{align} - [b] &= A^{2} L^{-2} \\ - [\zeta] &= A^{-1} -\end{align} diff --git a/modern-control-theory/nonlinear-control/ramsete_traj.py b/modern-control-theory/nonlinear-control/ramsete_traj.py deleted file mode 100755 index 16091eb4..00000000 --- a/modern-control-theory/nonlinear-control/ramsete_traj.py +++ /dev/null @@ -1,294 +0,0 @@ -#!/usr/bin/env python3 - -""" -Simulates Ramsete unicycle controller with RKDP integration in chassis -coordinate frame. -""" - -import math -import sys - -import frccontrol as fct -import matplotlib as mpl -import matplotlib.pyplot as plt -import numpy as np -from wpimath.geometry import Pose2d -from wpimath.trajectory import TrajectoryConfig, TrajectoryGenerator - -from bookutil import latex, plotutil - -if "--noninteractive" in sys.argv: - mpl.use("svg") - - -def ramsete(pose_desired, v_desired, omega_desired, pose, b, zeta): - """Ramsete is a nonlinear time-varying feedback controller for unicycle - models that drives the model to a desired pose along a two-dimensional - trajectory. - - The reference pose, linear velocity, and angular velocity should come from - a drivetrain trajectory. - - Keyword arguments: - pose_desired -- the desired pose - v_desired -- the desired linear velocity - omega_desired -- the desired angular velocity - pose -- the current pose - b -- tuning parameter (b > 0) for which larger values make convergence more - aggressive like a proportional term - zeta -- tuning parameter (0 < zeta < 1) for which larger values provide more - damping in response - - Returns: - linear velocity and angular velocity commands - """ - e = pose_desired.relativeTo(pose) - - k = 2 * zeta * math.sqrt(omega_desired**2 + b * v_desired**2) - v = v_desired * e.rotation().cos() + k * e.x - omega = ( - omega_desired - + k * e.rotation().radians() - + b * v_desired * np.sinc(e.rotation().radians()) * e.y - ) - - return v, omega - - -class Drivetrain: - """An frccontrol system for a differential drive.""" - - def __init__(self, dt): - """Differential drive subsystem. - - Keyword arguments: - dt -- time between model/controller updates - """ - self.dt = dt - - # Radius of robot in meters - self.rb = 0.59055 / 2.0 - - # Gear ratio of differential drive - G = 60.0 / 11.0 - - # Drivetrain mass in kg - m = 52 - # Radius of wheels in meters - r = 0.08255 / 2.0 - # Moment of inertia of the differential drive in kg-m² - J = 6.0 - - motor = fct.models.gearbox(fct.models.MOTOR_CIM, 3.0) - - C1 = -(G**2) * motor.Kt / (motor.Kv * motor.R * r**2) - C2 = G * motor.Kt / (motor.R * r) - C3 = -(G**2) * motor.Kt / (motor.Kv * motor.R * r**2) - C4 = G * motor.Kt / (motor.R * r) - self.velocity_A = np.array( - [ - [(1 / m + self.rb**2 / J) * C1, (1 / m - self.rb**2 / J) * C3], - [(1 / m - self.rb**2 / J) * C1, (1 / m + self.rb**2 / J) * C3], - ] - ) - self.velocity_B = np.array( - [ - [(1 / m + self.rb**2 / J) * C2, (1 / m - self.rb**2 / J) * C4], - [(1 / m - self.rb**2 / J) * C2, (1 / m + self.rb**2 / J) * C4], - ] - ) - - # States: x position (m), y position (m), heading (rad), - # left velocity (m/s), right velocity (m/s) - # Inputs: left voltage (V), right voltage (V) - # Outputs: heading (rad), left velocity (m/s), right velocity (m/s) - self.observer = fct.ExtendedKalmanFilter( - 5, - 2, - self.f, - self.h, - [0.5, 0.5, 10.0, 1.0, 1.0], - [0.0001, 0.01, 0.01], - self.dt, - ) - self.velocity_controller = fct.LinearQuadraticRegulator( - self.velocity_A, self.velocity_B, [0.95, 0.95], [12.0, 12.0], self.dt - ) - - # Sim variables - self.x = np.zeros((5, 1)) - self.u = np.zeros((2, 1)) - self.y = np.zeros((3, 1)) - - self.u_min = np.array([[-12.0], [-12.0]]) - self.u_max = np.array([[12.0], [12.0]]) - - def f(self, x, u): - """ - Nonlinear differential drive dynamics. - - Keyword arguments: - x -- state vector - u -- input vector - - Returns: - dx/dt -- state derivative - """ - return ( - np.array( - [ - [(x[3, 0] + x[4, 0]) / 2.0 * math.cos(x[2, 0])], - [(x[3, 0] + x[4, 0]) / 2.0 * math.sin(x[2, 0])], - [(x[4, 0] - x[3, 0]) / (2.0 * self.rb)], - [self.velocity_A[0, 0] * x[3, 0] + self.velocity_A[0, 1] * x[4, 0]], - [self.velocity_A[1, 0] * x[3, 0] + self.velocity_A[1, 1] * x[4, 0]], - ] - ) - + np.block([[np.zeros((3, 2))], [self.velocity_B]]) @ u - ) - - # pragma pylint: disable=unused-argument - def h(self, x, u): - """ - Nonlinear differential drive dynamics. - - Keyword arguments: - x -- state vector - u -- input vector - - Returns: - dx/dt -- state derivative - """ - return x[2:, :] - - def update(self, r, next_r): - """ - Advance the model by one timestep. - - Keyword arguments: - r -- the current reference - next_r -- the next reference - """ - # Update sim model - self.x = fct.rkdp(self.f, self.x, self.u, self.dt) - self.y = self.h(self.x, self.u) - - self.observer.predict(self.u, self.dt) - self.observer.correct(self.u, self.y) - - # Feedforward - rdot = (next_r - r) / self.dt - u_ff = np.linalg.pinv(np.block([[np.zeros((3, 2))], [self.velocity_B]])) @ ( - rdot - self.f(r, np.zeros((2, 1))) - ) - - # Pose feedback - v_cmd, omega_cmd = ramsete( - Pose2d(r[0, 0], r[1, 0], r[2, 0]), - (r[3, 0] + r[4, 0]) / 2, - (r[4, 0] - r[3, 0]) / (2 * self.rb), - Pose2d( - self.observer.x_hat[0, 0], - self.observer.x_hat[1, 0], - self.observer.x_hat[2, 0], - ), - 2.0, - 0.7, - ) - - # Velocity feedback - u_fb = self.velocity_controller.calculate( - self.observer.x_hat[3:, :], - np.array([[v_cmd - omega_cmd * self.rb], [v_cmd + omega_cmd * self.rb]]), - ) - - self.u = u_ff + u_fb - - u_cap = np.max(np.abs(self.u)) - if u_cap > 12.0: - self.u = self.u / u_cap * 12.0 - - -def main(): - """Entry point.""" - dt = 0.02 - - # Radius of robot in meters - rb = 0.59055 / 2.0 - - trajectory = TrajectoryGenerator.generateTrajectory( - [Pose2d(1.330117, 13, 0), Pose2d(10.17, 18, 0)], - TrajectoryConfig(3.5, 3.5), - ) - - refs = [] - t_rec = np.arange(0, trajectory.totalTime(), dt) - for t in t_rec: - sample = trajectory.sample(t) - vl = sample.velocity - sample.velocity * sample.curvature * rb - vr = sample.velocity + sample.velocity * sample.curvature * rb - refs.append( - np.array( - [ - [sample.pose.X()], - [sample.pose.Y()], - [sample.pose.rotation().radians()], - [vl], - [vr], - ] - ) - ) - - x = np.array( - [[refs[0][0, 0] + 0.5], [refs[0][1, 0] + 0.5], [math.pi / 2], [0], [0]] - ) - drivetrain = Drivetrain(dt) - drivetrain.x = x - drivetrain.observer.x_hat = x - - # Run simulation - r_rec, x_rec, u_rec, _ = fct.generate_time_responses(drivetrain, refs) - - fig = plt.figure(1) - if "--noninteractive" in sys.argv: - plotutil.plot_xy( - fig, - r_rec[0, :], - r_rec[1, :], - x_rec[0, :], - x_rec[1, :], - ) - latex.savefig("ramsete_traj_xy") - else: - anim = plotutil.animate_xy( # pragma pylint: disable=unused-variable - fig, - r_rec[0, :], - r_rec[1, :], - x_rec[0, :], - x_rec[1, :], - dt, - ) - - fct.plot_time_responses( - [ - "x position (m)", - "y position (m)", - "Heading (rad)", - "Left velocity (m/s)", - "Right velocity (m/s)", - ], - ["Left voltage (V)", "Right voltage (V)"], - t_rec, - r_rec, - x_rec, - u_rec, - ) - - if "--noninteractive" in sys.argv: - latex.savefig("ramsete_traj_response") - else: - plt.show() - - -if __name__ == "__main__": - main() diff --git a/system-modeling/dynamics/differential-drive-kinematics.tex b/system-modeling/dynamics/differential-drive-kinematics.tex index 88fa1521..fa74927b 100644 --- a/system-modeling/dynamics/differential-drive-kinematics.tex +++ b/system-modeling/dynamics/differential-drive-kinematics.tex @@ -32,7 +32,7 @@ \subsection{Inverse kinematics} \nonumber \intertext{The magnitude of $\hat{i}$ is $1$, so the denominator cancels.} v_l &= (v_c - \omega r_b) \hat{i} \cdot \hat{i} \nonumber \\ - v_l &= v_c - \omega r_b \label{eq:diff_vl} + v_l &= v_c - \omega r_b \end{align} Next, we'll derive $v_r$. @@ -46,7 +46,7 @@ \subsection{Inverse kinematics} \nonumber \intertext{The magnitude of $\hat{i}$ is $1$, so the denominator cancels.} v_r &= (v_c + \omega r_b) \hat{i} \cdot \hat{i} \nonumber \\ - v_r &= v_c + \omega r_b \label{eq:diff_vr} + v_r &= v_c + \omega r_b \end{align} So the two inverse kinematic equations are as follows.