#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; // 返回创建的任务
}
MTC完成右臂抓取放置任务\\放置姿态设置
最新推荐文章于 2024-10-14 16:07:40 发布