def execute(self, userdata): process_stereo = rospy.ServiceProxy("clump_center_node/process_mono", ProcessMono) resp = process_stereo("wide_stereo/left") pt = resp.pts3d[0] z_offset = 0.06 GripUtils.go_to_pt(pt, roll=pi / 2, yaw=0, pitch=pi / 2, arm="l", z_offset=z_offset, grip=False, dur=5.0) GripUtils.close_gripper("l") while not GripUtils.has_object("l") and not rospy.is_shutdown(): z_offset -= 0.02 if (z_offset < 0): return FAILURE GripUtils.go_to_pt(pt, roll=pi / 2, yaw=0, pitch=pi / 2, arm="l", z_offset=z_offset, grip=False, dur=5.0) GripUtils.close_gripper("l") return SUCCESS
def open_sock(): x=0.5 y=0.0 z=TABLE_HEIGHT+0.4 frame="base_footprint" #old strategy GripUtils.go_to(x=x,y=y+0.007,z=z,roll=pi/2,yaw=pi/2,pitch=-pi/8,grip=True,frame=frame,arm="r",dur=5.0) GripUtils.go_to(x=x,y=y+0.05,z=z,roll=pi/2,yaw=-pi/2,pitch=0,grip=False,frame=frame,arm="l",dur=5.0) GripUtils.go_to(x=x,y=y-0.004,z=z+0.01,roll=pi/2,yaw=-pi/2,pitch=pi/5,grip=False,frame=frame,arm="l",dur=5.0) GripUtils.go_to(x=x,y=y-0.0035,z=z+0.001,roll=pi/2,yaw=-pi/2,pitch=pi/5,grip=True,frame=frame,arm="l",dur=2.0) #changed from -0.007 to -0.0065 #GripUtils.go_to(x=x,y=y,z=z,roll=pi/2,yaw=pi/2,pitch=0,grip=True,frame=frame,arm="r",dur=5.0) #GripUtils.go_to(x=x,y=y+0.04,z=z+0.05,roll=pi/2,yaw=-pi/2,pitch=pi/2,grip=False,frame=frame,arm="l",dur=5.0) #GripUtils.go_to(x=x,y=y+0.04,z=z+0.005,roll=pi/2,yaw=-pi/2,pitch=pi/2,grip=False,frame=frame,arm="l",dur=5.0) #GripUtils.go_to(x=x,y=y,z=z,roll=pi/2,yaw=pi/2,pitch=-pi/16,grip=True,frame=frame,arm="r",dur=5.0) #GripUtils.go_to(x=x,y=y+0.06,z=z-0.025,roll=pi/2,yaw=-pi/2,pitch=pi/2,grip=False,frame=frame,arm="l",dur=5.0) #GripUtils.go_to(x=x,y=y+0.025,z=z-0.225,roll=pi/2,yaw=-pi/2,pitch=pi/2,grip=True,frame=frame,arm="l",dur=1.0) #GripUtils.go_to(x=x,y=y+0.025,z=z,roll=pi/2,yaw=-pi/2,pitch=0,grip=True,frame=frame,arm="l",dur=5.0) GripUtils.close_gripper("l",extra_tight=True) open_amt = 0.025 GripUtils.go_to_multi (x_l=x,y_l=y+open_amt,z_l=z+0.01,roll_l=pi/2,yaw_l=-pi/2,pitch_l=0,grip_l=True,frame_l=frame ,x_r=x,y_r=y-open_amt,z_r=z+0.01,roll_r=pi/2,yaw_r=pi/2,pitch_r=0,grip_r=True,frame_r=frame ,dur=5.0)
def take_off_dopple(pt,arm): x = DOPPLE_X y = DOPPLE_Y if arm == "l": yaw = -pi/2 y -= 0.005 else: yaw = pi/2 y += 0.005 z = DOPPLE_HEIGHT+TABLE_HEIGHT - 0.045 frame = "base_footprint" GripUtils.go_to(x=x,y=y,z=z,roll=0,yaw=yaw,pitch=pi/4,arm=arm,grip=False,frame=frame,dur=5.0) GripUtils.close_gripper(arm,extra_tight=False) GripUtils.go_to(x=x,y=y,z=z+0.1,roll=0,yaw=yaw,pitch=0,arm=arm,grip=True,frame=frame,dur=5.0) GripUtils.go_to(x=x,y=y,z=z+0.2,roll=0,yaw=yaw,pitch=0,arm=arm,grip=True,frame=frame,dur=5.0) roll = 0 if arm=="l": yaw = -pi/2 else: yaw = pi/2 GripUtils.go_to(x=x-0.15,y=y,z=z+0.2,roll=roll,yaw=yaw,pitch=0,arm=arm,grip=True,frame=frame,dur=5.0) #Spreads out GripUtils.go_to_pt(pt,roll=roll,yaw=yaw,pitch=0,arm=arm,grip=True,z_offset = 0.2,y_offset=0.2,dur=5.0) # previously z_o= 0.08 y_o 0.1 GripUtils.go_to_pt(pt,roll=roll,yaw=yaw,pitch=pi/2,arm=arm,grip=True,dur=7.5,y_offset = -0.2,z_offset=0.015) GripUtils.open_gripper(arm) GripUtils.go_to_pt(pt,roll=roll,yaw=yaw,pitch=pi/4,arm=arm,grip=False,dur=2.5,y_offset = -0.2,z_offset=0.025) StanceUtils.call_stance("arms_up",5.0)
def __init__(self, title=None, transitions=None): NestedStateMachine.__init__(self, title, transitions=transitions, outcomes=DEFAULT_OUTCOMES) pr2_say(talk_greet) self.add('Arms_Up', ArmsUp(grip=False), { SUCCESS: 'Look_Down', FAILURE: FAILURE }) self.add('Look_Down', StanceState('look_down'), { SUCCESS: SUCCESS, FAILURE: FAILURE }) # Calibrate open/close tolerances automatically GripUtils.close_gripper('b') rospy.sleep(1.0) joint_states_msg = rospy.wait_for_message('joint_states', JointState) joint_states = dict( zip(joint_states_msg.name, joint_states_msg.position)) l_eps = joint_states['l_gripper_joint'] r_eps = joint_states['r_gripper_joint'] rel_tol = 0.1 abs_tol = 0.0001 l_has_obj_min = l_eps + max(abs_tol, abs(l_eps * rel_tol)) r_has_obj_min = r_eps + max(abs_tol, abs(l_eps * rel_tol)) rospy.set_param('l_has_obj_min', l_has_obj_min) rospy.set_param('r_has_obj_min', r_has_obj_min) pr2_say(talk_initialize)
def add_default_stances(self): rospy.Service("stances/pause", ExecuteStance, self.pause) rospy.Service("stances/open_left", ExecuteStance, lambda req: GripUtils.open_gripper("l")) rospy.Service("stances/open_right", ExecuteStance, lambda req: GripUtils.open_gripper("r")) rospy.Service("stances/open_both", ExecuteStance, lambda req: GripUtils.open_gripper("b")) rospy.Service("stances/close_left", ExecuteStance, lambda req: GripUtils.close_gripper("l")) rospy.Service("stances/close_right", ExecuteStance, lambda req: GripUtils.close_gripper("r")) rospy.Service("stances/close_both", ExecuteStance, lambda req: GripUtils.close_gripper("b")) self.default_stances = ()
def take_out_cloth(): print "Taking out cloth" RosUtils.call_service("move_torso",MoveTorso,height=0.01) DryerNavigationUtils.goToPosition("enter_dryer") GripUtils.go_to(x=-0.1,y=-0.33,z=-0.4,roll=0,pitch=0,yaw=0,grip=False,arm="l",frame="dryer") DryerNavigationUtils.goToPosition("into_dryer") GripUtils.go_to(x=0.15,y=-0.33,z=-0.65,roll=0,pitch=pi/2,yaw=0,grip=False,arm="l",frame="dryer") GripUtils.close_gripper("l") GripUtils.go_to(x=0.15,y=-0.33,z=-0.4,roll=0,pitch=0,yaw=0,grip=True,arm="l",frame="dryer") DryerNavigationUtils.goToPosition("enter_dryer") RosUtils.call_service("move_torso",MoveTorso,height=0.3) return True
def take_off_dopple_pair(): x = DOPPLE_X y = DOPPLE_Y yaw = pi/2 y += 0.005 z = DOPPLE_HEIGHT+TABLE_HEIGHT - 0.045 frame = "base_footprint" GripUtils.go_to(x=x,y=y,z=z,roll=0,yaw=yaw,pitch=pi/4,arm="r",grip=False,frame=frame,dur=5.0) GripUtils.close_gripper("r",extra_tight=False) GripUtils.go_to(x=x,y=y,z=z+0.1,roll=pi/2,yaw=yaw,pitch=0,arm="r",grip=True,frame=frame,dur=5.0) GripUtils.go_to(x=x,y=y,z=z+0.2,roll=pi/2,yaw=yaw,pitch=0,arm="r",grip=True,frame=frame,dur=5.0) GripUtils.go_to(x=x-0.2,y=y,z=z+0.2,roll=pi/2,yaw=yaw,pitch=0,arm="r",grip=True,frame=frame,dur=5.0)
def execute(self, userdata): process_mono = rospy.ServiceProxy("clump_center_node_white/process_mono",ProcessMono) resp = process_mono("wide_stereo/left") pt = resp.pts3d[0] z_offset = 0.06 GripUtils.go_to_pt(pt,roll=pi/2,yaw=0,pitch=pi/2,arm="l",z_offset=z_offset,grip=False,dur=5.0) GripUtils.close_gripper("l") while not GripUtils.has_object("l") and not rospy.is_shutdown(): z_offset -= 0.02 if(z_offset < 0): return FAILURE GripUtils.go_to_pt(pt,roll=pi/2,yaw=0,pitch=pi/2,arm="l",z_offset=z_offset,grip=False,dur=5.0) GripUtils.close_gripper("l") return SUCCESS
def take_off_dopple_pair(): x = DOPPLE_X y = DOPPLE_Y yaw = pi / 2 y += 0.005 z = DOPPLE_HEIGHT + TABLE_HEIGHT - 0.045 frame = "base_footprint" GripUtils.go_to(x=x, y=y, z=z, roll=0, yaw=yaw, pitch=pi / 4, arm="r", grip=False, frame=frame, dur=5.0) GripUtils.close_gripper("r", extra_tight=False) GripUtils.go_to(x=x, y=y, z=z + 0.1, roll=pi / 2, yaw=yaw, pitch=0, arm="r", grip=True, frame=frame, dur=5.0) GripUtils.go_to(x=x, y=y, z=z + 0.2, roll=pi / 2, yaw=yaw, pitch=0, arm="r", grip=True, frame=frame, dur=5.0) GripUtils.go_to(x=x - 0.2, y=y, z=z + 0.2, roll=pi / 2, yaw=yaw, pitch=0, arm="r", grip=True, frame=frame, dur=5.0)
def execute(self, userdata): pr2_say(talk_pickupclump) process_mono = rospy.ServiceProxy("clump_center_node/process_mono", ProcessMono) resp = process_mono("wide_stereo/left") pt = resp.pts3d[0] z_offset = 0.06 GripUtils.go_to_pt(pt, roll=pi / 2, yaw=0, pitch=pi / 2, arm="l", z_offset=z_offset, grip=False, dur=5.0) GripUtils.close_gripper("l") while not GripUtils.has_object("l") and not rospy.is_shutdown(): print "Z_offset = %f" % z_offset z_offset -= 0.02 if z_offset < -0.02: return FAILURE GripUtils.go_to_pt(pt, roll=pi / 2, yaw=0, pitch=pi / 2, arm="l", z_offset=z_offset, grip=False, dur=5.0) GripUtils.close_gripper("l") GripUtils.recall_arm("l", grip=True) return SUCCESS
def pickup_sock(sock): z_off = 0.0 """ while not rospy.is_shutdown(): StanceUtils.call_stance("close_right",5.0) (grip_pt,angle) = get_grip_point(sock) GripUtils.go_to_pt(grip_pt,roll=pi/2,yaw=pi/2-angle,pitch=pi/2,arm="r",grip=True,z_offset=z_off) rospy.sleep(5.0) z_off = float(raw_input("What is the new z_offset?")) StanceUtils.call_stance("arms_up",5.0) """ (grip_pt,angle) = get_grip_point(sock) if MODE==THICK_SOCK: OFFSET = 0.02 else: OFFSET = 0.025 y_offset = OFFSET*cos(angle) x_offset = OFFSET*sin(angle) #x_offset += 0.005 z_offset = 0.02 pitch = pi/2 roll = pi/2 yaw = pi/2-angle GripUtils.go_to_pt(grip_pt,roll=roll,yaw=yaw,pitch=pitch,arm="r",x_offset=x_offset,y_offset=y_offset,z_offset=z_offset,grip=False,dur=5.0) z_offset = -0.02 z_offset -= 0.001 GripUtils.go_to_pt(grip_pt,roll=roll,yaw=yaw,pitch=pitch,arm="r",x_offset=x_offset,y_offset=y_offset,z_offset=z_offset,grip=False,dur=5.0) #Drag thin socks if MODE == THIN_SOCK: y_offset += 0.02 GripUtils.go_to_pt(grip_pt,roll=roll,yaw=yaw,pitch=pitch,arm="r",x_offset=x_offset,y_offset=y_offset,z_offset=z_offset,grip=False,dur=5.0) while not GripUtils.has_object("r"): StanceUtils.call_stance("open_right",2.0) pitch -= ANGLE_INCREMENT y_offset -= 0.005*cos(angle) x_offset -= 0.005*sin(angle) z_offset += 0.0015 GripUtils.go_to_pt(grip_pt,roll=roll,yaw=yaw,pitch=pitch,arm="r",x_offset=x_offset,y_offset=y_offset,z_offset=z_offset,grip=False,dur=5.0) GripUtils.close_gripper("r",extra_tight=True) break return grip_pt
def __init__(self, title=None, transitions=None): NestedStateMachine.__init__(self, title, transitions=transitions, outcomes=DEFAULT_OUTCOMES) pr2_say(talk_greet) self.add("Arms_Up", ArmsUp(grip=False), {SUCCESS: "Look_Down", FAILURE: FAILURE}) self.add("Look_Down", StanceState("look_down"), {SUCCESS: SUCCESS, FAILURE: FAILURE}) # Calibrate open/close tolerances automatically GripUtils.close_gripper("b") rospy.sleep(1.0) joint_states_msg = rospy.wait_for_message("joint_states", JointState) joint_states = dict(zip(joint_states_msg.name, joint_states_msg.position)) l_eps = joint_states["l_gripper_joint"] r_eps = joint_states["r_gripper_joint"] rel_tol = 0.1 abs_tol = 0.0001 l_has_obj_min = l_eps + max(abs_tol, abs(l_eps * rel_tol)) r_has_obj_min = r_eps + max(abs_tol, abs(l_eps * rel_tol)) rospy.set_param("l_has_obj_min", l_has_obj_min) rospy.set_param("r_has_obj_min", r_has_obj_min) pr2_say(talk_initialize)
def take_out_cloth(): print "Taking out cloth" RosUtils.call_service("move_torso", MoveTorso, height=0.01) DryerNavigationUtils.goToPosition("enter_dryer") GripUtils.go_to(x=-0.1, y=-0.33, z=-0.4, roll=0, pitch=0, yaw=0, grip=False, arm="l", frame="dryer") DryerNavigationUtils.goToPosition("into_dryer") GripUtils.go_to(x=0.15, y=-0.33, z=-0.65, roll=0, pitch=pi / 2, yaw=0, grip=False, arm="l", frame="dryer") GripUtils.close_gripper("l") GripUtils.go_to(x=0.15, y=-0.33, z=-0.4, roll=0, pitch=0, yaw=0, grip=True, arm="l", frame="dryer") DryerNavigationUtils.goToPosition("enter_dryer") RosUtils.call_service("move_torso", MoveTorso, height=0.3) return True
def open_sock(): x = 0.5 y = 0.0 z = TABLE_HEIGHT + 0.4 frame = "base_footprint" #old strategy GripUtils.go_to(x=x, y=y + 0.007, z=z, roll=pi / 2, yaw=pi / 2, pitch=-pi / 8, grip=True, frame=frame, arm="r", dur=5.0) GripUtils.go_to(x=x, y=y + 0.05, z=z, roll=pi / 2, yaw=-pi / 2, pitch=0, grip=False, frame=frame, arm="l", dur=5.0) GripUtils.go_to(x=x, y=y - 0.004, z=z + 0.01, roll=pi / 2, yaw=-pi / 2, pitch=pi / 5, grip=False, frame=frame, arm="l", dur=5.0) GripUtils.go_to(x=x, y=y - 0.0035, z=z + 0.001, roll=pi / 2, yaw=-pi / 2, pitch=pi / 5, grip=True, frame=frame, arm="l", dur=2.0) #changed from -0.007 to -0.0065 #GripUtils.go_to(x=x,y=y,z=z,roll=pi/2,yaw=pi/2,pitch=0,grip=True,frame=frame,arm="r",dur=5.0) #GripUtils.go_to(x=x,y=y+0.04,z=z+0.05,roll=pi/2,yaw=-pi/2,pitch=pi/2,grip=False,frame=frame,arm="l",dur=5.0) #GripUtils.go_to(x=x,y=y+0.04,z=z+0.005,roll=pi/2,yaw=-pi/2,pitch=pi/2,grip=False,frame=frame,arm="l",dur=5.0) #GripUtils.go_to(x=x,y=y,z=z,roll=pi/2,yaw=pi/2,pitch=-pi/16,grip=True,frame=frame,arm="r",dur=5.0) #GripUtils.go_to(x=x,y=y+0.06,z=z-0.025,roll=pi/2,yaw=-pi/2,pitch=pi/2,grip=False,frame=frame,arm="l",dur=5.0) #GripUtils.go_to(x=x,y=y+0.025,z=z-0.225,roll=pi/2,yaw=-pi/2,pitch=pi/2,grip=True,frame=frame,arm="l",dur=1.0) #GripUtils.go_to(x=x,y=y+0.025,z=z,roll=pi/2,yaw=-pi/2,pitch=0,grip=True,frame=frame,arm="l",dur=5.0) GripUtils.close_gripper("l", extra_tight=True) open_amt = 0.025 GripUtils.go_to_multi(x_l=x, y_l=y + open_amt, z_l=z + 0.01, roll_l=pi / 2, yaw_l=-pi / 2, pitch_l=0, grip_l=True, frame_l=frame, x_r=x, y_r=y - open_amt, z_r=z + 0.01, roll_r=pi / 2, yaw_r=pi / 2, pitch_r=0, grip_r=True, frame_r=frame, dur=5.0)
def pickup_sock(sock): z_off = 0.0 """ while not rospy.is_shutdown(): StanceUtils.call_stance("close_right",5.0) (grip_pt,angle) = get_grip_point(sock) GripUtils.go_to_pt(grip_pt,roll=pi/2,yaw=pi/2-angle,pitch=pi/2,arm="r",grip=True,z_offset=z_off) rospy.sleep(5.0) z_off = float(raw_input("What is the new z_offset?")) StanceUtils.call_stance("arms_up",5.0) """ (grip_pt, angle) = get_grip_point(sock) if MODE == THICK_SOCK: OFFSET = 0.02 else: OFFSET = 0.025 y_offset = OFFSET * cos(angle) x_offset = OFFSET * sin(angle) #x_offset += 0.005 z_offset = 0.02 pitch = pi / 2 roll = pi / 2 yaw = pi / 2 - angle GripUtils.go_to_pt(grip_pt, roll=roll, yaw=yaw, pitch=pitch, arm="r", x_offset=x_offset, y_offset=y_offset, z_offset=z_offset, grip=False, dur=5.0) z_offset = -0.02 z_offset -= 0.001 GripUtils.go_to_pt(grip_pt, roll=roll, yaw=yaw, pitch=pitch, arm="r", x_offset=x_offset, y_offset=y_offset, z_offset=z_offset, grip=False, dur=5.0) #Drag thin socks if MODE == THIN_SOCK: y_offset += 0.02 GripUtils.go_to_pt(grip_pt, roll=roll, yaw=yaw, pitch=pitch, arm="r", x_offset=x_offset, y_offset=y_offset, z_offset=z_offset, grip=False, dur=5.0) while not GripUtils.has_object("r"): StanceUtils.call_stance("open_right", 2.0) pitch -= ANGLE_INCREMENT y_offset -= 0.005 * cos(angle) x_offset -= 0.005 * sin(angle) z_offset += 0.0015 GripUtils.go_to_pt(grip_pt, roll=roll, yaw=yaw, pitch=pitch, arm="r", x_offset=x_offset, y_offset=y_offset, z_offset=z_offset, grip=False, dur=5.0) GripUtils.close_gripper("r", extra_tight=True) break return grip_pt
def take_off_dopple(pt, arm): x = DOPPLE_X y = DOPPLE_Y if arm == "l": yaw = -pi / 2 y -= 0.005 else: yaw = pi / 2 y += 0.005 z = DOPPLE_HEIGHT + TABLE_HEIGHT - 0.045 frame = "base_footprint" GripUtils.go_to(x=x, y=y, z=z, roll=0, yaw=yaw, pitch=pi / 4, arm=arm, grip=False, frame=frame, dur=5.0) GripUtils.close_gripper(arm, extra_tight=False) GripUtils.go_to(x=x, y=y, z=z + 0.1, roll=0, yaw=yaw, pitch=0, arm=arm, grip=True, frame=frame, dur=5.0) GripUtils.go_to(x=x, y=y, z=z + 0.2, roll=0, yaw=yaw, pitch=0, arm=arm, grip=True, frame=frame, dur=5.0) roll = 0 if arm == "l": yaw = -pi / 2 else: yaw = pi / 2 GripUtils.go_to(x=x - 0.15, y=y, z=z + 0.2, roll=roll, yaw=yaw, pitch=0, arm=arm, grip=True, frame=frame, dur=5.0) #Spreads out GripUtils.go_to_pt(pt, roll=roll, yaw=yaw, pitch=0, arm=arm, grip=True, z_offset=0.2, y_offset=0.2, dur=5.0) # previously z_o= 0.08 y_o 0.1 GripUtils.go_to_pt(pt, roll=roll, yaw=yaw, pitch=pi / 2, arm=arm, grip=True, dur=7.5, y_offset=-0.2, z_offset=0.015) GripUtils.open_gripper(arm) GripUtils.go_to_pt(pt, roll=roll, yaw=yaw, pitch=pi / 4, arm=arm, grip=False, dur=2.5, y_offset=-0.2, z_offset=0.025) StanceUtils.call_stance("arms_up", 5.0)
def execute(self, userdata): if not GripUtils.close_gripper(self.arm): return FAILURE else: return SUCCESS