当前位置： 代码迷 >> 综合 >> MOVEIT
  详细解决方案

MOVEIT

热度：80   发布时间：2023-12-15 15:11:31.0

本文主要是学习记录，转载文章：https://www.freesion.com/article/8353235782/，如有侵权，自会删除。
一、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;
}

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

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_thresholdtrajectory_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 objectrobot_trajectory::RobotTrajectory rt(group.getCurrentState()->getRobotModel(), "hand");// Second get a RobotTrajectory from trajectoryrt.setRobotTrajectoryMsg(*group.getCurrentState(), trajectory_msg); // Thrid create a IterativeParabolicTimeParameterization objecttrajectory_processing::IterativeParabolicTimeParameterization iptp;// Fourth compute computeTimeStampsItSuccess = iptp.computeTimeStamps(rt);ROS_INFO("Computed time stamp %s",ItSuccess?"SUCCEDED":"FAILED");// Get RobotTrajectory_msg from RobotTrajectoryrt.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_thresholdtrajectory_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 objectrobot_trajectory::RobotTrajectory rt(group.getCurrentState()->getRobotModel(), "hand");// Second get a RobotTrajectory from trajectoryrt.setRobotTrajectoryMsg(*group.getCurrentState(), trajectory_msg); // Thrid create a IterativeParabolicTimeParameterization objecttrajectory_processing::IterativeParabolicTimeParameterization iptp;// Fourth compute computeTimeStampsItSuccess = iptp.computeTimeStamps(rt);ROS_INFO("Computed time stamp %s",ItSuccess?"SUCCEDED":"FAILED");// Get RobotTrajectory_msg from RobotTrajectoryrt.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;
}

五、输出规划位置、速度和加速度曲线
通过MoveIt!进行运动规划采用的是五次样条曲线进行拟合，具体可看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);}
}

参考链接：

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

  相关解决方案

代码迷 - 您身边的软件异常，代码异常，编程异常助手（发现，解决，分享）
Copyright © 2009-2013 DAIMAMI.COM. All rights reserved.