最近在调试机械臂的项目，需要涉及机械臂的路径规划，于是在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之前的字母修改到代码中去
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
