ROS进阶——MoveIt!基础编程

其他 2018-08-08 15:03:11 阅读次数: 0

 环境：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++库，主要用于运动规划，其中包含了大量用于运动规划的先进算法。该库中的算法以采样规划算法为主，通过其可以实现不同目标点之间的路径规划。


二、自定义目标位置并完成规划

    编程调用C++ API自定义一个目标位置，然后进行运动规划并驱动机械臂运动到指定位置。

#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;
}

注意：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");


三、添加路径约束

    路径约束可有多个，通过给与不同权重进行重要性排序，完成规划后需清除约束。

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

    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
    ItSuccess = iptp.computeTimeStamps(rt);
    ROS_INFO("Computed time stamp %s",ItSuccess?"SUCCEDED":"FAILED");
    // Get RobotTrajectory_msg from RobotTrajectory
    rt.getRobotTrajectoryMsg(trajectory_msg);

完整例程

#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);//加入限制列表
   group.setPathConstraints(endEffector_constraints);
	
    //设置运动路径
    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, 
						                            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
    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;
    sleep(1.0);
    group.execute(plan);
    sleep(1.0);

   ros::shutdown(); 
   return 0;
}


参考

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

猜你喜欢

转载自blog.csdn.net/Kalenee/article/details/80818658
今日推荐
基于大语言模型的开源知识库问答系统 MaxKB GitHub Star 数量突破 5,000 个！
美国拟限制 AI 大模型出口中国和俄罗斯
苹果将与 OpenAI 达成协议，将 ChatGPT 应用于 iPhone
openKylin 社区生态委员会第六次会议圆满召开
阿里云正式发布通义千问 2.5
Python 3.13 发布首个 Beta：实验性自由线程模式和 JIT、改进交互式解释器
Stack Overflow 拿我的代码去训练 AI 大模型，还封了我的账号
Pop!_OS 的 COSMIC 桌面完成 App Store 上架工作
《2024 年一季度互联网投融资运行情况》研究报告
报告：Django 仍然是 74% 开发者的首选
15 年前上了“FFmpeg 耻辱柱”，今天他还得谢谢咱——腾讯QQPlayer一雪前耻？
TIOBE 5 月榜单：Fortran “复活”进入 Top 10
周排行
记一下去大梅沙的准备（2018-05-26）
Spring 注解 事务
基于HTTP协议的客户端缓存
阿里云rds 备份和还原
[PHP] 几个拖慢 PHP 程序/API 运行速度的点
python 代码风格------------PEP8规则
js控制json生成菜单——自制菜单（一）
将字符串: 'k:1|k1:2|k2:3|k3:4 ' ,处理成 python 字典: {'k':1, 'k1':2, ...}
微信小程序转支付宝小程序
Qt551.窗口滚动条
每日归档
更多
2024-05-13(18)
2024-05-12(0)
2024-05-11(38)
2024-05-10(38)
2024-05-09(35)
2024-05-08(42)
2024-05-07(14)
2024-05-06(40)
2024-05-05(0)
2024-05-04(7)

代码天地 © 2018 codetd.com

境外服务器   最新电视剧2022