【Coppeliasim C++】焊接机械臂仿真

项目思维导图

该项目一共三个demo：

机械臂末端走直线

2. 变位机转台转动

3.机械臂末端多点样条运动

笔记：

基于等级的蚁群系统在3D网格地图中搜索路径的方法:

基于等级的蚁群系统(Hierarchical Ant Colony System,HACS)是一种改进的蚁群优化算法。它在传统的蚁群算法基础上,通过构建等级结构来优化搜索过程。

在3D网格地图中,我们可以将地图分为多个等级层次。最高层次是整张地图的概览,地图被等分为较大的网格区域。在较低层次,每个网格区域内又被等分为更小的子区域。最后,最底层是每个子区域内的详情网格。

在搜索路径时,蚁群按照层次顺序进行。在高层次,蚁群用更加宽泛的视野搜索整个地图,找到连接开始点和目标点的大致路径区域。然后在较低层次内搜索,逐步优化路径,找到更加详细和精确的路线。

与传统蚁群相比,这种分层搜索方法可以更快地锁定搜索区域,减少无效搜索,从而提高搜索效率。同时,低层次的详细搜索也可以找到更短和更优的路径。

总之,HACS利用地图的层次信息指导搜索,使蚁群系统既有高层次的宏观视野,也有低层次的局部优化能力。这种方法可有效提高在3D网格地图中搜索路径的性能。

使用蚁群系统解决广义旅行商问题：

广义旅行商问题(Generalized Traveling Salesman Problem, GTSP)是旅行商问题的推广,使用蚁群系统可以有效解决。

GTSP问题是将多个城市分组,要求旅行商访问每个组中的一个城市,并最小化总路程。蚁群算法解GTSP步骤如下:

构建解空间。将城市分组,每个组看作为一个虚拟城市。
蚁群搜索。蚁群按照传统TSP规则搜索路径,但每经过一个虚拟城市时,会随机选择该组内的一个真实城市访问。
信息更新。当蚁群完成一轮搜索后,更新信息素,包括每个城市内的信息素和连接两个虚拟城市的信息素。
重复搜索。反复迭代上述搜索和更新过程,逐步得到更优解。
结果输出。迭代终止后,输出当前最优解作为GTSP问题的近似最优解。

这种方法融合了蚁群算法的分布式搜索能力和 GTSP 问题的组内选择要求。相比暴力算法,可以大幅减少搜索空间,更快获得近似最优解。同时也比随机算法更有针对性。因此使用蚁群系统可以高效地求解 GTSP 问题。

要在windows系统下测试需修改Timer.h：

#ifndef PROJECT_TIMER_H
#define PROJECT_TIMER_H#include <assert.h>
#include <stdint.h>
#include <time.h>
#include <windows.h>//#include <ctime>class Timer 
{public:LARGE_INTEGER frequency;LARGE_INTEGER _startTime;explicit Timer() { start(); }// void start() { clock_gettime(CLOCK_REALTIME, &_startTime); }// clock_gettime(CLOCK_MONOTONIC, &_startTime);void start() {QueryPerformanceFrequency(&frequency);QueryPerformanceCounter(&_startTime);}double getMs() { return (double)getNs() / 1.e6; }int64_t getNs() {//struct timespec now;  LARGE_INTEGER now;QueryPerformanceCounter(&now);//clock_gettime(CLOCK_MONOTONIC, &now);//return (int64_t)(now.tv_nsec - _startTime.tv_nsec) +//1000000000 * (now.tv_sec - _startTime.tv_sec);return static_cast<double>(now.QuadPart - _startTime.QuadPart) / frequency.QuadPart*1.e9;}double getSeconds() { return (double)getNs() / 1.e9; }//struct timespec _startTime;
};#endif  // PROJECT_TIMER_H

main程序源码：

/* Includes ------------------------------------------------------------------*/
#include "matplotlibcpp.h"
#include <iostream>
#include "CoppeliaSim.h"
#include "sys_log.h"
#include "core/BSplineBasic.h"
#include "core/BezierCurve.h"
#include "core/Timer.h"
#include "core/ACSRank_3D.hpp"
#include "core/read_STL.hpp"
#include "core/ACS_GTSP.hpp"/* Usr defines ---------------------------------------------------------------*/
using namespace std;
namespace plt = matplotlibcpp;
enum Pose_t
{x,y,z,alpha,beta,_gamma};#ifndef M_PI
#define M_PI 3.14159265358979323846
#define M_PI_2 M_PI/2
#endif_simObjectHandle_Type *Tip_target;
_simObjectHandle_Type *Tip_op;
_simObjectHandle_Type *Joint[6];
_simObjectHandle_Type *platform[2];
_simSignalHandle_Type *weld_cmd;
/*Test*/
STLReader model;
ACS_Rank mySearchPath;//使用基于等级的蚁群系统在基于网格的 3D 地图中搜索路径，ACSRnk_3D 被优化为自动选择搜索参数。//
ACS_GTSP GlobalRoute;//使用蚁群系统解决广义旅行商问题//
BezierCurve<float, 3> straight_line(2);//模板类BezierCurve,用于生成贝塞尔曲线//
BezierCurve<float, 2> platform_angle(2);
BS_Basic<float, 3, 0, 0, 0> *smooth_curve1;/*B样条曲线*/
Timer timer;//计时器//
int demo_type;//演示类型//
bool is_running = false;
float start_time = 0;
float total_time = 0;
float current_pt[6];//当前点//
float target_pt[6];//目标点//
std::vector<float> smooth_x, smooth_y, smooth_z;
/* Founctions ----------------------------------------------------------------*/
// float[6], float[3]
bool go_next_point(float *next, float *res)
{static float last[6] = {0};bool state = false;for (int i(0); i < 6; i++){state |= (next[i] != last[i]) ? 1 : 0;}if(state){ float start_pt[3] = {current_pt[0], current_pt[1], current_pt[2]};float next_pt[3] = {next[0], next[1], next[2]};float **ctrl_pt = new float *[3];ctrl_pt[0] = start_pt;ctrl_pt[1] = next_pt;straight_line.SetParam(ctrl_pt, total_time);start_time = timer.getMs();for (int i(0); i < 6; i++){last[i] = next[i];}}float now_time = (float)timer.getMs() - start_time;if (now_time >= total_time)return false;else{straight_line.getCurvePoint(now_time, res);printf("This point: %.3f, %.3f, %.3f \n", res[0], res[1], res[2]);return true;}
}void manual_input()
{if (is_running == true)//运行中//{if (demo_type == 1)//演示类型1  机械手//{// exit: current time > move time ?float now_time = (float)timer.getMs() - start_time;if (now_time >= total_time)is_running = false;float res[3] = {};straight_line.getCurvePoint(now_time, res);//获取下一点//target_pt[0] = res[0];target_pt[1] = res[1];target_pt[2] = res[2];std::cout << "Target(x,y,z):" << target_pt[0] << ", " << target_pt[1] << ", " << target_pt[2] << endl;}else if (demo_type == 2)//演示类型2  转台//{// exit: current time > move time ?float now_time = (float)timer.getMs() - start_time;if (now_time >= total_time)is_running = false;float res[2];platform_angle.getCurvePoint(now_time, res);//获取转台的下一点：两个转角//platform[0]->obj_Target.angle_f = res[0];platform[1]->obj_Target.angle_f = res[1];std::cout << "Target(pitch, yaw):" << res[0] << ", " << res[1] << endl;}else//其他类型//{static int i = 0;if(i < smooth_x.size()){static clock_t lastTime = clock();if (clock() - lastTime >= 10){lastTime = clock();if (smooth_x[i] - target_pt[0] < 0.3 && smooth_y[i] - target_pt[1] < 0.3&& smooth_z[i] - target_pt[2] < 0.3){target_pt[0] = smooth_x[i];target_pt[1] = smooth_y[i];target_pt[2] = smooth_z[i];std::cout << "Target(x,y,z):" << target_pt[0] << ", " << target_pt[1] << ", " << target_pt[2] << endl;i++;}}}else{is_running = false;}// // 论文和答辩中简单的演示,简单的状态机////float* res;// static int stage = 0;// switch(stage)// {//     case 0://     {//         //到第一个点//         total_time = 3000;//         float next[3] = {mySearchPath.route_points[0].x, mySearchPath.route_points[0].y, mySearchPath.route_points[0].z};//         if(go_next_point(next,res))//         {//              target_pt[0] = res[0];//              target_pt[1] = res[1];//              target_pt[2] = res[2];//                // target_pt[0] = res[0];//             // target_pt[1] = res[1];//             // target_pt[2] = res[2];//         }//         else//         {//             start_time = timer.getMs();//             stage = 1;//         }//     }//     break;//     case 1://     {////         //开始焊接，到第二个点////         weld_cmd->target = 1;//         float next[3] = {mySearchPath.route_points[1].x, mySearchPath.route_points[1].y, mySearchPath.route_points[1].z};//         if(go_next_point(next,res))//         {//             /* target_pt[0] = res[0];//              target_pt[1] = res[1];//              target_pt[2] = res[2];*///              target_pt[0] = res[0];//              target_pt[1] = res[1];//              target_pt[2] = res[2];//         }//         else{//             const std::vector<ACS_Node<float> *> *path = mySearchPath.best_matrix[1][2].getPath();//             int pt_num = (*path).size();//             float start_pt[3] = {mySearchPath.route_points[1].x, mySearchPath.route_points[1].y, mySearchPath.route_points[1].z};//             float end_pt[3] = {mySearchPath.route_points[2].x, mySearchPath.route_points[2].y, mySearchPath.route_points[2].z};//             float **ctrl_pt = new float *[pt_num];//             for (int i = 0; i < pt_num; ++i)//             {//                 ctrl_pt[i] = new float[3];//                 ctrl_pt[i][0] = (*path)[i]->pt.x;//                 ctrl_pt[i][1] = (*path)[i]->pt.y;//                 ctrl_pt[i][2] = (*path)[i]->pt.z;//             }//             smooth_curve1 = new BS_Basic<float, 3, 0, 0, 0>(pt_num);//             smooth_curve1->SetParam(start_pt, end_pt, ctrl_pt, total_time);//             start_time = timer.getMs();//             weld_cmd->target = 0;//             stage = 2;//         }//     }//     break;//     case 2://     {//         //停止焊接，到下面焊路////         float now_time = (float)timer.getMs() - start_time;//         if(now_time < total_time + 500)//         {//             smooth_curve1->getCurvePoint(now_time, res);//         }//         else//         {//             weld_cmd->target = 1;//             stage = 3;//         }//     }//     break;//     case 3://     {//         // 第二段焊路////         float next[3] = {mySearchPath.route_points[3].x, mySearchPath.route_points[3].y, mySearchPath.route_points[3].z};//         if(go_next_point(next,res))//         {//         }//         else//         {//             while(1){}//         }//     }//     break;//     default://         break;// }// target_pt[0] = res[0];// target_pt[1] = res[1];// target_pt[2] = res[2];//}}}else//未运行//{//Select type 选择类型//cout << "Please choose control type: 1) Manipulator 2) Platform 3) Demo : ";cin >> demo_type;if (demo_type == 1)//机械手//{//Set terminal pointsfloat start_pt[3] = {current_pt[0], current_pt[1], current_pt[2]};float next_pt[3];float **ctrl_pt = new float *[3];ctrl_pt[0] = start_pt;ctrl_pt[1] = next_pt;cout << "Current point:(" << current_pt[0] << ", " << current_pt[1] << ", " << current_pt[2] << ")" << endl;cout << "Next point(x, y, z) and Time(t): ";cin >> next_pt[0] >> next_pt[1] >> next_pt[2] >> total_time;straight_line.SetParam(ctrl_pt, total_time);//Set timestart_time = timer.getMs();is_running = true;}else if (demo_type == 2)//转台//{//Set terminal pointsfloat start_pt[2] = {platform[0]->obj_Data.angle_f, platform[1]->obj_Data.angle_f};float next_pt[2];float **ctrl_pt = new float *[2];ctrl_pt[0] = start_pt;ctrl_pt[1] = next_pt;cout << "Current point:(" << platform[0]->obj_Data.angle_f << ", " << platform[1]->obj_Data.angle_f << ")" << endl;cout << "Target angle(pitch, yaw) and Time(t): ";cin >> next_pt[0] >> next_pt[1] >> total_time;platform_angle.SetParam(ctrl_pt, total_time);//Set timestart_time = timer.getMs();is_running = true;}else if(demo_type == 3)//演示//{/*读取工件模型//*/model.readFile("./files/cubic.stl");const std::vector<Triangles<float>> meshes = model.TriangleList();/*搜索路径//*/mySearchPath.creatGridMap(meshes, 0.005, 10,"./files/cubic_grid_map.in");mySearchPath.searchBestPathOfPoints(0.5, "./files/cubic_weld_points.in", "./files/graph.in");//没有文件，//GlobalRoute.readFromGraphFile("./files/graph.in");GlobalRoute.computeSolution();GlobalRoute.read_all_segments(mySearchPath.best_matrix);/*曲线平滑//*/int pt_num = GlobalRoute.g_path_x.size();float start_pt[3] = {GlobalRoute.g_path_x[0], GlobalRoute.g_path_y[0], GlobalRoute.g_path_z[0]};float end_pt[3] = {GlobalRoute.g_path_x[pt_num - 1], GlobalRoute.g_path_y[pt_num - 1], GlobalRoute.g_path_z[pt_num - 1]};float **ctrl_pt = new float *[pt_num];for (int i = 0; i < pt_num; ++i){ctrl_pt[i] = new float[3];ctrl_pt[i][0] = GlobalRoute.g_path_x[i];ctrl_pt[i][1] = GlobalRoute.g_path_y[i];ctrl_pt[i][2] = GlobalRoute.g_path_z[i];}BS_Basic<float, 3, 0, 0, 0> smooth_curve(pt_num);smooth_curve.SetParam(start_pt, end_pt, ctrl_pt, 150);clock_t base_t = clock();clock_t now_t = clock()-base_t;float res[3];do{if(clock() - base_t - now_t >= 10){now_t = clock() - base_t;smooth_curve.getCurvePoint(now_t, res);smooth_x.push_back(res[0]);smooth_y.push_back(res[1]);smooth_z.push_back(res[2]);//printf("Curve point: %f, %f, %f, time:%d \n", res[0], res[1], res[2], now_t);}} while (now_t <= 150);//二次平滑//const float constrain = 0.05;pt_num = smooth_y.size();float second_start_pt[9] = {GlobalRoute.g_path_x[0], GlobalRoute.g_path_y[0], GlobalRoute.g_path_z[0],0,0,0,0,0,0}; float second_end_pt[9] = {GlobalRoute.g_path_x[pt_num - 1], GlobalRoute.g_path_y[pt_num - 1], GlobalRoute.g_path_z[pt_num - 1],0,0,0,0,0,0};float **second_pt = new float*[pt_num];for (int i = 0; i < pt_num; ++i){second_pt[i] = new float[9];second_pt[i][0] = smooth_x[i];second_pt[i][1] = smooth_y[i];second_pt[i][2] = smooth_z[i];for (int j(3); j < 9; j++)second_pt[i][j] = constrain;}smooth_x.clear();smooth_y.clear();smooth_z.clear();BS_Basic<float, 3, 2, 2, 2> second_curve(pt_num);second_curve.SetParam(second_start_pt,second_end_pt,second_pt, 6000);base_t = clock();now_t = clock()-base_t;do{if(clock() - base_t - now_t >= 50){now_t = clock() - base_t;second_curve.getCurvePoint(now_t, res);smooth_x.push_back(res[0]);smooth_y.push_back(res[1]);smooth_z.push_back(res[2]);//printf("Second point: %f, %f, %f, time:%d \n", res[0], res[1], res[2], now_t);}} while (now_t <= 6000);start_time = timer.getMs();is_running = true;}else{cout << "Unidentified type, please select again." << endl;}}
}
/**
* @brief This is the main function for user.
*/
void Usr_Main()
{//这里是主循环，可以运行我们的各部分算法//manual_input();
}/**
* @brief User can config simulation client in this function.
* @note  It will be called before entering the main loop.   
*/
void Usr_ConfigSimulation() //读取句柄//
{//添加关节对象到Joint_list，每个关节可以读写位置和速度，不用单独控制每个关节可以注释下面这段//Joint[0] = CoppeliaSim->Add_Object("IRB4600_joint1", JOINT, {SIM_VELOCITY | CLIENT_RW, SIM_POSITION | CLIENT_RW});Joint[1] = CoppeliaSim->Add_Object("IRB4600_joint2", JOINT, {SIM_VELOCITY | CLIENT_RW, SIM_POSITION | CLIENT_RW});Joint[2] = CoppeliaSim->Add_Object("IRB4600_joint3", JOINT, {SIM_VELOCITY | CLIENT_RW, SIM_POSITION | CLIENT_RW});Joint[3] = CoppeliaSim->Add_Object("IRB4600_joint4", JOINT, {SIM_VELOCITY | CLIENT_RW, SIM_POSITION | CLIENT_RW});Joint[4] = CoppeliaSim->Add_Object("IRB4600_joint5", JOINT, {SIM_VELOCITY | CLIENT_RW, SIM_POSITION | CLIENT_RW});Joint[5] = CoppeliaSim->Add_Object("IRB4600_joint6", JOINT, {SIM_VELOCITY | CLIENT_RW, SIM_POSITION | CLIENT_RW});//读写执行末端相对于器件坐标系的位姿//Tip_target = CoppeliaSim->Add_Object("IRB4600_IkTarget", OTHER_OBJECT, {SIM_POSITION | CLIENT_WO, SIM_ORIENTATION | CLIENT_WO});Tip_op = CoppeliaSim->Add_Object("IRB4600_IkTip", OTHER_OBJECT, {SIM_POSITION | CLIENT_RO, SIM_ORIENTATION | CLIENT_RO});platform[0] = CoppeliaSim->Add_Object("platform_yaw", JOINT, {SIM_POSITION | CLIENT_RW});platform[1] = CoppeliaSim->Add_Object("platform_pitch", JOINT, {SIM_POSITION | CLIENT_RW});weld_cmd = CoppeliaSim->Add_Object("weld_cmd", SIM_INTEGER_SIGNAL, {SIM_SIGNAL_OP | CLIENT_WO});/*Init value*/target_pt[x] = 1.76; //-0.2;target_pt[y] = 0.09;target_pt[z] = 1.42;target_pt[alpha] = 0;target_pt[beta] = M_PI_2 + M_PI_2/2;target_pt[_gamma] = -M_PI_2;Tip_target->obj_Target.position_3f[0] = target_pt[x] + 0;//1.7;  Tip_target->obj_Target.position_3f[1] = target_pt[y] + 0;Tip_target->obj_Target.position_3f[2] = target_pt[z] + 0;Tip_target->obj_Target.orientation_3f[0] = target_pt[alpha];Tip_target->obj_Target.orientation_3f[1] = target_pt[beta];Tip_target->obj_Target.orientation_3f[2] = target_pt[_gamma];
}/**
* @brief These two function will be called for each loop.
*        User can set their message to send or read from sim enviroment.
*/
void Usr_SendToSimulation()//设置目标位姿//
{//这里可以设置关节指令//Tip_target->obj_Target.position_3f[0] = target_pt[x] + 0; //1.7;Tip_target->obj_Target.position_3f[1] = target_pt[y] + 0;Tip_target->obj_Target.position_3f[2] = target_pt[z] + 0;Tip_target->obj_Target.orientation_3f[0] = target_pt[alpha];Tip_target->obj_Target.orientation_3f[1] = target_pt[beta];Tip_target->obj_Target.orientation_3f[2] = target_pt[_gamma];
}
//读取tip当前位姿参数//
void Usr_ReadFromSimulation()
{//这里可以读取反馈//current_pt[x] = Tip_op->obj_Data.position_3f[0] - 0; //1.7;current_pt[y] = Tip_op->obj_Data.position_3f[1] - 0;current_pt[z] = Tip_op->obj_Data.position_3f[2] - 0;current_pt[alpha] = Tip_op->obj_Data.orientation_3f[0];current_pt[beta] = Tip_op->obj_Data.orientation_3f[1];current_pt[_gamma] = Tip_op->obj_Data.orientation_3f[2];
}/**
* @brief It's NOT recommended that user modefies this function.
*        Plz programm the functions with the prefix "Usr_". 
*/
int main(int argc, char *argv[])
{/*System Logger tool init.*/std::cout << "[System Logger] Configuring... \n";std::cout << "[System Logger] Logger is ready ! \n";/*Simulation connection init.*/CoppeliaSim_Client *hClient = &CoppeliaSim_Client::getInstance();std::cout << "[CoppeliaSim Client] Connecting to server.. \n";while (!hClient->Start("127.0.0.1", 5000, 5, false)){};std::cout << "[CoppeliaSim Client] Successfully connected to server, configuring...\n";Usr_ConfigSimulation();std::cout << "[CoppeliaSim Client] Configure done, simulation is ready ! \n";while (1){// Abandon top 5 datastatic int init_num = 5;if (hClient->Is_Connected()){hClient->ComWithServer();}if (init_num > 0)init_num--;else{Usr_ReadFromSimulation();Usr_Main();Usr_SendToSimulation();}};
}

结语：demo并不完美，尤其演示3机械臂末端走样条曲线。源码具有一定学术价值，实际应用效果并不看好，或许这些算法并不适合于连续焊接场景，蚁群算法类可借鉴。仿真场景Lua脚本（焊接火花模拟）可借鉴。

The End