Compute the C tensor so that nle(q,vq) = vq C vq.
Noting Cv = C*vq for an arbitrary robot velocity vq:
Since Mdot = (Cv+Cv')/2, then C = dCv/dvq and dM/dq = (Cv+Cv')
Q is index by i,j,k i.e. tau_k = v.T*Q[:,:,k]*v
At level k, Qk is a lower triangular matrix (same shape as H) where:
* rows i s.t. i>k are equals to Sk.T*Ycrb_i*S_ixS_j (j the col index),
* rows i s.t. i<=k are equals to Sk.T*Ycrb_k*S_ixS_j (j the col index)
To avoid the call to Ycrb_i>k, the first par is set up while computing Q_k
Definition at line 125 of file dcrba.py.