Weighting matrix for optimisation

[ssutjipto]

Currently, in the SerialKinematicControl class, the weighting matrix used to resolve_endpoint_motion for a non redundant robot is the Cartesian Stiffness matrix. For the redundant case it is the joint inertia matrix.

This is just temporary for now, but in future we could think about defaulting to the identity, and only use the weighting if the user specifies it?

Unless there is a compelling case to utilise a particular weighting matrix? But we should discuss how that impacts the robot behaviour when using it.

[Woolfrey]

Hey @ssutjipto, I was just thinking about this the other day. Wasn't sure if it was worth making an issue (but as I'm writing out this issue it's becoming more complex than I thought...). But I do have an answer! The solution should be rooted in physics.

First of all, the redundancy resolution at the torque level follows from Gauss' principle of least constraint [1]:

\begin{align}
\min_\mathbf{\ddot{q}} \frac{1}{2}\left(\boldsymbol{\tau}_\varnothing - \mathbf{M\ddot{q}}\right)^\mathrm{T}\mathbf{M}^{-1}\left(\boldsymbol{\tau}_\varnothing - \mathbf{M\ddot{q}}\right) \\
\text{subject to: } \mathbf{J\ddot{q} = \ddot{x} - \dot{J}\dot{q}}
\end{align}

This formulation is profound because it leads to the dynamically consistent null space projection matrix [2]

\mathbf{N}_\mathrm{M} = \mathbf{I} - \mathbf{M}^{-1}\mathbf{J}^\mathrm{T}
\underbrace{
\left(\mathbf{JM}^{-1}\mathbf{J}^\mathrm{T}\right)^{-1}
}_{\boldsymbol{\Lambda}}
\mathbf{J}.

But you can see the Cartesian inertia matrix $\boldsymbol{\Lambda}$ naturally appears.

Since this is grounded in physics, we should do the same for all other cases.

But there's a deeper question...

Consider the velocity level problem:

\begin{align}
\min_\mathbf{\dot{q}} \frac{1}{2}\left(\mathbf{\dot{q}_\varnothing - \dot{q}}\right)^\mathrm{T}\mathbf{W}\left(\mathbf{\dot{q}_\varnothing - \dot{q}}\right) \\
\text{subject to: } \mathbf{J\dot{q} = \ddot{x}}
\end{align}.

• If we choose $\mathbf{W = M}$, then we are minimizing the instantaneous kinetic energy, but if we choose:
• $\mathbf{W = D}$, we are minimizing power from viscous friction.

Minimising kinetic energy is probably better since it too leads to the dynamically consistent Jacobian & null space projection :thinking:

But what should we choose for the non-redundant case?

For the dynamic level:

\min_\mathbf{\ddot{q}} \frac{1}{2}\left(\mathbf{\ddot{x} - J\ddot{q} - \dot{J}\dot{q}}\right)^\mathrm{T}\mathbf{W}\left(\mathbf{\ddot{x} - J\ddot{q} - \dot{J}\dot{q}}\right)

my instinct now is to choose the Cartesian inertia $\mathbf{W} = \boldsymbol{\Lambda}$.

For the velocity level:

\min_\mathbf{\ddot{q}} \frac{1}{2}\left(\mathbf{\dot{x} - J\dot{q}}\right)^\mathrm{T}\mathbf{W}\left(\mathbf{\dot{x} - J\dot{q}}\right)

I thought yesterday to choose the Cartesian damping $\mathbf{W = D}$. But after typing up all this math now I am thinking again to use $\boldsymbol{\Lambda}$ instead :thinking:

We should solve the math and see what the answer leads to.

If the math is interesting, we could write a paper about it comparing the different choices!

---

[1] Bruyninckx, H., & Khatib, O. (2000, April). Gauss' principle and the dynamics of redundant and constrained manipulators. In Proceedings 2000 ICRA. Millennium Conference. IEEE International Conference on Robotics and Automation. Symposia Proceedings (Cat. No. 00CH37065) (Vol. 3, pp. 2563-2568). IEEE.

[2] Featherstone, R., & Khatib, O. (1997). Load independence of the dynamically consistent inverse of the Jacobian matrix. The International Journal of Robotics Research, 16(2), 168-170.

[Woolfrey]

OK, so at the velocity level, the non-redundant solution is:

\mathbf{\dot{q}} = \left(\mathbf{J^\mathrm{T}WJ}\right)^{-1}\mathbf{J^\mathrm{T}W\dot{x}}

If:

• $\mathbf{W = D}$, then $\mathbf{D\dot{x}}$ is a force, so $\mathbf{J^\mathrm{T}D\dot{x}}$ is a torque.
• $\mathbf{W} = \boldsymbol{\Lambda}$ then $\boldsymbol{\Lambda}\mathbf{\dot{x}}$ is momentum.

In the first case, it is not logical to have velocity proportional to torque.

The second is more logically consistent: force is to acceleration as momentum is to velocity.

We should use Cartesian inertia as the weighting matrix.

For the dynamic level:

\mathbf{\ddot{q}} = \left(\mathbf{J^\mathrm{T}WJ}\right)^{-1}\mathbf{J^\mathrm{T}W\left(\ddot{x} - \dot{J}\dot{q}\right)}

• Using damping $\mathbf{W = D}$ does not make any physical sense.
• Using $\mathbf{W} = \boldsymbol{\Lambda}$ leads to $\boldsymbol{\Lambda}\left(\mathbf{\ddot{x} - \dot{J}\dot{q}}\right)$ is a (dynamic) force, which is logically consistent. Then $\mathbf{J}^\mathrm{T}$ maps this to a joint torque :+1:

---

Also, I tried to see if I could reduce:

\left(\mathbf{J}^\mathrm{T}\boldsymbol{\Lambda}\mathbf{J}\right)^{-1}\mathbf{J}^\mathrm{T}\boldsymbol{\Lambda}

using SVD:

\mathbf{J = USV^\mathrm{T}}

but it cannot be simplified.

So we need to pre-compute $\boldsymbol{\Lambda}$, probably by means of a matrix decomposition (my suggestion is LDLT).

Fastest way is probably:

A = J*M.ldlt().solve(J.transpose()); // Inverse of Cartesian inertia
Adecomp = A.ldlt(); // Pre-compute because we'll use it twice
H = J.transpose()*Adecomp.solve(J); // Hessian
f = -J.transpose()*Adecomp.solve(xdot); // Linear component

Next is to see if we can make sense of DLS in the same way.

[Woolfrey]

Did some quick math. The standard formulation for DLS is:

\min_\mathbf{\dot{q}} \frac{1}{2}\left(\mathbf{\dot{x} - J\dot{q}}\right)^\mathrm{T}\left(\mathbf{\dot{x} - J\dot{q}}\right) + \frac{1}{2}\lambda^2\mathbf{\dot{q}^\mathrm{T}\dot{q}}

$\lambda$ is analogous to damping since it penalizes large joint velocities, slowing them down in the solution.

Instead we could do:

\min_\mathbf{\dot{q}} \frac{1}{2}\left(\mathbf{\dot{x} - J\dot{q}}\right)^\mathrm{T}\boldsymbol{\Lambda}\left(\mathbf{\dot{x} - J\dot{q}}\right) + \frac{1}{2}\mathbf{\dot{q}^\mathrm{T}M\dot{q}}

whose solution is:

\mathbf{\dot{q}} = \left(\mathbf{J}^\mathrm{T}\boldsymbol{\Lambda}\mathbf{J + M}\right)^{-1}\mathbf{J}^\mathrm{T}\boldsymbol{\Lambda}\mathbf{\dot{x}}.

The only issue, then, is that $\boldsymbol{\Lambda}$ is singular :thinking:

[Woolfrey]

Based on [3], there's another option:

\min_\mathbf{\dot{q}} \frac{1}{2}\left(\mathbf{\dot{x} - J\dot{q}}\right)^\mathrm{T}\mathbf{W}_\mathrm{x}\left(\mathbf{\dot{x} - J\dot{q}}\right) + \left(\mathbf{\dot{q}}_0 - \mathbf{\dot{q}}\right)^\mathrm{T}\mathbf{W}_\mathrm{q}\left(\mathbf{\dot{q}}_0 - \mathbf{\dot{q}}\right)

whose solution is:

\mathbf{\dot{q}} =
\left(\mathbf{J^\mathrm{T}W_\mathrm{x}J + W_\mathrm{q}}\right)^{-1}\left(\mathbf{J^\mathrm{T}W_\mathrm{x}\dot{x} + W_\mathrm{q}}\mathbf{\dot{q}}_0\right)

We could choose:

• $\mathbf{W\mathrm{x} = K\mathrm{d}}$, i.e. the Cartesian damping,
• $\mathbf{W_\mathrm{q} = M}$; the joint inertia matrix, and
• $\mathbf{q}_0 = \mathbf{q}(t-\Delta t)$ as the velocity at the previous time step.

That way $\mathbf{M\dot{q}}_0$ is momentum. If the robot is moving, it will keep moving, hopefully through a singularity?

The term $\mathbf{J^\mathrm{T}K_\mathrm{d}\dot{x}}$ should pull the robot in the desired direction.

A flaw with DLS is that it penalizes all joint motion, tending towards zero, so the robot is sluggish whether its moving toward or away from singularities.

---

[3] Hersch, M., & Billard, A. G. (2008). Reaching with multi-referential dynamical systems. Autonomous robots, 25, 71-83.

[Woolfrey]

@ssutjipto, here's what I've settled on for the kinematic control:

---

Non-Redundant Robots: Weighted by Cartesian inertia

\min_\mathbf{\dot{q}} \frac{1}{2} \left(\mathbf{\dot{x} - J\dot{q}}\right)^\mathrm{T}\boldsymbol{\Lambda}\left(\mathbf{\dot{x} - J\dot{q}}\right)

---

Redundant Robots: Weighted by Joint Inertia

\begin{align}
\min_\mathbf{\dot{q}} \frac{1}{2} \left(\mathbf{\dot{q}}_\varnothing - \mathbf{\dot{q}}\right)^\mathrm{T}\mathbf{M} \left(\mathbf{\dot{q}}_\varnothing - \mathbf{\dot{q}}\right) \\
\text{subject to: } \mathbf{J\dot{q} = \dot{x}}
\end{align}

---

Singular Case: No Weighting

\begin{align}
\min_\mathbf{\dot{q}} \left(\mathbf{\dot{x} - J\dot{q}}\right)^\mathrm{T} \left(\mathbf{\dot{x} - J\dot{q}}\right) \\
\text{subject to: } \frac{\partial\mu}{\partial\mathbf{q}}^\mathrm{T}\mathbf{\dot{q}} > 0
\end{align}

---

We need to test the robustness against singularities still. Inside the QP solver it's using LDLT decomposition which should be robust, but it still might be numerically unstable.