2.4.1. 设置目标关节角度
2.4.1.1. 依赖
xml
<depend>rclcpp</depend>
<depend>moveit_ros_planning_interface</depend>**VSCode 路径：**Ctrl + Shift + P 并输入
C/C++选择编辑配置(JSON)：json"includePath": [ "${workspaceFolder}/**", "/opt/ros/humble/include/**", "/usr/include/**" ]include：
rclcpp/rclcpp.hpp、moveit/move_group_interface/move_group_interface.h
2.4.1.2. 步骤与 API
步骤：新建节点 -> 单开线程并 Node Spin -> 创建 MoveGroupInterface 对象 -> 设置目标关节+规划执行
示例 API：
cpp
// 节点
auto node = rclcpp::Node::make_shared("end_effector_cmd");
// 线程
rclcpp::executors::SingleThreadedExecutor executor;
executor.add_node(node);
std::thread spin_thread([&executor]() { executor.spin(); });
// MoveGroupInterface 对象
moveit::planning_interface::MoveGroupInterface arm(node, "arm");
// 获取关节角与关节名称
std::vector<double> current_joints = arm.getCurrentJointValues();
std::vector<std::string> joint_names = arm.getJointNames();
// 设置目标关节角
bool success = arm.setJointValueTarget(target_joints);
// 规划
moveit::planning_interface::MoveGroupInterface::Plan plan;
moveit::core::MoveItErrorCode err_code = arm.plan(plan);
// 执行
err_code = arm.execute(plan);
// 关闭节点 + 终止线程
rclcpp::shutdown();
spin_thread.join();2.4.2. 设置目标位姿
2.4.2.1. 传入 MoveIt2 所有配置参数：
在 MoveIt 2 (ROS 2) 中，全局参数服务器（Parameter Server）已不存在；每个节点都是独立的实体，必须在启动时显式获取其所需的全部上下文参数：
python# run.launch.py from launch import LaunchDescription from launch_ros.actions import Node from moveit_configs_utils import MoveItConfigsBuilder def generate_launch_description(): # 构建 MoveIt 配置 moveit_config = MoveItConfigsBuilder( "dm_arm_description", package_name="dm_arm_moveit_config" ).to_moveit_configs() # 创建节点并传入 moveit_config.to_dict() node = Node( package="dm_arm_controller", executable="end_effector_cmd", output="screen", parameters=[ moveit_config.to_dict(), {"use_sim_time": True}, ], ) return LaunchDescription([node])
2.4.2.2. 设置目标 Pose / Position / Orientation
cpp
using TargetVariant = std::variant<
geometry_msgs::msg::Pose,
geometry_msgs::msg::Point,
geometry_msgs::msg::Quaternion
>;
/**
* @brief 设置末端执行器的目标位姿、位置或姿态
* @param target 目标位姿、位置或姿态(geometry_msgs::msg::Pose / geometry_msgs::msg::Point / geometry_msgs::msg::Quaternion)
* @return 设置是否成功
*/
bool ArmController::set_target(const TargetVariant& target) {
bool success = false;
if(auto* pose = std::get_if<geometry_msgs::msg::Pose>(&target)) {
success = _arm_.setPoseTarget(*pose);
}
else if(auto* point = std::get_if<geometry_msgs::msg::Point>(&target)) {
success = _arm_.setPositionTarget(point->x, point->y, point->z);
}
else if(auto* quat = std::get_if<geometry_msgs::msg::Quaternion>(&target)) {
success = _arm_.setOrientationTarget(quat->x, quat->y, quat->z, quat->w);
}
else { return false; }
return success;
}2.4.2.3. RPY 转四元数
- MoveIt2 的接口要求使用四元数，避免欧拉角的万向锁以及插值不连续问题
- RPY 转四元数基于 TF2 (ROS 2 的坐标变换)：
cpp
/**
* @brief 将 Roll-Pitch-Yaw 角转换为四元数
* @param roll 滚转角（绕 X 轴旋转）
* @param pitch 俯仰角（绕 Y 轴旋转）
* @param yaw 偏航角（绕 Z 轴旋转）
* @return 转换后的四元数
*/
geometry_msgs::msg::Quaternion ArmController::rpy_to_quaternion(double roll, double pitch, double yaw) {
tf2::Quaternion quat;
quat.setRPY(roll, pitch, yaw);
quat.normalize();
geometry_msgs::msg::Quaternion quat_msg;
quat_msg.x = quat.x();
quat_msg.y = quat.y();
quat_msg.z = quat.z();
quat_msg.w = quat.w();
return quat_msg;
}2.4.3. TF 坐标变换
2.4.3.1. 作用
- 在 ROS2 中，坐标变换由 tf2 系统维护，核心思想：维护一棵坐标系树（Transform Tree）
- TF 处理连杆之间的随时间变化的齐次变换矩阵，所以必须要有时间戳，所有 API 都要求有时间戳
- 依赖：
tf2_geometry_msgs - include：
tf2_ros
2.4.3.2. 相关 API
初始化：
cppstd::shared_ptr<tf2_ros::Buffer> _tf_buffer_; std::shared_ptr<tf2_ros::TransformListener> _tf_listener_; _tf_buffer_ = std::make_shared<tf2_ros::Buffer>(_node_->get_clock()); _tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*_tf_buffer_);查询并执行两坐标系之间的变换：
cpp
geometry_msgs::msg::TransformStamped tf = tf_buffer->lookupTransform(target, source, time); tf2::doTransform(in, out, transform);
// 异常： tf2::LookupException tf2::ExtrapolationException tf2::ConnectivityException
- **更高层变换 API：** `_tf_buffer_->transform(in, target_frame);`
### 2.4.3.3. 代码
```cpp
/**
* @brief 将输入的位姿、位置或姿态从底座坐标系转换到末端执行器坐标系
* @param in 输入的位姿、位置或姿态(geometry_msgs::msg::Pose / geometry_msgs::msg::Point / geometry_msgs::msg::Quaternion / geometry_msgs::msg::PoseStamped)
* @param out 转换后的位姿、位置或姿态(geometry_msgs::msg::Pose / geometry_msgs::msg::Point / geometry_msgs::msg::Quaternion / geometry_msgs::msg::PoseStamped)
* @return 转换是否成功
*/
template<class T>
bool ArmController::base_to_end_tf(const T& in, T& out) {
// 检查输入类型
static_assert(
std::is_same_v<T, geometry_msgs::msg::Pose> ||
std::is_same_v<T, geometry_msgs::msg::Point> ||
std::is_same_v<T, geometry_msgs::msg::Quaternion> ||
std::is_same_v<T, geometry_msgs::msg::PoseStamped>,
"仅支持 Pose(Stamped)、Point 和 Quaternion 的坐标变换");
try {
// 带时间戳则直接变换
if constexpr(std::is_same_v<T, geometry_msgs::msg::PoseStamped>) {
auto tf_stamped = _tf_buffer_->lookupTransform(_eef_link_, _base_link_, in.header.stamp);
tf2::doTransform(in, out, tf_stamped);
}
// 不带时间戳则构造 PoseStamped 进行变换后提取
else {
geometry_msgs::msg::PoseStamped pose_in;
pose_in.header.frame_id = _base_link_;
pose_in.header.stamp = _node_->now();
// 构造 PoseStamped
if constexpr(std::is_same_v<T, geometry_msgs::msg::Pose>) {
pose_in.pose = in;
}
else if constexpr(std::is_same_v<T, geometry_msgs::msg::Point>) {
pose_in.pose.position = in;
pose_in.pose.orientation.w = 1.0;
}
else if constexpr(std::is_same_v<T, geometry_msgs::msg::Quaternion>) {
pose_in.pose.orientation = in;
}
// 进行坐标变换
geometry_msgs::msg::PoseStamped pose_out = _tf_buffer_->transform(pose_in, _eef_link_);
if constexpr(std::is_same_v<T, geometry_msgs::msg::Pose>) {
out = pose_out.pose;
}
else if constexpr(std::is_same_v<T, geometry_msgs::msg::Point>) {
out = pose_out.pose.position;
}
else if constexpr(std::is_same_v<T, geometry_msgs::msg::Quaternion>) {
out = pose_out.pose.orientation;
}
}
RCLCPP_INFO(_node_->get_logger(), "坐标变换成功：%s -> %s", _base_link_.c_str(), _eef_link_.c_str());
return true;
}
catch(const tf2::TransformException& e) {
RCLCPP_ERROR(_node_->get_logger(), "坐标变换失败：%s", e.what());
return false;
}
}2.4.4. 参数配置
在功能包里增加 config/params.yaml 文件，并按照以下格式：
yaml
/**: # 表示所有节点均可使用
ros__parameters: # 声明为 ros 的参数服务
motion_planning: # 键值对
planning_time: 5.0 # 获取时用 "." 来表示包含
planning_attempts: 10 # 先声明再获取
target_tolerance: # _node_->declare_parameter("tf.timeout", 0.1);
position: 0.001 # a = _node_->get_parameter("tf.timeout", 0.1);
orientation: 0.01
joint: 0.01这是关于赞助的一些描述
- 本文链接：https://kaede-rei.github.io/learning-path/arm-motion-control/phase-2/2-4
- 版权声明：本博客所有文章除特别声明外，均默认采用 CC BY-NC-SA 许可协议。
