Motion control routines for the manipulator and gripper

Summary
Motion control routines for the manipulator and gripper