UR机械臂配置moveit_config和moveit_servo​（保姆级教程）

UR机械臂配置moveit_servo​

本教程在ros2 humble下使用moveit2的新特性moveit_servo功能包实现UR16e机械臂的末端伺服，ur其他系列也可参照此教程。
本教程还包含了UR机械臂手动配置moveit_config的简易教程。

下文是从零开始的配置，如果使用该工程文件，你完成编译过后，可以通过如下指令启动moveit_servo的UR16e仿真案例:

#环境变量
source ~/humble_ws/moveit_main_ws/install/setup.bash
source ~/humble_ws/ros_ur_driver/install/setup.bash#moveit_servo节点
ros2 launch moveit_servo demo_twist_ur16e_api.launch.py#等待robot state update提示
[servo_node-2] [INFO] [1749624571.154165652] [servo_node]: Waiting to receive robot state update.#ur robot driver
ros2 launch ur_robot_driver ur16e.launch.py  robot_ip:=yyy.yyy.yyy.yyy initial_joint_controller:=joint_trajectory_controller use_fake_hardware:=true launch_rviz:=true#键盘输入控制
ros2 run moveit_servo servo_keyboard_input

文件下载和编译

下载

1.下载ur humble驱动，下载指定版本的moveit2，main分支下commit id为fb34bff593b81638c41cfe1ea567905b12b28fac。

2.官方ur驱动文件需要修改scaled_joint_trajectory_controller.cpp的realtime_tools类型和moveit版本适配问题。

#include "ur_controllers/scaled_joint_trajectory_controller.hpp"
// 其他头文件...namespace ur_controllers
{
controller_interface::return_type ScaledJointTrajectoryController::update(const rclcpp::Time& time, const rclcpp::Duration& period)
{// 其他代码...// 行 140：修复 rt_has_pending_goal_ 的逻辑运算if (current_external_msg != *new_external_msg && (*rt_has_pending_goal_.readFromRT() && !active_goal) == false) {// 内部逻辑...}// 行 207：修复 rt_is_holding_ 的取反if (!before_last_point && !(*rt_is_holding_.readFromRT()) && cmd_timeout_ > 0.0 && time_difference > cmd_timeout_) {// 内部逻辑...}// 行 221：修复 rt_is_holding_ 的取反if ((before_last_point || first_sample) && !(*rt_is_holding_.readFromRT()) &&// 其他条件...) {// 内部逻辑...}// 行 227：修复 rt_is_holding_ 的取反if (!before_last_point && !(*rt_is_holding_.readFromRT()) &&// 其他条件...) {// 内部逻辑...}// 行 297：修复 rt_has_pending_goal_ 的赋值rt_has_pending_goal_.writeFromNonRT(false);// 行 313：修复 rt_has_pending_goal_ 的赋值rt_has_pending_goal_.writeFromNonRT(false);// 行 330：修复 rt_has_pending_goal_ 的赋值rt_has_pending_goal_.writeFromNonRT(false);// 行 338：修复 rt_has_pending_goal_ 的取反} else if (tolerance_violated_while_moving && !(*rt_has_pending_goal_.readFromRT())) {// 内部逻辑...}// 行 344：修复 rt_has_pending_goal_ 的取反} else if (!before_last_point && !within_goal_time && !(*rt_has_pending_goal_.readFromRT())) {// 内部逻辑...}// 其他代码...return controller_interface::return_type::OK;
}
}  // namespace ur_controllers

安装依赖

sudo apt install -y \build-essential \cmake \git \python3-colcon-common-extensions \python3-flake8 \python3-rosdep \python3-setuptools \python3-vcstool \wget

sudo apt update
sudo apt dist-upgrade
rosdep update

source /opt/ros/humble/setup.bash
rosdep install -r --from-paths . --ignore-src -y

安装 colcon mixin

sudo apt install python3-colcon-common-extensions
sudo apt install python3-colcon-mixin
colcon mixin add default https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml
colcon mixin update default

编译

将所下载的ur驱动和moveit2移动到不同的工作空间,（假设为_{/humble_ws/moveit_main_ws和}/humble_ws/ros_ur_driver/）。

编译moveit2

moveit2安装先参照教程。拉去完整文件：

cd ~/humble_ws/moveit_main_ws/src
git clone -b <branch> https://github.com/moveit/moveit2_tutorials ##请注意你的moveit2版本！这是编译成功的关键一步
vcs import --recursive < moveit2_tutorials/moveit2_tutorials.repos
sudo apt remove ros-$ROS_DISTRO-moveit*sudo apt update && rosdep install -r --from-paths . --ignore-src --rosdistro $ROS_DISTRO -y

然后执行以下命令：
编译moveit2

source /opt/ros/humble/setup.bash
cd ~/humble_ws/moveit_main_ws
colcon build --symlink-install --mixin release compile-commands --executor sequential --parallel-workers 8

注意：编译之后如果将工作空间移动到了其他位置，需要删除 build install 文件夹，再次编译

编译ur ros2驱动

ur的ros2 humble驱动安装参照教程

cd $YOUR_WS
git clone -b humble https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver.git src/Universal_Robots_ROS2_Driver
vcs import src --skip-existing --input src/Universal_Robots_ROS2_Driver/Universal_Robots_ROS2_Driver-not-released.${ROS_DISTRO}.repos
rosdep update
rosdep install --ignore-src --from-paths src -y
colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release
source install/setup.bash

测试ur驱动是否安装成功（可选项）,你也可以直接转到下一步

source ~/humble_ws/ros_ur_driver/install/setup.bash
#启动一个ur16e fake hardware
ros2 launch ur_robot_driver ur16e.launch.py  robot_ip:=yyy.yyy.yyy.yyy initial_joint_controller:=joint_trajectory_controller use_fake_hardware:=true launch_rviz:=true  activate_joint_controller:=false

测试ur16e关节轨迹跟踪驱动是否正常启动

ros2 topic pub --once /scaled_joint_trajectory_controller/joint_trajectory trajectory_msgs/msg/JointTrajectory "{ header: { stamp: { sec: 0, nanosec: 0 }, frame_id: '' }, joint_names: ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint'], points: [ { positions: [0.0, -1.57, 0.0, -1.0, 0.0, 0.0], velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], accelerations: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0], time_from_start: { sec: 5, nanosec: 0 } } ] }"

配置UR的moveit_config

moveit_servo的依赖moveit_config文件提供机器人描述和ros2 controller等信息。准备开始配置moveit包（如果你从apt安装ur官方驱动其实会自动安装moveit,但是我们所使用的moveit_servo属于新特性，标准的moveit发行版并不支持，所以一切都需要手动配置）

1.输入ros2 pkg prefix ur_description，找到你的ur_decscription安装位置，为你的ur机械臂导出urdf文件,以ur16e为例。

ros2 run xacro xacro /opt/ros/humble/share/ur_description/urdf/ur.urdf.xacro ur_type:=ur16e name:=ur16e > ur16e.urdf

2.打开moveit_setup_assistant

source ~/humble_ws/moveit_main_ws/install/setup.bash
ros2 run moveit_setup_assistant moveit_setup_assistant 

3.配置moveit,第一次使用可以看看这个帖子，以下是一些关键配置的截图。

导入刚刚生成的urdf文件

生成自碰撞检测文件

配置move_group

控制接口定义

3D sensor配置

这里的控制器名称要和驱动里用来进行关节轨迹控制的控制器名称保持一致，joint_trajectory_controller。

3.配置完成后，我们生成了一个配置包ur16e_moveit_config。手动修改ur16e_moveit_config/config/joint_limits.yaml中的加速度配置选项为

has_acceleration_limits: truemax_acceleration: 3.1415926535897931

编译ur16e_moveit_config，建议把包放在ur驱动的src里面

 colcon build --packages-select ur16e_moveit_config

4.进行moveit规划执行测试（可选项，建议执行）
按如下顺序分别启动ur驱动、moveit_group、moveit_rviz

#记得source 一下
ros2 launch ur_robot_driver ur16e.launch.py  robot_ip:=yyy.yyy.yyy.yyy initial_joint_controller:=joint_trajectory_controller use_fake_hardware:=true launch_rviz:=trueros2 launch ur16e_moveit_config move_group.launch.pyros2 launch ur16e_moveit_config moveit_rviz.launch.py

把末端指示器移动一下位置，点击plan&execute,如果一切顺利末端会随着规划成功的轨迹移动。

配置moveit_servo

moveit_servo包位于/moveit2/moveit_ros/moveit_servo下，支持位姿和速度伺服。为了让moveit_servo成功在我们的ur机械臂上运行，我们需要做一些适配。

添加config文件

src/moveit2/moveit_ros/moveit_servo/config

###############################################
# Modify all parameters related to servoing here
################################################ Optionally override Servo's internal velocity scaling when near singularity or collision (0.0 = use internal velocity scaling)
# override_velocity_scaling_factor = 0.0 # valid range [0.0:1.0]## Properties of outgoing commands
publish_period: 0.01  # 1/Nominal publish rate [seconds]
max_expected_latency: 0.1  # delay between sending a command and the robot executing it [seconds]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.2  # Max linear velocity. Unit is [m/s]. Only used for Cartesian commands.rotational:  0.8  # Max angular velocity. Unit is [rad/s]. Only used for Cartesian commands.# Max joint angular/linear velocity. Only used for joint commands on joint_command_in_topic.joint: 0.5# What type of topic does your robot driver expect?
# Currently supported are std_msgs/Float64MultiArray or trajectory_msgs/JointTrajectory
command_out_type: trajectory_msgs/JointTrajectory# What to publish? Can save some bandwidth as most robots only require positions or velocities
publish_joint_positions: true
publish_joint_velocities: true
publish_joint_accelerations: false## Plugins for smoothing outgoing commands
use_smoothing: true
smoothing_filter_plugin_name: "online_signal_smoothing::AccelerationLimitedPlugin"# 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: true
check_octomap_collisions: false  # Check collision against the octomap (if a 3D sensor plugin is available)## MoveIt properties
move_group_name: ur16e_arm  # Often 'manipulator' or 'arm'## Configure handling of singularities and joint limits
lower_singularity_threshold: 10.0  # Start decelerating when the condition number hits this (close to singularity)
hard_stop_singularity_threshold: 30.0  # Stop when the condition number hits this
leaving_singularity_threshold_multiplier: 2.0  # Multiply the hard stop limit by this when leaving singularity (see https://github.com/moveit/moveit2/pull/620)
# Added as a buffer to joint variable position bounds [in that joint variable's respective units].
# Can be of size 1, which applies the margin to all joints, or the same size as the number of degrees of freedom of the active joint group.
# If moving quickly, make these values larger.
joint_limit_margins: [0.12]## 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  # Get joint states from this topic
status_topic: ~/status  # Publish status to this topic
command_out_topic: /joint_trajectory_controller/joint_trajectory  # Publish outgoing commands here## Collision checking for the entire robot body
check_collisions: true  # Check collisions?
collision_check_rate: 10.0  # [Hz] Collision-checking can easily bog down a CPU if done too often.
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]

添加launch文件

import os
import launch
import launch_ros
from ament_index_python.packages import get_package_share_directory
from launch_param_builder import ParameterBuilder
from moveit_configs_utils import MoveItConfigsBuilder
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch.conditions import IfCondition, UnlessConditiondef generate_launch_description():# MoveIt Config for UR16emoveit_config = MoveItConfigsBuilder("ur16e", package_name="ur16e_moveit_config").to_moveit_configs()# Launch Servo as a standalone node or as a "node component" for better latency/efficiencylaunch_as_standalone_node = LaunchConfiguration("launch_as_standalone_node", default="true")# Get parameters for the Servo nodeservo_params = {"moveit_servo": ParameterBuilder("moveit_servo").yaml("config/ur16e_simulated_config.yaml").to_dict()}# This sets the update rate and planning group name for the acceleration limiting filteracceleration_filter_update_period = {"update_period": 0.01}planning_group_name = {"planning_group_name": "ur16e_arm"}# RVizrviz_config_file = (get_package_share_directory("moveit_servo")+ "/config/demo_rviz_config_ros.rviz")rviz_node = launch_ros.actions.Node(package="rviz2",executable="rviz2",name="rviz2",output="log",arguments=["-d", rviz_config_file],parameters=[moveit_config.robot_description,moveit_config.robot_description_semantic,],)# Launch as much as possible in componentscontainer = launch_ros.actions.ComposableNodeContainer(name="moveit_servo_demo_container",namespace="/",package="rclcpp_components",executable="component_container_mt",composable_node_descriptions=[launch_ros.descriptions.ComposableNode(package="tf2_ros",plugin="tf2_ros::StaticTransformBroadcasterNode",name="static_tf2_broadcaster",parameters=[{"child_frame_id": "/base_link", "frame_id": "/world"}],),],output="screen",)# Launch a standalone Servo node.# As opposed to a node component, this may be necessary (for example) if Servo is running on a different PCservo_node = launch_ros.actions.Node(package="moveit_servo",executable="servo_node",name="servo_node",parameters=[servo_params,acceleration_filter_update_period,planning_group_name,moveit_config.robot_description,moveit_config.robot_description_semantic,moveit_config.robot_description_kinematics,moveit_config.joint_limits,],output="screen",condition=IfCondition(launch_as_standalone_node),)return launch.LaunchDescription([rviz_node,servo_node,container,])

修改servo_keyboard_input.cpp

const std::string PLANNING_FRAME_ID = "world";
const std::string EE_FRAME_ID = "flange";
...
joint_msg->joint_names.resize(6);joint_msg->joint_names = { "elbow_joint", "shoulder_lift_joint", "shoulder_pan_joint", "wrist_1_joint","wrist_2_joint", "wrist_3_joint"};joint_msg->velocities.resize(6);
...

启动！

配置完成过后再编译一下moveit_servo,可以启动。

colcon build --packages-select moveit_servo
#环境变量
source ~/humble_ws/moveit_main_ws/install/setup.bash
source ~/humble_ws/ros_ur_driver/install/setup.bash#moveit_servo节点
ros2 launch moveit_servo demo_twist_ur16e_api.launch.py#等待robot state update提示
[servo_node-2] [INFO] [1749624571.154165652] [servo_node]: Waiting to receive robot state update.#ur robot driver
ros2 launch ur_robot_driver ur16e.launch.py  robot_ip:=yyy.yyy.yyy.yyy initial_joint_controller:=joint_trajectory_controller use_fake_hardware:=true launch_rviz:=true#键盘输入控制
ros2 run moveit_servo servo_keyboard_input

在真机上进行测试只需要启动真机对应的驱动，保证joint_trajectory_controller真实有效即可。

references:

MoveIt Setup Assistant — MoveIt Documentation: Rolling documentation

Realtime Servo — MoveIt Documentation: Rolling documentation

此教程由ZorattC（https://github.com/zorattc） 和MirTITH（https://github.com/MirTITH）编写. Copyright belongs to ZorattC and MirTITH.

​