MoveIt创建抓取位姿的完整指南

在机器人抓取任务中，创建合适的抓取位姿(Grasp Pose)是关键步骤。下面我将详细介绍如何使用MoveIt在ROS2中创建和配置抓取位姿。

1. 准备工作

1.1 确保已安装MoveIt

bash

sudo apt install ros-${ROS_DISTRO}-moveit

1.2 创建MoveIt配置包

如果你还没有MoveIt配置包，可以使用MoveIt Setup Assistant创建：

bash

ros2 launch moveit_setup_assistant setup_assistant.launch.py

2. 定义抓取位姿

2.1 创建抓取姿态消息

msg/GraspPose.msg:

text

# 抓取位姿
geometry_msgs/PoseStamped pose

# 抓取方向 (可选)
string approach_direction  # 如"top", "side", "front"

# 抓取宽度 (单位:米)
float32 grasp_width

# 抓取质量评分 (0-1)
float32 quality

2.2 在CMakeLists.txt中添加消息生成

cmake

find_package(rosidl_default_generators REQUIRED)

rosidl_generate_interfaces(${PROJECT_NAME}
  "msg/GraspPose.msg"
  DEPENDENCIES geometry_msgs
)

3. 使用MoveIt Grasp Library

3.1 创建抓取数据配置文件

config/grasps.yaml:

yaml

grasps:
  - id: "top_grasp"
    pose:
      position: {x: 0.0, y: 0.0, z: 0.1}
      orientation: {x: 0.707, y: 0.0, z: 0.0, w: 0.707}
    approach_direction: "top"
    grasp_width: 0.05
    quality: 0.9
    pre_grasp_posture: [0.0, 0.0]  # 夹爪预抓取位置
    grasp_posture: [0.8, 0.8]      # 夹爪抓取位置
    
  - id: "side_grasp"
    pose:
      position: {x: 0.0, y: 0.05, z: 0.0}
      orientation: {x: 0.0, y: 0.707, z: 0.0, w: 0.707}
    approach_direction: "side"
    grasp_width: 0.04
    quality: 0.7
    pre_grasp_posture: [0.0, 0.0]
    grasp_posture: [0.7, 0.7]

3.2 创建抓取位姿生成器

src/grasp_pose_generator.cpp:

cpp

#include <rclcpp/rclcpp.hpp>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit_msgs/msg/grasp.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <yaml-cpp/yaml.h>

class GraspPoseGenerator : public rclcpp::Node
{
public:
    GraspPoseGenerator() : Node("grasp_pose_generator")
    {
        // 加载抓取配置
        this->declare_parameter("grasp_config", "");
        std::string grasp_config = this->get_parameter("grasp_config").as_string();
        
        if (grasp_config.empty()) {
            RCLCPP_ERROR(this->get_logger(), "未提供抓取配置文件路径");
            return;
        }
        
        // 解析YAML文件
        try {
            YAML::Node config = YAML::LoadFile(grasp_config);
            loadGraspConfigurations(config);
        } catch (const YAML::Exception & e) {
            RCLCPP_ERROR(this->get_logger(), "加载抓取配置失败: %s", e.what());
        }
    }

private:
    std::vector<moveit_msgs::msg::Grasp> grasps_;
    
    void loadGraspConfigurations(const YAML::Node & config)
    {
        const YAML::Node & grasp_nodes = config["grasps"];
        
        for (const auto & grasp_node : grasp_nodes) {
            moveit_msgs::msg::Grasp grasp;
            
            // 设置抓取位姿
            grasp.grasp_pose.header.frame_id = "world";
            grasp.grasp_pose.pose.position.x = grasp_node["pose"]["position"]["x"].as<double>();
            grasp.grasp_pose.pose.position.y = grasp_node["pose"]["position"]["y"].as<double>();
            grasp.grasp_pose.pose.position.z = grasp_node["pose"]["position"]["z"].as<double>();
            grasp.grasp_pose.pose.orientation.x = grasp_node["pose"]["orientation"]["x"].as<double>();
            grasp.grasp_pose.pose.orientation.y = grasp_node["pose"]["orientation"]["y"].as<double>();
            grasp.grasp_pose.pose.orientation.z = grasp_node["pose"]["orientation"]["z"].as<double>();
            grasp.grasp_pose.pose.orientation.w = grasp_node["pose"]["orientation"]["w"].as<double>();
            
            // 设置预抓取和抓取姿态
            for (const auto & value : grasp_node["pre_grasp_posture"]) {
                grasp.pre_grasp_posture.points[0].positions.push_back(value.as<double>());
            }
            
            for (const auto & value : grasp_node["grasp_posture"]) {
                grasp.grasp_posture.points[0].positions.push_back(value.as<double>());
            }
            
            // 设置抓取方向
            std::string direction = grasp_node["approach_direction"].as<std::string>();
            setApproachDirection(grasp, direction);
            
            // 设置其他参数
            grasp.grasp_quality = grasp_node["quality"].as<double>();
            grasp.max_contact_force = 0.0;  // 使用默认值
            
            grasps_.push_back(grasp);
        }
        
        RCLCPP_INFO(this->get_logger(), "加载了 %zu 个抓取配置", grasps_.size());
    }
    
    void setApproachDirection(moveit_msgs::msg::Grasp & grasp, const std::string & direction)
    {
        geometry_msgs::msg::Vector3Stamped approach;
        approach.header.frame_id = "world";
        
        if (direction == "top") {
            approach.vector.z = -1.0;  // 从上方接近
        } else if (direction == "side") {
            approach.vector.y = -1.0;  // 从侧面接近
        } else if (direction == "front") {
            approach.vector.x = 1.0;   // 从前方接近
        }
        
        grasp.pre_grasp_approach.direction = approach;
        grasp.post_grasp_retreat.direction = approach;
        
        // 设置接近和撤退距离
        grasp.pre_grasp_approach.desired_distance = 0.1;
        grasp.pre_grasp_approach.min_distance = 0.05;
        grasp.post_grasp_retreat.desired_distance = 0.1;
        grasp.post_grasp_retreat.min_distance = 0.05;
    }
    
    void generateGraspPoses(const geometry_msgs::msg::PoseStamped & object_pose)
    {
        // 创建MoveIt接口
        auto move_group_interface = moveit::planning_interface::MoveGroupInterface(shared_from_this(), "arm_group");
        auto planning_scene_interface = moveit::planning_interface::PlanningSceneInterface();
        
        // 转换抓取位姿到物体坐标系
        std::vector<moveit_msgs::msg::Grasp> object_grasps;
        
        for (const auto & grasp : grasps_) {
            moveit_msgs::msg::Grasp transformed_grasp = grasp;
            
            // 转换位姿到物体坐标系
            tf2::Transform object_tf, grasp_tf, result_tf;
            tf2::fromMsg(object_pose.pose, object_tf);
            tf2::fromMsg(grasp.grasp_pose.pose, grasp_tf);
            
            result_tf = object_tf * grasp_tf;
            
            transformed_grasp.grasp_pose.pose = tf2::toMsg(result_tf);
            transformed_grasp.grasp_pose.header = object_pose.header;
            
            object_grasps.push_back(transformed_grasp);
        }
        
        // 执行抓取规划
        move_group_interface.planGraspsAndPick("object_id", object_grasps);
    }
};

int main(int argc, char * argv[])
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<GraspPoseGenerator>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}

4. 动态生成抓取位姿

对于更复杂的场景，可以基于物体几何动态生成抓取位姿：

cpp

#include <moveit/grasp_generation/grasp_generator.h>

void generateDynamicGrasps(const shape_msgs::msg::SolidPrimitive & object_shape,
                          const geometry_msgs::msg::PoseStamped & object_pose)
{
    // 创建抓取生成器
    grasp_generation::GraspGenerator generator;
    
    // 设置生成器参数
    generator.setGraspAngleDelta(M_PI / 8);  // 22.5度间隔
    generator.setGraspDepth(0.05);          // 抓取深度5cm
    generator.setApproachRetreatDesiredDist(0.1);
    generator.setApproachRetreatMinDist(0.05);
    
    // 生成抓取位姿
    std::vector<moveit_msgs::msg::Grasp> grasps;
    generator.generateGrasps(object_shape, object_pose, grasps);
    
    // 可选: 过滤和排序抓取位姿
    filterGrasps(grasps);
}

5. 在MoveIt中集成抓取位姿

5.1 创建抓取动作服务器

src/grasp_action_server.cpp:

cpp

#include <rclcpp_action/rclcpp_action.hpp>
#include <control_msgs/action/grasp.hpp>

class GraspActionServer : public rclcpp::Node
{
public:
    using Grasp = control_msgs::action::Grasp;
    using GoalHandle = rclcpp_action::ServerGoalHandle<Grasp>;

    GraspActionServer() : Node("grasp_action_server")
    {
        // 创建动作服务器
        action_server_ = rclcpp_action::create_server<Grasp>(
            this,
            "grasp_action",
            std::bind(&GraspActionServer::handle_goal, this, std::placeholders::_1, std::placeholders::_2),
            std::bind(&GraspActionServer::handle_cancel, this, std::placeholders::_1),
            std::bind(&GraspActionServer::handle_accepted, this, std::placeholders::_1));
    }

private:
    rclcpp_action::Server<Grasp>::SharedPtr action_server_;
    
    rclcpp_action::GoalResponse handle_goal(
        const rclcpp_action::GoalUUID & uuid,
        std::shared_ptr<const Grasp::Goal> goal)
    {
        RCLCPP_INFO(this->get_logger(), "收到抓取目标");
        (void)uuid;
        return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
    }
    
    rclcpp_action::CancelResponse handle_cancel(
        const std::shared_ptr<GoalHandle> goal_handle)
    {
        RCLCPP_INFO(this->get_logger(), "抓取动作被取消");
        (void)goal_handle;
        return rclcpp_action::CancelResponse::ACCEPT;
    }
    
    void handle_accepted(const std::shared_ptr<GoalHandle> goal_handle)
    {
        std::thread{std::bind(&GraspActionServer::execute, this, std::placeholders::_1), goal_handle}.detach();
    }
    
    void execute(const std::shared_ptr<GoalHandle> goal_handle)
    {
        const auto goal = goal_handle->get_goal();
        auto result = std::make_shared<Grasp::Result>();
        auto feedback = std::make_shared<Grasp::Feedback>();
        
        // 1. 移动到预抓取位置
        feedback->state = "移动到预抓取位置";
        goal_handle->publish_feedback(feedback);
        
        if (!moveToPose(goal->grasp.pre_grasp_approach.desired_distance)) {
            result->success = false;
            goal_handle->abort(result);
            return;
        }
        
        // 2. 接近物体
        feedback->state = "接近物体";
        goal_handle->publish_feedback(feedback);
        
        if (!moveToPose(goal->grasp.grasp_pose)) {
            result->success = false;
            goal_handle->abort(result);
            return;
        }
        
        // 3. 执行抓取
        feedback->state = "执行抓取";
        goal_handle->publish_feedback(feedback);
        
        if (!executeGrasp(goal->grasp.grasp_posture)) {
            result->success = false;
            goal_handle->abort(result);
            return;
        }
        
        // 4. 撤退
        feedback->state = "撤退";
        goal_handle->publish_feedback(feedback);
        
        if (!moveToPose(goal->grasp.post_grasp_retreat.desired_distance)) {
            result->success = false;
            goal_handle->abort(result);
            return;
        }
        
        // 完成
        result->success = true;
        goal_handle->succeed(result);
    }
    
    bool moveToPose(const geometry_msgs::msg::PoseStamped & pose)
    {
        // 实现移动到指定位姿的逻辑
        // 使用MoveGroupInterface进行规划执行
        return true;
    }
    
    bool executeGrasp(const trajectory_msgs::msg::JointTrajectory & posture)
    {
        // 实现夹爪控制逻辑
        return true;
    }
};

int main(int argc, char * argv[])
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<GraspActionServer>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}

6. 测试抓取位姿

6.1 使用RViz测试

1. 启动MoveIt和RViz:

bash

ros2 launch your_robot_moveit_config demo.launch.py

1. 在RViz中:

   • 添加"MotionPlanning"显示

   • 添加"GraspMarker"显示(如果可用)

   • 手动设置目标位姿测试抓取

6.2 使用命令行测试

bash

# 发布测试抓取位姿
ros2 topic pub /grasp_pose geometry_msgs/PoseStamped "{
  header: {stamp: {sec: 0, nanosec: 0}, frame_id: 'world'},
  pose: {
    position: {x: 0.4, y: 0.0, z: 0.2},
    orientation: {x: 0.0, y: 0.707, z: 0.0, w: 0.707}
  }
}"

7. 高级技巧

1. 抓取位姿优化:

   • 使用Grasp Quality Metrics评估抓取质量

   • 考虑物体形状、摩擦系数和夹爪特性

2. 机器学习方法:

   • 使用GraspNet等深度学习模型生成抓取位姿

   • 集成6D姿态估计提高抓取精度

3. 力控抓取:

   • 集成力/力矩传感器反馈

   • 实现自适应抓取力控制

4. 多抓取策略:

   • 根据物体形状和场景选择最佳抓取方式

   • 实现抓取-放置任务序列

通过以上方法，你可以在MoveIt中创建有效的抓取位姿，实现可靠的机器人抓取操作。根据你的具体应用场景和硬件配置，可能需要调整参数和算法。