def test_arm_control_lowlevel_local():
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
# test all bins
for bin_key in "ABCDEFGHIJKL":
pos = np.asarray([150, 150, -200, 0, 0, 0])
print "move to bin_%s/%s" % (bin_key, pos)
result = arm_control_wrapper.move_left_arm_local("bin_" + bin_key, pos)
print "result:", result
if raw_input("press enter to continue or q to quit: ") == "q":
return
def test_arm_control_lowlevel2():
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
rospy.wait_for_service('update_calibration')
s = rospy.ServiceProxy('update_calibration', CalibrationUpdate)
response = s(upper_left=Point(1250, 35, 1037),
upper_right=Point(1250, -835, 1037))
# test all bins
for bin_key in "BCDEFGHIIJKL":
#for pos in [np.asarray([0,0,-660,89,89,89]),np.asarray([100,100,-560,89,89,89])]:
for pos in [np.asarray([-600, -100, 100, 89, -89, 89])]:
#for pos in [Twist(Vector3(0,0,100),Vector3()),Twist(Vector3(100,100,150),Vector3(90,-90,90))]:
print "move to bin_%s/%s" % (bin_key, pos)
result = arm_control_wrapper.move_left_arm_local(
"bin_" + bin_key, pos)
print "result:", result
raw_input("press enter to continue or ctrl-c to quit ")
def move(self, grabber):
grabber.pipe_90_deg()
print 'pre_target_pos'
arm_control_wrapper.move_left_arm_local(self.bin,
self.pre_target_pos,
keitai='fut000',
kakujiku='chokusen')
grabber.vacuum_on()
print 'approach_target_pos'
arm_control_wrapper.move_left_arm_local(self.bin,
self.approach_target_pos,
keitai='fut000',
kakujiku='chokusen')
grabber.vacuum_on()
print 'target_pos'
arm_control_wrapper.move_left_arm_local(self.bin,
self.target_pos,
keitai='fut000',
kakujiku='chokusen')
time.sleep(0.5)
print 'approach_target_pos_ret'
arm_control_wrapper.move_left_arm_local(self.bin,
self.approach_target_pos_ret,
keitai='fut000',
kakujiku='chokusen')
force_retry = True if self.item in force_retry_items else False
time.sleep(0.5)
if (not grabber.vac_ok(self.item).success):
grabber.vacuum_on()
print 'high pressure -> retry #1'
if self.lr == 'left':
arm_control_wrapper.move_left_arm_local(
self.bin,
self.target_pos + np.asarray([0, -10, 0, 0, 0, 0]),
keitai='fut000',
kakujiku='chokusen')
arm_control_wrapper.move_left_arm_local(
self.bin,
self.approach_target_pos,
keitai='fut000',
kakujiku='chokusen')
else:
arm_control_wrapper.move_left_arm_local(
self.bin,
self.target_pos + np.asarray([0, 10, 0, 0, 0, 0]),
keitai='fut000',
kakujiku='chokusen')
arm_control_wrapper.move_left_arm_local(
self.bin,
self.approach_target_pos,
keitai='fut000',
kakujiku='chokusen')
if (not grabber.vac_ok(self.item).success):
print 'high pressure -> retry #2'
if self.lr == 'left':
arm_control_wrapper.move_left_arm_local(
self.bin,
self.target_pos + np.asarray([20, -10, 0, 0, 0, 0]),
keitai='fut000',
kakujiku='chokusen')
arm_control_wrapper.move_left_arm_local(
self.bin,
self.approach_target_pos,
keitai='fut000',
kakujiku='chokusen')
else:
arm_control_wrapper.move_left_arm_local(
self.bin,
self.target_pos + np.asarray([20, 10, 0, 0, 0, 0]),
keitai='fut000',
kakujiku='chokusen')
arm_control_wrapper.move_left_arm_local(
self.bin,
self.approach_target_pos,
keitai='fut000',
kakujiku='chokusen')
if (not grabber.vac_ok(self.item).success):
print 'high pressure -> retry #3'
if self.lr == 'left':
arm_control_wrapper.move_left_arm_local(
self.bin,
self.target_pos + np.asarray([-20, -10, 0, 0, 0, 0]),
keitai='fut000',
kakujiku='chokusen')
arm_control_wrapper.move_left_arm_local(
self.bin,
self.approach_target_pos,
keitai='fut000',
kakujiku='chokusen')
else:
arm_control_wrapper.move_left_arm_local(
self.bin,
self.target_pos + np.asarray([-20, 10, 0, 0, 0, 0]),
keitai='fut000',
kakujiku='chokusen')
arm_control_wrapper.move_left_arm_local(
self.bin,
self.approach_target_pos,
keitai='fut000',
kakujiku='chokusen')
print 'pre_target_pos_ret'
arm_control_wrapper.move_left_arm_local(self.bin,
self.pre_target_pos_ret,
keitai='fut000',
kakujiku='chokusen')
time.sleep(0.5)
if not grabber.vac_ok(self.item).success:
rospy.loginfo("pick tried but seemes not success (vac not ok)")
arm_control_wrapper.move_left_arm_local(self.bin,
self.pre_target_pos_ret,
keitai='fut000',
kakujiku='chokusen')
arm_control_wrapper.move_left_arm_to_bin(self.bin,
'pull',
keitai='fut000',
kakujiku='chokusen')
arm_control_wrapper.move_left_arm_to_bin(self.bin,
'pre',
keitai='nut000',
kakujiku='kakujiku')
if force_retry:
arm_control_wrapper.move_left_arm_to_bin('bin_K',
'pre',
keitai='nut000',
kakujiku='chokusen',
speed="fast")
grabber.vacuum_off()
arm_control_wrapper.move_left_arm_to_bin('bin_H',
'pre',
keitai='nut000',
kakujiku='chokusen',
speed="fast")
else:
grabber.vacuum_off()
return False
arm_control_wrapper.move_left_arm_to_bin(
self.bin, 'pull', keitai='fut000',
kakujiku='chokusen') # TODO kore ha global de ugokashi tain dakedo
return True
def move(self, grabber):
grabber.pipe_90_deg()
print 'pre_target_pos'
arm_control_wrapper.move_left_arm_local(self.bin,
self.pre_target_pos,
keitai='fut000',
kakujiku='chokusen')
grabber.vacuum_on()
print 'approach_target_pos'
arm_control_wrapper.move_left_arm_local(self.bin,
self.approach_target_pos,
keitai='fut000',
kakujiku='chokusen')
grabber.vacuum_on()
print 'target_pos'
arm_control_wrapper.move_left_arm_local(self.bin,
self.target_pos,
keitai='fut000',
kakujiku='chokusen')
time.sleep(0.5)
force_retry = True if self.item in force_retry_items else False
# retry
if (not grabber.vac_ok(self.item).success) and self.target_pos[2] > 40:
grabber.vacuum_on()
print 'high pressure -> retry #1'
grabber.pipe_45_deg()
arm_control_wrapper.move_left_arm_local(
self.bin,
self.target_pos + np.asarray([20, 0, -20, 0, 0, 0]),
keitai='fut000',
kakujiku='chokusen')
if (not grabber.vac_ok(
self.item).success) and self.target_pos[0] < 450:
print 'high pressure -> retry #2'
grabber.pipe_90_deg()
arm_control_wrapper.move_left_arm_local(self.bin,
self.approach_target_pos,
keitai='fut000',
kakujiku='chokusen')
arm_control_wrapper.move_left_arm_local(
self.bin,
self.target_pos + np.asarray([20, 0, 0, 90, 3, -90]),
keitai='fut000',
kakujiku='chokusen')
if (not grabber.vac_ok(self.item).success):
print 'high pressure -> retry #3'
grabber.pipe_90_deg()
arm_control_wrapper.move_left_arm_local(self.bin,
self.approach_target_pos,
keitai='fut000',
kakujiku='chokusen')
arm_control_wrapper.move_left_arm_local(
self.bin,
self.target_pos + np.asarray([-20, 0, 0, 90, 3, -90]),
keitai='fut000',
kakujiku='chokusen')
if self.item in [
'dasani_water_bottle', 'command_hooks',
'platinum_pets_dog_bowl'
]: # transparent items. (they are sometimes depth deeper)
if not grabber.vac_ok(self.item).success:
print 'high pressure -> retry #4'
grabber.pipe_90_deg()
arm_control_wrapper.move_left_arm_local(
self.bin,
self.approach_target_pos,
keitai='fut000',
kakujiku='chokusen')
arm_control_wrapper.move_left_arm_local(
self.bin,
self.target_pos + np.asarray([-70, 0, 0, 0, 0, 0]),
keitai='fut000',
kakujiku='chokusen')
else:
if (not grabber.vac_ok(
self.item).success) and self.target_pos[0] < 400:
print 'high pressure -> retry #4'
grabber.pipe_90_deg()
arm_control_wrapper.move_left_arm_local(
self.bin,
self.approach_target_pos,
keitai='fut000',
kakujiku='chokusen')
arm_control_wrapper.move_left_arm_local(
self.bin,
self.target_pos + np.asarray([50, 0, 0, 90, 3, -90]),
keitai='fut000',
kakujiku='chokusen')
#if not grabber.vac_ok(self.item).success:
# print 'high pressure -> retry #5'
# grabber.pipe_90_deg()
# arm_control_wrapper.move_left_arm_local(self.bin, self.approach_target_pos, keitai='fut000', kakujiku='chokusen')
# arm_control_wrapper.move_left_arm_local(self.bin, self.target_pos + np.asarray([-50,0,0,0,0,0]), keitai='fut000', kakujiku='chokusen')
print 'approach_target_pos_ret'
arm_control_wrapper.move_left_arm_local(self.bin,
self.approach_target_pos_ret,
keitai='fut000',
kakujiku='chokusen')
print 'pre_target_pos_ret'
arm_control_wrapper.move_left_arm_local(self.bin,
self.pre_target_pos_ret,
keitai='fut000',
kakujiku='chokusen')
time.sleep(0.5)
if not grabber.vac_ok(self.item).success:
rospy.loginfo("pick tried but seemes not success (vac not ok)")
arm_control_wrapper.move_left_arm_local(self.bin,
self.pre_target_pos_ret,
keitai='fut000',
kakujiku='chokusen')
arm_control_wrapper.move_left_arm_to_bin(self.bin,
'pull',
keitai='fut000',
kakujiku='chokusen')
arm_control_wrapper.move_left_arm_to_bin(self.bin,
'pre',
keitai='nut000',
kakujiku='kakujiku')
if force_retry:
arm_control_wrapper.move_left_arm_to_bin('bin_K',
'pre',
keitai='nut000',
kakujiku='chokusen',
speed="fast")
grabber.vacuum_off()
arm_control_wrapper.move_left_arm_to_bin('bin_H',
'pre',
keitai='nut000',
kakujiku='chokusen',
speed="fast")
else:
grabber.vacuum_off()
return False
arm_control_wrapper.move_left_arm_to_bin(
self.bin, 'pull', keitai='fut000',
kakujiku='chokusen') # TODO kore ha global de ugokashi tain dakedo
return True