QT与Coin3D实现机器人的仿真

前言

Coin3D编程环境配置好后，开始下一步的机器人仿真任务。本文根据在研5轴机器人，完成三维模型生成与机器人的仿真。主要参考博主冯Jungle的博客，在此表示感谢。
——2021.05.23

环境

Win10：VS2019、QT5.15.2、Coin3D、SolidWorks2016

机器人三维模型生成

目前广泛应用于因特网的VRML ( Virtual Reality Modeling Language，虚拟现实建模语言)为用户提供一种可参与的、并能对场景中对象主动做出反应的虚拟现实环境，现已成为一种比较有效的程序设计语言。
创建VRML文件一般有三种方法。第一种方法是直接使用文本编辑器书写VRML文件。这样做的不足之处是只能创建比较简单的物体和场景，因为几乎不可能使用文本编辑器直接编写出复杂的场景。场景之间的相互关系和位置确定起来太过复杂，对复杂物体的建模显得无能为力，且对数学能力的要求也较高。第二种方法是使用可视化编辑器建模。这就避免了直接使用文本建模时的一些问题，如不能生成复杂场景等缺陷。这些工具的一个突出的优点就是使用方便，不需要手工输入大量命令而只需拖动鼠标即可创建复杂的场景。常见的可视化编辑器有Cosmo World 2.0、Virtual Home Space Builder、Community Place、Pioneer 以及World View等。第三种方法是使用常见的三维建模软件进行场景文件的转换。
本文使用第三种建模方式，利用SolidWorks2016直接将三维模型导出为VRML格式。

将三维模型装配成相互独立的部件（我这里是5轴机器人，所以有六个部件）

根据后面Coin3D中机器人三维模型连接的两种方式，这里生成三维模型也可以有两种方式。

方式一：将坐标系建立在旋转轴原点

以部件3为例，只需建立在旋转轴的原点，方向任意

方式二：将坐标系与根据DH法建立的坐标系对应

原点对应，方向也对应

我这里DH法建模坐标系如下图所示：

所以部件3与部件4的坐标系建立如下图所示：

同一高度

导出VRML格式

另存为->保存类型：VRML

点击选项

输出为版本：VRML97，单位：毫米
选择在单一文件中保存装配体
输出坐标系选择自己建立好的坐标系

至此已经生成了用于仿真的三维模型文件

UI界面编写

Coin3D中机器人仿真

Coin3D对3D数据使用的是右手坐标系，从屏幕内指向外，表示z轴的正方向。所有的角度单位都是弧度。对象都是在自己的局部坐标系空间下进行描述的，既众所周知的“对象坐标系空间”（object coordinate space）。当场景中的所有物体都已经进行完坐标变换后，那么它们就都在“世界坐标系空间”下描述了（world coordinate space）。照相机和灯光节点处于世界坐标系空间下。

本文的五轴机器人仿真模型节点图如图所示

具体流程为：

1、读入三维模型

SoVRMLGroup * Robot::openModel(QString fileName)
{
	SoInput * myInput = new SoInput;
	if (myInput->openFile(fileName.toStdString().data()))
	{
		SoVRMLGroup *model = SoDB::readAllVRML(myInput);
		myInput->closeFile();
		delete myInput;
		return model;
	}
	myInput->closeFile();
	delete myInput;
	return NULL;
}

2、进行坐标变换

方式一：
根据前面三维模型生成中方式一的三维模型

核心思想是，将模型的坐标系相对当前局部坐标系变换到目标坐标系，变换完成后部件就在世界坐标系下描述了。

核心代码如下（有注释）：

#include <Inventor/nodes/SoTransform.h>
#include <Inventor/nodes/SoRotationXYZ.h>

SoTransform* angleAxisTransform = new SoTransform;//模型变换节点
SoRotationXYZ* angleAxisRotation = new SoRotationXYZ;
angleAxisTransform->translation.setValue(0, 0, 500);//将模型沿Z轴平移500mm
angleAxisTransform->rotation.setValue(SbVec3f(1, 0, 0), 1.5707963f);//将模型绕X轴旋转90度
angleAxisRotation->axis.setValue("Z");//设置旋转轴为Z（变换后的坐标系Z轴）

方式二：

根据前面三维模型生成中方式二的三维模型

核心思想是根据节点SoMatrixTransform，将坐标变换描述成DH法中T0*T1*T2*T3*T4*T5的形式，优点是可以进行正逆运动学解算等操作。

这里需要理解一下几种不同的节点，变换节点是累乘的，所以angleAxis会向下传递，这样就有了前面轴转动后面部件跟着一起转动的效果。参考Coin3D相关模块学习

核心代码如下：

SoDHTransform.h

#ifndef SODHTRANSFORM_H
#define SODHTRANSFORM_H

#include <QString>
#include <QDebug>

#include <Inventor/nodes/SoMatrixTransform.h>
#include <qmath.h>

class SoDHTransform : public SoMatrixTransform
{
public:
	enum DHTransformType
	{
		FIXED, 
		ROTATION, 
		MOVEMENT
	};

	SoDHTransform();
	~SoDHTransform();
	void init(double itheta, double id, double ialpha, double ia, DHTransformType type = ROTATION);
	void transform(double value);
	SoMatrixTransform * inverseMatrix();
	double getValue();
	QString getTpye();

	QString name;
	QString description;
	double max_ang;
	double min_ang;

private:
	DHTransformType type;
	double theta;
	double d;
	double a;
	double alpha;
	void calculate();
};

#endif // SODHTRANSFORM_H

SoDHTransform.cpp

#include "sodhtransform.h"

SoDHTransform::SoDHTransform()
	:theta(0.0), d(0.0), a(0.0), alpha(0.0), type(ROTATION), name(""), description(""), max_ang(180), min_ang(-180)
{

}

SoDHTransform::~SoDHTransform()
{

}

void SoDHTransform::init(double itheta, double id, double ialpha, double ia, DHTransformType type)
{
	this->type = type;
	this->theta = itheta / 180 * 3.141592653;
	this->d = id;
	this->a = ia;
	this->alpha = ialpha / 180 * 3.141592653;
	calculate();
}

void SoDHTransform::transform(double value)
{
	if (this->type == ROTATION)
	{
		this->theta = value / 180 * 3.141592653;
	}
	else if(this->type == MOVEMENT)
	{
		this->d = value;
	}
	else //this->type = FIXED
	{
		qDebug() << "Warning: this->type = FIXED";
	}
	calculate();
}

void SoDHTransform::calculate()
{
	this->matrix.setValue(
		 qCos(theta), qSin(theta)*qCos(alpha),  qSin(theta)*qSin(alpha),  0.0,
		-qSin(theta), qCos(theta)*qCos(alpha),  qCos(theta)*qSin(alpha),  0.0,
		         0.0,            -qSin(alpha),              qCos(alpha),  0.0,
		           a,        -d * qSin(alpha),            d*qCos(alpha),  1.0);
}

SoMatrixTransform * SoDHTransform::inverseMatrix()
{
	SoMatrixTransform *inMatrix = new SoMatrixTransform;
	inMatrix->matrix.setValue(this->matrix.getValue().inverse());
	return inMatrix;
}

double SoDHTransform::getValue()
{
	if (type == FIXED)
	{
		return 0.0;
	}
	else if (type == MOVEMENT)
	{
		return d;
	}
	else if (type == ROTATION)
	{
		return theta * 180.0 / 3.141592653;
	}
	else
		return -1;
}

QString SoDHTransform::getTpye()
{
	if (type == FIXED)
	{
		return "FIXED";
	}
	else if (type == MOVEMENT)
	{
		return "MOVEMENT";
	}
	else if (type == ROTATION)
	{
		return "ROTATION";
	}
	else
		return "";
}

根据DH法结果，设置DH参数

void Robot::setDHParameters()
{
	a2 = 400.00;
	a3 = 185.00;  
	a4 = 384.50;
	d2 = 677.15;

	angleAxis1->init( 0.0,   0.0,   0.0,  0.0, SoDHTransform::MOVEMENT);
	angleAxis2->init( 0.0,   d2,  0.0,   0.0);
	angleAxis3->init( 0.0,  0.0,   0.0,   a2);
	angleAxis4->init( 0.0,   a3,  -90.0,   0);	
	angleAxis5->init( 0.0,  0, -90.0,  0);
}

效果

参考资料：

Qt设计机器人仿真控制器

Coin3D相关模块学习