一文搞定！ROS2 中用 MoveIt2 精准操控 Panda 机械臂末端至固定位姿

视频讲解

一文搞定！ROS2 中用 MoveIt2 精准操控 Panda 机械臂末端至固定位姿

在《ROS2 应用：按键控制 MoveIt2 中 Panda 机械臂关节位置》中介绍了如何控制关节位置，今天介绍下如何控制笛卡尔空间下的末端位姿

创建package，命名panda_moveit_controller，依赖moveit_ros_planning_interface

ros2 pkg create --build-type ament_cmake panda_moveit_controller --dependencies rclcpp moveit_ros_planning_interface

在~/ros2_ws/src/panda_moveit_controller/src目录下创建panda_moveit_control.cpp

#include <rclcpp/rclcpp.hpp>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>

#include <tf2/LinearMath/Quaternion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

#include <cmath>

// 定义一个将度数转换为弧度的函数
double degreesToRadians(double degrees) {
    return degrees * M_PI / 180.0;
}

int main(int argc, char** argv)
{
    rclcpp::init(argc, argv);
    auto node = rclcpp::Node::make_shared("panda_moveit_control");

    // 创建MoveGroupInterface对象，指定规划组名称
    moveit::planning_interface::MoveGroupInterface move_group(node, "panda_arm");

    // 创建PlanningSceneInterface对象
    moveit::planning_interface::PlanningSceneInterface planning_scene_interface;

    // 获取规划组的参考坐标系
    std::string planning_frame = move_group.getPlanningFrame();
    RCLCPP_INFO(node->get_logger(), "Planning frame: %s", planning_frame.c_str());

    // 获取规划组的末端执行器名称
    std::string end_effector_link = move_group.getEndEffectorLink();
    RCLCPP_INFO(node->get_logger(), "End effector link: %s", end_effector_link.c_str());

    // 设置目标位置
    geometry_msgs::msg::Pose target_pose;

    // 定义欧拉角（单位：弧度）
    double roll = degreesToRadians(0.0);
    double pitch = degreesToRadians(180.0);
    double yaw = degreesToRadians(0.0);

    // 创建一个四元数对象
    tf2::Quaternion quaternion;
    // 根据欧拉角设置四元数
    quaternion.setRPY(roll, pitch, yaw);

    // 将tf2的四元数转换为geometry_msgs的四元数
    target_pose.orientation = tf2::toMsg(quaternion);

    target_pose.position.x = 0.4;
    target_pose.position.y = 0.1;
    target_pose.position.z = 0.4;
    move_group.setPoseTarget(target_pose);

    // 进行运动规划
    moveit::planning_interface::MoveGroupInterface::Plan my_plan;
    bool success = (move_group.plan(my_plan) == moveit::core::MoveItErrorCode::SUCCESS);
    RCLCPP_INFO(node->get_logger(), "Planning %s", success ? "SUCCEEDED" : "FAILED");

    // 执行运动规划
    if (success) {
        move_group.execute(my_plan);
    }

    rclcpp::shutdown();
    return 0;
}

修改CMakeLists.txt文件

cmake_minimum_required(VERSION 3.8)
project(panda_moveit_controller)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# 查找依赖项
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(moveit_ros_planning_interface REQUIRED)

# 添加可执行文件
add_executable(panda_moveit_control src/panda_moveit_control.cpp)
ament_target_dependencies(panda_moveit_control rclcpp moveit_ros_planning_interface)

# 安装可执行文件
install(TARGETS
  panda_moveit_control
  DESTINATION lib/${PROJECT_NAME}
)

ament_package()

编译运行

一个终端启动panda机械臂在Rviz中的场景

ros2 launch moveit2_tutorials demo.launch.py rviz_config:=panda_moveit_config_demo_empty.rviz

另一个启动末端控制

cd ~/ros2_ws
colcon build --packages-select panda_moveit_controller
source install/setup.bash
ros2 run panda_moveit_controller panda_moveit_control