def startup():
global joint_pub, tip_pub, mass_pub, active_state, multi_step
if multi_step == 0:
mass_msg = MassChange()
publish_msg(mass_pub, mass_msg)
tip_cmd = PoseCommand()
tip_cmd.pose_follow = True
tip_cmd.pos = deepcopy(curr_state.pos)
tip_cmd.pos.z += 0.3
publish_msg(tip_pub, tip_cmd)
multi_step += 1
elif multi_step == 1:
joint_msg = JointCommand()
publish_msg(joint_pub, joint_msg)
active_state = wait_bottle_dets
multi_step = 0
def grab_bottle_cap():
global detections, tip_pub, mass_pub, multi_step, active_state, g_tip_cmd, g_mass_cmd
detections[CAP_DET].z = 0.18
if math.sqrt(detections[CAP_DET].x**2 +
(detections[CAP_DET].y - 0.112)**2) < 0.75:
detections[CAP_DET].z -= 0.02
print('Grabbing by cap: Stage', multi_step)
if multi_step == 0:
g_tip_cmd = PoseCommand()
g_tip_cmd.pose_follow = True
g_tip_cmd.pos = deepcopy(detections[CAP_DET])
g_tip_cmd.pos.z += 0.05
g_mass_cmd = MassChange()
g_mass_cmd.mass_status = g_mass_cmd.EMPTY
multi_step += 1
elif multi_step == 1:
g_tip_cmd.pos = deepcopy(detections[CAP_DET])
multi_step += 1
elif multi_step == 2:
g_tip_cmd.gripper = 1
g_tip_cmd.wrist_roll = 3.14
multi_step += 1
elif multi_step == 3:
g_mass_cmd.mass_status = g_mass_cmd.BOTTLE_TOP
multi_step += 1
elif multi_step == 4:
g_tip_cmd.pos.z += 0.10
g_tip_cmd.wrist_roll = 0
multi_step = 0
detections[CAP_DET] = None
active_state = return_home
publish_msg(tip_pub, g_tip_cmd)
publish_msg(mass_pub, g_mass_cmd)
def shutdown():
global active_state, tip_pub, mass_pub, g_tip_cmd
print("Killed! Starting shutdown sequence...")
active_state = do_nothing
mass_cmd = MassChange()
publish_msg(mass_pub, mass_cmd)
g_tip_cmd = PoseCommand()
g_tip_cmd.pose_follow = True
g_tip_cmd.pos = deepcopy(curr_state.pos)
g_tip_cmd.pos.z += 0.1
publish_msg(tip_pub, g_tip_cmd)
ros.sleep(ros.Duration(0.2))
while (not curr_state.is_at_target):
ros.sleep(ros.Duration(0.1))
g_tip_cmd.pose_follow = False
g_tip_cmd.pos.x = -0.11
g_tip_cmd.pos.y = 0.45
g_tip_cmd.pos.z = 0.1
publish_msg(tip_pub, g_tip_cmd)
ros.sleep(ros.Duration(0.2))
while (not curr_state.is_at_target):
ros.sleep(ros.Duration(0.1))
g_tip_cmd.pose_follow = True
g_tip_cmd.pos.z = -0.09
publish_msg(tip_pub, g_tip_cmd)
ros.sleep(ros.Duration(0.2))
while (not curr_state.is_at_target):
ros.sleep(ros.Duration(0.1))
print("Shutdown complete!")
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)
want_detections = {
CAP_DET: False,
RIM_DET: False,
BAND_DET: False,
TARGET_DET: False
}
# Publishers
joint_pub = None
tip_pub = None
throw_pub = None
mass_pub = None
# Global commands
g_joint_cmd = JointCommand()
g_tip_cmd = PoseCommand()
g_mass_cmd = MassChange()
# Lock to make this code effectively single-threaded to avoid bugs
lock = Lock()
# Convert a ROS vector to numpy
def toNpVec(vec):
return np.array([vec.x, vec.y, vec.z])
# Convert a numpy vec to ROS
def toRosVec(vec):
rvec = Vector3()
rvec.x = vec[0]
mass_pub = rospy.Publisher('/mass_updates', MassChange, queue_size=10)
rospy.sleep(rospy.Duration(2))
while curr_state.is_at_target == False and not rospy.is_shutdown():
rospy.sleep(rospy.Duration(0.1))
mass = MassChange()
mass.mass_status = MassChange.EMPTY
mass_pub.publish(mass)
cmd = JointCommand()
pub_and_wait(joint_pub, cmd)
cmd = PoseCommand()
cmd.pos.x = 0.63
cmd.pos.y = 0.20
cmd.pos.z = 0.2
cmd.wrist_angle = 0
cmd.wrist_roll = -2.4
cmd.gripper = 0
cmd.pose_follow = True
pub_and_wait(tip_pub, cmd)
cmd.pos.z = -0.02
pub_and_wait(tip_pub, cmd)
cmd.gripper = 1