Joint posture task computation incorrect for robot with `nq != nv`

Hello,

I noticed a computation error for the joint posture task if nq != nv :

Lines 157 to 163 in b0d6bff

    
           m_p = q.tail(m_robot.na()); 
        
           m_v = v.tail(m_robot.na()); 
        
           m_p_error = m_p - m_ref.getValue(); 
        
           m_v_error = m_v - m_ref.getDerivative(); 
        
           m_a_des = - m_Kp.cwiseProduct(m_p_error) 
        
                     - m_Kd.cwiseProduct(m_v_error) 
        
                     + m_ref.getSecondDerivative();

The configuration vector is cropped to the wrong size to remove the root joint (because nq != nv):

tsid/src/tasks/task-joint-posture.cpp

Line 157 in b0d6bff

m_p = q.tail(m_robot.na());

And the error is compute with a term by term difference:

tsid/src/tasks/task-joint-posture.cpp

Line 159 in b0d6bff

m_p_error = m_p - m_ref.getValue();

Instead I propose this implementation (using pinocchio::difference and by cropping the error, instead of the input configuration)

m_v = v.tail(m_robot.na()); 

m_p_error = pinocchio::difference(m_robot.model(), m_p, m_ref.getValue());
m_p_error = m_p_error.tail(m_robot.na());

m_v_error = m_v - m_ref.getDerivative(); 
m_a_des = - m_Kp.cwiseProduct(m_p_error) 
          - m_Kd.cwiseProduct(m_v_error) 
          + m_ref.getSecondDerivative();

I will open a PR, because this will cause some other small changes in the code.

Also, I think this mistake is done at several places in the code. I could try to track them and patch them too.
What do you think ?

Very nice initiative @EtienneAr. This is fine on my side.

Thanks @EtienneAr and @jcarpent for the comment and the review.
I am a bit surprise because we have been using this task quite a lot.
Where does the problem comes from ?

tsid/src/tasks/task-joint-posture.cpp

Lines 157 to 159 in b0d6bff

    
           m_p = q.tail(m_robot.na()); 
        
           m_v = v.tail(m_robot.na()); 
        
           m_p_error = m_p - m_ref.getValue();

looks good to me if we assume that the free-flyer is at the beginning of the vector.
Is this because the reference value m_ref is not given properly (i.e. with the free-flyer instead of without the free-flyer) ?

I think @olivier-stasse is right. There is no bug in the code. The reference q should contain only the joint angles because the posture task controls only the joint angles, without the floating base.

Yes, but our URDF contains joints where nq != nv (in our case revolute unbounded joint, but it could also be spherical joint).
Thus, in this case difference(q_1, q_2) != q_2 - q_1. And it doesn't make sense to crop q using na because na = nv_robot - nv_rootjoint != nq_robot - nq_rootjoint.

I agree that this is a rare case, as most of the time robots will be composed exclusively of revolute and prismatic joints (root joint excluded).

Ok, now I see. However, the code you suggested assumes that q_ref contains the whole robot configuration (floating-base included), which is not how the posture task should work, so that should be fixed.

Thanks @EtienneAr for the clarification, this is indeed a case that we did not yet face.

Ok, now I see. However, the code you suggested assumes that q_ref contains the whole robot configuration (floating-base included), which is not how the posture task should work, so that should be fixed.

Ok, I see. I am thinking of a good way to do it. Do you have suggestions ?

Create an augmented reference vector that contains also the floating-base configuration, you copy the joint part from the original reference vector, do the difference with pinocchio::difference, and then take the tail of the result.

Also, I think the first step to fix this would be to provide a regression test: you can start by writing a test which fails as in your use case.

We should also ensure that we have another test that pass in use cases similar to what was already used.

Then it will be easy to check in any subsequent fix will make the new unit test pass while not breaking the other one.

Hello, I wrote some test (see #161 (comment)) :)

Create an augmented reference vector that contains also the floating-base configuration, you copy the joint part from the original reference vector, do the difference with pinocchio::difference, and then take the tail of the result.

Yes, it is what I planned to do.
But the "trickiest" part for me is to find a proper way to get the robot nq without the rootJoint (to size properly the reference vector, to update this assert, ... ). Because the rootJoint type (or rootJoint nq) are not 'saved' anywhere and only na is kept...

If I add it to the RobotWrapper properties, what name should I give it ?

Maybe we can add a method bool has_floating_base to RobotWrapper, which you can use to decide whether to subtract 7 from nq to get the size of the joint posture reference?

Closing since #161 has been merged.
Thank you.

	m_p = q.tail(m_robot.na());
	m_v = v.tail(m_robot.na());
	m_p_error = m_p - m_ref.getValue();
	m_v_error = m_v - m_ref.getDerivative();
	m_a_des = - m_Kp.cwiseProduct(m_p_error)
	- m_Kd.cwiseProduct(m_v_error)
	+ m_ref.getSecondDerivative();