Equality constraints (m_eq) in formulation

[jacqueszhong]

Hello,
I was checking inverse-dynamics-formulation-acc-force.cpp I have a few questions about the variable m_eq for hard equality constraints.

Firstly, I'm not sure why we add 6 constraints at line 43. If it's to enforce the base position, why use a hard constraint ?

Secondly, for Contact6D, I use a soft priority for the motion_task (which gives me better stability overall). So I'm not sure why we still increment m_eq at line 242 in this case.

Thanks in advance.

[andreadelprete]

Dear @jacqueszhong

we add 6 constraints at line 43 to enforce the floating base dynamics, which is not the same thing as enforcing the base position. We don't need to enforce the remaining lines of the dynamics (i.e. the joint dynamics) because we don't use the joint torques as variables of the problem. Read the wiki of this repository for more details about this.

Regarding your second point, I think it might be a bug. I agree that we shouldn't increase the number of equality constraints if we add the motion constraints as penalties. I'll check that tomorrow. I am surprised all the tests made with soft contact constraints have not caught this bug though.

[andreadelprete]

You were right, the second point you raised was indeed a bug. I've fixed it with my last commit to devel.