基于MoveIt的机械臂运动规划小demo演示

以下是一个基于MoveIt的机械臂运动规划代码示例，涵盖关节空间规划、笛卡尔路径规划、避障等关键环节。代码包含详细注释和实际应用中的注意事项：

#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit_msgs/CollisionObject.h>
#include <moveit/robot_trajectory/robot_trajectory.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

class ArmMotionPlanner {
private:
    moveit::planning_interface::MoveGroupInterface move_group_;
    moveit::planning_interface::PlanningSceneInterface planning_scene_interface_;
    const std::string PLANNING_GROUP_ = "manipulator";
    
public:
    ArmMotionPlanner() : move_group_(PLANNING_GROUP_) {
        // 初始化参数设置
        move_group_.setPlanningTime(5.0);          // 规划时间限制
        move_group_.setNumPlanningAttempts(5);     // 规划尝试次数
        move_group_.setMaxVelocityScalingFactor(0.5);  // 速度缩放因子
        move_group_.setMaxAccelerationScalingFactor(0.3);
    }

    // 基础关节空间运动规划
    bool planToJointTarget(const std::vector<double>& joint_target) {
        move_group_.setJointValueTarget(joint_target);
        
        moveit::planning_interface::MoveGroupInterface::Plan plan;
        bool success = (move_group_.plan(plan) == moveit::core::MoveItErrorCode::SUCCESS);
        
        if(success) {
            ROS_INFO("Joint space planning succeeded");
            move_group_.execute(plan);  // 实际部署建议使用异步执行
            return true;
        }
        else {
            ROS_ERROR("Joint space planning failed");
            return false;
        }
    }

    // 笛卡尔空间直线路径规划
    bool planCartesianPath(const geometry_msgs::Pose& target_pose) {
        std::vector<geometry_msgs::Pose> waypoints;
        geometry_msgs::Pose start_pose = move_group_.getCurrentPose().pose;
        
        waypoints.push_back(start_pose);  // 起始点
        waypoints.push_back(target_pose); // 目标点

        moveit_msgs::RobotTrajectory trajectory;
        const double jump_threshold = 0.0;  // 禁用跳跃检查
        const double eef_step = 0.01;       // 路径分辨率（米）
        
        double fraction = move_group_.computeCartesianPath(
            waypoints, eef_step, jump_threshold, trajectory);
            
        if(fraction >= 0.9) {  // 完成度阈值
            moveit::planning_interface::MoveGroupInterface::Plan plan;
            plan.trajectory_ = trajectory;
            move_group_.execute(plan);
            return true;
        }
        else {
            ROS_ERROR("Cartesian path planning failed (%.2f%% achieved)", fraction * 100.0);
            return false;
        }
    }

    // 带避障的规划场景设置
    void addCollisionObjects() {
        std::vector<moveit_msgs::CollisionObject> collision_objects;
        
        // 添加桌面障碍物
        moveit_msgs::CollisionObject table;
        table.id = "table";
        table.header.frame_id = move_group_.getPlanningFrame();
        
        table.primitives.resize(1);
        table.primitives[0].type = table.primitives[0].BOX;
        table.primitives[0].dimensions = {0.8, 1.5, 0.1};  // 尺寸
        
        geometry_msgs::Pose table_pose;
        table_pose.orientation.w = 1.0;
        table_pose.position.z = -0.05;  // 位于机械臂下方
        table.primitive_poses.push_back(table_pose);
        
        collision_objects.push_back(table);
        planning_scene_interface_.addCollisionObjects(collision_objects);
    }

    // 复杂场景下的运动规划
    bool advancedPlanningDemo() {
        // 步骤1：移动到观察位置
        std::vector<double> observe_pos = {0.0, -M_PI/4, 0.0, -3*M_PI/4, 0.0, M_PI/2, M_PI/4};
        if(!planToJointTarget(observe_pos)) return false;

        // 步骤2：添加环境障碍物
        addCollisionObjects();
        ros::Duration(1.0).sleep();  // 等待场景更新

        // 步骤3：设置目标位姿（示例数据）
        geometry_msgs::Pose target_pose;
        target_pose.position.x = 0.4;
        target_pose.position.y = 0.2;
        target_pose.position.z = 0.3;
        tf2::Quaternion q;
        q.setRPY(0, M_PI/2, 0);  // 末端执行器垂直向下
        target_pose.orientation = tf2::toMsg(q);

        // 步骤4：尝试笛卡尔路径规划
        if(planCartesianPath(target_pose)) {
            return true;
        }
        else {
            // 回退策略：关节空间规划
            ROS_WARN("Fallback to joint space planning");
            move_group_.setPoseTarget(target_pose);
            moveit::planning_interface::MoveGroupInterface::Plan plan;
            return (move_group_.plan(plan) == moveit::core::MoveItErrorCode::SUCCESS);
        }
    }
};

int main(int argc, char** argv) {
    ros::init(argc, argv, "arm_motion_planner");
    ros::AsyncSpinner spinner(2);
    spinner.start();
    
    ArmMotionPlanner planner;
    planner.advancedPlanningDemo();
    
    ros::waitForShutdown();
    return 0;
}

关键代码解析：

1. MoveGroup初始化：

   • 设置规划时间、尝试次数、运动速度限制
   • 使用manipulator规划组（需在SRDF中定义）

2. 关节空间规划：

   • 直接设置关节角度目标
   • 使用默认规划器生成轨迹

3. 笛卡尔路径规划：

   • 通过computeCartesianPath生成直线路径
   • 控制路径分辨率（eef_step）影响轨迹精度
   • 处理部分成功的规划结果

4. 避障场景构建：

   • 使用PlanningSceneInterface添加碰撞物体
   • 定义桌面等环境障碍物

5. 混合规划策略：

   • 优先尝试笛卡尔路径
   • 失败时回退到关节空间规划

实际应用增强建议：

1. 轨迹执行监控：

// 使用异步执行并添加完成回调
move_group_.asyncExecute(plan, [](const actionlib::SimpleClientGoalState& state) {
    if(state == actionlib::SimpleClientGoalState::SUCCEEDED) {
        ROS_INFO("Execution completed");
    } else {
        ROS_ERROR("Execution failed: %s", state.toString().c_str());
    }
});

2. 动态障碍物处理：

// 订阅点云数据并更新规划场景
void pointCloudCallback(const sensor_msgs::PointCloud2::ConstPtr& msg) {
    moveit_msgs::CollisionObject dynamic_obstacle;
    // 转换点云为碰撞物体...
    planning_scene_interface_.addCollisionObjects({dynamic_obstacle});
}

3. 运动约束配置：

// 添加末端方向约束
moveit_msgs::Constraints constraints;
moveit_msgs::OrientationConstraint ocm;
ocm.link_name = move_group_.getEndEffectorLink();
ocm.orientation = target_pose.orientation;
ocm.absolute_x_axis_tolerance = 0.1;
ocm.absolute_y_axis_tolerance = 0.1;
ocm.weight = 1.0;
constraints.orientation_constraints.push_back(ocm);
move_group_.setPathConstraints(constraints);

4. 轨迹优化配置：

// 使用STOMP优化器
move_group_.setPlannerId("STOMPConfigDefault");
move_group_.setPlanningTime(10.0);  // 需要更长的规划时间

建议将代码与以下配置文件配合使用：

1. MoveIt配置包：

   • config/ompl_planning.yaml 配置规划算法参数
   • config/kinematics.yaml 设置逆运动学求解器

2. 启动文件：

<launch>
  <include file="$(find your_robot_moveit_config)/launch/move_group.launch">
    <arg name="allow_trajectory_execution" value="true"/>
  </include>
  
  <node name="motion_planner" pkg="your_pkg" type="motion_planner_node" 
        output="screen"/>
</launch>

实际部署时需注意：

1. 规划前检查机器人状态（move_group_.getCurrentState()）
2. 处理关节限位和奇异点问题
3. 使用RobotCommander进行更底层的控制
4. 集成力传感器数据实现柔顺控制

总结

上面的代码使用了两种路径规划，基础关节空间运动规划与笛卡尔空间直线路径规划，在代码中首先利用前者（关节规划），快速接近目标，进入观察点，接着利用笛卡尔规划精确调整末端位姿，实现抓取，这是一种混合规划策略，先粗调后微调，实际开发中大多是以这种策略为基础，进行优化和改进。

以上只是一个简单的demo，在实际的工作或者实验中，需要基于多种通信模式，订阅相机话题，接收实时位姿信息，与执行器的交互可能需要使用action来异步执行任务并监控状态。在一些协作机器人项目中可能会有动态障碍进行处理，这里需要接入opencv模块，深度学习进行轨迹预测，实时监控障碍物，对路线进行实时调节。

后续会加上各种模块

大家周末愉快！