Control of trajectory in case of a limb, joint disturbation, failure