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 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): self.location = userdata.location self.distance = userdata.distance initial_separation = 0.11 if self.arm=="b": #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): return FAILURE 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.04, 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.04, link_frame_r="r_wrist_back_frame",dur=2.0): return FAILURE 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.03, 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.03, link_frame_r="r_wrist_back_frame",dur=2.0): return FAILURE else: #Right is negative in the y axis if self.arm=="r": y_multiplier = -1 else: y_multiplier = 1.0 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): return FAILURE 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): return FAILURE 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.00,arm=self.arm, link_frame="%s_wrist_back_frame"%self.arm,dur=2.0): 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_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 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 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): bl = userdata.bl br = userdata.br tl = userdata.tl tr = userdata.tr center = bl center.point.x = (bl.point.x + tl.point.x + br.point.x + tr.point.x) / 4 center.point.y = (bl.point.y + tl.point.y + br.point.y + tr.point.y) / 4 center.point.z = (bl.point.z + tl.point.z + br.point.z + tr.point.z) / 4 #calculate width rightTip = PointStamped() rightTip.header.stamp = rospy.Time() rightTip.frame_id = "r_tip_frame" rFromL = self.listener.transformPoint("l_tip_frame", rightTip) width = math.sqrt(rFromL.point.x**2 + rFromL.point.y**2 + rFromL.point.z**2) #distance from right to left #set point_l and point_r (remember to set z to the height of the table) point_l = br point_l.x = center.point.x point_l.y = center.point.y + width / 2 point_l.z = TABLE_HEIGHT #is this wrong? what frame are bl, br, tl, and tr given in? point_r = tl point_r.x = center.point.x point_r.y = center.point.y - width / 2 point_r.z = TABLE_HEIGHT #is this wrong? what frame are bl, br, tl, and tr given in? if not GripUtils.go_to_pts(point_l, pi / 2, pi / 4, -pi / 2, True, point_r, -pi / 2, pi / 4, pi / 2, True, z_offset_l=0.4, z_offset_r=0.4): return FAILURE numTowelsInStack = 0 try: numTowelsInStack = userdata.stackNum except KeyError: pass z_offset = -0.4 + 0.02 * numTowelsInStack if not GripUtils.go_to_relative_multi( 0, 0, z_offset, True, 0, 0, z_offset, True, "base_footprint"): return FAILURE if not GripUtils.open_grippers(): return FAILURE userdata.stackNum = numTowelsInStack + 1 return SUCCESS
def execute(self, userdata): self.location = userdata.location self.distance = userdata.distance initial_separation = 0.11 if self.arm == "b": #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): return FAILURE 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.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): return FAILURE 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.03, 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.03, link_frame_r="r_wrist_back_frame", dur=2.0): return FAILURE else: #Right is negative in the y axis if self.arm == "r": y_multiplier = -1 else: y_multiplier = 1 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): return FAILURE 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): return FAILURE 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): return FAILURE return SUCCESS
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): bl = userdata.bl br = userdata.br tl = userdata.tl tr = userdata.tr center = bl center.point.x = (bl.point.x + tl.point.x + br.point.x + tr.point.x) / 4 center.point.y = (bl.point.y + tl.point.y + br.point.y + tr.point.y) / 4 center.point.z = (bl.point.z + tl.point.z + br.point.z + tr.point.z) / 4 # calculate width rightTip = PointStamped() rightTip.header.stamp = rospy.Time() rightTip.frame_id = "r_tip_frame" rFromL = self.listener.transformPoint("l_tip_frame", rightTip) width = math.sqrt( rFromL.point.x ** 2 + rFromL.point.y ** 2 + rFromL.point.z ** 2 ) # distance from right to left # set point_l and point_r (remember to set z to the height of the table) point_l = br point_l.x = center.point.x point_l.y = center.point.y + width / 2 point_l.z = TABLE_HEIGHT # is this wrong? what frame are bl, br, tl, and tr given in? point_r = tl point_r.x = center.point.x point_r.y = center.point.y - width / 2 point_r.z = TABLE_HEIGHT # is this wrong? what frame are bl, br, tl, and tr given in? if not GripUtils.go_to_pts( point_l, pi / 2, pi / 4, -pi / 2, True, point_r, -pi / 2, pi / 4, pi / 2, True, z_offset_l=0.4, z_offset_r=0.4, ): return FAILURE numTowelsInStack = 0 try: numTowelsInStack = userdata.stackNum except KeyError: pass z_offset = -0.4 + 0.02 * numTowelsInStack if not GripUtils.go_to_relative_multi(0, 0, z_offset, True, 0, 0, z_offset, True, "base_footprint"): return FAILURE if not GripUtils.open_grippers(): return FAILURE userdata.stackNum = numTowelsInStack + 1 return SUCCESS