Moveit! 学习记录1——moveit入门

最近在调试机械臂的项目，需要涉及机械臂的路径规划，于是在B站学习了相关教程

视频连接哈萨克斯坦x的个人空间-哈萨克斯坦x个人主页-哔哩哔哩视频讲得比较清楚。

好记性不如烂笔头，写篇博客记录下。

一.简介

以视觉抓取为例

主要涉及运动学、运动规划、碰撞感知

二.Moveit！核心节点

三.机器人的ros包

四.Moveit！使用流程

前三个部分下载下来的功能包一般都做好了

4.1.使用Setup Assistant工具生成配置文件

4.1.1先在终端启动roscore，进入Setup Assistant工具下

在输入rosrun moveit_setup_assistant moveit_setup_assistant

共有两个选项，左侧是创建一个新的包，右侧是使用已有的包

下面以创建一个新的包为例

4.1.2导入description文件夹下的urdf文件

如果不能成功导入，可以进入到功能包的工作空间下，source一下，在rosrun moveit_setup_assistant moveit_setup_assistant

如果还不能成功导入，考虑文件本身不构成机器人模型

4.1.3 碰撞检测

直接点击Generate Collision Matrix 用于检测各个关节之间有没有可能发生碰撞

4.1.4 虚拟坐标系

目前不需要

相当于机械臂基坐标系和世界坐标系产生一个距离，不设置的话默认在world中心

4.1.5 Planning Group（最重要）

首先group name一般习惯设置： mainpulator

Kinematic Solver（正逆运动学求解器）：一般选择kdl，有的机械臂也提供了它本身自带的求解器

OMPL Planning（路径规划算法）：一般使用RRTConnect，比较快

最后一定要Add Kin.Chain,然后expand all collapse all Base link部分填base_link,Tip Link部分填ee_link（我这里是link_tcp），记着save

4.1.6剩余部分

robot poses：设置一些简单的pose，方便直接回到某些点

author information ：写邮箱和名字

最后点exit生成

使用已有的moveit

和上面一样，加载一个moveit_config文件，然后可查看各种信息

4.1.7moveit_config包

主要包含config和launch两个文件

其中 config中 controller是和控制器相关的，joint_limits是和关节限位相关的，kinematics是和感知有关的，ompl是轨迹规划相关的，sensors是和视觉感知有关，srdf是moveit的一个配置文件，之前设置的相关内容都会在里面显示

另外launch文件夹下 最重要的有两个，demo和planner相关的

4.2 C++实现Moveit!路径规划

直接看up主给的开源代码，之后使用直接按需求修改添加就行

#include <ros/ros.h>

#include <moveit/move_group_interface/move_group_interface.h>

#include <moveit/robot_trajectory/robot_trajectory.h>

#include <moveit/planning_scene_interface/planning_scene_interface.h>

#include <moveit_msgs/AttachedCollisionObject.h>

#include <moveit_msgs/CollisionObject.h>

#include <tf2/LinearMath/Quaternion.h>

#include <vector>

#include <string>

#include <iostream>

using namespace std;

//创建了一个类，在下面的函数中调用

class MoveIt_Control

{

public:

//在类的构造函数中，设置了机械臂的各种参数，如容忍的位置和姿态偏差、最大加速度和速度缩放因子、控制规划组、目标参考坐标系等。

MoveIt_Control(const ros::NodeHandle &nh,moveit::planning_interface::MoveGroupInterface &arm,const string &PLANNING_GROUP) {

this->arm_ = &arm;

this->nh_ = nh;

arm_->setGoalPositionTolerance(0.001);

//设置机械臂在运动到目标位置时允许的位置容忍度为0.001米。这意味着当机械臂的末端执行器与目标位置的位置差小于或等于0.001米时，就认为已经到达目标位置。

arm_->setGoalOrientationTolerance(0.01);

//设置机械臂在运动到目标姿态时允许的姿态容忍度为0.01弧度。这意味着当机械臂的末端执行器与目标姿态的姿态差小于或等于0.01弧度时，就认为已经到达目标姿态。

arm_->setGoalJointTolerance(0.001);

//设置机械臂在运动到目标关节角时允许的关节角容忍度为0.001弧度。这表示当机械臂的各关节角与目标关节角的差小于或等于0.001弧度时，就认为已经到达目标关节角。

arm_->setMaxAccelerationScalingFactor(0.5);

//设置机械臂的最大加速度缩放因子为0.5。这个因子可以将机械臂的最大加速度进行缩放，从而限制机械臂运动时的加速度上限。通过设置较小的缩放因子，可以使机械臂的运动更加平缓和缓慢。范围是0-1

arm_->setMaxVelocityScalingFactor(0.5);

//设置机械臂的最大速度缩放因子为0.5。这个因子可以将机械臂的最大速度进行缩放，从而限制机械臂运动时的速度上限。通过设置较小的缩放因子，可以使机械臂的运动速度更加慢和精确。

const moveit::core::JointModelGroup *joint_model_group = arm_->getCurrentState()->getJointModelGroup(PLANNING_GROUP);

//arm_->getCurrentState()：通过getCurrentState()方法获得机械臂的当前状态，即当前的关节角度和链接姿态等信息。

//getJointModelGroup(PLANNING_GROUP)：通过getJointModelGroup()方法，传入一个规划组（Planning group）的名称作为参数，来获取对应的关节模型组（JointModelGroup）对象。

this->end_effector_link = arm_->getEndEffectorLink();

//通过getEndEffectorLink()方法，获取机械臂的末端执行器（也称为末端执行器关节）的链接名称。末端执行器是机械臂末端的执行器，用于实际执行各种任务，如抓取、放置等。该代码将链接名称存储在end_effector_link变量中，以供后续使用。

this->reference_frame = "base";

//设置参考坐标系为"link_base"。参考坐标系是机械臂运动规划和控制时的参考框架或基准，用于确定位置和姿态的参考点。在这里，通过将"link_base"赋值给reference_frame变量，将参考坐标系设置为"link_base"。

arm_->setPoseReferenceFrame(reference_frame);

//设置机械臂的姿态参考坐标系为reference_frame。姿态参考坐标系用于表示机械臂的目标姿态或位置。通过设置参考坐标系，可以确保机械臂在规划和控制时使用正确的参考框架。

//规划部分

arm_->allowReplanning(true);

//允许机械臂进行重新规划。当设为true时，如果机械臂在执行过程中遇到障碍物或其它问题，会自动重新规划路径以避开障碍物或实现更优的路径。

arm_->setPlanningTime(5.0);

//设置机械臂的运动规划时间为5.0秒。运动规划时间指规划器分配给机械臂进行路径规划的时间限制。上面设置了true，5秒规划失败了之后会重新继续规划

arm_->setPlannerId("TRRT");

//设置规划器的ID为"TRRT"。规划器（Planner）是用于机械臂路径规划的算法。

//调用了以下两个函数

go_home(); //回到了home状态，这里是到达up点

create_table(); //创建了一个环境，并发布出来

}

void go_home() {

// moveit::planning_interface::MoveGroupInterface arm("manipulator");

arm_->setNamedTarget("up");

arm_->move();

sleep(0.5);

}

//调用这个函数需要传入六个关节的值，并移动到该姿态

bool move_j(const vector<double> &joint_group_positions) {

// moveit::planning_interface::MoveGroupInterface arm("manipulator");

arm_->setJointValueTarget(joint_group_positions);

arm_->move();

sleep(0.5);

return true;

}

//调用这个函数需要传入六个值xyzrpy，并移动到该位置

bool move_p(const vector<double> &pose) {

// moveit::planning_interface::MoveGroupInterface arm("manipulator");

// const std::string reference_frame = "base";

// arm.setPoseReferenceFrame(reference_frame);

geometry_msgs::Pose target_pose;

target_pose.position.x = pose[0];

target_pose.position.y = pose[1];

target_pose.position.z = pose[2];

tf2::Quaternion myQuaternion;

myQuaternion.setRPY(pose[3], pose[4], pose[5]); //因为moveit都是用四元数去表示姿态的，所以要把rpy转换成四元数

target_pose.orientation.x = myQuaternion.getX();

target_pose.orientation.y = myQuaternion.getY();

target_pose.orientation.z = myQuaternion.getZ();

target_pose.orientation.w = myQuaternion.getW();

arm_->setStartStateToCurrentState();

arm_->setPoseTarget(target_pose);

moveit::planning_interface::MoveGroupInterface::Plan plan;

moveit::planning_interface::MoveItErrorCode success = arm_->plan(plan);

ROS_INFO("move_p:%s", success ? "SUCCESS" : "FAILED");

if (success) {

arm_->execute(plan);

sleep(1);

return true;

}

return false;

}

//这个和上面的区别：上面的move_p在规划时，到达目标点的国策划嗯中末端的姿态可能会发生改变；而move_p_with_constrains在规划时末端姿态不会发生改变，比如末端的夹爪

bool move_p_with_constrains(const vector<double>& pose) {

// moveit::planning_interface::MoveGroupInterface arm("manipulator");

// const std::string reference_frame = "base";

// arm.setPoseReferenceFrame(reference_frame);

// arm.setPlannerId("TRRT");

arm_->setMaxAccelerationScalingFactor(0.5);

arm_->setMaxVelocityScalingFactor(0.5);

geometry_msgs::Pose target_pose;

target_pose.position.x = pose[0];

target_pose.position.y = pose[1];

target_pose.position.z = pose[2];

tf2::Quaternion myQuaternion;

myQuaternion.setRPY(pose[3], pose[4], pose[5]);

target_pose.orientation.x = myQuaternion.getX();

target_pose.orientation.y = myQuaternion.getY();

target_pose.orientation.z = myQuaternion.getZ();

target_pose.orientation.w = myQuaternion.getW();

// geometry_msgs::PoseStamped current_pose_modified = arm.getCurrentPose(this->end_effector_link);

// current_pose_modified.header.frame_id = "base";

// current_pose_modified.pose.position.x = -current_pose_modified.pose.position.x ;

// current_pose_modified.pose.position.y = -current_pose_modified.pose.position.y ;

// current_pose_modified.pose.orientation.x = myQuaternion.getX();

// current_pose_modified.pose.orientation.y = myQuaternion.getY();

// current_pose_modified.pose.orientation.z = myQuaternion.getZ();

// current_pose_modified.pose.orientation.w = myQuaternion.getW();

// arm.setPoseTarget(current_pose_modified.pose);arm.move();

//set constraint

moveit_msgs::OrientationConstraint ocm;

ocm.link_name = "base";

ocm.header.frame_id = "ee_link";

ocm.orientation.x = myQuaternion.getX();

ocm.orientation.y = myQuaternion.getY();

ocm.orientation.z = myQuaternion.getZ();

ocm.orientation.w = myQuaternion.getW();

ocm.absolute_x_axis_tolerance = 0.1; //但是在规划时不能完全保证他末端姿态不发生改变，所以设置了容忍系数，若是设置成0的话可能会规划失败

ocm.absolute_y_axis_tolerance = 0.1;

ocm.absolute_z_axis_tolerance = 0.1;

ocm.weight = 1.0;

// Now, set it as the path constraint for the group.

moveit_msgs::Constraints test_constraints;

test_constraints.orientation_constraints.push_back(ocm);

arm_->setPathConstraints(test_constraints); //把限制加进来

/*moveit::core::RobotState start_state(*move_group_interface.getCurrentState());

geometry_msgs::Pose start_pose2;

start_pose2.orientation.w = 1.0;

start_pose2.position.x = 0.55;

start_pose2.position.y = -0.05;

start_pose2.position.z = 0.8;

start_state.setFromIK(joint_model_group, start_pose2);

move_group_interface.setStartState(start_state);*/

// Now we will plan to the earlier pose target from the new

// start state that we have just created.

arm_->setStartStateToCurrentState();

arm_->setPoseTarget(target_pose); //到达目标点

// Planning with constraints can be slow because every sample must call an inverse kinematics solver.

// Lets increase the planning time from the default 5 seconds to be sure the planner has enough time to succeed.

arm_->setPlanningTime(10.0); //规划的时候时间相较于上面的的设置长一点

moveit::planning_interface::MoveGroupInterface::Plan plan;

moveit::planning_interface::MoveItErrorCode success = arm_->plan(plan);

ROS_INFO("move_p_with_constrains :%s", success ? "SUCCESS" : "FAILED");

//下面再把规划时间设置回5s，并清除掉添加的限制

arm_->setPlanningTime(5.0);

arm_->clearPathConstraints();

if (success) {

arm_->execute(plan);

sleep(1);

return true;

}

return false;

}

//传入一个点，直线运动到这个点

bool move_l(const vector<double>& pose) {

// moveit::planning_interface::MoveGroupInterface arm("manipulator");

vector<geometry_msgs::Pose> waypoints;

geometry_msgs::Pose target_pose;

target_pose.position.x = pose[0];

target_pose.position.y = pose[1];

target_pose.position.z = pose[2];

tf2::Quaternion myQuaternion;

myQuaternion.setRPY(pose[3], pose[4], pose[5]);

target_pose.orientation.x = myQuaternion.getX();

target_pose.orientation.y = myQuaternion.getY();

target_pose.orientation.z = myQuaternion.getZ();

target_pose.orientation.w = myQuaternion.getW();

waypoints.push_back(target_pose);

moveit_msgs::RobotTrajectory trajectory;

const double jump_threshold = 0.0;

const double eef_step = 0.01;

double fraction = 0.0;

int maxtries = 100;

int attempts = 0;

while (fraction < 1.0 && attempts < maxtries)

{

fraction = arm_->computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory);

attempts++;

}

if (fraction == 1)

{

ROS_INFO("Path computed successfully. Moving the arm.");

moveit::planning_interface::MoveGroupInterface::Plan plan;

plan.trajectory_ = trajectory;

arm_->execute(plan);

sleep(1);

return true;

}

else

{

ROS_INFO("Path planning failed with only %0.6f success after %d attempts.", fraction, maxtries);

return false;

}

}

//函数重载，根据传入不同的参数类型选择不同的move_l

//可以一次传入多个点，从一个点直线移动到下一个点

bool move_l(const vector<vector<double>>& posees) {

// moveit::planning_interface::MoveGroupInterface arm("manipulator");

vector<geometry_msgs::Pose> waypoints;

for (int i = 0; i < posees.size(); i++) {

geometry_msgs::Pose target_pose;

target_pose.position.x = posees[i][0];

target_pose.position.y = posees[i][1];

target_pose.position.z = posees[i][2];

tf2::Quaternion myQuaternion;

myQuaternion.setRPY(posees[i][3], posees[i][4], posees[i][5]);

target_pose.orientation.x = myQuaternion.getX();

target_pose.orientation.y = myQuaternion.getY();

target_pose.orientation.z = myQuaternion.getZ();

target_pose.orientation.w = myQuaternion.getW();

waypoints.push_back(target_pose);

}

moveit_msgs::RobotTrajectory trajectory;

const double jump_threshold = 0.0;

const double eef_step = 0.01;

double fraction = 0.0;

int maxtries = 100;

int attempts = 0;

while (fraction < 1.0 && attempts < maxtries)

{

fraction = arm_->computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory);

attempts++;

}

if (fraction == 1)

{

ROS_INFO("Path computed successfully. Moving the arm.");

moveit::planning_interface::MoveGroupInterface::Plan plan;

plan.trajectory_ = trajectory;

arm_->execute(plan);

sleep(1);

return true;

}

else

{

ROS_INFO("Path planning failed with only %0.6f success after %d attempts.", fraction, maxtries);

return false;

}

}

//创建了一个table，并发布出去

void create_table() {

// Now let's define a collision object ROS message for the robot to avoid.

ros::Publisher planning_scene_diff_publisher = nh_.advertise<moveit_msgs::PlanningScene>("planning_scene", 1);

ros::WallDuration sleep_t(0.5);

while (planning_scene_diff_publisher.getNumSubscribers() < 1)

{

sleep_t.sleep();

}

moveit::planning_interface::PlanningSceneInterface planning_scene_interface;

moveit_msgs::PlanningScene planning_scene;

moveit_msgs::CollisionObject collision_object;

collision_object.header.frame_id = arm_->getPlanningFrame();

// The id of the object is used to identify it.

collision_object.id = "table";

// Define a box to add to the world.

shape_msgs::SolidPrimitive primitive;

primitive.type = primitive.BOX;

primitive.dimensions.resize(3);

primitive.dimensions[primitive.BOX_X] = 2; //长宽2米，高0.01米

primitive.dimensions[primitive.BOX_Y] = 2;

primitive.dimensions[primitive.BOX_Z] = 0.01;

// Define a pose for the box (specified relative to frame_id)

geometry_msgs::Pose box_pose;

box_pose.orientation.w = 1.0; //设置box的位姿

box_pose.position.x = 0.0;

box_pose.position.y = 0.0;

box_pose.position.z = -0.01/2 -0.02;

collision_object.primitives.push_back(primitive);

collision_object.primitive_poses.push_back(box_pose);

collision_object.operation = collision_object.ADD; //若要放置两个物体，把从这往上到Define a box to add to the world再复制一份

planning_scene.world.collision_objects.push_back(collision_object);

planning_scene.is_diff = true;

planning_scene_diff_publisher.publish(planning_scene);

ROS_INFO("Added an table into the world");

}

//可能会用到的函数

void some_functions_maybe_useful(){

// moveit::planning_interface::MoveGroupInterface arm("manipulator");

//获得当前的位姿

geometry_msgs::PoseStamped current_pose = this->arm_->getCurrentPose(this->end_effector_link);

ROS_INFO("current pose:x:%f,y:%f,z:%f,Quaternion:[%f,%f,%f,%f]",current_pose.pose.position.x,current_pose.pose.position.y,

current_pose.pose.position.z,current_pose.pose.orientation.x,current_pose.pose.orientation.y,

current_pose.pose.orientation.z,current_pose.pose.orientation.w);

//获得当前的关节角度

std::vector<double> current_joint_values = this->arm_->getCurrentJointValues();

ROS_INFO("current joint values:%f,%f,%f,%f,%f,%f",current_joint_values[0],current_joint_values[1],current_joint_values[2],

current_joint_values[3],current_joint_values[4],current_joint_values[5]);

//获得末端的RPY

std::vector<double> rpy = this->arm_->getCurrentRPY(this->end_effector_link);

ROS_INFO("current rpy:%f,%f,%f",rpy[0],rpy[1],rpy[2]);

//获得当前的规划算法

string planner = this->arm_->getPlannerId();

ROS_INFO("current planner:%s",planner.c_str());

std::cout<<"current planner:"<<planner<<endl;

}

~MoveIt_Control() {

ros::shutdown();

}

 

public:

string reference_frame;

string end_effector_link;

ros::NodeHandle nh_;

moveit::planning_interface::MoveGroupInterface *arm_; //创建了一个arm_指针,上面把arm的地址传给了arm_

};

int main(int argc, char** argv) {

ros::init(argc, argv, "moveit_control_server_cpp");

ros::AsyncSpinner spinner(1);

ros::NodeHandle nh;

spinner.start();

static const std::string PLANNING_GROUP = "manipulator";

moveit::planning_interface::MoveGroupInterface arm(PLANNING_GROUP);

//使用moveit库创建一个MoveGroupInterface对象来控制特定的规划组（“manipulator”）的机械臂，调用PlanningSceneInterface

//关节模型组（JointModelGroup）包含了机械臂中一组关联的关节，可以通过这个对象进行关节空间的规划和控制。它提供了一些常用的功能，如获取关节名称、关节数量、关节边界等

MoveIt_Control moveit_server(nh,arm,PLANNING_GROUP);//创建一个MoveIt_Control对象，传递ros::NodeHandle、MoveGroupInterface和规划组名称作为参数，并调用了moveit_control的构造函数

 

// Test 测试案例

// test for move_j

cout<<"-----------------------test for move_j----------------------"<<endl;

vector<double> joints ={0,0,-1.57,0,0,0};

moveit_server.move_j(joints);

// test for move_p and move_l(1 point)

cout<<"-----------------------test for move_p and move_l---------------------"<<endl;

vector<double> xyzrpy={0.3,0.1,0.4,-3.1415,0,0};

moveit_server.move_p(xyzrpy);

xyzrpy[2]=0.2;

moveit_server.move_l(xyzrpy);

// test for move_l (>=2 points)

cout<<"-----------------------test for move_l(more points)----------------------"<<endl;

vector<vector<double>> xyzrpys;

xyzrpys.push_back(xyzrpy);

xyzrpy[1]=0.2;

xyzrpys.push_back(xyzrpy);

xyzrpy[0]=0.4;

moveit_server.move_l(xyzrpys);

// test for move_p_with constrains

cout<<"-----------------------test for move_p_with_constrains----------------------"<<endl;

vector<double> pose1={0.4,0,0.4,0,3.141592/2,0};

moveit_server.move_p(pose1);

vector<double> pose2={0.4,0.2,0.2,0,3.141592/2,0}; //3.141592/2

moveit_server.move_p_with_constrains(pose2);

vector<double> pose3={0.0,0.5,0.3,0,3.141592/2,0};

moveit_server.move_p_with_constrains(pose3);

// test for some useful functions

cout<<"-----------------------test for other functions----------------------"<<endl;

moveit_server.some_functions_maybe_useful();

return 0;

}

 使用之前要修改cmakelist，添加cpp文件

add_executable(moveitServerCpp src/moveitServerCpp.cpp)

target_link_libraries(moveitServerCpp ${catkin_LIBRARIES})

五.路径规划算法的选择

5.1RRT系列算法理论基础

5.1.1为什么要路径规划

首先：避障，避免与静态物体和动态物体发生碰撞

其次：任务对运动的路径有要求，具有运动学和动力学约束

5.1.2路径规划分类

机械臂的运动规划算法一般选择基于采样的

5.1.3基于采样的路径规划算法

时间无穷大必能找到解

5.2Moveit内置算法

5.3更换路径规划算法(三种方式)

5.3.1Rviz

5.3.2 在moveit_setup_assistant

编译现有的moveit_config包

5.3.3在程序中调用相关接口更换

已C++为例

arm_->setPlannerId("TRRT");
 
更换后重新编译即可

可更换的算法可以在moveit_config/config/ompl_planning.yaml中查看

把ConfigDefault之前的字母修改到代码中去