Once the initial pose is selected, throwing the ball is simply a matter of running joints 1, 3, and 5 at their set velocities for RELEASE_TIME seconds, opening the gripper, and then following through. The follow-through is needed because the gripper takes time to release the ball — in order to ensure the ball is actually released at the right speed, we keep that speed up for a short time as the ball is being thrown.
# -----------------------------------------
# --- throw ball (hopefully into a cup) ---
# -----------------------------------------
# motion before release
_ = input('to throw')
t0 = time.time()
while not rospy.is_shutdown():
limb.set_joint_velocities({elbow_joint: elbow_speed, wrist_joint: wrist_speed, shoulder_joint: shoulder_speed})
d = time.time() - t0
if d >= RELEASE_TIME: # THROW_TIME is defined as 0.3 seconds
break
# open gripper
gripper.open()
# follow-through motion
while not rospy.is_shutdown():
limb.set_joint_velocities({elbow_joint: elbow_speed, wrist_joint: wrist_speed, shoulder_joint: shoulder_speed})
d = time.time() - t0
if d >= THROW_TIME: # THROW_TIME is defined as 0.4 seconds
break
print('done!')