ROS进阶——MoveIt!
环境：Ubuntu16.04+ROS Kinetic
一、MoveIt!简介
在实现机械臂的自主抓取中机械臂的运动规划是其中最重要的一部分，其中包含运动学正逆解算、碰撞检测、环境感知和动作规划等。在本课题中，采用ROS系统提供的MoveIt!完成在操作系统层的运动规划。
MoveIt! 是ROS系统中集合了与移动操作相关的组件包的运动规划库。它包含了运动规划中所需要的大部分功能，同时其提供友好的配置和调试界面便于完成机器人在ROS系统上的初始化及调试，其具体架构如下图所示。
- move_group：move_group是MoveIt!的核心部分，它的功能在于集成机器人的各独立模块并为用户提供一系列的动作指令和服务。其本身并不具备太多的功能，起到的是积分器的作用，完成各功能包和插件的集成。
- 运动规划（motion panning）：在MoveIt!中，运动规划器起的作用是实现运动规划算法，其以插件的方式通过ROS的pluginlib接口完成信息的通讯，根据需求可制作或选用不同的规划算法。
- 场景规划（Planning Scene）：通过场景规划，用户可以创建一个具体的工作环境或者加入障碍物。
- 运动学（Kinematics）：运动学算法是机械臂控制算法中的核心内容，其中包括正运动学算法和逆运动学算法，在MoveIt!中，运动学算法同样以插件的形式完成于ROS的集成，因此可根据实际需求，编写自己的运动学算法来控制机器人。
- 碰撞检测（collision checking）：为避免机器人自身或者和环境发生干涉导致意外的发生，在进行运动规划时碰撞检测是必不可少的一部分，在MoveIt!中，采用FCL（Flexible Collision Library）进行对象碰撞的运算。
- 开源运动规划库（open motion planning library）：OMPL是一个的开源的C++库，主要用于运动规划，其中包含了大量用于运动规划的先进算法。该库中的算法以采样规划算法为主，通过其可以实现不同目标点之间的路径规划。
1.1 move_group
move_group以ROS node的形式加载，通过ROS param server获取参数，通过topics和actions和其他节点进行信息交互，监控机械臂状态（Joint State Information），坐标变换（Transform Information），规划场景（Planning Scene），实现控制（Controller Interface），同时可通过plugin（Extensible Capabilities）实现功能扩展。
1.2 Motion_Planning
Motion_Planning以plugin的方式作为功能插件整合在MoveIt上，通过ROS Action和service（move_group提供）进行信息的交互。
1.3 Planning_Scene
规划场景（Planning_Scene）保存了机器人及其环境，通过move group节点内部的监视器，实时检测机器人状态，传感器检测数据和用户输入的场景信息。
二、运动学
2.1 正运动学
获取机器人模型（robot_description为机器人模型描述参数名字，在MoveIt!中默认使用该名字，参数内容为URDF文件内容），然后获取关节数据并设置各关节角度，最后获取特定link位姿。
//获取模型
robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
robot_model::RobotModelPtr kinematic_model = robot_model_loader.getModel();
robot_state::RobotStatePtr kinematic_state(
new robot_state::RobotState(kinematic_model));
kinematic_state->setToDefaultValues();
//获取关节数据，robot为规划组
const robot_state::JointModelGroup *joint_model_group =
kinematic_model->getJointModelGroup("robot");
//设置角度
std::vector joint_values;
kinematic_state->copyJointGroupPositions(joint_model_group, joint_values);
joint_values[0] = 0.0;
joint_values[1] = 0.0;
joint_values[2] = 0.0;
joint_values[3] = 0.0;
joint_values[4] = 1.57;
joint_values[6] = 0.0;
kinematic_state->setJointGroupPositions(joint_model_group, joint_values);
//获取正解，effector为需要获取位姿的link
const Eigen::Affine3d &end_effector_state =
kinematic_state->getGlobalLinkTransform("effector");
ROS_INFO_STREAM("Translation: " << end_effector_state.translation());
ROS_INFO_STREAM("Rotation: " << end_effector_state.rotation()); 2.2 逆运动学
自定义一个目标位置，然后进行运动规划并驱动机械臂运动到指定位置。
#include
int main(int argc, char **argv)
{
ros::init(argc, argv, "move_group_interface_tutorial");
ros::NodeHandle node_handle;
ros::AsyncSpinner spinner(1);
spinner.start();
moveit::planning_interface::MoveGroupInterface group("robot");
moveit::planning_interface::MoveItErrorCode success;
//设置初始位置
group.setStartState(*group.getCurrentState());
//设置抓取目标点
geometry_msgs::Pose target_pose;
//末端姿态四元数
target_pose.orientation.w = 1.000000;
target_pose.orientation.x = 0.000000;
target_pose.orientation.y = 0.000000;
target_pose.orientation.z = 0.000000;
//末端姿态三维坐标
target_pose.position.x = -0.020000;
target_pose.position.y = -0.005000;
target_pose.position.z = 0.350000;
//进行运动规划
group.setPoseTarget(target_pose);
moveit::planning_interface::MoveGroupInterface::Plan plan;
success = group.plan(plan);
//运动规划输出
ROS_INFO("Visualizing plan (stateCatch pose) %s",success == moveit_msgs::MoveItErrorCodes::SUCCESS ? "SUCCESS" : "FAILED");
if (success == moveit_msgs::MoveItErrorCodes::SUCCESS) group.execute(plan);
ros::shutdown();
return 0;
} 注意1：Kinetic之后版本与其之前版本关于运动规划的api稍有不同，
Kinetic版本：
#include moveit::planning_interface::MoveGroupInterface group("robot");之前版本：
#include moveit::planning_interface::MoveGroup group("robot");注意2：若采用ros::AsyncSpinner建立多线程，操作需要在spinner.start()之后执行，同时需要在子函数或者类中调用move_group接口类需要增加线程数量。
详细参考：'Failed to fetch current robot state' when using the 'plan_kinematic_path' service
三、路径规划
3.1 路径约束
约束种类
- JointConstraint[] joint_constraints：关节约束
- PositionConstraint[] position_constraints：位置约束
- OrientationConstraint[] orientation_constraints：旋转约束
- VisibilityConstraint[] visibility_constraints：可见性约束
路径约束可有多个，通过给与不同权重进行重要性排序，完成规划后需清除约束。
moveit_msgs::Constraints endEffector_constraints;
moveit_msgs::OrientationConstraint ocm;
ocm.link_name = "gripper";//需要约束的链接
ocm.header.frame_id = "base_link";//基坐标系
//四元数约束
ocm.orientation.w = 1.0;
//欧拉角约束
ocm.absolute_x_axis_tolerance = 0.1;
ocm.absolute_y_axis_tolerance = 0.1;
ocm.absolute_z_axis_tolerance = 2*3.14;
ocm.weight = 1.0;//此限制权重
endEffector_constraints.orientation_constraints.push_back(ocm);//加入限制列表
group.setPathConstraints(endEffector_constraints);
group.clearPathConstraints();//清除约束下面例程为对末端执行器进行约束，在这里的规划希望末端执行器不会绕x轴和y轴旋转，而不在意其是否绕z轴旋转，在约束时可以以四元数的方式进行约束，也可以以欧拉角的方式进行约束。
#include
#include
#include
int main(int argc, char **argv)
{
ros::init(argc, argv, "move_group_interface_tutorial");
ros::NodeHandle node_handle;
ros::AsyncSpinner spinner(1);
spinner.start();
moveit::planning_interface::MoveGroupInterface group("robot");
moveit::planning_interface::MoveItErrorCode success;
//设置初始位置
group.setStartState(*group.getCurrentState());
//设置约束
moveit_msgs::Constraints endEffector_constraints;
moveit_msgs::OrientationConstraint ocm;
ocm.link_name = "gripper";//需要约束的链接
ocm.header.frame_id = "base_link";//基坐标系
//四元数约束
ocm.orientation.w = 1.0;
//欧拉角约束
ocm.absolute_x_axis_tolerance = 0.1;
ocm.absolute_y_axis_tolerance = 0.1;
ocm.absolute_z_axis_tolerance = 2*3.14;
ocm.weight = 1.0;//此限制权重
endEffector_constraints.orientation_constraints.push_back(ocm);//加入限制列表
group.setPathConstraints(endEffector_constraints);//设置约束
//设置抓取目标点
geometry_msgs::Pose target_pose;
//末端姿态四元数
target_pose.orientation.w = 1.000000;
target_pose.orientation.x = 0.000000;
target_pose.orientation.y = 0.000000;
target_pose.orientation.z = 0.000000;
//末端姿态三维坐标
target_pose.position.x = -0.020000;
target_pose.position.y = -0.005000;
target_pose.position.z = 0.350000;
//进行运动规划
group.setPoseTarget(target_pose);
moveit::planning_interface::MoveGroupInterface::Plan plan;
success = group.plan(plan);
group.clearPathConstraints();//清除约束
//运动规划输出
ROS_INFO("Visualizing plan (stateCatch pose) %s",success == moveit_msgs::MoveItErrorCodes::SUCCESS ? "SUCCESS" : "FAILED");
if (success == moveit_msgs::MoveItErrorCodes::SUCCESS) group.execute(plan);
ros::shutdown();
return 0;
} 3.2 笛卡尔坐标路径规划
在笛卡尔坐标系下进行路径规划，通过输入离散位姿构成一条空间轨迹。在moveit中提供computeCartesianPath函数用于路径规划，该函数为直线插补，函数说明如下：
double moveit::planning_interface::MoveGroup::computeCartesianPath(
const std::vector< geometry_msgs::Pose > & waypoints, //路径点
double eef_step,//末端最大步距
double jump_threshold,//机械臂最小移动距离，设置0.0则忽略检查
moveit_msgs::RobotTrajectory & trajectory,//计算返回路径
const moveit_msgs::Constraints & path_constraints,//路径约束
bool avoid_collisions = true,//是否检测碰撞
moveit_msgs::MoveItErrorCodes * error_code = NULL //返回错误代码
) 在路径规划对速度/加速度调整中默认使用default_planner_request_adapters/AddTimeParameterization模块，采用的算法为Time-Optimal Path Parameterization Algorithm算法，简称TOPP，只需要输入一条路径所有点的位置信息，就可以根据时间最优的原则，输出所有点的速度、加速度、时间信息，缺陷是存在加速度抖动的问题（可以改用TOTP）。
详细运动规划分析可参考： ROS进阶——运动规划分析
3.3 完整例程
#include
#include
#include
#include
#include
#include
int main(int argc, char **argv)
{
ros::init(argc, argv, "move_group_interface_tutorial");
ros::NodeHandle node_handle;
ros::AsyncSpinner spinner(1);
spinner.start();
moveit::planning_interface::MoveGroupInterface group("robot");
moveit::planning_interface::MoveItErrorCode success;
//设置初始位置
group.setStartState(*group.getCurrentState());
//设置约束
moveit_msgs::Constraints endEffector_constraints;
moveit_msgs::OrientationConstraint ocm;
ocm.link_name = "gripper";//需要约束的链接
ocm.header.frame_id = "base_link";//基坐标系
//四元数约束
ocm.orientation.w = 1.0;
//欧拉角约束
ocm.absolute_x_axis_tolerance = 0.1;
ocm.absolute_y_axis_tolerance = 0.1;
ocm.absolute_z_axis_tolerance = 2*3.14;
ocm.weight = 1.0;//此限制权重
endEffector_constraints.orientation_constraints.push_back(ocm);//加入限制列表
//设置运动路径
std::vector waypoints;
geometry_msgs::Pose target_pose;
target_pose.orientation.w = 1.000000;
target_pose.orientation.x = 0.000000;
target_pose.orientation.y = 0.000000;
target_pose.orientation.z = 0.000000;
target_pose.position.x = 0.000789;
target_pose.position.y = -0.089177;
target_pose.position.z = 0.247533;
waypoints.push_back(target_pose);
target_pose.orientation.w = 1.000000;
target_pose.orientation.x = 0.000000;
target_pose.orientation.y = 0.000000;
target_pose.orientation.z = 0.000000;
target_pose.position.x = -0.020000;
target_pose.position.y = -0.005000;
target_pose.position.z = 0.350000;
waypoints.push_back(target_pose);
//进行运动规划
moveit_msgs::RobotTrajectory trajectory_msg;
double fraction = group.computeCartesianPath( waypoints,
0.01, // eef_step,
0.0, // jump_threshold
trajectory_msg,
endEffector_constraints,//constraints
false);
ROS_INFO("Visualizing plan (cartesian path) (%.2f%% acheived)",fraction * 100.0);
group.clearPathConstraints();
//输出运动
moveit::planning_interface::MoveGroupInterface::Plan plan;
plan.trajectory_ = trajectory_msg;
group.execute(plan);
ros::shutdown();
return 0;
}
四、输出规划位置、速度和加速度曲线
通过MoveIt!进行运动规划采用的是五次样条曲线进行拟合，具体可看http://www.guyuehome.com/752。在ROS内提供了rqt_plot工具用于将数据图形化，但通过MoveIt!规划的结果无法直接通过rqt_plot进行呈现，需要获取规划结果后编写发布器将数据打包发送再用rqt_plot接收查看。
ros::Publisher plan_positions_pub = nh.advertise("/plan/fake_robot_state", 100);
void pubMotionData(trajectory_msgs::JointTrajectory planData) {
sensor_msgs::JointState fake_robot_state;
fake_robot_state.header = planData.header;
ros::Time init_time(0.0);
for (int i = 0; i < planData.points.size(); i++) {
fake_robot_state.header.stamp = init_time + planData.points[i].time_from_start;
fake_robot_state.position = planData.points[i].positions;
fake_robot_state.velocity = planData.points[i].velocities;
fake_robot_state.effort = planData.points[i].accelerations;
plan_positions_pub.publish(fake_robot_state);
}
} 
提示：左按可拖动界面，右按左右拖动可改变X轴间距，右按上下拖动可改变Y轴间距。
参考
ROS探索总结（二十六）——MoveIt编程
机械臂轨迹规划——三次样条对比研究
moveit_tutorials-kinetic
moveit概述