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()