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)