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'