ROS进阶——MoveIt!基础编程

 环境：Ubuntu16.04+ROS Kinetic

 

一、MoveIt!简介

   
在实现机械臂的自主抓取中机械臂的运动规划是其中最重要的一部分，其中包含运动学正逆解算、碰撞检测、环境感知和动作规划等。在本课题中，采用ROS系统提供的MoveIt!完成在操作系统层的运动规划。
    MoveIt!
是ROS系统中集合了与移动操作相关的组件包的运动规划库。它包含了运动规划中所需要的大部分功能，同时其提供友好的配置和调试界面便于完成机器人在ROS系统上的初始化及调试，其具体架构如下图所示。

 




（1）move_group：move_group是MoveIt!的核心部分，它的功能在于集成机器人的各独立模块并为用户提供一系列的动作指令和服务。其本身并不具备太多的功能，起到的是积分器的作用，完成各功能包和插件的集成。

（2）场景规划（Planning Scene）：通过场景规划，用户可以创建一个具体的工作环境或者加入障碍物。

（3）运动规划（motion
panning）：在MoveIt!中，运动规划器起的作用是实现运动规划算法，其以插件的方式通过ROS的pluginlib接口完成信息的通讯，根据需求可制作或选用不同的规划算法。


（4）运动学（Kinematics）：运动学算法是机械臂控制算法中的核心内容，其中包括正运动学算法和逆运动学算法，在MoveIt!中，运动学算法同样以插件的形式完成于ROS的集成，因此可根据实际需求，编写自己的运动学算法来控制机器人。

（5）碰撞检测（collision
checking）：为避免机器人自身或者和环境发生干涉导致意外的发生，在进行运动规划时碰撞检测是必不可少的一部分，在MoveIt!中，采用FCL（Flexible
Collision Library）进行对象碰撞的运算。

（6）开源运动规划库（open motion planning
library）：OMPL是一个的开源的C++库，主要用于运动规划，其中包含了大量用于运动规划的先进算法。该库中的算法以采样规划算法为主，通过其可以实现不同目标点之间的路径规划。

 

二、运动学

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<double>
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 <moveit/move_group_interface/move_group_interface.h> 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/move_group_interface/move_group_interface.h>
moveit::planning_interface::MoveGroupInterface group("robot");
之前版本：
#include <moveit/move_group_interface/move_group.h>
moveit::planning_interface::MoveGroup group("robot");
注意2：若采用ros::AsyncSpinner建立多线程，操作需要在spinner.start()
之后执行，同时需要在子函数或者类中调用move_group接口类需要增加线程数量。

https://github.com/ros-planning/moveit/issues/868
<https://github.com/ros-planning/moveit/issues/868>

 

三、路径约束

约束种类(http://docs.ros.org/melodic/api/moveit_msgs/html/msg/Constraints.html
<http://docs.ros.org/melodic/api/moveit_msgs/html/msg/Constraints.html>)

* 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 <moveit/move_group_interface/move_group_interface.h> #include
<moveit_msgs/AttachedCollisionObject.h> #include
<moveit_msgs/CollisionObject.h> 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; }
 

四、笛卡尔坐标路径规划

   
在笛卡尔坐标系下进行路径规划，通过输入路径途径点构成一条空间轨迹，并设置其中机械臂末端的姿态，注意离散轨迹点跨度不应太大，可以通过直线插值或者圆弧插值补充，或者将整个轨迹分成几个小轨迹进行规划后合成，完成路径规划后需要通过addTimeParametrization模块进行处理，为这条空间轨迹添加速度和加速度的约束。其算法采用的是 Time-Optimal
Path Parameterization
Algorithm，简称TOPP，只需要输入一条路径所有点的位置信息，就可以根据时间最优的原则，输出所有点的速度、加速度、时间信息，缺陷是存在加速度抖动的问题。

算法的原始实现：https://github.com/quangounet/TOPP

模块代码实现：
https://github.com/ros-planning/moveit/blob/melodic-devel/moveit_core/trajectory_processing/src/iterative_time_parameterization.cpp

<https://github.com/ros-planning/moveit/blob/melodic-devel/moveit_core/trajectory_processing/src/iterative_time_parameterization.cpp>

使用说明：https://github.com/ros-planning/moveit_core/issues/141
<https://github.com/ros-planning/moveit_core/issues/141>
moveit_msgs::RobotTrajectory trajectory_msg; double fraction =
group.computeCartesianPath( waypoints, 0.01, // eef_step, 0.0, //
jump_threshold trajectory_msg, false); ROS_INFO("Visualizing plan (cartesian
path) (%.2f%% acheived)",fraction * 100.0); // The trajectory needs to be
modified so it will include velocities as well. // First to create a
RobotTrajectory object robot_trajectory::RobotTrajectory
rt(group.getCurrentState()->getRobotModel(), "hand"); // Second get a
RobotTrajectory from trajectory
rt.setRobotTrajectoryMsg(*group.getCurrentState(), trajectory_msg); // Thrid
create a IterativeParabolicTimeParameterization object
trajectory_processing::IterativeParabolicTimeParameterization iptp; // Fourth
compute computeTimeStamps bool ItSuccess = iptp.computeTimeStamps(rt);
ROS_INFO("Computed time stamp %s",ItSuccess?"SUCCEDED":"FAILED"); // Get
RobotTrajectory_msg from RobotTrajectory
rt.getRobotTrajectoryMsg(trajectory_msg);
笛卡尔坐标系下的路径规划的约束方式与点动驱动的约束不同，为直接在函数中赋值。具体函数如下
double moveit::planning_interface::MoveGroup::computeCartesianPath( const
std::vector< geometry_msgs::Pose > & waypoints, double eef_step, double
jump_threshold, moveit_msgs::RobotTrajectory & trajectory, const
moveit_msgs::Constraints & path_constraints, bool avoid_collisions = true,
moveit_msgs::MoveItErrorCodes * error_code = NULL )
完整例程
#include <moveit/move_group_interface/move_group_interface.h> #include
<moveit_msgs/AttachedCollisionObject.h> #include
<moveit_msgs/CollisionObject.h> #include
<moveit/planning_request_adapter/planning_request_adapter.h> #include
<moveit/trajectory_processing/iterative_time_parameterization.h> #include
<geometry_msgs/PointStamped.h> 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<geometry_msgs::Pose> 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(); // The trajectory needs to be modified so
it will include velocities as well. // First to create a RobotTrajectory object
robot_trajectory::RobotTrajectory rt(group.getCurrentState()->getRobotModel(),
"hand"); // Second get a RobotTrajectory from trajectory
rt.setRobotTrajectoryMsg(*group.getCurrentState(), trajectory_msg); // Thrid
create a IterativeParabolicTimeParameterization object
trajectory_processing::IterativeParabolicTimeParameterization iptp; // Fourth
compute computeTimeStamps bool ItSuccess = iptp.computeTimeStamps(rt);
ROS_INFO("Computed time stamp %s",ItSuccess?"SUCCEDED":"FAILED"); // Get
RobotTrajectory_msg from RobotTrajectory
rt.getRobotTrajectoryMsg(trajectory_msg); //输出运动
moveit::planning_interface::MoveGroupInterface::Plan plan; plan.trajectory_ =
trajectory_msg; group.execute(plan); ros::shutdown(); return 0; }
 

五、输出规划位置、速度和加速度曲线

通过MoveIt!进行运动规划采用的是五次样条曲线进行拟合，具体可看http://www.guyuehome.com/752
<http://www.guyuehome.com/752>
。在ROS内提供了rqt_plot工具用于将数据图形化，但通过MoveIt!规划的结果无法直接通过rqt_plot进行呈现，需要获取规划结果后编写发布器将数据打包发送再用rqt_plot接收查看。
ros::Publisher plan_positions_pub =
nh.advertise<sensor_msgs::JointState>("/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轴间距。

 

参考

http://www.guyuehome.com/455

http://www.guyuehome.com/752


http://docs.ros.org/kinetic/api/moveit_tutorials/html/doc/move_group_interface/move_group_interface_tutorial.html

• « 上一篇：回归模型的几个评价指标
• » 下一篇：从2-3树到红黑树，B/B+/B*树，唉！