ROS2中使用MoveIt2精准操控Panda机械臂末端至固定位姿

ROS2中使用MoveIt2精准操控Panda机械臂末端至固定位姿

引用

CSDN

1.

https://m.blog.csdn.net/weixin_38428827/article/details/146029552

在机器人领域，精准控制机械臂末端执行器的位置和姿态是实现复杂任务的基础。ROS（机器人操作系统）和MoveIt2作为机器人开发的重要工具，提供了强大的运动规划和控制能力。本文将详细介绍如何在ROS2中使用MoveIt2精准操控Panda机械臂末端至固定位姿，帮助读者掌握这一关键技术。

创建ROS2包

首先，我们需要创建一个ROS2包，命名为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文件

在panda_moveit_controller包的根目录下修改CMakeLists.txt文件，添加对rclcpp和moveit_ros_planning_interface的依赖，并添加可执行文件。

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()

编译运行

在两个终端分别执行以下命令：

1. 启动Panda机械臂在Rviz中的场景

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

2. 启动末端控制

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