此博文主要是用来记录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;
}