def execute(self, userdata): try: getattr(userdata, 'arm_ps') getattr(userdata, 'arm_mode') except: rospy.logerr('No params specified!') return 'error' if userdata.arm_mode is 0: xm_arm_moveit_name('nav_poseb') return 'succeeded' else: pass if userdata.arm_mode is 1: gripper_service_level(True) gripper_service_level(True) xm_arm_moveit_name('nav_poseb') base_ps = self.tf_listener.transformPoint('base_link', userdata.arm_ps) solution_list = list() self.haha = 0 solution_list.append(0.069) solution_list.append(math.atan2(base_ps.point.y, base_ps.point.x)) #distance = math.sqrt(base_ps.point.y*base_ps.point.y + base_ps.point.x * base_ps.point.x) #distance -= 0.33 solution_list.append(-0.6384) solution_list.append(1.0073) solution_list.append(0.6044) solution_list.append(-1.57) #if solution_list[3]>1.57: # solution_list[3]=1.57 # solution_list[2]=0.1 lifting_value = solution_list[0] joint_value = solution_list[1:6] lifting_controller_level(lifting_value) rospy.sleep(10) #joint_value_turn = solution_list[1:6] #joint_value_turn[1] = -1.56999 #joint_value_turn[2] = 1.61619 #joint_value_turn[3] = 1.23 #joint_value_turn[4] = -1.57 #arm_controller_level(joint_value_turn) arm_controller_level(joint_value) rospy.sleep(5) #gripper_service_level(False) #0.000261899782345 -1.56999063492 1.6161891222 1.54923319817 0.0 joint_value[3] = 1.5472 joint_value[2] = 1.6154 joint_value[1] = -1.57 #joint_value[3]=0.0 arm_controller_level(joint_value) rospy.sleep(1) #if self.haha == 159: # xm_arm_moveit_name('after_high') #xm_arm_moveit_name('grasp_pose') #xm_arm_moveit_name('nav_poseb')#抓完之后的位置 return 'succeeded'
def execute(self, userdata): global middle_joints, grasp_joints try: getattr(userdata, 'arm_ps') userdata.arm_ps.point.z += 0.07 userdata.arm_ps.point.x += 0.0 userdata.arm_ps.point.y += 0.0 except: rospy.logerr('No params specified') return 'error' service_bool = self.ik_client.wait_for_service(timeout=10) if service_bool == False: rospy.logerr('time out, ik-service is no avaiable') return 'aborted' else: # this is the first motion rospy.logwarn('first step') ik_req = xm_SolveIKRequest() ik_req.goal = deepcopy(userdata.arm_ps) ik_res = self.ik_client.call(ik_req) ik_bool = True self.ik_client.close() if ik_res.result == True: solution_list = list(ik_res.solution) lifting_value = solution_list[0] joint_value = solution_list[1:] grasp_joints = deepcopy(solution_list) arm_controller_level(joint_value) # move the lifting lifting_controller_level(lifting_value) rospy.sleep(1.0) else: ik_bool = False rospy.logwarn('ik error') # this is the second motion, will open or close the gripper # true open false close rospy.logwarn('second step') gripper_mode = False gripper_service_level(gripper_mode) rospy.sleep(1.0) if ik_bool == True: rospy.logwarn('third step') joint_value[2] += 0.3 joint_value[3] -= 0.2 arm_controller_level(joint_value) # move the lifting lifting_controller_level(lifting_value) rospy.sleep(1.0) else: pass rospy.logwarn('fourth step') xm_arm_moveit_name('after_grasp_bag') rospy.sleep(1.0) return 'succeeded'
def laiya(): rospy.wait_for_service('arm_stack') grasp = rospy.ServiceProxy('arm_stack', xm_PickOrPlace) req = xm_PickOrPlaceRequest() req.action = 1 req.objmode = 0 req.goal_position.header.frame_id = 'base_link' req.goal_position.point.x = 0.8 req.goal_position.point.y = 0.0 req.goal_position.point.z = 0.7 res1 = grasp(req) xm_arm_moveit_name('after_grasp_things') gripper_service_level(2) req.action = 3 req.goal_position.point.y += 0.2 res2 = grasp(req) xm_arm_moveit_name('after_grasp_things') gripper_service_level(2) req.goal_position.point.y += 0.2 res3 = grasp(req)
def execute(self, userdata): try: getattr(userdata, 'arm_ps') getattr(userdata, 'arm_mode') except: rospy.logerr('No params specified!') return 'error' if userdata.arm_mode is 0: xm_arm_moveit_name('nav_posec') return 'succeeded' else: pass if userdata.arm_mode is 1: gripper_service_level(True) gripper_service_level(True) xm_arm_moveit_name('nav_posec') base_ps = self.tf_listener.transformPoint('base_link', userdata.arm_ps) solution_list = list() self.haha = 0 solution_list.append(0.069) solution_list.append(math.atan2(base_ps.point.y, base_ps.point.x)) #distance = math.sqrt(base_ps.point.y*base_ps.point.y + base_ps.point.x * base_ps.point.x) #distance -= 0.33 solution_list.append(-0.2760) solution_list.append(0.6692) solution_list.append(-0.1692) solution_list.append(-1.57) #if solution_list[3]>1.57: # solution_list[3]=1.57 # solution_list[2]=0.1 lifting_value = solution_list[0] joint_value = solution_list[1:6] lifting_controller_level(lifting_value) rospy.sleep(10) arm_controller_level(joint_value) rospy.sleep(3) gripper_service_level(False) rospy.sleep(5) joint_value[3] = 0.8220 joint_value[2] = 1.6154 joint_value[1] = -1.57 joint_value[0] = -0.6 #joint_value[3]=0.0 #xm_arm_moveit_name('bag') arm_controller_level(joint_value) rospy.sleep(5) #if self.haha == 159: # xm_arm_moveit_name('after_high') #xm_arm_moveit_name('grasp_pose') #xm_arm_moveit_name('nav_poseb')#抓完之后的位置 return 'succeeded'
def execute(self, userdata): try: getattr(userdata, 'arm_ps') getattr(userdata, 'arm_mode') except: rospy.logerr('No params specified!') return 'error' if userdata.arm_mode is 0: xm_arm_moveit_name('nav_poseb') return 'succeeded' else: pass if userdata.arm_mode is 1: gripper_service_level(True) gripper_service_level(True) xm_arm_moveit_name('nav_poseb') base_ps = self.tf_listener.transformPoint('base_link', userdata.arm_ps) solution_list = list() self.haha = 0 solution_list.append(0.0) solution_list.append(0) solution_list.append(-0.6901) solution_list.append(1.2422) solution_list.append(-0.4352) solution_list.append(0) #if solution_list[3]>1.57: # solution_list[3]=1.57 # solution_list[2]=0.1 lifting_value = solution_list[0] joint_value = solution_list[1:6] lifting_controller_level(lifting_value) rospy.sleep(10) arm_controller_level(joint_value) rospy.sleep(3) gripper_service_level(False) rospy.sleep(5) joint_value[1] = -1 joint_value[2] = 1.4 joint_value[3] = 1 arm_controller_level(joint_value) rospy.sleep(5) #if self.haha == 159: # xm_arm_moveit_name('after_high') #xm_arm_moveit_name('grasp_pose') #xm_arm_moveit_name('nav_poseb')#抓完之后的位置 return 'succeeded'
def __init__(self): rospy.init_node('xm_arm_smach') rospy.on_shutdown(self.shutdown) gripper_service_level(True) xm_arm_moveit_name('nav_pose') rospy.logerr('Link Start!') self.arm_stack_service = rospy.Service('arm_stack', xm_PickOrPlace, self.callback) self.sm_ArmStack = StateMachine( outcomes=['succeeded', 'aborted', 'error'], input_keys=['arm_point', 'arm_mode']) #there will be no info in the registered of statemachine # we should make the target be a fit-fixed distance with the robot # we make the rules: # the target should be 1m with the base_link # the arm should be 0.2m away from the target # all the state which using the moveit should be 'aborted' if failed # we should notice that, sometimes we need to give the target we get to person, here we donnot need to # avoid the obstacle with self.sm_ArmStack: StateMachine.add('EASY', ChangeMode(), remapping={'arm_mode': "arm_mode"}, transitions={ 'succeeded': 'CREAT_MAP', 'aborted': 'ARM_DIRECT', 'preempted': 'ARM_INIT', 'error': 'error' }) StateMachine.add('CREAT_MAP', CreatMap(), transitions={ 'succeeded': 'INIT_POSE', 'aborted': 'aborted' }) self.sm_ArmStack.userdata.arm_mode_1 = 0 StateMachine.add('INIT_POSE', PreArm(), remapping={ 'arm_ps': 'arm_point', 'arm_mode': 'arm_mode_1' }, transitions={ 'succeeded': 'ARM_MOVEIT', 'aborted': 'ARM_INIT', 'error': 'error' }) StateMachine.add('ARM_MOVEIT', PreArm(), remapping={ 'arm_ps': 'arm_point', 'arm_mode': 'arm_mode' }, transitions={ 'succeeded': 'ARM_DIRECT', 'aborted': 'ARM_INIT', 'error': 'error' }) StateMachine.add('ARM_DIRECT', ArmDirect(), transitions={ 'succeeded': 'OBSTACLE_DEAL', 'aborted': 'aborted', 'error': 'error' }, remapping={ 'gripper_mode': 'arm_mode', 'arm_ps': 'arm_point' }) StateMachine.add('OBSTACLE_DEAL', ObstacleDeal(), remapping={'gripper_mode': 'arm_mode'}, transitions={ 'succeeded': 'WAIT', 'aborted': 'WAIT', 'error': 'error' }) self.sm_ArmStack.userdata.rec = 2.0 StateMachine.add('WAIT', Wait(), remapping={'rec': 'rec'}, transitions={ 'succeeded': 'ARM_INIT', 'error': 'error' }) StateMachine.add('ARM_INIT', PreArm(), remapping={ 'arm_ps': 'arm_point', 'arm_mode': 'arm_mode_1' }, transitions={ 'succeeded': 'succeeded', 'aborted': 'aborted', 'error': 'error' }) rospy.spin()
def execute(self, userdata): global middle_joints, grasp_joints try: getattr(userdata, 'gripper_mode') getattr(userdata, 'arm_ps') #getattr(userdata,'objmode') except: rospy.logerr('No params specified') return 'error' if userdata.gripper_mode == 0: rospy.logerr('No need to move') return 'aborted' # print userdata.arm_ps service_bool = self.ik_client.wait_for_service(timeout=10) if service_bool == False: rospy.logerr('time out, ik-service is no avaiable') return 'aborted' else: # this is the first motion rospy.logwarn('first step') ik_req = xm_SolveIKRequest() ik_req.goal = deepcopy(userdata.arm_ps) ik_res = self.ik_client.call(ik_req) ik_bool = True self.ik_client.close() if ik_res.result == True: solution_list = list(ik_res.solution) lifting_value = solution_list[0] joint_value = solution_list[1:] arm_controller_level(joint_value) # move the lifting lifting_controller_level(lifting_value) rospy.sleep(1.0) else: ik_bool = False rospy.logwarn('ik error') # this is the second motion, will open or close the gripper # true open false close rospy.logwarn('second step') if userdata.gripper_mode == 0: rospy.logerr('you may be joking') return 'aborted' elif userdata.gripper_mode == 1: #if userdata.objmode == 0: gripper_mode = 0 #elif userdata.objmode == 1: # gripper_mode = 1 elif userdata.gripper_mode == 2 or userdata.gripper_mode == 3: gripper_mode = 1 else: rospy.logerr('params error') return 'aborted' gripper_service_level(gripper_mode) rospy.sleep(1.0) if ik_bool == True: rospy.logwarn('third step') joint_value[2] += 0.3 joint_value[3] -= 0.2 arm_controller_level(joint_value) # move the lifting lifting_controller_level(lifting_value) rospy.sleep(1.0) else: pass rospy.logwarn('fourth step') lifting_value = grasp_joints[0] joint_value = grasp_joints[1:] arm_controller_level(joint_value) # move the lifting_link lifting_controller_level(lifting_value) rospy.sleep(1.0) # this is the fourth motion, will make the arm back rospy.logwarn('fifth step') lifting_value = middle_joints[0] joint_value = middle_joints[1:] arm_controller_level(joint_value) # move the lifting lifting_controller_level(lifting_value) rospy.sleep(1.0) return 'succeeded'
def execute(self, userdata): try: getattr(userdata, 'arm_ps') getattr(userdata, 'arm_mode') except: rospy.logerr('No params specified!') return 'error' if userdata.arm_mode is 0: xm_arm_moveit_name('nav_pose') return 'succeeded' else: pass gripper_service_level(True) xm_arm_moveit_name('nav_pose') base_ps = self.tf_listener.transformPoint('base_link', userdata.arm_ps) solution_list = list() self.haha = 0 solution_list.append(base_ps.point.z - 0.92) if solution_list[0] > 0.10: self.haha = 159 rospy.logerr('too high!!!') xm_arm_moveit_name('high') solution_list[0] = 0.08 solution_list.append(math.atan2(base_ps.point.y, base_ps.point.x)) solution_list.append(math.asin((base_ps.point.z - 1.0) / 0.25)) distance = math.sqrt(base_ps.point.y * base_ps.point.y + base_ps.point.x * base_ps.point.x) distance = distance - 0.1 - math.sqrt(0.25 * 0.25 - (base_ps.point.z - 1.0) * (base_ps.point.z - 1.0)) tempvar = math.acos( (distance * distance + 0.15 * 0.15 - 0.19 * 0.19) / 2 / distance / 0.19) solution_list.append(tempvar - solution_list[2]) solution_list.append((-1) * tempvar - math.acos( (distance * distance + 0.19 * 0.19 - 0.15 * 0.15) / 2 / distance / 0.19)) solution_list.append(0.0) else: solution_list.append(math.atan2(base_ps.point.y, base_ps.point.x)) distance = math.sqrt(base_ps.point.y * base_ps.point.y + base_ps.point.x * base_ps.point.x) distance -= 0.3 solution_list.append((-1) * math.acos( (distance * distance + 0.28 * 0.28 - 0.15 * 0.15) / 2 / distance / 0.28)) solution_list.append((-1) * solution_list[2] + math.acos( (distance * distance + 0.15 * 0.15 - 0.28 * 0.28) / 2 / distance / 0.15)) solution_list.append((-1) * math.acos( (distance * distance + 0.15 * 0.15 - 0.28 * 0.28) / 2 / distance / 0.15)) solution_list.append(0.0) lifting_value = solution_list[0] joint_value = solution_list[1:6] lifting_controller_level(lifting_value) joint_value_turn = solution_list[1:6] joint_value_turn[1] = -1.56999 joint_value_turn[2] = 1.61619 joint_value_turn[3] = 1.5492333 joint_value_turn[4] = 0.0 arm_controller_level(joint_value_turn) rospy.sleep(1) arm_controller_level(joint_value) gripper_service_level(False) #0.000261899782345 -1.56999063492 1.6161891222 1.54923319817 0.0 joint_value[1] = -1 joint_value[2] = 1.7 joint_value[3] = 0.0 arm_controller_level(joint_value) rospy.sleep(3) #if self.haha == 159: # xm_arm_moveit_name('after_high') #xm_arm_moveit_name('grasp_pose') joint_value_turn[1] = -0.3968 joint_value_turn[2] = 0.9462 joint_value_turn[3] = 1.4022 joint_value_turn[4] = 0.0 arm_controller_level(joint_value_turn) rospy.sleep(3) xm_arm_moveit_name('nav_pose') #抓完之后的位置 return 'succeeded'
import subprocess from copy import deepcopy def xm_arm_moveit_wave(pose_name): try: moveit_commander.roscpp_initialize(sys.argv) xm_arm_ = moveit_commander.MoveGroupCommander('xm_arm') xm_arm_.set_goal_joint_tolerance(0.01) xm_arm_.allow_replanning(True) xm_arm_.set_planner_id('SBLkConfigDefault') xm_arm_.set_start_state_to_current_state() xm_arm_.set_named_target(pose_name) xm_arm_.go() except: return False rospy.sleep(1.0) else: return True if __name__ == "__main__": try: while True: # xm_arm_moveit_name('wave_pose1') gripper_service_level(True) # xm_arm_moveit_name('wave_pose2') gripper_service_level(False) except KeyboardInterrupt: pass
def execute(self, userdata): try: getattr(userdata, 'arm_ps') getattr(userdata, 'arm_mode') except: rospy.logerr('No params specified!') return 'error' if userdata.arm_mode is 0: xm_arm_moveit_name('nav_pose') return 'succeeded' else: pass if userdata.arm_mode is 1: gripper_service_level(True) xm_arm_moveit_name('nav_pose') base_ps = self.tf_listener.transformPoint('base_link', userdata.arm_ps) solution_list = list() self.haha = 0 solution_list.append(base_ps.point.z - 0.92) if solution_list[0] > 0.08: self.haha = 159 rospy.logerr('too high!!!') xm_arm_moveit_name('high') solution_list[0] = 0.08 solution_list.append(math.atan2(base_ps.point.y, base_ps.point.x)) solution_list.append(math.asin((base_ps.point.z - 1.0) / 0.25)) distance = math.sqrt(base_ps.point.y * base_ps.point.y + base_ps.point.x * base_ps.point.x) distance = distance - 0.1 - math.sqrt(0.25 * 0.25 - (base_ps.point.z - 1.0) * (base_ps.point.z - 1.0)) tempvar = math.acos( (distance * distance + 0.15 * 0.15 - 0.19 * 0.19) / 2 / distance / 0.19) solution_list.append(tempvar - solution_list[2]) solution_list.append((-1) * tempvar - math.acos( (distance * distance + 0.19 * 0.19 - 0.15 * 0.15) / 2 / distance / 0.19)) solution_list.append(0.0) elif solution_list[0] < -0.17: rospy.logerr('too low!!!') solution_list[0] = -0.17 return 'error' else: solution_list.append(math.atan2(base_ps.point.y, base_ps.point.x)) distance = math.sqrt(base_ps.point.y * base_ps.point.y + base_ps.point.x * base_ps.point.x) distance -= 0.33 solution_list.append((-1) * math.acos( (distance * distance + 0.28 * 0.28 - 0.15 * 0.15) / 2 / distance / 0.28)) solution_list.append((-1) * solution_list[2] + math.acos( (distance * distance + 0.15 * 0.15 - 0.28 * 0.28) / 2 / distance / 0.15)) solution_list.append((-1) * math.acos( (distance * distance + 0.15 * 0.15 - 0.28 * 0.28) / 2 / distance / 0.15)) solution_list.append(0.0) lifting_value = solution_list[0] joint_value = solution_list[1:6] lifting_controller_level(lifting_value) arm_controller_level(joint_value) gripper_service_level(False) if self.haha == 159: xm_arm_moveit_name('after_high') xm_arm_moveit_name('grasp_pose') xm_arm_moveit_name('nav_pose2') return 'succeeded'
def execute(self, userdata): try: getattr(userdata, 'arm_ps') getattr(userdata, 'arm_mode') getattr(userdata, 'fgm') except: rospy.logerr('No params specified!') return 'error' if userdata.arm_mode is 0: #xm_arm_moveit_name('nav_pose') return 'succeeded' else: pass gripper_service_level(True) base_ps = self.tf_listener.transformPoint('base_link', userdata.arm_ps) solution_list = list() solution_list1 = list() self.haha = 0 rospy.logerr(userdata.fgm) if userdata.fgm == 2: xm_arm_moveit_name('nav_p') lifting_value = 0.02 solution_list1.append(0.02) solution_list1.append(0.0) solution_list1.append(-0.3796) solution_list1.append(1.4308) solution_list1.append(0.6768) solution_list1.append(0.0) lifting_controller_level(lifting_value) joint_value = solution_list1[1:6] arm_controller_level(joint_value) rospy.sleep(3) solution_list.append(base_ps.point.z - 0.66) solution_list.append(math.atan2(base_ps.point.y, 0.85)) distance = math.sqrt(base_ps.point.y * base_ps.point.y + base_ps.point.x * base_ps.point.x) if base_ps.point.x >= 1.05: distance -= 0.70 elif base_ps.point.x >= 0.99: distance -= 0.63 elif base_ps.point.x >= 0.93: distance -= 0.58 else: distance -= 0.53 solution_list.append((-1) * math.acos( (distance * distance + 0.28 * 0.28 - 0.15 * 0.15) / 2 / distance / 0.28)) #solution_list.append((-1)*solution_list[2]+math.acos((distance*distance + 0.15*0.15 - 0.28*0.28)/2/distance/0.15)) solution_list.append(3.14 - math.acos(( (-1) * distance * distance + 0.15 * 0.15 + 0.28 * 0.28) / 2 / 0.28 / 0.15)) solution_list.append((-1) * math.acos( (distance * distance + 0.15 * 0.15 - 0.28 * 0.28) / 2 / distance / 0.15)) solution_list.append(0.0) lifting_value = solution_list[0] joint_value = solution_list[1:6] lifting_controller_level(lifting_value) joint_value_turn = solution_list[1:6] joint_value_turn[1] = -1.56999 joint_value_turn[2] = 1.61619 joint_value_turn[3] = 1.5492333 joint_value_turn[4] = 0.0 #arm_controller_level(joint_value_turn) #rospy.sleep(2) arm_controller_level(joint_value) rospy.sleep(10) gripper_service_level(False) #0.000261899782345 -1.56999063492 1.6161891222 1.54923319817 0.0 lifting_value = solution_list[0] + 0.1 lifting_controller_level(lifting_value) rospy.sleep(2) joint_value[1] = -1 joint_value[2] = 1.4 joint_value[3] = 1 arm_controller_level(joint_value) rospy.sleep(3) joint_value_turn[1] = -0.3968 joint_value_turn[2] = 0.9462 joint_value_turn[3] = 1.4022 joint_value_turn[4] = 0.0 #arm_controller_level(joint_value_turn) rospy.sleep(3) lifting_controller_level(-0.0568) joints3 = [0, -1.4700, 1.3692, 1.5472, 0] arm_controller_level(joints3) xm_arm_moveit_name('nav_pose') #抓完之后的位置 return 'succeeded'