官方教程教程 — MoveIt 文档Humble 文档#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_sharedrclcpp::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 its a variable thats 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_uniquemtc::stages::CurrentState(current); current_state_ptr stage_state_current.get(); task.add(std::move(stage_state_current)); auto sampling_planner std::make_sharedmtc::solvers::PipelinePlanner(node_); auto interpolation_planner std::make_sharedmtc::solvers::JointInterpolationPlanner(); auto cartesian_planner std::make_sharedmtc::solvers::CartesianPath(); cartesian_planner-setMaxVelocityScalingFactor(1.0); cartesian_planner-setMaxAccelerationScalingFactor(1.0); cartesian_planner-setStepSize(.01); auto stage_open_hand std::make_uniquemtc::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)); // Add the next lines of codes to define more stages here, e.g.: auto stage_move_to_pick std::make_uniquemtc::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)); mtc::Stage* attach_object_stage nullptr; // Forward attach_object_stage to place pose generator auto grasp std::make_uniquemtc::SerialContainer(pick object); task.properties().exposeTo(grasp-properties(), { eef, group, ik_frame }); grasp-properties().configureInitFrom(mtc::Stage::PARENT,{ eef, group, ik_frame }); { auto stage std::make_uniquemtc::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)); } // Sample grasp pose { auto stage std::make_uniquemtc::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 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; // Compute IK auto wrapper std::make_uniquemtc::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)); } { auto stage std::make_uniquemtc::stages::ModifyPlanningScene(allow collision (hand,object)); stage-allowCollisions(object, task.getRobotModel() -getJointModelGroup(hand_group_name) -getLinkModelNamesWithCollisionGeometry(), true); grasp-insert(std::move(stage)); } { auto stage std::make_uniquemtc::stages::MoveTo(close hand, interpolation_planner); stage-setGroup(hand_group_name); stage-setGoal(close); grasp-insert(std::move(stage)); } { auto stage std::make_uniquemtc::stages::ModifyPlanningScene(attach object); stage-attachObject(object, hand_frame); attach_object_stage stage.get(); grasp-insert(std::move(stage)); } { auto stage std::make_uniquemtc::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)); } //将串行容器插入到任务中 task.add(std::move(grasp)); auto stage_move_to_place std::make_uniquemtc::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)); auto place std::make_uniquemtc::SerialContainer(place object); task.properties().exposeTo(place-properties(), { eef, group, ik_frame }); place-properties().configureInitFrom(mtc::Stage::PARENT, { eef, group, ik_frame }); // Sample place pose { auto stage std::make_uniquemtc::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_uniquemtc::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)); } { auto stage std::make_uniquemtc::stages::MoveTo(open hand, interpolation_planner); stage-setGroup(hand_group_name); stage-setGoal(open); place-insert(std::move(stage)); } { auto stage std::make_uniquemtc::stages::ModifyPlanningScene(forbid collision (hand,object)); stage-allowCollisions(object, task.getRobotModel() -getJointModelGroup(hand_group_name) -getLinkModelNamesWithCollisionGeometry(), false); place-insert(std::move(stage)); } { auto stage std::make_uniquemtc::stages::ModifyPlanningScene(detach object); stage-detachObject(object, hand_frame); place-insert(std::move(stage)); } { auto stage std::make_uniquemtc::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)); } task.add(std::move(place)); { auto stage std::make_uniquemtc::stages::MoveTo(return home, interpolation_planner); stage-properties().configureInitFrom(mtc::Stage::PARENT, { group }); stage-setGoal(ready); task.add(std::move(stage)); } // Stages all added to the task above this line 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_sharedMTCTaskNode(options); rclcpp::executors::MultiThreadedExecutor executor; auto spin_thread std::make_uniquestd::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; }