def main():
# initiate ros, robot, assign variables...
rospy.init_node("pour_water_demo")
rs = baxter_interface.RobotEnable(CHECK_VERSION)
rs.enable()
gripper("both", "open")
# limb = "left"
# angles = dict(zip(baxter_interface.limb.Limb(limb).joint_names(),[-1.0787719878479005, 1.604927397491455, 1.0204807179748536, -0.23469906027832033, 0.2304806131164551, 1.5243934062194826, 1.9527575407470705]))
# baxter_interface.limb.Limb(limb).move_to_joint_positions(angles)
# move to natural position
# baxter_interface.limb.Limb("left").move_to_neutral()
# baxter_interface.limb.Limb("right").move_to_neutral()
pick_up_cylinder.move_to_start("left")
pick_up_cylinder.move_to_start("right")
print "ready for operation"
"""step 1: detect cup put on coaster"""
coaster_position = None
while True:
coaster_position = get_object_position("Coaster")
if coaster_position == False:
break
last_position = coaster_position
rospy.sleep(0.01)
coaster_position = last_position
print ("detect cup on coaster!"), coaster_position
"""step 2: check with user"""
try:
srv_h = rospy.ServiceProxy("/socket_android/android_interact", android_interact)
resp = srv_h(
"Hello, do you want chocolate m & m's or peanut m & m's ?/45000",
"I repeat, which m & m's do you want? chocolate or peanut/4000",
"OK",
["chocolate", "peanut"],
)
except rospy.ServiceException, e:
print "service dall failed: %s" % e
# drop to rotate
gripper(limb, "open")
# move away
move_keep_orientation(limb, 0, 0, -0.01, 1)
rospy.sleep(0.5)
move_keep_orientation(limb, -0.025, 0, 0, 1)
rospy.sleep(0.5)
move_keep_orientation(limb, 0, 0.1, 0, 1)
move_keep_orientation(limb, 0, 0, 0.3, 1.5)
rotate_object.rotate_to_original(limb, object)
move_keep_orientation(limb, 0, 0, 0.1, 1.5)
move_keep_orientation(limb, 0, 0.2, 0.15, 3.5)
# pick up
pick_up_cylinder.move_to_start(limb)
pick_up_cylinder.find_grip_cylinder(limb, object)
# pick_up_cylinder.find_grip_cylinder(limb,object)
move_keep_orientation(limb, 0, 0, 0.3, 1.5)
"""STEP 6: PRE POURING POSE"""
moveTrajectory(
"right",
[
ik_position_list(
"right",
0.649274286384,
-0.124420979527,
0.0556079809065,
-0.601030426405,
