Skip to content
Anima Mundi Anima Mundi

实时 Servo

MoveIt Servo 可实现对机械臂的实时控制。

MoveIt Servo 接受以下任意类型的命令：

  1. 单个关节速度。
  2. 末端执行器的期望速度。
  3. 末端执行器的期望位姿。

这使得可以通过多种输入方案进行遥操作，或由其他自主软件控制机器人——例如用于视觉伺服（visual servoing）或闭环位置控制。

入门

Section titled “入门”

如果尚未完成，请先完成 Getting Started 中的步骤。

设计概述

Section titled “设计概述”

MoveIt Servo 由两个主要部分组成：提供 C++ 接口的核心实现 Servo，以及封装该 C++ 接口并提供 ROS 接口的 ServoNode。Servo 的配置通过 servo_parameters.yaml 中定义的 ROS 参数来完成。

除了伺服功能之外，MoveIt Servo 还提供了一些便捷特性，例如：

  • 奇异点检测
  • 碰撞检测
  • 运动平滑
  • 关节位置和速度限制的强制执行

奇异点检测和碰撞检测是安全特性：当接近奇异点或发生碰撞（自碰撞或与其他物体碰撞）时，会按比例降低速度。碰撞检测和平滑是可选特性，可分别通过 check_collisions 参数和 use_smoothing 参数禁用。

逆运动学通过逆雅可比计算来处理；如果提供了机器人的 IK 求解器，则由该求解器处理。

Servo 中的逆运动学

Section titled “Servo 中的逆运动学”

逆运动学可以由 MoveIt Servo 内部通过逆雅可比计算来处理。不过，你也可以使用 IK 插件。要为 MoveIt Servo 配置 IK 插件，你的机器人配置包必须在 kinematics.yaml 文件中定义一个插件，例如 Panda 配置包中的写法。

多个 IK 插件可供选择，例如 MoveIt 内置的插件，以及外部提供的插件。bio_ik/BioIKKinematicsPlugin 是最常见的选择。

一旦你的 kinematics.yaml 文件配置好，就在启动文件中将它连同其他传给 Servo 节点的 ROS 参数一起包含进来：

moveit_config = (
    MoveItConfigsBuilder("moveit_resources_panda")
    .robot_description(file_path="config/panda.urdf.xacro")
    .to_moveit_configs()
)
servo_node = Node(
    package="moveit_servo",
    executable="servo_node",
    parameters=[
        servo_params,
        low_pass_filter_coeff,
        moveit_config.robot_description,
        moveit_config.robot_description_semantic,
        moveit_config.robot_description_kinematics, # here is where kinematics plugin parameters are passed
    ],
)

以上摘录取自 MoveIt 中的 servo_example.launch.py。在上面的示例中，kinematics.yaml 文件取自工作空间中的 moveit_resources 仓库，具体路径为 moveit_resources/panda_moveit_config/config/kinematics.yaml。通过加载该 yaml 文件，实际传递的 ROS 参数名的格式为 robot_description_kinematics.<group_name>.<param_name>，例如 robot_description_kinematics.panda_arm.kinematics_solver。

由于 moveit_servo 不允许在 Servo 节点上设置 kinematics.yaml 文件中未声明的参数，因此自定义求解器参数需要在你的插件代码内部声明。

例如，bio_ik 在 bio_ik/src/kinematics_plugin.cpp 中定义了一个 getROSParam() 函数，如果在 Servo 节点上找不到参数，该函数会自动声明这些参数。

线程优先级

Section titled “线程优先级”

为了在控制硬件时获得最佳性能，你希望主 Servo 循环的抖动尽可能少。普通的 Linux 内核针对计算吞吐量进行了优化，因此不太适合硬件控制。两个最简单的内核选项是 Real-time Ubuntu 22.04 LTS Beta 或 Debian Bullseye 上的 linux-image-rt-amd64。

如果你安装了实时内核，ServoNode 的主线程会自动尝试配置优先级为 40 的 SCHED_FIFO。更多文档请参见 config/servo_parameters.yaml。

在新机器人上进行配置

Section titled “在新机器人上进行配置”

在你的机器人上运行 MoveIt Servo 的最低要求包括：

  1. 有效的机器人 URDF 和 SRDF。
  2. 能够接受关节位置或速度命令的控制器。
  3. 能够提供快速、准确的关节位置反馈的关节编码器。

由于运动学由 MoveIt 的核心部分处理，建议你为机器人准备一个有效的配置包，并且能够运行其中附带的演示启动文件。

使用 C++ API

Section titled “使用 C++ API”

当存在性能要求、需要避免 ROS 通信基础设施的开销，或者 Servo 生成的输出需要送入某个没有 ROS 接口的其他控制器时，使用 C++ 接口会很有用。

通过 C++ 接口使用 MoveIt Servo 时，三种输入命令类型分别是 JointJogCommand、TwistCommand 和 PoseCommand。使用 C++ 接口时，Servo 的输出是 KinematicState——一个包含关节名称、位置、速度和加速度的结构体，其定义见 datatypes 头文件。

第一步是创建一个 Servo 实例。

// Import the Servo headers.
#include <moveit_servo/servo.hpp>
#include <moveit_servo/utils/common.hpp>


// The node to be used by Servo.
rclcpp::Node::SharedPtr node = std::make_shared<rclcpp::Node>("servo_tutorial");


// Get the Servo parameters.
const std::string param_namespace = "moveit_servo";
const std::shared_ptr<const servo::ParamListener> servo_param_listener =
    std::make_shared<const servo::ParamListener>(node, param_namespace);
const servo::Params servo_params = servo_param_listener->get_params();


// Create the planning scene monitor.
const planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor =
    createPlanningSceneMonitor(node, servo_params);


// Create a Servo instance.
Servo servo = Servo(node, servo_param_listener, planning_scene_monitor);

使用 JointJogCommand

using namespace moveit_servo;


// Create the command.
JointJogCommand command;
command.joint_names = {"panda_link7"};
command.velocities = {0.1};


// Set JointJogCommand as the input type.
servo.setCommandType(CommandType::JOINT_JOG);


// Get the joint states required to follow the command.
// This is generally run in a loop.
KinematicState next_joint_state = servo.getNextJointState(command);

使用 TwistCommand

using namespace moveit_servo;


// Create the command.
TwistCommand command{"panda_link0", {0.1, 0.0, 0.0, 0.0, 0.0, 0.0};


// Set the command type.
servo.setCommandType(CommandType::TWIST);


// Get the joint states required to follow the command.
// This is generally run in a loop.
KinematicState next_joint_state = servo.getNextJointState(command);

使用 PoseCommand

using namespace moveit_servo;


// Create the command.
Eigen::Isometry3d ee_pose = Eigen::Isometry3d::Identity(); // This is a dummy pose.
PoseCommand command{"panda_link0", ee_pose};


// Set the command type.
servo.setCommandType(CommandType::POSE);


// Get the joint states required to follow the command.
// This is generally run in a loop.
KinematicState next_joint_state = servo.getNextJointState(command);

然后，next_joint_state 结果可用于控制流程中的后续步骤。

可以通过以下方式获取 MoveIt Servo 在最后一条命令之后的状态：

StatusCode status = servo.getStatus();

用户可以利用该状态进行更高层的决策。

有关使用 C++ 接口的完整示例，请参见 moveit_servo/demos。可以使用 moveit_servo/launch 中的启动文件来运行这些演示。

Terminal window
ros2 launch moveit_servo demo_joint_jog.launch.py
ros2 launch moveit_servo demo_twist.launch.py
ros2 launch moveit_servo demo_pose.launch.py

使用 ROS API

Section titled “使用 ROS API”

要通过 ROS 接口使用 MoveIt Servo，必须将其作为 Node 或 Component 连同所需参数一起启动，如此处所示。

通过 ROS 接口使用 MoveIt Servo 时，命令是以下类型的 ROS 消息，发布到由 Servo 参数指定的相应话题上：

  1. control_msgs::msg::JointJog，发布到由 joint_command_in_topic 参数指定的话题。
  2. geometry_msgs::msg::TwistStamped，发布到由 cartesian_command_in_topic 参数指定的话题。目前，twist 消息必须位于机器人的规划坐标系中。（此限制后续会更新。）
  3. geometry_msgs::msg::PoseStamped，发布到由 pose_command_in_topic 参数指定的话题。

Twist 和 Pose 命令要求始终指定 header.frame_id。ServoNode（ROS 接口）的输出可以是 trajectory_msgs::msg::JointTrajectory 或 std_msgs::msg::Float64MultiArray，通过 command_out_type 参数选择，并发布到由 command_out_topic 参数指定的话题上。

可以使用 ServoCommandType 服务来选择命令类型，参见 ServoCommandType 的定义。

从命令行（CLI）操作：

Terminal window
ros2 service call /<node_name>/switch_command_type moveit_msgs/srv/ServoCommandType "{command_type: 1}"

以编程方式：

switch_input_client = node->create_client<moveit_msgs::srv::ServoCommandType>("/<node_name>/switch_command_type");
auto request = std::make_shared<moveit_msgs::srv::ServoCommandType::Request>();
request->command_type = moveit_msgs::srv::ServoCommandType::Request::TWIST;
if (switch_input_client->wait_for_service(std::chrono::seconds(1)))
{
  auto result = switch_input_client->async_send_request(request);
  if (result.get()->success)
  {
    RCLCPP_INFO_STREAM(node->get_logger(), "Switched to input type: Twist");
  }
  else
  {
    RCLCPP_WARN_STREAM(node->get_logger(), "Could not switch input to: Twist");
  }
}

类似地，可以使用暂停服务 <node_name>/pause_servo（类型为 std_msgs::srv::SetBool）暂停伺服。

使用 ROS 接口时，Servo 的状态可在话题 /<node_name>/status 上获取，参见 ServoStatus 的定义。

启动 ROS 接口演示：

Terminal window
ros2 launch moveit_servo demo_ros_api.launch.py

演示运行后，可以通过键盘对机器人进行遥操作。

启动键盘演示：

Terminal window
ros2 run moveit_servo servo_keyboard_input

有关在开门场景中使用 pose 命令进行伺服的示例，请参见此示例。

Terminal window
ros2 launch moveit2_tutorials pose_tracking_tutorial.launch.py

信号平滑

Section titled “信号平滑”

查看 servo_parameters.yaml，注意 smoothing_filter_plugin_name 参数。它有助于平滑发送给机器人的关节命令中的不规则性，例如命令周期不一致的情况。这可以大大减少机器人执行机构的磨损——事实上，许多机器人只有在收到的命令足够平滑时才会移动。这些都是插件，因此任何用户都可以编写自己的插件。

当前可用的选项有：

online_signal_smoothing::ButterworthFilterPlugin：一个非常简单的低通滤波器，比移动平均略微高级一些。

优点：计算效率高，在关节空间中永远不会超出命令值。

缺点：在笛卡尔空间中可能会略微偏离“直线运动”。不会显式限制执行机构的加加速度或加速度。

online_signal_smoothing::AccelerationLimitedPlugin：一种基于优化的算法，在可行的情况下遵循机器人的加速度限制。更多信息请阅读 PR #2651。

优点：只要运动学上可行，就会保持期望的运动方向，对于“急转弯”很有用，并确保不会违反机器人关节的加速度限制。

缺点：不会显式限制执行机构的加加速度。如果传入的命令不可行，仍可能偏离预期的运动方向。可能会超调。

online_signal_smoothing::RuckigFilterPlugin：使用著名的 Ruckig 库 来确保机器人运动始终遵守关节和加速度限制。更多信息请阅读 PR #2956。

优点：最平滑的选项。某些工业机器人需要它。

缺点：有时会偏离预期的运动方向。例如，在急转弯处往往会形成旋转运动。要防止旋转运动，需要对传入命令（在 MoveIt Servo 之外）进行额外的逻辑处理。可能会超调。