Control trajectory in case of joint limit, clamping of joint