Added image of puma560 kinematics for clarity
Improved operation of avoid_joint_limits constraint