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 if not GripUtils.grab_points( point_l=bl, roll_l=pi / 2, yaw_l=-pi / 2, pitch_l=pi / 4, x_offset_l=0.01, point_r=br, roll_r=-pi / 2, yaw_r=pi / 2, pitch_r=pi / 4, x_offset_r=0.01, y_offset_l=-0.01, y_offset_r=0.02, 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 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 if not GripUtils.grab_points(point_l=bl, roll_l=pi / 2, yaw_l=-pi / 2, pitch_l=pi / 4, x_offset_l=0.01, point_r=br, roll_r=-pi / 2, yaw_r=pi / 2, pitch_r=pi / 4, x_offset_r=0.01, y_offset_l=-0.01, y_offset_r=0.02, INIT_SCOOT_AMT=0.01): 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 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 executeFold(self,gripPts,endPts,color_current='blue',color_next='blue'): """ execute a fold """ #--- TODO--- convert gripPts,endPts to current frame of robot gripPts_new = [] for pt in gripPts: if pt == None: continue pt_world = self.convert_to_world_frame(pt) 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) gripPts_new.append(pt_transformed) endPts_new = [] for pt in endPts: if pt == None: continue pt_world = self.convert_to_world_frame(pt) 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) endPts_new.append(pt_transformed) gripPts = gripPts_new endPts = endPts_new if(gripPts[1] == None): self.executeFold_left(gripPt=gripPts[0],endPt=endPts[0],color_current=color_current,color_next=color_next) return elif(gripPts[0] == None): self.executeFold_right(gripPt=gripPts[1],endPt=endPts[1],color_current=color_current,color_next=color_next) return # if blue fold, move arms to gripping position. (if red fold, arms already holding cloth at gripping position) if (color_current == 'blue'): if (len(gripPts) > 2) or (len(endPts) > 2): rospy.loginfo("Requires too many grippers") return False startpoints = [] for pt in gripPts: #pt_world = self.convert_to_world_frame(pt) pt_world = self.dupl_PointStamped(pt) startpoints.append(pt_world) # determine approach yaws #yaw_l = -pi/3 if gripPts[0].point.x < endPts[0].point.x else -4*pi/3 # left gripper #yaw_r = pi/3 if gripPts[1].point.x > endPts[1].point.x else 4*pi/3 # right gripper # move both arms 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 midpoints1 = [] midpoints = [] for pt in gripPts: pt_world = pt# self.convert_to_world_frame(pt) midpoints1.append(pt_world) i = 0 for pt in endPts: pt_world = self.dupl_PointStamped(pt) #self.convert_to_world_frame(pt) pt_world.point.x = (pt_world.point.x + midpoints1[i].point.x)/2.0 pt_world.point.y = (pt_world.point.y + midpoints1[i].point.y)/2.0 pt_world.point.z = pt_world.point.z + 0.1 midpoints.append(pt_world) i +=1 pitch_l = pitch_r = pi/4 roll_l = -pi/2 roll_r = pi/2 yaw_l=-pi/2 yaw_r= pi/2 grip_l=grip_r=True 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=roll_l,pitch_l=pitch_l,yaw_l=yaw_l,grip_l=grip_l,frame_l=frame_l, x_r=midpoints[1].point.x,y_r=midpoints[1].point.y,z_r=midpoints[1].point.z,roll_r=roll_r,pitch_r=pitch_r,yaw_r=yaw_r,grip_r=grip_r,frame_r=frame_r,dur=7.5): print "failure" #return False endpoints = [] for pt in endPts: pt_world = self.dupl_PointStamped(pt) #self.convert_to_world_frame(pt) endpoints.append(pt_world) frame_l=frame_r = endpoints[0].header.frame_id if not GripUtils.go_to_multi (x_l=endpoints[0].point.x,y_l=endpoints[0].point.y,z_l=endpoints[0].point.z,roll_l=roll_l,pitch_l=pitch_l,yaw_l=yaw_l,grip_l=grip_l,frame_l=frame_l ,x_r=endpoints[1].point.x,y_r=endpoints[1].point.y,z_r=endpoints[1].point.z,roll_r=roll_r,pitch_r=pitch_r,yaw_r=yaw_r,grip_r=grip_r,frame_r=frame_r ,dur=7.5): print "failure" #return False if(color_next == 'blue'): initRobot() return True