Qt中动态显示六轴机械臂的STL三维模型

运动仿真

刚好手头有个项目要用Qt做一个六轴机械臂的控制系统，ROS虽然好用，但是RVIZ在Qt里面集成有点困难，就自己用STL模型在Qt里面做了个简单的运动仿真。

STL模型

首先要对机械臂三维模型进行简化，尽量把不需要的孔和复杂结构给直接去掉，这样得到的模型文件会小很多，在处理的时候速度也可以加快，要不然会比较卡顿，尤其在零件比较多的时候。六轴机械臂分成了10个零件，分成了连杆和关节。
再有就是坐标的问题，在导出STL模型的时候一定要设置好导出坐标系，因为要做运动仿真，最好是按照建立的DH坐标系导出，关节与连杆均是，在solidworks中建立好DH坐标系，然后导出STL模型，导出的时候选择好导出坐标系，这样在后面拼接和运动仿真的时候不需要坐标变换，直接通过正运动学公式即可连接起来。导出格式二进制和ASCII格式都可以，ASCII格式相对好操作一点。

openGL显示STL

网上看的显示STL大都用openGL来显示，这方面我也不太会，github大法好
参考链接：github大佬的链接
链接放在这，需要的自取：https://github.com/vladkrylov/STLViewer

虽然我不会写，但是还是可以看懂的，这里简单说一下：

ASCII格式的STL文件

ASCII码格式的STL文件逐行给出三角面片的几何信息，每一行以1个或2个关键字开头。在STL文件中的三角面片的信息单元 facet 是一个带矢量方向的三角面片，STL三维模型就是由一系列这样的三角面片构成。整个STL文件的首行给出了文件路径及文件名。在一个 STL文件中，每一个facet由7 行数据组成，facet normal 是三角面片指向实体外部的法矢量坐标，outer loop 说明随后的3行数据分别是三角面片的3个顶点坐标，3顶点沿指向实体外部的法矢量方向逆时针排列。

STL的ASCII文件形式：

  facet normal -9.941380e-001 -1.081190e-001 -7.282242e-016
    outer loop
      vertex   3.711158e+001 8.168877e+000 2.470000e+001
      vertex   3.800000e+001 0.000000e+000 1.500000e+001
      vertex   3.800000e+001 0.000000e+000 2.470000e+001
    endloop
  endfacet

如图为基座的STL模型

读取STL文件

这一块实在没啥好说的，虽然我也很想说说，但是确实没啥好说的，直接参考大佬的代码就可以了
代码在头文件里面定义了两个类，一个用来读facet的数据，Model类则是前一个的集合，读取整个文件模型
读取过程也比较简单，从文件开头读，读到关键字，然后去关键字后面找数据，存起来就完事了，存到Model类里面
读文件的过程可以参考Qt读txt文件的过程，从第一行读到最后一行
看吧，确实没啥好说的

openGL中显示STL模型

在STL显示过程中需要先对openGL进行设置，有个坑在这说下：

1. 光源问题，光源的设置是一个比较麻烦的问题，这个可以参考网上的东西做一个比较好的光源，我在这只用了一个简单的光源，看起来也还可以

	glEnable(GL_LIGHTING);
	glEnable(GL_LIGHT0);
	
	glEnable(GL_COLOR_MATERIAL);
	glLightModeli( GL_FRONT, GL_AMBIENT_AND_DIFFUSE );//设置材料属性，表面漫射反射之类的
	
	static GLfloat lightPosition[4] = {
      1.f, 1.0f, 1.f, 0.f };
	glLightfv(GL_LIGHT0, GL_POSITION, lightPosition);

2. 调节视角:

    //旋转与平移视角
    glMatrixMode(GL_MODELVIEW);
    glScalef(0.15*scale, 0.15*scale, 0.15*scale);//调节视角大小比例
    glRotatef(xRot / 2.0f, 1.0f, 0.0f, 0.0f);//调节视角，与鼠标和键盘回调函数结合使用
    glRotatef(yRot / 2.0f, 0.0f, 1.0f, 0.0f);
    glRotatef(zRot / 2.0f, 0.0f, 0.0f, 1.0f);
    glTranslated(tran_x, tran_y, 0);//平移视角

//回调函数
void AppGLWidget::mousePressEvent(QMouseEvent *event)
{
     
    mouseLastPos = event->pos();
}

void AppGLWidget::mouseMoveEvent(QMouseEvent *event)
{
     
    int dx = event->x() - mouseLastPos.x();
    int dy = event->y() - mouseLastPos.y();
    mouseLastPos = event->pos();

    xRot += dy;
    yRot += dx;
    update();
}

void AppGLWidget::wheelEvent(QWheelEvent *event)
{
     
    scale *= 1 + event->delta() * (s0 - 1);
    update();
}

void AppGLWidget::keyPressEvent(QKeyEvent * event)
{
     
    switch (event->key()) {
     
        case Qt::Key_Up:
        tran_y += 10;
        break;
        case Qt::Key_Down:
        tran_y -= 10;
        break;
        case Qt::Key_Left:
        tran_x += 10;
        break;
        case Qt::Key_Right:
        tran_x -= 10;
        break;
        case Qt::Key_R:
        tran_x = 0;tran_y = 0;
        break;
        case Qt::Key_A:
        zRot += 10;
        break;
        case Qt::Key_D:
        zRot -= 10;
        break;

    }

    update();
}

3. 画图函数如下：

void AppGLWidget::DrawOneModel(Model m, size_t indexR, size_t indexP)
{
     
    QVector3D n;
    QVector3D v;
    STLTriangle t;
    Matrix3d R;
    Vector3d P;
    Vector3d vert;//顶点

	//根据零件编号选择旋转与平移矩阵进行运动学坐标变换
    if(indexR == 0 && indexP == 0)
    {
     
        R = Matrix3d::Identity(3, 3);
        P = Vector3d::Zero(3, 1);
    }
    else
    {
     
        R = kollkine.getRotation(indexR);
        P = kollkine.getPosition(indexP);
    }

    for(int i = 0; i < m.GetNTriangles(); ++i) {
     
        t = m.GetTriangle(i);
        glBegin(GL_TRIANGLES);
            n = t.GetNormal();
            glNormal3f(n.x(), n.y(), n.z());
            for(size_t j=0; j<3; ++j) {
     
                v = t.GetVertex(j);
                vert<<v.x(), v.y(), v.z();
                vert = R*vert+P;//对顶点进行旋转平移变换
                glColor4f(0.8f*indexP/10, 0.8f*indexP/10, 0.5f*indexP/10, 0.0f);//上色
                //set the display position
                glVertex3f(vert(0),
                           vert(1),
                           vert(2));
            }
        glEnd();
    }
}

运动学变换

这方面比较简单，学过机器人学应该都能理解，就正运动学进行坐标变换即可，但是需要将运动学的DH坐标系和STL的导出坐标系一致方可。
这里正运动学计算用的是Eigen库，使用方法的链接贴在这：Eigen
在Qt里面使用只需要把Eigen的包下载下来，然后在.pro文件里包含下路径就可以

INCLUDEPATH += F:\qtmodel\STLViewer-master\eigen-3.3.9\Eigen

//计算正运动学
void Kinematics::cal_kinematics(void)
{
     

    for(size_t i=0; i<7; i++)
    {
     
        T[i] <<  cos(angle[i]), -sin(angle[i]), 0, a[i],
                 sin(angle[i])*cos(A[i]), cos(angle[i])*cos(A[i]),  -sin(A[i]),  -sin(A[i])*d[i],
                 sin(angle[i])*sin(A[i]), cos(angle[i])*sin(A[i]),  cos(A[i]),   cos(A[i])*d[i],
                 0, 0, 0, 1;
    }

    T[1]=T[0]*T[1];
    T[2]=T[1]*T[2];
    T[3]=T[2]*T[3];
    T[4]=T[3]*T[4];
    T[5]=T[4]*T[5];
    T[6]=T[5]*T[6];

    for(size_t i=0; i<7; i++)
    {
     
        R[i] << T[i].block(0, 0, 3, 3);
        P[i] << T[i].block(0, 3, 3, 1);
    }
}

两个坑

1. Eigen在C++类里用静态矩阵的时候会报错，没法运行，奇怪的是在debugger模式下可以运行，可能是什么内存的原因，我也不太懂，解决方法就是在类里加这么一句话：

	public:
	    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

2. 回调函数的调用会有焦点的问题，当时在写键盘回调的时候就一直触发不了，后来在对象中加了这么两句话才解决

    setFocusPolicy(Qt::StrongFocus);
    installEventFilter(this);

最终效果

最终效果大概就这样，这个光实在是很诡异（我实在是懒得调了…），但是毕竟谁会嫌弃自己的孩子丑呢对吧