Bug fix :
Silent out of bound access in checkIfJointsWithinBounds wreak havoc later (random segfault/memory corruption) :
joint->getFirstVariableIndex() don't match and exceed new_jt_traj.points[0].velocities indexes when there is more joints in the robot model than in the move_group used.
Bug fix : Silent out of bound access in checkIfJointsWithinBounds wreak havoc later (random segfault/memory corruption) : joint->getFirstVariableIndex() don't match and exceed new_jt_traj.points[0].velocities indexes when there is more joints in the robot model than in the move_group used.