Modified RNEA for passivity-based control

[Cfather]

Hi, I have a simple question. Will there be a modified rnea implementation to compute

M(q) \ddot{q}_r + C(q, \dot{q}) \dot{q}_r + g(q)

for passivity-based control.
An example paper is here. I guess it's some simple modification to the existing algorithm, but will need some extra data structure to store intermediate variables.
Thanks for your attention!

[jcarpent]

Could you explain further your notations, notably the _r ?

[Cfather]

\dot{q}_r = \dot{q}_d + K_r (q_d - q) is defined as auxiliary velocity, where q_d is the desired position and \dot{q}_d is the desired velocity. K_r is a positive definite diagonal matrix, that serves as feedback gains.
A more detailed introduction can be found in Section 6.5.3 in Springer Handbook of Robotics.
But in any case, the difference between this and RNEA is that it needs to plug in two different velocities in the Coriolis vector.

[jcarpent]

Thanks for the quick update.

[pwensing]

@jcarpent In case it helps grease the skids on your side, you can find a MATLAB version of a modified ID that accomplishes this goal in my repo (link). I think this one should be a little easier to integrate than the DeLuca Paper due to its closer connection with your current ID method. (Note: qdd in the code I linked plays the same role as \ddot{q}_r in Bohao's original post here).

The implementation is based on Niemeyer and Slotine's paper (link, see Table 1). The main differences from the regular RNEA are that you need to

1. propagate both the true velocity $v$, and a reference velocity $w$ for each link [so, some new data structure is needed]
2. modify the acceleration propagation slightly $a_i = {}^iX_{p(i)} a_{p(i)} + v_i\times S_i \dot{q}_{r,i} + S_i \ddot{q}_{r,i}$
3. modify the way the body Coriolis force calculates $v\times I v$. Lot's of options there, but the simplest would be $v \times I w$ like you had in the original Coriolis matrix computations. I have three options in my repo, but if you were to pick one, I'd use the updated factorization that you implemented in Extend current Coriolis computations to account for Cristoffel symbol of first kind #1665 since it is a more canonical choice.

I hope this might help better scope the required changes if this were to move forward.

[jcarpent]

Thanks a lot @pwensing for the nice detailed explanations.

[Cfather]

Thank you very much @pwensing for the explanation.
I also have a similar implementation and that would be what I propose to be implemented in pinocchio. It should be some small changes in pinocchio's original RNEA algorithm and should be the fastest in terms of computation time.

One warning though (maybe others want to cross-validate this for me):
The output of this modified RNEA will NOT be the same as computing things explicitly. To be more specific, you can also do something like:

Eigen::MatrixXd M = pinocchio::crba(model, data, q);
for (int i = 0; i < M.rows(); i++) {
    for (int j = i + 1; j < M.cols(); j++) {
        M(j, i) = M(i, j);
    }
}
pinocchio::computeCoriolisMatrix(model, data, q, q_d);
pinocchio::computeGeneralizedGravity(model, data, q);
tau_modified = M * q_dd + data.C * q_aux_d + data.g;

However, the result here is different from that from the modified RNEA algorithm proposed by @pwensing, since these two methods essentially generate DIFFERENT Coriolis matrix.

[pwensing]

@Cfather, Regarding

However, the result here is different from that from the modified RNEA algorithm proposed by @pwensing, since these two methods essentially generate DIFFERENT Coriolis matrix.

As long as one is careful, these two methods will generate the same output. Specifically, one has to be careful that the body-level Coriolis factorization used in the computeCoriolisMatrix method is the same as the one adopted in the modifications to the RNEA.

For example, there is a body-level factorization adopted for the Coriolis matrix calculations here

pinocchio/include/pinocchio/algorithm/rnea.hxx

Lines 549 to 550 in 0b594a0

	data.B[i] = data.oYcrb[i].variation(Scalar(0.5) * data.ov[i]);
	addForceCrossMatrix(Scalar(0.5) * data.oh[i], data.B[i]);

which satisfies $v_i \times I_i v_i = B_i v_i$. (There's a more detailed discussion about this line in #1663.)

As long as modification 3 in my original post above is used to replace $v_i \times I_i v_i$ in the RNEA with a term $B_i w_i$, then the explicit formula you propose will match the output of that modified RNEA.

You can find unit test of this claim in MATLAB here (link, see Line 75).

[Cfather]

Thank you very much for your response! @pwensing
This makes a lot of sense to me now.

[jcarpent]

@Cfather, why not give it a try? We can have a quick chat to see what remains to do.

[Cfather]

Yep. I can have a try on this next week.

[Cfather]

Hi, @jcarpent, I have submitted a pull request #2421.
Could you take a look whenever you are available? I'm happy to make any changes to merge the devel branch.
And thanks again to @pwensing for your nice explanations!