added moving up after gripper closes
Need to completely revamp the gripper movement. Use trajopt
Working on ArmControl, which will replace CommandPose and CommandTwist