Improve robustness of spline planning
Previously, there were corner cases in the spline math that we handled
poorly due to laziness in how we handled certain constraints.
This patch should make it so that we can correctly plan any spline that
we actually produce--although due to the fact that it is now more
properly following the actual physical constraints of the splines, it
actually forced me to increase slightly the tolerances in the tests.
This has yet to be tested on a real robot.
Change-Id: I63f68eede9d0fbe6d41bf3caea8aca19ece9fa1f
diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index 71c7278..a723d96 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -628,10 +628,12 @@
linkstatic = True,
deps = [
":trajectory",
+ ":drivetrain_test_lib",
"//aos/testing:googletest",
"//aos/testing:test_shm",
"//y2016:constants",
"//y2016/control_loops/drivetrain:polydrivetrain_plants",
+ "//y2019/control_loops/drivetrain:drivetrain_base",
] + cpu_select({
"amd64": [
"//third_party/matplotlib-cpp",
diff --git a/frc971/control_loops/drivetrain/distance_spline_test.cc b/frc971/control_loops/drivetrain/distance_spline_test.cc
index c9cb9f0..55b40dd 100644
--- a/frc971/control_loops/drivetrain/distance_spline_test.cc
+++ b/frc971/control_loops/drivetrain/distance_spline_test.cc
@@ -73,6 +73,8 @@
point += dpoint * ddistance;
dpoint += distance_spline_.DDXY(distance) * ddistance;
+ EXPECT_FLOAT_EQ(distance_spline_.DDXY(distance).norm(),
+ ::std::abs(distance_spline_.DTheta(distance)));
}
#if defined(SUPPORT_PLOT)
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index b2a533e..365bac6 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -80,8 +80,8 @@
const double expected_y =
CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())->y();
const ::Eigen::Vector2d actual = drivetrain_plant_.GetPosition();
- EXPECT_NEAR(actual(0), expected_x, 2e-2);
- EXPECT_NEAR(actual(1), expected_y, 2e-2);
+ EXPECT_NEAR(actual(0), expected_x, 3e-2);
+ EXPECT_NEAR(actual(1), expected_y, 3e-2);
}
void WaitForTrajectoryPlan() {
diff --git a/frc971/control_loops/drivetrain/libspline.cc b/frc971/control_loops/drivetrain/libspline.cc
index 5f5b02e..f0b9537 100644
--- a/frc971/control_loops/drivetrain/libspline.cc
+++ b/frc971/control_loops/drivetrain/libspline.cc
@@ -130,8 +130,8 @@
void deleteTrajectory(Trajectory *t) { delete t; }
- void TrajectorySetLongitudalAcceleration(Trajectory *t, double accel) {
- t->set_longitudal_acceleration(accel);
+ void TrajectorySetLongitudinalAcceleration(Trajectory *t, double accel) {
+ t->set_longitudinal_acceleration(accel);
}
void TrajectorySetLateralAcceleration(Trajectory *t, double accel) {
diff --git a/frc971/control_loops/drivetrain/spline_math.tex b/frc971/control_loops/drivetrain/spline_math.tex
new file mode 100644
index 0000000..49164bf
--- /dev/null
+++ b/frc971/control_loops/drivetrain/spline_math.tex
@@ -0,0 +1,567 @@
+\documentclass{article}
+\usepackage[utf8]{inputenc}
+\usepackage{amsmath}
+\DeclareMathOperator{\sgn}{sgn}
+\usepackage{hyperref}
+\usepackage{enumitem}
+\usepackage{commath}
+\usepackage[margin=0.5in]{geometry}
+\title{Spline Math}
+\author{James Kuszmaul}
+\begin{document}
+
+\maketitle
+
+\section{Introductions}
+
+This document describes some of the math going on in computing constraints on
+the maximum velocities in \texttt{trajectory.*}. Some of the thoughts are
+somewhat disorganized and represent a stream-of-consciousness more than actually
+useful documentation.
+
+For a given path, we will have:
+
+\begin{tabular}{r|l}
+Name & Description \\ \hline
+$v_l$, $v_r$ & Left and right wheel velocities \\
+$\theta$ & Robot heading/yaw at a position along the path \\
+$r$ & Robot radius (assuming center of mass in center of robot) \\
+$u_l$, $u_r$ & Left and right wheel voltages \\
+$V_{max}$ & Maximum allowable voltage (typically 12V) \\
+$v_{max}$ & User-specified maximum velocity \\
+$u$ & $\begin{bmatrix} u_l \\ u_r \end{bmatrix}$ \\
+$A$, $B$ & LTI, matrices such that $\begin{bmatrix} \dot{v}_l \\ \dot{v}_r
+\end{bmatrix} = A \begin{bmatrix} v_l \\ v_r \end{bmatrix} + B u$ \\
+$s$ & Distance along the path \\
+\end{tabular}
+
+We are interested in discovering the fastest way to traverse the path in
+question, given constraints on:
+\begin{itemize}
+\item Friction limits of the wheels (assumed to static and time/velocity
+ invariant).
+\item Voltage limits on the motors.
+\item Maximum velocity.
+\end{itemize}
+
+The voltage constraints are essentially provided by the linear dynamics shown
+above:
+
+\begin{align}
+\begin{bmatrix} \dot{v}_l \\ \dot{v}_r \end{bmatrix} &=
+ A \begin{bmatrix} v_l \\ v_r \end{bmatrix} + B u
+\end{align}
+
+Also, at any given point we must maintain velocities/accelerations that actually
+allow us to follow the path (as opposed to optimizing $v_l$/$v_r$ separately...).
+
+Using the velocities of the individual wheels as a starting point, and
+introducing dummy variables for the velocity/acceleration of the center of the
+robot (corresponding to time derivatives of $s$):
+
+\begin{align*}
+v_c &= \frac{v_l r_r + v_r r_l}{r_r + r_l} \\
+\begin{bmatrix} v_l \\ v_r \end{bmatrix} &=
+ \begin{bmatrix} 1 - r_l \frac{d\theta}{ds} \\ 1 + r_r \frac{d\theta}{ds}
+\end{bmatrix} v_c \\
+\begin{bmatrix} \dot{v}_l \\ \dot{v}_r \end{bmatrix} &=
+ \begin{bmatrix} 1 - r_l \frac{d\theta}{ds} \\ 1 + r_r \frac{d\theta}{ds}
+ \end{bmatrix} \dot{v}_c + \begin{bmatrix} - r_l \frac{d^2\theta}{ds^2} v_c
+ \\ r_r \frac{d^2\theta}{ds^2} v_c \end{bmatrix} v_c \\
+\begin{bmatrix} \dot{v}_l \\ \dot{v}_r \end{bmatrix} &=
+ \begin{bmatrix} 1 - r_l \frac{d\theta}{ds} \\ 1 + r_r \frac{d\theta}{ds}
+ \end{bmatrix} \dot{v}_c + \begin{bmatrix} -r_l \\ r_r \end{bmatrix} \frac{d^2\theta}{ds^2} v_c^2 \\
+\end{align*}
+
+And finally, we need to know the lateral accelerations of the two wheels as
+part of determining whether we would be breaking friction. Note that, because of
+the general assumptions we are making about the setup of the robot, the lateral
+acceleration on the two wheels will be identical (if they were not identical,
+that would imply the robot was tearing itself apart side-to-side).
+
+\begin{align*}
+a_{lat} &= \frac{d\theta}{ds} v_c^2 \\
+\end{align*}
+
+Finally, we need to be combine the lateral/longitudinal accelerations of each
+wheel to determine whether the wheel would break friction. While technically
+there is only one expression for this, as a practical matter we have a bit of
+leeway in defining exactly what the relationship should be (e.g., if we want
+separate max longitudinal/lateral accelerations, we can create an ellipse; we
+could also just create a raw rectangle and decouple the two, although that would
+probably be a particularly poor approximation of the actual dynamics). For now
+we will say that our friction is limited by some nonnegative convex function
+$g(a_{lat}, a_{lng}) \le 1$ for any given wheel.
+
+Summarizing our equations and constraints, we have:
+
+\begin{align*}
+v_c &= \frac{v_l r_r + v_r r_l}{r_r + r_l} \\
+a_{lat} &= \frac{d\theta}{ds} v_c^2 \\
+g(a_{lat}, \dot{v}_l) &\le 1 \\
+g(a_{lat}, \dot{v}_r) &\le 1 \\
+\abs{u_l} &\le V_{max} \\
+\abs{u_r} &\le V_{max} \\
+\begin{bmatrix} v_l \\ v_r \end{bmatrix} &= \begin{bmatrix} 1 - r_l
+ \frac{d\theta}{ds} \\ 1 + r_r \frac{d\theta}{ds}
+\end{bmatrix} v_c \\
+\begin{bmatrix} \dot{v}_l \\ \dot{v}_r \end{bmatrix} &=
+ A \begin{bmatrix} v_l \\ v_r \end{bmatrix} + B \begin{bmatrix} u_l \\ u_r
+ \end{bmatrix} \\
+\begin{bmatrix} \dot{v}_l \\ \dot{v}_r \end{bmatrix} &=
+ \begin{bmatrix} 1 - r_l \frac{d\theta}{ds} \\ 1 + r_r \frac{d\theta}{ds}
+ \end{bmatrix} \dot{v}_c + \begin{bmatrix} -r_l \\ r_r \end{bmatrix} \frac{d^2\theta}{ds^2} v_c^2 \\
+\end{align*}
+
+With $v_l$, $v_r$, $u_l$, and $u_r$ free at all $s$ except the start and end, at
+which the velocities must
+be zero (technically, we could make do with allowing non-zero velocities, but
+then we could end up in situations that would be impossible to solve--because
+the splines have differentiable curvature, any of the splines we use will always
+have some valid velocity profile, but depending on the curvature, that velocity
+may be arbitrarily small).
+
+We can provide an upper limit on the goal velocity at any given point with the
+min of the specified max velocity and the lateral-acceleration limited velocity.
+This also gives an initial pass of the longitudinal accelerations that would be
+required to follow this profile.
+
+\begin{align*}
+v_{lat} &= \min (v_{max}, \sqrt{\frac{a_{lat,max}}{\frac{d\theta}{ds}}}) \\
+\dot{v}_{lat} &= \begin{cases} 0 & v_{lat} = v_{max} \\
+-\frac12 \sqrt{\frac{a_{lat,max}}{\frac{d\theta}{ds}^3}} \frac{d^2\theta}{ds^2}
+v_{lat} & v_{lat} \ne v_{max} \\
+\end{cases} \\
+&= \begin{cases} 0 & v_{lat} = v_{max} \\
+-\frac12 \frac{a_{lat,max}}{\frac{d\theta}{ds}^2} \frac{d^2\theta}{ds^2}
+ & v_{lat} \ne v_{max} \\
+\end{cases} \\
+\end{align*}
+
+If we start to consider accounting also for longitudinal accelerations, we can
+start with:
+
+\begin{align}
+\begin{bmatrix} \dot{v}_l \\ \dot{v}_r \end{bmatrix} &=
+ \begin{bmatrix} 1 - r_l \frac{d\theta}{ds} \\ 1 + r_r \frac{d\theta}{ds}
+ \end{bmatrix} \dot{v}_c + \begin{bmatrix} -r_l \\ r_r \end{bmatrix} \frac{d^2\theta}{ds^2} v_c^2 \\
+(r_r + r_l)
+\left[ \begin{bmatrix} \dot{v}_l \\ \dot{v}_r \end{bmatrix} &=
+ \begin{bmatrix} 1 - r_l \frac{d\theta}{ds} \\ 1 + r_r \frac{d\theta}{ds}
+ \end{bmatrix} \frac{\dot{v}_l r_r + \dot{v}_r r_l}{r_r + r_l} + \begin{bmatrix} -r_l \\ r_r \end{bmatrix} \frac{d^2\theta}{ds^2} v_c^2
+ \right] \\
+\begin{bmatrix}
+r_l + r_l r_r \frac{d\theta}{ds} & -r_l + r_lr_l \frac{d\theta}{ds} \\
+-r_r - r_r r_r \frac{d\theta}{ds} & r_r - r_r r_l \frac{d\theta}{ds} \\
+\end{bmatrix}
+\begin{bmatrix} \dot{v}_l \\ \dot{v}_r \end{bmatrix} &=
+ \begin{bmatrix} -r_l \\ r_r \end{bmatrix} (r_l + r_r) \frac{d^2\theta}{ds^2} v_c^2 \\
+\begin{bmatrix}
+1 + r_r \frac{d\theta}{ds} & -1 + r_l \frac{d\theta}{ds} \\
+-1 - r_r \frac{d\theta}{ds} & 1 - r_l \frac{d\theta}{ds} \\
+\end{bmatrix}
+\begin{bmatrix} \dot{v}_l \\ \dot{v}_r \end{bmatrix} &=
+ \begin{bmatrix} -1 \\ 1 \end{bmatrix} (r_l + r_r) \frac{d^2\theta}{ds^2} v_c^2 \\
+\begin{bmatrix}
+1 + r_r \frac{d\theta}{ds} & -1 + r_l \frac{d\theta}{ds} \\
+1 + r_r \frac{d\theta}{ds} & -1 + r_l \frac{d\theta}{ds} \\
+\end{bmatrix}
+\begin{bmatrix} \dot{v}_l \\ \dot{v}_r \end{bmatrix} &=
+ -(r_l + r_r) \frac{d^2\theta}{ds^2} v_c^2 \\
+(1 + r_r \frac{d\theta}{ds}) \dot{v}_l + (-1 + r_l \frac{d\theta}{ds}) \dot{v}_r &=
+ -(r_l + r_r) \frac{d^2\theta}{ds^2} v_c^2 \\
+%(1 + r \frac{d\theta}{ds}) \dot{v}_l + (-1 + r \frac{d\theta}{ds}) (2\dot{v}_c -
+%\dot{v}_l) &=
+% -2 r \frac{d^2\theta}{ds^2} v_c^2 \\
+%2 \dot{v}_l + 2(-1 + r \frac{d\theta}{ds}) \dot{v}_c
+% &= -2 r \frac{d^2\theta}{ds^2} v_c^2 \\
+%\dot{v}_l &= (1 - r \frac{d\theta}{ds}) \dot{v}_c - 2 r \frac{d^2\theta}{ds^2} v_c^2 \\
+%\dot{v}_r &= (1 + r \frac{d\theta}{ds}) \dot{v}_c + 2 r \frac{d^2\theta}{ds^2} v_c^2 \\
+\begin{bmatrix}
+1 & 0 \\
+0 & 1 \\
+-1 & 0 \\
+0 & -1 \\
+\end{bmatrix}
+\begin{bmatrix} \dot{v}_l \\ \dot{v}_r \end{bmatrix} &\le g'(v_c, s) \\
+\begin{bmatrix}
+1 & 0 \\
+0 & 1 \\
+-1 & 0 \\
+0 & -1 \\
+\end{bmatrix} \left(
+\begin{bmatrix} 1 - r_l \frac{d\theta}{ds} \\ 1 + r_r \frac{d\theta}{ds}
+\end{bmatrix} \dot{v}_c + \begin{bmatrix} -r_l \\ r_r \end{bmatrix} \frac{d^2\theta}{ds^2} v_c^2
+\right) &\le g'(v_c, s)
+\end{align}
+
+Note how we have a linear relationship between $\dot{v}_l$ and $\dot{v}_r$,
+where the slope of the line between the two is constant at any given point on
+the path, but the y-intercept and the max/min bounds get tighter with speed.
+Furthermore, the velocity along the path ($v_c$) is a linear function of the
+wheel velocities $v_l$ and $v_r$. Thus, we can state, at any given point on the
+path, a lower velocity along the path will not decrease our acceleration
+bounds (if we are travelling in a straight line, then our speed does not affect
+the straight-line acceleration limits).
+
+We also can introduce the voltage constraints:
+
+\begin{align}
+\begin{bmatrix} \dot{v}_l \\ \dot{v}_r \end{bmatrix} &=
+ A \begin{bmatrix} v_l \\ v_r \end{bmatrix} + B \begin{bmatrix} u_l \\ u_r
+ \end{bmatrix} \\
+\begin{bmatrix} \dot{v}_l \\ \dot{v}_r \end{bmatrix} &=
+ A \begin{bmatrix} 1 - r_l \frac{d\theta}{ds} \\ 1 + r_r \frac{d\theta}{ds}
+ \end{bmatrix} v_c + B \begin{bmatrix} u_l \\ u_r \end{bmatrix} \\
+\begin{bmatrix} 1 - r_l \frac{d\theta}{ds} \\ 1 + r_r \frac{d\theta}{ds}
+ \end{bmatrix} \dot{v}_c + \begin{bmatrix} -r_l \\ r_r \end{bmatrix} \frac{d^2\theta}{ds^2} v_c^2
+ &= A \begin{bmatrix} 1 - r_l \frac{d\theta}{ds} \\ 1 + r_r \frac{d\theta}{ds}
+ \end{bmatrix} v_c + B \begin{bmatrix} u_l \\ u_r \end{bmatrix} \\
+\end{align}
+
+With these constraints, we can no longer guarantee that the set of feasible
+velocities at any given point along the path is contiguous. However, we can
+guarantee that at any given point there will be at most 2 contiguous sets of
+valid velocities---in the case where there are two separate sets of allowable
+velocities, then one corresponds to the lower velocities which start at zero and
+go up until we can no longer use our finite available voltage to track the path.
+The second zone can occur at higher velocities where we are going so fast that
+the deceleration from the Back-EMF of the motor is enough to slow us down, but
+not so fast that we would lose friction.
+
+This divergence does technically create a bit of a challenge--if it were to,
+e.g., split into arbitrarily many possible paths than that would complicate the
+optimization problem substantially. However, because:
+\begin{enumerate}
+\item We only split into two branches at once; and
+\item we should only have a limited number of branchings for a given N-spline
+\footnote{I'll leave this as an exercise to the reader/my future self. However,
+logically, a spline with a given number of points can only have so many features
+and so should be able to have a very finite number of branchings.}.
+\end{enumerate}
+
+It should not actually be an issue to exhaustively attempt to plan for all
+possible branchings, knowing that the path of ``always follow the slower
+branch'' will always provide a feasible path, and choose whatever feasible plan
+is fastest.
+
+Once we choose which branch to take, it is a relatively simple matter of
+exercising the existing method. Essentially, taking the set of feasible
+velocities/accelerations as a starting point we do a forwards and backwards pass
+(order is unimportant)) where we start at our initial/terminal velocity, and
+accelerate forwards/decelerate backwards as hard as possible for each pass. If
+attempting to accelerate as hard as possible on the forwards pass (or,
+conversely, decelerate on the backwards) results in us violating the feasible
+velocity bounds, we can simply set the velocity at that point for that pass to
+the highest feasible velocity. We can guarantee that the pass in the opposite
+direction will \emph{not} get stuck at that same point because if that would
+suggest we had different physics going backwards than forwards, which is not the
+case\footnote{I'm too lazy to spell this logic out more carefully, and I don't
+think it is wrong. That is not guaranteed, however.}.
+
+\section{Specialization for Friction Circle}
+
+As our particular friction circle, we will attempt to use a straight
+ellipse\footnote{Note that this has numerical stability issues due to the
+infinite slope near zero}.
+With a $a_{lat,max}$ and $a_{lng,max}$ for the maximum lateral and longitudinal
+accelerations, i.e.:
+
+\begin{align*}
+g(a_{lat}, a_{lng}) &= \left(\frac{a_{lat}}{a_{lat,max}}\right)^2
+ + \left(\frac{a_{lng}}{a_{lng,max}}\right)^2 \\
+\end{align*}
+
+We care about turning this into a function of the path geometry and the current
+velocity that returns the max longitudinal acceleration and lets us calculate
+the friction-limited maximum acceleration, $v_{c,fmax}$. In order to do so, we
+note that, in order to be at the edge of a feasible velocity, we observe that,
+at any given $v_c$, we will have two pairs of constraints on the center-of-mass
+longitudinal acceleration that both must be met,
+corresponding to the fact that the each wheel creates an upper/lower bound
+on the valid accelerations of the center-of-mass of the robot, but at high
+$\frac{d^2\theta}{ds^2}v_c^2$, the constraints from each wheel may not overlap.
+Thus, we care about the $v_c$ such that the pairs of constraints \emph{just
+barely} overlap. This will happen when one constraint from the right wheel and
+one from the left are just on the edge of being violated; furthermore, of the
+four possible left/right pairings (+/- of left/right), only two pairs correspond
+to being on the edge of overlapping (the other two pairs correspond to, e.g.,
+having the upper acceleration limit in the same spot, as occurs when driving
+straight; in such a situation, there are plenty of valid accelerations, we just
+happen to start slipping both wheels at the same time). Which pair to look at is
+determined by the signs on $\dot{v}_c$--if they are the same (as would occur
+when going nearly straight), then we care about when the opposite limits
+overlap (e.g., upper limit on the left, lower on the right).
+
+\begin{align*}
+\dot{v}_l, \dot{v}_r &\le a_{lng, max} \sqrt{1 - \left(\frac{a_{lat}}{a_{lat,max}}\right)^2} \\
+\dot{v}_l, \dot{v}_r &\le a_{lng, max} \sqrt{1 - \left(\frac{\frac{d\theta}{ds}v_c^2}{a_{lat,max}}\right)^2} \\
+ \begin{bmatrix} 1 - r_l \frac{d\theta}{ds} \\ 1 + r_r \frac{d\theta}{ds} \\
+ -1 + r_l \frac{d\theta}{ds} \\ -1 - r_r \frac{d\theta}{ds}
+ \end{bmatrix} \dot{v}_c &\le \begin{bmatrix} r_l \\ -r_r \\ -r_l \\ r_r \end{bmatrix} \frac{d^2\theta}{ds^2} v_c^2
+ + a_{lng, max} \sqrt{1 - \left(\frac{\frac{d\theta}{ds}v_c^2}{a_{lat,max}}\right)^2} \\
+0 &= \pm \{r_l,r_r\} \frac{d^2\theta}{ds^2} v_{c,fmax}^2 + a_{lng, max} \sqrt{1 -
+ \left(\frac{\frac{d\theta}{ds}v_{c,fmax}^2}{a_{lat,max}}\right)^2} \\
+\left(\frac{d^2\theta}{ds^2} \frac{\{r_l,r_r\}v_{c,fmax}^2}{a_{lng, max}}\right)^2 &=
+ 1 - \left(\frac{\frac{d\theta}{ds}v_{c,fmax}^2}{a_{lat,max}}\right)^2 \\
+v_{c,fmax} &= \min_{r \in \{r_l, r_r\}}
+\left(\left(\frac{d^2\theta}{ds^2} \frac{r}{a_{lng, max}}\right)^2 +
+ \left(\frac{\frac{d\theta}{ds}}{a_{lat,max}}\right)^2\right)^{-\frac14} \\
+\begin{bmatrix} 1 - r_l \frac{d\theta}{ds} \\ 1 + r_r \frac{d\theta}{ds}
+\end{bmatrix} \dot{v}_c + \begin{bmatrix} -r_l \\ r_r \end{bmatrix} \frac{d^2\theta}{ds^2} v_c^2
+&= \begin{bmatrix}\pm 1 \\ \pm 1\end{bmatrix}
+ a_{lng, max} \sqrt{1 - \left(\frac{\frac{d\theta}{ds}v_c^2}{a_{lat,max}}\right)^2} \\
+K_2 \dot{v}_c + K_1 v_c^2
+&= \begin{bmatrix}\pm 1 \\ \pm 1\end{bmatrix}
+ a_{lng, max} \sqrt{1 - \left(\frac{\frac{d\theta}{ds}v_c^2}{a_{lat,max}}\right)^2} \\
+K_2' K_2 \dot{v}_c + K_2' K_1 v_c^2
+&= K_2' \begin{bmatrix}\pm 1 \\ \pm 1\end{bmatrix}
+ a_{lng, max} \sqrt{1 - \left(\frac{\frac{d\theta}{ds}v_c^2}{a_{lat,max}}\right)^2} \\
+K_2' K_1 v_c^2
+&= K_2' \begin{bmatrix}\pm 1 \\ \pm 1\end{bmatrix}
+ a_{lng, max} \sqrt{1 - \left(\frac{\frac{d\theta}{ds}v_c^2}{a_{lat,max}}\right)^2} \\
+\left(\frac{K_2' K_1 v_c^2}{K_2' \begin{bmatrix} 1 \\ \pm 1\end{bmatrix}a_{lng,max}}\right)^2
+&= 1 - \left(\frac{\frac{d\theta}{ds}v_c^2}{a_{lat,max}}\right)^2 \\
+v_c &= \min \left(
+\left(\frac{K_2'K_1}{K_2' \begin{bmatrix} 1 \\ \pm 1\end{bmatrix}a_{lng, max}}\right)^2 +
+ \left(\frac{\frac{d\theta}{ds}}{a_{lat,max}}\right)^2
+ \right)^{-\frac14}
+\end{align*}
+
+Where the $\pm 1$ term will have opposite signs of the two coefficients of
+$K_2$ have the same sign, and will have the same sign of the coefficients of
+$K_2$ have opposite signs.
+
+$K_2'$ refers to a transformed version of $K_2$ such that $K_2'K_2 = 0$ (see
+below for exact definition).
+
+\section{Calculating Feasible Voltage Velocities}
+
+Given:
+
+\begin{align*}
+\begin{bmatrix} 1 - r_l \frac{d\theta}{ds} \\ 1 + r_r \frac{d\theta}{ds}
+ \end{bmatrix} \dot{v}_c + \begin{bmatrix} -r_l \\ r_r \end{bmatrix} \frac{d^2\theta}{ds^2} v_c^2
+ &= A \begin{bmatrix} 1 - r_l \frac{d\theta}{ds} \\ 1 + r_r \frac{d\theta}{ds}
+ \end{bmatrix} v_c + B \begin{bmatrix} u_l \\ u_r \end{bmatrix} \\
+\abs{u} &\le V_{max} \\
+\norm{B^{-1} \left(\begin{bmatrix} 1 - r_l \frac{d\theta}{ds} \\ 1 + r_r \frac{d\theta}{ds}
+ \end{bmatrix} \dot{v}_c + \begin{bmatrix} -r_l \\ r_r \end{bmatrix} \frac{d^2\theta}{ds^2} v_c^2
+ - A \begin{bmatrix} 1 - r_l \frac{d\theta}{ds} \\ 1 + r_r \frac{d\theta}{ds}
+ \end{bmatrix} v_c\right) }_\infty \le V_{max} \\
+\end{align}
+
+This is a linear program for fixed $v_c$. We wish to determine the limits of
+$v_c$ for which the linear program will have a solution.
+
+In order to analyze this linear program in $\dot{v}_c$, it is useful to know the
+signs of the coefficients of $\dot{v}_c$, which in this case are
+$B^{-1} \begin{bmatrix}1 - r_l\frac{d\theta}{ds} \\ 1 + r_r \frac{d\theta}{ds}\end{bmatrix}$
+
+If either coefficient is zero, then for any given
+$v_c$, then the linear program
+becomes trivial, as the term with $\dot{v}_c$ will drop out of one of the rows
+and so the other row will be guaranteed to be valid for some $\dot{v}_c$ while
+the row with the dropped $\dot{v}_c$ will either be valid or not. If $B$ were
+the identity matrix, then this would
+correspond to situations where one wheel is stationary and the
+acceleration of that wheel is not affected by the overall acceleration along the
+path. Determining the limits of allowable velocities is then an issue of
+calculating the point at which the stationary wheel goes from being controllable
+to not, for a given overall velocity.
+
+Otherwise, the edge of feasibility will occur
+when the pair of constraints (where one pair is for the left voltage, one for
+the right) cross and so we will have each voltage as $\pm V_{max}$; if
+the signs of the coefficients differ, then we will have
+$u_l = u_r$. Otherwise, $u_l = -u_r$.
+
+For $u_l = u_r$, attempting to maximize $v_c$:
+\begin{align*}
+B^{-1} \left(\begin{bmatrix} 1 - r_l \frac{d\theta}{ds} \\ 1 + r_r \frac{d\theta}{ds}
+ \end{bmatrix} \dot{v}_c + \begin{bmatrix} -r_l \\ r_r \end{bmatrix} \frac{d^2\theta}{ds^2} v_c^2
+ - A \begin{bmatrix} 1 - r_l \frac{d\theta}{ds} \\ 1 + r_r \frac{d\theta}{ds}
+ \end{bmatrix} v_c\right) &= \pm \begin{bmatrix} V_{max} \\ V_{max} \end{bmatrix} \\
+\begin{pmatrix} 1 + r_r\frac{d\theta}{ds} \\ r_l\frac{d\theta}{ds} - 1
+ \end{pmatrix}\left(
+\begin{bmatrix} 1 - r_l \frac{d\theta}{ds} \\ 1 + r_r \frac{d\theta}{ds}
+ \end{bmatrix} \dot{v}_c + \begin{bmatrix} -r_l \\ r_r \end{bmatrix} \frac{d^2\theta}{ds^2} v_c^2
+ - A \begin{bmatrix} 1 - r_l \frac{d\theta}{ds} \\ 1 + r_r \frac{d\theta}{ds}
+ \end{bmatrix} v_c &= \pm B \begin{bmatrix} V_{max} \\ V_{max} \end{bmatrix}
+ \right) \\
+(r_l \frac{d\theta}{ds} - 1) (1 + r_r \frac{d\theta}{ds}) \dot{v}_c +
+ \begin{bmatrix} -r_l - r_lr_r\frac{d\theta}{ds} \\ -r_r +
+ r_lr_r\frac{d\theta}{ds} \end{bmatrix} \frac{d^2\theta}{ds^2} v_c^2
+ - \begin{bmatrix} 1 + r_r \frac{d\theta}{ds} & 0 \\ 0 & r_l \frac{d\theta}{ds} - 1
+ \end{bmatrix} A \begin{bmatrix} 1 - r_l \frac{d\theta}{ds} \\ 1 + r_r \frac{d\theta}{ds}
+ \end{bmatrix} v_c &=
+ \begin{bmatrix} 1 + r_r \frac{d\theta}{ds} & 0 \\ 0 & r_l \frac{d\theta}{ds} - 1
+ \end{bmatrix} B
+ \begin{bmatrix} \pm V_{max} \\ \pm V_{max} \end{bmatrix}
+\end{align*}
+
+\begin{align*}
+ -(r_l + r_r) \frac{d^2\theta}{ds^2} v_c^2
+ - ((1 + r_r\frac{d\theta}{ds})(A_{11}(1 - r_l\frac{d\theta}{ds}) + A_{12}(1 + r_r\frac{d\theta}{ds}))
+ + (r_l\frac{d\theta}{ds} - 1)(A_{12}(1 - r_l\frac{d\theta}{ds}) + A_{11}(1 + r_r\frac{d\theta}{ds}))
+ ) v_c &= \\
+ \pm V_{max}\frac{d\theta}{ds}(B_{12}r_l + B_{11}r_l + B_{11}r_r + B_{12}r_r) \\
+ (r_l + r_r) \frac{d^2\theta}{ds^2} v_c^2 +
+ \frac{d\theta}{ds}( A_{11} (-r_l - r_r) + A_{12}(r_r + r_l) +
+ r_r(A_{11}(1 - r_l\frac{d\theta}{ds}) + A_{12}(1 + r_r\frac{d\theta}{ds})) +
+ r_l(A_{12}(1 - r_l\frac{d\theta}{ds}) + A_{11}(1 + r_r\frac{d\theta}{ds}))
+ ) v_c &= \\
+ (r_l + r_r) \frac{d^2\theta}{ds^2} v_c^2 +
+ \frac{d\theta}{ds}( (r_l + r_r) (A_{12} - A_{11}) +
+ A_{11}(r_l + r_r) + A_{12}(r_l + r_r) +
+ A_{12}\frac{d\theta}{ds}(r_r^2 - r_l^2)) v_c &= \\
+ (r_l + r_r) \left( \frac{d^2\theta}{ds^2} v_c^2 +
+ \frac{d\theta}{ds}A_{12}\left(2 +
+ \frac{d\theta}{ds}(r_r - r_l)\right) v_c\right) &= \\
+ \pm V_{max}(r_l + r_r)\frac{d\theta}{ds}(B_{11} + B_{12}) \\
+ \frac{d^2\theta}{ds^2} v_c^2 +
+ \frac{d\theta}{ds}A_{12}\left(2 + \frac{d\theta}{ds}(r_r - r_l)\right) v_c =
+ \pm V_{max}\frac{d\theta}{ds}(B_{11} + B_{12}) \\
+\end{align*}
+
+Note that when I eliminate parts of $A$ and $B$ I am taking advantage of the
+fact that different rows of $A$ and $B$ are just reordered version of one
+another (i.e., each is of the format $\begin{bmatrix} a & b \\ b & a
+\end{bmatrix}$).
+
+In the final equation, we then execute the quadratic formula for $v_c$.
+Note that if you write
+the quadratic formula out there will be up to 4 real solutions (the two
+solutions to the quadratic formula by the two $\pm V_{max}$ results); because
+of the placement of the $\pm V_{max}$, then
+0, 1, or 3 of the solutions will be greater than zero \footnote{The basic format of
+the quadratic formula
+in this case will be $b \pm \sqrt{b^2 \pm k}$ or some equivalent, and
+$0 < \abs{k} \le b^2$ ($\abs{k} \le b^$ is necessary to have 4 real solutions;
+and $k = 0$ only if
+$b$ also is zero, or if $V_{max} = 0$, which are not particularly
+relevant corner cases). If $b$ is positive,
+then 3 will be positive ($b +$ anything is positive, and since
+$\sqrt{b^2 - k} \le b$, $b - \sqrt{b^2 - k} \ge 0$, while $b - \sqrt{b^2 + k}
+\le 0$). A similar argument holds for negative $b$. If $b = 0$, then if there
+are four real solutions, all four are zero. If there are not four real
+solutions, then $\abs{k} > b^2$ and we have two real solutions. In this case,
+because $\sqrt{b^2 + k} > b$, we will have one positive and one negative
+solution.}. The zero positive solution case is not particularly interesting, as
+it only applies at zero curvature (which cannot apply here, as we already stated
+that $\abs{r\frac{d\theta}{ds}} > 1$ (or, if we have zero $V_{max}$). The one
+or three provide us our cutoffs for where there may be a gap in allowable
+accelerations.
+
+For situations where the coefficients have the same sign, attempting to maximize $v_c$:
+
+\begin{align*}
+(r_l \frac{d\theta}{ds} - 1) (1 + r_r \frac{d\theta}{ds}) \dot{v}_c +
+ \begin{bmatrix} -r_l - r_lr_r\frac{d\theta}{ds} \\ -r_r +
+ r_lr_r\frac{d\theta}{ds} \end{bmatrix} \frac{d^2\theta}{ds^2} v_c^2
+ - \begin{bmatrix} 1 + r_r \frac{d\theta}{ds} & 0 \\ 0 & r_l \frac{d\theta}{ds} - 1
+ \end{bmatrix} A \begin{bmatrix} 1 - r_l \frac{d\theta}{ds} \\ 1 + r_r \frac{d\theta}{ds}
+ \end{bmatrix} v_c &=
+ \begin{bmatrix} 1 + r_r \frac{d\theta}{ds} & 0 \\ 0 & r_l \frac{d\theta}{ds} - 1
+ \end{bmatrix} B
+ \begin{bmatrix} \pm V_{max} \\ \mp V_{max} \end{bmatrix}
+\end{align*}
+
+\begin{align*}
+ (r_l + r_r) \left( \frac{d^2\theta}{ds^2} v_c^2 +
+ \frac{d\theta}{ds}A_{12}\left(2 +
+ \frac{d\theta}{ds}(r_r - r_l)\right) v_c\right) &=
+ \pm V_{max}(B_{11} - B_{12})(2 + \frac{d\theta}{ds}(r_r - r_l)) \\
+ \frac{d^2\theta}{ds^2} v_c^2 +
+ \frac{d\theta}{ds}A_{12}\left(2 +
+ \frac{d\theta}{ds}(r_r - r_l)\right) v_c &=
+ \pm V_{max}\frac{(B_{11} - B_{12})(2 + \frac{d\theta}{ds}(r_r - r_l))}{r_r + r_l}\\
+\end{align*}
+
+Where the same basic rules apply to the quadratic formula solutions.
+
+Considering situations where one of the rows in the equations disappear,
+we end up with the $\dot{v}_c$ term disappearing and
+we must solve the following equation for whichever row of $B^{-1}$ $B^{-1}_N$
+causes the $\dot{v}_c$ term to disappear:
+\begin{align*}
+B^{-1} &= \frac{1}{B_{11}^2 - B_{12}^2}\begin{bmatrix} B_{11} & -B_{12} \\
+ -B_{12} & B_{11} \end{bmatrix} \\
+B^{-1}_N\left(
+ \begin{bmatrix} -r_l \\ r_r \end{bmatrix} \frac{d^2\theta}{ds^2} v_c^2
+ - A \begin{bmatrix} 1 - r_l \frac{d\theta}{ds} \\
+ 1 + r_r \frac{d\theta}{ds} \end{bmatrix} v_c \right)
+ &= \pm V_{max} \\
+\end{align*}
+
+
+Trying to be more general:
+\begin{align*}
+B^{-1} \left(\begin{bmatrix} 1 - r_l \frac{d\theta}{ds} \\ 1 + r_r \frac{d\theta}{ds}
+ \end{bmatrix} \dot{v}_c + \begin{bmatrix} -r_l \\ r_r \end{bmatrix} \frac{d^2\theta}{ds^2} v_c^2
+ - A \begin{bmatrix} 1 - r_l \frac{d\theta}{ds} \\ 1 + r_r \frac{d\theta}{ds}
+ \end{bmatrix} v_c\right) &= \pm \begin{bmatrix} V_{max} \\ V_{max} \end{bmatrix} \\
+B^{-1} \left(K_2 \dot{v}_c + K_1 v_c^2 - A K_2 v_c\right) &=
+ \pm \begin{bmatrix} V_{max} \\ V_{max} \end{bmatrix} \\
+K_2' &= K_2^T \begin{bmatrix} 0 & 1 \\ -1 & 0 \end{bmatrix} \\
+K_2' \left(K_2 \dot{v}_c + K_1 v_c^2 - A K_2 v_c\right) &=
+ K_2' B \begin{bmatrix} \pm 1 \\ \pm 1 \end{bmatrix} V_{max} \\
+K_2' \left(K_2 \dot{v}_c + K_1 v_c^2 - A K_2 v_c\right) &=
+ K_2' B \begin{bmatrix} \pm 1 \\ \pm 1 \end{bmatrix} V_{max} \\
+K_2' \left(K_1 v_c^2 - A K_2 v_c\right) &=
+ K_2' B \begin{bmatrix} \pm 1 \\ \pm 1 \end{bmatrix} V_{max} \\
+ \end{align*}
+
+The quadratic formula coefficients:
+\begin{align*}
+a v_c^2 + b v_c + c &= 0 \\
+a &= K_2'K_1 \\
+b &= -K_2'AK_2 \\
+c &= -K_2' B \begin{bmatrix} \pm 1 \\ \pm 1 \end{bmatrix} V_{max} \\
+b^2 - 4 ac &= (K_2'AK_2)^2 + 4K_2'K_1 K_2'B \begin{bmatrix} \pm 1 \\ \pm 1 \end{bmatrix} V_{max} \\
+\end{align*}
+
+%Interestingly, the solutions for both basically devolve to:
+%\begin{align*}
+% \frac{d^2\theta}{ds^2} v_c^2 + 2 \frac{d\theta}{ds} A_{12} v_c
+% = \pm V_{max}\max(\abs{\frac{d\theta}{ds}}, 1)(B_{11} + B_{12}) \\
+%\end{align*}
+%
+%While not overly interesting, the moments when we will have a split in
+%feasibility will be at:
+%
+%\begin{align*}
+%0 < \abs{4V_{max}\max(\abs{\frac{d\theta}{ds}}, 1)(B_{11} +
+%B_{12})\frac{d^2\theta}{ds^2}} &\le \abs{2\frac{d\theta}{ds} A_{12}} \\
+%\abs{\frac{d\theta}{ds}} &\le 1 : \\
+%0 < \abs{2V_{max}(B_{11} + B_{12})\frac{d^2\theta}{ds^2}} &\le
+%\abs{\frac{d\theta}{ds} A_{12}} \\
+%\abs{\frac{d\theta}{ds}} &> 1 : \\
+%0 < \abs{2V_{max}(B_{11} + B_{12})\frac{d^2\theta}{ds^2}} &\le \abs{A_{12}} \\
+%\end{align*}
+%
+%Note that, in general, $A_{12}$ will be relatively small when compared to
+%$2V_{max}(B_{11} + B_{12})$.
+
+%\begin{align*}
+%B_{12} = B_{21} &, B_{11} = B_{22} \\
+%2 r \frac{d^2\theta}{ds^2} v_c^2 &= \pm 2V_{max}r\frac{d\theta}{ds} (B_{12} - B_{11}) \\
+%v_c &= \sqrt{\pm\frac{V_{max}\frac{d\theta}{ds}}{\frac{d^2\theta}{ds^2}} (B_{12} - B_{11})} \\
+%\end{align*}
+%
+%\begin{align*}
+%\begin{bmatrix} 1 - r^2 \frac{d\theta}{ds}^2 \\ 1 - r^2 \frac{d\theta}{ds}^2
+% \end{bmatrix} \dot{v}_c + \begin{bmatrix} -1 - r\frac{d\theta}{ds} \\ 1 -
+% r\frac{d\theta}{ds} \end{bmatrix} r \frac{d^2\theta}{ds^2} v_c^2
+% - A \begin{bmatrix} 1 - r^2 \frac{d\theta}{ds}^2 \\ 1 - r^2 \frac{d\theta}{ds}^2
+% \end{bmatrix} v_c &= B
+% \begin{bmatrix} \mp V_{max} (1 + r\frac{d\theta}{ds}) \\ \pm V_{max} (1 -
+% r\frac{d\theta}{ds}) \end{bmatrix} \\
+% 2 r \frac{d^2\theta}{ds^2} v_c^2
+% &= \pm V_{max}(B_{21} - B_{22} - B_{11} + B_{12}) \\
+% r \frac{d^2\theta}{ds^2} v_c^2
+% &= \pm V_{max}(B_{12} - B_{11}) \\
+%v_c &= \sqrt{\pm \frac{V_{max}}{r\frac{d^2\theta}{ds^2}} (B_{12} - B_{11})} \\
+%\end{align*}
+
+\end{document}
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.cc b/frc971/control_loops/drivetrain/splinedrivetrain.cc
index c467e1f..bfd0c96 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.cc
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.cc
@@ -73,7 +73,7 @@
case frc971::ConstraintType_CONSTRAINT_TYPE_UNDEFINED:
break;
case frc971::ConstraintType_LONGITUDINAL_ACCELERATION:
- future_trajectory_->set_longitudal_acceleration(constraint.value);
+ future_trajectory_->set_longitudinal_acceleration(constraint.value);
break;
case frc971::ConstraintType_LATERAL_ACCELERATION:
future_trajectory_->set_lateral_acceleration(constraint.value);
diff --git a/frc971/control_loops/drivetrain/trajectory.cc b/frc971/control_loops/drivetrain/trajectory.cc
index ba4c0c2..97662d4 100644
--- a/frc971/control_loops/drivetrain/trajectory.cc
+++ b/frc971/control_loops/drivetrain/trajectory.cc
@@ -18,22 +18,20 @@
const DrivetrainConfig<double> &config, double vmax,
int num_distance)
: spline_(spline),
- velocity_drivetrain_(
- ::std::unique_ptr<StateFeedbackLoop<2, 2, 2, double,
- StateFeedbackHybridPlant<2, 2, 2>,
- HybridKalman<2, 2, 2>>>(
- new StateFeedbackLoop<2, 2, 2, double,
- StateFeedbackHybridPlant<2, 2, 2>,
- HybridKalman<2, 2, 2>>(
- config.make_hybrid_drivetrain_velocity_loop()))),
+ velocity_drivetrain_(::std::unique_ptr<
+ StateFeedbackLoop<2, 2, 2, double, StateFeedbackHybridPlant<2, 2, 2>,
+ HybridKalman<2, 2, 2>>>(
+ new StateFeedbackLoop<2, 2, 2, double,
+ StateFeedbackHybridPlant<2, 2, 2>,
+ HybridKalman<2, 2, 2>>(
+ config.make_hybrid_drivetrain_velocity_loop()))),
robot_radius_l_(config.robot_radius),
robot_radius_r_(config.robot_radius),
- longitudal_acceleration_(3.0),
+ longitudinal_acceleration_(3.0),
lateral_acceleration_(2.0),
Tlr_to_la_((::Eigen::Matrix<double, 2, 2>() << 0.5, 0.5,
-1.0 / (robot_radius_l_ + robot_radius_r_),
- 1.0 / (robot_radius_l_ + robot_radius_r_))
- .finished()),
+ 1.0 / (robot_radius_l_ + robot_radius_r_)).finished()),
Tla_to_lr_(Tlr_to_la_.inverse()),
plan_(num_distance == 0
? ::std::max(100, static_cast<int>(spline_->length() / 0.0025))
@@ -44,56 +42,293 @@
void Trajectory::LateralAccelPass() {
for (size_t i = 0; i < plan_.size(); ++i) {
const double distance = Distance(i);
- plan_[i] = ::std::min(plan_[i], LateralVelocityCurvature(distance));
+ const double velocity_limit = LateralVelocityCurvature(distance);
+ if (velocity_limit < plan_[i]) {
+ plan_[i] = velocity_limit;
+ plan_segment_type_[i] = CURVATURE_LIMITED;
+ }
}
}
-// TODO(austin): Deduplicate this potentially with the backward accel function.
-// Need to sort out how the max velocity limit is going to work since the
-// velocity and acceleration need to match at all points.
-// TODO(austin): Accel check the wheels instead of the center of mass.
-double Trajectory::ForwardAcceleration(const double x, const double v) {
+void Trajectory::VoltageFeasibilityPass(VoltageLimit limit_type) {
+ for (size_t i = 0; i < plan_.size(); ++i) {
+ const double distance = Distance(i);
+ const double velocity_limit = VoltageVelocityLimit(distance, limit_type);
+ if (velocity_limit < plan_[i]) {
+ plan_[i] = velocity_limit;
+ plan_segment_type_[i] = VOLTAGE_LIMITED;
+ }
+ }
+}
+
+double Trajectory::BestAcceleration(double x, double v, bool backwards) {
::Eigen::Matrix<double, 2, 1> K3;
::Eigen::Matrix<double, 2, 1> K4;
::Eigen::Matrix<double, 2, 1> K5;
K345(x, &K3, &K4, &K5);
- const ::Eigen::Matrix<double, 2, 1> C = K3 * v * v + K4 * v;
// Now, solve for all a's and find the best one which meets our criteria.
- double maxa = -::std::numeric_limits<double>::infinity();
+ const ::Eigen::Matrix<double, 2, 1> C = K3 * v * v + K4 * v;
+ double min_voltage_accel = ::std::numeric_limits<double>::infinity();
+ double max_voltage_accel = -min_voltage_accel;
for (const double a : {(voltage_limit_ - C(0, 0)) / K5(0, 0),
(voltage_limit_ - C(1, 0)) / K5(1, 0),
(-voltage_limit_ - C(0, 0)) / K5(0, 0),
(-voltage_limit_ - C(1, 0)) / K5(1, 0)}) {
const ::Eigen::Matrix<double, 2, 1> U = K5 * a + K3 * v * v + K4 * v;
if ((U.array().abs() < voltage_limit_ + 1e-6).all()) {
- maxa = ::std::max(maxa, a);
+ min_voltage_accel = ::std::min(a, min_voltage_accel);
+ max_voltage_accel = ::std::max(a, max_voltage_accel);
}
}
+ double best_accel = backwards ? min_voltage_accel : max_voltage_accel;
- // Then, assume an acceleration oval and stay inside it.
- const double lateral_acceleration = v * v * spline_->DDXY(x).norm();
- const double ellipse_down_shift = longitudal_acceleration_ * 1.0;
- const double ellipse_width_stretch = ::std::sqrt(
- 1.0 / (1.0 - ::std::pow(ellipse_down_shift / (longitudal_acceleration_ +
- ellipse_down_shift),
- 2.0)));
- const double squared =
- 1.0 - ::std::pow(lateral_acceleration / lateral_acceleration_ /
- ellipse_width_stretch,
- 2.0);
- // If we would end up with an imaginary number, cap us at 0 acceleration.
- // TODO(austin): Investigate when this happens, why, and fix it.
- if (squared < 0.0) {
- AOS_LOG(ERROR, "Imaginary %f, maxa: %f, fa(%f, %f) -> 0.0\n", squared, maxa,
- x, v);
- return 0.0;
+ double min_friction_accel, max_friction_accel;
+ FrictionLngAccelLimits(x, v, &min_friction_accel, &max_friction_accel);
+ if (backwards) {
+ best_accel = ::std::max(best_accel, min_friction_accel);
+ } else {
+ best_accel = ::std::min(best_accel, max_friction_accel);
}
- const double longitudal_acceleration =
- ::std::sqrt(::std::abs(squared)) *
- (longitudal_acceleration_ + ellipse_down_shift) -
- ellipse_down_shift;
- return ::std::min(longitudal_acceleration, maxa);
+
+ if (max_friction_accel < min_friction_accel - 0.1 ||
+ best_accel < min_voltage_accel || best_accel > max_voltage_accel) {
+ AOS_LOG(WARNING,
+ "Viable friction limits and viable voltage limits do not overlap (x: "
+ "%f, v: %f, backwards: %d) best_accel = %f, min voltage %f, max "
+ "voltage %f min friction %f max friction %f.\n",
+ x, v, backwards, best_accel, min_voltage_accel, max_voltage_accel,
+ min_friction_accel, max_friction_accel);
+ // Don't actually do anything--this will just result in attempting to drive
+ // higher voltages thatn we have available. In practice, that'll probably
+ // work out fine.
+ }
+
+ return best_accel;
+}
+
+double Trajectory::LateralVelocityCurvature(double distance) const {
+ // To calculate these constraints, we first note that:
+ // wheel accels = K2 * v_robot' + K1 * v_robot^2
+ // All that this logic does is solve for v_robot, leaving v_robot' free,
+ // assuming that the wheels are at their limits.
+ // To do this, we:
+ //
+ // 1) Determine what the wheel accels will be at the limit--since we have
+ // two free variables (v_robot, v_robot'), both wheels will be at their
+ // limits--if in a sufficiently tight turn (such that the signs of the
+ // coefficients of K2 are different), then the wheels will be accelerating
+ // in opposite directions; otherwise, they accelerate in the same direction.
+ // The magnitude of these per-wheel accelerations is a function of velocity,
+ // so it must also be solved for.
+ //
+ // 2) Eliminate that v_robot' term (since we don't care
+ // about it) by multiplying be a "K2prime" term (where K2prime * K2 = 0) on
+ // both sides of the equation.
+ //
+ // 3) Solving the relatively tractable remaining equation, which is
+ // basically just grouping all the terms together in one spot and taking the
+ // 4th root of everything.
+ const double dtheta = spline_->DTheta(distance);
+ const ::Eigen::Matrix<double, 1, 2> K2prime =
+ K2(dtheta).transpose() *
+ (::Eigen::Matrix<double, 2, 2>() << 0, 1, -1, 0).finished();
+ // Calculate whether the wheels are spinning in opposite directions.
+ const bool opposites = K2prime(0) * K2prime(1) < 0;
+ const ::Eigen::Matrix<double, 2, 1> K1calc = K1(spline_->DDTheta(distance));
+ const double lat_accel_squared =
+ ::std::pow(dtheta / lateral_acceleration_, 2);
+ const double curvature_change_term =
+ (K2prime * K1calc).value() /
+ (K2prime *
+ (::Eigen::Matrix<double, 2, 1>() << 1.0, (opposites ? -1.0 : 1.0))
+ .finished() *
+ longitudinal_acceleration_)
+ .value();
+ const double vel_inv = ::std::sqrt(
+ ::std::sqrt(::std::pow(curvature_change_term, 2) + lat_accel_squared));
+ if (vel_inv == 0.0) {
+ return ::std::numeric_limits<double>::infinity();
+ }
+ return 1.0 / vel_inv;
+}
+
+void Trajectory::FrictionLngAccelLimits(double x, double v, double *min_accel,
+ double *max_accel) const {
+ // First, calculate the max longitudinal acceleration that can be achieved
+ // by either wheel given the friction elliipse that we have.
+ const double lateral_acceleration = v * v * spline_->DDXY(x).norm();
+ const double max_wheel_lng_accel_squared =
+ 1.0 - ::std::pow(lateral_acceleration / lateral_acceleration_, 2.0);
+ if (max_wheel_lng_accel_squared < 0.0) {
+ AOS_LOG(DEBUG,
+ "Something (probably Runge-Kutta) queried invalid velocity %f at "
+ "distance %f\n",
+ v, x);
+ // If we encounter this, it means that the Runge-Kutta has attempted to
+ // sample points a bit past the edge of the friction boundary. If so, we
+ // gradually ramp the min/max accels to be more and more incorrect (note
+ // how min_accel > max_accel if we reach this case) to avoid causing any
+ // numerical issues.
+ *min_accel =
+ ::std::sqrt(-max_wheel_lng_accel_squared) * longitudinal_acceleration_;
+ *max_accel = -*min_accel;
+ return;
+ }
+ *min_accel = -::std::numeric_limits<double>::infinity();
+ *max_accel = ::std::numeric_limits<double>::infinity();
+
+ // Calculate max/min accelerations by calculating what the robots overall
+ // longitudinal acceleration would be if each wheel were running at the max
+ // forwards/backwards longitudinal acceleration.
+ const double max_wheel_lng_accel =
+ longitudinal_acceleration_ * ::std::sqrt(max_wheel_lng_accel_squared);
+ const ::Eigen::Matrix<double, 2, 1> K1v2 = K1(spline_->DDTheta(x)) * v * v;
+ const ::Eigen::Matrix<double, 2, 1> K2inv =
+ K2(spline_->DTheta(x)).cwiseInverse();
+ // Store the accelerations of the robot corresponding to each wheel being at
+ // the max/min acceleration. The first coefficient in each vector
+ // corresponds to the left wheel, the second to the right wheel.
+ const ::Eigen::Matrix<double, 2, 1> accels1 =
+ K2inv.array() * (-K1v2.array() + max_wheel_lng_accel);
+ const ::Eigen::Matrix<double, 2, 1> accels2 =
+ K2inv.array() * (-K1v2.array() - max_wheel_lng_accel);
+
+ // If either term is non-finite, that suggests that a term of K2 is zero
+ // (which is physically possible when turning such that one wheel is
+ // stationary), so just ignore that side of the drivetrain.
+ if (::std::isfinite(accels1(0))) {
+ // The inner max/min in this case determines which of the two cases (+ or
+ // - acceleration on the left wheel) we care about--in a sufficiently
+ // tight turning radius, the left hweel may be accelerating backwards when
+ // the robot as a whole accelerates forwards. We then use that
+ // acceleration to bound the min/max accel.
+ *min_accel = ::std::max(*min_accel, ::std::min(accels1(0), accels2(0)));
+ *max_accel = ::std::min(*max_accel, ::std::max(accels1(0), accels2(0)));
+ }
+ // Same logic as previous if-statement, but for the right wheel.
+ if (::std::isfinite(accels1(1))) {
+ *min_accel = ::std::max(*min_accel, ::std::min(accels1(1), accels2(1)));
+ *max_accel = ::std::min(*max_accel, ::std::max(accels1(1), accels2(1)));
+ }
+}
+
+double Trajectory::VoltageVelocityLimit(
+ double distance, VoltageLimit limit_type,
+ Eigen::Matrix<double, 2, 1> *constraint_voltages) const {
+ // To sketch an outline of the math going on here, we start with the basic
+ // dynamics of the robot along the spline:
+ // K2 * v_robot' + K1 * v_robot^2 = A * K2 * v_robot + B * U
+ // We need to determine the maximum v_robot given constrained U and free
+ // v_robot'.
+ // Similarly to the friction constraints, we accomplish this by first
+ // multiplying by a K2prime term to eliminate the v_robot' term.
+ // As with the friction constraints, we also know that the limits will occur
+ // when both sides of the drivetrain are driven at their max magnitude
+ // voltages, although they may be driven at different signs.
+ // Once we determine whether the voltages match signs, we still have to
+ // consider both possible pairings (technically we could probably
+ // predetermine which pairing, e.g. +/- or -/+, we acre about, but we don't
+ // need to).
+ //
+ // For each pairing, we then get to solve a quadratic formula for the robot
+ // velocity at those voltages. This gives us up to 4 solutions, of which
+ // up to 3 will give us positive velocities; each solution velocity
+ // corresponds to a transition from feasibility to infeasibility, where a
+ // velocity of zero is always feasible, and there will always be 0, 1, or 3
+ // positive solutions. Among the positive solutions, we take both the min
+ // and the max--the min will be the highest velocity such that all
+ // velocities between zero and that velocity are valid; the max will be
+ // the highest feasible velocity. Which we return depends on what the
+ // limit_type is.
+ //
+ // Sketching the actual math:
+ // K2 * v_robot' + K1 * v_robot^2 = A * K2 * v_robot +/- B * U_max
+ // K2prime * K1 * v_robot^2 = K2prime * (A * K2 * v_robot +/- B * U_max)
+ // a v_robot^2 + b v_robot +/- c = 0
+ const ::Eigen::Matrix<double, 2, 2> B =
+ velocity_drivetrain_->plant().coefficients().B_continuous;
+ const double dtheta = spline_->DTheta(distance);
+ const ::Eigen::Matrix<double, 2, 1> BinvK2 = B.inverse() * K2(dtheta);
+ // Because voltages can actually impact *both* wheels, in order to determine
+ // whether the voltages will have opposite signs, we need to use B^-1 * K2.
+ const bool opposite_voltages = BinvK2(0) * BinvK2(1) > 0.0;
+ const ::Eigen::Matrix<double, 1, 2> K2prime =
+ K2(dtheta).transpose() *
+ (::Eigen::Matrix<double, 2, 2>() << 0, 1, -1, 0).finished();
+ const double a = K2prime * K1(spline_->DDTheta(distance));
+ const double b = -K2prime *
+ velocity_drivetrain_->plant().coefficients().A_continuous *
+ K2(dtheta);
+ const ::Eigen::Matrix<double, 1, 2> c_coeff = -K2prime * B;
+ // Calculate the "positive" version of the voltage limits we will use.
+ const ::Eigen::Matrix<double, 2, 1> abs_volts =
+ voltage_limit_ *
+ (::Eigen::Matrix<double, 2, 1>() << 1.0, (opposite_voltages ? -1.0 : 1.0))
+ .finished();
+
+ double min_valid_vel = ::std::numeric_limits<double>::infinity();
+ if (limit_type == VoltageLimit::kAggressive) {
+ min_valid_vel = 0.0;
+ }
+ // Iterate over both possibilites for +/- voltage, and solve the quadratic
+ // formula. For every positive solution, adjust the velocity limit
+ // appropriately.
+ for (const double sign : {1.0, -1.0}) {
+ const ::Eigen::Matrix<double, 2, 1> U = sign * abs_volts;
+ const double prev_vel = min_valid_vel;
+ const double c = c_coeff * U;
+ const double determinant = b * b - 4 * a * c;
+ if (a == 0) {
+ // If a == 0, that implies we are on a constant curvature path, in which
+ // case we just have b * v + c = 0.
+ // Note that if -b * c > 0.0, then vel will be greater than zero and b
+ // will be non-zero.
+ if (-b * c > 0.0) {
+ const double vel = -c / b;
+ if (limit_type == VoltageLimit::kConservative) {
+ min_valid_vel = ::std::min(min_valid_vel, vel);
+ } else {
+ min_valid_vel = ::std::max(min_valid_vel, vel);
+ }
+ } else if (b == 0) {
+ // If a and b are zero, then we are travelling in a straight line and
+ // have no voltage-based velocity constraints.
+ min_valid_vel = ::std::numeric_limits<double>::infinity();
+ }
+ } else if (determinant > 0) {
+ const double sqrt_determinant = ::std::sqrt(determinant);
+ const double high_vel = (-b + sqrt_determinant) / (2.0 * a);
+ const double low_vel = (-b - sqrt_determinant) / (2.0 * a);
+ if (low_vel > 0) {
+ if (limit_type == VoltageLimit::kConservative) {
+ min_valid_vel = ::std::min(min_valid_vel, low_vel);
+ } else {
+ min_valid_vel = ::std::max(min_valid_vel, low_vel);
+ }
+ }
+ if (high_vel > 0) {
+ if (limit_type == VoltageLimit::kConservative) {
+ min_valid_vel = ::std::min(min_valid_vel, high_vel);
+ } else {
+ min_valid_vel = ::std::max(min_valid_vel, high_vel);
+ }
+ }
+ } else if (determinant == 0 && -b * a > 0) {
+ const double vel = -b / (2.0 * a);
+ if (vel > 0.0) {
+ if (limit_type == VoltageLimit::kConservative) {
+ min_valid_vel = ::std::min(min_valid_vel, vel);
+ } else {
+ min_valid_vel = ::std::max(min_valid_vel, vel);
+ }
+ }
+ }
+ if (constraint_voltages != nullptr && prev_vel != min_valid_vel) {
+ *constraint_voltages = U;
+ }
+ }
+ return min_valid_vel;
}
void Trajectory::ForwardPass() {
@@ -107,57 +342,13 @@
[this](double x, double v) { return ForwardAcceleration(x, v); },
plan_[i], distance, delta_distance);
- if (new_plan_velocity < plan_[i + 1]) {
+ if (new_plan_velocity <= plan_[i + 1]) {
plan_[i + 1] = new_plan_velocity;
plan_segment_type_[i] = SegmentType::ACCELERATION_LIMITED;
}
}
}
-double Trajectory::BackwardAcceleration(double x, double v) {
- ::Eigen::Matrix<double, 2, 1> K3;
- ::Eigen::Matrix<double, 2, 1> K4;
- ::Eigen::Matrix<double, 2, 1> K5;
- K345(x, &K3, &K4, &K5);
-
- // Now, solve for all a's and find the best one which meets our criteria.
- const ::Eigen::Matrix<double, 2, 1> C = K3 * v * v + K4 * v;
- double mina = ::std::numeric_limits<double>::infinity();
- for (const double a : {(voltage_limit_ - C(0, 0)) / K5(0, 0),
- (voltage_limit_ - C(1, 0)) / K5(1, 0),
- (-voltage_limit_ - C(0, 0)) / K5(0, 0),
- (-voltage_limit_ - C(1, 0)) / K5(1, 0)}) {
- const ::Eigen::Matrix<double, 2, 1> U = K5 * a + K3 * v * v + K4 * v;
- if ((U.array().abs() < voltage_limit_ + 1e-6).all()) {
- mina = ::std::min(mina, a);
- }
- }
-
- // Then, assume an acceleration oval and stay inside it.
- const double lateral_acceleration = v * v * spline_->DDXY(x).norm();
- const double ellipse_down_shift = longitudal_acceleration_ * 1.0;
- const double ellipse_width_stretch = ::std::sqrt(
- 1.0 / (1.0 - ::std::pow(ellipse_down_shift / (longitudal_acceleration_ +
- ellipse_down_shift),
- 2.0)));
- const double squared =
- 1.0 - ::std::pow(lateral_acceleration / lateral_acceleration_ /
- ellipse_width_stretch,
- 2.0);
- // If we would end up with an imaginary number, cap us at 0 acceleration.
- // TODO(austin): Investigate when this happens, why, and fix it.
- if (squared < 0.0) {
- AOS_LOG(ERROR, "Imaginary %f, mina: %f, fa(%f, %f) -> 0.0\n", squared, mina,
- x, v);
- return 0.0;
- }
- const double longitudal_acceleration =
- -::std::sqrt(::std::abs(squared)) *
- (longitudal_acceleration_ + ellipse_down_shift) +
- ellipse_down_shift;
- return ::std::max(longitudal_acceleration, mina);
-}
-
void Trajectory::BackwardPass() {
const double delta_distance = Distance(0) - Distance(1);
plan_.back() = 0.0;
@@ -169,7 +360,7 @@
[this](double x, double v) { return BackwardAcceleration(x, v); },
plan_[i], distance, delta_distance);
- if (new_plan_velocity < plan_[i - 1]) {
+ if (new_plan_velocity <= plan_[i - 1]) {
plan_[i - 1] = new_plan_velocity;
plan_segment_type_[i - 1] = SegmentType::DECELERATION_LIMITED;
}
@@ -195,6 +386,9 @@
double acceleration;
double velocity;
+ // TODO(james): While technically correct for sufficiently small segment
+ // steps, this method of switching between limits has a tendency to produce
+ // sudden jumps in acceelrations, which is undesirable.
switch (plan_segment_type_[DistanceToSegment(distance)]) {
case SegmentType::VELOCITY_LIMITED:
acceleration = 0.0;
@@ -205,9 +399,20 @@
break;
case SegmentType::CURVATURE_LIMITED:
velocity = vcurvature;
- acceleration = 0.0;
+ FrictionLngAccelLimits(distance, velocity, &acceleration, &acceleration);
+ break;
+ case SegmentType::VOLTAGE_LIMITED:
+ // Normally, we expect that voltage limited plans will all get dominated
+ // by the acceleration/deceleration limits. This may not always be true;
+ // if we ever encounter this error, we just need to back out what the
+ // accelerations would be in this case.
+ LOG(FATAL) << "Unexpectedly got VOLTAGE_LIMITED plan.";
break;
case SegmentType::ACCELERATION_LIMITED:
+ // TODO(james): The integration done here and in the DECELERATION_LIMITED
+ // can technically cause us to violate friction constraints. We currently
+ // don't do anything about it to avoid causing sudden jumps in voltage,
+ // but we probably *should* at some point.
velocity = IntegrateAccelForDistance(
[this](double x, double v) { return ForwardAcceleration(x, v); },
plan_[before_index], before_distance, distance - before_distance);
@@ -226,11 +431,6 @@
break;
}
- if (vcurvature < velocity) {
- velocity = vcurvature;
- acceleration = 0.0;
- AOS_LOG(ERROR, "Curvature won\n");
- }
return (::Eigen::Matrix<double, 3, 1>() << distance, velocity, acceleration)
.finished();
}
diff --git a/frc971/control_loops/drivetrain/trajectory.h b/frc971/control_loops/drivetrain/trajectory.h
index 66dcba1..a5b9807 100644
--- a/frc971/control_loops/drivetrain/trajectory.h
+++ b/frc971/control_loops/drivetrain/trajectory.h
@@ -43,9 +43,9 @@
Trajectory(const DistanceSpline *spline,
const DrivetrainConfig<double> &config, double vmax = 10.0,
int num_distance = 0);
- // Sets the plan longitudal acceleration limit
- void set_longitudal_acceleration(double longitudal_acceleration) {
- longitudal_acceleration_ = longitudal_acceleration;
+ // Sets the plan longitudinal acceleration limit
+ void set_longitudinal_acceleration(double longitudinal_acceleration) {
+ longitudinal_acceleration_ = longitudinal_acceleration;
}
// Sets the plan lateral acceleration limit
void set_lateral_acceleration(double lateral_acceleration) {
@@ -56,10 +56,42 @@
voltage_limit_ = voltage_limit;
}
- // Returns the velocity limit for a defined distance.
- double LateralVelocityCurvature(double distance) const {
- return ::std::sqrt(lateral_acceleration_ / spline_->DDXY(distance).norm());
- }
+ // Returns the friction-constrained velocity limit at a given distance along
+ // the path. At the returned velocity, one or both wheels will be on the edge
+ // of slipping.
+ // There are some very disorganized thoughts on the math here and in some of
+ // the other functions in spline_math.tex.
+ double LateralVelocityCurvature(double distance) const;
+
+ // Returns the range of allowable longitudinal accelerations for the center of
+ // the robot at a particular distance (x) along the path and velocity (v).
+ // min_accel and max_accel correspodn to the min/max accelerations that can be
+ // achieved without breaking friction limits on one or both wheels.
+ // If max_accel < min_accel, that implies that v is too high for there to be
+ // any valid acceleration. FrictionLngAccelLimits(x,
+ // LateralVelocityCurvature(x), &min_accel, &max_accel) should result in
+ // min_accel == max_accel.
+ void FrictionLngAccelLimits(double x, double v, double *min_accel,
+ double *max_accel) const;
+
+ enum class VoltageLimit {
+ kConservative,
+ kAggressive,
+ };
+
+ // Calculates the maximum voltage at which we *can* track the path. In some
+ // cases there will be two ranges of feasible velocities for traversing the
+ // path--in such a situation, from zero to velocity A we will be able to track
+ // the path, from velocity A to B we can't, from B to C we can and above C we
+ // can't. If limit_type = kConservative, we return A; if limit_type =
+ // kAggressive, we return C. We currently just use the kConservative limit
+ // because that way we can guarantee that all velocities between zero and A
+ // are allowable and don't have to handle a more complicated planning problem.
+ // constraint_voltages will be populated by the only wheel voltages that are
+ // valid at the returned limit.
+ double VoltageVelocityLimit(
+ double distance, VoltageLimit limit_type,
+ Eigen::Matrix<double, 2, 1> *constraint_voltages = nullptr) const;
// Limits the velocity in the specified segment to the max velocity.
void LimitVelocity(double starting_distance, double ending_distance,
@@ -67,25 +99,28 @@
// Runs the lateral acceleration (curvature) pass on the plan.
void LateralAccelPass();
-
- // Returns the forward acceleration for a distance along the spline taking
- // into account the lateral acceleration, longitudinal acceleration, and
- // voltage limits.
- double ForwardAcceleration(const double x, const double v);
+ void VoltageFeasibilityPass(VoltageLimit limit_type);
// Runs the forwards pass, setting the starting velocity to 0 m/s
void ForwardPass();
- // Returns the backwards acceleration for a distance along the spline taking
- // into account the lateral acceleration, longitudinal acceleration, and
- // voltage limits.
- double BackwardAcceleration(double x, double v);
+ // Returns the forwards/backwards acceleration for a distance along the spline
+ // taking into account the lateral acceleration, longitudinal acceleration,
+ // and voltage limits.
+ double BestAcceleration(double x, double v, bool backwards);
+ double BackwardAcceleration(double x, double v) {
+ return BestAcceleration(x, v, true);
+ }
+ double ForwardAcceleration(double x, double v) {
+ return BestAcceleration(x, v, false);
+ }
// Runs the forwards pass, setting the ending velocity to 0 m/s
void BackwardPass();
// Runs all the planning passes.
void Plan() {
+ VoltageFeasibilityPass(VoltageLimit::kConservative);
LateralAccelPass();
ForwardPass();
BackwardPass();
@@ -167,13 +202,29 @@
VELOCITY_LIMITED,
CURVATURE_LIMITED,
ACCELERATION_LIMITED,
- DECELERATION_LIMITED
+ DECELERATION_LIMITED,
+ VOLTAGE_LIMITED,
};
const ::std::vector<SegmentType> &plan_segment_type() const {
return plan_segment_type_;
}
+ // Returns K1 and K2.
+ // K2 * d^x/dt^2 + K1 (dx/dt)^2 = A * K2 * dx/dt + B * U
+ const ::Eigen::Matrix<double, 2, 1> K1(double current_ddtheta) const {
+ return (::Eigen::Matrix<double, 2, 1>()
+ << -robot_radius_l_ * current_ddtheta,
+ robot_radius_r_ * current_ddtheta).finished();
+ }
+
+ const ::Eigen::Matrix<double, 2, 1> K2(double current_dtheta) const {
+ return (::Eigen::Matrix<double, 2, 1>()
+ << 1.0 - robot_radius_l_ * current_dtheta,
+ 1.0 + robot_radius_r_ * current_dtheta)
+ .finished();
+ }
+
private:
// Computes alpha for a distance.
size_t DistanceToSegment(double distance) const {
@@ -184,22 +235,6 @@
(plan_.size() - 1)))));
}
- // Returns K1 and K2.
- // K2 * d^x/dt^2 + K1 (dx/dt)^2 = A * K2 * dx/dt + B * U
- const ::Eigen::Matrix<double, 2, 1> K1(double current_ddtheta) const {
- return (::Eigen::Matrix<double, 2, 1>()
- << -robot_radius_l_ * current_ddtheta,
- robot_radius_r_ * current_ddtheta)
- .finished();
- }
-
- const ::Eigen::Matrix<double, 2, 1> K2(double current_dtheta) const {
- return (::Eigen::Matrix<double, 2, 1>()
- << 1.0 - robot_radius_l_ * current_dtheta,
- 1.0 + robot_radius_r_ * current_dtheta)
- .finished();
- }
-
// Computes K3, K4, and K5 for the provided distance.
// K5 a + K3 v^2 + K4 v = U
void K345(const double x, ::Eigen::Matrix<double, 2, 1> *K3,
@@ -229,11 +264,11 @@
HybridKalman<2, 2, 2>>>
velocity_drivetrain_;
- // Left and right robot radiuses.
+ // Robot radiuses.
const double robot_radius_l_;
const double robot_radius_r_;
// Acceleration limits.
- double longitudal_acceleration_;
+ double longitudinal_acceleration_;
double lateral_acceleration_;
// Transformation matrix from left, right to linear, angular
const ::Eigen::Matrix<double, 2, 2> Tlr_to_la_;
diff --git a/frc971/control_loops/drivetrain/trajectory_plot.cc b/frc971/control_loops/drivetrain/trajectory_plot.cc
index 2a990f9..60bd21f 100644
--- a/frc971/control_loops/drivetrain/trajectory_plot.cc
+++ b/frc971/control_loops/drivetrain/trajectory_plot.cc
@@ -57,7 +57,7 @@
&distance_spline,
::y2019::control_loops::drivetrain::GetDrivetrainConfig());
trajectory.set_lateral_acceleration(2.0);
- trajectory.set_longitudal_acceleration(1.0);
+ trajectory.set_longitudinal_acceleration(1.0);
// Grab the spline.
::std::vector<double> distances = trajectory.Distances();
@@ -76,6 +76,8 @@
// Compute the velocity plan.
::aos::monotonic_clock::time_point start_time = ::aos::monotonic_clock::now();
::std::vector<double> initial_plan = trajectory.plan();
+ trajectory.VoltageFeasibilityPass(Trajectory::VoltageLimit::kConservative);
+ ::std::vector<double> voltage_plan = trajectory.plan();
trajectory.LateralAccelPass();
::std::vector<double> curvature_plan = trajectory.plan();
trajectory.ForwardPass();
diff --git a/frc971/control_loops/drivetrain/trajectory_test.cc b/frc971/control_loops/drivetrain/trajectory_test.cc
index f65adf1..a443b1e 100644
--- a/frc971/control_loops/drivetrain/trajectory_test.cc
+++ b/frc971/control_loops/drivetrain/trajectory_test.cc
@@ -4,6 +4,7 @@
#include <vector>
#include "aos/testing/test_shm.h"
+#include "frc971/control_loops/drivetrain/drivetrain_test_lib.h"
#include "gflags/gflags.h"
#include "gtest/gtest.h"
#if defined(SUPPORT_PLOT)
@@ -14,8 +15,9 @@
#include "y2016/control_loops/drivetrain/hybrid_velocity_drivetrain.h"
#include "y2016/control_loops/drivetrain/kalman_drivetrain_motor_plant.h"
#include "y2016/control_loops/drivetrain/polydrivetrain_dog_motor_plant.h"
+#include "y2019/control_loops/drivetrain/drivetrain_base.h"
-DEFINE_bool(plot, false, "If true, plot");
+DECLARE_bool(plot);
namespace frc971 {
namespace control_loops {
@@ -37,47 +39,10 @@
EXPECT_NEAR(4.0, v, 1e-8);
}
-const constants::ShifterHallEffect kThreeStateDriveShifter{0.0, 0.0, 0.25,
- 0.75};
-
-// TODO(austin): factor this out of drivetrain_lib_test.cc
-const DrivetrainConfig<double> &GetDrivetrainConfig() {
- static DrivetrainConfig<double> kDrivetrainConfig{
- ::frc971::control_loops::drivetrain::ShifterType::HALL_EFFECT_SHIFTER,
- ::frc971::control_loops::drivetrain::LoopType::CLOSED_LOOP,
- ::frc971::control_loops::drivetrain::GyroType::SPARTAN_GYRO,
- IMUType::IMU_X,
- ::y2016::control_loops::drivetrain::MakeDrivetrainLoop,
- ::y2016::control_loops::drivetrain::MakeVelocityDrivetrainLoop,
- ::y2016::control_loops::drivetrain::MakeKFDrivetrainLoop,
- ::y2016::control_loops::drivetrain::MakeHybridVelocityDrivetrainLoop,
-
- chrono::duration_cast<chrono::nanoseconds>(
- chrono::duration<double>(::y2016::control_loops::drivetrain::kDt)),
- ::y2016::control_loops::drivetrain::kRobotRadius,
- ::y2016::control_loops::drivetrain::kWheelRadius,
- ::y2016::control_loops::drivetrain::kV,
-
- ::y2016::control_loops::drivetrain::kHighGearRatio,
- ::y2016::control_loops::drivetrain::kLowGearRatio,
- ::y2016::control_loops::drivetrain::kJ,
- ::y2016::control_loops::drivetrain::kMass,
- kThreeStateDriveShifter,
- kThreeStateDriveShifter,
- false,
- 0,
-
- 0.25,
- 1.00,
- 1.00};
-
- return kDrivetrainConfig;
-};
-
struct SplineTestParams {
- ::Eigen::Matrix<double, 2, 4> control_points;
+ ::Eigen::Matrix<double, 2, 6> control_points;
double lateral_acceleration;
- double longitudal_acceleration;
+ double longitudinal_acceleration;
double velocity_limit;
double voltage_limit;
::std::function<void(Trajectory *)> trajectory_modification_fn;
@@ -89,29 +54,35 @@
: public ::testing::TestWithParam<SplineTestParams> {
public:
::aos::testing::TestSharedMemory shm_;
- static constexpr chrono::nanoseconds kDt =
- chrono::duration_cast<chrono::nanoseconds>(
- chrono::duration<double>(::y2016::control_loops::drivetrain::kDt));
::std::unique_ptr<DistanceSpline> distance_spline_;
::std::unique_ptr<Trajectory> trajectory_;
::std::vector<::Eigen::Matrix<double, 3, 1>> length_plan_xva_;
- ParameterizedSplineTest() {}
+ ParameterizedSplineTest()
+ : dt_config_(GetTestDrivetrainConfig()) {}
void SetUp() {
distance_spline_ = ::std::unique_ptr<DistanceSpline>(
- new DistanceSpline(Spline(Spline4To6(GetParam().control_points))));
+ new DistanceSpline(Spline(GetParam().control_points)));
+ // Run lots of steps to make the feedforwards terms more accurate.
trajectory_ = ::std::unique_ptr<Trajectory>(
- new Trajectory(distance_spline_.get(), GetDrivetrainConfig(),
+ new Trajectory(distance_spline_.get(), dt_config_,
GetParam().velocity_limit));
trajectory_->set_lateral_acceleration(GetParam().lateral_acceleration);
- trajectory_->set_longitudal_acceleration(GetParam().longitudal_acceleration);
+ trajectory_->set_longitudinal_acceleration(
+ GetParam().longitudinal_acceleration);
trajectory_->set_voltage_limit(GetParam().voltage_limit);
GetParam().trajectory_modification_fn(trajectory_.get());
initial_plan_ = trajectory_->plan();
+ trajectory_->VoltageFeasibilityPass(
+ Trajectory::VoltageLimit::kAggressive);
+ aggressive_voltage_plan_ = trajectory_->plan();
+ trajectory_->VoltageFeasibilityPass(
+ Trajectory::VoltageLimit::kConservative);
+ voltage_plan_ = trajectory_->plan();
trajectory_->LateralAccelPass();
curvature_plan_ = trajectory_->plan();
trajectory_->ForwardPass();
@@ -119,18 +90,20 @@
trajectory_->BackwardPass();
backward_plan_ = trajectory_->plan();
- length_plan_xva_ = trajectory_->PlanXVA(kDt);
+ length_plan_xva_ = trajectory_->PlanXVA(dt_config_.dt);
}
void TearDown() {
printf(" Spline takes %f seconds to follow\n",
- length_plan_xva_.size() * ::y2016::control_loops::drivetrain::kDt);
+ length_plan_xva_.size() *
+ ::aos::time::DurationInSeconds(dt_config_.dt));
#if defined(SUPPORT_PLOT)
if (FLAGS_plot) {
::std::vector<double> distances = trajectory_->Distances();
for (size_t i = 0; i < length_plan_xva_.size(); ++i) {
- length_plan_t_.push_back(i * ::y2016::control_loops::drivetrain::kDt);
+ length_plan_t_.push_back(i *
+ ::aos::time::DurationInSeconds(dt_config_.dt));
length_plan_x_.push_back(length_plan_xva_[i](0));
length_plan_v_.push_back(length_plan_xva_[i](1));
length_plan_a_.push_back(length_plan_xva_[i](2));
@@ -153,6 +126,8 @@
matplotlibcpp::plot(distances, backward_plan_, {{"label", "backward"}});
matplotlibcpp::plot(distances, forward_plan_, {{"label", "forward"}});
matplotlibcpp::plot(distances, curvature_plan_, {{"label", "lateral"}});
+ matplotlibcpp::plot(distances, aggressive_voltage_plan_, {{"label", "aggressive voltage"}});
+ matplotlibcpp::plot(distances, voltage_plan_, {{"label", "voltage"}});
matplotlibcpp::plot(distances, initial_plan_, {{"label", "initial"}});
matplotlibcpp::legend();
@@ -169,6 +144,7 @@
#endif
}
+ const DrivetrainConfig<double> dt_config_;
static constexpr double kXPos = 0.05;
static constexpr double kYPos = 0.05;
static constexpr double kThetaPos = 0.2;
@@ -192,6 +168,8 @@
::std::vector<double> curvature_plan_;
::std::vector<double> forward_plan_;
::std::vector<double> backward_plan_;
+ ::std::vector<double> voltage_plan_;
+ ::std::vector<double> aggressive_voltage_plan_;
::std::vector<double> length_plan_t_;
::std::vector<double> length_plan_x_;
@@ -202,12 +180,174 @@
};
-constexpr chrono::nanoseconds ParameterizedSplineTest::kDt;
+// Tests that the VoltageVelocityLimit function produces correct results by
+// calculating the limit at a variety of points and then ensuring that we can
+// indeed drive +/- 12V at the limit and that we can't go faster.
+TEST_P(ParameterizedSplineTest, VoltageFeasibilityCheck) {
+ for (size_t i = 0; i < length_plan_xva_.size(); ++i) {
+ const double distance = length_plan_xva_[i](0);
+ ::Eigen::Matrix<double, 2, 1> U;
+ const ::Eigen::Matrix<double, 2, 1> K2 =
+ trajectory_->K2(distance_spline_->DTheta(distance));
+ const ::Eigen::Matrix<double, 2, 1> K1 =
+ trajectory_->K1(distance_spline_->DDTheta(distance));
+ const ::Eigen::Matrix<double, 2, 2> A =
+ trajectory_->velocity_drivetrain().plant().coefficients().A_continuous;
+ const ::Eigen::Matrix<double, 2, 2> B =
+ trajectory_->velocity_drivetrain().plant().coefficients().B_continuous;
+ const double conservative_limit = trajectory_->VoltageVelocityLimit(
+ distance, Trajectory::VoltageLimit::kConservative, &U);
+ ASSERT_LT(0.0, conservative_limit)
+ << "Voltage limit should be strictly positive.";
+ const bool on_straight_line = distance_spline_->DTheta(distance) == 0 &&
+ distance_spline_->DDTheta(distance) == 0;
+ if (on_straight_line) {
+ EXPECT_EQ(::std::numeric_limits<double>::infinity(), conservative_limit);
+ } else {
+ const ::Eigen::Matrix<double, 2, 1> wheel_accel =
+ A * K2 * conservative_limit + B * U;
+ // TODO(james): Technically, K2 can contain zeros.
+ const ::Eigen::Matrix<double, 2, 1> implied_accels =
+ K2.cwiseInverse().cwiseProduct(
+ wheel_accel - K1 * ::std::pow(conservative_limit, 2));
+ EXPECT_NEAR(implied_accels(0), implied_accels(1), 1e-9);
+ EXPECT_TRUE((GetParam().voltage_limit == U.array().abs()).all()) << U;
+ const double accel = implied_accels(0);
+ // Check that we really are at a limit by confirming that even slightly
+ // disturbing the acceleration in either direction would result in invalid
+ // accelerations.
+ for (const double perturbed_accel : {accel - 1e-5, accel + 1e-5}) {
+ const ::Eigen::Matrix<double, 2, 1> perturbed_wheel_accels =
+ K2 * perturbed_accel + K1 * conservative_limit * conservative_limit;
+ const ::Eigen::Matrix<double, 2, 1> perturbed_voltages =
+ B.inverse() *
+ (perturbed_wheel_accels - A * K2 * conservative_limit);
+ EXPECT_GT(perturbed_voltages.lpNorm<::Eigen::Infinity>(),
+ GetParam().voltage_limit)
+ << "We were able to perturb the voltage!!.";
+ }
+ }
+ length_plan_vl_.push_back(U(0));
+ length_plan_vr_.push_back(U(1));
+
+ // And check the same for the "aggressive" configuration:
+ {
+ const double aggressive_limit = trajectory_->VoltageVelocityLimit(
+ distance, Trajectory::VoltageLimit::kAggressive, &U);
+ if (on_straight_line) {
+ EXPECT_EQ(::std::numeric_limits<double>::infinity(),
+ aggressive_limit);
+ continue;
+ }
+ EXPECT_TRUE((GetParam().voltage_limit == U.array().abs()).all()) << U;
+ const ::Eigen::Matrix<double, 2, 1> wheel_accel =
+ A * K2 * aggressive_limit + B * U;
+ const ::Eigen::Matrix<double, 2, 1> implied_accels =
+ K2.cwiseInverse().cwiseProduct(wheel_accel -
+ K1 * ::std::pow(aggressive_limit, 2));
+ EXPECT_NEAR(implied_accels(0), implied_accels(1), 1e-9);
+ EXPECT_LE(conservative_limit, aggressive_limit)
+ << "The aggressive velocity limit should not be less than the "
+ "conservative.";
+ const double accel = implied_accels(0);
+ for (const double perturbed_accel : {accel - 1e-5, accel + 1e-5}) {
+ const ::Eigen::Matrix<double, 2, 1> perturbed_wheel_accels =
+ K2 * perturbed_accel + K1 * aggressive_limit * aggressive_limit;
+ const ::Eigen::Matrix<double, 2, 1> perturbed_voltages =
+ B.inverse() *
+ (perturbed_wheel_accels - A * K2 * aggressive_limit);
+ EXPECT_GT(perturbed_voltages.lpNorm<::Eigen::Infinity>(),
+ GetParam().voltage_limit)
+ << "We were able to perturb the voltage!!.";
+ }
+ }
+
+ }
+}
+
+// Tests that the friction-based velocity limits are correct.
+TEST_P(ParameterizedSplineTest, FrictionLimitCheck) {
+ // To do this check, retrieve the lateral acceleration velocity limit and
+ // confirm that we can indeed travel at that velocity without violating
+ // friction constraints and that we cannot go any faster.
+ for (size_t i = 0; i < length_plan_xva_.size(); ++i) {
+ const double distance = length_plan_xva_[i](0);
+ const ::Eigen::Matrix<double, 2, 1> K2 =
+ trajectory_->K2(distance_spline_->DTheta(distance));
+ const ::Eigen::Matrix<double, 2, 1> K1 =
+ trajectory_->K1(distance_spline_->DDTheta(distance));
+ const ::Eigen::Matrix<double, 2, 2> A =
+ trajectory_->velocity_drivetrain().plant().coefficients().A_continuous;
+ const ::Eigen::Matrix<double, 2, 2> B =
+ trajectory_->velocity_drivetrain().plant().coefficients().B_continuous;
+ const bool on_straight_line = distance_spline_->DTheta(distance) == 0 &&
+ distance_spline_->DDTheta(distance) == 0;
+ const double velocity_limit =
+ trajectory_->LateralVelocityCurvature(distance);
+ ASSERT_LT(0.0, velocity_limit)
+ << "Acceleration limit should be strictly positive.";
+ if (on_straight_line) {
+ EXPECT_EQ(::std::numeric_limits<double>::infinity(), velocity_limit);
+ continue;
+ }
+ const double lat_accel =
+ velocity_limit * velocity_limit * distance_spline_->DTheta(distance);
+ const double allowed_lng_accel =
+ GetParam().longitudinal_acceleration *
+ ::std::sqrt(1.0 -
+ ::std::pow(lat_accel / GetParam().lateral_acceleration, 2));
+ ::Eigen::Matrix<double, 2, 1> wheel_accels;
+ wheel_accels << 1.0, ((K2(0) * K2(1) > 0.0) ? -1.0 : 1.0);
+ wheel_accels *= allowed_lng_accel;
+ const ::Eigen::Matrix<double, 2, 1> implied_accels1 =
+ K2.cwiseInverse().cwiseProduct(wheel_accels -
+ K1 * ::std::pow(velocity_limit, 2));
+ const ::Eigen::Matrix<double, 2, 1> implied_accels2 =
+ K2.cwiseInverse().cwiseProduct(-wheel_accels -
+ K1 * ::std::pow(velocity_limit, 2));
+ const double accels1_err =
+ ::std::abs(implied_accels1(0) - implied_accels1(1));
+ const double accels2_err =
+ ::std::abs(implied_accels2(0) - implied_accels2(1));
+ EXPECT_LT(::std::min(accels1_err, accels2_err), 1e-10);
+ const double implied_accel =
+ (accels1_err < accels2_err) ? implied_accels1(0) : implied_accels2(0);
+ // Confirm that we are indeed on the edge of feasibility by testing that
+ // we can't accelerate any faste/slower at this velocity without violating
+ // acceleration constraints.
+ for (const double perturbed_accel :
+ {implied_accel - 1e-5, implied_accel + 1e-5}) {
+ const ::Eigen::Matrix<double, 2, 1> perturbed_wheel_accels =
+ K2 * perturbed_accel + K1 * velocity_limit * velocity_limit;
+ EXPECT_GT(perturbed_wheel_accels.lpNorm<::Eigen::Infinity>(),
+ allowed_lng_accel)
+ << "We were able to perturb the acceleration!!.";
+ }
+ {
+ const ::Eigen::Matrix<double, 2, 1> U =
+ B.inverse() * (wheel_accels - A * K2 * velocity_limit);
+ length_plan_vl_.push_back(U(0));
+ length_plan_vr_.push_back(U(1));
+ }
+
+ {
+ // Also test the utility function for determining the acceleration
+ // limits:
+ double min_accel, max_accel;
+ trajectory_->FrictionLngAccelLimits(distance, velocity_limit, &min_accel,
+ &max_accel);
+ // If on the limit, the min/max acceleration limits should be identical.
+ EXPECT_NEAR(min_accel, max_accel, 1e-10);
+ EXPECT_NEAR(implied_accel, min_accel, 1e-10);
+ }
+ }
+}
// Tests that following a spline with feed forwards only gets pretty darn close
// to the right point.
TEST_P(ParameterizedSplineTest, FFSpline) {
::Eigen::Matrix<double, 5, 1> state = ::Eigen::Matrix<double, 5, 1>::Zero();
+ state = trajectory_->GoalState(0.0, 0.0);
for (size_t i = 0; i < length_plan_xva_.size(); ++i) {
const double distance = length_plan_xva_[i](0);
@@ -223,11 +363,11 @@
return ContinuousDynamics(trajectory_->velocity_drivetrain().plant(),
trajectory_->Tlr_to_la(), X, U);
},
- state, U, ::y2016::control_loops::drivetrain::kDt);
+ state, U, ::aos::time::DurationInSeconds(dt_config_.dt));
}
EXPECT_LT((state - trajectory_->GoalState(trajectory_->length(), 0.0)).norm(),
- 2e-2);
+ 4e-2);
}
// Tests that following a spline with both feed forwards and feed back gets
@@ -244,7 +384,7 @@
const ::Eigen::Matrix<double, 5, 1> state_error = goal_state - state;
const ::Eigen::Matrix<double, 2, 5> K =
- trajectory_->KForState(state, ParameterizedSplineTest::kDt, Q, R);
+ trajectory_->KForState(state, dt_config_.dt, Q, R);
const ::Eigen::Matrix<double, 2, 1> U_ff = trajectory_->FFVoltage(distance);
const ::Eigen::Matrix<double, 2, 1> U_fb = K * state_error;
@@ -258,18 +398,22 @@
return ContinuousDynamics(trajectory_->velocity_drivetrain().plant(),
trajectory_->Tlr_to_la(), X, U);
},
- state, U, ::y2016::control_loops::drivetrain::kDt);
+ state, U, ::aos::time::DurationInSeconds(dt_config_.dt));
}
EXPECT_LT((state - trajectory_->GoalState(trajectory_->length(), 0.0)).norm(),
- 1.2e-2);
+ 2e-2);
}
// Tests that Iteratively computing the XVA plan is the same as precomputing it.
TEST_P(ParameterizedSplineTest, IterativeXVA) {
+ for (double v : trajectory_->plan()) {
+ EXPECT_TRUE(::std::isfinite(v));
+ }
::Eigen::Matrix<double, 2, 1> state = ::Eigen::Matrix<double, 2, 1>::Zero();
for (size_t i = 1; i < length_plan_xva_.size(); ++i) {
- ::Eigen::Matrix<double, 3, 1> xva = trajectory_->GetNextXVA(kDt, &state);
+ ::Eigen::Matrix<double, 3, 1> xva =
+ trajectory_->GetNextXVA(dt_config_.dt, &state);
EXPECT_LT((length_plan_xva_[i] - xva).norm(), 1e-2);
}
}
@@ -289,54 +433,69 @@
INSTANTIATE_TEST_CASE_P(
SplineTest, ParameterizedSplineTest,
::testing::Values(
- // Normal spline.
- MakeSplineTestParams({(::Eigen::Matrix<double, 2, 4>() << 0.0, 1.2,
- -0.2, 1.0, 0.0, 0.0, 1.0, 1.0)
- .finished(),
- 2.0 /*lateral acceleration*/,
- 1.0 /*longitudinal acceleration*/,
- 10.0 /* velocity limit */, 12.0 /* volts */,
- NullTrajectoryModificationFunction}),
+ MakeSplineTestParams(
+ {Spline4To6((::Eigen::Matrix<double, 2, 4>() << 0.0, 1.2, -0.2, 1.0,
+ 0.0, 0.0, 1.0, 1.0)
+ .finished()),
+ 2.0 /*lateral acceleration*/, 1.0 /*longitudinal acceleration*/,
+ 10.0 /* velocity limit */, 12.0 /* volts */,
+ NullTrajectoryModificationFunction}),
// Be velocity limited.
- MakeSplineTestParams({(::Eigen::Matrix<double, 2, 4>() << 0.0, 6.0,
- -1.0, 5.0, 0.0, 0.0, 1.0, 1.0)
- .finished(),
- 2.0 /*lateral acceleration*/,
- 1.0 /*longitudinal acceleration*/,
- 0.5 /* velocity limit */, 12.0 /* volts */,
- NullTrajectoryModificationFunction}),
+ MakeSplineTestParams(
+ {Spline4To6((::Eigen::Matrix<double, 2, 4>() << 0.0, 6.0, -1.0, 5.0,
+ 0.0, 0.0, 1.0, 1.0)
+ .finished()),
+ 2.0 /*lateral acceleration*/, 1.0 /*longitudinal acceleration*/,
+ 0.5 /* velocity limit */, 12.0 /* volts */,
+ NullTrajectoryModificationFunction}),
// Hit the voltage limit.
- MakeSplineTestParams({(::Eigen::Matrix<double, 2, 4>() << 0.0, 6.0,
- -1.0, 5.0, 0.0, 0.0, 1.0, 1.0)
- .finished(),
- 2.0 /*lateral acceleration*/,
- 3.0 /*longitudinal acceleration*/,
- 10.0 /* velocity limit */, 5.0 /* volts */,
- NullTrajectoryModificationFunction}),
+ MakeSplineTestParams(
+ {Spline4To6((::Eigen::Matrix<double, 2, 4>() << 0.0, 6.0, -1.0, 5.0,
+ 0.0, 0.0, 1.0, 1.0)
+ .finished()),
+ 2.0 /*lateral acceleration*/, 3.0 /*longitudinal acceleration*/,
+ 10.0 /* velocity limit */, 5.0 /* volts */,
+ NullTrajectoryModificationFunction}),
// Hit the curvature limit.
- MakeSplineTestParams({(::Eigen::Matrix<double, 2, 4>() << 0.0, 1.2,
- -0.2, 1.0, 0.0, 0.0, 1.0, 1.0)
- .finished(),
- 1.0 /*lateral acceleration*/,
- 3.0 /*longitudinal acceleration*/,
- 10.0 /* velocity limit */, 12.0 /* volts */,
- NullTrajectoryModificationFunction}),
+ MakeSplineTestParams(
+ {Spline4To6((::Eigen::Matrix<double, 2, 4>() << 0.0, 1.2, -0.2, 1.0,
+ 0.0, 0.0, 1.0, 1.0)
+ .finished()),
+ 1.0 /*lateral acceleration*/, 3.0 /*longitudinal acceleration*/,
+ 10.0 /* velocity limit */, 12.0 /* volts */,
+ NullTrajectoryModificationFunction}),
// Add an artifical velocity limit in the middle.
- MakeSplineTestParams({(::Eigen::Matrix<double, 2, 4>() << 0.0, 6.0,
- -1.0, 5.0, 0.0, 0.0, 1.0, 1.0)
- .finished(),
- 2.0 /*lateral acceleration*/,
- 3.0 /*longitudinal acceleration*/,
- 10.0 /* velocity limit */, 12.0 /* volts */,
- LimitMiddleOfPathTrajectoryModificationFunction}),
+ MakeSplineTestParams(
+ {Spline4To6((::Eigen::Matrix<double, 2, 4>() << 0.0, 6.0, -1.0, 5.0,
+ 0.0, 0.0, 1.0, 1.0)
+ .finished()),
+ 2.0 /*lateral acceleration*/, 3.0 /*longitudinal acceleration*/,
+ 10.0 /* velocity limit */, 12.0 /* volts */,
+ LimitMiddleOfPathTrajectoryModificationFunction}),
// Add a really short artifical velocity limit in the middle.
- MakeSplineTestParams({(::Eigen::Matrix<double, 2, 4>() << 0.0, 6.0,
- -1.0, 5.0, 0.0, 0.0, 1.0, 1.0)
- .finished(),
- 2.0 /*lateral acceleration*/,
- 3.0 /*longitudinal acceleration*/,
- 10.0 /* velocity limit */, 12.0 /* volts */,
- ShortLimitMiddleOfPathTrajectoryModificationFunction})));
+ MakeSplineTestParams(
+ {Spline4To6((::Eigen::Matrix<double, 2, 4>() << 0.0, 6.0, -1.0, 5.0,
+ 0.0, 0.0, 1.0, 1.0)
+ .finished()),
+ 2.0 /*lateral acceleration*/, 3.0 /*longitudinal acceleration*/,
+ 10.0 /* velocity limit */, 12.0 /* volts */,
+ ShortLimitMiddleOfPathTrajectoryModificationFunction}),
+ // Spline known to have caused issues in the past.
+ MakeSplineTestParams(
+ {(::Eigen::Matrix<double, 2, 6>() << 0.5f, 3.5f, 4.0f, 8.0f, 10.0f,
+ 10.2f, 1.0f, 1.0f, -3.0f, -2.0f, -3.5f, -3.65f)
+ .finished(),
+ 2.0 /*lateral acceleration*/, 3.0 /*longitudinal acceleration*/,
+ 200.0 /* velocity limit */, 12.0 /* volts */,
+ NullTrajectoryModificationFunction}),
+ // Perfectly straight line (to check corner cases).
+ MakeSplineTestParams(
+ {Spline4To6((::Eigen::Matrix<double, 2, 4>() << 0.0, 1.0, 2.0, 3.0,
+ 0.0, 0.0, 0.0, 0.0)
+ .finished()),
+ 2.0 /*lateral acceleration*/, 3.0 /*longitudinal acceleration*/,
+ 200.0 /* velocity limit */, 12.0 /* volts */,
+ NullTrajectoryModificationFunction})));
// TODO(austin): Handle saturation. 254 does this by just not going that
// fast... We want to maybe replan when we get behind, or something. Maybe
diff --git a/frc971/control_loops/python/lib_spline_test.py b/frc971/control_loops/python/lib_spline_test.py
index 4e725a0..d544cbb 100644
--- a/frc971/control_loops/python/lib_spline_test.py
+++ b/frc971/control_loops/python/lib_spline_test.py
@@ -32,7 +32,7 @@
trajectory = Trajectory(dSpline)
trajectory.Plan()
plan = trajectory.GetPlanXVA(5.05*1e-3)
- assert(plan.shape == (3, 611))
+ self.assertEqual(plan.shape, (3, 617))
def testLimits(self):
""" A plan with a lower limit should take longer. """
@@ -44,7 +44,7 @@
trajectory.LimitVelocity(0, trajectory.Length(), 3)
trajectory.Plan()
plan = trajectory.GetPlanXVA(5.05*1e-3)
- self.assertEqual(plan.shape, (3, 644))
+ self.assertEqual(plan.shape, (3, 650))
if __name__ == '__main__':
diff --git a/y2019/control_loops/drivetrain/localizer_test.cc b/y2019/control_loops/drivetrain/localizer_test.cc
index b126c6c..fbce275 100644
--- a/y2019/control_loops/drivetrain/localizer_test.cc
+++ b/y2019/control_loops/drivetrain/localizer_test.cc
@@ -588,7 +588,7 @@
.finished(),
/*noisify=*/true,
/*disturb=*/false,
- /*estimate_tolerance=*/0.2,
+ /*estimate_tolerance=*/0.4,
/*goal_tolerance=*/0.4,
}),
// Repeats perfect scenario, but add initial estimator error.
@@ -633,7 +633,7 @@
.finished(),
/*noisify=*/false,
/*disturb=*/true,
- /*estimate_tolerance=*/2.5e-2,
+ /*estimate_tolerance=*/3e-2,
/*goal_tolerance=*/0.15,
}),
// Add noise and some initial error in addition: