使用Webots进行并联机构仿真+绘制末端运动轨迹+工作空间分析
本文简介：
本文介绍了如何在Webots中进行并联机构的仿真，包括并联机构模型的构建和修正，controller的构建和末端轨迹球的插入。涉及基本知识较多，先对Solidwork和Webots的基本操作熟悉后再阅读本文更佳。
相关链接 / 参考文献：
Webots是一个功能强大的仿真软件，可以进行各种实体包括机器人的仿真。通过Webots我们可以实现并联机构仿真，并可以对其末端工作空间进行分析。
环境
- Webots2023b及以上版本
- Solidwork2019及以上版本
- Solidwork插件：SolidWorks to URDF Exporter
- Python3.5以上版本
- Pip最新版本
1. 进行模型装配
在进行模型装配时，要使模型尽量对称，方便后续在导出PROTO模型出问题时定位问题。此外，装配后的模型的初始姿态，也会影响关节的零点，因此，机构装配的形状需要仔细设计。比如一个3-RRR的并联机构，装配好后如图：
2. 在Solidwork中建立关节坐标系
一般习惯上关节坐标系是以Z轴作为关节的旋转轴。在Solidwork中建立坐标系时，要注意坐标系原点是否处于关节转轴上，Z轴是否和转轴共线。不要使用零件的坐标系，在装配体中自己重新建坐标系。并注意命名规范。
一个3-RRR并联机构建立的几个坐标系如图：
3. 使用SolidWorks to URDF Exporter导出URDF模型
打开SolidWorks to URDF Exporter
并配置好各个Link和关节的转轴、坐标系、关节类型以及Link对应的零件。
注意，该工具不能够直接导出并联机构的URDF，因此我们在每条链的最后都选中同一个动平台末端，但注意3条链和动平台连接的关节和转轴都是不一样的。
填写相关参数并检查无误后导出。
4. 使用urdf2webots工具将URDF模型转为PROTO模型
5. 修正PROTO模型
由于在导出URDF模型时，我们的动平台被导出、链接了3次，因此，需要使用PROTO模型的引用语法修正模型。
可以看到，导出后3条链对应的动平台模型代码分别如图所示，并注意第一条链对应的动平台命名如箭头所指
将上图中第2、第3条链连接的动平台的
endPoint Solid {
......
}
替换为
endPoint SolidReference {
solidName "LinkEnd"
}
这里的LinkEnd应当和第1条链中图片所指的名字一致，如下图：
保存，此时将PROTO文件导入Webots中，就已经可以看到并联机构正常显示了。
除了需要修改动平台的PROTO代码外，还需要将被动副的电机力矩全部设为0（或者直接把整个RotationalMotor删掉）：
6. 给机构（机械臂）末端添加一个用于定位的实体
在第一条链连接动平台的代码的地方，插入一个实体用于末端定位（注意代码插入的位置）：
# 用于获取末端位姿的节点
HingeJoint {
jointParameters HingeJointParameters {
axis 0.000000 0.000000 1.000000
}
endPoint DEF EndForMeasure Solid {
translation 0.118000 0.000000 -0.06500
rotation 0.000000 0.000000 0.000000 1.000000
children [
Shape {
appearance DEF LinkEndMeasure_material PBRAppearance {
baseColor 1 0 0
roughness 1.000000
metalness 0
}
geometry DEF LinkEndMeasure Sphere { radius 0.002 subdivision 5 }
}
]
name "LinkEndMeasure"
}
}
效果如图（一个小红球）：
如何修改这个定位实体的位置呢？通过修改代码中的translation后面的3个数值，就可以改变定位实体的位置：
endPoint DEF EndForMeasure Solid {
translation 0.118000 0.000000 -0.06500 #修改这里3个数值，分别对应XYZ
注意，这里的XYZ，参考的坐标系为链1直接连接动平台的关节的坐标系，也就是这个坐标系：
再通过对模型进行测量，即可精确获得XYZ分别应该是多少。
7. 导入模型，构建controller代码
导入模型前，将PROTO模型的supervisor值设为TRUE
导入模型，并新建3个控制盒（如图的3个方块）：
对控制盒进行DEF（全局定义）：
控制盒的产生主要是为了模仿滑动条的方式控制机构关节。也可以通过supervisor产生轨迹球的方式（见下文）动态产生。
新建一个controller：
controller主循环代码如下，注意将机器人初始化为supervisor：
#include <webots/Motor.hpp>
#include <webots/Supervisor.hpp>
#include <iostream>
using namespace webots;
#define move_ratio 10 //盒子运动的距离和关节运动的系数
// 机构的位置（要使用supervisor进行强制，避免机构抖动
const double robotTranslation[3] = {0, 0, 0};
const double robotRotation[4] = {0, 0, 1, 0};
// 控制盒的默认位置
const double defaultBox1Translation[3] = {-0.25, 0, 0};
const double defaultBox2Translation[3] = {-0.35, 0, 0};
const double defaultBox3Translation[3] = {-0.45, 0, 0};
// 控制盒的当前位置
const double *Joint1_Box_Position;
const double *Joint2_Box_Position;
const double *Joint3_Box_Position;
// 定位实体的当前位置
const double *Measure_Sphere_Position;
int main(int argc, char **argv) {
Supervisor *robot = new Supervisor();
// get the time step of the current world.
int timeStep = (int)robot->getBasicTimeStep();
// 获取机构主动副电机
Motor* joint1 = robot->getMotor("Joint1");
Motor* joint2 = robot->getMotor("Joint2");
Motor* joint3 = robot->getMotor("Joint3");
// 初始化控制盒的位置
robot->getFromDef("Joint1_box")->getField("translation")->setSFVec3f(defaultBox1Translation);
robot->getFromDef("Joint2_box")->getField("translation")->setSFVec3f(defaultBox2Translation);
robot->getFromDef("Joint3_box")->getField("translation")->setSFVec3f(defaultBox3Translation);
while (robot->step(timeStep) != -1) {
// 强制机器人的位置和姿态,防止抖动偏移
robot->getFromDef("binglianrobot")->getField("translation")->setSFVec3f(robotTranslation);
robot->getFromDef("binglianrobot")->getField("rotation")->setSFRotation(robotRotation);
// 获取控制盒的位置XYZ
Joint1_Box_Position = robot->getFromDef("Joint1_box")->getPosition();
Joint2_Box_Position = robot->getFromDef("Joint2_box")->getPosition();
Joint3_Box_Position = robot->getFromDef("Joint3_box")->getPosition();
// 获取定位实体的位置XYZ
Measure_Sphere_Position = robot->getFromDef("binglianrobot")->getFromProtoDef("EndForMeasure")->getPosition();
// 根据控制盒的y方向的位置，控制关节位置
joint1->setPosition(Joint1_Box_Position[1]*move_ratio);
joint2->setPosition(Joint2_Box_Position[1]*move_ratio);
joint3->setPosition(Joint3_Box_Position[1]*move_ratio);
// 显示末端轨迹
ViewTrajectary(robot, Joint1_Box_Position[1], Joint2_Box_Position[1], Joint3_Box_Position[1], Measure_Sphere_Position);
};
delete robot;
return 0;
}
其中，ViewTrajectary函数定义如下：
#include <string>
#define maxTracBallNum 500 //最多的轨迹球的数量
/**
* @brief 轨迹可视化
*
* @param robot 机器人
* @param Joint1_Box_Position 控制盒y方向的位置
* @param Joint2_Box_Position 控制盒y方向的位置
* @param Joint3_Box_Position 控制盒y方向的位置
* @param Measure_Sphere_Position 定位实体的当前位置XYZ
*/
void ViewTrajectary(Supervisor *&robot, const double Joint1_Box_Position, const double Joint2_Box_Position, const double Joint3_Box_Position, const double *&Measure_Sphere_Position)
{
std::string ballprotoString;
static double last_joint1_box_pos = 0, last_joint2_box_pos = 0, last_joint3_box_pos = 0; //控制盒y方向上一次的位置
static int tracBallNum = 0; //当前轨迹球数量，数值循环
if (Joint1_Box_Position != last_joint1_box_pos || Joint2_Box_Position != last_joint2_box_pos || Joint3_Box_Position != last_joint3_box_pos) //如果控制盒发生了移动
{
if(robot->getFromDef("TracBall"+std::to_string(tracBallNum+1))) //尝试删除原有的轨迹球
{
robot->getFromDef("TracBall" + std::to_string(tracBallNum + 1))->remove();
}
// 产生轨迹球命令字符串
ballprotoString = "DEF TracBall" + std::to_string(tracBallNum + 1) + " trajectary {translation " + std::to_string(Measure_Sphere_Position[0]) + " " + std::to_string(Measure_Sphere_Position[1]) + " " + std::to_string(Measure_Sphere_Position[2]) + "}";
robot->getRoot()->getField("children")->importMFNodeFromString(-1, ballprotoString); //导入轨迹球
tracBallNum++; //轨迹球计数
tracBallNum %= maxTracBallNum;
//更新控制盒y方向上一次的位置
last_joint1_box_pos = Joint1_Box_Position;
last_joint2_box_pos = Joint2_Box_Position;
last_joint3_box_pos = Joint3_Box_Position;
}
}
核心是通过robot->getRoot()->getField("children")->importMFNodeFromString的方式，直接在世界中插入多个Solid实体。注意到，这里的Solid是通过已有的PROTO定义的，因此我们需要在该仿真项目的protos文件夹下新建一个trajectary.proto文件，并定义好轨迹球的模型：
PROTO trajectary [
field SFVec3f translation 0 0 0
field SFRotation rotation 0 0 1 0
field SFString name "trajectaryball"
]
{
Solid {
translation IS translation
rotation IS rotation
children [
Shape {
appearance DEF base_link_material PBRAppearance {
baseColor 1 0 0 #这里可以改颜色RGB
roughness 1.000000
metalness 0
}
geometry Box { size 0.0015 0.0015 0.0015} #这里可以改大小
}
]
name IS name
}
}
轨迹球模型推荐用Box，不推荐用球（轨迹密集会导致仿真卡顿和模型起飞）。使用代码调用轨迹球的PROTO前记得点击webots界面的“IMPORTABLE EXTERNPROTO”，选择刚刚建好的轨迹球PROTO。
编译运行：
运行效果如图，点击轨迹球就可以读出对应的坐标信息了，非常直观：
8. 结语
由于涉及大量Webots的入门知识，在此写得比较粗略，建议读者结合Webots官方的入门教程多读几次便可理解。
最后感谢Goldfish提供的机构模型。
