def stow_in_tote(self, pos): print 'prev grip type : %d' % self.prev_grip_type print self.prev_grip_armpose #if self.prev_grip_type == 1: #horizontal #change angle ap = [500, -220, -110] agl = [-180, 0, 90] rospy.loginfo('gripper tote0') arm_control_wrapper.move_right_arm_global(l2a(ap, agl), 'nut000', 'kakujiku', 'normal') pos = [550, -220, -980] #stow pos bp = [0, 0, 0] bp[0] = pos[0] bp[1] = pos[1] + self.hand_params.grip_center_diff_y bp[2] = pos[2] + self.hand_params.arm_L_x rospy.loginfo('gripper tote1') arm_control_wrapper.move_right_arm_global(l2a(bp, agl), 'nut000', 'kakujiku', 'normal') if self.prev_grip_type < 10: self.move_grip('2000') else: #for cup self.move_grip('5000') time.sleep(1.0) self.move_grip('3500') rospy.loginfo('gripper tote2') hup = 300 cp = bp cp[2] = cp[2] + hup arm_control_wrapper.move_right_arm_global(l2a(cp, agl), 'nut000', 'kakujiku', 'normal')
def move_for_grip(self, pos, angle): arm_control_wrapper.move_right_arm_local(self.target_bin, l2a(pos, angle), 'fut000', 'kakujiku', 'normal') self.prev_grip_armpose = l2a(pos, angle)
def move_for_grip_tote(self, pos, angle): arm_control_wrapper.move_right_arm_global(l2a(pos, angle), 'nut000', 'kakujiku', 'normal') self.prev_grip_armpose = l2a(pos, angle)
def move_for_scan(self, pos, angle): arm_control_wrapper.move_right_arm_local(self.target_bin, l2a(pos, angle), 'nut000', 'chokusen', 'normal') self.prev_grip_armpose = l2a(pos, angle)