运动仿真

刚好手头有个项目要用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-016outer loopvertex   3.711158e+001 8.168877e+000 2.470000e+001vertex   3.800000e+001 0.000000e+000 1.500000e+001vertex   3.800000e+001 0.000000e+000 2.470000e+001endloopendfacet

如图为基座的STL模型

读取STL文件

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

openGL中显示STL模型

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

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

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

调节视角:

    //旋转与平移视角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();
}

画图函数如下：

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

两个坑

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

 public:EIGEN_MAKE_ALIGNED_OPERATOR_NEW

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

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

最终效果

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

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

六轴机械臂下位机（arduino）+上位机（ROS+Moveit）---（一）机械臂硬件
六轴机械臂下位机(arduino)+上位机(ROS+Moveit)---(一)机械臂硬件机械部分机械臂制作时的注意点!!!(坑) 零件的3D打印控制器接线问题机械部分六轴机械臂在工业领域的运 ...
编写简单的六轴机械臂
一.关键词: link 连杆 joint 连轴 orgin 原点 axis 轴 visual 视觉 geometry 几何 material 材料 collision 碰撞 inertial 惯量 f ...
六轴机械臂DIY（一）机械臂DIY总体规划
一直想搞一个六轴机械臂玩玩,查了查网上的资料,发现这个开源项目已经较为成熟,但没有一个总体的教程.正好我可以记录一下我接下来的DIY过程,作为一个项目日记.(当然不确定项目会不会烂尾) 本项目参考gi ...
六轴机械臂DIY（三）开源项目介绍
就这样一年半了,项目断断续续仍在进行,期间我混了个毕业,相信大家也经历了很多吧.最近上海疫情,毕业后的我哪里也去不了,只能在寝室等学校的投喂,那么正好,让我们项目继续. 本节主要完整介绍这个机械臂的开 ...
六轴机械臂运动学算法原理及其推导过程
网站上关于六轴机械臂piper算法的讲解有很多,但其腕点姿态的推到较为模糊,故此写一篇关于六轴机械臂piper算法的推导讲解,供有缘人参考,如果您觉得有用,可以点个赞,吾将不胜感激,若是推导过程存在错 ...
MyCobot六轴机械臂的基本操作（二）
上一讲我们做ssh和vnc的设置,有小伙伴问设置些有什么用,那么这里我先来解释一下这些功能有什么用处,首先我们可以通过ssh在我们的Windows桌面进行程序开发,然后上传到树莓派进行验证,我们也可以 ...
如何实现六轴机械臂的逆解计算？
1. 机械臂运动学介绍机械臂运动学机器人运动学就是根据末端执行器与所选参考坐标系之间的几何关系,确定末端执行器的空间位置和姿态与各关节变量之间的数学关系.包括正运动学(Forward Kinema ...
六轴机械臂算法-引导篇
最近一直在研究 6轴机械臂算法,整理出了如下几个计算六轴机械臂正解和逆解的关键点: 01_机器人坐标系和关节的说明 02_算法坐标系的建立 03_D-H参数表的建立 04_FK(正解)算法 05_Ma ...
四轴六轴机械臂基础运动仿真实验（Matlab）
博主最近在学一本叫做<A Mathematical Introduction to Robotic Manipulation>的机器人学教材,并且在学习之余根据了老师的要求完成了一个仿真项 ...

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