panda 物体抓取和放置

一、规避障碍物
1.1 Add include for Planning Scene Interface

添加引用头文件:

#include <moveit/planning_scene_interface/planning_scene_interface.h>

1.2 Change the Target Pose

重新设定目标位姿:

    // Set a target Pose
    auto const target_pose = [] {
        geometry_msgs::msg::Pose msg;
        msg.orientation.w = 1.0;
        msg.position.x = 0.28;
        msg.position.y = 0.4;  // <---- This value was changed
        msg.position.z = 0.5;
        return msg;
    }();
    move_group_interface.setPoseTarget(target_pose);

1.3 Create a Collision Object

在场景中建立一个障碍物模型。如果机器人连接有感知设备,则这一信息应当由感知设备提供。这段代码最后的ADD将这一模型加入碰撞检测的场景中。这段代码应该位于设定目标位姿和进行运动规划的代码之间。

    // Create collision object for the robot to avoid
    auto const collision_object = [frame_id = move_group_interface.getPlanningFrame()] // 捕获列表
    {
        moveit_msgs::msg::CollisionObject collision_object; // 构造CollisionObject消息
        
        // 设置物体属性
        collision_object.header.frame_id = frame_id;
        collision_object.id = "box1";
        
        // 定义物体形状
        shape_msgs::msg::SolidPrimitive primitive;
        primitive.type = primitive.BOX;
        primitive.dimensions.resize(3);
        primitive.dimensions[primitive.BOX_X] = 0.5; // X,Y,Z尺寸
        primitive.dimensions[primitive.BOX_Y] = 0.1;
        primitive.dimensions[primitive.BOX_Z] = 0.5;
     
        // 设置物体位姿
        geometry_msgs::msg::Pose box_pose;
        box_pose.orientation.w = 1.0;
        box_pose.position.x = 0.2;
        box_pose.position.y = 0.2;
        box_pose.position.z = 0.25;
     
            // 组合消息
        collision_object.primitives.push_back(primitive);
        collision_object.primitive_poses.push_back(box_pose);
        collision_object.operation = collision_object.ADD;
     
        return collision_object;
    }();  // 立即执行lambda

1.4 Add the Object to the Planning Scene

将以上碰撞场景模型加入到运动规划场景中:

    // Add the collision object to the scene
      // 添加碰撞障碍物到场景中
     moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
     planning_scene_interface.applyCollisionObject(collision_object);

1.5 Run the Program and Observe the Change

按照之前Visualizing RViz的步骤进行操作:首先,在 termianl0 中构建程序:

    cd ~/ws_moveit2
    source /opt/ros/humble/setup.bash
    colcon build --mixin debug

然后,在 terminal1 中打开RViz:

    cd ~/ws_moveit2
    source install/setup.bash
    ros2 launch moveit2_tutorials demo.launch.py

取消其中的MotionPlanning;

增加MarkerArray,设置其节点为/rviz_visual_tools;

增加一个RVizVisualToolGUI。

最后,在terminal 2 中运行场景设置和运动规划程序:

    cd ~/ws_moveit2
    source install/setup.bash
    ros2 run hello_moveit hello_moveit

可以看到RViz场景中加入了障碍物,并在这一前提下进行了运动规划并进行执行:

1.6 完整代码

    #include <moveit/move_group_interface/move_group_interface.hpp>
    #include <moveit/planning_scene_interface/planning_scene_interface.hpp>
    #include <moveit_visual_tools/moveit_visual_tools.h>
     
    #include <memory>
    #include <rclcpp/rclcpp.hpp>
    #include <thread>
     
    int main(int argc, char *argv[])
    {
        // Initialize ROS and create the Node
        rclcpp::init(argc, argv);
        auto const node = std::make_shared<rclcpp::Node>(
            "hello_moveit", rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true));
     
        // Create a ROS logger
        auto const logger = rclcpp::get_logger("hello_moveit");
     
        // We spin up a SingleThreadedExecutor for the current state monitor to get
        // information about the robot's state.
        rclcpp::executors::SingleThreadedExecutor executor;
        executor.add_node(node);
        auto spinner = std::thread([&executor]()
                                   { executor.spin(); });
     
        // Create the MoveIt MoveGroup Interface
        using moveit::planning_interface::MoveGroupInterface;
        auto move_group_interface = MoveGroupInterface(node, "manipulator");
     
        // Construct and initialize MoveItVisualTools
        auto moveit_visual_tools =
            moveit_visual_tools::MoveItVisualTools{node, "base_link", rviz_visual_tools::RVIZ_MARKER_TOPIC,
                                                   move_group_interface.getRobotModel()};
        moveit_visual_tools.deleteAllMarkers();
        moveit_visual_tools.loadRemoteControl();
     
        // Create a closure for updating the text in rviz
        auto const draw_title = [&moveit_visual_tools](auto text)
        {
            auto const text_pose = []
            {
                auto msg = Eigen::Isometry3d::Identity();
                msg.translation().z() = 1.0;
                return msg;
            }();
            moveit_visual_tools.publishText(text_pose, text, rviz_visual_tools::WHITE, rviz_visual_tools::XLARGE);
        };
        auto const prompt = [&moveit_visual_tools](auto text)
        { moveit_visual_tools.prompt(text); };
        auto const draw_trajectory_tool_path =
            [&moveit_visual_tools, jmg = move_group_interface.getRobotModel()->getJointModelGroup("manipulator")](
                auto const trajectory)
        { moveit_visual_tools.publishTrajectoryLine(trajectory, jmg); };
     
        // Set a target Pose with updated values !!!
        auto const target_pose = []
        {
            geometry_msgs::msg::Pose msg;
            msg.orientation.y = 0.8;
            msg.orientation.w = 0.6;
            msg.position.x = 0.1;
            msg.position.y = 0.4;
            msg.position.z = 0.4;
            return msg;
        }();
        move_group_interface.setPoseTarget(target_pose);
     
        // Create collision object for the robot to avoid
        auto const collision_object = [frame_id = move_group_interface.getPlanningFrame()] // 捕获列表
        {
            moveit_msgs::msg::CollisionObject collision_object; // 构造CollisionObject消息
            
            // 设置物体属性
            collision_object.header.frame_id = frame_id;
            collision_object.id = "box1";
            
            // 定义物体形状
            shape_msgs::msg::SolidPrimitive primitive;
            primitive.type = primitive.BOX;
            primitive.dimensions.resize(3);
            primitive.dimensions[primitive.BOX_X] = 0.5; // X,Y,Z尺寸
            primitive.dimensions[primitive.BOX_Y] = 0.1;
            primitive.dimensions[primitive.BOX_Z] = 0.5;
     
            // 设置物体位姿
            geometry_msgs::msg::Pose box_pose;
            box_pose.orientation.w = 1.0;
            box_pose.position.x = 0.2;
            box_pose.position.y = 0.2;
            box_pose.position.z = 0.25;
     
                // 组合消息
            collision_object.primitives.push_back(primitive);
            collision_object.primitive_poses.push_back(box_pose);
            collision_object.operation = collision_object.ADD;
     
            return collision_object;
        }();  // 立即执行lambda
     
        // Add the collision object to the scene
        moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
        planning_scene_interface.applyCollisionObject(collision_object);
     
        // Create a plan to that target pose
        prompt("Press 'next' in the RvizVisualToolsGui window to plan");
        draw_title("Planning");
        moveit_visual_tools.trigger(); // 强制渲染
        auto const [success, plan] = [&move_group_interface]
        {
            moveit::planning_interface::MoveGroupInterface::Plan msg;
            auto const ok = static_cast<bool>(move_group_interface.plan(msg));
            return std::make_pair(ok, msg);
        }();
     
        // Execute the plan
        if (success)
        {
            draw_trajectory_tool_path(plan.trajectory);
            moveit_visual_tools.trigger();
            prompt("Press 'next' in the RvizVisualToolsGui window to execute");
            draw_title("Executing");
            moveit_visual_tools.trigger();
            move_group_interface.execute(plan);
        }
        else
        {
            draw_title("Planning Failed!");
            moveit_visual_tools.trigger();
            RCLCPP_ERROR(logger, "Planning failed!");
        }
     
        // Shutdown ROS
        rclcpp::shutdown();
        spinner.join();
        return 0;
    }

二、panda 机械臂抓取和放置

直接展示完整代码:含详细解析 :

    /*
     * @Author: Gui林
     * @Date: 2025-04-15 13:22:22
     * @function: 实现了一个基于 MoveIt Task Constructor(MTC) 的机械臂任务规划与执行示例,
                  核心功能是创建一个包含多个规划阶段的任务,并执行该任务.
     * @FilePath: \ROS2\moveit_control.cpp
     * @LastEditTime: 2025-04-23 13:35:13
    */
    #include <rclcpp/rclcpp.hpp>
    #include <moveit/planning_scene/planning_scene.h>
    #include <moveit/planning_scene_interface/planning_scene_interface.h>
    #include <moveit/task_constructor/task.h>
    #include <moveit/task_constructor/solvers.h>
    #include <moveit/task_constructor/stages.h>
    #if __has_include(<tf2_geometry_msgs/tf2_geometry_msgs.hpp>) // 宏检测是否存在某个头文件
    #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
    #else
    #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
    #endif
    #if __has_include(<tf2_eigen/tf2_eigen.hpp>)
    #include <tf2_eigen/tf2_eigen.hpp>
    #else
    #include <tf2_eigen/tf2_eigen.h>
    #endif
     
    static const rclcpp::Logger LOGGER = rclcpp::get_logger("mtc_tutorial");
    namespace mtc = moveit::task_constructor;
     
    class MTCTaskNode
    {
    public:
        MTCTaskNode(const rclcpp::NodeOptions &options);
        rclcpp::node_interfaces::NodeBaseInterface::SharedPtr getNodeBaseInterface();
        void doTask();
        void setupPlanningScene();
     
    private:
        // Compose an MTC task from a series of stages.
        mtc::Task createTask();
        mtc::Task task_;
        rclcpp::Node::SharedPtr node_;
    };
     
    // 获取节点的基础接口:让外部代码访问 ROS 2 节点的 最小功能接口(NodeBaseInterface),而不暴露完整的 Node 对象。
    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr MTCTaskNode::getNodeBaseInterface()
    {
        return node_->get_node_base_interface();
    }
     
    MTCTaskNode::MTCTaskNode(const rclcpp::NodeOptions &options) : node_{std::make_shared<rclcpp::Node>("mtc_node", options)}
    {
    }
     
    // 规划场景设置
    void MTCTaskNode::setupPlanningScene()
    {
        moveit_msgs::msg::CollisionObject object;
        object.id = "object";
        object.header.frame_id = "world";
        object.primitives.resize(1);
        object.primitives[0].type = shape_msgs::msg::SolidPrimitive::CYLINDER;
        object.primitives[0].dimensions = {0.1, 0.02};
     
        geometry_msgs::msg::Pose pose;
        pose.position.x = 0.5;
        pose.position.y = -0.25;
        pose.orientation.w = 1.0;
        object.pose = pose;
     
        moveit::planning_interface::PlanningSceneInterface psi;
        psi.applyCollisionObject(object);
    }
     
    // 任务执行流程
    void MTCTaskNode::doTask()
    {
        task_ = createTask(); // 1.创建任务
     
        try
        {
            task_.init(); // 2.初始化任务
        }
        catch (mtc::InitStageException &e)
        {
            RCLCPP_ERROR_STREAM(LOGGER, e);
            return;
        }
     
        if (!task_.plan(5))
        { // 3.规划任务,超时5秒
            RCLCPP_ERROR_STREAM(LOGGER, "Task planning failed");
            return;
        }
        task_.introspection().publishSolution(*task_.solutions().front()); // 4.发布可视化结果
     
        auto result = task_.execute(*task_.solutions().front()); // 5.执行规划结果
        if (result.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS)
        {
            RCLCPP_ERROR_STREAM(LOGGER, "Task execution failed");
            return;
        }
     
        return;
    }
     
    // 任务构造
    mtc::Task MTCTaskNode::createTask()
    {
        mtc::Task task;
        task.stages()->setName("demo task");
        task.loadRobotModel(node_); // 加载机器人模型
     
        const auto &arm_group_name = "panda_arm";
        const auto &hand_group_name = "hand";
        const auto &hand_frame = "panda_hand";
     
        // 设置任务属性
        task.setProperty("group", arm_group_name); // 控制机械臂组
        task.setProperty("eef", hand_group_name);  // 末端执行器组
        task.setProperty("ik_frame", hand_frame);  // 逆运动学参考框架
     
    // Disable warnings for this line, as it's a variable that's set but not used in this example
    #pragma GCC diagnostic push
    #pragma GCC diagnostic ignored "-Wunused-but-set-variable"
        mtc::Stage *current_state_ptr = nullptr; // Forward current_state on to grasp pose generator
    #pragma GCC diagnostic pop
     
        // 添加任务阶段
        auto stage_state_current = std::make_unique<mtc::stages::CurrentState>("current");
        current_state_ptr = stage_state_current.get(); // 保存指针供后续使用
        task.add(std::move(stage_state_current));      // 添加当前状态阶段
     
        // 求解器
        auto sampling_planner = std::make_shared<mtc::solvers::PipelinePlanner>(node_);           // 使用 MoveIt 的规划管道,通常默认为 OMPL
        auto interpolation_planner = std::make_shared<mtc::solvers::JointInterpolationPlanner>(); // 是一个简单的规划器,它在 Start 和 Goal Joint 状态之间进行插值
     
        auto cartesian_planner = std::make_shared<mtc::solvers::CartesianPath>(); // 笛卡尔路径规划器
        cartesian_planner->setMaxVelocityScalingFactor(1.0);
        cartesian_planner->setMaxAccelerationScalingFactor(1.0);
        cartesian_planner->setStepSize(.01);
     
        // 添加“打开手爪”阶段
        auto stage_open_hand = std::make_unique<mtc::stages::MoveTo>("open hand", interpolation_planner); // 创建移动任务阶段,并指定关节空间插值规划器
        stage_open_hand->setGroup(hand_group_name);                                                       // 设置目标运动组 如 "hand" 或 "gripper"
        stage_open_hand->setGoal("open");                                                                 // 设置目标为命名位形(来自SRDF) 需预定义
        task.add(std::move(stage_open_hand));                                                             // 将阶段添加到任务流水线
     
        // Pick stages
        auto stage_move_to_pick = std::make_unique<mtc::stages::Connect>(
            "move to pick",     // Connect会尝试连接前一个阶段的结束状态和下一个阶段的开始状态
            mtc::stages::Connect::GroupPlannerVector{{arm_group_name, sampling_planner}}); // 规划器和运动组配置
        stage_move_to_pick->setTimeout(5.0);                                                                                                            // 超时时间设为 5 秒
        stage_move_to_pick->properties().configureInitFrom(mtc::Stage::PARENT);                                                                         // 继承父阶段属性
        task.add(std::move(stage_move_to_pick));                                                                                                        // 添加到任务
     
        mtc::Stage *attach_object_stage = nullptr; // 创建一个指向MoveIt Task Constructor阶段对象的指针,并将其设置为nullptr,稍后将用于保存阶段。
        {
            auto grasp = std::make_unique<mtc::SerialContainer>("pick object");
            task.properties().exposeTo(grasp->properties(), {"eef", "group", "ik_frame"});           // 将父任务(task)的指定属性 动态绑定 到子容器(grasp)的属性
            grasp->properties().configureInitFrom(mtc::Stage::PARENT, {"eef", "group", "ik_frame"}); // 初始化属性继承
     
            { // 1.创建一个MoveRelative阶段,用于接近物体
                auto stage = std::make_unique<mtc::stages::MoveRelative>("approach object", cartesian_planner); // MTC 中的相对运动阶段,沿指定方向移动一定距离
                stage->properties().set("marker_ns", "approach_object");                                        // 设置可视化标记命名空间
                stage->properties().set("link", hand_frame);                                                    // 指定运动参考坐标系
                stage->properties().configureInitFrom(mtc::Stage::PARENT, {"group"});                           // 从父容器(grasp)继承 group 属性
                stage->setMinMaxDistance(0.1, 0.15);                                                            // 规划器会在此范围内生成运动路径(例如:沿方向移动 10~15 厘米)
     
                geometry_msgs::msg::Vector3Stamped vec; // Set hand forward direction
                vec.header.frame_id = hand_frame;
                vec.vector.z = 1.0;
                stage->setDirection(vec);
                grasp->insert(std::move(stage)); // 将阶段添加到容器
            }
     
            { // 2.创建一个阶段来生成抓取姿态
                // 生成围绕目标物体(object)的多个候选抓取位姿,考虑物体几何和抓取方向
                auto stage = std::make_unique<mtc::stages::GenerateGraspPose>("generate grasp pose");
                stage->properties().configureInitFrom(mtc::Stage::PARENT); // 继承父容器属性
                stage->properties().set("marker_ns", "grasp_pose");        // RViz中显示的标记命名空间
                stage->setPreGraspPose("open");                            // 预抓取姿态(来自SRDF)
                stage->setObject("object");                                // 目标物体的ID(需与PlanningScene中一致)
                stage->setAngleDelta(M_PI / 12);                           // 旋转步长(15度)
                stage->setMonitoredStage(current_state_ptr);               // 监听当前状态
     
                // 抓取坐标系变换 (Eigen::Isometry3d)
                Eigen::Isometry3d grasp_frame_transform;
                Eigen::Quaterniond q = Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitX()) *
                                       Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitY()) *
                                       Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitZ());
                grasp_frame_transform.linear() = q.matrix();
                grasp_frame_transform.translation().z() = 0.1; // = 0.1 表示抓取点沿手爪Z轴偏移10cm(补偿物体中心到手爪的距离)
     
                // 将生成的抓取位姿通过逆运动学(IK)转换为机械臂关节空间解
                auto wrapper = std::make_unique<mtc::stages::ComputeIK>("grasp pose IK", std::move(stage));
                wrapper->setMaxIKSolutions(8);                                                   // 最大IK解数量
                wrapper->setMinSolutionDistance(1.0);                                            // 解之间的最小关节空间距离
                wrapper->setIKFrame(grasp_frame_transform, hand_frame);                          // 设置IK参考坐标系
                wrapper->properties().configureInitFrom(mtc::Stage::PARENT, {"eef", "group"});   // 继承属性
                wrapper->properties().configureInitFrom(mtc::Stage::INTERFACE, {"target_pose"}); // 从接口继承目标位姿
                grasp->insert(std::move(wrapper));                                               // 是外层的SerialContainer,此阶段会作为抓取任务的一个子阶段插入
            }
     
            { // 3.修改规划场景(ModifyPlanningScene) 的阶段,专门用于在抓取过程中允许机械手(hand)和目标物体(object)之间的碰撞
                // 创建了一个名为"allow collision (hand,object)"的ModifyPlanningScene阶段
                auto stage = std::make_unique<mtc::stages::ModifyPlanningScene>("allow collision (hand,object)");
                stage->allowCollisions("object", //目标物体指定
                                                 // 获取机械手组(hand_group_name)中所有具有碰撞几何体的连杆名称
                                        task.getRobotModel() ->getJointModelGroup(hand_group_name) ->getLinkModelNamesWithCollisionGeometry(),
                                        true);   // 参数true表示允许这些连杆与物体之间的碰撞
                grasp->insert(std::move(stage)); // 将这个阶段添加到抓取容器(grasp)中
            }
     
            { // 4.关闭手部
                auto stage = std::make_unique<mtc::stages::MoveTo>("close hand", interpolation_planner);
                stage->setGroup(hand_group_name);
                stage->setGoal("close");
                grasp->insert(std::move(stage));
            }
     
            { // 5.再次使用ModifyPlanningScene阶段,这次是将物体附加到手部,使用attachObject
                auto stage = std::make_unique<mtc::stages::ModifyPlanningScene>("attach object");
                stage->attachObject("object", hand_frame);
                attach_object_stage = stage.get();
                grasp->insert(std::move(stage));
            }
     
            { // 6.创建了一个 MoveRelative 阶段,用于在抓取物体后将其垂直提起。这是抓取操作中常见的"提起"步骤,确保物体安全离开支撑表面
                // 创建一个相对运动阶段,命名为 "lift object",使用 cartesian_planner(笛卡尔空间规划器)
                auto stage = std::make_unique<mtc::stages::MoveRelative>("lift object", cartesian_planner);
                stage->properties().configureInitFrom(mtc::Stage::PARENT, { "group" }); // 从父容器(grasp)继承 group 属性
                stage->setMinMaxDistance(0.1, 0.3); // 沿指定方向移动 10-30厘米,规划器会在此范围内寻找无碰撞的最优距离
                stage->setIKFrame(hand_frame); // 指定逆运动学计算的参考坐标系(末端执行器坐标系)如 "panda_hand"
                stage->properties().set("marker_ns", "lift_object"); // RViz 可视化标记
                
                // 运动方向定义
                geometry_msgs::msg::Vector3Stamped vec;
                vec.header.frame_id = "world";  // 参考坐标系设为世界坐标系
                vec.vector.z = 1.0;             // 沿世界坐标系的Z轴正方向(垂直向上)
                stage->setDirection(vec);
                grasp->insert(std::move(stage));
            }
            task.add(std::move(grasp));
        }
     
        // Place stages
        { // 1.创建了一个 Connect 阶段,用于规划机械臂和手爪从当前位置到放置位置的过渡运动
            auto stage_move_to_place = std::make_unique<mtc::stages::Connect>(
                "move to place",
                mtc::stages::Connect::GroupPlannerVector{{arm_group_name, sampling_planner},    // 机械臂运动组
                                                         {hand_group_name, sampling_planner}}); // 手爪运动组
            stage_move_to_place->setTimeout(5.0);  // 5秒超时
            stage_move_to_place->properties().configureInitFrom(mtc::Stage::PARENT); // 从父任务继承 eef、group 等属性
            task.add(std::move(stage_move_to_place));
        }
     
        { // 2.创建了一个 串行容器(SerialContainer) 用于组织放置物体的多个子阶段,并通过属性配置确保其继承父任务的参数
            auto place = std::make_unique<mtc::SerialContainer>("place object");
            task.properties().exposeTo(place->properties(), {"eef", "group", "ik_frame"}); // 将父任务(task)的 eef(末端执行器)、group(运动组)、ik_frame(逆运动学参考帧)属性 动态绑定 到子容器。
            place->properties().configureInitFrom(mtc::Stage::PARENT, {"eef", "group", "ik_frame"});// 从父任务 复制初始值 到子容器(仅初始化时生效,后续父任务修改不影响子容器)
            
            { // 3.创建了一个 放置位姿生成与逆运动学求解 的复合阶段,用于计算机械臂放置物体时的目标位姿和可行关节解
                auto stage = std::make_unique<mtc::stages::GeneratePlacePose>("generate place pose"); // 生成放置位姿阶段
                stage->properties().configureInitFrom(mtc::Stage::PARENT);  // 继承父容器属性
                stage->properties().set("marker_ns", "place_pose");         // RViz可视化标记的命名空间
                stage->setObject("object");                                 // 要放置的物体ID
     
                // 放置位姿设置
                geometry_msgs::msg::PoseStamped target_pose_msg;
                target_pose_msg.header.frame_id = "object";     // 相对于物体坐标系
                target_pose_msg.pose.position.y = 0.5;          // 沿物体Y轴偏移0.5米
                target_pose_msg.pose.orientation.w = 1.0;       // 无旋转(四元数单位态)
                stage->setPose(target_pose_msg);                // 设置相对位姿
                stage->setMonitoredStage(attach_object_stage);  // 确保物体仍附着在机械手上
     
                // 逆运动学求解阶段
                auto wrapper = std::make_unique<mtc::stages::ComputeIK>("place pose IK", std::move(stage)); // 将放置位姿转换为机械臂关节空间解
                wrapper->setMaxIKSolutions(2);          // 最多计算2个IK解
                wrapper->setMinSolutionDistance(1.0);   // 解之间的最小关节空间距离
                wrapper->setIKFrame("object");          // 以物体坐标系为IK参考
                wrapper->properties().configureInitFrom(mtc::Stage::PARENT, {"eef", "group"});  // 继承属性
                wrapper->properties().configureInitFrom(mtc::Stage::INTERFACE, {"target_pose"});// 从接口继承目标位姿
                place->insert(std::move(wrapper));      // 加入容器
            }
     
            { // 4.创建了一个 MoveTo 阶段,用于在放置物体后控制手爪(夹爪)张开到预定义的"open"位置
                auto stage = std::make_unique<mtc::stages::MoveTo>("open hand", interpolation_planner);
                stage->setGroup(hand_group_name);
                stage->setGoal("open");
                place->insert(std::move(stage));
            }
     
            { // 5.创建了一个 ModifyPlanningScene 阶段,用于在放置操作完成后重新禁止机械手与物体之间的碰撞检测
                auto stage = std::make_unique<mtc::stages::ModifyPlanningScene>("forbid collision"); // 创建一个修改规划场景的阶段,命名为 "forbid collision"
                stage->allowCollisions("object", // 目标物体ID
                                    // 获取机械手所有带碰撞几何的连杆名称
                                       task.getRobotModel()->getJointModelGroup(hand_group_name)->getLinkModelNamesWithCollisionGeometry(),
                                       false);  // 禁止碰撞
                place->insert(std::move(stage));// 加入容器
            }
     
            { // 6.创建了一个 ModifyPlanningScene 阶段,专门用于将已抓取的物体从机械手上分离(detach)
                auto stage = std::make_unique<mtc::stages::ModifyPlanningScene>("detach object");
                stage->detachObject("object", hand_frame); // 物体分离操作
                place->insert(std::move(stage));// 加入容器
            }
     
            { // 7.创建了一个 MoveRelative 阶段,用于在放置物体后控制机械臂沿世界坐标系X轴负方向后退一定距离,实现安全撤离
                // 创建一个相对运动阶段,命名为 "retreat"(安全撤离)规划器:使用 cartesian_planner(笛卡尔空间规划器)
                auto stage = std::make_unique<mtc::stages::MoveRelative>("retreat", cartesian_planner);
                stage->properties().configureInitFrom(mtc::Stage::PARENT, {"group"}); // 从父容器(place)继承 group 属性(如 "panda_arm")
                stage->setMinMaxDistance(0.1, 0.3); // 沿指定方向移动 10-30厘米
                stage->setIKFrame(hand_frame);      // 逆运动学参考坐标系
                stage->properties().set("marker_ns", "retreat"); //  RViz可视化标记
     
                // 运动方向定义
                geometry_msgs::msg::Vector3Stamped vec;
                vec.header.frame_id = "world";  // 参考坐标系设为世界坐标系 为什么用世界坐标系? 确保撤离方向固定(通常为机械臂基座的后方),不受机械手当前姿态影响
                vec.vector.x = -0.5;            // 沿世界坐标系X轴负方向
                stage->setDirection(vec);
                place->insert(std::move(stage));
            }
     
            task.add(std::move(place));
        }
     
        { // 8.创建了一个 MoveTo 阶段,用于控制机械臂返回预定义的"ready"(就绪)位置,通常用于任务结束后的复位操作
            auto stage = std::make_unique<mtc::stages::MoveTo>("return home", interpolation_planner);
            stage->properties().configureInitFrom(mtc::Stage::PARENT, {"group"});
            stage->setGoal("ready");
            task.add(std::move(stage));
        }
     
        return task;
    }
     
    int main(int argc, char **argv)
    {
        rclcpp::init(argc, argv);
     
        // 作用:启用节点参数的自动声明机制,允许通过构造函数参数或命令行参数直接覆盖参数值,而无需预先声明
        rclcpp::NodeOptions options;
        options.automatically_declare_parameters_from_overrides(true);
     
        auto mtc_task_node = std::make_shared<MTCTaskNode>(options);
        rclcpp::executors::MultiThreadedExecutor executor;
     
        auto spin_thread = std::make_unique<std::thread>([&executor, &mtc_task_node]()
                                                         {
                                                             executor.add_node(mtc_task_node->getNodeBaseInterface());    // 添加节点到执行器
                                                             executor.spin();                                             // 启动执行器
                                                             executor.remove_node(mtc_task_node->getNodeBaseInterface()); // 清理节点
                                                         });
     
        mtc_task_node->setupPlanningScene(); // 设置场景
        mtc_task_node->doTask();             // 执行任务
     
        spin_thread->join(); // 等待 spin_thread 线程结束
        rclcpp::shutdown();  // 关闭 ROS 2 节点
        return 0;
    }

2.1 下载 MoveIt 任务构造函数

    cd ~/ws_moveit2/src
    git clone https://github.com/ros-planning/moveit_task_constructor.git -b humble

 创建新包:

ros2 pkg create mtc_tutorial --build-type ament_cmake --dependencies moveit_task_constructor_core rclcpp --node-name mtc_tutorial

添加依赖:

    <?xml version="1.0"?>
    <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
    <package format="3">
    <name>mtc_tutorial</name>
    <version>0.0.0</version>
    <description>TODO: Package description</description>
    <maintainer email="youremail@domain.com">user</maintainer>
    <license>TODO: License declaration</license>
     
    <buildtool_depend>ament_cmake</buildtool_depend>
     
    <depend>moveit_task_constructor_core</depend>
    <depend>rclcpp</depend>
     
    <test_depend>ament_lint_auto</test_depend>
    <test_depend>ament_lint_common</test_depend>
     
    <export>
        <build_type>ament_cmake</build_type>
    </export>
    </package>

    cmake_minimum_required(VERSION 3.8)
    project(mtc_tutorial)
     
    if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
    add_compile_options(-Wall -Wextra -Wpedantic)
    endif()
     
    # find dependencies
    find_package(ament_cmake REQUIRED)
    find_package(moveit_task_constructor_core REQUIRED)
    find_package(rclcpp REQUIRED)
    # uncomment the following section in order to fill in
    # further dependencies manually.
    # find_package(<dependency> REQUIRED)
     
    add_executable(mtc_tutorial src/mtc_tutorial.cpp)
    ament_target_dependencies(mtc_tutorial moveit_task_constructor_core rclcpp)
    target_include_directories(mtc_tutorial PUBLIC
    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
    $<INSTALL_INTERFACE:include>)
    target_compile_features(mtc_tutorial PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17
     
    install(TARGETS mtc_tutorial
    DESTINATION lib/${PROJECT_NAME})
     
    if(BUILD_TESTING)
    find_package(ament_lint_auto REQUIRED)
    # the following line skips the linter which checks for copyrights
    # uncomment the line when a copyright and license is not present in all source files
    #set(ament_cmake_copyright_FOUND TRUE)
    # the following line skips cpplint (only works in a git repo)
    # uncomment the line when this package is not in a git repo
    #set(ament_cmake_cpplint_FOUND TRUE)
    ament_lint_auto_find_test_dependencies()
    endif()
     
    ament_package()

2.2 使用 MoveIt 任务构造函数设置项目

在您选择的编辑器中打开,然后粘贴以下代码。mtc_tutorial.cpp

    #include <rclcpp/rclcpp.hpp>
    #include <moveit/planning_scene/planning_scene.h>
    #include <moveit/planning_scene_interface/planning_scene_interface.h>
    #include <moveit/task_constructor/task.h>
    #include <moveit/task_constructor/solvers.h>
    #include <moveit/task_constructor/stages.h>
    #if __has_include(<tf2_geometry_msgs/tf2_geometry_msgs.hpp>)
    #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
    #else
    #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
    #endif
    #if __has_include(<tf2_eigen/tf2_eigen.hpp>)
    #include <tf2_eigen/tf2_eigen.hpp>
    #else
    #include <tf2_eigen/tf2_eigen.h>
    #endif
     
    static const rclcpp::Logger LOGGER = rclcpp::get_logger("mtc_tutorial");
    namespace mtc = moveit::task_constructor;
     
    class MTCTaskNode
    {
    public:
      MTCTaskNode(const rclcpp::NodeOptions& options);
     
      rclcpp::node_interfaces::NodeBaseInterface::SharedPtr getNodeBaseInterface();
     
      void doTask();
     
      void setupPlanningScene();
     
    private:
      // Compose an MTC task from a series of stages.
      mtc::Task createTask();
      mtc::Task task_;
      rclcpp::Node::SharedPtr node_;
    };
     
    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr MTCTaskNode::getNodeBaseInterface()
    {
      return node_->get_node_base_interface();
    }
     
    MTCTaskNode::MTCTaskNode(const rclcpp::NodeOptions& options)
      : node_{ std::make_shared<rclcpp::Node>("mtc_node", options) }
    {
    }
     
    void MTCTaskNode::setupPlanningScene()
    {
      moveit_msgs::msg::CollisionObject object;
      object.id = "object";
      object.header.frame_id = "world";
      object.primitives.resize(1);
      object.primitives[0].type = shape_msgs::msg::SolidPrimitive::CYLINDER;
      object.primitives[0].dimensions = { 0.1, 0.02 };
     
      geometry_msgs::msg::Pose pose;
      pose.position.x = 0.5;
      pose.position.y = -0.25;
      pose.orientation.w = 1.0;
      object.pose = pose;
     
      moveit::planning_interface::PlanningSceneInterface psi;
      psi.applyCollisionObject(object);
    }
     
    void MTCTaskNode::doTask()
    {
      task_ = createTask();
     
      try
      {
        task_.init();
      }
      catch (mtc::InitStageException& e)
      {
        RCLCPP_ERROR_STREAM(LOGGER, e);
        return;
      }
     
      if (!task_.plan(5))
      {
        RCLCPP_ERROR_STREAM(LOGGER, "Task planning failed");
        return;
      }
      task_.introspection().publishSolution(*task_.solutions().front());
     
      auto result = task_.execute(*task_.solutions().front());
      if (result.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS)
      {
        RCLCPP_ERROR_STREAM(LOGGER, "Task execution failed");
        return;
      }
     
      return;
    }
     
    mtc::Task MTCTaskNode::createTask()
    {
      mtc::Task task;
      task.stages()->setName("demo task");
      task.loadRobotModel(node_);
     
      const auto& arm_group_name = "panda_arm";
      const auto& hand_group_name = "hand";
      const auto& hand_frame = "panda_hand";
     
      // Set task properties
      task.setProperty("group", arm_group_name);
      task.setProperty("eef", hand_group_name);
      task.setProperty("ik_frame", hand_frame);
     
    // Disable warnings for this line, as it's a variable that's set but not used in this example
    #pragma GCC diagnostic push
    #pragma GCC diagnostic ignored "-Wunused-but-set-variable"
      mtc::Stage* current_state_ptr = nullptr;  // Forward current_state on to grasp pose generator
    #pragma GCC diagnostic pop
     
      auto stage_state_current = std::make_unique<mtc::stages::CurrentState>("current");
      current_state_ptr = stage_state_current.get();
      task.add(std::move(stage_state_current));
     
      auto sampling_planner = std::make_shared<mtc::solvers::PipelinePlanner>(node_);
      auto interpolation_planner = std::make_shared<mtc::solvers::JointInterpolationPlanner>();
     
      auto cartesian_planner = std::make_shared<mtc::solvers::CartesianPath>();
      cartesian_planner->setMaxVelocityScalingFactor(1.0);
      cartesian_planner->setMaxAccelerationScalingFactor(1.0);
      cartesian_planner->setStepSize(.01);
     
      auto stage_open_hand =
          std::make_unique<mtc::stages::MoveTo>("open hand", interpolation_planner);
      stage_open_hand->setGroup(hand_group_name);
      stage_open_hand->setGoal("open");
      task.add(std::move(stage_open_hand));
     
      return task;
    }
     
    int main(int argc, char** argv)
    {
      rclcpp::init(argc, argv);
     
      rclcpp::NodeOptions options;
      options.automatically_declare_parameters_from_overrides(true);
     
      auto mtc_task_node = std::make_shared<MTCTaskNode>(options);
      rclcpp::executors::MultiThreadedExecutor executor;
     
      auto spin_thread = std::make_unique<std::thread>([&executor, &mtc_task_node]() {
        executor.add_node(mtc_task_node->getNodeBaseInterface());
        executor.spin();
        executor.remove_node(mtc_task_node->getNodeBaseInterface());
      });
     
      mtc_task_node->setupPlanningScene();
      mtc_task_node->doTask();
     
      spin_thread->join();
      rclcpp::shutdown();
      return 0;
    }

代码解释:

        1.代码的顶部包括此包使用的 ROS 和 MoveIt 库。

    rclcpp/rclcpp.hpp包括核心 ROS2 功能
    moveit/planning_scene/planning_scene.h并包括与 Robot 模型和碰撞对象交互的功能moveit/planning_scene_interface/planning_scene_interface.h
    moveit/task_constructor/task.h、 和 ,并包括示例中使用的 MoveIt 任务构造函数的不同组件moveit/task_constructor/solvers.hmoveit/task_constructor/stages.h
    tf2_geometry_msgs/tf2_geometry_msgs.hpp,并且不会在此初始示例中使用,但当我们向 MoveIt Task Constructor 任务添加更多阶段时,它们将用于姿势生成。tf2_eigen/tf2_eigen.hpp

        2.下一行获取新节点的日志( Logger)。为了方便起见,我们还为moveit::task_constructor创建了一个命名空间别名。

    #include <rclcpp/rclcpp.hpp>
    #include <moveit/planning_scene/planning_scene.h>
    #include <moveit/planning_scene_interface/planning_scene_interface.h>
    #include <moveit/task_constructor/task.h>
    #include <moveit/task_constructor/solvers.h>
    #include <moveit/task_constructor/stages.h>
    #if __has_include(<tf2_geometry_msgs/tf2_geometry_msgs.hpp>)
    #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
    #else
    #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
    #endif
    #if __has_include(<tf2_eigen/tf2_eigen.hpp>)
    #include <tf2_eigen/tf2_eigen.hpp>
    #else
    #include <tf2_eigen/tf2_eigen.h>
    #endif
     
    static const rclcpp::Logger LOGGER = rclcpp::get_logger("mtc_tutorial");
    namespace mtc = moveit::task_constructor;

        3.定义一个类,该类将包含主要的 MoveIt Task Constructor 功能。

    class MTCTaskNode
    {
    public:
      MTCTaskNode(const rclcpp::NodeOptions& options);
     
      rclcpp::node_interfaces::NodeBaseInterface::SharedPtr getNodeBaseInterface();
     
      void doTask();
     
      void setupPlanningScene();
     
    private:
      // Compose an MTC task from a series of stages.
      mtc::Task createTask();
      mtc::Task task_;
      rclcpp::Node::SharedPtr node_;
    };

        4.获取 node 的基本接口,该接口稍后将用于 executor。

    rclcpp::node_interfaces::NodeBaseInterface::SharedPtr MTCTaskNode::getNodeBaseInterface()
    {
      return node_->get_node_base_interface();
    }

        5.初始化节点。

    MTCTaskNode::MTCTaskNode(const rclcpp::NodeOptions& options)
      : node_{ std::make_shared<rclcpp::Node>("mtc_node", options) }
    {
    }

        6.设置示例中使用的规划场景。它将创建一个尺寸由object.primitives[0].dimensions设置,pose.position.z和pose.position.x  指定位置的圆柱体。您可以尝试更改这些数字以调整圆柱体的大小并四处移动。如果将圆柱体移到机器人够不到的地方,规划就会失败。

    void MTCTaskNode::setupPlanningScene()
    {
      moveit_msgs::msg::CollisionObject object;
      object.id = "object";
      object.header.frame_id = "world";
      object.primitives.resize(1);
      object.primitives[0].type = shape_msgs::msg::SolidPrimitive::CYLINDER;
      object.primitives[0].dimensions = { 0.1, 0.02 };
     
      geometry_msgs::msg::Pose pose;
      pose.position.x = 0.5;
      pose.position.y = -0.25;
      object.pose = pose;
     
      moveit::planning_interface::PlanningSceneInterface psi;
      psi.applyCollisionObject(object);
    }

        7.此函数与 MoveIt Task Constructor 任务对象交互。它首先创建一个任务,其中包括设置一些属性和添加阶段。

    void MTCTaskNode::doTask()
    {
      task_ = createTask();
     
      try
      {
        task_.init();
      }
      catch (mtc::InitStageException& e)
      {
        RCLCPP_ERROR_STREAM(LOGGER, e);
        return;
      }
     
      if (!task_.plan(5))
      {
        RCLCPP_ERROR_STREAM(LOGGER, "Task planning failed");
        return;
      }
      task_.introspection().publishSolution(*task_.solutions().front());
     
      auto result = task_.execute(*task_.solutions().front());
      if (result.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS)
      {
        RCLCPP_ERROR_STREAM(LOGGER, "Task execution failed");
        return;
      }
     
      return;
    }

        8.此函数创建一个 MoveIt Task Constructor 对象并设置一些初始属性。我们将任务名称设置为 “demo_task”。

    mtc::Task MTCTaskNode::createTask()
    {
      moveit::task_constructor::Task task;
      task.stages()->setName("demo task");
      task.loadRobotModel(node_);
     
      const auto& arm_group_name = "panda_arm";
      const auto& hand_group_name = "hand";
      const auto& hand_frame = "panda_hand";
     
      // Set task properties
      task.setProperty("group", arm_group_name);
      task.setProperty("eef", hand_group_name);
      task.setProperty("ik_frame", hand_frame);

        9.将示例阶段添加到节点。创建一个阶段(生成器阶段)并将其添加到我们的任务中,这将以当前状态启动机器人。

    mtc::Stage* current_state_ptr = nullptr;  // Forward current_state on to grasp pose generator
    auto stage_state_current = std::make_unique<mtc::stages::CurrentState>("current");
    current_state_ptr = stage_state_current.get();
    task.add(std::move(stage_state_current));

        10.为了规划机器人运动,我们需要指定一个求解器。MoveIt Task Constructor 有三个求解器选项:

    1.PipelinePlanner    使用 MoveIt 的规划管道,通常默认为 OMPL。
    2.CartesianPath      用于在笛卡尔空间中沿直线移动末端效应器。
    3.JointInterpolation 是一个简单的规划器,它在 Start 和 Goal Joint 状态之间进行插值。它通常用于简单运动,因为它计算速度快,但不支持复杂运动。

        11.为 Cartesian planner 设置特定的属性。

    auto sampling_planner = std::make_shared<mtc::solvers::PipelinePlanner>(node_);
    auto interpolation_planner = std::make_shared<mtc::solvers::JointInterpolationPlanner>();
     
    auto cartesian_planner = std::make_shared<mtc::solvers::CartesianPath>();
    cartesian_planner->setMaxVelocityScalingFactor(1.0);
    cartesian_planner->setMaxAccelerationScalingFactor(1.0);
    cartesian_planner->setStepSize(.01);

        12.添加夹爪控制,由于打开是一个相对简单的动作,我们可以使用 joint interpolation planner。此阶段计划移动到“打开”姿势,这是在 SRDF 中为 panda 机器人定义的命名姿势。

     auto stage_open_hand =
          std::make_unique<mtc::stages::MoveTo>("open hand", interpolation_planner);
      stage_open_hand->setGroup(hand_group_name);
      stage_open_hand->setGoal("open");
      task.add(std::move(stage_open_hand));
     
      return task;
    }

        13.最后,使用上面定义的类创建一个节点,并调用类方法来设置和执行基本的 MTC 任务。在此示例中,任务执行完毕后,我们不会取消执行程序,以保持节点处于活动状态以检查 RViz 中的解决方案。

    int main(int argc, char** argv)
    {
      rclcpp::init(argc, argv);
     
      rclcpp::NodeOptions options;
      options.automatically_declare_parameters_from_overrides(true);
     
      auto mtc_task_node = std::make_shared<MTCTaskNode>(options);
      rclcpp::executors::MultiThreadedExecutor executor;
     
      auto spin_thread = std::make_unique<std::thread>([&executor, &mtc_task_node]() {
        executor.add_node(mtc_task_node->getNodeBaseInterface());
        executor.spin();
        executor.remove_node(mtc_task_node->getNodeBaseInterface());
      });
     
      mtc_task_node->setupPlanningScene();
      mtc_task_node->doTask();
     
      spin_thread->join();
      rclcpp::shutdown();
      return 0;
    }

2.3 运行 Demo

        需要创建一个启动文件来启动move_group、ros2_control、static_tf、robot_state_publisher和rviz等节点。同时,还需要一个启动文件来启动mtc_tutorial可执行文件,并加载URDF、SRDF和OMPL参数。

    from launch import LaunchDescription
    from launch_ros.actions import Node
    from moveit_configs_utils import MoveItConfigsBuilder
     
    def generate_launch_description():
        moveit_config = MoveItConfigsBuilder("moveit_resources_panda").to_dict()
     
        # MTC Demo node
        pick_place_demo = Node(
            package="mtc_tutorial",
            executable="mtc_tutorial",
            output="screen",
            parameters=[
                moveit_config,
            ],
        )
     
        return LaunchDescription([pick_place_demo])

将这个文件保存为pick_place_demo.launch.py,并确保在CMakeLists.txt中添加以下行以正确安装启动文件:

install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})

构建工作空间:

    cd ~/ws_moveit2
    colcon build --mixin release
    source ~/ws_moveit2/install/setup.bash

启动第一个启动文件:

ros2 launch moveit2_tutorials mtc_demo.launch.py

启动mtc_tutorial节点:

ros2 launch mtc_tutorial pick_place_demo.launch.py

您应该看到机械臂执行任务,使用单个阶段打开手部,其前面是绿色的圆柱体。它应如下所示:

如果您还没有创建自己的包,但仍想查看它是什么样子,您可以从教程中启动此文件:

ros2 launch moveit2_tutorials mtc_demo_minimal.launch.py

2.4 添加 pick-and-place 任务

        1.我们需要移动机械臂到可以抓取物体的位置。这通过阶段实现,它尝试连接前一个阶段和后一个阶段的结果。这个阶段初始化时需要一个名称和一个GroupPlannerVector,用于指定规划组和规划器。然后,设置阶段的超时时间、属性,并将其添加到任务中。

    auto stage_move_to_pick = std::make_unique<mtc::stages::Connect>(
        "move to pick",
        mtc::stages::Connect::GroupPlannerVector{ { arm_group_name, sampling_planner } });
    stage_move_to_pick->setTimeout(5.0);
    stage_move_to_pick->properties().configureInitFrom(mtc::Stage::PARENT);
    task.add(std::move(stage_move_to_pick));

        2.接下来,创建一个指向MoveIt Task Constructor阶段对象的指针,并将其设置为nullptr,稍后将用于保存阶段。

mtc::Stage* attach_object_stage = nullptr;  // Forward attach_object_stage to place pose generator

        3.创建一个SerialContainer,它可以添加到任务中,并可以包含多个子阶段。在这个例子中,创建一个包含抓取动作相关阶段的SerialContainer。使用exposeTo声明父任务的属性,并使用configureInitFrom初始化它们,以便子阶段可以访问这些属性。

    {
      auto grasp = std::make_unique<mtc::SerialContainer>("pick object");
      task.properties().exposeTo(grasp->properties(), { "eef", "group", "ik_frame" });
      grasp->properties().configureInitFrom(mtc::Stage::PARENT, { "eef", "group", "ik_frame" });

        4.创建一个MoveRelative阶段,用于接近物体。这个阶段是一个传播阶段,它接收相邻阶段的解决方案并传播到下一个或上一个阶段。使用cartesian_planner找到一个涉及末端执行器直线运动的解决方案。设置属性,并设置最小和最大移动距离。然后,创建一个Vector3Stamped消息,指示我们想要移动的方向——在这个例子中,是从手部框架的Z方向。最后,将这个阶段添加到SerialContainer中。

    {
      auto stage = std::make_unique<mtc::stages::MoveRelative>("approach object", cartesian_planner);
      stage->properties().set("marker_ns", "approach_object");
      stage->properties().set("link", hand_frame);
      stage->properties().configureInitFrom(mtc::Stage::PARENT, { "group" });
      stage->setMinMaxDistance(0.1, 0.15);
     
      // Set hand forward direction
      geometry_msgs::msg::Vector3Stamped vec;
      vec.header.frame_id = hand_frame;
      vec.vector.z = 1.0;
      stage->setDirection(vec);
      grasp->insert(std::move(stage));
    }

        5.创建一个阶段来生成抓取姿态。这是一个生成阶段,因此它在不考虑前一个和后一个阶段的情况下计算结果。第一个阶段CurrentState也是一个生成阶段——为了连接第一个阶段和这个阶段,必须使用连接阶段,我们已经在上面创建了。这段代码设置阶段属性,设置抓取前的姿态、角度增量和监控阶段。角度增量是GenerateGraspPose阶段的一个属性,用于确定要生成的姿态数量;在生成解决方案时,MoveIt Task Constructor将尝试从许多不同方向抓取物体,方向之间的差异由角度增量指定。角度增量越小,抓取方向就越接近。在定义当前阶段时,我们设置了current_state_ptr,现在用于将物体姿态和形状的信息传递给逆运动学求解器。这个阶段不会像以前那样直接添加到SerialContainer中,因为我们还需要对它生成的姿态进行逆运动学计算。

    {
      // Sample grasp pose
      auto stage = std::make_unique<mtc::stages::GenerateGraspPose>("generate grasp pose");
      stage->properties().configureInitFrom(mtc::Stage::PARENT);
      stage->properties().set("marker_ns", "grasp_pose");
      stage->setPreGraspPose("open");
      stage->setObject("object");
      stage->setAngleDelta(M_PI / 12);
      stage->setMonitoredStage(current_state_ptr);  // Hook into current state

        6.在计算上面生成的姿态的逆运动学之前,我们首先需要定义框架。这可以通过geometry_msgs中的PoseStamped消息完成,或者在这个例子中,我们使用Eigen变换矩阵和相关链接的名称来定义变换矩阵。

    Eigen::Isometry3d grasp_frame_transform;
    Eigen::Quaterniond q = Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitX()) *
                          Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitY()) *
                          Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d::UnitZ());
    grasp_frame_transform.linear() = q.matrix();
    grasp_frame_transform.translation().z() = 0.1;

        7.现在,创建ComputeIK阶段,并给它命名为grasp pose IK,以及上面定义的generate grasp pose阶段。有些机器人对于给定姿态有多个逆运动学解——我们设置最多解算8个解。我们还设置最小解距离,这是对解必须不同的一个阈值:如果解的关节位置与前一个解太相似,它将被标记为无效。接下来,配置一些额外的属性,并将ComputeIK阶段添加到SerialContainer中。

      // Compute IK
      auto wrapper = std::make_unique<mtc::stages::ComputeIK>("grasp pose IK", std::move(stage));
      wrapper->setMaxIKSolutions(8);
      wrapper->setMinSolutionDistance(1.0);
      wrapper->setIKFrame(grasp_frame_transform, hand_frame);
      wrapper->properties().configureInitFrom(mtc::Stage::PARENT, { "eef", "group" });
      wrapper->properties().configureInitFrom(mtc::Stage::INTERFACE, { "target_pose" });
      grasp->insert(std::move(wrapper));
    }

        8.为了抓取物体,我们必须允许手部与物体之间的碰撞。这可以通过ModifyPlanningScene阶段完成。allowCollisions函数让我们指定要禁用的碰撞。allowCollisions可以与容器名称一起使用,因此我们可以使用getLinkModelNamesWithCollisionGeometry获取手部组中所有具有碰撞几何形状的链接名称。

    {
      auto stage = std::make_unique<mtc::stages::ModifyPlanningScene>("allow collision (hand,object)");
      stage->allowCollisions("object",
                            task.getRobotModel()
                                ->getJointModelGroup(hand_group_name)
                                ->getLinkModelNamesWithCollisionGeometry(),
                            true);
      grasp->insert(std::move(stage));
    }

        9.允许碰撞后,我们现在可以关闭手部。这通过MoveTo阶段完成,类似于上面的open hand阶段,只是移动到SRDF中定义的close位置。

    {
      auto stage = std::make_unique<mtc::stages::MoveTo>("close hand", interpolation_planner);
      stage->setGroup(hand_group_name);
      stage->setGoal("close");
      grasp->insert(std::move(stage));
    }

        10.现在再次使用ModifyPlanningScene阶段,这次是将物体附加到手部,使用attachObject。与current_state_ptr类似,我们获取这个阶段的指针,稍后在生成物体放置姿态时使用。

    {
      auto stage = std::make_unique<mtc::stages::ModifyPlanningScene>("attach object");
      stage->attachObject("object", hand_frame);
      attach_object_stage = stage.get();
      grasp->insert(std::move(stage));
    }

        11.接下来,使用MoveRelative阶段提起物体,类似于approach_object阶段。

    {
      auto stage = std::make_unique<mtc::stages::MoveRelative>("lift object", cartesian_planner);
      stage->properties().configureInitFrom(mtc::Stage::PARENT, { "group" });
      stage->setMinMaxDistance(0.1, 0.3);
      stage->setIKFrame(hand_frame);
      stage->properties().set("marker_ns", "lift_object");
     
      // Set upward direction
      geometry_msgs::msg::Vector3Stamped vec;
      vec.header.frame_id = "world";
      vec.vector.z = 1.0;
      stage->setDirection(vec);
      grasp->insert(std::move(stage));
    }

        12.至此,我们已经完成了抓取物体所需的所有阶段。现在,将包含所有子阶段的SerialContainer添加到任务中。如果按照当前包构建,可以看到机器人计划抓取物体。

      task.add(std::move(grasp));
    }

2.5 放置阶段

        1.现在抓取阶段已经完成,我们继续定义放置物体的阶段。我们首先使用Connect阶段连接两者,因为我们很快将使用生成阶段来生成放置物体的姿态。

    {
      auto stage_move_to_place = std::make_unique<mtc::stages::Connect>(
          "move to place",
          mtc::stages::Connect::GroupPlannerVector{ { arm_group_name, sampling_planner },
                                                    { hand_group_name, sampling_planner } });
      stage_move_to_place->setTimeout(5.0);
      stage_move_to_place->properties().configureInitFrom(mtc::Stage::PARENT);
      task.add(std::move(stage_move_to_place));
    }

        2.我们还为放置阶段创建了一个SerialContainer。这与抓取SerialContainer的创建方式类似。接下来的阶段将添加到SerialContainer而不是任务中。

    {
      auto place = std::make_unique<mtc::SerialContainer>("place object");
      task.properties().exposeTo(place->properties(), { "eef", "group", "ik_frame" });
      place->properties().configureInitFrom(mtc::Stage::PARENT, { "eef", "group", "ik_frame" });

        3.接下来的阶段生成用于放置物体的姿态,并为这些姿态计算逆运动学——它与抓取SerialContainer中的generate grasp pose阶段有些相似。我们首先创建一个阶段来生成姿态并继承任务属性。我们使用geometry_msgs中的PoseStamped消息指定我们想要放置物体的位置——在这个例子中,我们在"object"框架中选择y = 0.5。然后,我们使用setPose将目标姿态传递给阶段。接下来,我们使用setMonitoredStage并传递attach_object阶段的指针。这允许阶段知道物体是如何附加的。然后,我们创建一个ComputeIK阶段并将我们的GeneratePlacePose阶段传递给它——其余的逻辑与抓取阶段相同。

    {
      // Sample place pose
      auto stage = std::make_unique<mtc::stages::GeneratePlacePose>("generate place pose");
      stage->properties().configureInitFrom(mtc::Stage::PARENT);
      stage->properties().set("marker_ns", "place_pose");
      stage->setObject("object");
     
      geometry_msgs::msg::PoseStamped target_pose_msg;
      target_pose_msg.header.frame_id = "object";
      target_pose_msg.pose.position.y = 0.5;
      target_pose_msg.pose.orientation.w = 1.0;
      stage->setPose(target_pose_msg);
      stage->setMonitoredStage(attach_object_stage);  // Hook into attach_object_stage
     
      // Compute IK
      auto wrapper = std::make_unique<mtc::stages::ComputeIK>("place pose IK", std::move(stage));
      wrapper->setMaxIKSolutions(2);
      wrapper->setMinSolutionDistance(1.0);
      wrapper->setIKFrame("object");
      wrapper->properties().configureInitFrom(mtc::Stage::PARENT, { "eef", "group" });
      wrapper->properties().configureInitFrom(mtc::Stage::INTERFACE, { "target_pose" });
      place->insert(std::move(wrapper));
    }

        4.现在我们准备放置物体,我们使用MoveTo阶段和关节插值规划器打开手部。

    {
      auto stage = std::make_unique<mtc::stages::MoveTo>("open hand", interpolation_planner);
      stage->setGroup(hand_group_name);
      stage->setGoal("open");
      place->insert(std::move(stage));
    }

        5.现在我们可以重新启用与物体的碰撞,因为我们不再需要持有它。这通过allowCollisions几乎完全相同的方式完成,只是将最后一个参数设置为false而不是true。

    {
      auto stage = std::make_unique<mtc::stages::ModifyPlanningScene>("forbid collision (hand,object)");
      stage->allowCollisions("object",
                            task.getRobotModel()
                                ->getJointModelGroup(hand_group_name)
                                ->getLinkModelNamesWithCollisionGeometry(),
                            false);
      place->insert(std::move(stage));
    }

        6.现在,我们可以使用detachObject分离物体。

    {
      auto stage = std::make_unique<mtc::stages::ModifyPlanningScene>("detach object");
      stage->detachObject("object", hand_frame);
      place->insert(std::move(stage));
    }

        10.我们使用MoveRelative阶段从物体后退,这与approach object和lift object阶段类似。

    {
      auto stage = std::make_unique<mtc::stages::MoveRelative>("retreat", cartesian_planner);
      stage->properties().configureInitFrom(mtc::Stage::PARENT, { "group" });
      stage->setMinMaxDistance(0.1, 0.3);
      stage->setIKFrame(hand_frame);
      stage->properties().set("marker_ns", "retreat");
     
      // Set retreat direction
      geometry_msgs::msg::Vector3Stamped vec;
      vec.header.frame_id = "world";
      vec.vector.x = -0.5;
      stage->setDirection(vec);
      place->insert(std::move(stage));
    }

        11.我们完成放置SerialContainer并将其添加到任务中。

      task.add(std::move(place));
    }

        12.最后一步是返回初始位置:我们使用MoveTo阶段并传递目标姿态ready,这是在panda SRDF中定义的一个姿态。

    {
      auto stage = std::make_unique<mtc::stages::MoveTo>("return home", interpolation_planner);
      stage->properties().configureInitFrom(mtc::Stage::PARENT, { "group" });
      stage->setGoal("ready");
      task.add(std::move(stage));
    }

        13.所有这些阶段都应该添加到这些行的上方。

      // Stages all added to the task above this line
     
      return task;
    }

        现在已经使用MoveIt Task Constructor定义了一个抓取和放置任务!
2.6 运行

在第一个终端中:

ros2 launch moveit2_tutorials mtc_demo.launch.py

在第二个终端中:

ros2 launch moveit2_tutorials pick_place_demo.launch.py
                       
 

评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值