Position Update in Chrono

[Milad Rakhsha]

Hi All,

I want to ask for some details about the position update and the rigid body integration in chrono. 
Looking at the code and the documentation, it seems the underlying system is solved at the velocity level through the so-called r-omega formulation where velocities are in R^6. However, the position generalized coordinates for each body are positions and Euler parameters (quaternions) in R^7.

If I am looking at the right place, the position gets updated according to 

void ChBody::VariablesQbIncrementPosition(double dt_step)

where 

• Wvel is first transformed to the global frame w
• the increment in the angle of rotation is calculated via d_theta=||w||*dt
• the axis of rotation is computed via n=w/||w||
• a quaternion is formed via p=(d_tetha, n)
• the orientation of the body is updated via  p_new= p o p_old where o denotes the quaternion product

My questions:

1. How/where is it ensured that the quaternions remain normalized? Is this a manual re-normalization, or is there a constraint  to ensure ||p||=1? where can I see more about this? Currently 
   virtual void SetRot(const ChQuaternion<Real>& mrot) 
   in ChFrame.h requires that the mrot be already be normalized, but is this guaranteed by the quaternion product operation p_new= p o p_old ? 
2. Regarding the mass matrix, it should remain as diag(m,m,m,Ixx,Iyy,Izz), because the velocities are not the time-derivative of the position coordinates. Otherwise, some transformation in the form of L^T*M*L would be necessary, is that right? here, L in R^{7*nb,6*nb} is the kinematic map matrix used in transforming velocities to the time-derivative of the position coordinates as \dot{q}=L*u
3. Consider a case where the integrator needs to solve equations for positions like M*(q-q_old)+Cq*lambda=0, how is this possible with the current formulation? Would this require a lot of changes? Because here for instance the mass matrix should look like something like L^T*M*L \in R^{7*nb, 7*nb}. 
4. Also, could you confirm that the Jacobian matrices denoted by Cq are actually (\partial C/ \partial q) * L not the partial derivative of the constraint w.r.t the generalized coordinates (\partial C/ \partial q)?  Because if Cq is \partial C/ \partial q then the size of the Jacobian for a single constraint should be 1*7 not 1*6

Thanks

Milad