MoveIt! 學習筆記6-MotionPlanningAPI -MoveIt 中的軌跡規劃API
阿新 • • 發佈：2020-12-30
此博文主要是用來記錄ROS-Kinetic 中，用於機器人軌跡規劃的MoveIt功能包的學習記錄。
引： MotionPlanning API的主要功能是提供一個軌跡規劃介面，載入機器人planner，然後設定目標位置，使用planner進行軌跡規劃+執行。
這個例子有一個優點：詳細介紹了物件建立過程 + 軌跡規劃過程，也同時展示了，如何在RVIZ中顯示機器人軌跡+目標點資訊+機器人狀態。 同時引入了RVIZ的 GUI工具包，可以在程式執行的時候，手工控制程式執行步驟（中途阻塞），使程式不用一直從頭執行到尾。
官方教程主要以程式碼例項為主，所以，在下邊的程式碼中，主要通過註釋的方式，解釋程式碼含義，通過程式碼例項，學習MoveIt內部內容。
/********************************************************************* * Software License Agreement (BSD License) * * Copyright (c) 2012, Willow Garage, Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * * Redistributions of source code must retain the above copyright * notice, this list of conditions and the following disclaimer. * * Redistributions in binary form must reproduce the above * copyright notice, this list of conditions and the following * disclaimer in the documentation and/or other materials provided * with the distribution. * * Neither the name of Willow Garage nor the names of its * contributors may be used to endorse or promote products derived * from this software without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ /* Author: Sachin Chitta, Michael Lautman */ #include <pluginlib/class_loader.h> #include <ros/ros.h> // MoveIt! #include <moveit/robot_model_loader/robot_model_loader.h> #include <moveit/planning_interface/planning_interface.h> #include <moveit/planning_scene/planning_scene.h> #include <moveit/kinematic_constraints/utils.h> #include <moveit_msgs/DisplayTrajectory.h> #include <moveit_msgs/PlanningScene.h> #include <moveit_visual_tools/moveit_visual_tools.h> #include <boost/scoped_ptr.hpp> int main(int argc, char** argv) { const std::string node_name = "motion_planning_tutorial"; ros::init(argc, argv, node_name); ros::AsyncSpinner spinner(1); spinner.start(); ros::NodeHandle node_handle("~"); //引：這個教程主要實現了四個運動學規劃：移動機器人到指定軸角位置，機器人返回原點，機器人在受約束條件下的運動（直線運動） //同時，這個教程也包含了：RVIZ中，顯示目標點名稱，顯示機器人末端執行器軌跡，模擬機器人運動等功能，很值得學習！ //Part0: 總介紹 //這個教程主要包含幾部分： // 1.載入ROS的運動學規劃器planner // 2. 建立一個RobotModel物件，來從ROS-Server上載入機器人模型； // 3. 建立一個PlanningScene物件，用於制定Robot執行場景。 //Part1： 建立一個RobotModelLoader物件，並從ROS-Server上載入機器人模型；並設定模型相關引數 const std::string PLANNING_GROUP = "panda_arm"; //（1）設定規劃組名稱 robot_model_loader::RobotModelLoader robot_model_loader("robot_description"); //（2）建立robot_model_loader，從"robot_description"這個topic裡面載入模型 robot_model::RobotModelPtr robot_model = robot_model_loader.getModel(); //從loader中，獲取機器人模型，並存在robot_model內 //×Step 1.1 建立一個RobotState和JointModelGroup物件，分別儲存機器人當前狀態+機器人運動規劃組panda_arm的關節狀態 robot_state::RobotStatePtr robot_state(new robot_state::RobotState(robot_model)); const robot_state::JointModelGroup* joint_model_group = robot_state->getJointModelGroup(PLANNING_GROUP); //×Step 1.2 使用moveit_core核心的“RobotModel”， 可以建立一個規劃場景（planning_scene）； // 這個規劃場景可以維持當前模擬環境的狀態（包括機器人的狀態） planning_scene::PlanningScenePtr planning_scene(new planning_scene::PlanningScene(robot_model)); //×Step 1.3 設定規劃場景物件，並將當前的機器人關節狀態設定為“ready”狀態 planning_scene->getCurrentStateNonConst().setToDefaultValues(joint_model_group, "ready"); //Part2： 載入ROS運動學規劃的plugIn，載入一個運動學規劃器（通過Plugin的名稱載入）（注意！ 此處使用了ROS-pluginlib庫） boost::scoped_ptr<pluginlib::ClassLoader<planning_interface::PlannerManager>> planner_plugin_loader;//loader planning_interface::PlannerManagerPtr planner_instance; //？ std::string planner_plugin_name; //儲存plugin名稱 // ×Step2.1 通過ROS server中，找到正在執行的指定plugin，並將其載入。 // 其主要功能是，將”planning_plugin“這個topic裡面的plugin名稱讀取出來， 並存在planner_plugin_name物件內。 if (!node_handle.getParam("planning_plugin", planner_plugin_name)) ROS_FATAL_STREAM("Could not find planner plugin name"); try { planner_plugin_loader.reset(new pluginlib::ClassLoader<planning_interface::PlannerManager>( "moveit_core", "planning_interface::PlannerManager")); } catch (pluginlib::PluginlibException& ex) { ROS_FATAL_STREAM("Exception while creating planning plugin loader " << ex.what()); } try { planner_instance.reset(planner_plugin_loader->createUnmanagedInstance(planner_plugin_name)); if (!planner_instance->initialize(robot_model, node_handle.getNamespace())) ROS_FATAL_STREAM("Could not initialize planner instance"); ROS_INFO_STREAM("Using planning interface '" << planner_instance->getDescription() << "'"); } catch (pluginlib::PluginlibException& ex) { const std::vector<std::string>& classes = planner_plugin_loader->getDeclaredClasses(); std::stringstream ss; for (std::size_t i = 0; i < classes.size(); ++i) ss << classes[i] << " "; ROS_ERROR_STREAM("Exception while loading planner '" << planner_plugin_name << "': " << ex.what() << std::endl << "Available plugins: " << ss.str()); } //Part3： 視覺化（建立在RVIZ中，顯示座標及軌跡的資訊物件） //功能：MoveItVisualTools這個工具物件具有很多的功能，包括： // -可以視覺化顯示物體，顯示機器人； // -可以視覺化顯示規劃器規劃出來的機器人軌跡； // -可以載入RVIZ內部的GUI按鈕，點選按鈕進行指令碼的點動除錯。（總體的程式一步一步執行，而不是一次全部執行完） //×Step3.1： 建立MoveItVisualTools物件，建立一個topic，釋出機器人狀態 namespace rvt = rviz_visual_tools; moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0"); visual_tools.loadRobotStatePub("/display_robot_state"); visual_tools.enableBatchPublishing(); //重要！ 這條語句的作用是，為了防止傳送資訊過於頻繁導致的堵塞問題，將資訊打包，在trigger時，統一發送。 visual_tools.deleteAllMarkers(); // clear all old markers visual_tools.trigger(); //*Step3.2 ： 載入remoteControl，主要功能是啟用RVIZ裡面的“按鈕GUI”，操作人員可以通過GUI按鈕/鍵盤來控制程式執行下一步。 visual_tools.loadRemoteControl(); //提示：RVIZ中，有很多種方式來顯示標記： 文字+圓柱（就是XYZ座標軸）+球體（沿著軌跡線均勻分佈） /* RViz provides many types of markers, in this demo we will use text, cylinders, and spheres*/ //×Step3.3：重要！ ！！展示如何在RVIZ中，顯示點的座標資訊！！ Eigen::Affine3d text_pose = Eigen::Affine3d::Identity(); text_pose.translation().z() = 1.75; visual_tools.publishText(text_pose, "Motion Planning API Demo", rvt::WHITE, rvt::XLARGE);//！（點變數名，顯示文字，顏色，文字大小） //*提示：使用Batch的資訊釋出策略，若傳送的需要顯示的內容過多時，可以提高效率 visual_tools.trigger(); //顯示上邊的text——pose點的資訊 //* 提示！！ 這條語句主要是阻塞程式執行，帶操作人員點選GUI按鈕或者鍵盤，繼續執行後續程式。 visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo"); // Part4：設定一個目標點座標，並顯示，然後設定帶有約束條件的目標點 //Step4.1 顯示當前機器人狀態，並設定目標點座標+設定允許最大誤差 visual_tools.publishRobotState(planning_scene->getCurrentStateNonConst(), rviz_visual_tools::GREEN); //使用綠色顯示機器人當前狀態 visual_tools.trigger(); //關聯：enableBatchPublishing, 將上邊綠色的標記顯示出來。 planning_interface::MotionPlanRequest req; //建立一個運動規劃”請求“物件和“返回”物件 planning_interface::MotionPlanResponse res; geometry_msgs::PoseStamped pose; //建立一個四元數的位姿，並賦值； pose.header.frame_id = "panda_link0"; pose.pose.position.x = 0.3; pose.pose.position.y = 0.4; pose.pose.position.z = 0.75; pose.pose.orientation.w = 1.0; std::vector<double> tolerance_pose(3, 0.01); //設定運動學規劃的允許誤差，在xyz長度方向上：0.01m誤差； 在轉角上：0.01rad std::vector<double> tolerance_angle(3, 0.01); //這個3是什麼意思？ //Step4.2 設定一個帶有約束條件的，目標點物件；這個物件將作為輸入值，傳入到plannar進行軌跡規劃 // .. _kinematic_constraints: // http://docs.ros.org/indigo/api/moveit_core/html/namespacekinematic__constraints.html#a88becba14be9ced36fefc7980271e132 moveit_msgs::Constraints pose_goal = kinematic_constraints::constructGoalConstraints("panda_link8", pose, tolerance_pose, tolerance_angle); //（link名稱，目標點座標，位置允許誤差，轉角允許誤差） // Part5： 進行軌跡規劃 //Step5.1 將前邊帶有約束的目標點輸入規劃器 req.group_name = PLANNING_GROUP; //設定運動規劃組名稱 req.goal_constraints.push_back(pose_goal); //將帶有約束條件的目標點，輸入到MotionPlanRequest req物件中 //Step5.2 建立一個‘規劃環境’（planning context），並將規劃場景，規劃請求，規劃反饋值封住在裡面； // 然後使用這個‘規劃環境’進行軌跡規劃。 planning_interface::PlanningContextPtr context = planner_instance->getPlanningContext(planning_scene, req, res.error_code_); context->solve(res); //重要！！這句是進行軌跡規劃的執行指令 if (res.error_code_.val != res.error_code_.SUCCESS) { ROS_ERROR("Could not compute plan successfully"); return 0; } // Part6： 在RVIZ中，顯示規劃成功的軌跡結果 //×Step6.1 建立一個publisher，向其中“”topic，傳送moveit_msgs::DisplayTrajectory型別的軌跡內容 ros::Publisher display_publisher = node_handle.advertise<moveit_msgs::DisplayTrajectory>("/move_group/display_planned_path", 1, true); moveit_msgs::DisplayTrajectory display_trajectory; //！！！！建立將被publish的物件 moveit_msgs::MotionPlanResponse response; //建立一個儲存軌跡的response物件，並將res中的軌跡儲存至response物件中。 res.getMessage(response); display_trajectory.trajectory_start = response.trajectory_start; //向被publish的物件中，儲存軌跡起點+軌跡資訊 display_trajectory.trajectory.push_back(response.trajectory); visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group); //設定軌跡視覺化 visual_tools.trigger(); display_publisher.publish(display_trajectory);//向topic中，傳送軌跡資訊 //*Step6.2 將規劃好的軌跡，載入到robot——state裡面，然後使用指令，控制機器人運動至相應位置 robot_state->setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions); planning_scene->setCurrentState(*robot_state.get()); //× Step6.3 顯示機器人當前位置（座標點，座標名稱） visual_tools.publishRobotState(planning_scene->getCurrentStateNonConst(), rviz_visual_tools::GREEN); visual_tools.publishAxisLabeled(pose.pose, "goal_1"); visual_tools.publishText(text_pose, "Pose Goal (1)", rvt::WHITE, rvt::XLARGE); visual_tools.trigger(); //× Step6.4 使用RIVZ visual tool，等待操作人員點選GUI的按鈕，或者按下鍵盤N，以進行後續內容（此處阻塞程式執行） visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); // Part7 設定一個機器人關節位姿（每個關節的轉角），顯示軌跡，然後控制機器人到達該位置。 // ^^^^^^^^^^^^^^^^^ // Now, setup a joint space goal robot_state::RobotState goal_state(robot_model); std::vector<double> joint_values = { -1.0, 0.7, 0.7, -1.5, -0.7, 2.0, 0.0 }; goal_state.setJointGroupPositions(joint_model_group, joint_values); moveit_msgs::Constraints joint_goal = kinematic_constraints::constructGoalConstraints(goal_state, joint_model_group); req.goal_constraints.clear(); req.goal_constraints.push_back(joint_goal); // Call the planner and visualize the trajectory /* Re-construct the planning context */ context = planner_instance->getPlanningContext(planning_scene, req, res.error_code_); /* Call the Planner */ context->solve(res); //詞句進行軌跡規劃 /* Check that the planning was successful */ if (res.error_code_.val != res.error_code_.SUCCESS) { ROS_ERROR("Could not compute plan successfully"); return 0; } /* 與上邊part6 方法一致， */ res.getMessage(response); display_trajectory.trajectory.push_back(response.trajectory); /* Now you should see two planned trajectories in series*/ visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group); visual_tools.trigger(); display_publisher.publish(display_trajectory); /* We will add more goals. But first, set the state in the planning scene to the final state of the last plan */ robot_state->setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions); planning_scene->setCurrentState(*robot_state.get()); //控制機器人運動到規劃的位置 // Display the goal state 顯示軌跡終點資訊 visual_tools.publishRobotState(planning_scene->getCurrentStateNonConst(), rviz_visual_tools::GREEN); visual_tools.publishAxisLabeled(pose.pose, "goal_2"); visual_tools.publishText(text_pose, "Joint Space Goal (2)", rvt::WHITE, rvt::XLARGE); visual_tools.trigger(); /* Wait for user input *///同樣方法，阻塞程式執行，並且等待輸入 visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); /* Part8 規劃當前位置到初始位置的軌跡，並執行 // 注意！！重要！！下邊的軌跡規劃過程更加簡潔，可以作為模板參考使用 req.goal_constraints.clear(); req.goal_constraints.push_back(pose_goal); context = planner_instance->getPlanningContext(planning_scene, req, res.error_code_); context->solve(res); //進行軌跡規劃 res.getMessage(response); display_trajectory.trajectory.push_back(response.trajectory); visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group); visual_tools.trigger(); display_publisher.publish(display_trajectory); /* Set the state in the planning scene to the final state of the last plan */ robot_state->setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions); planning_scene->setCurrentState(*robot_state.get()); //詞句是控制機器人執行軌跡，返回原點 // Display the goal state visual_tools.publishRobotState(planning_scene->getCurrentStateNonConst(), rviz_visual_tools::GREEN); visual_tools.trigger(); /* Wait for user input *///阻塞程式，等待輸入 visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); /* Part9 重要！！ 規劃一個帶有約束條件的運動軌跡（末端執行器，保持末端方向不變的情況下，沿著直線運動） // ^^^^^^^^^^^^^^^^^^^^^^^ // Let's add a new pose goal again. This time we will also add a path constraint to the motion. /* Let's create a new pose goal */ pose.pose.position.x = 0.32; pose.pose.position.y = -0.25; pose.pose.position.z = 0.65; pose.pose.orientation.w = 1.0; moveit_msgs::Constraints pose_goal_2 = kinematic_constraints::constructGoalConstraints("panda_link8", pose, tolerance_pose, tolerance_angle);//仍然使用之前的約束條件 /* Now, let's try to move to this new pose goal*/ req.goal_constraints.clear(); req.goal_constraints.push_back(pose_goal_2); /* But, let's impose a path constraint on the motion. Here, we are asking for the end-effector to stay level*/ //重要！！此處是約束了末端沿著一個位姿運動的指令！！！！！ //方法是：定義一個四元數，只設定旋轉位姿，然後將其設定為約束。 geometry_msgs::QuaternionStamped quaternion; quaternion.header.frame_id = "panda_link0"; quaternion.quaternion.w = 1.0; req.path_constraints = kinematic_constraints::constructGoalConstraints("panda_link8", quaternion); // Imposing path constraints requires the planner to reason in the space of possible positions of the end-effector // (the workspace of the robot) // because of this, we need to specify a bound for the allowed planning volume as well; // Note: a default bound is automatically filled by the WorkspaceBounds request adapter (part of the OMPL pipeline, // but that is not being used in this example). // We use a bound that definitely includes the reachable space for the arm. This is fine because sampling is not done // in this volume // when planning for the arm; the bounds are only used to determine if the sampled configurations are valid. req.workspace_parameters.min_corner.x = req.workspace_parameters.min_corner.y = req.workspace_parameters.min_corner.z = -2.0; req.workspace_parameters.max_corner.x = req.workspace_parameters.max_corner.y = req.workspace_parameters.max_corner.z = 2.0; // Call the planner and visualize all the plans created so far. context = planner_instance->getPlanningContext(planning_scene, req, res.error_code_); context->solve(res); //此處進行軌跡規劃 res.getMessage(response); display_trajectory.trajectory.push_back(response.trajectory); visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group); visual_tools.trigger(); display_publisher.publish(display_trajectory);//顯示機器人受約束軌跡 /* Set the state in the planning scene to the final state of the last plan */ robot_state->setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions); planning_scene->setCurrentState(*robot_state.get()); //機器人運動 // Display the goal state visual_tools.publishRobotState(planning_scene->getCurrentStateNonConst(), rviz_visual_tools::GREEN); visual_tools.publishAxisLabeled(pose.pose, "goal_3"); visual_tools.publishText(text_pose, "Orientation Constrained Motion Plan (3)", rvt::WHITE, rvt::XLARGE); visual_tools.trigger(); // END_TUTORIAL /* Wait for user input */ visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to exit the demo"); planner_instance.reset(); return 0; }