Derivations
Variable Name |
Symbol |
|---|---|
Configuration |
\(q\) |
Configuration displacement |
\(\Delta q\) |
Integration timestep |
\(dt\) |
Velocity in tangent space |
\(v = \frac{\Delta q}{dt}\) |
Configuration limits |
\(q_{\text{min}}, q_{\text{max}}\) |
Maximum joint velocity magnitude |
\(v_{\text{max}}\) |
Identity matrix |
\(I\) |
Limits
Configuration limit
Using a first-order Taylor expansion on the configuration, we can write the limit as:
\[\begin{split}\begin{aligned}
q_{\text{min}} &\leq q \oplus v \cdot dt \leq q_{\text{max}} \\
q_{\text{min}} &\leq q \oplus \Delta q \leq q_{\text{max}} \\
q_{\text{min}} &\ominus q \leq \Delta q \leq q_{\text{max}} \ominus q
\end{aligned}\end{split}\]
Rewriting as \(G \Delta q \leq h\), we separate the inequalities:
\[\begin{split}\begin{aligned}
&+I \cdot \Delta q \leq q_{\text{max}} \ominus q \\
&-I \cdot \Delta q \leq q \ominus q_{\text{min}}
\end{aligned}\end{split}\]
Stacking these inequalities, we define:
\[\begin{split}\begin{aligned}
G &= \begin{bmatrix} +I \\ -I \end{bmatrix}, \\
h &= \begin{bmatrix} q_{\text{max}} \ominus q \\ q \ominus q_{\text{min}} \end{bmatrix}
\end{aligned}\end{split}\]
Velocity limit
Given the maximum joint velocity magnitudes \(v_{\text{max}}\), the joint velocity limits can be expressed as:
\[\begin{split}\begin{aligned}
-v_{\text{max}} &\leq v \leq v_{\text{max}} \\
-v_{\text{max}} &\leq \frac{\Delta q}{dt} \leq v_{\text{max}} \\
-v_{\text{max}} \cdot dt &\leq \Delta q \leq v_{\text{max}} \cdot dt
\end{aligned}\end{split}\]
Rewriting as \(G \Delta q \leq h\), we separate the inequalities:
\[\begin{split}\begin{aligned}
&+I \cdot \Delta q \leq v_{\text{max}} \cdot dt \\
&-I \cdot \Delta q \leq v_{\text{max}} \cdot dt
\end{aligned}\end{split}\]
Stacking these inequalities, we define:
\[\begin{split}\begin{aligned}
G \Delta q &\leq h \\
\begin{bmatrix} +I \\ -I \end{bmatrix} \Delta q &\leq \begin{bmatrix} v_{\text{max}} \cdot dt \\ v_{\text{max}} \cdot dt \end{bmatrix}
\end{aligned}\end{split}\]