使用Webots进行并联机构仿真+绘制末端运动轨迹+工作空间分析

本文简介：

本文介绍了如何在Webots中进行并联机构的仿真，包括并联机构模型的构建和修正，controller的构建和末端轨迹球的插入。涉及基本知识较多，先对Solidwork和Webots的基本操作熟悉后再阅读本文更佳。

---

相关链接 / 参考文献：

• 如何将URDF转为PROTO并导入Webots（内部链接）
• 如何将URDF转为PROTO并导入Webots
• 项目仓库
• Tutorial 8: The Supervisor
• Webots Reference Manual R2023b Supervisor

---

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模型

参考《如何将URDF转为PROTO并导入Webots》

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提供的机构模型。