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 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): 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): 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): 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