def left_arm_go_to_joint_goal(self, jointnum):
# 设置动作对象变量,此处为right_arm
left_arm = self.left_arm
# 获取当前末端执行器位置姿态
left_joint_goal = left_arm.get_current_joint_values()
# 设置末端关节目标值
left_joint_goal[0] = return_joint_state(jointnum)[0]
left_joint_goal[1] = return_joint_state(jointnum)[1]
left_joint_goal[2] = return_joint_state(jointnum)[2]
left_joint_goal[3] = return_joint_state(jointnum)[3]
left_joint_goal[4] = return_joint_state(jointnum)[4]
left_joint_goal[5] = return_joint_state(jointnum)[5]
left_joint_goal[6] = return_joint_state(jointnum)[6]
print "End effector joint goal %s" % left_joint_goal
# 规划并执行路径动作
left_arm.go(left_joint_goal, wait=False)
left_arm.stop()
def right_arm_go_to_joint_goal(self, jointnum):
# 设置动作对象变量,此处为right_arm
right_arm = self.right_arm
# 获取当前末端执行器位置姿态
right_joint_goal = right_arm.get_current_joint_values()
# 设置末端关节目标值
right_joint_goal[0] = return_joint_state(jointnum)[0]
right_joint_goal[1] = return_joint_state(jointnum)[1]
right_joint_goal[2] = return_joint_state(jointnum)[2]
right_joint_goal[3] = return_joint_state(jointnum)[3]
right_joint_goal[4] = return_joint_state(jointnum)[4]
right_joint_goal[5] = return_joint_state(jointnum)[5]
right_joint_goal[6] = return_joint_state(jointnum)[6]
print "End effector joint goal %s" % right_joint_goal
# 规划并执行路径动作
right_arm.go(right_joint_goal, wait=False)
right_arm.stop()
