def upright_bottle():
global detections, tip_pub, mass_pub, multi_step, active_state, g_tip_cmd, g_mass_cmd
band = toNpVec(detections[BAND_DET])
rim = toNpVec(detections[RIM_DET])
band[2] = 0
rim[2] = 0
ori = rim - band
ori /= np.linalg.norm(ori)
angle = math.atan2(ori[1], ori[0])
grab_point = band - 0.06 * ori
grab_vec = toRosVec(grab_point)
print('Uprighting bottle at angle', angle, ': Stage', multi_step)
if multi_step == 0:
g_tip_cmd = PoseCommand()
g_tip_cmd.pose_follow = True
g_tip_cmd.pos = deepcopy(grab_vec)
g_tip_cmd.pos.z += 0.1
g_tip_cmd.wrist_roll = angle
g_mass_cmd = MassChange()
g_mass_cmd.mass_status = g_mass_cmd.EMPTY
multi_step += 1
elif multi_step == 1:
g_tip_cmd.pos.z -= 0.1
multi_step += 1
elif multi_step == 2:
g_tip_cmd.gripper = 1
multi_step += 1
elif multi_step == 3:
g_mass_cmd.mass_status = g_mass_cmd.BOTTLE_SIDE
multi_step += 1
elif multi_step == 4:
g_tip_cmd.pos.z += 0.2
g_tip_cmd.pos.y = 0
g_tip_cmd.pos.x = 0.8
multi_step += 1
elif multi_step == 5:
g_tip_cmd.wrist_roll = 0
g_tip_cmd.wrist_angle = 1.57
multi_step += 1
elif multi_step == 6:
g_tip_cmd.pos.z -= 0.15
multi_step += 1
elif multi_step == 7:
g_mass_cmd.mass_status = g_mass_cmd.EMPTY
multi_step += 1
elif multi_step == 8:
g_tip_cmd.gripper = 0
multi_step += 1
elif multi_step == 9:
g_tip_cmd.pos.z += 0.25
detections[BAND_DET] = None
detections[RIM_DET] = None
active_state = wait_bottle_dets
multi_step = 0
publish_msg(tip_pub, g_tip_cmd)
publish_msg(mass_pub, g_mass_cmd)
def drag_bottle():
global detections, tip_pub, g_tip_cmd, multi_step, active_state
if detections[CAP_DET] is not None:
target = detections[CAP_DET]
else:
target = detections[BAND_DET]
if multi_step == 0:
g_tip_cmd = PoseCommand()
g_tip_cmd.pose_follow = True
g_tip_cmd.pos.x = -0.01
g_tip_cmd.pos.y = 0.66
g_tip_cmd.pos.z = 0.1
g_tip_cmd.wrist_roll = -1.57
multi_step += 1
elif multi_step == 1:
g_tip_cmd.pos.z = 0.04
multi_step += 1
elif multi_step == 2:
g_tip_cmd.gripper = 1
multi_step += 1
elif multi_step == 3:
g_tip_cmd.pos.z = 0.2
multi_step += 1
elif multi_step == 4:
g_tip_cmd.wrist_angle = 1.57
g_tip_cmd.wrist_roll = 1.57
g_tip_cmd.pos.z = 0.3
multi_step += 1
elif multi_step == 5:
g_tip_cmd.pos = target
g_tip_cmd = hoeTipToGripperTip(g_tip_cmd)
g_tip_cmd.pos.z = 0.5
multi_step += 1
elif multi_step == 6:
g_tip_cmd.pos.z = 0.1
multi_step += 1
elif multi_step == 7:
v = toNpVec(g_tip_cmd.pos)
g_tip_cmd.pos = toRosVec((0.55 / np.linalg.norm(v)) * v)
multi_step += 1
elif multi_step == 8:
g_tip_cmd.pos.z = 0.5
multi_step += 1
elif multi_step == 9:
g_tip_cmd.pos.x = -0.03
g_tip_cmd.pos.y = 0.66
g_tip_cmd.pos.z = 0.3
g_tip_cmd.wrist_roll = 3.14
multi_step += 1
elif multi_step == 10:
g_tip_cmd.pos.x = -0.01
g_tip_cmd.wrist_roll = 3.14 + 1.57
g_tip_cmd.wrist_angle = 0
g_tip_cmd.pos.z = 0.2
multi_step += 1
elif multi_step == 11:
g_tip_cmd.pos.z = 0.05
multi_step += 1
elif multi_step == 12:
g_tip_cmd.gripper = 0
multi_step += 1
elif multi_step == 13:
g_tip_cmd.pos.z = 0.3
multi_step = 0
active_state = wait_bottle_dets
detections[CAP_DET] = None
detections[BAND_DET] = None
detections[RIM_DET] = None
publish_msg(tip_pub, g_tip_cmd)