Formula reference

Steps from angle

$$\text{microsteps} = \frac{\theta°}{360} \times 200 \times \text{microstepping} \times \text{gear ratio}$$

TMC2209 RMS current

$$I_{\text{RMS}} = \frac{CS + 1}{32} \times \frac{V_{\text{FS}}}{R_{\text{sense}} + 0.02\,\Omega} \times \frac{1}{\sqrt{2}}$$ $$V_{\text{FS}} = 0.325\,\text{V} \;\; (\texttt{vsense} = 0) \qquad\text{or}\qquad 0.180\,\text{V} \;\; (\texttt{vsense} = 1)$$

StallGuard stall condition

$$\texttt{SG\_RESULT} \le 2 \times \texttt{SGTHRS} \qquad \text{and only while} \;\; \texttt{TSTEP} \le \texttt{TCOOLTHRS}$$

DH transform

$$T = \begin{bmatrix} \cos\theta & -\sin\theta\cos\alpha & \sin\theta\sin\alpha & a\cos\theta \\ \sin\theta & \cos\theta\cos\alpha & -\cos\theta\sin\alpha & a\sin\theta \\ 0 & \sin\alpha & \cos\alpha & d \\ 0 & 0 & 0 & 1 \end{bmatrix}$$

Homogeneous transform inverse

$$T^{-1} = \begin{bmatrix} R^{\mathsf{T}} & -R^{\mathsf{T}} p \\ 0 & 1 \end{bmatrix}$$

PAROL6 inverse kinematics — verified

Wrist centre:

$$p_{wc} = p_{\text{flange}} + R_{\text{flange}} \cdot \begin{bmatrix} 45.25 & 0 & -62.80 \end{bmatrix}^{\mathsf{T}}$$

Joint 1, with $+\pi$ for the flipped branch:

$$\theta_1 = \operatorname{atan2}(y_{wc},\, x_{wc})$$

Planar coordinates, taking the minus sign for the flipped branch:

$$u = \pm\sqrt{x_{wc}^2 + y_{wc}^2} - a_2 \qquad v = a_1 - z_{wc}$$

Constants:

$$L = \sqrt{a_4^2 + a_5^2} = 181.636\,\text{mm} \qquad \varphi = \operatorname{atan2}(a_4, a_5) = 0.24184\,\text{rad} = 13.856°$$

Joints 2 and 3 — no solution if $|\cos B| > 1$; the sign of $B$ selects elbow up or elbow down:

$$\begin{aligned} \cos B &= \frac{u^2 + v^2 - a_3^2 - L^2}{2 \, a_3 L} \\ A &= \operatorname{atan2}(v, u) - \operatorname{atan2}\big(L\sin B,\; a_3 + L\cos B\big) \\ \theta_2 &= A + \pi/2 \\ \theta_3 &= \pi/2 - \varphi - B \end{aligned}$$

Wrist, with $R_3^6 = \big(R_0^3\big)^{\mathsf{T}} R_{\text{target}}$:

$$\begin{aligned} \theta_5 &= \operatorname{atan2}\left(\sqrt{r_{13}^2 + r_{23}^2},\; -r_{33}\right) \\ \theta_4 &= \operatorname{atan2}(-r_{23},\, -r_{13}) \\ \theta_6 &= \operatorname{atan2}(r_{32},\, r_{31}) \end{aligned}$$

The flipped branch is $\theta_4 + \pi$, $-\theta_5$, $\theta_6 + \pi$.

Verified by round trip against forward kinematics over 5,000 random poses: worst-case pose error $2.3 \times 10^{-13}$.

Generic two-link planar IK — link lengths $L_1$, $L_2$, target at distance $D$; no solution if $|\cos\theta_2| > 1$

$$\begin{aligned} \cos\theta_2 &= \frac{D^2 - L_1^2 - L_2^2}{2 L_1 L_2} \\ \theta_1 &= \operatorname{atan2}(y, x) - \operatorname{atan2}\big(L_2 \sin\theta_2,\; L_1 + L_2 \cos\theta_2\big) \end{aligned}$$

Jacobian column for revolute joint $i$

$$J_i = \begin{bmatrix} z_i \times (p - o_i) \\ z_i \end{bmatrix}$$

Damped least squares IK step

$$\Delta q = J^{\mathsf{T}} \left( J J^{\mathsf{T}} + \lambda^2 I \right)^{-1} e$$

Manipulability, which goes to zero at a singularity

$$w = \sqrt{\det\big(J J^{\mathsf{T}}\big)}$$