Floating base robot q and dq

[jn-tang]

Hi, I'm using Pinocchio to calculate floating base robot forward dynamics and its derivatives, I found that in Pinocchio's convention the base orientation is represented by quaternion, so model.nq = model.nv + 1. But when I calculate derivatives of aba w.r.t q, The dimension of dq is equal to v, hence is 1 less than q, why is this dimension mismatch happens?
Also, I'm a new comer to robotic community, I cannot find any precise definition of free-flyer joint, is there any brief explanation of free-flyer joint?

[fabinsch]

Hi @jn-tang, I recommend reading the book Rigid Body Dynamics Algorithms by Roy Featherstone to understand these concepts. For example, Chapter 4.5 Spherical Motion, explains very well the difference between Euler Angles and Euler Parameters and how you derive the derivatives. Chapter 9.3 Floating Bases introduces you to the concept of free-flyer joints.

[jn-tang]

Thanks @fabinsch for recommendation of this nice book, I glanced the sections you mentioned and found great help.

Actually, I somehow have to implement some functionality in PyTorch based on Pinocchio, given input pose configurations q (and v, tau of course), call Pinocchio's aba function to calculate the accelerations qddot. The forward pass seems OK. But I also need the backward pass to meet my requirement, so I have to call computeABADerivatives for the partial derivatives of qddot w.r.t q, but the dimension of q is 1 larger than that of ddq_dq, therefore the backward pass is broken. What can I do to fix this dimensionality mismatch to correctly back propagate the gradients from qddot to q? I suspect equation (4.13) in Rigid Body Dynamics Algorithms might do the work, but it is dealing with time derivatives.

[cmastalli]

You should operate in the tangent space. This is exactly what we do when optimising trajectories within Crocoddyl.

I do not have enough details of your problem, but it is likely you want to define dq as the input of the network and have a constant q0 posture (kind of nominal posture).

[jn-tang]

I'm building a realtime mocap system that leverages dynamics laws to improve physical plausibility and performance.

The detail of my problem is as follows: to capture human motion, the first stage of the system is a network that somehow regresses joint orientations from observations, and then feeds them into an optimisation module. This module in essence should computes some dynamics quantities, like accelerations, torque exerted on each joints etc. , then optimise these dynamics quantities to make it physically plausible. For some reason I want to end-to-end train such network (by using cvxpylayer I can differentiate an optimisation problem), so the computation chain involving Pinocchio has to be differentiable too. The input to the optimisation module is q0, so I think I cannot work with tangent spaces?

Maybe I could simply replace my current floating-base human model by a fixed-base human model, is this gonna work (just to match the dimension of q and ddq_dq)?

[cmastalli]

You could define a "default configuration" $\mathbf{q}_0$ and use $\Delta\mathbf{q}$ as input to the optimisation layer. You can retrieve the configuration needed to run forward and backward calculations by simply calling $\mathbf{q} = \mathbf{q}_0 \oplus \Delta\mathbf{q}$, where $\oplus$ operator is defined by pinocchio.integrate function.

[jn-tang]

Thanks @cmastalli , I finally understand what you mean by work in tangent spaces. I also found a code snippet from Memory Of Motion summer school that transforms vectors from tangent space into configuration space:

def dExpSO3(quat):
    """
    Return the vector "coefficient-wise" jacobian of w->quat(x)Exp(w), which is a 4x3 matrix,
    where Exp: w->quat2 is the exponential for quaternion, i.e. returning the quaternion quat2
    so that quat2.matrix() is the same as pin.exp3(w); and (x) is the quaternion product.
    The quaternion quat is supposed to have the order (x,y,z,w), and the jacobian rows
    are ordered in the same way.
    """
    x,y,z,w = quat/2
    return  np.array([[  w,  -z,   y],
                      [  z,   w,  -x],
                      [ -y,   x,   w],
                      [ -x,  -y,  -z]])

def dExpQ(rmodel,q):
    """
    Return the vector "coefficient-wise" jacobian of vq -> rmodel.integrate(q,vq), which is a
    NQ x NV matrix.
    The function is ad-hoc and only works for a robot model whose first joint is a free flyer
    and other joints are 1d (euclidean) joints.
    """
    if np.all([ j.nq==1 for j in rmodel.joints]): return np.eye(rmodel.nv)
    assert(rmodel.joints[1].nq==7 and rmodel.joints[1].nv==6)
    for j in rmodel.joints[2:]:
        assert(j.nq==1 and j.nv==1)
    Q = np.zeros([rmodel.nq,rmodel.nv])
    Q[:3,:3] = pin.Quaternion(q[6],q[3],q[4],q[5]).matrix()
    Q[3:7,3:6] = dExpSO3(q[3:7])
    np.fill_diagonal(Q[7:,6:],1)
    return Q

def dExpQ_inv(rmodel,q):
    """
    Return the pseudo inverse of dExpQ (only works for free-flyer+revolute robots).
    """
    if np.all([ j.nq==1 for j in rmodel.joints]): return np.eye(rmodel.nv)
    assert(rmodel.joints[1].nq==7 and rmodel.joints[1].nv==6)
    for j in rmodel.joints[2:]:
        assert(j.nq==1 and j.nv==1)
    Q = np.zeros([rmodel.nv,rmodel.nq])
    Q[:3,:3] = pin.Quaternion(q[6],q[3],q[4],q[5]).matrix().T
    # dExpSO3 is half of a projector Q=.5*P, hence its inverse is twice the projector
    # transposed, i.e. inv(Q) = 2*P.T = 4*Q.T
    Q[3:6,3:7] = 4*dExpSO3(q[3:7]).T
    np.fill_diagonal(Q[6:,7:],1)
    return Q

I think the "dimension mismatch" problem can be solved by multiplying ddq_dq with dExpQ_inv. Once again thank you for your patience.