def _move_arm_to_bin(which, bin, position,keitai, kakujiku, speed = 'normal'):
# create and send the goal
client = actionlib.SimpleActionClient('move_to_bin_' + which,
BinToteMoveAction)
client.wait_for_server()
goal = BinToteMoveGoal(bin=bin, position=position,
fut000=change_fut(keitai, speed), kakujiku=kak_dict[kakujiku])
client.send_goal(goal)
done_before_timeout = client.wait_for_result(rospy.Duration.from_sec(1600.0))
# error check
if not done_before_timeout:
rospy.logerr("robot arm move did not finish in time")
raise util.RobotTimeoutException(which, goal)
state = client.get_state()
if state != GoalStatus.SUCCEEDED:
rospy.logerr("robot move action did not finish correctly")
raise util.RobotActionFailure(which, goal, state)
result = client.get_result()
if not result.success:
rospy.logerr("robot arm did not move correctly")
raise util.RobotMoveFailure(which, goal)
return (result.success,
util.twist2array(result.position),
result.is_calibrated,
util.twist2array(result.global_position))
def __init__(self, bin, item, pos, lr):
super(pick_shelf_from_side, self).__init__()
self.bin = bin
self.item = item
self.lr = lr
target_pos = np.asarray([0, 0, 0, 0, 0, 0]).astype('f')
target_pos[:3] += pos
target_pos_bin = twist2array(
self.t_bin_srv(bin=self.bin, point=array2twist(target_pos)).point)
if lr == 'left':
target_pos_bin[3:] = np.asarray([90, 0, 90]).astype('f')
if bin in ['bin_J']:
target_pos_bin[3:] = np.asarray([90, -20, 90]).astype('f')
target_pos_bin[:3] += np.asarray([-650, 40, 0]).astype('f')
self.target_pos = target_pos_bin + np.asarray([0, 0, 0, 0, 0, 0])
self.pre_target_pos = target_pos_bin + np.asarray(
[-200, 50, 50, 0, 0, 0])
self.approach_target_pos = target_pos_bin + np.asarray(
[0, 50, 50, 0, 0, 0])
self.approach_target_pos_ret = target_pos_bin + np.asarray(
[0, 50, 50, 0, 0, 0])
self.pre_target_pos_ret = target_pos_bin + np.asarray(
[-200, 50, 50, 0, 0, 0])
elif lr == 'right':
target_pos_bin[3:] = np.asarray([90, -180, 90]).astype('f')
target_pos_bin[:3] += np.asarray([-650, -40, 0]).astype('f')
self.target_pos = target_pos_bin + np.asarray([0, 0, 0, 0, 0, 0])
self.pre_target_pos = target_pos_bin + np.asarray(
[-200, -50, 50, 0, 0, 0])
self.approach_target_pos = target_pos_bin + np.asarray(
[0, -50, 50, 0, 0, 0])
self.approach_target_pos_ret = target_pos_bin + np.asarray(
[0, -50, 50, 0, 0, 0])
self.pre_target_pos_ret = target_pos_bin + np.asarray(
[-200, -50, 50, 0, 0, 0])
# hikidashi ha absolute coordinate
self.pre_target_pos[0] = -100 - 650
self.pre_target_pos_ret[0] = -100 - 650
# hashira wo yokeru
if bin in ['bin_A', 'bin_D', 'bin_G', 'bin_J']:
if target_pos_bin[1] > -130:
self.approach_target_pos[1] -= 20
self.approach_target_pos_ret[1] -= 20
self.pre_target_pos_ret[1] -= 20
elif bin in ['bin_C', 'bin_F', 'bin_I', 'bin_L']:
if target_pos_bin[1] < -120:
self.approach_target_pos[1] += 20
self.approach_target_pos_ret[1] += 20
self.pre_target_pos_ret[1] += 20
def sensing_normal(self, bin, item):
#scan poses
pf = np.asarray([-410, -100, 315, -180, -24, -180])
pl = np.asarray([-410, 30, 315, -180, -24, -200])
pr = np.asarray([-410, -260, 315, -180, -24, 200])
pb = np.asarray([-520, -100, 115, -180, 0, 180])
for i, p in enumerate([pf, pl, pr, pb]):
rospy.loginfo('gripper -- scan pos')
res = arm_control_wrapper.move_right_arm_local(
bin, p, 'nut000', 'kakujiku')
rospy.loginfo(res)
#raw_input("stop")
rospy.loginfo('gripper -- convert arm pose to global')
ct = CoordinateTransformRequest()
ct.bin = bin
ct.point = util.array2twist(p)
res = self.sv_bin2global(ct)
pp = util.twist2array(res.point)
rospy.loginfo(pp)
rospy.loginfo('gripper -- add pcl data')
req = ItemPoseEstAddDataRequest()
req.item = item
req.x = pp[0]
req.y = pp[1]
req.z = pp[2]
req.ax = pp[3]
req.ay = pp[4]
req.az = pp[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")
if res.success == False:
return False, None
if res.no_item == True:
return False, None
rospy.loginfo('gripper -- convert item coordinates global to bin')
ct = CoordinateTransformRequest()
ct.bin = bin
ct.point = util.array2twist(
[res.x, res.y, res.z, res.ax, res.ay, res.az])
res_ = self.sv_global2bin(ct)
itmps_bin = util.twist2array(res_.point)
itmps_bin[3] = res.ax
itmps_bin[4] = res.ay
itmps_bin[5] = res.az
rospy.loginfo('gripper -- item in bin %f %f %f %f %f %f' %
(itmps_bin[0], itmps_bin[1], itmps_bin[2], itmps_bin[3],
itmps_bin[4], itmps_bin[5]))
return True, itmps_bin
def sensing_norma_for_pencilcup(self, bin, item):
#scan poses
if bin == 'bin_B' or bin == 'bin_E' or bin == 'bin_H' or bin == 'bin_K':
cy = -150
else:
cy = -125
pf = np.asarray([-430, cy, 304, -180, -25, -180])
pp = np.asarray([-530, cy, 150, -180, -25, -180])
if bin == 'bin_A' or bin == 'bin_B' or bin == 'bin_C':
res = arm_control_wrapper.move_right_arm_local(
bin, pp, 'nut000', 'kakujiku')
for i, p in enumerate([pf]):
rospy.loginfo('gripper -- scan pos')
res = arm_control_wrapper.move_right_arm_local(
bin, p, 'nut000', 'kakujiku')
rospy.loginfo(res)
#raw_input("stop")
time.sleep(1)
rospy.loginfo('gripper -- convert arm pose to global')
ct = CoordinateTransformRequest()
ct.bin = bin
ct.point = util.array2twist(p)
res = self.sv_bin2global(ct)
pp = util.twist2array(res.point)
rospy.loginfo(pp)
rospy.loginfo('gripper -- add pcl data')
req = ItemPoseEstAddDataRequest()
req.item = item
req.x = pp[0]
req.y = pp[1]
req.z = pp[2]
req.ax = pp[3]
req.ay = pp[4]
req.az = pp[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")
if res.success == False:
return False, None
if res.no_item == True:
return False, None
itmps_bin = [res.x, res.y, res.z, res.ax, res.ay, res.az]
rospy.loginfo('gripper -- pencil sensing in bin %f %f %f %f %f %f' %
(itmps_bin[0], itmps_bin[1], itmps_bin[2], itmps_bin[3],
itmps_bin[4], itmps_bin[5]))
return True, itmps_bin
def __init__(self, bin, item, pos):
super(pick_shelf_from_above, self).__init__()
self.bin = bin
self.item = item
target_pos = np.asarray([0, 0, 0, 0, 0, 0]).astype('f')
target_pos[:3] += pos
target_pos_bin = twist2array(
self.t_bin_srv(bin=self.bin, point=array2twist(target_pos)).point)
target_pos_bin[3:] = np.asarray([90, -90, 90]).astype('f')
if self.item == "kleenex_paper_towels":
if target_pos_bin[0] < 200:
target_pos_bin[0] = 200
target_pos_bin[:3] += np.asarray([-650, 0, 20]).astype('f')
self.target_pos = target_pos_bin + np.asarray([0, 0, 0, 0, 0, 0])
self.pre_target_pos = target_pos_bin + np.asarray(
[-200, 0, 100, 0, 0, 0])
self.approach_target_pos = target_pos_bin + np.asarray(
[0, 0, 100, 0, 0, 0])
self.approach_target_pos_ret = target_pos_bin + np.asarray(
[0, 0, 150, 0, 0, 0])
self.pre_target_pos_ret = target_pos_bin + np.asarray(
[-200, 0, 150, 0, 0, 0])
# hikidashi ha absolute coordinate
self.pre_target_pos[0] = -100 - 650
self.pre_target_pos_ret[0] = -100 - 650
# hashira wo yokeru
if bin in ['bin_A', 'bin_D', 'bin_G', 'bin_J']:
if target_pos_bin[1] > -50:
self.pre_target_pos[1] -= 30
self.approach_target_pos[1] -= 30
self.approach_target_pos_ret[1] -= 30
self.pre_target_pos_ret[1] -= 30
elif bin in ['bin_C', 'bin_F', 'bin_I', 'bin_L']:
if target_pos_bin[1] < -200:
self.pre_target_pos[1] += 30
self.approach_target_pos[1] += 30
self.approach_target_pos_ret[1] += 30
self.pre_target_pos_ret[1] += 30
# zlim based on item_size
item_min_height = util.item_string_to_3side(item)[2] * 10
self.target_pos[2] = max(item_min_height + 15, self.target_pos[2])
if bin in ['bin_A', 'bin_B', 'bin_C']:
zlim = 190
elif bin in ['bin_D', 'bin_E', 'bin_F']:
zlim = 160
elif bin in ['bin_G', 'bin_H', 'bin_I']:
zlim = 160
elif bin in ['bin_J', 'bin_K', 'bin_L']:
zlim = 190
self.pre_target_pos[2] = zlim
self.approach_target_pos[2] = zlim
self.approach_target_pos_ret[2] = zlim
self.pre_target_pos_ret[2] = zlim
def __init__(self, bin, item, pos):
super(pick_shelf_from_front, self).__init__()
self.bin = bin
self.item = item
target_pos = np.asarray([0, 0, 0, 0, 0, 0]).astype('f')
target_pos[:3] += pos
target_pos_bin = twist2array(
self.t_bin_srv(bin=self.bin, point=array2twist(target_pos)).point)
target_pos_bin[3:] = np.asarray([90, -90, 90]).astype('f')
target_pos_bin[:3] += np.asarray([-650 + 10, 0, 0]).astype('f')
self.target_pos = target_pos_bin + np.asarray([0, 0, 0, 0, 0, 0])
self.pre_target_pos = target_pos_bin + np.asarray(
[-200, 0, 80, 0, 0, 0])
self.approach_target_pos = target_pos_bin + np.asarray(
[-100, 0, 0, 0, 0, 0])
self.approach_target_pos_ret = target_pos_bin + np.asarray(
[0, 0, 80, 0, 0, 0])
self.pre_target_pos_ret = target_pos_bin + np.asarray(
[-200, 0, 80, 0, 0, 0])
# hikidashi ha absolute coordinate
self.pre_target_pos[0] = -100 - 650
self.pre_target_pos_ret[0] = -100 - 650
# hashira wo yokeru
if bin in ['bin_A', 'bin_D', 'bin_G', 'bin_J']:
if target_pos_bin[1] > -130:
self.pre_target_pos[1] -= 30
self.approach_target_pos[1] -= 30
self.approach_target_pos_ret[1] -= 30
self.pre_target_pos_ret[1] -= 30
elif bin in ['bin_C', 'bin_F', 'bin_I', 'bin_L']:
if target_pos_bin[1] < -120:
self.pre_target_pos[1] += 30
self.approach_target_pos[1] += 30
self.approach_target_pos_ret[1] += 30
self.pre_target_pos_ret[1] += 30
# xlim based on item_size
item_min_height = util.item_string_to_3side(item)[2] * 10
self.target_pos[0] = min(450 - item_min_height - 12,
self.target_pos[0])
if bin in ['bin_A', 'bin_B', 'bin_C']:
zlim = 180
elif bin in ['bin_D', 'bin_E', 'bin_F']:
zlim = 130
elif bin in ['bin_G', 'bin_H', 'bin_I']:
zlim = 130
elif bin in ['bin_J', 'bin_K', 'bin_L']:
zlim = 180
self.pre_target_pos[2] = min(zlim, self.pre_target_pos[2])
self.approach_target_pos_ret[2] = min(zlim,
self.approach_target_pos_ret[2])
self.pre_target_pos_ret[2] = min(zlim, self.pre_target_pos_ret[2])
if self.target_pos[2] < 18:
self.target_pos[2] = 18
self.approach_target_pos[2] = 18