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