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

需求

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

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

1. 直接通过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"不存在";}
}

2. 加载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();}
}

3. 加载二进制格式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);
}

4. 绘制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模型。这个类提供了鼠标旋转、方法缩小、平移、网格、坐标系等基本功能。

1. 绘制一个网格，使用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();          // 恢复存储的坐标系位置
}

2. 画坐标系

• 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();
}

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

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();
}

4. 设置显示颜色

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);
}

5. 设置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();
}

6. 重新基类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模型

1. 新建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;

2. 通过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);
}

3. 配置模型的相关参数

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};
}

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

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

1. 调用 drawGrid()、drawCoordinates() 和 drawGLForDesk() 函数绘制网格、坐标系和机器人的底座。
2. 绘制每个关节的连杆：
   • 对于每个关节，根据机器人当前配置（位置和姿态）调用 glTranslatef() 和 glRotatef() 函数来调整坐标系，并使用 mRobotModel 中对应的 draw() 函数来绘制连杆。
3. 如果需要，绘制每个关节的坐标系：
   • 调用 drawSTLCoordinates() 函数来绘制每个关节的坐标系。
4. 使用 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轴 +90if (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();
}

5. 重新基类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控件，触发相应的信号，没有其他特殊的处理。

1. 初始化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，用于显示机械臂模型控制界面。

1. 当我们在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();
}

2. 当我们在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();
}

效果：至此我们完成了整个机械臂的三维模型的搭建显示，已经相应的控制。

源代码链接

Qt5 基于OpenGL实现六轴机械臂三维仿真
https://download.csdn.net/download/ever__ever/88800031?spm=1001.2014.3001.5501