Companion to
GNC/Framing_Paper_coordinated_control/coordinated_control.md
(equation numbers refer to it) and
seven_dof_redundant_extension.md (the terse version).
Everything here is re-derivable by hand; the numeric checks live in
validation/test_robot7_nullspace.py.
Prior-art note (Jun 11). After this was written we read Giordano’s thesis (giordano2020wholebody) §5.2 + App. C.4: the redundant coordinated formulation is there — his dynamically-consistent condition (5.22) is the same construction as our \(\boldsymbol z_a\) (§4), and his §5.1 damping law (5.8c) is the finite-\(d\) version of §6/P4. Read this document as the intuition layer for that prior art, with our independent derivation serving as cross-validation; what is ours is the fiber-reconstruction implementation, the explicit kernel basis \(\hat{\boldsymbol k}\) with its base counter-translation reading, and the inspection-mission singularity application. His remark that the internal Coriolis stays “fully coupled” confirms §6/P3 in print.
The whole controller is built on a change of coordinates. The robot natively speaks
\[ \boldsymbol{x} = \begin{bmatrix} \boldsymbol{v}_b \\ \boldsymbol{\omega}_b \\ \dot{\boldsymbol{q}} \end{bmatrix} \quad\text{("what the motors do": base velocity, base spin, joint rates)}, \]
but we want to think in
\[ \boldsymbol{z} = \begin{bmatrix} \boldsymbol{v}_c \\ \boldsymbol{\omega}_b \\ \boldsymbol{\nu}_e^\oplus \end{bmatrix} \quad\text{("what the mission cares about": CoM velocity, base spin, EE motion about the CoM)}. \]
The translator between them is \(\boldsymbol{\Gamma}\) (eq. 19): \(\boldsymbol{z} = \boldsymbol{\Gamma}\boldsymbol{x}\).
Now count. With a 6-joint arm, \(\boldsymbol{x}\) has \(6+6=12\) entries and \(\boldsymbol{z}\) has \(3+3+6=12\). Twelve in, twelve out: the translation is one-to-one, \(\boldsymbol{\Gamma}\) is square, and you can always go backwards (\(\boldsymbol{x} = \boldsymbol{\Gamma}^{-1}\boldsymbol{z}\), eq. 34d). That backwards step is the only place Giordano uses \(n=6\) — his words: “Let us assume a nonredundant manipulator.”
Add a 7th joint and \(\boldsymbol{x}\) has 13 entries while \(\boldsymbol{z}\) still has 12. Thirteen knobs, twelve dials. There must be one way of moving the knobs that moves none of the dials. That invisible motion is the self-motion, and the entire 7-DOF project is:
Try it on yourself: touch your nose and hold it there. You can still swing your elbow in an arc. Hand fixed, nose fixed, elbow orbiting — that’s a self-motion, and the roll joint we added to the UR3 creates exactly this one.
Forget 3D. Put three things on a frictionless rail:
Two mission dials: the system CoM velocity \(v_c\) and the hand velocity \(\nu_e\). Three knobs: \(v_b, \dot q_1, \dot q_2\). With \(m_t = M + m_1 + m_2\):
\[ \underbrace{\begin{bmatrix} v_c \\ \nu_e \end{bmatrix}}_{\boldsymbol z} = \underbrace{\begin{bmatrix} 1 & \tfrac{m_1+m_2}{m_t} & \tfrac{m_2}{m_t} \\ 1 & 1 & 1 \end{bmatrix}}_{\boldsymbol\Gamma\ (2\times 3)} \underbrace{\begin{bmatrix} v_b \\ \dot q_1 \\ \dot q_2 \end{bmatrix}}_{\boldsymbol x}. \]
Find the invisible motion: solve \(\boldsymbol\Gamma\boldsymbol x = \boldsymbol 0\). Row 2 says \(v_b = -(\dot q_1 + \dot q_2)\). Substitute into row 1 and simplify: \(M\dot q_1 = -(M+m_1)\,\dot q_2\). One free choice — pick \(\dot q_2 = -M\), then:
\[ \boxed{\ \dot q_1 = M+m_1, \qquad \dot q_2 = -M, \qquad v_b = -m_1\ } \]
Read it like a story: the two joints fight each other so the hand doesn’t move (\(\dot q_1 + \dot q_2 = m_1\), and \(v_b\) cancels it). But the elbow mass does move — so the base counter-translates (at \(-m_1\), proportional to exactly the mass that’s swinging) to keep the CoM frozen. Hand still, CoM still, elbow swinging, base shuffling. That is the elbow orbit, in one dimension, with the base’s role made visible.
Everything in the real derivation is this napkin, dressed up.
The circumcentroidal Jacobian \(\boldsymbol{J}_{\nu_e}^\oplus\) (eq. 14;
code dyn.J_plus) is a machine: 7 joint rates in, 6
EE-velocity numbers out. A \(6\times
7\) machine always ignores at least one input direction. The SVD
finds it:
\[ \boldsymbol{J}_{\nu_e}^\oplus = \boldsymbol{U}\,\boldsymbol\Sigma\,\boldsymbol{V}^T , \]
where the columns of \(\boldsymbol V \in \mathbb{R}^{7\times 7}\) are input directions sorted by how much output they produce (\(\sigma_1 \ge \dots \ge \sigma_6 \ge\) nothing). The 7th column produces zero output:
\[ \hat{\boldsymbol n} \;=\; \boldsymbol V\text{'s last column}, \qquad \boldsymbol{J}_{\nu_e}^\oplus\,\hat{\boldsymbol n} = \boldsymbol 0, \qquad \|\hat{\boldsymbol n}\| = 1 . \]
Trap we hit in code:
np.linalg.svd(J, full_matrices=False)returns only the first 6 columns of \(\boldsymbol V\) — it describes what the machine can do and silently omits what it ignores. The kernel needsfull_matrices=True. And \(\sigma_6\) (code:s[5]) is the new “distance to singularity”: if \(\sigma_6 > 0\) the arm still spans all 6 EE directions while also having the spare motion.
\(\hat{\boldsymbol n}\) only says the joints can move without moving the EE-about-the-CoM. The full question is: which \((\boldsymbol v_b, \boldsymbol\omega_b, \dot{\boldsymbol q})\) moves none of the twelve dials? Solve \(\boldsymbol\Gamma\boldsymbol x = \boldsymbol 0\) row-block by row-block, using the structure of eq. (19):
\[ \boldsymbol{\Gamma} = \begin{bmatrix} \boldsymbol{R}_{cb} & -\boldsymbol{R}_{cb}[\boldsymbol{p}_{bc}]^\wedge & \boldsymbol{R}_{cb}\bar{\boldsymbol{J}}_v \\ \boldsymbol{0} & \boldsymbol{E} & \boldsymbol{0} \\ \boldsymbol{0} & \boldsymbol{G}_{\omega_b} & \boldsymbol{J}_{\nu_e}^\oplus \end{bmatrix} \begin{matrix} \leftarrow v_c \text{ row} \\ \leftarrow \omega_b \text{ row} \\ \leftarrow \nu_e^\oplus \text{ row} \end{matrix} \]
One free parameter \(\alpha\) — a one-dimensional family, spanned by
\[ \boxed{\;\hat{\boldsymbol k} \;=\; \begin{bmatrix} -\bar{\boldsymbol J}_v\,\hat{\boldsymbol n} \\ \boldsymbol 0_3 \\ \hat{\boldsymbol n} \end{bmatrix} \in \mathbb{R}^{13}, \qquad \boldsymbol\Gamma\,\hat{\boldsymbol k} = \boldsymbol 0\;} \]
Here \(\bar{\boldsymbol J}_v\) (eq. 7) is the mass-weighted average of how the joints move the links — i.e. \(\bar{\boldsymbol J}_v\dot{\boldsymbol q}\) is “how much the arm’s mass lump moves.” So \(\boldsymbol v_b = -\bar{\boldsymbol J}_v\hat{\boldsymbol n}\) is the napkin’s \(v_b = -m_1\): the base counter-translates by exactly the mass-weighted arm motion, so the CoM stays frozen. Arm swings along \(\hat{\boldsymbol n}\), base shuffles, CoM/attitude/EE all still.
Free-flying vs free-floating, one more time. The free-floating literature gets a reactionless motion from momentum conservation (base unactuated, base stays still). Ours is different: the base moves — we simply chose coordinates in which that combined motion registers as zero. Same algebra family, different physics. Don’t call ours an RNS in front of the committee; call it the kinematic self-motion of the coordinated coordinates.
Tonight’s check: \(\|\boldsymbol\Gamma\hat{\boldsymbol k}\| < 10^{-10}\) at 9 random configurations. The napkin holds in 3D.
§3 characterizes the self-motion subspace \(\ker\boldsymbol\Gamma = \mathrm{span}\{\hat{\boldsymbol k}\}\). To regulate the self-motion we need a velocity coordinate for it: a covector (row vector) \(\boldsymbol z_a^T\) — a linear functional on the generalized-velocity space — satisfying
\[ v_n = \boldsymbol z_a^T\boldsymbol x, \qquad \boldsymbol z_a^T\hat{\boldsymbol k} = 1 \quad\text{(normalized to read unity on a unit self-motion).} \]
The normalization fixes one degree of freedom of \(\boldsymbol z_a \in \mathbb R^{13}\), leaving a twelve-parameter family of admissible covectors. Among them, take the inertia-weighted choice
\[ \boxed{\;\boldsymbol z_a^T = \frac{\hat{\boldsymbol k}^T\boldsymbol M}{\hat{\boldsymbol k}^T\boldsymbol M\hat{\boldsymbol k}}\;} \qquad (\boldsymbol M \in \mathbb R^{13\times 13}\ \text{the generalized inertia matrix, eq. 4}). \]
This is the dynamically consistent weighting in Khatib’s sense; equivalently, \(\{v_n = 0\}\) is the horizontal subspace of the mechanical connection (Marsden–Montgomery). Three properties make the choice canonical.
(a) Well-posedness on \(\Omega\). The denominator \(\hat{\boldsymbol k}^T\boldsymbol M\hat{\boldsymbol k}\) is twice the kinetic energy of the self-motion at unit amplitude; since \(\boldsymbol M\) is symmetric positive definite and \(\hat{\boldsymbol k} \neq \boldsymbol 0\), it is strictly positive at every configuration, so \(\boldsymbol z_a\) is well defined on all of \(\Omega\). Contrast the extended-Jacobian alternative \(\boldsymbol z_a = \nabla g(\boldsymbol q)\): the augmentation degenerates wherever \(\nabla g \perp \hat{\boldsymbol n}\) — an algorithmic singularity, i.e. a rank loss of the augmented map at configurations where the mechanism itself is regular. The inertia weighting admits no such points.
(b) \(\boldsymbol M\)-orthogonality of task and self-motion. Append the row to \(\boldsymbol\Gamma\) to form the augmented transformation and its inverse:
\[ \boldsymbol\Gamma_a = \begin{bmatrix} \boldsymbol\Gamma \\ \boldsymbol z_a^T \end{bmatrix} \in \mathbb{R}^{13\times 13}, \qquad \boldsymbol\Gamma_a^{-1} = \begin{bmatrix} \boldsymbol c_1 \cdots \boldsymbol c_{12} \;\big|\; \boldsymbol c_{13} \end{bmatrix}. \]
Invertibility: if \(\boldsymbol\Gamma_a\boldsymbol x = \boldsymbol 0\), the first twelve rows give \(\boldsymbol\Gamma\boldsymbol x = \boldsymbol 0\), hence \(\boldsymbol x = \alpha\hat{\boldsymbol k}\) by §3.2; the last row then gives \(\alpha\,\boldsymbol z_a^T\hat{\boldsymbol k} = \alpha = 0\), so \(\boldsymbol\Gamma_a\) is nonsingular on \(\Omega\). The columns \(\boldsymbol c_i\) of the inverse are the basis vectors of the transformed velocity coordinates: \(\boldsymbol c_i\) is the unique generalized velocity producing one unit of the \(i\)-th coordinate and zero of the others. In particular \(\boldsymbol c_{13} = \hat{\boldsymbol k}\), since \(\hat{\boldsymbol k} \in \ker\boldsymbol\Gamma\) and \(\boldsymbol z_a^T\hat{\boldsymbol k} = 1\). Reading row 13, column \(i \le 12\) of the identity \(\boldsymbol\Gamma_a\boldsymbol\Gamma_a^{-1} = \boldsymbol E_{13}\):
\[ \boldsymbol z_a^T\boldsymbol c_i = 0 \quad\Longrightarrow\quad \hat{\boldsymbol k}^T\boldsymbol M\,\boldsymbol c_i = 0 , \]
i.e. every task basis velocity is \(\boldsymbol M\)-orthogonal to the self-motion. (If the implementation appends the normalized row \(\hat{\boldsymbol z}_a = \boldsymbol z_a/\|\boldsymbol z_a\|\) for conditioning, the last column scales to \(\boldsymbol c_{13} = \|\boldsymbol z_a\|\,\hat{\boldsymbol k}\); only the proportionality \(\boldsymbol c_{13} \propto \hat{\boldsymbol k}\) is used downstream.)
(c) Block-diagonal transformed inertia. Kinetic energy is invariant under a change of velocity coordinates, so the inertia transforms by congruence: \(\hat{\boldsymbol M} = \boldsymbol\Gamma_a^{-T}\boldsymbol M\,\boldsymbol\Gamma_a^{-1}\), with entries \(\hat{M}_{ij} = \boldsymbol c_i^T\boldsymbol M\boldsymbol c_j\). By (b), every entry coupling \(i \le 12\) to \(j = 13\) vanishes:
\[ \hat{\boldsymbol M} = \begin{bmatrix} \hat{\boldsymbol M}_{\text{task}} & \boldsymbol 0 \\ \boldsymbol 0^T & \hat m_n \end{bmatrix}, \qquad \hat m_n = \hat{\boldsymbol k}^T\boldsymbol M\hat{\boldsymbol k} \;(>0). \]
There are no kinetic-energy cross-terms between the self-motion and the task coordinates. This is the inertial decoupling the redundant-manipulator literature obtains from the dynamically consistent inverse (Khatib; Ott’s monograph on redundant arms), here read directly off one row of \(\boldsymbol\Gamma_a\boldsymbol\Gamma_a^{-1}=\boldsymbol E\). This is the hard half of proof obligation P1. The remaining half is bookkeeping: relating the \(12\times 12\) task block to the original \(\breve{\boldsymbol M}\).
The Lyapunov argument (eqs. 37–38) rests on one structural fact (eq. 23): the skew-symmetry (passivity) property
\[ \dot{\boldsymbol M} - 2\boldsymbol C \;\;\text{is skew-symmetric} \qquad\Big(\text{equivalently, the energy balance } \tfrac{d}{dt}\big(\tfrac12\boldsymbol x^T \boldsymbol M \boldsymbol x\big) = \boldsymbol x^T\boldsymbol F\text{: the Coriolis terms are workless}\Big). \]
The question is whether the augmentation preserves it. It does, for any invertible, smooth \(\boldsymbol\Gamma_a(q)\). Transform the dynamics in the standard way:
\[ \hat{\boldsymbol M} = \boldsymbol\Gamma_a^{-T}\boldsymbol M\boldsymbol\Gamma_a^{-1}, \qquad \hat{\boldsymbol C} = \boldsymbol\Gamma_a^{-T}\big(\boldsymbol C - \boldsymbol M\boldsymbol\Gamma_a^{-1}\dot{\boldsymbol\Gamma}_a\big)\boldsymbol\Gamma_a^{-1}. \]
(The \(\dot{\boldsymbol\Gamma}_a\) term is the standard correction for a configuration-dependent velocity transformation.)
Lemma (passivity survives). If \(\dot{\boldsymbol M}-2\boldsymbol C\) is skew, so is \(\dot{\hat{\boldsymbol M}}-2\hat{\boldsymbol C}\).
Proof. Two ingredients: the product rule, and \(\tfrac{d}{dt}(\boldsymbol\Gamma_a^{-1}) = -\boldsymbol\Gamma_a^{-1}\dot{\boldsymbol\Gamma}_a\boldsymbol\Gamma_a^{-1}\) (differentiate \(\boldsymbol\Gamma_a\boldsymbol\Gamma_a^{-1}=\boldsymbol E\) and solve). Differentiate \(\hat{\boldsymbol M}\), subtract \(2\hat{\boldsymbol C}\), and collect — two of the six terms cancel, leaving
\[ \dot{\hat{\boldsymbol M}} - 2\hat{\boldsymbol C} = \underbrace{\boldsymbol\Gamma_a^{-T}\big(\dot{\boldsymbol M} - 2\boldsymbol C\big)\boldsymbol\Gamma_a^{-1}}_{\text{congruence of the original skew term}} \;+\; \underbrace{\big(\boldsymbol S - \boldsymbol S^T\big)}_{\text{skew by construction}}, \qquad \boldsymbol S \;\triangleq\; \boldsymbol\Gamma_a^{-T}\boldsymbol M\boldsymbol\Gamma_a^{-1}\dot{\boldsymbol\Gamma}_a\boldsymbol\Gamma_a^{-1}. \]
A change of basis of a skew matrix is skew (\((\boldsymbol P^T\boldsymbol A\boldsymbol P)^T = -\boldsymbol P^T\boldsymbol A\boldsymbol P\)), and \(\boldsymbol S - \boldsymbol S^T\) is skew by construction. Sum of skews is skew. \(\blacksquare\)
So energy bookkeeping is safe under any augmentation — even a bad choice of \(\boldsymbol z_a\) wouldn’t break passivity, just the clean decoupling. The risk in this project never lived in the Lyapunov function; it lives in the structure (§6).
| # | Claim | Status | Why believe it / what’s hard |
|---|---|---|---|
| P1 | \(\hat{\boldsymbol M}\) decouples \(v_n\) from the 12 task coordinates | Proved above (§4b–c) | Remaining: relate the task block to the old \(\breve{\boldsymbol M}\) |
| P2 | CoM row stays \(m\dot{\boldsymbol v}_c = \boldsymbol f_c\), Coriolis-free | Expected free | König: \(T = \tfrac12 m\|\boldsymbol v_c\|^2 + T_{\text{int}}\) holds no matter how the internal motion is parameterized; verify the appendix-of-[4] cancellation |
| P3 | Coriolis cross-terms between \(v_n\) and \((\boldsymbol\omega_b,\boldsymbol\nu_e^\oplus)\) | Expect them to EXIST | Inertia decoupling is pointwise in \(q\); \(\hat{\boldsymbol M}(q)\) varies with configuration, and its rate enters the off-diagonal blocks of \(\hat{\boldsymbol C}\). These terms do not break passivity (§5), but they couple \(v_n\) to the task dynamics; do not claim a block-diagonal \(\hat{\boldsymbol C}\). |
| P4 | Closed loop: add \(u_n = -k_n\tilde x_n - d_n v_n\), extend \(V\) by \(\tfrac12\hat m_n v_n^2 + \tfrac12 k_n\tilde x_n^2\), re-run the cascade (35)–(38) | To do | \(d_n > 0\) is the dissipation real hardware gets for free from mechanical joint friction — our frictionless sim lacks it, hence the measured ringing. (Damping law: Giordano thesis §5.8c. Coordinated-control paper: RA-L 2019, which assumes a nonredundant arm. No “Giordano 2021” paper exists — verified Jun 16.) |
And the design choice behind P4: the null space is one-dimensional, but three secondary objectives compete for it (singular-value maximization, envelope clearance, pan-centering). Allocating one degree of freedom among three objectives is a weighting/scheduling decision to be made explicitly, with data.
| Math | Code | Note |
|---|---|---|
| \(\boldsymbol J_{\nu_e}^\oplus \in \mathbb{R}^{6\times 7}\) | dyn.J_plus |
eq. 14, unchanged construction |
| \(\boldsymbol\Gamma \in \mathbb{R}^{12\times 13}\) | dyn.Gamma |
built with self.n — generalized by itself |
| \(\hat{\boldsymbol n}\) | np.linalg.svd(J_plus, full_matrices=True) →
Vt[-1] |
economy SVD cannot see it |
| \(\sigma_6\) | s[5] |
the redundant arm’s “distance to singularity” |
| \(\hat{\boldsymbol k}\) | np.concatenate([-dyn.Jv_bar @ n_hat, np.zeros(3), n_hat]) |
verified \(\|\boldsymbol\Gamma\hat{\boldsymbol k}\| < 10^{-10}\) |
| \(\boldsymbol z_a\), \(v_n\) | (dyn.M @ k_hat) / (k_hat @ dyn.M @ k_hat);
z_a @ v |
smoke_7dof_closed_loop.py |
| right inverse \(\boldsymbol\Gamma^{-R}\) | dyn.Gam_inv (13×12) |
old block formula + Moore-Penrose J_inv; \(\boldsymbol\Gamma\boldsymbol\Gamma^{-R} =
\boldsymbol E_{12}\) verified |
Tonight’s measurement (Jun 10): running the unchanged controller on the 7-DOF arm (minimum-norm reconstruction ⇒ the self-motion is unregulated): tracking held (EE err ≤ 0.191 m), full row rank held (\(\sigma_6 \in [0.054, 0.077]\)), and the self-motion coordinate read \(\max|v_n| = 0.23\) rad/s, non-decaying. That plot is the “before” picture for P4 — the same phenomenon Giordano’s thesis addresses with its null-space damping law (§5.8c): on hardware, mechanical joint friction quietly supplies that \(d_n\), which our frictionless sim lacks. (No “RA-L 2021” paper exists — verified Jun 16; the canonical coordinated-control paper is RA-L 2019, which assumes a nonredundant arm.)