Jupyter Notebook 原型开发

本教程介绍如何将 Jupyter Notebook 与 MoveIt 2 Python API 结合使用。教程分为以下几个部分：

• 快速入门： 教程环境搭建要求概述。
• 理解 launch 文件： launch 文件结构说明。
• Notebook 配置： notebook 的导入与配置。
• 运动规划示例： 使用 moveit_py API 规划运动的示例。
• 遥操作示例： 使用 moveit_py API 通过游戏手柄远程操作机器人的示例。

本教程的代码可以在这里找到。

快速入门

要完成本教程，你需要先搭建一个包含 MoveIt 2 及其对应教程的 colcon 工作空间。Getting Started Guide 提供了关于如何搭建此类工作空间的详尽说明。

工作空间搭建完成后，运行以下命令来启动本教程的代码（本教程的 servo 部分需要 PS4 DualShock 手柄，如果没有手柄，请将该参数设置为 false）：

ros2 launch moveit2_tutorials jupyter_notebook_prototyping.launch.py start_servo:=true

• 这将启动完成本教程所需的各个节点。
• 同时，它还会启动一个 Jupyter Notebook 服务器，你可以连接到该服务器来运行教程代码。
• 如果浏览器没有自动打开 Jupyter Notebook 界面，也可以直接访问 http://localhost:8888 进行连接。
• 连接时需要输入 token，该 token 会在启动 launch 文件时打印到终端中。
• 终端输出中还会打印一个包含 token 的 URL，直接使用该 URL 即可连接，无需手动输入 token。

完成上述步骤后，就可以继续学习了。在执行 notebook 代码之前，下文先简要介绍 launch 文件的内容，帮助你理解 notebook 实例是如何启动的。

理解 launch 文件

本教程使用的 launch 文件与其他教程的主要区别在于：它额外启动了一个 Jupyter Notebook 服务器。下面简要回顾常见的 launch 文件代码，重点介绍 notebook 服务器的启动部分。

导入所需的包：

import os
import yaml
from launch import LaunchDescription
from launch.actions import ExecuteProcess, DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node, SetParameter
from ament_index_python.packages import get_package_share_directory
from moveit_configs_utils import MoveItConfigsBuilder

定义一个用于加载 yaml 文件的工具函数：

def load_yaml(package_name, file_path):
        package_path = get_package_share_directory(package_name)
        absolute_file_path = os.path.join(package_path, file_path)

        try:
                with open(absolute_file_path, 'r') as file:
                return yaml.safe_load(file)
        except EnvironmentError:  # parent of IOError, OSError *and* WindowsError where available
                return None

定义一个用于启动 servo 节点的 launch 参数：

start_servo = LaunchConfiguration('start_servo')

start_servo_arg = DeclareLaunchArgument(
        'start_servo',
        default_value='false',
        description='Start the servo node.')

定义 MoveIt 配置，这一步在之后配置 notebook 时也会用到：

moveit_config = (
        MoveItConfigsBuilder(
                robot_name="panda", package_name="moveit_resources_panda_moveit_config"
        )
        .robot_description(file_path="config/panda.urdf.xacro")
        .trajectory_execution(file_path="config/gripper_moveit_controllers.yaml")
        .moveit_cpp(
                file_path=os.path.join(
                        get_package_share_directory("moveit2_tutorials"),
                        "config",
                        "jupyter_notebook_prototyping.yaml"
                )
        )
        .to_moveit_configs()
)

定义好 MoveIt 配置后，启动以下节点：

• rviz_node： 启动 rviz2 用于可视化。
• static_tf： 发布 world 坐标系与 panda 基座坐标系之间的静态变换。
• robot_state_publisher： 发布更新后的机器人状态信息（变换）。
• ros2_control_node： 用于控制关节组。

rviz_config_file = os.path.join(
        get_package_share_directory("moveit2_tutorials"),
        "config", "jupyter_notebook_prototyping.rviz",
)
rviz_node = Node(
        package="rviz2",
        executable="rviz2",
        output="log",
        arguments=["-d", rviz_config_file],
        parameters=[
                moveit_config.robot_description,
                moveit_config.robot_description_semantic,
        ],
)

static_tf = Node(
        package="tf2_ros",
        executable="static_transform_publisher",
        name="static_transform_publisher",
        output="log",
        arguments=["--frame-id", "world", "--child-frame-id", "panda_link0"],
)

robot_state_publisher = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        name="robot_state_publisher",
        output="both",
        parameters=[moveit_config.robot_description],
)

ros2_controllers_path = os.path.join(
        get_package_share_directory("moveit_resources_panda_moveit_config"),
        "config",
        "ros2_controllers.yaml",
)
ros2_control_node = Node(
        package="controller_manager",
        executable="ros2_control_node",
        parameters=[ros2_controllers_path],
        remappings=[
                ("/controller_manager/robot_description", "/robot_description"),
        ],
        output="both",
)

load_controllers = []
for controller in [
        "panda_arm_controller",
        "panda_hand_controller",
        "joint_state_broadcaster",
]:
        load_controllers += [
                ExecuteProcess(
                cmd=["ros2 run controller_manager spawner {}".format(controller)],
                shell=True,
                output="screen",)
                ]

定义好上述节点后，还需要一个进程来启动 Jupyter Notebook 服务器：

notebook_dir = os.path.join(get_package_share_directory("moveit2_tutorials"), "src")
start_notebook = ExecuteProcess(
        cmd=["cd {} && python3 -m notebook".format(notebook_dir)],
        shell=True,
        output="screen",
)

如果需要启动 servo，还要额外定义 joy 节点和 servo 节点。最后，返回 LaunchDescription：

if start_servo:
        servo_yaml = load_yaml("moveit_servo", "config/panda_simulated_config.yaml")
        servo_params = {"moveit_servo": servo_yaml}

        joy_node = Node(
                package="joy",
                executable="joy_node",
                name="joy_node",
                output="screen",
        )

        servo_node = Node(
                package="moveit_servo",
                executable="servo_node_main",
                parameters=[
                        servo_params,
                        moveit_config.robot_description,
                        moveit_config.robot_description_semantic,
                        moveit_config.robot_description_kinematics,
                ],
                output="screen",
        )

        return LaunchDescription(
                [
                start_servo_arg,
                start_notebook,
                static_tf,
                robot_state_publisher,
                rviz_node,
                ros2_control_node,
                joy_node,
                servo_node,
        ]
        + load_controllers
)

如果不启动 servo，返回的 LaunchDescription 将包含上述所有其他节点和进程：

return LaunchDescription(
        [
        start_servo_arg,
        static_tf,
        robot_state_publisher,
        rviz_node,
        ros2_control_node,
        start_notebook,
        ]
        + load_controllers
)

Notebook 配置

Jupyter Notebook 服务器启动后，就可以开始执行 notebook 代码了。首先导入所需的包：

import os
import sys
import yaml
import rclpy
import numpy as np

# message libraries
from geometry_msgs.msg import PoseStamped, Pose

# moveit_py
from moveit.planning import MoveItPy
from moveit.core.robot_state import RobotState

# config file libraries
from moveit_configs_utils import MoveItConfigsBuilder
from ament_index_python.packages import get_package_share_directory

导入包之后，需要定义 moveit_py 节点的配置，通过 MoveItConfigsBuilder 完成：

moveit_config = (
        MoveItConfigsBuilder(robot_name="panda", package_name="moveit_resources_panda_moveit_config")
        .robot_description(file_path="config/panda.urdf.xacro")
        .trajectory_execution(file_path="config/gripper_moveit_controllers.yaml")
        .moveit_cpp(
                file_path=os.path.join(
                        get_package_share_directory("moveit2_tutorials"),
                        "config",
                        "jupyter_notebook_prototyping.yaml",
                )
        )
        .to_moveit_configs()
).to_dict()

这里我们将配置实例转换为字典，用于初始化 moveit_py 节点。接下来初始化 moveit_py 节点：

# initialise rclpy (only for logging purposes)
rclpy.init()

# instantiate moveit_py instance and a planning component for the panda_arm
panda = MoveItPy(node_name="moveit_py", config_dict=moveit_config)
panda_arm = panda.get_planning_component("panda_arm")

运动规划示例

首先创建一个辅助函数，稍后规划和执行轨迹时会用到它：

def plan_and_execute(
        robot,
        planning_component,
        single_plan_parameters=None,
        multi_plan_parameters=None,
):
        """A helper function to plan and execute a motion."""
        # plan to goal
        if multi_plan_parameters is not None:
                plan_result = planning_component.plan(
                        multi_plan_parameters=multi_plan_parameters
                )
        elif single_plan_parameters is not None:
                plan_result = planning_component.plan(
                single_plan_parameters=single_plan_parameters
        )
        else:
                plan_result = planning_component.plan()

        # execute the plan
        if plan_result:
                robot_trajectory = plan_result.trajectory
                robot.execute(robot_trajectory, controllers=[])
        else:
                print("Planning failed")

下面在 notebook 中演示一个简单的运动规划与执行：

# set plan start state using predefined state
panda_arm.set_start_state("ready")

# set pose goal using predefined state
panda_arm.set_goal_state(configuration_name = "extended")

# plan to goal
plan_and_execute(panda, panda_arm)

借助 notebook，我们可以交互式地进行运动规划（有关运动规划 API 的更多细节，请参阅运动规划教程）。假设开发代码时犯了如下错误：

# set plan start state using predefined state
panda_arm.set_start_state("ready") # This conflicts with the current robot configuration and will cause an error

# set goal using a pose message this time
pose_goal = PoseStamped()
pose_goal.header.frame_id = "panda_link0"
pose_goal.pose.orientation.w = 1.0
pose_goal.pose.position.x = 0.28
pose_goal.pose.position.y = -0.2
pose_goal.pose.position.z = 0.5
panda_arm.set_goal_state(pose_stamped_msg = pose_goal, pose_link = "panda_link8")

# plan to goal
plan_and_execute(panda, panda_arm)

由于使用的是 notebook，这个错误很容易修正，无需重新编译任何文件。只需将上面的单元格修改为以下内容，然后重新运行即可：

# set plan start state using predefined state
panda_arm.set_start_state_to_current_state()

# set goal using a pose message this time
pose_goal = PoseStamped()
pose_goal.header.frame_id = "panda_link0"
pose_goal.pose.orientation.w = 1.0
pose_goal.pose.position.x = 0.28
pose_goal.pose.position.y = -0.2
pose_goal.pose.position.z = 0.5
panda_arm.set_goal_state(pose_stamped_msg = pose_goal, pose_link = "panda_link8")

# plan to goal
plan_and_execute(panda, panda_arm)

遥操作示例

你可能还想对机器人进行实时远程操作（teleoperation）。借助 Python API，可以在不关闭和重启所有进程的情况下交互式地启动/停止远程操作。本示例通过一个实际场景来展示如何在 notebook 中实现：远程操作机器人、执行运动规划，然后再次远程操作。

本节需要一台支持通过 moveit_py 进行远程操作的设备，这里使用 PS4 DualShock 手柄。

要开始远程操作机器人，先将 PS4 DualShock 手柄初始化为遥操作设备（teleop device）：

from moveit.servo_client.devices.ps4_dualshock import PS4DualShockTeleop

# instantiate the teleoperating device
ps4 = PS4DualShockTeleop(ee_frame_name="panda_link8")

# start teleloperating the robot
ps4.start_teleop()

如果想执行运动规划，让机器人回到默认配置，只需停止远程操作，然后使用前面介绍的运动规划 API：

# stop teleoperating the robot
ps4.stop_teleop()

# plan and execute
# set plan start state using predefined state
panda_arm.set_start_state_to_current_state()

# set pose goal using predefined state
panda_arm.set_goal_state(configuration_name = "ready")

# plan to goal
plan_and_execute(panda, panda_arm)

这样机器人就会回到默认配置。从这个配置出发，我们可以再次开始远程操作：

ps4.start_teleop()