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'