Project mind map
There are three demos in this project :
The end of the arm walks in a straight line
2. The turntable of the positioner rotates
3. Multi-point spline movement at the end of the robotic arm
notes:
The method of searching path in 3D grid map based on level-based ant colony system:
Hierarchical Ant Colony System (HACS) is an improved ant colony optimization algorithm. Based on the traditional ant colony algorithm, it optimizes the search process by constructing a hierarchical structure.
In a 3D grid map, we can divide the map into multiple levels. The highest level is an overview of the entire map, which is equally divided into larger grid areas. At a lower level, each grid area is divided into smaller sub-areas. Finally, the bottom layer is the detail grid within each subregion.
When searching for the path, the ant colony proceeds in hierarchical order. At a high level, the ant colony uses a wider field of view to search the entire map to find the approximate path area connecting the start point and the goal point. Then search in the lower level, optimize the path step by step, and find a more detailed and precise route.
Compared with the traditional ant colony, this hierarchical search method can lock the search area faster, reduce invalid searches, and thus improve search efficiency. At the same time, the low-level detailed search can also find shorter and better paths.
In a word, HACS uses the hierarchical information of the map to guide the search, so that the ant colony system has both high-level macro vision and low-level local optimization ability. This method can effectively improve the performance of searching paths in 3D grid maps.
Solve the generalized traveling salesman problem using an ant colony system:
Generalized Traveling Salesman Problem (GTSP) is a generalization of the traveling salesman problem, which can be effectively solved by using ant colony system.
The GTSP problem is to group cities, ask the traveling salesman to visit one city in each group, and minimize the total distance traveled. The steps of ant colony algorithm to solve GTSP are as follows:
Build the solution space. Cities are grouped, and each group is regarded as a virtual city.
Ant colony search. The ant colony searches for paths according to the traditional TSP rules, but each time it passes through a virtual city, it randomly selects a real city in the group to visit.
information update. When the ant colony completes a round of search, the pheromones are updated, including the pheromones in each city and the pheromones connecting two virtual cities.
Repeat the search. Repeatedly iterate the above search and update process, and gradually get a better solution.
The result output. After the iteration is terminated, the current optimal solution is output as the approximate optimal solution of the GTSP problem.
This method combines the distributed search capability of the ant colony algorithm and the group selection requirement of the GTSP problem. Compared with the brute force algorithm, the search space can be greatly reduced, and the approximate optimal solution can be obtained faster. It is also more targeted than random algorithms. Therefore, using the ant colony system can efficiently solve the GTSP problem.
To test under the windows system, Timer.h needs to be modified:
#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_HMain program source code:
/* 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 points
float 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 time
start_time = timer.getMs();
is_running = true;
}
else if (demo_type == 2)//转台//
{
//Set terminal points
float 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 time
start_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 data
static int init_num = 5;
if (hClient->Is_Connected())
{
hClient->ComWithServer();
}
if (init_num > 0)
init_num--;
else
{
Usr_ReadFromSimulation();
Usr_Main();
Usr_SendToSimulation();
}
};
}Conclusion: The demo is not perfect, especially the demonstration of the spline curve at the end of the 3 robotic arms. The source code has a certain academic value, but the actual application effect is not good. Perhaps these algorithms are not suitable for continuous welding scenarios, and the ant colony algorithm can be used for reference. Simulation scene Lua script (welding spark simulation) can be used for reference.
The End