Handle trajectories with non checked joints
update computekinematicconstraints
Update test cases