这篇文章之前，我向大家道个歉，非常抱歉拖更了这么久，不知不觉二十来天了，那我们立刻开始今天的正题。
注意：本文只会介绍相关理论和提供部分必要代码，代码逻辑请各位自行实现，代码自己不多敲敲，找不到工作可咋办！！！！！！！！望大家理解！！！！！！！
一、结果展示
以下为今日的代码最后的结果的一个简单展示，其中包括了Link1~Link6的关节控制滑块QSlider，末端执行器的相对于世界坐标系的位置（即Link6建立的坐标系的位置）
二、QT+VTK实现步骤简要介绍（以球的显示为例）
我会以通俗易懂的语言给大家介绍一下它的实现步骤，基本思路就是观众透过窗户演员拿着物体在舞台上进行表演。
2.1 准备需要显示的球体，设置其空间位置与其他（非）必要的参数（如球在空间的位置、球的半径和球的颜色）
vtkSmartPointer
sphereSource->SetCenter(Center); //设置球心位置
sphereSource->SetRadius(Randius); //设置球半径
sphereSource->Update();
2.2 物体的初步理论规划已经做完了，现在就是要一个机器（mapper）将该物体给实体化
mapper_sphere->SetInputConnection(sphereSource->GetOutputPort()); //将球对象与映射器链接
2.3 物体已经实体化后，需要有一个演员（Actor）持有该物体
actor_sphere->SetMapper(mapper_sphere); //映射器与演员绑定
2.4 演员（Actor）已经拿着实体化的物体（mapper）了，现在要做的就是将演员放置在舞台（renderer）上去，并设置舞台背景颜色
renderer->AddActor(actor_sphere);
renderer->SetBackGround(0,0.5,0); //设置舞台背景颜色
2.5 然后在舞台的前面加个窗口（renderwindow），以便观众可以透过窗户看舞台
renderWindow->AddRenderer(renderer); //类似于将舞台放在窗口对面
//renderWindow->SetSize(100,100,100) //这一步是设置窗户大小，因为是在QT中，所以这一步可以不需要，因为在QT控件界面可以调整VTKWidget大小
2.6 设置一个交互器（interactor），方便观众（鼠标）与舞台（renderer）上的演员（Actor）互动（也就是通过鼠标调整窗口内的相机位置）
renderWindowInteractor->SetRenderWindow(renderWindow);
renderWindow->SetInteractor(ui.qvtkWidget->GetInteractor());// 将 QVTKWidget 的交互器与 RenderWindow 关联起来，以实现鼠标和键盘交互功能（可选）
2.7 最后一步，将窗口放在QVTKWidget中，不放的话窗口会独立显示，如果好奇可以试一试不放
ui.qvtkWidget->SetRenderWindow(renderWindow);
至此，QT+VTK简要显示的步骤就介绍到这里，以下为六自由度机械臂（ABB2600）的三维显示。
三、六自由度机械臂ABB2600的QT+VTK三维显示与关节运动
前文我已经教大家在SolidWorks中建立机械臂的坐标系并导出为STL格式的文件了，其中的关节坐标系的建立和关节坐标系相对于世界坐标系的位置尤为重要。
3.1 VTK中的STL模型坐标旋转与偏移
在VTK中，导入的STL模型不论你原先的坐标系是如何的，它首先会与世界坐标系重合，其位置也会在世界坐标系位置处，如下图所示：
如上图所示，我们所需要的参数就是该坐标系旋转的角度和偏移的量，那么它在VTK中如何表示呢，代码如下：
void RobotArm::RobbotArmAssemblyLink(vtkAssembly* assembly,std::string filename, double PositionXYZ[3],double Rotate[3])
{
vtkSmartPointer
vtkSmartPointer
reader->SetFileName(filename.c_str());
reader->Update();
vtkSmartPointer
vtkSmartPointer
mapper->SetInputConnection(reader->GetOutputPort());
actor->SetMapper(mapper);
actor->GetProperty()->SetDiffuse(0.8);
actor->GetProperty()->SetDiffuseColor(colors->GetColor3d("LightSteelBlue").GetData());
actor->GetProperty()->SetSpecular(0.3);
actor->GetProperty()->SetSpecularPower(60.0);
actor->RotateZ(Rotate[2]);
actor->RotateY(Rotate[1]);
actor->RotateX(Rotate[0]);
actor->SetPosition(PositionXYZ);
assembly->AddPart(actor);
}
其中，filename就是文件绝对路径；Rotate[]就是旋转角度；注意！旋转的角度是有顺序的，ZYX和XYZ得到的结果是不一样的，建议按照DH旋转顺序来，也就是ZYX；PositionXYZ[]是偏移值；Assembly下文讲。
3.2 VTK中的VTKAssembly的使用
本文中的VTKAssembly的作用就是使Link1动的时候带动Link2-Link6动，其余都是如此，就相当于Link1手上带着Link2，Link1动的话Link2也会一起动，Link2手上带着Link3，Link3手上带着Link4，以此类推，代码如下：
RobotArm robotarm; //这个是3.1部分的代码基于的类
vtkAssembly* assembly[] = { assembly_base ,assembly_link1 ,assembly_link2 ,assembly_link3 ,assembly_link4 ,assembly_link5 ,assembly_link6 ,assembly_Empty};
for (int i = 0; i < 7; i++)
{
robotarm.RobbotArmAssemblyLink(assembly[i], filename[i], AssemblyLinkCoordinateSystemOffset[i], InitialRotateAngle[i]);//加载模型并旋转至正确的角度
assembly[i]->AddPart(assembly[i + 1]);//添加装配体并将各个关节连接
assembly[i]->SetOrigin(AssemblyLinkCoordinateSystemOffset[i]);//设置关节的旋转中心
}
renderer->AddActor(assembly_base);
renderer->SetBackground(BackGroundColor);//设置背景颜色
renderWindow->Renderer(); //更新窗口
上述代码中的重要功能有：①加载了七个STL模型；②将各个装配体串联起来(注意：装配体添加Link的部分已经在3.1部分函数的最后一行实现了)；③设置关节的旋转中心；
至此，如果你实现到了这里，机械臂的模型部分你就能在QT中的VTKWidget进行显示了，网格和坐标系的建立等会直接给代码。
3.3 滑块QSlider与关节旋转的关联
QSlider在这儿呢~~~~~，要注意，QSlider接收的是整型变量int哈，不接受浮点型，不过接收整型你们就先这样玩玩，接收浮点型又要讲一堆，虽然简单，但是你们也动动脑，看看能不能解决。
该部分的基本逻辑就是，滑动滑块，值改变后，调用槽函数，更新窗口，然后实现旋转。
3.3.1 关节旋转部分
关节的旋转也就是Assembly的旋转，它的旋转轴为自身坐标系的旋转轴，相关函数代码如下：
void VTKTest02::LinkSetOrientation(vtkAssembly* assembly[], double Angle[6])
{
assembly[0]->SetOrientation(0, 0, Angle[0]);
assembly[1]->SetOrientation(0, Angle[1], 0);
assembly[2]->SetOrientation(0, Angle[2], 0);
assembly[3]->SetOrientation(Angle[3], 0, 0);
assembly[4]->SetOrientation(0, Angle[4], 0);
assembly[5]->SetOrientation(Angle[5], 0, 0);
}
3.3.2 槽函数编写部分
这部分简单，我直接给代码：
void VTKTest02::RobotArmLinkRotate()
{
vtkAssembly* assembly[] = { assembly_link1 ,assembly_link2 ,assembly_link3 ,assembly_link4 ,assembly_link5 ,assembly_link6};
double RotateAngle[6] = { double(ui.Link1Rotate->value()) ,double(ui.Link2Rotate->value()) ,double(ui.Link3Rotate->value()) ,double(ui.Link4Rotate->value()) ,double(ui.Link5Rotate->value()),double(ui.Link6Rotate->value()) };//ui.Link1Rotate，这个为QSlider
//设置机械臂的旋转随Qslider变换
LinkSetOrientation(assembly, RotateAngle);
renderWindow->Renderer(); //更新窗口
}
3.3.3 信号与槽
for (QSlider* slider : { ui.Link1Rotate, ui.Link2Rotate, ui.Link3Rotate, ui.Link4Rotate, ui.Link5Rotate, ui.Link6Rotate })
{
connect(slider, &QSlider::valueChanged, this, &VTKTest02::RobotArmLinkRotate);
}
简单解释一下：就是QSlider的值一变化就会调用槽函数，connect的参数顺序为：①谁发出信号；②发出什么信号；③谁处理信号；④怎么处理信号。
至此，关节显示与滑块运动到此结束，如果有什么问题可以与我交流，我虽然很少进来，但有问必答（当然，前提是我得会）。
4.其余代码
4.1 网格代码
void VTKTest02::GridDrawing(double int_row, double int_col)//网格绘制
{
//绘制水平线
for (float row = -int_row / 2; row <= int_row / 2; row += 1000)
{
double p0[3] = { row, -int_row / 2, 0 };
double p1[3] = { row, int_row / 2, 0 };
vtkSmartPointer
points->InsertNextPoint(p0);
points->InsertNextPoint(p1);
vtkSmartPointer
pLine->GetPointIds()->SetId(0, 0);
pLine->GetPointIds()->SetId(1, 1);
vtkSmartPointer
cellArray->InsertNextCell(pLine);
vtkSmartPointer
polyData->SetPoints(points);
polyData->SetLines(cellArray);
vtkSmartPointer
mapper->SetInputData(polyData);
vtkSmartPointer
lineActor_row->SetMapper(mapper);
lineActor_row->GetProperty()->SetColor(1, 1, 1);
lineActor_row->GetProperty()->SetLineWidth(1);
renderer->AddActor(lineActor_row);
}
//绘制垂直线
for (float col = -int_col / 2; col <= int_col / 2; col += 1000)
{
double p0[3] = { -int_col / 2, col, 0 };
double p1[3] = { int_col / 2 , col, 0 };
vtkSmartPointer
points->InsertNextPoint(p0);
points->InsertNextPoint(p1);
vtkSmartPointer
pLine->GetPointIds()->SetId(0, 0);
pLine->GetPointIds()->SetId(1, 1);
vtkSmartPointer
cellArray->InsertNextCell(pLine);
vtkSmartPointer
polyData->SetPoints(points);
polyData->SetLines(cellArray);
vtkSmartPointer
mapper->SetInputData(polyData);
vtkSmartPointer
lineActor_col->SetMapper(mapper);
lineActor_col->GetProperty()->SetColor(1, 1, 1);
lineActor_col->GetProperty()->SetLineWidth(1);
renderer->AddActor(lineActor_col);
}
renderWindow->Render();
}
4.2 坐标系代码
void VTKTest02::CartesianCoordinateSystem()//坐标系绘制
{
// 创建坐标轴演员
vtkSmartPointer
// 设置坐标轴演员的位置和大小
axesActor->SetPosition(0.0, 0.0, 0.0);
axesActor->SetTotalLength(1000, 1000, 100);
axesActor->SetScale(0.1, 0.1, 0.1);
axesActor->SetXAxisLabelText("x");
axesActor->SetYAxisLabelText("y");
axesActor->SetZAxisLabelText("z");
// 将坐标轴演员添加到渲染器中
renderer->AddActor(axesActor);
renderWindow->Render();
}
4.3 正运动学代码（也就是底部末端执行器位置部分）
//正运动学矩阵求解部分
//*-------------------------------------------------------------------------------------------------------------------*
//①绕X轴的旋转矩阵
Matrix4d PositiveAndInverseKinematics::XRotateMatrix(double XRotateAngle)
{
Matrix4d XRotateMatrix;
XRotateMatrix << 1, 0, 0 , 0,
0,cos(XRotateAngle),-sin(XRotateAngle), 0,
0,sin(XRotateAngle), cos(XRotateAngle), 0,
0, 0, 0, 1;
return XRotateMatrix;
}
//②沿X轴的移动矩阵
Matrix4d PositiveAndInverseKinematics::XMoveMatrix(double XMoveDistance)
{
Matrix4d XMoveMatrix;
XMoveMatrix << 1,0,0,XMoveDistance,
0,1,0,0,
0,0,1,0,
0,0,0,1 ;
return XMoveMatrix;
}
//③绕Z轴的旋转矩阵
Matrix4d PositiveAndInverseKinematics::ZRotateMatrix(double ZRotateAngle)
{
Matrix4d ZRotateMatrix;
ZRotateMatrix << cos(ZRotateAngle) ,-sin(ZRotateAngle),0, 0,
sin(ZRotateAngle),cos(ZRotateAngle) ,0, 0,
0, 0, 1, 0,
0, 0, 0, 1 ;
return ZRotateMatrix;
}
//④沿Z轴的平移矩阵
Matrix4d PositiveAndInverseKinematics::ZMoveMatrix(double ZMoveDistance)
{
Matrix4d ZMoveMatrix;
ZMoveMatrix << 1,0,0,0,
0,1,0,0,
0,0,1,ZMoveDistance,
0,0,0,1 ;
return ZMoveMatrix;
}
//⑤两个坐标系之间的转换矩阵
Matrix4d PositiveAndInverseKinematics::MatrixT(double XRotateAngle, double XMoveDistance, double ZRotateAngle, double ZMoveDistance)
{
Matrix4d MatrixT;
//结果矩阵
//MatrixT << cos(ZRotateAngle), -sin(ZRotateAngle), 0, XMoveDistance,
// sin(ZRotateAngle)*cos(XRotateAngle), cos(ZRotateAngle)* cos(XRotateAngle),-sin(XRotateAngle),-sin(XRotateAngle)* ZMoveDistance,
// sin(ZRotateAngle)* sin(XRotateAngle),cos(ZRotateAngle)*sin(XRotateAngle),cos(XRotateAngle),cos(XRotateAngle)* ZMoveDistance,
// 0,0,0,1;
MatrixT = XMoveMatrix(XMoveDistance) * XRotateMatrix(XRotateAngle) * ZMoveMatrix(ZMoveDistance) * ZRotateMatrix(ZRotateAngle);
return MatrixT;
}
//⑥末端执行器相对于世界坐标系的转换矩阵
Matrix4d PositiveAndInverseKinematics::MatrixT06(double XRotateAngle[6], double XMoveDistance[6], double ZRotateAngle[6], double ZMoveDistance[6])
{
Matrix4d MatrixT06;
Matrix4d MatrixT01; Matrix4d MatrixT12; Matrix4d MatrixT23;
Matrix4d MatrixT34; Matrix4d MatrixT45; Matrix4d MatrixT56;
MatrixT01 = MatrixT(XRotateAngle[0], XMoveDistance[0], ZRotateAngle[0], ZMoveDistance[0]);//坐标系0转换到坐标系1的位姿矩阵
MatrixT12 = MatrixT(XRotateAngle[1], XMoveDistance[1], ZRotateAngle[1], ZMoveDistance[1]);//坐标系1转换到坐标系2的位姿矩阵
MatrixT23 = MatrixT(XRotateAngle[2], XMoveDistance[2], ZRotateAngle[2], ZMoveDistance[2]);//坐标系2转换到坐标系3的位姿矩阵
MatrixT34 = MatrixT(XRotateAngle[3], XMoveDistance[3], ZRotateAngle[3], ZMoveDistance[3]);//坐标系3转换到坐标系4的位姿矩阵
MatrixT45 = MatrixT(XRotateAngle[4], XMoveDistance[4], ZRotateAngle[4], ZMoveDistance[4]);//坐标系4转换到坐标系5的位姿矩阵
MatrixT56 = MatrixT(XRotateAngle[5], XMoveDistance[5], ZRotateAngle[5], ZMoveDistance[5]);//坐标系5转换到坐标系6的位姿矩阵
MatrixT06 = MatrixT01 * MatrixT12 * MatrixT23 * MatrixT34 * MatrixT45 * MatrixT56;
return MatrixT06;
}
//⑦绕Y轴的旋转矩阵
Matrix4d PositiveAndInverseKinematics::YRotateMatrix(double YRotateAngle)
{
Matrix4d YRotateMatrix;
YRotateMatrix << cos(YRotateAngle), 0, sin(YRotateAngle), 0,
0 , 1, 0 , 0,
-sin(YRotateAngle), 0, cos(YRotateAngle), 0,
0, 0, 0 , 1;
return YRotateMatrix;
}
末端执行器位置只需要根据你的DH参数把上面的MatrixT06函数求出来后，提取出第1,2,3行的第4列就是对应的XYZ值。
OK，写了我两个小时，终于写完了，因为我把很多部分全部完善了，所以完整代码暂时不方便给哈，不过有什么问题可以问，我尽我所能回答你哦，我真是个好人，QLabel部分自己去想想然后实现一下，简单的嘞，实在不会再来找我，么么哒，别整天想着白嫖，不自己动脑子！！！！！这样是不对的！！！！
相关链接
您阅读本篇文章共花了：
发表评论