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

项目思维导图

该项目一共三个demo

  1. 机械臂末端走直线

2. 变位机转台转动

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

笔记:

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

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

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

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

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

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

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

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

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

  1. 构建解空间。将城市分组,每个组看作为一个虚拟城市。

  2. 蚁群搜索。蚁群按照传统TSP规则搜索路径,但每经过一个虚拟城市时,会随机选择该组内的一个真实城市访问。

  3. 信息更新。当蚁群完成一轮搜索后,更新信息素,包括每个城市内的信息素和连接两个虚拟城市的信息素。

  4. 重复搜索。反复迭代上述搜索和更新过程,逐步得到更优解。

  5. 结果输出。迭代终止后,输出当前最优解作为GTSP问题的近似最优解。

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

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

#ifndef PROJECT_TIMER_H
#define PROJECT_TIMER_H
 
#include 
#include 
#include 
#include 


//#include 


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(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 
#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 straight_line(2);//模板类BezierCurve,用于生成贝塞尔曲线//
BezierCurve platform_angle(2);
BS_Basic *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 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 *> *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(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> 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 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 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();
        }
    };
}

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

The End

你可能感兴趣的:(c++,算法,开发语言)