def main(args): rospy.init_node("unfolding_click") StanceUtils.call_stance("look_down", 3.0) StanceUtils.call_stance("arms_up", 3.0) while True and not rospy.is_shutdown(): process_mono = rospy.ServiceProxy("click_node/process_mono", ProcessMono) resp = process_mono("wide_stereo/left") pt = resp.pts3d[0] z_offset = 0.06 link_frame = "r_wrist_roll_link" GripUtils.go_to_pt(pt, roll=pi / 2, yaw=0, pitch=pi / 2, arm="r", z_offset=z_offset, grip=True, dur=5.0, link_frame=link_frame) z_offset = 0.00 GripUtils.go_to_pt(pt, roll=pi / 2, yaw=0, pitch=pi / 2, arm="r", z_offset=z_offset, grip=True, dur=5.0, link_frame=link_frame) rospy.sleep(5.0)
def execute(self, userdata): if self.side == 'l': arm = 'l' processor = 'far_left_finder_node' yaw = -pi / 2 else: arm = 'r' processor = 'far_right_finder_node' yaw = pi / 2 process_mono = rospy.ServiceProxy("%s/process_mono" % processor, ProcessMono) resp = process_mono("wide_stereo/left") pt = resp.pts3d[0] pt_opp = resp.pts3d[1] userdata.arm = 'l' if arm == 'r' else 'r' #Arm returns the arm which is currently gripping homogeneity = resp.params[resp.param_names.index("homogeneity")] if homogeneity > 0.13: print "Too homogeneous: %f" % homogeneity return FAILURE userdata.cloth_width = sqrt((pt_opp.point.x - pt.point.x)**2 + (pt_opp.point.y - pt.point.y)**2) #userdata.cloth_width = abs(pt_opp.point.y - pt.point.y) userdata.cloth_height = None if arm == "l": x_offset = -0.02 #was -0.01 else: x_offset = -0.01 if not GripUtils.grab_point( pt, roll=-pi / 2, yaw=yaw, arm=arm, x_offset=x_offset): return FAILURE else: if self.let_go: GripUtils.open_gripper(opp_arm(arm)) return SUCCESS
def execute(self, userdata): forward_amount = 0.375 height = 0.65 drop_amount = 0.25 lateral_amount = 0.05 if self.arm=="r": yaw_multiplier = 1 y_multiplier = -1 else: yaw_multiplier = -1 y_multiplier = 1 for i in range(self.num_shakes): if i == 0: duration = 4.0 if not GripUtils.go_to(x=forward_amount,y=0,z=height, roll=0,pitch=0,yaw=yaw_multiplier*pi/2,grip=True, frame="table_height",arm=self.arm,dur=duration): return FAILURE duration = .5 if not GripUtils.go_to(x=forward_amount,y=lateral_amount*y_multiplier,z=height-drop_amount, roll=0,pitch=pi/4,yaw=yaw_multiplier*pi/2,grip=True, frame="table_height",arm=self.arm,dur=duration): return FAILURE if not GripUtils.go_to(x=forward_amount,y=0,z=height, roll=0,pitch=0,yaw=yaw_multiplier*pi/2,grip=True, frame="table_height",arm=self.arm,dur=duration): return FAILURE return SUCCESS
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 execute(self, userdata): if self.side == 'l': arm = 'l' processor = 'far_left_finder_node' yaw = -pi/2 else: arm = 'r' processor = 'far_right_finder_node' yaw = pi/2 process_mono = rospy.ServiceProxy("%s/process_mono"%processor,ProcessMono) resp = process_mono("wide_stereo/left") pt = resp.pts3d[0] pt_opp = resp.pts3d[1] userdata.arm = 'l' if arm == 'r' else 'r' #Arm returns the arm which is currently gripping homogeneity = resp.params[resp.param_names.index("homogeneity")] if homogeneity > 0.13: print "Too homogeneous: %f"%homogeneity return FAILURE userdata.cloth_width = sqrt( (pt_opp.point.x - pt.point.x)**2 + (pt_opp.point.y - pt.point.y)**2 ) #userdata.cloth_width = abs(pt_opp.point.y - pt.point.y) userdata.cloth_height = None if arm == "l": x_offset = -0.02 #was -0.01 else: x_offset = -0.01 if not GripUtils.grab_point(pt,roll=-pi/2,yaw=yaw,arm=arm,x_offset=x_offset): return FAILURE else: if self.let_go: GripUtils.open_gripper(opp_arm(arm)) return SUCCESS
def execute(self, userdata): forward_amount = 0.5 hover_amount = 0.06 if self.direction == "r": drag_arm = "r" edge_y = -1.0 * self.table_width / 2.0 drag_yaw = pi/2 final_y = edge_y + self.drag_amount else: drag_arm = "l" edge_y = 1.0 * self.table_width / 2.0 drag_yaw = -pi/2 final_y = edge_y - self.drag_amount if not GripUtils.go_to(x=forward_amount*.8,y=edge_y*1.25,z=self.lift_amount, roll=0,pitch=0,yaw=drag_yaw/4,grip=True, frame="table_heig@staticmethodht",arm=drag_arm): print 'f1' return FAILURE if not GripUtils.go_to(x=forward_amount,y=edge_y,z=hover_amount, roll=0,pitch=pi/4,yaw=drag_yaw,grip=True, frame="table_height",arm=drag_arm): print 'f2' return FAILURE if not GripUtils.go_to(x=forward_amount,y=final_y,z=hover_amount, roll=0,pitch=pi/2,yaw=drag_yaw,grip=True, frame="table_height",arm=drag_arm): print 'f3' return FAILURE return SUCCESS
def executeFold_right(self,gripPt,endPt,color_current='blue',color_next='blue'): """ execute fold with right arm """ print "points",gripPt,endPt if (color_current == 'blue'): startpoint = self.convert_to_world_frame(gripPt) if not GripUtils.grab(x = startpoint.point.x,y=startpoint.point.y,z=startpoint.point.z,arm='r', roll=pi/2,yaw=pi/3,pitch=pi/4,approach= True,frame=startpoint.header.frame_id): print "failure" return False midpoint = self.dupl_PointStamped(endPt)#self.convert_to_world_frame(endPt) midpoint.point.x = (midpoint.point.x + startpoint.point.x)/2 midpoint.point.y = (midpoint.point.y + startpoint.point.y)/2 midpoint.point.z = (midpoint.point.z + 0.1) pitch = pi/4 roll = pi/2 yaw=pi/2 grip=True frame= midpoint.header.frame_id if not GripUtils.go_to(x=midpoint.point.x,y=midpoint.point.y,z=midpoint.point.z,roll=roll,pitch=pitch,yaw=yaw,grip=grip,frame=frame,arm='r',dur=7.5): print "failure to go to ",midpoint.point.x,midpoint.point.y,midpoint.point.z #return False endpoint = self.dupl_PointStamped(endPt)#self.convert_to_world_frame(endPt) frame=endpoint.header.frame_id if not GripUtils.go_to(x=endpoint.point.x,y=endpoint.point.y,z=endpoint.point.z,roll=roll,pitch=pitch,yaw=yaw,grip=grip,frame=frame,arm='r',dur=7.5): print "failure" #return False if(color_next == 'blue'): initRobot() return True
def execute(self, userdata): bl = userdata.bl br = userdata.br tl = userdata.tl tr = userdata.tr userdata.cloth_width = sqrt((bl.point.x - br.point.x)**2 + (bl.point.y - br.point.y)**2) * 1.075 userdata.cloth_height = max( [abs(tl.point.x - bl.point.x), abs(tr.point.x - br.point.x)]) if not GripUtils.grab_point(bl, roll=pi / 2, yaw=-pi / 2, pitch=pi / 4, arm="l", x_offset=0.01, INIT_SCOOT_AMT=0.01): return FAILURE if not GripUtils.grab_point(br, roll=-pi / 2, yaw=pi / 2, pitch=pi / 4, arm="r", x_offset=0.01, INIT_SCOOT_AMT=0.01): return FAILURE return SUCCESS
def execute(self, userdata): bl = userdata.bl br = userdata.br tl = userdata.tl tr = userdata.tr midleft = bl midleft.point.x = (bl.point.x + tl.point.x) / 2 midleft.point.y = (bl.point.y + tl.point.y) / 2 midleft.point.z = (bl.point.z + tl.point.z) / 2 midright = br midright.point.x = (br.point.x + tr.point.x) / 2 midright.point.y = (br.point.y + tr.point.y) / 2 midright.point.z = (br.point.z + tr.point.z) / 2 if not GripUtils.grab_point( midleft, roll=pi / 2, yaw=-pi / 2, pitch=pi / 4, arm="l", x_offset=-0.01, z_offset=0.01, INIT_SCOOT_AMT=0.01 ): return FAILURE if not GripUtils.grab_point( midright, roll=-pi / 2, yaw=pi / 2, pitch=pi / 4, arm="r", x_offset=-0.01, z_offset=0.01, INIT_SCOOT_AMT=0.01, ): return FAILURE if not GripUtils.go_to_relative_multi(0, 0, 0.4, True, 0, 0, 0.4, True, "base_footprint"): return FAILURE return SUCCESS
def execute(self, userdata): height = 0.35 lateral_amount = 0.65 forward_amount = 0.3 if self.arm == 'b': lateral_amount_r = lateral_amount * -1 lateral_amount_l = lateral_amount if not GripUtils.go_to_multi( x_l=forward_amount, y_l=lateral_amount_l, z_l=height, roll_l=0, pitch_l=0, yaw_l=0, frame_l="torso_lift_link", grip_l=False, x_r=forward_amount, y_r=lateral_amount_r, z_r=height, roll_r=0, pitch_r=0, yaw_r=0, frame_r="torso_lift_link", grip_r=False, dur=4.0): return FAILURE else: return SUCCESS else: if self.arm == 'r': lateral_amount = lateral_amount * -1 if not GripUtils.go_to( x=forward_amount, y=lateral_amount, z=height, roll=0, pitch=0, yaw=0, grip=False, frame="torso_lift_link", dur=4.0, arm=self.arm): return FAILURE else: return SUCCESS
def execute(self,userdata): arm = userdata.arm multiplier = 1 if arm == 'r' else -1 if not GripUtils.go_to(x=0.4,y=0,z=0.35,roll=0,yaw=pi/2*multiplier,pitch=pi/4,arm=arm,grip=True,frame="table_height",dur=3.0): return FAILURE GripUtils.open_gripper(arm) GripUtils.recall_arm("b") return SUCCESS
def grab_far_right(arm): StanceUtils.call_stance("look_down",5.0) rospy.sleep(2.5) process_mono = rospy.ServiceProxy("far_right_finder_node/process_mono",ProcessMono) resp = process_mono("wide_stereo/left") pt = resp.pts3d[0] GripUtils.grab_point(pt,roll=-pi/2,yaw=pi/2,arm=arm,z_offset = 0.02,x_offset = -0.01) return pt
def execute(self, userdata): process_mono = rospy.ServiceProxy("clump_center_node/process_mono",ProcessMono) resp = process_mono("wide_stereo/left") pt = resp.pts3d[0] z_offset = 0.16 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.open_gripper("l") return SUCCESS
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 main(args): rospy.init_node("unfolding_click") while True and not rospy.is_shutdown(): process_mono = rospy.ServiceProxy("click_node/process_mono",ProcessMono) resp = process_mono("wide_stereo/left") pt = resp.pts3d[0] print pt GripUtils.go_to_pt(point=pt,roll=pi/2,pitch=pi/2,yaw=0,grip=True,arm="l",dur=5.0,link_name="l_tip_frame") print "waiting.." a = raw_input()
def execute(self, userdata): process_mono = rospy.ServiceProxy("clump_center_node/process_mono",ProcessMono) resp = process_mono("wide_stereo/left") pt = resp.pts3d[0] pt.point.z += 0.05 self.listener.waitForTransform("/map", pt.header.frame_id, pt.header.stamp, rospy.Duration(15.0)) transpt = self.listener.transformPoint("/map", pt) GripUtils.go_to_pt(transpt,roll=0,yaw=0,pitch=pi/2,arm="l",grip=True,dur=5.0) userdata.locations['mark'] = transpt 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): bl = userdata.bl br = userdata.br tl = userdata.tl tr = userdata.tr userdata.cloth_width = sqrt( (bl.point.x-br.point.x)**2 + (bl.point.y - br.point.y)**2)*1.075 userdata.cloth_height = max([abs(tl.point.x-bl.point.x),abs(tr.point.x-br.point.x)]) if not GripUtils.grab_point(bl,roll=pi/2,yaw=-pi/2,pitch=pi/4,arm="l",x_offset=0.01,INIT_SCOOT_AMT = 0.01): return FAILURE if not GripUtils.grab_point(br,roll=-pi/2,yaw=pi/2,pitch=pi/4,arm="r",x_offset=0.01,INIT_SCOOT_AMT = 0.01): return FAILURE return SUCCESS
def execute(self, userdata): forward_amount = 0.375 height = 0.65 drop_amount = 0.25 lateral_amount = 0.05 if self.arm == "r": yaw_multiplier = 1 y_multiplier = -1 else: yaw_multiplier = -1 y_multiplier = 1 for i in range(self.num_shakes): if i == 0: duration = 4.0 if not GripUtils.go_to(x=forward_amount, y=0, z=height, roll=0, pitch=0, yaw=yaw_multiplier * pi / 2, grip=True, frame="table_height", arm=self.arm, dur=duration): return FAILURE duration = .5 if not GripUtils.go_to(x=forward_amount, y=lateral_amount * y_multiplier, z=height - drop_amount, roll=0, pitch=pi / 4, yaw=yaw_multiplier * pi / 2, grip=True, frame="table_height", arm=self.arm, dur=duration): return FAILURE if not GripUtils.go_to(x=forward_amount, y=0, z=height, roll=0, pitch=0, yaw=yaw_multiplier * pi / 2, grip=True, frame="table_height", arm=self.arm, dur=duration): return FAILURE return SUCCESS
def grab_far_right(arm): StanceUtils.call_stance("look_down", 5.0) rospy.sleep(2.5) process_mono = rospy.ServiceProxy("far_right_finder_node/process_mono", ProcessMono) resp = process_mono("wide_stereo/left") pt = resp.pts3d[0] GripUtils.grab_point(pt, roll=-pi / 2, yaw=pi / 2, arm=arm, z_offset=0.02, x_offset=-0.01) return pt
def execute(self, userdata): cloth_width = userdata.cloth_width if cloth_width < 0.35: rospy.logwarn("Cannot shake at less than 0.35m apart; tried to do %s" % str(cloth_width)) return FAILURE if cloth_width > 0.65: cloth_width = 0.65 forward_amount = 0.45 height = 0.625 drop_amount = 0.3 lateral_amount = 0.1 for i in range(self.num_shakes): if i == 0: duration = 4.0 if not GripUtils.go_to_multi(x_l=forward_amount,y_l=cloth_width/2.0,z_l=height, roll_l=0,pitch_l=0,yaw_l=-pi/2,grip_l=True, x_r=forward_amount,y_r=-cloth_width/2.0,z_r=height, roll_r=0,pitch_r=0,yaw_r=pi/2,grip_r=True, frame_l="table_height",frame_r="table_height",dur=duration): return FAILURE duration = .45 return_val = SUCCESS if not GripUtils.go_to_multi(x_l=forward_amount,y_l=cloth_width/2.0-lateral_amount,z_l=height-drop_amount, roll_l=0,pitch_l=pi/4,yaw_l=-pi/2,grip_l=True, x_r=forward_amount,y_r=-cloth_width/2.0+lateral_amount,z_r=height-drop_amount, roll_r=0,pitch_r=pi/4,yaw_r=pi/2,grip_r=True, frame_l="table_height",frame_r="table_height",dur=duration): return_val = FAILURE if not GripUtils.go_to_multi(x_l=forward_amount,y_l=cloth_width/2.0,z_l=height, roll_l=0,pitch_l=0,yaw_l=-pi/2,grip_l=True, x_r=forward_amount,y_r=-cloth_width/2.0,z_r=height, roll_r=0,pitch_r=0,yaw_r=pi/2,grip_r=True, frame_l="table_height",frame_r="table_height",dur=duration): return_val = FAILURE cloth_width = cloth_width * self.clothWidthScaleFactor if not GripUtils.go_to_multi(x_l=forward_amount+.2,y_l=cloth_width/2.0,z_l=height, roll_l=0,pitch_l=0,yaw_l=-pi/4,grip_l=True, x_r=forward_amount+.2,y_r=-cloth_width/2.0,z_r=height, roll_r=0,pitch_r=0,yaw_r=pi/4,grip_r=True, frame_l="table_height",frame_r="table_height",dur=4.0): return FAILURE return SUCCESS
def go_to_folding_station(): print "Going to folding station" DryerNavigationUtils.dryerToTable1() GripUtils.go_to(arm="l", x=0.5, y=0, z=0.35, roll=0, yaw=0, pitch=pi / 2, grip=True, frame="table_height") GripUtils.open_gripper("l") return True
def smooth(arm='b'): location = PointStamped() location.point.x = 0.5 location.header.frame_id = "table_height" distance = TABLE_WIDTH/3 initial_separation = 0.11 GripUtils.recall_arm(arm) if arm == 'b': #Put arms together, with a gap of initial_separation between grippers if not GripUtils.go_to_pts(point_l=location,grip_r=True, grip_l=True, point_r=location, roll_l=pi/2,yaw_l=0,pitch_l=-pi/2,y_offset_l=initial_separation/2.0,z_offset_l=0.05 ,link_frame_l="l_wrist_back_frame", roll_r=pi/2,yaw_r=0,pitch_r=-pi/2,y_offset_r=-1*initial_separation/2.0,z_offset_r=0.05 ,link_frame_r="r_wrist_back_frame",dur=4.0): success = False if not GripUtils.go_to_pts(point_l=location,grip_r=True, grip_l=True, point_r=location, roll_l=pi/2,yaw_l=0,pitch_l=-pi/2,y_offset_l=initial_separation/2.0,z_offset_l=-0.03, link_frame_l="l_wrist_back_frame", roll_r=pi/2,yaw_r=0,pitch_r=-pi/2,y_offset_r=-1*initial_separation/2.0,z_offset_r=-0.03, link_frame_r="r_wrist_back_frame",dur=2.0): success = False if not GripUtils.go_to_pts(point_l=location,grip_r=True, grip_l=True, point_r=location, roll_l=pi/2,yaw_l=0,pitch_l=-pi/2, y_offset_l=(distance+initial_separation)/2.0, z_offset_l=-0.03, link_frame_l="l_wrist_back_frame", roll_r=pi/2,yaw_r=0,pitch_r=-pi/2, y_offset_r=-1*(distance+initial_separation)/2.0, z_offset_r=-0.03, link_frame_r="r_wrist_back_frame",dur=2.0): success = False else: #Right is negative in the y axis if arm=="r": y_multiplier = -1 else: y_multiplier = 1 location.point.y -= y_multiplier*distance/2 if not GripUtils.go_to_pt(point=location,grip=True,roll=pi/2,yaw=0,pitch=-pi/2, z_offset=0.05,arm=arm, link_frame="%s_wrist_back_frame"%arm,dur=4.0): success = False if not GripUtils.go_to_pt(point=location,grip=True,roll=pi/2,yaw=0,pitch=-pi/2, z_offset=-0.01,arm=arm, link_frame="%s_wrist_back_frame"%arm,dur=2.0): success = False if not GripUtils.go_to_pt(point=location,grip=True,roll=pi/2,yaw=0,pitch=-pi/2, y_offset=y_multiplier*distance,z_offset=-0.01,arm=arm, link_frame="%s_wrist_back_frame"%arm,dur=2.0): success = False GripUtils.recall_arm(arm) return True
def execute(self, userdata): initial_separation = 0.11 print 'Smooth distance: %d' % self.distance if self.arm=="b": rospy.loginfo('Self.arm is b') outcome = SUCCESS #Put arms together, with a gap of initial_separation between grippers if not GripUtils.go_to_pts(point_l=self.location,grip_r=True, grip_l=True, point_r=self.location, roll_l=pi/2,yaw_l=0,pitch_l=-pi/2,y_offset_l=initial_separation/2.0,z_offset_l=0.05 ,link_frame_l="l_wrist_back_frame", roll_r=pi/2,yaw_r=0,pitch_r=-pi/2,y_offset_r=-1*initial_separation/2.0,z_offset_r=0.05 ,link_frame_r="r_wrist_back_frame",dur=4.0): outcome = FAILURE rospy.loginfo('Smooth on table goto 1: Success is %s', outcome==SUCCESS) if not GripUtils.go_to_pts(point_l=self.location,grip_r=True, grip_l=True, point_r=self.location, roll_l=pi/2,yaw_l=0,pitch_l=-pi/2,y_offset_l=initial_separation/2.0,z_offset_l=0.00, link_frame_l="l_wrist_back_frame", roll_r=pi/2,yaw_r=0,pitch_r=-pi/2,y_offset_r=-1*initial_separation/2.0,z_offset_r=0.00, link_frame_r="r_wrist_back_frame",dur=2.0): outcome = FAILURE rospy.loginfo('Smooth on table goto 2: Success is %s', outcome==SUCCESS) if not GripUtils.go_to_pts(point_l=self.location,grip_r=True, grip_l=True, point_r=self.location, roll_l=pi/2,yaw_l=0,pitch_l=-pi/2, y_offset_l=(self.distance+initial_separation)/2.0, z_offset_l=0.00, link_frame_l="l_wrist_back_frame", roll_r=pi/2,yaw_r=0,pitch_r=-pi/2, y_offset_r=-1*(self.distance+initial_separation)/2.0, z_offset_r=0.00, link_frame_r="r_wrist_back_frame",dur=2.0): outcome = FAILURE rospy.loginfo('Smooth on table goto 3: Success is %s', outcome==SUCCESS) GripUtils.recall_arm("b") return outcome else: #Right is negative in the y axis if self.arm=="r": y_multiplier = -1 else: y_multiplier = 1 rospy.loginfo('arm is %s, y multiplier is %s' % (self.arm, y_multiplier)) print 'Location: %s' % str(self.location) if not GripUtils.go_to_pt(point=self.location,grip=True,roll=pi/2,yaw=0,pitch=-pi/2, z_offset=0.05,arm=self.arm, link_frame="%s_wrist_back_frame"%self.arm,dur=4.0, verbose=True): return FAILURE rospy.loginfo('Step 2') if not GripUtils.go_to_pt(point=self.location,grip=True,roll=pi/2,yaw=0,pitch=-pi/2, z_offset=-0.01,arm=self.arm, link_frame="%s_wrist_back_frame"%self.arm,dur=2.0, verbose=True): #return FAILURE pass rospy.loginfo('Step 3') print 'Offset: %f' % (y_multiplier*self.distance) if not GripUtils.go_to_pt(point=self.location,grip=True,roll=pi/2,yaw=0,pitch=-pi/2, y_offset=y_multiplier*self.distance,z_offset=-0.01,arm=self.arm, link_frame="%s_wrist_back_frame"%self.arm,dur=2.0, verbose=True): return FAILURE rospy.loginfo('Step 4. Done!') return SUCCESS
def execute(self, userdata): forward_amount = 0.5 hover_amount = 0.06 if self.direction == "r": drag_arm = "r" edge_y = -1.0 * self.table_width / 2.0 drag_yaw = pi / 2 final_y = edge_y + self.drag_amount else: drag_arm = "l" edge_y = 1.0 * self.table_width / 2.0 drag_yaw = -pi / 2 final_y = edge_y - self.drag_amount if not GripUtils.go_to(x=forward_amount * .8, y=edge_y * 1.25, z=self.lift_amount, roll=0, pitch=0, yaw=drag_yaw / 4, grip=True, frame="table_height", arm=drag_arm): print 'f1' return FAILURE if not GripUtils.go_to(x=forward_amount, y=edge_y, z=hover_amount, roll=0, pitch=pi / 4, yaw=drag_yaw, grip=True, frame="table_height", arm=drag_arm): print 'f2' return FAILURE if not GripUtils.go_to(x=forward_amount, y=final_y, z=hover_amount, roll=0, pitch=pi / 2, yaw=drag_yaw, grip=True, frame="table_height", arm=drag_arm): print 'f3' return FAILURE return SUCCESS
def main(args): rospy.init_node("unfolding_click") StanceUtils.call_stance("look_down",3.0) StanceUtils.call_stance("arms_up",3.0) while True and not rospy.is_shutdown(): process_mono = rospy.ServiceProxy("click_node/process_mono",ProcessMono) resp = process_mono("wide_stereo/left") pt = resp.pts3d[0] z_offset = 0.06 link_frame = "r_wrist_roll_link" GripUtils.go_to_pt(pt,roll=pi/2,yaw=0,pitch=pi/2,arm="r",z_offset=z_offset,grip=True,dur=5.0,link_frame=link_frame) z_offset = 0.00 GripUtils.go_to_pt(pt,roll=pi/2,yaw=0,pitch=pi/2,arm="r",z_offset=z_offset,grip=True,dur=5.0,link_frame=link_frame) rospy.sleep(5.0)
def execute(self, userdata): bl = userdata.bl br = userdata.br tl = userdata.tl tr = userdata.tr midleft = bl midleft.point.x = (bl.point.x + tl.point.x) / 2 midleft.point.y = (bl.point.y + tl.point.y) / 2 midleft.point.z = (bl.point.z + tl.point.z) / 2 midright = br midright.point.x = (br.point.x + tr.point.x) / 2 midright.point.y = (br.point.y + tr.point.y) / 2 midright.point.z = (br.point.z + tr.point.z) / 2 if not GripUtils.grab_point(midleft, roll=pi / 2, yaw=-pi / 2, pitch=pi / 4, arm="l", x_offset=-0.01, z_offset=0.01, INIT_SCOOT_AMT=0.01): return FAILURE if not GripUtils.grab_point(midright, roll=-pi / 2, yaw=pi / 2, pitch=pi / 4, arm="r", x_offset=-0.01, z_offset=0.01, INIT_SCOOT_AMT=0.01): return FAILURE if not GripUtils.go_to_pts(point_l=midleft, roll_l=pi / 2, yaw_l=-pi / 2, pitch_l=pi / 4, grip_l=True, point_r=midright, roll_r=-pi / 2, yaw_r=pi / 2, pitch_r=pi / 4, grip_r=True, z_offset_l=0.4, z_offset_r=0.4): return FAILURE 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 sendTarget(self, dur, target1, target2=False,grab=False): resp = False if grab: point = target1.point pitch = target1.pitch roll = target1.roll yaw = target1.yaw GripUtils.grab_point(point=point,pitch=pitch,roll=roll,yaw=yaw,arm=target1.arm,approach=False) return target1.point.header.stamp = rospy.Time.now() if not target2: try: srv = rospy.ServiceProxy("move_one_arm",MoveOneArm) resp = srv(MoveOneArmRequest(target=target1,dur=dur)) except rospy.ServiceException,e: rospy.loginfo("Service Call Failed: %s"%e)
def execute(self, userdata): height = 0.35 lateral_amount = 0.65 forward_amount = 0.3 if not GripUtils.go_to_multi(x_l=forward_amount, y_l=lateral_amount, z_l=height, roll_l=0, pitch_l=0, yaw_l=0, grip_l=True, x_r=forward_amount, y_r=-lateral_amount, z_r=height, roll_r=0, pitch_r=0, yaw_r=0, grip_r=True, frame_l="torso_lift_link", frame_r="torso_lift_link", dur=4.0): return FAILURE else: return SUCCESS
def execute(self, userdata): if self.arm == "l": arm = "l" processor = "far_left_finder_node" yaw = -pi / 2 roll = -pi / 2 else: arm = "r" processor = "far_right_finder_node" yaw = pi / 2 roll = pi / 2 process_mono = rospy.ServiceProxy("%s/process_mono" % processor, ProcessMono) resp = process_mono("wide_stereo/left") pt = resp.pts3d[0] pt_opp = resp.pts3d[1] userdata.arm = "l" if arm == "r" else "r" # Arm returns the arm which is currently gripping homogeneity = resp.params[resp.param_names.index("homogeneity")] if homogeneity > 0.13: print "Too homogeneous: %f" % homogeneity return FAILURE userdata.cloth_width = sqrt((pt_opp.point.x - pt.point.x) ** 2 + (pt_opp.point.y - pt.point.y) ** 2) # userdata.cloth_width = abs(pt_opp.point.y - pt.point.y) userdata.cloth_height = None # Offsets for safety if arm == "l": y_offset = -0.02 z_offset = 0.01 else: y_offset = 0.02 x_offset = 0.02 if self.side == "t": x_offset = -0.01 elif self.side == "b": x_offset = 0.01 else: x_offset = 0 if not GripUtils.grab_point(pt, roll=roll, yaw=yaw, arm=arm, x_offset=x_offset, y_offset=y_offset): return FAILURE else: if self.let_go: GripUtils.open_gripper(opp_arm(arm)) if self.recall: GripUtils.recall_arm(arm, grip=True) return SUCCESS
def execute(self, userdata): height = 0.35 lateral_amount = 0.65 forward_amount = 0.3 if self.arm == 'b': lateral_amount_r = lateral_amount * -1 lateral_amount_l = lateral_amount if not GripUtils.go_to_multi(x_l=forward_amount, y_l=lateral_amount_l, z_l=height, roll_l=0, pitch_l=0, yaw_l=0, frame_l="torso_lift_link", grip_l=False, x_r=forward_amount, y_r=lateral_amount_r, z_r=height, roll_r=0, pitch_r=0, yaw_r=0, frame_r="torso_lift_link", grip_r=False, dur=4.0): return FAILURE else: return SUCCESS else: if self.arm == 'r': lateral_amount = lateral_amount * -1 if not GripUtils.go_to(x=forward_amount, y=lateral_amount, z=height, roll=0, pitch=0, yaw=0, grip=False, frame="torso_lift_link", dur=4.0, arm=self.arm): return FAILURE else: return SUCCESS
def main(args): rospy.init_node("lift_demo_node") listener = tf.TransformListener() GripUtils.go_to_relative(0, 0, 0.1, True, "l","base_footprint",listener) sm = OuterStateMachine(DEFAULT_OUTCOMES) START_STATE = 'Lift_Off' with sm: OuterStateMachine.add('Initialize',Initialize(),{SUCCESS:START_STATE,FAILURE:FAILURE}) OuterStateMachine.add('Lift_Off',LiftOff(),{SUCCESS:SUCCESS,FAILURE:FAILURE}) OuterStateMachine.add('Clump_To_Triangle',ClumpToTriangle(),{SUCCESS:'Triangle_To_Rectangle',FAILURE:'Initialize'}) OuterStateMachine.add('Triangle_To_Rectangle',TriangleToRectangle(),{SUCCESS:'Fold_Towel',FAILURE:FAILURE}) OuterStateMachine.add('Fold_Towel',FoldTowel(),{SUCCESS:SUCCESS,FAILURE:FAILURE}) sis = smach_ros.IntrospectionServer('demo_smach_server', sm, '/SM_ROOT') sis.start() outcome = sm.execute()
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_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 main(args): rospy.init_node("unfolding_click") while True and not rospy.is_shutdown(): process_mono = rospy.ServiceProxy("click_node/process_mono", ProcessMono) resp = process_mono("wide_stereo/left") pt = resp.pts3d[0] print pt GripUtils.go_to_pt(point=pt, roll=pi / 2, pitch=pi / 2, yaw=0, grip=True, arm="l", dur=5.0, link_name="l_tip_frame") print "waiting.." a = raw_input()
def execute(self, userdata): cloth_width = userdata.cloth_width*.90 if cloth_width < MINIMUM_SHAKE_WIDTH: rospy.logwarn("Cannot shake at less than %s m apart; tried to do %d" % (MINIMUM_SHAKE_WIDTH, cloth_width)) return FAILURE forward_amount = 0.45 height = 0.625 if self.violent: drop_amount = 0.3 else: drop_amount = 0.1 lateral_amount = 0.1 for i in range(self.num_shakes): if i == 0: duration = 4.0 if not GripUtils.go_to_multi(x_l=forward_amount,y_l=cloth_width/2.0,z_l=height, roll_l=0,pitch_l=0,yaw_l=-pi/2,grip_l=True, x_r=forward_amount,y_r=-cloth_width/2.0,z_r=height, roll_r=0,pitch_r=0,yaw_r=pi/2,grip_r=True, frame_l="table_height",frame_r="table_height",dur=duration): return_val = FAILURE duration = .6 return_val = SUCCESS if not GripUtils.go_to_multi(x_l=forward_amount,y_l=cloth_width/2.0-lateral_amount,z_l=height-drop_amount, roll_l=0,pitch_l=pi/4,yaw_l=-pi/2,grip_l=True, x_r=forward_amount,y_r=-cloth_width/2.0+lateral_amount,z_r=height-drop_amount, roll_r=0,pitch_r=pi/4,yaw_r=pi/2,grip_r=True, frame_l="table_height",frame_r="table_height",dur=duration): return_val = FAILURE if not GripUtils.go_to_multi(x_l=forward_amount,y_l=cloth_width/2.0,z_l=height, roll_l=0,pitch_l=0,yaw_l=-pi/2,grip_l=True, x_r=forward_amount,y_r=-cloth_width/2.0,z_r=height, roll_r=0,pitch_r=0,yaw_r=pi/2,grip_r=True, frame_l="table_height",frame_r="table_height",dur=duration): return_val = FAILURE return SUCCESS
def executeDrag(self,gripPts,endPts,color='blue'): """ drag from gripPts to endPts """ startpoints = [] for vertex in gripPts: pt_world = self.convert_to_world_frame(vertex) now = rospy.Time.now() self.listener.waitForTransform("base_footprint",pt_world.header.frame_id,now,rospy.Duration(10.0)) pt_transformed = self.listener.transformPoint("base_footprint",pt_world) startpoints.append(pt_transformed) #print "grabbed (x,y,z)",startpoints[0].point.x,startpoints[0].point.y,startpoints[0].point.z if not GripUtils.grab_points(point_l=startpoints[0],roll_l=-pi/2,yaw_l=-pi/3,pitch_l=pi/4,x_offset_l=-0.01, z_offset_l=0.005,approach= True, point_r=startpoints[1],roll_r=pi/2,yaw_r= pi/3,pitch_r=pi/4,x_offset_r=-0.01, z_offset_r=0.005): print "failure" return False else: print "grabbed (x,y,z)",startpoints[0].point.x,startpoints[0].point.y,startpoints[0].point.z print "done" midpoints = [] for vertex in endPts: pt_world = self.dupl_PointStamped(vertex)#self.convert_to_world_frame(vertex) midpoints.append(pt_world) frame_l = frame_r = midpoints[0].header.frame_id if not GripUtils.go_to_multi (x_l=midpoints[0].point.x,y_l=midpoints[0].point.y,z_l=midpoints[0].point.z,roll_l=-pi/2,yaw_l=-pi/2,pitch_l=pi/4,grip_l=True,frame_l=frame_l ,x_r=midpoints[1].point.x, y_r= midpoints[1].point.y ,z_r= midpoints[1].point.z ,roll_r=pi/2,yaw_r=pi/2,pitch_r=pi/4,grip_r=True,frame_r=frame_r,dur=5): print "failure" #return False else: print "dragging done" if (color == 'blue'): GripUtils.open_grippers() return True
def execute(self, userdata): height = 0.35 lateral_amount = 0.65 forward_amount = 0.3 if not GripUtils.go_to_multi( x_l=forward_amount, y_l=lateral_amount, z_l=height, roll_l=0, pitch_l=0, yaw_l=0, grip_l=self.grip, x_r=forward_amount, y_r=-lateral_amount, z_r=height, roll_r=0, pitch_r=0, yaw_r=0, grip_r=self.grip, frame_l="torso_lift_link", frame_r="torso_lift_link", dur=4.0): return FAILURE else: return SUCCESS
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 sendTarget(self, dur, target1, target2=False, grab=False): resp = False if grab: point = target1.point pitch = target1.pitch roll = target1.roll yaw = target1.yaw GripUtils.grab_point(point=point, pitch=pitch, roll=roll, yaw=yaw, arm=target1.arm, approach=False) return target1.point.header.stamp = rospy.Time.now() if not target2: try: srv = rospy.ServiceProxy("move_one_arm", MoveOneArm) resp = srv(MoveOneArmRequest(target=target1, dur=dur)) except rospy.ServiceException, e: rospy.loginfo("Service Call Failed: %s" % e)
def execute(self, userdata): process_mono = rospy.ServiceProxy("triangle_fitter_node/process_mono",ProcessMono) resp = process_mono("wide_stereo/left") pt_l = resp.pts3d[0] pt_r = resp.pts3d[1] pt_tl = resp.pts3d[2] pt_tr = resp.pts3d[3] #Compute approximate widths towel_width = sqrt( (pt_l.point.x - pt_tr.point.x)**2 + (pt_l.point.y - pt_tr.point.y)**2 ) towel_height = sqrt( (pt_l.point.x - pt_tl.point.x)**2 + (pt_l.point.y - pt_tl.point.y)**2 ) userdata.cloth_width = towel_width userdata.cloth_height = towel_height if resp.params[resp.param_names.index("left")] == 0: left = False else: left = True print "Correct?" tf = raw_input() if len(tf) > 0 and (tf[0] == 'n' or tf[0] == 'f'): left = not left if left: GripUtils.recall_arm("l") if GripUtils.grab_point(pt_l,roll=pi/2,yaw=-pi/3,pitch=pi/4,arm="l",x_offset=0.005): return SUCCESS else: return FAILURE else: GripUtils.recall_arm("r") if GripUtils.grab_point(pt_r,roll=-pi/2,yaw=pi/3,pitch=pi/4,arm="r",x_offset=0.005): return SUCCESS else: return FAILURE
def table1ToTable2(width): global numStacked GripUtils.go_to_relative_multi(0, 0, 0.2, True, 0, 0, 0.2, True, "base_footprint", None) goFromAToB("Table1", "Table2") GripUtils.go_to_relative_multi(0, 0, -0.2+0.02*numStacked, True, 0, 0, -0.2+0.02*numStacked, True, "base_footprint", None) numStacked += 1 GripUtils.open_grippers()
def smooth(pt,side): y_offset = -0.08 x_offset = 0.02 x = pt.point.x + x_offset y = pt.point.y + y_offset z = pt.point.z frame = "base_footprint" x_l = x x_r = x y_l = y y_r = y z_l = z_r = z roll_l = roll_r = 0 yaw_l = -pi/2 yaw_r = pi/2 pitch_l = pitch_r=pi/6 grip_l = grip_r = True GripUtils.go_to_multi (x_l=x,y_l=y_l,z_l=z+0.04,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame ,x_r=x,y_r=y_r,z_r=z+0.04,roll_r=roll_l,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame ,dur=5.0) GripUtils.go_to_multi (x_l=x,y_l=y_l,z_l=z,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame ,x_r=x,y_r=y_r,z_r=z,roll_r=roll_l,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame ,dur=3.0) y_l += 0.06 y_r -= 0.06 pitch_l = pi/4 pitch_r = pi/4 GripUtils.go_to_multi (x_l=x,y_l=y_l,z_l=z+0.01,roll_l=roll_l,yaw_l=yaw_l,pitch_l=pitch_l,grip_l=grip_l,frame_l=frame ,x_r=x,y_r=y_r,z_r=z+0.01,roll_r=roll_l,yaw_r=yaw_r,pitch_r=pitch_r,grip_r=grip_r,frame_r=frame ,dur=5.0)
def execute(self, userdata): forward_amount = self.forward_amount hover_amount = 0.06 if self.direction == "r": pr2_say(talk_drag2) drag_arm = "r" edge_y = self.table_width / 2.0 drag_yaw = pi/2 final_y = edge_y - self.drag_amount else: pr2_say(talk_drag1) drag_arm = "l" edge_y = -1 * self.table_width / 2.0 drag_yaw = -pi/2 final_y = edge_y + self.drag_amount #roll = 0 roll = pi/2 GripUtils.go_to(x=forward_amount,y=edge_y,z=self.lift_amount, roll=roll,pitch=0,yaw=drag_yaw,grip=True, frame="table_height",arm=drag_arm,dur=2.0) rospy.sleep(1.0) #Give time to stabilize if not GripUtils.go_to(x=forward_amount,y=edge_y,z=hover_amount, roll=roll,pitch=pi/4,yaw=drag_yaw,grip=True, frame="table_height",arm=drag_arm,dur=2.0): return FAILURE if not GripUtils.go_to(x=forward_amount,y=(edge_y+final_y)/2.0,z=hover_amount, roll=roll,pitch=pi/4,yaw=drag_yaw,grip=True, frame="table_height",arm=drag_arm,dur=2.5): return FAILURE if not GripUtils.go_to(x=forward_amount,y=final_y,z=hover_amount, roll=roll,pitch=pi/4,yaw=drag_yaw,grip=True, frame="table_height",arm=drag_arm,dur=2.5): return FAILURE return SUCCESS
def execute(self,userdata): pt_bl = userdata.bl pt_tl = userdata.tl pt_br = userdata.br pt_tr = userdata.tr #if not GripUtils.go_to_pts(pt_tl, -pi/2, -pi/3, pi/4, pt_tr, pi/2, pi/3, pi/4, x_offset_l=-0.04,\ # x_offset_r=-0.04): # return FAILURE #FIXME Hard-coded X offsets to compensate for poor calibration. Re-calibrate and REMOVE ''' if not GripUtils.grab_point(pt_tl,roll=-pi/2,yaw=-pi/3,pitch=pi/4,arm="l",x_offset=-0.04): return FAILURE if not GripUtils.grab_point(pt_tr,roll=pi/2,yaw= pi/3,pitch=pi/4,arm="r",x_offset=-0.04): return FAILURE ''' #FIXME: For some reason large offsets required, #DEBUG if not GripUtils.grab_points(pt_tl,roll_l=-pi/2,yaw_l=-pi/3,pitch_l=pi/4,x_offset_l=-0.05, z_offset_l=-0.0025 ,point_r=pt_tr,roll_r=pi/2,yaw_r= pi/3,pitch_r=pi/4,x_offset_r=-0.06, y_offset_r=0.00, y_offset_l=-0.00, z_offset_r=0.01): return FAILURE (bl_x,bl_y,bl_z) = (pt_bl.point.x,pt_bl.point.y,pt_bl.point.z) (tl_x,tl_y,tl_z) = (pt_tl.point.x,pt_tl.point.y,pt_tl.point.z) (br_x,br_y,br_z) = (pt_br.point.x,pt_br.point.y,pt_br.point.z) (tr_x,tr_y,tr_z) = (pt_tr.point.x,pt_tr.point.y,pt_tr.point.z) x_l = (tl_x+bl_x)/2.0 y_l = (tl_y+bl_y)/2.0 z_l = bl_z + sqrt( (bl_x-tl_x)**2 + (bl_y-tl_y)**2 )/2.0 x_r = (tr_x+br_x)/2.0 y_r = (tr_y+br_y)/2.0 z_r = br_z + sqrt( (br_x-tr_x)**2 + (br_y-tr_y)**2 )/2.0 pitch_l = pitch_r = pi/4 roll_l = 0 roll_r = 0 yaw_l=-pi/2 yaw_r= pi/2 grip_l=grip_r=True frame_l=frame_r = pt_bl.header.frame_id if not GripUtils.go_to_multi (x_l=x_l,y_l=y_l,z_l=z_l,roll_l=roll_l,pitch_l=pitch_l,yaw_l=yaw_l,grip_l=grip_l,frame_l=frame_l ,x_r=x_r,y_r=y_r,z_r=z_r,roll_r=roll_r,pitch_r=pitch_r,yaw_r=yaw_r,grip_r=grip_r,frame_r=frame_r ,dur=4.0): return_val = FAILURE print "Folding down!" x_l = bl_x + 0.02 # overshoots fold down x_r = br_x + 0.02 y_l = bl_y-0.01 y_r = br_y+0.01 z_l = z_r = bl_z + 0.02 yaw_l = -3*pi/4 yaw_r = 3*pi/4 pitch_l=pitch_r = pi/4 roll_l = pi/2 roll_r = -pi/2 GripUtils.go_to_multi (x_l=x_l,y_l=y_l,z_l=z_l,roll_l=roll_l,pitch_l=pitch_l,yaw_l=yaw_l,grip_l=grip_l,frame_l=frame_l ,x_r=x_r,y_r=y_r,z_r=z_r,roll_r=roll_r,pitch_r=pitch_r,yaw_r=yaw_r,grip_r=grip_r,frame_r=frame_r ,dur=7.5) GripUtils.recall_arm("b") pr2_say(talk_pose2) return SUCCESS
def execute(self, userdata): bl = userdata.bl br = userdata.br tl = userdata.tl tr = userdata.tr midleft = bl midleft.point.x = (bl.point.x + tl.point.x) / 2 midleft.point.y = (bl.point.y + tl.point.y) / 2 midleft.point.z = (bl.point.z + tl.point.z) / 2 midright = br midright.point.x = (br.point.x + tr.point.x) / 2 midright.point.y = (br.point.y + tr.point.y) / 2 midright.point.z = (br.point.z + tr.point.z) / 2 if not GripUtils.grab_point(midleft, roll=pi / 2, yaw=-pi / 2, pitch=pi / 4, arm="l", x_offset=-0.01, z_offset=0.01, INIT_SCOOT_AMT=0.01): return FAILURE if not GripUtils.grab_point(midright, roll=-pi / 2, yaw=pi / 2, pitch=pi / 4, arm="r", x_offset=-0.01, z_offset=0.01, INIT_SCOOT_AMT=0.01): return FAILURE if not GripUtils.go_to_relative_multi(0, 0, 0.4, True, 0, 0, 0.4, True, "base_footprint"): return FAILURE return SUCCESS
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 execute(self,userdata): pt_bl = userdata.bl pt_tl = userdata.tl pt_br = userdata.br pt_tr = userdata.tr #FIXME Hard-coded X offsets to compensate for poor calibration. Re-calibrate and REMOVE if not GripUtils.grab_point(pt_tl,roll=-pi/2,yaw=-pi/3,pitch=pi/4,arm="l",x_offset=-0.04): return FAILURE if not GripUtils.grab_point(pt_tr,roll=pi/2,yaw= pi/3,pitch=pi/4,arm="r",x_offset=-0.04): return FAILURE (bl_x,bl_y,bl_z) = (pt_bl.point.x,pt_bl.point.y,pt_bl.point.z) (tl_x,tl_y,tl_z) = (pt_tl.point.x,pt_tl.point.y,pt_tl.point.z) (br_x,br_y,br_z) = (pt_br.point.x,pt_br.point.y,pt_br.point.z) (tr_x,tr_y,tr_z) = (pt_tr.point.x,pt_tr.point.y,pt_tr.point.z) x_l = (tl_x+bl_x)/2.0 y_l = (tl_y+bl_y)/2.0 z_l = bl_z + sqrt( (bl_x-tl_x)**2 + (bl_y-tl_y)**2 )/2.0 x_r = (tr_x+br_x)/2.0 y_r = (tr_y+br_y)/2.0 z_r = br_z + sqrt( (br_x-tr_x)**2 + (br_y-tr_y)**2 )/2.0 pitch_l = pitch_r = pi/4 roll_l = 0 roll_r = 0 yaw_l=-pi/2 yaw_r= pi/2 grip_l=grip_r=True frame_l=frame_r = pt_bl.header.frame_id if not GripUtils.go_to_multi (x_l=x_l,y_l=y_l,z_l=z_l,roll_l=roll_l,pitch_l=pitch_l,yaw_l=yaw_l,grip_l=grip_l,frame_l=frame_l ,x_r=x_r,y_r=y_r,z_r=z_r,roll_r=roll_r,pitch_r=pitch_r,yaw_r=yaw_r,grip_r=grip_r,frame_r=frame_r ,dur=7.5): return_val = FAILURE print "Folding down!" x_l = bl_x-0.015 y_l = bl_y+0.025 z_l = z_r = bl_z + 0.02 x_r = br_x-0.015 y_r = br_y-0.025 yaw_l = -3*pi/4 yaw_r = 3*pi/4 pitch_l=pitch_r = pi/4 roll_l = pi/2 roll_r = -pi/2 GripUtils.go_to_multi (x_l=x_l,y_l=y_l,z_l=z_l,roll_l=roll_l,pitch_l=pitch_l,yaw_l=yaw_l,grip_l=grip_l,frame_l=frame_l ,x_r=x_r,y_r=y_r,z_r=z_r,roll_r=roll_r,pitch_r=pitch_r,yaw_r=yaw_r,grip_r=grip_r,frame_r=frame_r ,dur=7.5) GripUtils.recall_arm("b") return SUCCESS
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 fold_cloth(dir='l'): print "Folding cloth" _cloth_tracker.scoot(-0.1) GripUtils.recall_arm("b") (waist_l_base, waist_r_base) = get_waist_points() now = rospy.Time.now() waist_l_base.header.stamp = now waist_r_base.header.stamp = now _listener.waitForTransform("table_height", waist_l_base.header.frame_id, now, rospy.Duration(5.0)) waist_l = _listener.transformPoint("table_height", waist_l_base) waist_r = _listener.transformPoint("table_height", waist_r_base) waist_l.point.z = 0 waist_r.point.z = 0 #Grab the waist point smooth() GripUtils.grab_point(waist_l, arm="l", yaw=-pi / 2, x_offset=0.02) #Fold over ctr_x = (waist_l.point.x + waist_r.point.x) / 2 + 0.02 ctr_y = (waist_l.point.y + waist_r.point.y) / 2 ctr_z = waist_l.point.y - ctr_y GripUtils.go_to(x=ctr_x, y=ctr_y, z=ctr_z, roll=pi / 2, pitch=pi / 2, yaw=-pi / 2, arm="l", grip=True, frame=waist_l.header.frame_id) GripUtils.go_to_pt(waist_r, arm="l", roll=pi / 2, pitch=3 * pi / 4, yaw=-pi / 2, grip=True, y_offset=0.01, x_offset=0.02) GripUtils.go_to_pt(waist_r, arm="l", roll=pi / 2, pitch=3 * pi / 4, yaw=-pi / 2, grip=False, y_offset=-0.05, x_offset=0.02, dur=2.5) GripUtils.go_to_pt(waist_r, arm="l", roll=pi / 2, pitch=3 * pi / 4, yaw=-pi / 2, grip=False, y_offset=-0.05, x_offset=0.02, z_offset=0.05, dur=2.5) GripUtils.recall_arm("b") #Grab waist scoot_amt = 0.2 _cloth_tracker.scoot(-scoot_amt) ctr_x = 0.25 * waist_l.point.x + 0.75 * waist_r.point.x + scoot_amt ctr_y = 0.25 * waist_l.point.y + 0.75 * waist_r.point.y ctr_z = waist_l.point.z GripUtils.grab(x=ctr_x, y=ctr_y, z=ctr_z, roll=pi / 2, pitch=pi / 4, yaw=0, arm="r", frame=waist_l.header.frame_id) sweep_drag_amount = 0.95 * TABLE_WIDTH sweep_lift_amount = 0.6 (x, y, z, r, p, yw) = sweep_cloth_with_scoot("r", sweep_drag_amount, TABLE_WIDTH, sweep_lift_amount, roll=pi / 2) print "y is %f" % y print "(%f,%f,%f,%f,%f,%f)" % (x, y, z, r, p, y) smooth("l") GripUtils.go_to(x=x, y=y + PANTS_LENGTH / 4 - 0.03, z=PANTS_LENGTH / 4, roll=r, pitch=3 * pi / 8, yaw=yw, arm="r", grip=True, frame="table_height", dur=2.5) GripUtils.go_to(x=x, y=y + PANTS_LENGTH / 2 - 0.03, z=PANTS_LENGTH / 2, roll=r, pitch=pi / 2, yaw=yw, arm="r", grip=True, frame="table_height", dur=2.5) GripUtils.go_to(x=x, y=y + 3 * PANTS_LENGTH / 4 - 0.03, z=PANTS_LENGTH / 4, roll=r, pitch=5 * pi / 8, yaw=yw, arm="r", grip=True, frame="table_height", dur=2.5) GripUtils.go_to(x=x, y=y + PANTS_LENGTH - 0.03, z=0.01, roll=r, pitch=pi - p, yaw=yw, arm="r", grip=True, frame="table_height", dur=2.5) GripUtils.go_to(x=x, y=y + PANTS_LENGTH + 0.05, z=0.01, roll=r, pitch=pi - p, yaw=yw, arm="r", grip=False, frame="table_height", dur=2.5) GripUtils.go_to(x=x, y=y + PANTS_LENGTH + 0.05, z=0.1, roll=r, pitch=pi - p, yaw=yw, arm="r", grip=False, frame="table_height", dur=2.5) GripUtils.recall_arm("r") smooth() _cloth_tracker.scoot(scoot_amt + 0.08) return True
def sweep_cloth_with_scoot(direction, drag_amount, table_width, lift_amount, roll=0, scoot=0): forward_amount = 0.5 hover_amount = 0.06 edge_distance = (table_width / 2.0) + .04 edge_distance_low = (table_width / 2.0) if direction == "r": drag_arm = "r" edge_y_high = edge_distance edge_y_low = edge_distance_low drag_yaw = pi / 2 waypoint_y = edge_y_low - (drag_amount / 2.0) final_y = edge_y_low - drag_amount else: drag_arm = "l" edge_y_high = -1 * edge_distance edge_y_low = -1 * edge_distance_low drag_yaw = -pi / 2 waypoint_y = edge_y_low + (drag_amount / 2.0) final_y = edge_y_low + drag_amount # Take it to table edge and hold it high if not GripUtils.go_to(x=forward_amount - .21, y=edge_y_high, z=lift_amount + 0.01, roll=roll, pitch=0, yaw=drag_yaw, grip=True, frame="table_height", arm=drag_arm, dur=3): return False for i in range(2): GripUtils.go_to(x=forward_amount - .21, y=edge_y_high - .02, z=lift_amount - .2, roll=roll, pitch=0, yaw=drag_yaw, grip=True, frame="table_height", arm=drag_arm, dur=1.0 / (0.7 * i + 1)) # Lower it at table edge if not GripUtils.go_to(x=forward_amount, y=edge_y_low, z=hover_amount, roll=roll, pitch=pi / 4, yaw=drag_yaw, grip=True, frame="table_height", arm=drag_arm, dur=1): return False # Sweep it back halfway if not GripUtils.go_to(x=forward_amount, y=waypoint_y, z=hover_amount, roll=roll, pitch=pi / 4, yaw=drag_yaw, grip=True, frame="table_height", arm=drag_arm, dur=2.5): return False # Sweep it back the rest of the way if not GripUtils.go_to(x=forward_amount, y=final_y, z=hover_amount, roll=roll, pitch=pi / 4, yaw=drag_yaw, grip=True, frame="table_height", arm=drag_arm, dur=2.5): return False print "Final_y is: %f" % final_y return (forward_amount, final_y, hover_amount, roll, pi / 4, drag_yaw)
def smooth(arm='b'): location = PointStamped() location.point.x = 0.5 location.header.frame_id = "table_height" distance = TABLE_WIDTH / 3 initial_separation = 0.11 GripUtils.recall_arm(arm) if arm == 'b': #Put arms together, with a gap of initial_separation between grippers if not GripUtils.go_to_pts(point_l=location, grip_r=True, grip_l=True, point_r=location, roll_l=pi / 2, yaw_l=0, pitch_l=-pi / 2, y_offset_l=initial_separation / 2.0, z_offset_l=0.05, link_frame_l="l_wrist_back_frame", roll_r=pi / 2, yaw_r=0, pitch_r=-pi / 2, y_offset_r=-1 * initial_separation / 2.0, z_offset_r=0.05, link_frame_r="r_wrist_back_frame", dur=4.0): success = False if not GripUtils.go_to_pts(point_l=location, grip_r=True, grip_l=True, point_r=location, roll_l=pi / 2, yaw_l=0, pitch_l=-pi / 2, y_offset_l=initial_separation / 2.0, z_offset_l=-0.03, link_frame_l="l_wrist_back_frame", roll_r=pi / 2, yaw_r=0, pitch_r=-pi / 2, y_offset_r=-1 * initial_separation / 2.0, z_offset_r=-0.03, link_frame_r="r_wrist_back_frame", dur=2.0): success = False if not GripUtils.go_to_pts( point_l=location, grip_r=True, grip_l=True, point_r=location, roll_l=pi / 2, yaw_l=0, pitch_l=-pi / 2, y_offset_l=(distance + initial_separation) / 2.0, z_offset_l=-0.03, link_frame_l="l_wrist_back_frame", roll_r=pi / 2, yaw_r=0, pitch_r=-pi / 2, y_offset_r=-1 * (distance + initial_separation) / 2.0, z_offset_r=-0.03, link_frame_r="r_wrist_back_frame", dur=2.0): success = False else: #Right is negative in the y axis if arm == "r": y_multiplier = -1 else: y_multiplier = 1 location.point.y -= y_multiplier * distance / 2 if not GripUtils.go_to_pt(point=location, grip=True, roll=pi / 2, yaw=0, pitch=-pi / 2, z_offset=0.05, arm=arm, link_frame="%s_wrist_back_frame" % arm, dur=4.0): success = False if not GripUtils.go_to_pt(point=location, grip=True, roll=pi / 2, yaw=0, pitch=-pi / 2, z_offset=-0.01, arm=arm, link_frame="%s_wrist_back_frame" % arm, dur=2.0): success = False if not GripUtils.go_to_pt(point=location, grip=True, roll=pi / 2, yaw=0, pitch=-pi / 2, y_offset=y_multiplier * distance, z_offset=-0.01, arm=arm, link_frame="%s_wrist_back_frame" % arm, dur=2.0): success = False GripUtils.recall_arm(arm) return True
def execute(self, userdata): GripUtils.recall_arm('b') return SUCCESS