- Modify the joints for Ty, Ry, Rz to have only one bushing joint. - Compensation of gravity
Next verified that the function to compute the wanted sample position is working => yes