def sensing_tote(self, item):
#bef moving
rospy.loginfo('gripper -- pre moving')
arm_control_wrapper.move_right_arm_global(
l2a_([550, -900, 100, -180, -0, -180]), 'nut000', 'chokusen',
'normal')
#arm_control_wrapper.move_right_arm_global( l2a_([550,-900,100, -90, 90, -90]), 'fut000', 'kakujiku', 'normal')
#arm_control_wrapper.move_right_arm_global( l2a_([550,-900,100,90,0,90]), 'fut000', 'kakujiku', 'fast')
arm_control_wrapper.move_right_arm_global(
l2a_([550, -900, 100, 90, -90, 90]), 'fut000', 'kakujiku',
'normal')
prg = np.asarray([550, -550, -220, 90, -90, 90])
plg = np.asarray([550, -250, -220, 90, -90, 90])
for i, p in enumerate([prg, plg]):
rospy.loginfo('gripper -- scan pos')
arm_control_wrapper.move_right_arm_global(l2a_(p), 'fut000',
'chokusen', 'normal')
rospy.loginfo('gripper -- add pcl data')
req = ItemPoseEstAddDataRequest()
req.item = item
req.x = p[0]
req.y = p[1]
req.z = p[2]
req.ax = p[3]
req.ay = p[4]
req.az = p[5]
req.is_first_data = True if i == 0 else False
self.sv_item_pose_est_adddata(req)
#raw_input("stop")
rospy.loginfo('gripper -- call item_pose_est')
res = self.sv_item_pose_est(item)
rospy.loginfo(res)
#raw_input("stop")
rospy.loginfo('gripper -- %f %f %f %f %f %f' %
(res.x, res.y, res.z, res.ax, res.ay, res.az))
#post moving
rospy.loginfo('gripper -- post moving')
arm_control_wrapper.move_right_arm_global(
l2a_([550, -900, 100, 90, -90, 90]), 'fut000', 'chokusen',
'normal')
#arm_control_wrapper.move_right_arm_global( l2a_([550,-900,100,90,0,90]), 'fut000', 'kakujiku', 'normal')
#arm_control_wrapper.move_right_arm_global( l2a_([550,-900,100, -90, 90, -90]), 'fut000', 'kakujiku', 'normal')
arm_control_wrapper.move_right_arm_global(
l2a_([550, -900, 100, -180, -0, -180]), 'nut000', 'kakujiku',
'normal')
if res.success == False:
return False, None
if res.no_item == True:
return False, None
return True, [res.x, res.y, res.z, res.ax, res.ay, res.az]
def grip_from_tote(self, itempose): #global
rospy.loginfo('gripper -- grip from tote')
x = itempose[0]
y = itempose[1]
z = itempose[2]
ax = itempose[3] #seibun
ay = itempose[4]
az = itempose[5]
if abs(ax - ay) < 0.2:
#stand
ax = 0
else:
az_ = math.atan2(ay, ax)
tote_z = -1160
z1 = -1160 + self.hand_params.arm_L_x + 300
z2 = -1160 + self.hand_params.arm_L_x + 0
arm_control_wrapper.move_right_arm_global(
l2a_([x, y, z1, -180, -0, az]), 'nut000', 'kakujiku',
'normal') #-180,0, then az
self.move_grip('2000')
arm_control_wrapper.move_right_arm_global(
l2a_([x, y, z2, -180, -0, az]), 'nut000', 'chokusen',
'normal') #-180,0, then az
self.move_grip('0')
arm_control_wrapper.move_right_arm_global(
l2a_([x, y, z1, -180, -0, az]), 'nut000', 'chokusen',
'normal') #-180,0, then az
arm_control_wrapper.move_right_arm_global(
l2a_([550, y, z1, -180, -0, az]), 'nut000', 'chokusen', 'normal')
arm_control_wrapper.move_right_arm_global(
l2a_([550, y, z1, 90, 0, 90]), 'fut000', 'kakujiku', 'normal')
def test_arm_control_lowlevel_right():
if not raw_input("This will send commands to the robot. " +
"Is the area safe? [yN] ").lower() == "y":
rospy.loginfo("Aborted pick process because not safe")
print "abort"
return
# some position (should be bin_A/photo)
target = np.asarray([725, -102, 720, 180, -20, 180])
print "move to %s" % (target, )
result = arm_control_wrapper.move_right_arm_global(target)
print "result:", result
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_tote(self, p):
arm_control_wrapper.move_right_arm_global(p, 'fut000', 'kakujiku',
'normal')
self.prev_grip_armpose = l2a(pos, angle)