Control trajectory to avoid joint limit as well as obstacle collision