目录

1. Moveit2 Servo 与 MoveIt2 Trajectory Execution

2. MoveIt2 Servo

2.1 MoveIt2 Servo 的主要特点：

2.2 MoveIt2 Servo 的工作流程：

2.3 使用 MoveIt2 Servo 的典型步骤：

3. 以UR机器人为例

3.1 servo_node驱动forward_position_controller

3.2 servo_node驱动scaled_joint_trajectory_controller

---

1. Moveit2 Servo 与 MoveIt2 Trajectory Execution

特性	MoveIt2 Servo	MoveIt2 Trajectory Execution
控制类型	实时速度控制	预先规划的轨迹执行
适用场景	遥操作、实时跟踪、动态响应	任务规划、精确运动
规划延迟	低延迟(同步)	较高(异步)
控制输入	速度命令（笛卡尔空间或关节空间）	轨迹规划（position、velocity 等）

---

2. MoveIt2 Servo

MoveIt2 Servo 是 MoveIt2 框架中的一个组件，专用于实时机器人臂控制。它提供一种通过 伺服控制（Servo）的方法，使机器人臂能够实时响应命令，通常用于以下应用场景：

• 遥操作

• 手柄/设备控制

• 实时跟踪目标

• 虚拟现实 (VR) 控制

2.1 MoveIt2 Servo 的主要特点

1. 实时控制：允许用户通过输入的速度和姿态命令实时控制机器人臂的运动。

2. 支持多种输入：可以接受用户输入的关节速度或笛卡尔空间速度（即末端执行器的速度）。

3. 运动限制检查：通过 MoveIt2 提供的运动学和碰撞检测功能，自动约束运动，避免非法或危险的轨迹。

4. 高性能：提供低延迟的控制输入到执行输出，适合对实时性要求高的任务。

2.2 MoveIt2 Servo 的工作流程

1. 输入速度命令：通过关节空间或笛卡尔空间的速度指令输入给 Servo。

2. 内部规划与约束检查：使用机器人模型进行运动学求解;执行碰撞检测和关节限制约束

3. 指令输出：将处理后的关节速度命令发送到机器人控制器进行运动。

2.3 使用 MoveIt2 Servo 的典型步骤

1.配置Moveit2 Servo

在参数文件中设置Servo参数，例如：

moveit_servo:
  command_in_type: "unitless"  # 输入类型: 可为 unitless, joint, twist
  cartesian_command_in_topic: "/servo_server/delta_twist_cmds"
  joint_command_in_topic: "/servo_server/delta_joint_cmds"
  publish_period: 0.01  # 控制周期
  move_group_name: "manipulator"
  planning_frame: "base_link"
  ...

2.启动 Servo 服务

ros2 launch moveit_servo servo_server.launch.py

3.发布速度命令

• 通过ROS2 topic手动发布速度指令，例如在笛卡尔空间中移动末端执行器：

ros2 topic pub /servo_server/delta_twist_cmds geometry_msgs/msg/TwistStamped '{header: {frame_id: "base_link"}, twist: {linear: {x: 0.1, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}}'

• 通过自定义节点实时发布

3. 以UR机器人为例

3.1 servo_node驱动forward_position_controller

官方servo参数文件：

###############################################
# Modify all parameters related to servoing here
###############################################
use_gazebo: false # Whether the robot is started in a Gazebo simulation environment

## Properties of incoming commands
command_in_type: "speed_units" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s
scale:
  # Scale parameters are only used if command_in_type=="unitless"
  linear:  0.6  # Max linear velocity. Meters per publish_period. Unit is [m/s]. Only used for Cartesian commands.
  rotational:  0.3 # Max angular velocity. Rads per publish_period. Unit is [rad/s]. Only used for Cartesian commands.
  # Max joint angular/linear velocity. Rads or Meters per publish period. Only used for joint commands on joint_command_in_topic.
  joint: 0.01
# This is a fudge factor to account for any latency in the system, e.g. network latency or poor low-level
# controller performance. It essentially increases the timestep when calculating the target pose, to move the target
# pose farther away. [seconds]
system_latency_compensation: 0.04

## Properties of outgoing commands
publish_period: 0.004  # 1/Nominal publish rate [seconds]
low_latency_mode: false  # Set this to true to publish as soon as an incoming Twist command is received (publish_period is ignored)

# What type of topic does your robot driver expect?
# Currently supported are std_msgs/Float64MultiArray or trajectory_msgs/JointTrajectory
command_out_type: std_msgs/Float64MultiArray

# What to publish? Can save some bandwidth as most robots only require positions or velocities
publish_joint_positions: true
publish_joint_velocities: false
publish_joint_accelerations: false

## Plugins for smoothing outgoing commands
smoothing_filter_plugin_name: "online_signal_smoothing::ButterworthFilterPlugin"

# If is_primary_planning_scene_monitor is set to true, the Servo server's PlanningScene advertises the /get_planning_scene service,
# which other nodes can use as a source for information about the planning environment.
# NOTE: If a different node in your system is responsible for the "primary" planning scene instance (e.g. the MoveGroup node),
# then is_primary_planning_scene_monitor needs to be set to false.
is_primary_planning_scene_monitor: false

## Incoming Joint State properties
low_pass_filter_coeff: 10.0  # Larger --> trust the filtered data more, trust the measurements less.

## MoveIt properties
move_group_name:  ur_manipulator  # Often 'manipulator' or 'arm'
planning_frame: base_link  # 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

## Stopping behaviour
incoming_command_timeout:  0.1  # Stop servoing if X seconds elapse without a new command
# If 0, republish commands forever even if the robot is stationary. Otherwise, specify num. to publish.
# Important because ROS may drop some messages and we need the robot to halt reliably.
num_outgoing_halt_msgs_to_publish: 4

## Configure handling of singularities and joint limits
lower_singularity_threshold:  100.0  # Start decelerating when the condition number hits this (close to singularity)
hard_stop_singularity_threshold: 200.0 # Stop when the condition number hits this
joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger.

## Topic names
cartesian_command_in_topic: ~/delta_twist_cmds  # Topic for incoming Cartesian twist commands
joint_command_in_topic: ~/delta_joint_cmds # Topic for incoming joint angle commands
joint_topic: /joint_states
status_topic: ~/status # Publish status to this topic
command_out_topic: /forward_position_controller/commands # Publish outgoing commands here

## Collision checking for the entire robot body
check_collisions: true # Check collisions?
collision_check_rate: 5.0 # [Hz] Collision-checking can easily bog down a CPU if done too often.
# Two collision check algorithms are available:
# "threshold_distance" begins slowing down when nearer than a specified distance. Good if you want to tune collision thresholds manually.
# "stop_distance" stops if a collision is nearer than the worst-case stopping distance and the distance is decreasing. Requires joint acceleration limits
collision_check_type: threshold_distance
# Parameters for "threshold_distance"-type collision checking
self_collision_proximity_threshold: 0.01 # Start decelerating when a self-collision is this far [m]
scene_collision_proximity_threshold: 0.02 # Start decelerating when a scene collision is this far [m]
# Parameters for "stop_distance"-type collision checking
collision_distance_safety_factor: 1000.0 # Must be >= 1. A large safety factor is recommended to account for latency
min_allowable_collision_distance: 0.01 # Stop if a collision is closer than this [m]

servo节点图

此命令向servo node发布/servo_node/delta_twist_cmds话题（x轴方向以-0.1m/s速度平移）

ros2 topic pub /servo_node/delta_twist_cmds geometry_msgs/msg/TwistStamped "{ header: { stamp: 'now' },  twist: {linear: {x: -0.1}, angular: {  }}}" -r 1

机器人图:

3.2 servo_node驱动scaled_joint_trajectory_controller

servo_node 除了forward_position_controller，也可以驱动其他类型的控制器，比如scaled_joint_trajectory_controller。只需要修改配置文件即可实现其他类型控制器的实时控制。

节点图