详细解析并部署 Moveit servo 控制UR10e（ /速度控制/伺服控制 ）（ROS 1+Moveit 1）版本！！

最近在两位老师的建议下，配置了Moveit servo用于从速度层面控制UR10e真机，摆脱了moveit_commander.MoveGroupCommander从位置层面控制响应慢，无法形成闭环的尴尬。Moveit servo配置花了非常多时间，由于我要配置的是ROS 1+Moveit 1环境下的，这个网上中文资料很少，github和Moveit 官方文档也都是UR5或者非常抽象的介绍，因此这里我准备把自己配置的详细过程记录下来，防止遗忘。同时发扬一下开源精神！

包含的章节有：Moveit servo介绍、Moveit servo官方资源、Moveit servo配置过程、Moveit servo自定义控制自己的机械臂、Moveit servo大坑记录

1. Moveit servo介绍

MoveIt! Servo是MoveIt!框架下的一个实时控制插件包，旨在为机器人提供高效且精确的实时控制能力。它专门用于机器人执行基于力/力矩传感器、视觉反馈等输入源的精细运动控制，尤其适用于高精度和动态控制任务，比如跟踪目标、精确接触控制（如恒力控制）等。

1.1 主要特点

1. 实时控制：相比于传统的基于轨迹规划的控制方法，MoveIt! Servo提供更为直接的控制方式。它通过实时的力控制和反馈机制来调整机器人末端执行器的位置和姿态。

2. 基于输入的控制：它可以基于外部传感器（如力传感器或视觉传感器）的输入进行控制，适用于接触控制、障碍物回避、物体抓取等任务。

3. 高频率低延迟控制：它可以在较高的频率下控制机器人运动（通常是100Hz或更高），从而能够适应动态和快速变化的环境。

1.2 应用场景 

1. 恒力跟踪 2. 抓取和放置 3. 动态调整路径

2. Moveit servo官方资源

2.1 Moveit servo-ROS2的文档：https://moveit.picknik.ai/humble/doc/examples/realtime_servo/realtime_servo_tutorial.html

2.2 Moveit servo-Moveit 1- ROS noetic的文档：

Realtime Arm Servoing — moveit_tutorials Noetic documentation

其实这里面有很多，在moveit官方文档中可以选版本的。

有个比较尴尬的事是，似乎Moveit servo在ROS 1+Moveit 1环境下并不受重视，他的github代码仓库是空的！
所以我也不是很敢用moveit2下的代码，害怕不兼容问题，这个也是头疼的地方。

2.3 我这个环境下可以兼容的官方包：

师兄找到的：

moveit/moveit_ros/moveit_servo at master · moveit/moveit (github.com)

我就是用这个配置成功的！

3. Moveit servo配置过程

3.1 安装Moveit servo包

首先是安装Moveit servo包，这个就不说了，直接拉下来，记得编译自己的工作空间就行了。

包的结构大致如下：最重要的是launch文件和yaml文件，其他的根据不同项目会不一样的

moveit_servo/
├── CMakeLists.txt
├── package.xml
├── config/            # 配置文件夹
│   ├── servo.yaml     # 主要的配置文件
│   
├── launch/            # 启动文件夹
│   ├── moveit_servo.launch  # 主要的启动文件
│   ├── moveit_servo_rviz.launch  # RViz可视化相关的启动文件
├── src/
│   ├── servo_server.cpp  # 主要的Servo控制实现文件
└── README.md

.yaml：主要用于配置 MoveIt! Servo 的控制参数，如运动控制的速度、力反馈等。

.launch：启动 MoveIt! Servo 控制服务器的文件。

3.2 .yaml 配置文件解析

下面这个是我自己受搓的配置，都是我的项目里的，用的时候可以按照自己的情况改一改。

use_gazebo: false  # 是否在Gazebo仿真环境中运行机器人
command_in_type: "speed_units"  # 接受速度命令的单位类型
scale:
  linear:  0.6  # 末端执行器最大线速度 [m/s]
  rotational:  0.3  # 末端执行器最大角速度 [rad/s]
  joint: 1.0

low_pass_filter_coeff: 0.05  # 平滑命令数据的滤波器系数

publish_period: 0.01  # 发布周期 10ms
low_latency_mode: true  # 开启低延迟模式，忽略发布周期，尽快发布命令

control_mode: velocity  # 切换到速度控制模式
max_angular_velocity: 0.5  # 可以增加最大角速度
max_linear_velocity: 0.5   # 可以增加最大线速度
command_out_type: std_msgs/Float64MultiArray

# MoveIt 相关设置
move_group_name: "manipulator"  # MoveIt 的组名
planning_frame: "base_link"  # 规划框架，通常是机器人基座链接

# End effector 设置
ee_frame_name: "tool0"  # 末端执行器的 frame 名称
robot_link_command_frame: "base_link"  # 命令给定的 frame，可以是base或end effector

# 停止行为
incoming_command_timeout: 1.  # 超过这个时间没有新命令则停止运动
num_outgoing_halt_msgs_to_publish: 4  # 停止命令的发送次数

# 关节极限与奇异点处理
lower_singularity_threshold: 17  # 奇异点的触发阈值
hard_stop_singularity_threshold: 30  # 强制停止阈值
joint_limit_margin: 0.1  # 关节极限的安全边际（弧度）

# 话题设置
cartesian_command_in_topic: "/tool0/velocity"  # 用于接收末端执行器速度命令的Twist话题
joint_command_in_topic: "delta_joint_cmds"  # 可选，用于接收关节命令的Twist话题
joint_topic: "/joint_states"  # 关节状态话题
status_topic: "/servo_server/status"
command_out_topic: "/joint_group_vel_controller/command"  # MoveIt Servo 发布控制命令给机器人控制器
# 发布命令话题

# 碰撞检测设置
check_collisions: true  # 开启碰撞检测
collision_check_rate: 50  # 碰撞检测频率 [Hz]
collision_check_type: "threshold_distance"  # 碰撞检查方式
self_collision_proximity_threshold: 0.01  # 自身碰撞的检测阈值 [m]
scene_collision_proximity_threshold: 0.05  # 场景碰撞的检测阈值 [m]


# For each joint, specify the acceleration limits
joint_acceleration_limits:
  shoulder_pan_joint: 0.5  # 根据你机器人的硬件限制设置
  shoulder_lift_joint: 0.5
  elbow_joint: 0.5
  wrist_1_joint: 0.5
  wrist_2_joint: 0.5
  wrist_3_joint: 0.5
  ground_plane_box: 0.
  ur_socket_joint: 0.

# 参数：发布关节位置、速度、加速度等
publish_joint_positions: true  # 发布关节位置
publish_joint_velocities: true  # 发布关节速度
publish_joint_accelerations: true  # 发布关节加速度

# 设置碰撞检测参数
collision_distance_safety_factor: 0.1  # 碰撞安全距离系数
min_allowable_collision_distance: 0.05  # 最小可接受的碰撞距离

3.3 .launch文件解析

注意！！！这里我只launch了servo_server这一个节点，用于启动我的Moveit servo，但是，还要提前启动的有：

1. UR10e本身的外部控制（这里会定义不同的控制器！！非常重要，这个下边说，这个是servo发出命令的接收者，也就是下游）

2. 机器人的moveit相关的，包含URDF、碰撞、动力学参数等各种.urdf,.xacro

3. 可视化的比如 rviz

为了方便调试，我单独启动下边的Moveit servo

<launch>

  <!-- 启动 MoveIt Servo（用于速度控制） -->
  <arg name="config" default="$(find moveit_servo)/config/ur_simulated_config.yaml"/>

  <node name="servo_server" pkg="moveit_servo" type="servo_server" output="screen">
    <rosparam command="load" file="$(arg config)" />
    <param name="control_mode" value="velocity" />  <!-- 设置为速度控制模式 -->
  </node>

</launch>

4. Moveit servo自定义控制自己的机械臂

控制自己的机械臂的话，首先要改的就是：

## MoveIt properties
move_group_name:  manipulator  # Often 'manipulator' or 'arm'
planning_frame: base  # The MoveIt planning frame. Often 'base_link' or 'world'

## Other frames
ee_frame_name: tool0  # The name of the end effector link, used to return the EE pose
robot_link_command_frame:  tool0  # commands must be given in the frame of a robot link. Usually either the base or end effector

这几个参数相当于是你想控制你的啥，就改成啥。

然后是话题：

/tool0/velocity就是我专门写了个速度发布的程序，发布的就是/tool0/velocity，就会被Moveit servo接收到。

"/joint_group_vel_controller/command"就是经过Moveit servo处理后，发给谁，一般是发给某个控制器，这里要检查控制器名字是否符合自己的UR官方的启动文件中的控制器！！！

## Topic names
cartesian_command_in_topic: "/tool0/velocity"  # 用于接收末端执行器速度命令的Twist话题
joint_command_in_topic: "delta_joint_cmds"  # 可选，用于接收关节命令的Twist话题
joint_topic: "/joint_states"  # 关节状态话题
status_topic: "/servo_server/status"
command_out_topic: "/joint_group_vel_controller/command"  # MoveIt Servo 发布控制命令给机器人控制器

然后就是一些关节速度限制、关节加速度限制等比较个性化的小参数，改改就行。

5. Moveit servo大坑记录

踩了非常多的坑，这里挑选几个记录：

5.1 控制器没启动：

官方文档说了，想用Moveit servo，必须有两个控制器是启动的，

joint_group_position_controller or joint_group_velocity_controller should be running

我是想控制速度的，关节速度。我一开始去查我的UR启动，发现我起了：

 <arg name="controllers" default="joint_state_controller speed_scaling_state_controller force_torque_sensor_controller joint_group_vel_controller" doc="Controllers that are activated by default."/>
  <arg name="stopped_controllers" default="pos_joint_traj_controller scaled_pos_joint_traj_controller" doc="Controllers that are initally loaded, but not started."/>

但是，我的机械臂一直不动！，我后来发现，虽然joint_group_vel_controller启动了，但是他是initialize的状态，而不是running！！！而且名字也不一样。

可以用以下命令查看自己的控制器们都是什么状态：

rosservice call /controller_manager/list_controllers

如果没有启动或是启动了不在running，就去UR的启动项里改！

5.2 yaml文件中话题对不上

这个刚刚介绍了话题怎么设计，一定要保证这几个节点、话题名称都是互相符合的。

以下是几个常用的排查：

rostopic echo /joint_group_vel_controller/command
这个查看到底我们的速度控制器有没有收到关节速度，这是servo解算后发出的。

 rostopic echo /tool0/velocity
这个查看有没有人给servo发送末端期望速度等（在我的场景里是这个话题）

rosnode list | grep servo_server
看看这个servo到底启动没有？

rostopic echo /moveit_servo/status
看看servo有输入的时候，他的自身的一些状态

5.3 控制模式只能选一种

我调的时候，有几次servo这个节点起来就死掉了，一直提示

[servo_server-1] process has died [pid 1014233, exit code 1, cmd.................

这其实是yaml中，控制模式重复了！以下三个，只能有一个true！！！！

publish_joint_positions: true
publish_joint_velocities: true
publish_joint_accelerations: false

模式不选对，关节会速度非常大，要注意安全！我这最开始忘改了，关节速度无限大，机械臂直接甩起来急停。

5.4 command_out_type: std_msgs/Float64MultiArray

不同控制方法，这个数据类型也不一样，不同版本也不一样，这个不细说了。

综上，ROS 1+Moveit 1）版本部署Moveit servo应该就没问题了。