When adding some concept of "pose naturalness" to an IK solver model I was building, I came across this mysterious char_len term. These notes elaborate on the math behind computing the magnitude of a spatial velocity!

TL;DR: Naturalness metrics (e.g. manipulability) derived upon the spatial Jacobian entail mixing the units of translational vs. angular velocity, and there provably exists no canonical mixing method in $\text{SE}(3)$.

Spatial Jacobian

The spatial Jacobian $J_s \in \mathbb{R}^{6 \times N}$ maps joint velocities to both linear velocity $v$ and angular velocity $\omega$: $$\begin{bmatrix} v \\ \omega \end{bmatrix} = \begin{bmatrix} J_v (q) \\ J_\omega (q) \end{bmatrix} \dot{q} \iff \frac{dx}{dt} \approx \frac{dx}{dq} \frac{dq}{dt}$$

What manipulability is actually asking

A manipulator with joint angles $q \in \mathbb{R}^n$ has end-effector velocity (a twist) given by: $$V = J(q) \dot{q}, \quad V = (v, \omega) \in \text{SE}(3) \cong \mathbb{R}^6, \quad \dot{q} \in \mathbb{R}^n$$ Yoshikawa's manipulability ellipsoid is the image of the unit ball of joint velocities (unit ball of joint velocities → ellipsoid of task-space velocities)$$\mathcal{E} = \{V = J \dot{q}: \dot{q}^\top \dot{q} \leq 1\} \subset \text{SE}(3)$$and the scalar manipulability is (proportional to) its volume, $$w = \sqrt{\det(J J^\top)} = \prod_i \sigma_i$$with $\sigma_i$ the singular values of $J$. The condition number is $\kappa$, defined as: $$\kappa = \frac{\sigma_{\max}}{\sigma_{\min}}.$$

Let's analyze the intuition behind these quantities!

• Volume of $\mathcal{E}$ measures - "how much velocity-space does the ellipsoid fill?"
• The axis lengths $\sigma_i$ measure - "how long is this twist?" (longer means that, on a relative scale, small changes in joint velocity map to larger changes in task-space velocity)
• Condition number $\kappa$ measures - "how does the length of one twist compare to the length of another?"

Each of these ^ requires asking "how long is $V$?" and "what is the angle between $V_1$ and $V_2$?" To answer these questions, we need to take the inner product of $SE(3)$ on itself: $$\langle \cdot , \cdot \rangle : \text{SE}(3) \times \text{SE}(3) \rightarrow \mathbb{R}$$

• 💡 Manipulability requires measuring distances within velocity space $\text{SE}(3)$.
• 💡 ^ We can prove that SVD on $J$ requires computing $V^\top V$: https://share.gemini.google/9bepvZCI4vNb

Measuring distances within $\text{SE}(3)$ is a finicky business

If we want to compute the magnitude of a spatial velocity, we may be tempted to use the standard Euclidean inner product: $$||V||^2 = V^\top V = v^\top v + \omega^\top \omega$$But this equation doesn't make physical sense! We can't meaningfully add $m^2/s$ to $\text{rad}^2/s$.

• ⚠️ Because the units are heterogenous, the space is non-Euclidean.
• ⚠️ Changing units will change the singular values of $J$! We can see this explicitly with an example.
  • Let $J(q) \in \mathbb{R}^{6 \times N}$ be the geometric Jacobian expressed in meters. We can partition it into its linear and angular components: $$J = \begin{bmatrix} J_v \\ J_\omega \end{bmatrix}$$ The singular values $\sigma_i$ of $J$ are the square roots of the eigenvalues of the matrix $A = J^\top J$. Expanding this, we get: $$A = J_v^\top J_v + J_\omega^\top J_\omega$$ Now, let's change our linear units from meters to mm - the angular velocity component remains untouched, but the linear velocity component scales by a factor of $c = 1000$. Mathematically, this unit swap is equivalent to multiplying $J$ by a diagonal scaling matrix $S$: $$S = \begin{bmatrix}cI_{3 \times 3} & 0 \\ 0 & I_{3 \times 3} \end{bmatrix}$$The new Jacobian in mm ($J'$) becomes: $$J = SJ = \begin{bmatrix} cJ_v \\ J_\omega \end{bmatrix}$$Computing $A' = (J')^\top J'$, we get: $$A' = (J')^\top J' = (cJ_v)^\top (cJ_v) + J_\omega^\top J_\omega = c^2 J_v^\top J_v + J_\omega^\top J_\omega$$We can see without computation that $A'$ has different eigenvalues from $A$, so $J'$ has different singular values from $J'$: the singular value spectrum distorts anisotropically.
  • ⚠️ ^ By changing units from meters to mm, a robot configuration that looked perfectly well-conditioned can suddenly appear horribly ill-conditioned if our definition of "well vs. ill conditioned" relies on the singular values of $J$.

Definition (Euclidean $n$-space): A Euclidean $n$-space is the coordinate space $\mathbb{R}^n$ equipped with the dot product inner product. (Read more in: https://en.wikipedia.org/wiki/Inner_product_space)

Summary of the problem

The problem: Computing the magnitude of a raw spatial velocity vector yields a physically contradictory mix of units.

• 🔑 Can we find a mathematical operator that steps in before the dot product occurs - to neutralize the angular units and convert everything into a single, homogeneous unit metric?
• Standard SVD algorithms (e.g. PyTorch, C++ Eigen, ...) do not know what meters or radians are - so we need to feed them matrices where units have been "stripped out."
  • Under the hood, standard SVD computes singular values using the standard Euclidean dot product $V^\top V$
  • As we proved earlier, if you feed the raw spatial Jacobian $J$ into a standard SVD solver, it implicitly computes $v^\top v + \omega^\top \omega$, returning a physically meaningless answer.
• 💡 Solution^: Pre-warp the Jacobian matrix so that when we feed it into a standard SVD solver, the solver computes the correct physics.

Roadmap for the solution

Goal: Create a new, physically-meaningful variant of the Jacobian ("warped $J$") that strips out units so that it is safe to feed to an SVD solver.

0. SVD($J$) requires computing $V^\top V$, which is physically meaningless in its vanilla form.
1. Define a "legal" inner product $||V||^2_G$ that is "unitless"
2. We want to create a warped Jacobian $\tilde{J}$ s.t. the standard SVD solver will auto resolve to computing $||V||^2_G = V^\top G V = V^\top (W^\top W) V = \tilde{V}^\top \tilde{V}$ instead of the problematic $V^\top V$. How do we do this?
   1. The relation we need to preserve for this to work out is: $\tilde{V} = \tilde{J} \dot{q}$, for some $\tilde{J}$ that we can define.
   2. ^ Working this out will show: $WV = W(J \dot{q})$ needs to equal $\tilde{J} \dot{q}$ → we can define $\tilde{J} = WJ$ and voila!

(Read the following sections for more math.)

Constructing the metric tensor $G$

Background knowledge

When a space is non-Euclidean (i.e. the standard dot product $V^\top V$ doesn't work), we can define a valid inner product by introducing a metric tensor, denoted $G$. The metric tensor intercepts the two vectors being multiplied and maps them properly.

The new, legally valid inner product becomes: $$||V||_G^2 = V^\top G V$$

• To convert a rotational displacement (radians) into a linear displacement (meters), we use the classical arc length formula: $s = r\theta$, where $r$ is a radius (length) and $\theta$ is the angle in radians.

Math for constructing $G$

We need the equation $V^\top G V$ to output a scalar where every term strictly has units of $m^2 / s^2$.

1. Let $G$ be a $6 \times 6$ symmetric block-diagonal matrix. To keep the linear velocity terms exactly as they are (since they are already in $m/s$), the top-left $3 \times 3$ block of $G$ must simply be the identity matrix $I_{3 \times 3}$.
2. To fix the angular velocity terms, we must multiply the $(\text{rad}/s)^2$ terms by a squared length. Introduce a characteristic length $L$ measured in meters.
   • The bottom-right $3 \times 3$ block of $G$ becomes $L^2 I_{3 \times 3}$: $$G = \begin{bmatrix} I_{3 \times 3} & 0 \\ 0 & L^2 I_{3 \times 3} \end{bmatrix}$$
   • Let's verify that this fixes the unit mismatch: $$\begin{align*} ||V||^2_G &= V^\top G V = \begin{bmatrix} v^\top & \omega^\top \end{bmatrix} \begin{bmatrix} I & 0 \\ 0 & L^2 I\end{bmatrix} \begin{bmatrix} v \\ \omega \end{bmatrix} \\ &= \begin{bmatrix} v^\top & \omega^\top \end{bmatrix} \begin{bmatrix} v \\ L^2\omega \end{bmatrix} \\ &= v^\top v + L^2 \omega^\top \omega\end{align*}$$Let's check units! $$v^\top v + L^2 \omega^\top \omega = \left(\frac{m}{s}\right)^2 + m^2 \cdot \left(\frac{\text{rad}}{s}\right)^2 = \left(\frac{m}{s}\right)^2 + \left(\frac{m}{s}\right)^2$$ (^ because radians are unit-less.)

Recap so far

By applying $G$, $\text{SE}(3)$ has been successfully metricized into a Euclidean space! With $G$, we can legally compute the squared magnitude of a spatial velocity vector.

• 🔑 Next, we need to apply this metric tensor directly to the Jacobian $J$, so that when we compute the SVD of the Jacobian, it inherently respects this new unit-consistent space.

Decomposing $G$ to scale the Jacobian

Background knowledge

In linear algebra, if a matrix $G$ is symmetric and positive-definite, it can be decomposed into the product of a matrix and its transpose.

Positive-definite matrix: A symmetric matrix $M$ with real entries is positive-definite if the real number $x^\top M x$ is positive for every nonzero real column vector $x$.

• Our $G$ constructed earlier is positive-definite since $L>0$, so we can write: $$G = W^\top W$$Let's work through the proof that $G$ is positive-definite:
  1. We need to show that $\forall x \in \mathbb{R}^6 \setminus \{0\}$, $x^\top G x >0$.
  2. [FILL IN LATER] last message on https://share.gemini.google/AOUPEFskNeoF
• We'll substitute $G$ with $W^\top W$ inside our inner product equation, regroup the terms, and see what happens to the velocity vector $V$.

Math to embed $G$ directly into $J$ to create a new, pre-scaled matrix $\tilde{J}$

Let's find the weighting matrix $W$ such that $G = W^\top W$. Because $G$ is block-diagonal, $W$ is simply the square root of the diagonal blocks: $$W = \begin{bmatrix}I_{3 \times 3} & 0 \\ 0 & L \cdot I_{3 \times 3}\end{bmatrix}$$Now, substitute $G = W^\top W$ back into our legally valid inner product: $$||V||^2_G = V^\top G V = V^\top (W^\top W) V = (WV)^\top (WV)$$The term $(WV)$ is just a new 6D vector - let's call it $\tilde{V}$. $$\tilde{V} = WV = \begin{bmatrix}I_{3 \times 3} & 0 \\ 0 & L \cdot I_{3 \times 3}\end{bmatrix} \begin{bmatrix}v \\ \omega\end{bmatrix} = \begin{bmatrix}v \\ L \cdot \omega\end{bmatrix}$$By multiplying $V$ by $W$, we created a mapped velocity vector $\tilde{V}$ that naturally lives in a perfectly Euclidean space (i.e. $\tilde{V}^\top \tilde{V} = ||V||_G^2$ - has a valid inner product!). Since we know that the raw velocity is generated by the raw Jacobian ($V = J \dot{q}$), we can substitute that into our $\tilde{V}$ equation: $$\tilde{V} = WV = W(J\dot{q}) = (WJ) \dot{q} \implies \text{define } \tilde{J} = WJ$$ The term $\tilde{J}$ is our scaled spatial Jacobian! $$\tilde{J} = WJ = \begin{bmatrix}I_{3 \times 3} & 0 \\ 0 & L \cdot I_{3 \times 3}\end{bmatrix} \begin{bmatrix} J_v \\ J_\omega \end{bmatrix} = \begin{bmatrix} J_v \\ L \cdot J_\omega \end{bmatrix}$$

Recap so far

We've created a warped Jacobian $\tilde{J}$ such that $$\tilde{V} = \tilde{J} \dot{q},$$so when we feed it to a standard SVD solver, the solver will be computing $\tilde{V}^\top \tilde{V} = ||V||^2_G$ under the hood (the physically meaningful, unit-independent magnitude) instead of $V^\top V$ (the physically meaningless number we started with).

• 🔖 Next: How do we pick $L$?

Physical grounding of $L$

What should $L$ be?

Intuition: $L$ can be interpreted as "how many meters of translation we treat as equivalent to 1 rad of rotation."

• 🔑 $L$ needs to convert "1 radian of rotation" into "← some matching number of meters"
• 💡 We can use the classical relation $s = r\theta$:

- ^ so the conversion factor we want for $L$ is "the radius at which the end-effector typically swings" - which is its typical moment arm (moment arm := lever arm := robot arm)! 1 radian of EE rotation drags the EE roughly $r$ meters, so setting $L = r$ makes 1 radian of rotation count the same as the $r$ meters of linear sweep it actually produces. 

• 📍 But $r = ||p_{ee} - p_\text{shoulder}||$ isn't a single number - it changes with configuration as the arm folds and extends!
  • To freeze one $L$ into $G$, we need a single representative radius - we can take the median EE reach, i.e.:
• ⚠️ Notice that $L$ is a choice: we need to freeze it in downstream usages and use the same value everywhere for one robot!