Qt5 基于OpenGL实现六轴机械臂三维仿真

需求

在Qt中通过OPenGL方式加载三维模型STL文件，然后将多个结构的STL文件类型的模型进行组装，形成6轴机械臂三维模型的显示，并且可以对每个关节进行关节角度的控制。

新建一个C++类STLFileLoader，用于加载STL文件，并进行文件解析。

直接通过QT的QFile类进行文件读取，然后通过判断STL的格式进行文件解析，一种是ASCII格式stl文件，一种是二进制格式stl文件。

void STLFileLoader::loadStl(const QString &filename) {
    QFile file(filename);
    if (file.open(QIODevice::ReadOnly | QIODevice::Text)) {
        QByteArray arr;
        arr = file.read(5);
        file.close();
        if (arr == "solid") {
            loadTextStl(filename);
        } else {
            loadBinaryStl(filename);
        }
    } else {
        qDebug() << filename << u8"不存在";
    }
}

加载ASCII格式的STL文件：

清空之前加载的模型数据，以便重新加载新的STL文件。
创建一个 QFile 对象来打开指定的文件。
如果文件成功打开，则进入循环，逐行读取文件内容。
对于每一行，使用 trimmed() 函数去除首尾的空白字符，并将其分割成单词。
根据单词的内容，判断当前行属于STL文件中的哪一部分。
- 如果单词的第一个部分是 “facet”，表示当前行描述了一个三角形的法线。
- 如果单词的第一个部分是 “vertex”，表示当前行描述了一个三角形的顶点。
- 如果单词的第一个部分是 “endloop”，表示当前三角形描述结束，可以构建一个完整的三角形并将其添加到模型中。
在解析完整个文件后，关闭文件。


//加载ASCII格式STL文件
void STLFileLoader::loadTextStl(const QString &filename) {
    qDebug() << "load text file:" << filename;
    model.clear(); //清除模型
    QList <QVector3D> triangle;
    STLTriangle tSTLTriangle;
    QFile file(filename);

    if (file.open(QIODevice::ReadOnly | QIODevice::Text)) {
        while (!file.atEnd()) {
            QString line = file.readLine().trimmed();
            QStringList words = line.split(' ', QString::SkipEmptyParts);

            if (words[0] == "facet") {
                triangle.clear();
                tSTLTriangle.reset();
                tSTLTriangle.setNormal(words[2].toFloat(), words[3].toFloat(), words[4].toFloat());
            } else if (words[0] == "vertex") {
                triangle.append(QVector3D(words[1].toFloat(), words[2].toFloat(), words[3].toFloat()));
            } else if (words[0] == "endloop") {
                if (triangle.length() == 3) {
                    for (int i = 0; i < 3; ++i) {
                        tSTLTriangle.setVertex(i, triangle[i]);
                    }
                    model.append(tSTLTriangle);
                }
            }
        }
        file.close();
    }
}

加载二进制格式STL文件，速度更快

打印出要加载的二进制文件的名称，以便调试时查看。
清空之前加载的模型数据，以便重新加载新的STL文件。
创建一个 QFile 对象来打开指定的文件。
获取文件的大小，并根据文件大小动态分配内存缓冲区 buf。
尝试以只读方式打开文件，如果打开失败则返回。
创建一个 QDataStream 对象 stream，用于从文件中读取二进制数据。
使用 stream.readRawData() 从文件中读取所有数据到缓冲区 buf 中。
关闭文件。
设置指针 p 指向缓冲区的起始位置。
使用 memcpy() 从缓冲区中依次读取文件名、三角形个数以及每个三角形的法向量和顶点信息。
每次读取一个三角形的数据后，将其添加到模型中。
释放内存缓冲区。

void STLFileLoader::loadBinaryStl(const QString &filename) {
    qDebug() << "load Binary file:" << filename;
    model.clear();          //清除模型
    QList <QVector3D> triangle;
    STLTriangle tSTLTriangle;

    QFile STL_file(filename);

    int fileSize = STL_file.size();
    char *buf = (char *) malloc(sizeof(char) * fileSize);

    bool isOk = STL_file.open(QIODevice::ReadOnly);
    if (!isOk) return;

    QDataStream stream(&STL_file);
    stream.readRawData(buf, fileSize);
    STL_file.close();

    const char *p = buf;
    char name[80];          //起始80个字节 文件名
    int triangle_num;       //4个字节 三角形个数
    float n1, n2, n3;       //法向量
    float v1, v2, v3;       //定点

    memcpy(name, p, 80);                        //记录文件名
    p += 80;                                    //跳过文件名

    memcpy(&triangle_num, p, 4);                //记录三角形个数
    p += 4;                                     //跳过个数标识

    for (int i = 0; i < triangle_num; i++) {     //读取法向量
        memcpy(&n1, p, 4);
        p += 4;
        memcpy(&n2, p, 4);
        p += 4;
        memcpy(&n3, p, 4);
        p += 4;

        triangle.clear();
        tSTLTriangle.reset();
        tSTLTriangle.setNormal(n1, n2, n3);

        for (int j = 0; j < 3; j++) {             //读取顶点信息
            memcpy(&v1, p, 4);
            p += 4;
            memcpy(&v2, p, 4);
            p += 4;
            memcpy(&v3, p, 4);
            p += 4;
            triangle.append(QVector3D(v1, v2, v3));
        }
        if (triangle.length() == 3) {
            for (int i = 0; i < 3; ++i) {
                tSTLTriangle.setVertex(i, triangle[i]);
            }
            model.append(tSTLTriangle);
        }
        p += 2;//跳过尾部标志 两字节
    }
    free(buf);
}

绘制STL文件的三维模型

创建一个副本 triangles，以便遍历模型中的所有三角形。
使用 glBegin(GL_TRIANGLES) 开始绘制一个或多个三角形。
对于每个三角形，获取其法向量，并使用 glNormal3f() 函数设置法向量。
对于每个顶点，获取其坐标，并使用 glVertex3f() 函数设置顶点坐标。
完成一个三角形的绘制后，继续处理下一个三角形，直到所有三角形都绘制完成。
使用 glEnd() 结束绘制。

总之，这段代码使用OpenGL绘制了STL模型中的所有三角形，其中 mRatio 用于缩放模型，以便适应特定的显示区域。

void STLFileLoader::draw() {
    QList <STLTriangle> triangles = model;
    QVector3D normal;
    QVector3D vertex;
    glBegin(GL_TRIANGLES); // 绘制一个或多个三角形
    foreach(STLTriangle tri, triangles) {
        normal = tri.getNormal();
        glNormal3f(mRatio * normal.x(), mRatio * normal.y(), mRatio * normal.z());
        for (int j = 0; j < 3; ++j) {
            vertex = tri.getVertex(j);
            glVertex3f(mRatio * vertex.x(), mRatio * vertex.y(), mRatio * vertex.z());
        }
    }
    glEnd();
}

二、新建一个类RRGLWidget，继承QGLWidget，用于绘制OpenGL图形的小部件，实现展示3D模型。这个类提供了鼠标旋转、方法缩小、平移、网格、坐标系等基本功能。

绘制一个网格，使用OpenGL的基本绘图功能绘制一系列水平和垂直线段，表示平面上的网格效果。

glPushMatrix(): 将当前的模型视图矩阵压入堆栈，保存当前坐标系的位置和状态。
定义一个颜色数组 color[]，用来表示网格线的颜色。这里的颜色数组中的值是RGB颜色空间中的颜色分量，每个分量的取值范围在0到1之间。
使用 glMaterialfv(GL_FRONT, GL_AMBIENT_AND_DIFFUSE, color) 指定材质对漫射光的反射率，即设置网格线的颜色。
设定网格的步长和网格线的数量。
循环绘制水平和垂直方向的网格线。对于每一条水平线，绘制从左边界到右边界的线段；对于每一条垂直线，绘制从下边界到上边界的线段。
glEnd(): 结束绘制线段的过程。
glPopMatrix(): 弹出之前存储的模型视图矩阵，恢复坐标系的位置和状态到之前的状态。

总之，这段代码用于绘制一个网格，以辅助在OpenGL场景中定位和绘制其他图形。

void RRGLWidget::drawGrid() {
    glPushMatrix();         // 存储当前坐标系位置
    GLfloat color[] = {8.0f / 255, 108.0f / 255, 162.0f / 255};
    ///
    /// \brief glMaterialfv  指定材质对漫射光的反射率
    /// @param face   决定该材质运用于图元的正面还是反面
    /// @param pname  表示对何种光进行设置(环境光和漫射光)
    /// @param params 四维数组，这个数组描述了反光率的RGBA值，每一项取值都为0-1之间
    glMaterialfv(GL_FRONT, GL_AMBIENT_AND_DIFFUSE, color);

    int step = 50;
    int num = 15;
    for (int i = -num; i < num + 1; i++) {
        glBegin(GL_LINES);
        glVertex3f(i * step, -num * step, 0);
        glVertex3f(i * step, num * step, 0);
        glVertex3f(-num * step, i * step, 0);
        glVertex3f(num * step, i * step, 0);
        glEnd();
    }
    glPopMatrix();          // 恢复存储的坐标系位置
}

画坐标系

glPushMatrix(): 将当前的模型视图矩阵压入堆栈，保存当前坐标系的位置和状态。
**glLineWidth(2.0f): **设置线段的宽度为2.0个单位。
**setupColor(255, 255, 255): **设置颜色为白色。该函数根据提供的RGB值设置OpenGL材质的颜色。
**glBegin(GL_LINES): **开始绘制线段。
绘制X轴：从(-900, 0, 0)到(900, 0, 0)。
绘制Y轴：从(0, -900, 0)到(0, 900, 0)。
绘制Z轴：从(0, 0, 0)到(0, 0, 700)。
**glEnd(): **结束绘制线段的过程。
绘制轴线上的标签：
- “-X” 在X轴负方向的末端。
- “+X” 在X轴正方向的末端。
- “-Y” 在Y轴负方向的末端。
- “+Y” 在Y轴正方向的末端。
- “+Z” 在Z轴正方向的末端。
**glLineWidth(1.0f): **将线段的宽度恢复为默认值。
**glPopMatrix(): **弹出之前存储的模型视图矩阵，恢复坐标系的位置和状态到之前的状态。

这段代码绘制了X、Y、Z三个轴线以及相应的标签，用于在OpenGL场景中表示坐标系。

void RRGLWidget::drawCoordinates() {
    glPushMatrix();
    glLineWidth(2.0f);
    setupColor(255, 255, 255);
    glBegin(GL_LINES);

    glVertex3f(-900, 0, 0);
    glVertex3f(900, 0, 0);
    glVertex3f(0, -900, 0);
    glVertex3f(0, 900, 0);
    glVertex3f(0, 0, 0);
    glVertex3f(0, 0, 700);
    glEnd();

    // 标签
    qglColor(QColor::fromRgbF(1, 0, 0));
    renderText(-900, 0, 0, "-X", QFont("helvetica", 12, QFont::Bold, true));
    renderText(900, 0, 0, "+X", QFont("helvetica", 12, QFont::Bold, true));
    qglColor(QColor::fromRgbF(0, 1, 0));
    renderText(0, -900, 0, "-Y", QFont("helvetica", 12, QFont::Bold, true));
    renderText(0, 900, 0, "+Y", QFont("helvetica", 12, QFont::Bold, true));
    qglColor(QColor::fromRgbF(0, 0, 1));
    renderText(0, 0, 700, "+Z", QFont("helvetica", 12, QFont::Bold, true));
    glLineWidth(1.0f);
    glPopMatrix();
}

画每个组件变换后的坐标系，同上。

void RRGLWidget::drawSTLCoordinates(int r, int g, int b) {
    glPushMatrix();
    glLineWidth(1.5f);
    setupColor(r, g, b);
    glBegin(GL_LINES);

    glVertex3f(-300, 0, 0);
    glVertex3f(300, 0, 0);
    glVertex3f(0, -300, 0);
    glVertex3f(0, 300, 0);
    glVertex3f(0, 0, 0);
    glVertex3f(0, 0, 500);
    glEnd();

    // 标签
    qglColor(QColor::fromRgbF(1, 0, 0));
    renderText(-300, 0, 0, "-X", QFont("helvetica", 12, QFont::Bold, true));
    renderText(300, 0, 0, "+X", QFont("helvetica", 12, QFont::Bold, true));
    qglColor(QColor::fromRgbF(0, 1, 0));
    renderText(0, -300, 0, "-Y", QFont("helvetica", 12, QFont::Bold, true));
    renderText(0, 300, 0, "+Y", QFont("helvetica", 12, QFont::Bold, true));
    qglColor(QColor::fromRgbF(0, 0, 1));
    renderText(0, 0, 500, "+Z", QFont("helvetica", 12, QFont::Bold, true));
    glLineWidth(1.0f);
    glPopMatrix();
}

设置显示颜色

void RRGLWidget::setupColor(int r, int g, int b) {
    GLfloat color[] = {static_cast<GLfloat>(r / 255.0), static_cast<GLfloat>(g / 255.0),
                       static_cast<GLfloat>(b / 255.0)};
    glMaterialfv(GL_FRONT, GL_AMBIENT_AND_DIFFUSE, color);
}

设置x、y轴旋转和平移

RRGLWidget::setXRotation(int angle): 设置X轴方向的旋转角度。参数 angle 表示要旋转的角度值。如果新的角度值不等于之前的角度值 xRot，则更新 xRot，发出信号 xRotationChanged(angle)，清除颜色缓冲区和深度缓冲区，并调用 updateGL() 更新OpenGL场景。
RRGLWidget::setYRotation(int angle): 设置Y轴方向的旋转角度。参数 angle 表示要旋转的角度值。如果新的角度值不等于之前的角度值 yRot，则更新 yRot，发出信号 yRotationChanged(angle)，清除颜色缓冲区和深度缓冲区，但不调用 updateGL() 更新OpenGL场景。
RRGLWidget::setXYTranslate(int dx, int dy): 设置X和Y轴方向的平移量。参数 dx 和 dy 分别表示在X和Y轴上的位移量。根据 dx 和 dy 更新 xTran 和 yTran 的值，然后调用 updateGL() 更新OpenGL场景，实现平移效果。

这些函数可以通过外部调用来控制OpenGL场景中视角的旋转和平移。

void RRGLWidget::setXRotation(int angle) {
    int tangle = angle;         // normalizeAngle(angle);
    if (tangle != xRot) {
        xRot = tangle;
        emit xRotationChanged(angle);
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
        updateGL();
    }
}

void RRGLWidget::setYRotation(int angle) {
    int tangle = angle;         // normalizeAngle(angle);
    if (tangle != yRot) {
        yRot = tangle;
        emit yRotationChanged(angle);
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
    }
}

void RRGLWidget::setXYTranslate(int dx, int dy) {
    xTran += 3.0 * dx;
    yTran -= 3.0 * dy;
    updateGL();
}

重新基类QGLWidget中的部分方法

    void initializeGL() override;

    void paintGL() override;

    void resizeGL(int w, int h) override;

    void mousePressEvent(QMouseEvent *event) override;

    void mouseMoveEvent(QMouseEvent *event) override;

6.1 初始化OPenGL环境

GLfloat ambientLight[] = {0.7f, 0.7f, 0.7f, 1.0f};: 定义了环境光的强度数组，包含RGBA值，表示光的颜色和强度。
GLfloat diffuseLight[] = {0.7f, 0.8f, 0.8f, 1.0f};: 定义了散射光的强度数组，包含RGBA值，表示光的颜色和强度。
GLfloat specularLight[] = {0.4f, 0.4f, 0.4f, 1.0f};: 定义了镜面反射光的强度数组，包含RGBA值，表示光的颜色和强度。
GLfloat positionLight[] = {20.0f, 20.0f, 20.0f, 0.0f};: 定义了光源的位置数组，包含X、Y、Z坐标和一个额外的参数。
glLightfv(GL_LIGHT0, GL_AMBIENT, ambientLight);: 设置0号光源的环境光属性。
glLightfv(GL_LIGHT0, GL_DIFFUSE, diffuseLight);: 设置0号光源的散射光属性。
glLightfv(GL_LIGHT0, GL_SPECULAR, specularLight);: 设置0号光源的镜面反射光属性。
glLightModelf(GL_LIGHT_MODEL_TWO_SIDE, 1.0);: 设置照明模型参数，启用双面照明。
glLightfv(GL_LIGHT0, GL_POSITION, positionLight);: 设置0号光源的位置属性。
glEnable(GL_LIGHTING);: 启用光照。
glEnable(GL_LIGHT0);: 打开光源。
glClearDepth(1.0);: 设置深度缓存。
glEnable(GL_DEPTH_TEST);: 启用深度测试。
glDepthFunc(GL_LEQUAL);: 设置深度测试的类型。
glEnable(GL_NORMALIZE);: 启用法线向量的自动归一化。
glClearColor(0.0, 0.0, 0.0, 1.0);: 设置背景清除颜色为黑色。

这些操作旨在配置OpenGL环境，包括光照、深度测试和背景颜色等，以便正确显示OpenGL场景。

void RRGLWidget::initializeGL() {
    //用来初始化这个OpenGL窗口部件的，可以在里面设定一些有关选项
    GLfloat ambientLight[] = {0.7f, 0.7f, 0.7f, 1.0f};      //光源环境光强度数组
    GLfloat diffuseLight[] = {0.7f, 0.8f, 0.8f, 1.0f};      //光源散射光强度数组
    GLfloat specularLight[] = {0.4f, 0.4f, 0.4f, 1.0f};     //光源镜面反射光强度数组
    GLfloat positionLight[] = {20.0f, 20.0f, 20.0f, 0.0f};  //光源位置数组

    glLightfv(GL_LIGHT0, GL_AMBIENT, ambientLight);     //设置0号光源的环境光属性
    glLightfv(GL_LIGHT0, GL_DIFFUSE, diffuseLight);     //设置0号光源的散射光属性
    glLightfv(GL_LIGHT0, GL_SPECULAR, specularLight);   //设置0号光源的镜面反射光属性
    glLightModelf(GL_LIGHT_MODEL_TWO_SIDE, 1.0);        //设置照明模型参数
    glLightfv(GL_LIGHT0, GL_POSITION, positionLight);   //设置0号光源的位置属性

    glEnable(GL_LIGHTING);   //启用光照
    glEnable(GL_LIGHT0);     //打开光源
    //glEnable(GL_DEPTH_TEST); //隐藏表面消除，打开深度缓冲区，绘制3D图像时候使用

    glClearDepth(1.0);       // 设置深度缓存
    glEnable(GL_DEPTH_TEST); // 启用深度测试
    glDepthFunc(GL_LEQUAL);  // 设置深度测试的类型
    glEnable(GL_NORMALIZE);

    glClearColor(0.0, 0.0, 0.0, 1.0);
}

6.2 窗口大小变化，width和height就是新的大小状态下的宽和高，另外resizeGL()在处理完后会自动刷新屏幕。

void RRGLWidget::resizeGL(int w, int h) {
    if (w < 0 || h < 0) {
        return;
    }
    glViewport(0, 0, w, h);
    glMatrixMode(GL_PROJECTION);
    glLoadIdentity();

    GLfloat zNear = 1.0;
    GLfloat zFar = 20000.0;
    GLfloat aspect = (GLfloat) w / (GLfloat) h;
    GLfloat fH = tan(GLfloat(70.0 / 360.0 * 3.14159)) * zNear;
    GLfloat fW = fH * aspect;
    glFrustum(-fW, fW, -fH, fH, zNear, zFar);       //将当前矩阵与一个透视矩阵相乘，把当前矩阵转变成透视矩阵，

    glMatrixMode(GL_MODELVIEW);
    glLoadIdentity();
    glTranslated(0.0, 0.0, -40.0);
}

6.3 鼠标按下和移动

mousePressEvent: 当鼠标按下时调用，记录当前鼠标位置为 lastPos。
mouseMoveEvent: 当鼠标移动时调用，计算鼠标移动的增量 dx 和 dy，然后根据鼠标按键的不同执行不同的操作：
- 如果按下的是左键 (Qt::LeftButton)，则调用 setXRotation 和 setYRotation 函数来设置 X 和 Y 轴的旋转角度，以实现场景的旋转。
- 如果按下的是右键 (Qt::RightButton)，则调用 setZoom 函数来设置缩放参数，以实现场景的缩放。
- 如果按下的是中键 (Qt::MidButton)，则调用 setXYTranslate 函数来设置 X 和 Y 轴的平移参数，以实现场景的平移。

这些操作使得用户可以通过鼠标在 OpenGL 窗口中交互式地旋转、缩放和平移场景。

void RRGLWidget::mousePressEvent(QMouseEvent *event) {
    lastPos = event->pos();
}

void RRGLWidget::mouseMoveEvent(QMouseEvent *event) {
    int dx = event->x() - lastPos.x();
    int dy = event->y() - lastPos.y();
    // 这里必须使用buttons()
    if (event->buttons() & Qt::LeftButton) {  //进行的按位与
        setXRotation(xRot + 4 * dy);
        setYRotation(yRot - 4 * dx);
    } else if (event->buttons() & Qt::RightButton) {
        setZoom(z_zoom + 5.0 * dy);
    } else if (event->buttons() & Qt::MidButton) {
        setXYTranslate(dx, dy);
    }
    lastPos = event->pos();
}

加载六轴机械臂三维模型，绘制OPenGL场景，搭建机械臂3D模型

新建C++类DDR6RobotWidget，继承RRGLWidget。我们需要绘制机械臂模型、再加上一个实验桌。

    /// 7个小部件组成
    typedef struct DD6RobotModel {
        STLFileLoader *link0;
        STLFileLoader *link1;
        STLFileLoader *link2;
        STLFileLoader *link3;
        STLFileLoader *link4;
        STLFileLoader *link5;
        STLFileLoader *link6;
    } DDR6RobotSTLModel;
    /// 桌子
    typedef struct DeskModel {
        STLFileLoader *link0;
    } DeskModel;

    /// 机械臂模型
    DDR6RobotSTLModel mRobotModel;
    /// 桌子模型
    DeskModel mDeskModel;

通过STLFileLoader类加载STL文件

void DDR6RobotWidget::loadRobotModelSTLFile() {
    //模型由7个小部件组成
    mRobotModel.link0 = new STLFileLoader(":/res/binary/base_link.STL", 1000);
    mRobotModel.link1 = new STLFileLoader(":/res/binary/link_1.STL", 1000);
    mRobotModel.link2 = new STLFileLoader(":/res/binary/link_2.STL", 1000);
    mRobotModel.link3 = new STLFileLoader(":/res/binary/link_3.stl", 1000);
    mRobotModel.link4 = new STLFileLoader(":/res/binary/link_4.STL", 1000);
    mRobotModel.link5 = new STLFileLoader(":/res/binary/link_5.STL", 1000);
    mRobotModel.link6 = new STLFileLoader(":/res/binary/link_6.STL", 1000);
    mDeskModel.link0 = new STLFileLoader(":/res/binary/desk.stl", 1);
}

配置模型的相关参数

void DDR6RobotWidget::configureModelParams() {
    //注意：经过旋转、平移后坐标系会改变
    mRobotConfig.d = {0, 127.00, -122.00, -101.00, -1.0, 0.00, 0.00};           //沿z轴平移
    mRobotConfig.JVars = {0, 0, 0, 0, 0, 0, 0};                                 //绕z轴旋转角度
    mRobotConfig.a = {0, 0, 0, 0, 0, 0, 0};                                     //沿x轴平移
    mRobotConfig.alpha = {0, 0, 180.00, 0, 0, 0, 0};                            //绕X轴旋转角度

    // 默认开启网格
    mGlobalConfig = {true, false, false, false, false, false, false, false, false};
}

重新基类RRGLWidget的drawGL()方法，将机械臂的各个关节进行组装，这里需要对各个关节的坐标系进行不断地调整。此处待优化，调整坐标系不通用，这里写死了。

调用一系列 OpenGL 函数来绘制每个关节的连杆，并根据机器人的当前状态（位置、姿态等）调整每个连杆的位置和方向。
以下是主要的步骤：

调用 drawGrid()、drawCoordinates() 和 drawGLForDesk() 函数绘制网格、坐标系和机器人的底座。
绘制每个关节的连杆：
- 对于每个关节，根据机器人当前配置（位置和姿态）调用 glTranslatef() 和 glRotatef() 函数来调整坐标系，并使用 mRobotModel 中对应的 draw() 函数来绘制连杆。
如果需要，绘制每个关节的坐标系：
- 调用 drawSTLCoordinates() 函数来绘制每个关节的坐标系。
使用 glPopMatrix() 恢复初始坐标系。


void DDR6RobotWidget::drawGL() {
    //方法：不断调整每个link的坐标系（glTranslatef、glRotatef），依次组合起所有link
    //TODO: 此处待优化，调整坐标系不通用，这里写死了
    glPushMatrix();

    if (mGlobalConfig.isDrawGrid) drawGrid();
    if (mGlobalConfig.isDrawWorldCoord) drawCoordinates();
    if (mGlobalConfig.isDrawDesk) drawGLForDesk();

    // 基座
    setupColor(20, 126, 60);
    mRobotModel.link0->draw();

    // 一关节
    if (mGlobalConfig.isDrawJoint1Coord) {
        drawSTLCoordinates(255, 0, 0);
    }
    setupColor(169, 169, 169);
    glTranslatef(0.0, 0.0, mRobotConfig.d[1]);                  // z轴方向平移
    glRotatef(mRobotConfig.JVars[1], 0.0, 0.0, 1.0);            // 绕z轴旋转
    glTranslatef(mRobotConfig.a[1], 0.0, 0.0);                  // x轴方向平移
    glRotatef(mRobotConfig.alpha[1], 1.0, 0.0, 0.0);            // 绕x轴旋转
    mRobotModel.link1->draw();

    // 调整坐标系
    glRotatef(90, 1.0, 0.0, 0.0);

    // 二关节  修改2关节的Z轴 +90
    if (mGlobalConfig.isDrawJoint2Coord) {
        drawSTLCoordinates(0, 255, 0);
    }
    setupColor(20, 126, 60);
    glTranslatef(0.0, 0.0, mRobotConfig.d[2]);                  // z轴方向平移
    glRotatef(mRobotConfig.JVars[2] + 90, 0.0, 0.0, 1.0);       // 绕z轴旋转
    glTranslatef(mRobotConfig.a[2], 0.0, 0.0);                  // x轴方向平移
    glRotatef(mRobotConfig.alpha[2], 1.0, 0.0, 0.0);            // 绕x轴旋转
    mRobotModel.link2->draw();

    // 调整坐标系
    glTranslatef(300, 0.0, 0.0);

    // 三关节
    if (mGlobalConfig.isDrawJoint3Coord) {
        drawSTLCoordinates(0, 0, 255);
    }
    setupColor(169, 169, 169);
    glTranslatef(0.0, 0.0, mRobotConfig.d[3]);                  // z轴方向平移
    glRotatef(mRobotConfig.JVars[3], 0.0, 0.0, 1.0);            // 绕z轴旋转
    glTranslatef(mRobotConfig.a[3], 0.0, 0.0);                  // x轴方向平移
    glRotatef(mRobotConfig.alpha[3], 1.0, 0.0, 0.0);            // 绕x轴旋转
    mRobotModel.link3->draw();

    // 调整坐标系
    glTranslatef(260, 0.0, 0.0);
    glRotatef(-90, 0.0, 0.0, 1.0);                              // 绕x轴旋转

    // 四关节
    if (mGlobalConfig.isDrawJoint4Coord) {
        drawSTLCoordinates(255, 255, 0);
    }
    setupColor(20, 126, 60);
    glTranslatef(0.0, 0.0, mRobotConfig.d[4]);                  // z轴方向平移
    glRotatef(mRobotConfig.JVars[4], 0.0, 0.0, 1.0);            // 绕z轴旋转
    glTranslatef(mRobotConfig.a[4], 0.0, 0.0);                  // x轴方向平移
    glRotatef(mRobotConfig.alpha[4], 1.0, 0.0, 0.0);            // 绕x轴旋转
    mRobotModel.link4->draw();

    // 调整坐标系
    glTranslatef(0.0, 0.0, 110.0);
    glRotatef(-90, 1.0, 0.0, 0.0);                              // 绕x轴旋转

    // 五关节
    if (mGlobalConfig.isDrawJoint5Coord) {
        drawSTLCoordinates(0, 255, 255);
    }
    setupColor(169, 169, 169);
    glTranslatef(0.0, 0.0, mRobotConfig.d[5]);                  // z轴方向平移
    glRotatef(mRobotConfig.JVars[5], 0.0, 0.0, 1.0);            // 绕z轴旋转
    glTranslatef(mRobotConfig.a[5], 0.0, 0.0);                  // x轴方向平移
    glRotatef(mRobotConfig.alpha[5], 1.0, 0.0, 0.0);            // 绕x轴旋转
    mRobotModel.link5->draw();

    // 调整坐标系
    glTranslatef(0.0, 0.0, 110.0);
    glRotatef(90, 1.0, 0.0, 0.0);                               // 绕x轴逆时针旋转90°

    // 六关节
    if (mGlobalConfig.isDrawJoint6Coord) {
        drawSTLCoordinates(255, 0, 255);
    }

    setupColor(20, 126, 60);
    glTranslatef(0.0, 0.0, mRobotConfig.d[6]);                  // z轴方向平移
    glRotatef(mRobotConfig.JVars[6], 0.0, 0.0, 1.0);            // 绕z轴旋转
    glTranslatef(mRobotConfig.a[6], 0.0, 0.0);                  // x轴方向平移
    glRotatef(mRobotConfig.alpha[6], 1.0, 0.0, 0.0);            // 绕x轴旋转
    mRobotModel.link6->draw();

    glPopMatrix();
}

重新基类RRGLWidget的paintGL()方法，绘制OpenGL的窗口

具体步骤如下：

调用 glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT) 函数清除颜色缓存和深度缓存。
使用 glPushMatrix() 函数保存当前的模型视图矩阵状态，以便后续的绘制操作。
使用 glTranslated() 函数根据用户的缩放操作调整视图的缩放比例。
使用 glTranslated() 函数根据用户的平移操作调整视图的平移位置。
使用 glRotated() 函数根据用户的旋转操作绕 x、y 和 z 轴进行旋转。
调用 drawGL() 函数绘制机器人的三维模型。
使用 glPopMatrix() 函数恢复之前保存的模型视图矩阵状态，以确保后续的绘制操作不受影响。

这段代码的主要作用是在每次窗口需要重新绘制时，更新机器人模型的位置、姿态和大小，并根据用户的操作实时更新视图。

void DDR6RobotWidget::paintGL() {
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);         //清除屏幕和深度缓存
    glPushMatrix();
    glTranslated(0, 0, z_zoom);
    glTranslated(xTran, yTran, 0);
    glRotated(xRot / 16.0, 1.0, 0.0, 0.0); //绕x轴旋转
    glRotated(yRot / 16.0, 0.0, 1.0, 0.0); //绕y轴旋转
    glRotated(zRot / 16.0, 0.0, 0.0, 1.0); //绕z轴旋转
    glRotated(+90.0, 1.0, 0.0, 0.0);
    drawGL();
    glPopMatrix();
}

新建一个控制机械臂模型的Qt QWidget窗口RobotControlForm，用于控制各个关节的角度，以及相关配置的显示和隐藏。

这个类里面主要通过控制UI控件，触发相应的信号，没有其他特殊的处理。

初始化UI界面控件的信号和槽函数

void RobotControlForm::initializeWindow() {
    //隐藏关节坐标系
    QList<QCheckBox *> cks =  ui->groupBoxRobot->findChildren<QCheckBox *>();
    for (auto &item : cks) {
        item->hide();
    }

    // 滑动条
    QList < QSlider * > SliderList = ui->groupBoxRobot->findChildren<QSlider *>();
    for (int i = 0; i < SliderList.size(); i++) {
        QSlider *slider = SliderList.at(i);
        slider->setMinimum(-180);
        slider->setMaximum(180);
        slider->setTickInterval(1);
        connect(slider, &QSlider::valueChanged, this, &RobotControlForm::slotUpdateJVarsValue, Qt::UniqueConnection);
        slider->setValue(0);
    }

    // checkBox 环境变化
    connect(ui->checkBoxGrid_real, &QCheckBox::stateChanged, this, [=](int state) {
        if (state == 0) {
            slotCheckStateChanged(false);
        } else if (state == 2) {
            slotCheckStateChanged(true);
        }
    });
    connect(ui->checkBoxWorldCoordinate_real, &QCheckBox::stateChanged, this, [=](int state) {
        if (state == 0) {
            slotCheckStateChanged(false);
        } else if (state == 2) {
            slotCheckStateChanged(true);
        }
    });
    connect(ui->checkBoxDesk_real, &QCheckBox::stateChanged, this, [=](int state) {
        if (state == 0) {
            slotCheckStateChanged(false);
        } else if (state == 2) {
            slotCheckStateChanged(true);
        }
    });
}


void RobotControlForm::slotUpdateJVarsValue(int value) {
    QSlider *slider = (QSlider *) sender();
    QString objectName = slider->objectName();
    QString index = objectName.at(objectName.size() - 1);
    emit sigJoinValueChanged(index.toInt(), value);
}

void RobotControlForm::slotCheckStateChanged(bool value) {
    if (sender()->objectName() == "checkBox_showModel") {
        emit sigSetModelRealTimeShow(value);
        return;
    }
    emit sigCheckStateChanged();
}

新建一个Qt QWidget类Robot3DForDDR6Form，用于显示三维模型和模型控制界面。

界面上两个窗口，一个继承DDR6RobotWidget，用于显示模型
一个集成RobotControlForm，用于显示机械臂模型控制界面。

当我们在RobotControlForm进行滑动条变化，来控制机械臂模型时，会触发信号sigJoinValueChanged(),此处进行相应的处理。


connect(ui->robotControl, &RobotControlForm::sigJoinValueChanged, this,
        &Robot3DForDDR6Form::slotJVarsValueChange, Qt::UniqueConnection);

void Robot3DForDDR6Form::slotJVarsValueChange(int index, int value) {
    ui->robot3D_virtual->mRobotConfig.JVars[index] = value;
    if (index == 2) {
        ui->robot3D_virtual->mRobotConfig.JVars[index] = -value;
    }
    ui->robot3D_virtual->update();
}

当我们在RobotControlForm进行勾选框变化时，来控制场景中的坐标系、桌子等显示和隐藏，会触发信号sigCheckStateChanged(),此处进行相应的处理。

connect(ui->robotControl, &RobotControlForm::sigCheckStateChanged, this,
        &Robot3DForDDR6Form::slotUpdateGlobalConfig, Qt::UniqueConnection);
void Robot3DForDDR6Form::slotUpdateGlobalConfig() {
    ui->robot3D_virtual->mGlobalConfig.isDrawGrid = ui->robotControl->getIsRealGridChecked();
    ui->robot3D_virtual->mGlobalConfig.isDrawWorldCoord = ui->robotControl->getIsRealWorldCoord();
    ui->robot3D_virtual->mGlobalConfig.isDrawDesk = ui->robotControl->getIsRealShowDesk();
    ui->robot3D_virtual->mGlobalConfig.isDrawJoint1Coord = ui->robotControl->getIsJointChecked(1);
    ui->robot3D_virtual->mGlobalConfig.isDrawJoint2Coord = ui->robotControl->getIsJointChecked(2);
    ui->robot3D_virtual->mGlobalConfig.isDrawJoint3Coord = ui->robotControl->getIsJointChecked(3);
    ui->robot3D_virtual->mGlobalConfig.isDrawJoint4Coord = ui->robotControl->getIsJointChecked(4);
    ui->robot3D_virtual->mGlobalConfig.isDrawJoint5Coord = ui->robotControl->getIsJointChecked(5);
    ui->robot3D_virtual->mGlobalConfig.isDrawJoint6Coord = ui->robotControl->getIsJointChecked(6);

    ui->robot3D_virtual->updateGL();
}