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,