当前位置： 首页 > article >正文

MTC完成右臂抓取放置任务\\放置姿态设置

article 2024/11/10 17:48:05

MT

#include "mtc_tutorial/mtc_glass_bottle.hpp"
static const rclcpp::Logger LOGGER = rclcpp::get_logger("mtc_glass_right"); 

// 获取节点基础接口的实现
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr MTCTaskNode_Right::getNodeBaseInterface()
{
  return node_->get_node_base_interface();
}

// 构造函数的实现，创建节点对象
MTCTaskNode_Right::MTCTaskNode_Right(const rclcpp::NodeOptions& options)
  : node_{ std::make_shared<rclcpp::Node>("mtc_right_node", options) }  // 初始化节点名称为 "mtc_node"
{
}

// 设置规划场景的实现
void MTCTaskNode_Right::setupPlanningScene()
{
  const double table_height = 1.02;
  std::string frame_id = "world";

  geometry_msgs::msg::PoseStamped bottlePose;
  bottlePose.header.frame_id = frame_id;
  bottlePose.pose.position.x = 0.3;
  bottlePose.pose.position.y = -0.5;
  bottlePose.pose.position.z = table_height;
  bottlePose.pose.orientation.w = 1.0;

  geometry_msgs::msg::PoseStamped glassPose;
  glassPose.header.frame_id = frame_id;
  glassPose.pose.position.x = -0.6;
  glassPose.pose.position.y = -0.5;
  glassPose.pose.position.z = table_height;
  glassPose.pose.orientation.w = 1.0;

  // center of table surface 
  geometry_msgs::msg::PoseStamped tabletopPose;
  tabletopPose.header.frame_id = frame_id;
  tabletopPose.pose.position.x = 0;
  tabletopPose.pose.position.y = -0.25;
  tabletopPose.pose.position.z = table_height;
  tabletopPose.pose.orientation.w = 1.0;
  

  // 创建规划场景接口并将碰撞对象添加到场景中
  mtc_pour::cleanup();
  moveit::planning_interface::PlanningSceneInterface psi;
  std::vector<moveit_msgs::msg::CollisionObject> objs;
  mtc_pour::setupTable(objs, tabletopPose);
  mtc_pour::setupObjects(objs, bottlePose, glassPose,
                          "package://mtc_pour/meshes/small_bottle.stl");
  psi.applyCollisionObjects(objs);
}

// 执行任务的实现
void MTCTaskNode_Right::doTask()
{
  task_ = createTask();  // 创建任务

  // 尝试初始化任务
  try
  {
    task_.init();  // 初始化任务
  }
  catch (mtc::InitStageException& e)  // 捕获初始化失败的异常
  {
    RCLCPP_ERROR_STREAM(LOGGER, e);  // 打印错误信息
    return;
  }

  // 规划任务路径，最大规划尝试次数为 5
  if (!task_.plan(10))
  {
    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_Right::createTask()
{
  mtc::Task task;  // 创建一个 MTC 任务对象
  task.stages()->setName("grabbing the glass on the right");  // 为任务命名
  task.loadRobotModel(node_);  // 加载机器人模型

  // 定义机器人操作相关的参数
  // const auto& left_arm_group_name = "left_ur_manipulator";  // 机械臂的操作组名称
  // const auto& left_hand_group_name = "left_robotiq_2f_85_gripper";  // 手部操作组名称
  // const auto& left_hand_frame = "left_robotiq_85_base_link";  // 机械手的坐标系

  const auto& right_arm_group_name = "right_ur_manipulator";  // 机械臂的操作组名称
  const auto& right_hand_group_name = "right_robotiq_2f_85_gripper";  // 手部操作组名称
  const auto& right_hand_frame = "right_grasp_point";  // 机械手的坐标系

  // 设置任务的属性
  task.setProperty("group", right_arm_group_name);
  task.setProperty("eef", "right_robotiq_2f_85_gripper_ee");
  task.setProperty("ik_frame", right_hand_frame);

  // 忽略未使用变量的编译器警告
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-but-set-variable"
  mtc::Stage* current_state_ptr = nullptr;  // 将current_stage传递给grasp pose generator
#pragma GCC diagnostic pop

  // 创建并添加获取当前状态的阶段
  auto stage_state_current = std::make_unique<mtc::stages::CurrentState>("right-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);  // 设置最大速度缩放因子为 1.0
  cartesian_planner->setMaxAccelerationScalingFactor(1.0);  // 设置最大加速度缩放因子为 1.0
  cartesian_planner->setStepSize(.01);  // 设置步长为 0.01

  // 创建并添加手部动作的阶段（打开手）
  auto stage_open_right_hand =
      std::make_unique<mtc::stages::MoveTo>("open right_hand", interpolation_planner);  // 创建移动到目标状态的阶段
  stage_open_right_hand->setGroup(right_hand_group_name);  // 设置操作组为手部
  stage_open_right_hand->setGoal("right_open");  // 设置手部打开的目标状态
  task.add(std::move(stage_open_right_hand));  // 将阶段添加到任务中
  
  // Add the next lines of codes to define more stages here
  auto stage_move_to_pick_right = std::make_unique<mtc::stages::Connect>(
    "move to pick-right",
    mtc::stages::Connect::GroupPlannerVector{{right_arm_group_name,sampling_planner}});
  stage_move_to_pick_right->setTimeout(5.0);
  stage_move_to_pick_right->properties().configureInitFrom(mtc::Stage::PARENT);
  task.add(std::move(stage_move_to_pick_right));

  mtc::Stage* attach_object_stage_right = nullptr;// Forward attach_object_stage  to place pose generator
  {
    auto grasp_right = std::make_unique<mtc::SerialContainer>("pick glass-right");
    task.properties().exposeTo(grasp_right->properties(),{"eef","group","ik_frame"});
    grasp_right->properties().configureInitFrom(mtc::Stage::PARENT,{"eef","group","ik_frame"});

    {
      auto stage = std::make_unique<mtc::stages::MoveRelative>("approach glass-right",cartesian_planner);
      stage->properties().set("marker_ns","approach_glass-right");
      stage->properties().set("link",right_hand_frame);// TODO:猜测这是与物体发生接触的link
      stage->properties().configureInitFrom(mtc::Stage::PARENT,{"group"});
      stage->setMinMaxDistance(0.05,0.15);
      // Set hand forward direction
      geometry_msgs::msg::Vector3Stamped vec;
      vec.header.frame_id = right_hand_frame;
      vec.vector.z =1.0;
      stage->setDirection(vec);
      grasp_right->insert(std::move(stage));
    }

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

      Eigen::Isometry3d grasp_frame_transform_right;
      
      //末端执行器坐标系的Z轴朝向杯子的X轴，末端执行器坐标系的X轴朝向杯子的Y轴，末端执行器坐标系的Y轴朝向杯子的Z轴
      Eigen::Quaterniond q = Eigen:: AngleAxisd(-M_PI/2,Eigen::Vector3d::UnitY())*
                            Eigen::AngleAxisd(-M_PI/2,Eigen::Vector3d::UnitX());
      
      // //末端执行器坐标系的Y轴朝向杯子的X轴，末端执行器坐标系的X轴朝向杯子的Y轴,末端执行器坐标系的Z轴朝向杯子的-Z轴
      // Eigen::Quaterniond q = Eigen:: AngleAxisd(M_PI/2,Eigen::Vector3d::UnitZ())*
      //                       Eigen::AngleAxisd(-M_PI,Eigen::Vector3d::UnitX());

      grasp_frame_transform_right.linear() = q.matrix();
      grasp_frame_transform_right.translation().z()=0;


      // Compute IK
      // 将stage对象的所有权转移给wrapper_right
      // wrapper 通常指的是一层封装，用于将某个功能或一组功能封装起来，
      auto wrapper = std::make_unique<mtc::stages::ComputeIK>("grasp pose IK - right",std::move(stage));
      wrapper->setMaxIKSolutions(8);
      wrapper->setMinSolutionDistance(1.0);
      wrapper->setIKFrame(grasp_frame_transform_right,right_hand_frame);
      wrapper->properties().configureInitFrom(mtc::Stage::PARENT,{"eef","group"});
      wrapper->properties().configureInitFrom(mtc::Stage::INTERFACE,{"target_pose"});
      grasp_right->insert(std::move(wrapper));
    }

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

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

    {
      auto stage = std::make_unique<mtc::stages::ModifyPlanningScene>("attcah object - right");
      stage->attachObject("glass",right_hand_frame);
      attach_object_stage_right = stage.get();
      grasp_right->insert(std::move(stage));
    }

    {
      auto stage = std::make_unique<mtc::stages::MoveRelative>("lift object - right",cartesian_planner);
      stage->properties().configureInitFrom(mtc::Stage::PARENT,{"group"});
      stage->setMinMaxDistance(0.1,0.3);
      stage->setIKFrame(right_hand_frame);
      stage->properties().set("marker_ns","lift_object_right");

      // Set upward direction
      geometry_msgs::msg::Vector3Stamped vec;
      vec.header.frame_id = "dual_base";
      vec.vector.y = 1.0;
      stage->setDirection(vec);
      grasp_right->insert(std::move(stage));
    }

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

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

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

    // {
    //   //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_right");
    //   stage->setObject("glass");

    //   geometry_msgs::msg::PoseStamped target_pose_msg_right;
    //   target_pose_msg_right.header.frame_id = "dual_base";
    //   target_pose_msg_right.pose.position.x = -0.5;
    //   target_pose_msg_right.pose.position.y = -0.5;
    //   target_pose_msg_right.pose.position.z = 0.55;
    //   tf2::Quaternion qtn;
    //   qtn.setRPY(0.0,0.0,-1.57);
    //   target_pose_msg_right.pose.orientation.x = qtn.x();
    //   target_pose_msg_right.pose.orientation.y = qtn.y();
    //   target_pose_msg_right.pose.orientation.z = qtn.z();
    //   target_pose_msg_right.pose.orientation.w = qtn.w();
    //   stage->setPose(target_pose_msg_right);
    //   stage->setMonitoredStage(attach_object_stage_right); //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("glass");
    //   wrapper->properties().configureInitFrom(mtc::Stage::PARENT,{"eef","group"});
    //   wrapper->properties().configureInitFrom(mtc::Stage::INTERFACE,{"target_pose"});
    //   place_right->insert(std::move(wrapper));

    //  }

    //  {
    //     //Sample place pose
    //     auto stage = std::make_unique<mtc::stages::GeneratePlacePose>("generate place pose -right");
    //     stage->properties().configureInitFrom(mtc::Stage::PARENT);
    //     stage->properties().set("marker_ns","place_pose_right");
    //     stage->setObject("glass");

    //     // 定义放置位姿
    //     geometry_msgs::msg::PoseStamped target_pose_msg_right;
    //     target_pose_msg_right.header.frame_id = "dual_base";
    //     target_pose_msg_right.pose.position.x = -0.5;
    //     target_pose_msg_right.pose.position.y = 0.5;
    //     target_pose_msg_right.pose.position.z = 0.2;
    //     tf2::Quaternion qtn;
    //     qtn.setRPY(0.0, 0.0, -M_PI );  // 将RPY角度转换成四元数
    //     target_pose_msg_right.pose.orientation.x = qtn.x();
    //     target_pose_msg_right.pose.orientation.y = qtn.y();
    //     target_pose_msg_right.pose.orientation.z = qtn.z();
    //     target_pose_msg_right.pose.orientation.w = qtn.w();
    //     stage->setPose(target_pose_msg_right);

    //     stage->setMonitoredStage(attach_object_stage_right); //Hook into attach_object_stage

    //     // 定义放置时的末端执行器坐标系变换
    //     Eigen::Isometry3d place_frame_transform_right;
    //     Eigen::Quaterniond q = Eigen::AngleAxisd(-M_PI / 2, Eigen::Vector3d::UnitY()) *
    //                           Eigen::AngleAxisd(-M_PI / 2, Eigen::Vector3d::UnitX());
    //     place_frame_transform_right.linear() = q.matrix();
    //     place_frame_transform_right.translation().z() = 0;

    //     // 计算IK
    //     auto wrapper = std::make_unique<mtc::stages::ComputeIK>("place pose IK - right", std::move(stage));
    //     wrapper->setMaxIKSolutions(8); // 设置最大解的数量
    //     wrapper->setMinSolutionDistance(1.0);
    //     wrapper->setIKFrame(place_frame_transform_right, right_hand_frame);
    //     wrapper->properties().configureInitFrom(mtc::Stage::PARENT, {"eef", "group"});
    //     wrapper->properties().configureInitFrom(mtc::Stage::INTERFACE, {"target_pose"});
    //     place_right->insert(std::move(wrapper));
    // }

      {
      // Sample place pose
      auto stage = std::make_unique<mtc::stages::GeneratePlacePose>("generate place pose -right");
      stage->properties().configureInitFrom(mtc::Stage::PARENT);
      stage->properties().set("marker_ns", "place_pose_right");
      stage->setObject("glass");

      // 定义放置位姿
      geometry_msgs::msg::PoseStamped target_pose_msg_right;
      target_pose_msg_right.header.frame_id = "dual_base";
      target_pose_msg_right.pose.position.x = -0.3;
      target_pose_msg_right.pose.position.y = -0.5;
      target_pose_msg_right.pose.position.z = 0.3;

      // 计算放置位姿的正确姿态
      Eigen::Quaterniond q_place = Eigen::AngleAxisd(-M_PI / 2, Eigen::Vector3d::UnitZ())*
                                  Eigen::AngleAxisd(0, Eigen::Vector3d::UnitX());
      Eigen::Quaterniond q_object = q_place.inverse();

      tf2::Quaternion qtn(q_object.x(), q_object.y(), q_object.z(), q_object.w());
      target_pose_msg_right.pose.orientation = tf2::toMsg(qtn);

      stage->setPose(target_pose_msg_right);
      stage->setMonitoredStage(attach_object_stage_right); // Hook into attach_object_stage

      // 设置 IK 框架，与抓取阶段一致
      Eigen::Isometry3d place_frame_transform_right;
      place_frame_transform_right.linear() = q_place.toRotationMatrix();
      place_frame_transform_right.translation().z() = 0;

      // 计算 IK
      auto wrapper = std::make_unique<mtc::stages::ComputeIK>("place pose IK - right", std::move(stage));
      wrapper->setMaxIKSolutions(8); // 设置最大解的数量
      wrapper->setMinSolutionDistance(1.0);
      wrapper->setIKFrame(place_frame_transform_right, right_hand_frame);
      wrapper->properties().configureInitFrom(mtc::Stage::PARENT, { "eef", "group" });
      wrapper->properties().configureInitFrom(mtc::Stage::INTERFACE, { "target_pose" });
      place_right->insert(std::move(wrapper));
  }

    


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

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

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

    {
      auto stage = std::make_unique<mtc::stages::MoveRelative>("retreat -right",cartesian_planner);
      stage->properties().configureInitFrom(mtc::Stage::PARENT,{"group"});
      stage->setMinMaxDistance(0.1,0.3);
      stage->setIKFrame(right_hand_frame);
      stage->properties().set("marker_ns","retreat-right");

      // Set retreat direction
      geometry_msgs::msg::Vector3Stamped vec;
      vec.header.frame_id = "world";
      vec.vector.z=0.5;
      stage->setDirection(vec);
      place_right->insert(std::move(stage));
    }

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

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

  // Stages all added to the task above this line
  return task;  // 返回创建的任务
}

http://www.kler.cn/a/306749.html

相关文章：

  • SQL（2）
  • linux基础-完结（详讲补充）
  • SQL语句-MySQL
  • ubuntu24.04.1 安装 mysql
  • Linux（CentOS）安装 JDK
  • Linux awk命令详解-参数-选项-内置变量-内置函数-脚本(多图、多示例)
  • 【SQL】百题计划：SQL判断条件OR的使用。
  • 如何为子域名配置 Nginx 反向代理到 Flask 应用
  • IEEE会议论文引用格式
  • 在 Android 中，事件的分发机制
  • 淘宝商品详情API返回值中的预售与定制信息解析
  • xtu oj 折纸
  • [网络]从零开始的计算机网络基础知识讲解
  • eureka.client.service-url.defaultZone的坑
  • 数据库系统 第50节 数据库灾难恢复计划
  • python实现冒泡排序的算法
  • cmd命令
  • IDS Clearing House Core 项目入门
  • 【云原生监控】Prometheus监控系统
  • Python [ GUI编程自学 ]，虽然但是，还是想出一个系列
  • 完整指南：CNStream流处理多路并发框架适配到NVIDIA Jetson Orin (四) 运行、调试、各种问题解决
  • ai智能电销机器人是新科技产业
  • charls基于夜神模拟器抓取安卓7.0应用程序https请求
  • 数据结构 Java DS——分享部分链表题目 (2)
  • Linux下的简单TCP客户端和服务器
  • [论文笔记] LLM大模型剪枝篇——4、Qwen2系列剪枝实现