def execute(self, userdata): # make the preempt_requested if self.preempt_requested(): return 'preempted' else: pass self.obj_pos = userdata.obj_pos if self.obj_pos.header.frame_id is None: rospy.logerr('lack the frame_id of the object_position') return 'aborted' self.ik_client.wait_for_service(timeout=10.0) req = xm_SolveIKRequest() req.goal.header.frame_id = self.obj_pos.header.frame_id req.goal.point = self.obj_pos.point res = self.ik_client.call(req) if res.result == True: solution_list = list(res.solution) # make the gripper foward to the bottle solution_list[2] += 0.3 solution_list[3] -= 0.3 lifting_value = solution_list[0] joint_value = solution_list[1:] arm_controller_level(joint_value) # move the lifting lifting_controller_level(lifting_value) rospy.logdebug('I will go to catch the bottle') solution_list = list(res.solution) joint_value = solution_list[1:] arm_controller_level(joint_value) return 'succeeded' else: return 'aborted'
def execute(self, userdata): global middle_joints, grasp_joints try: getattr(userdata, 'arm_ps') getattr(userdata, 'arm_mode') except: rospy.logerr('No parms specified') return 'error' if userdata.arm_mode is 0: ###################### xm_arm_moveit_name('nav_pose') return 'succeeded' else: pass # print userdata.arm_ps service_bool = self.ik_client.wait_for_service(timeout=10) if service_bool == False: rospy.logerr('time-out, ik server is no avaiable') return 'aborted' else: ik_req = xm_SolveIKRequest() ik_req.goal = deepcopy(userdata.arm_ps) ik_req.goal.point.x -= 0.15 ik_res = self.ik_client.call(ik_req) self.ik_client.close() if ik_res.result == True: solution_list = list(ik_res.solution) #solution_list[1] = 1.40 if solution_list[4] < 2.1: solution_list[4] = 2.0 middle_joints = deepcopy(solution_list) xm_arm_moveit_level(solution_list) rospy.logwarn('moveit task compelete') rospy.sleep(1.0) # JudgeMent the moveit is succeeded? (tran, rot) = self.tf_listener.lookupTransform( 'base_link', 'gripper_link', rospy.Time()) if ( tran[2] - userdata.arm_ps.point.z ) < -0.05: # too low we think is not safe for the following operation rospy.logwarn('moveit may be over, donnot use arm again') return 'aborted' else: pass solution_list = list(ik_res.solution) if solution_list[4] < 2.1: solution_list[4] = 2.0 grasp_joints = deepcopy(solution_list) lifting_value = solution_list[0] joint_value = solution_list[1:] arm_controller_level(joint_value) # move the lifting lifting_controller_level(lifting_value) return 'succeeded' else: return 'aborted'
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 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): 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): joints1 = list() joints1 = [0.0313531421125,-1.53575694561,2.09988284111,1.16043531895,-1.57] lifting_controller_level(-0.0789030939341) arm_controller_level(joints1) #xm_arm_moveit_name('after_grasp_bag') ##############################################3 # rospy.sleep(5) # joints2 =[0.0313406735659,-1.53585672379,2.09977054596,-1.88381099701,-1.57] #lifting_controller_level(-0.0789094045758) # arm_controller_level(joints2) return 'succeeded' return 'error'
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 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'
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'