9、教程-5 四轮机器人

本教程旨在从头开始创建您的第一个机器人。机器人将由一个身体、四个轮子和两个距离传感器组成。结果如图所示。下图显示了机器人的俯视图。
tutorial_4_wheels_robot.png

四轮机器人的三维视图。请注意，机器人主体及其轮子的坐标系表示以相同的方式定向。它们的+x矢量（红色）定义机器人的左侧，+y矢量（绿色）定义机器人顶部，+z矢量（蓝色）定义机器人前部。距离传感器以不同的方式定向，它们的+x矢量指示传感器的方向。

四轮机器人的俯视图。机器人后面的网格尺寸为0.2 x 0.3米。文本标签与设备名称相对应。

在实体节点中分离机器人

一些定义和规则来创建机器人模型：

• 包含实体节点及其所有派生节点的集合称为实体节点。

• 类似的定义适用于“设备Device”、“机器人Robot”、“关节Joint”和“电机Motor”节点。您可以在节点图表中获得有关节点层次结构的详细信息。

• 大多数传感器和执行器同时是实体节点和设备Device节点。

• 机器人模型的主要结构是连接在一起的实体节点树。该树的根节点应该是Robot节点。

• 实体通过关节Joint节点链接在一起。设备Device节点应该是机器人节点、实体节点或关节节点的直接子节点。

• 关节Joint节点用于在其父节点和子节点之间添加一个（或两个）自由度（DOF）。关节节点的直接父节点和子节点都是实体节点。

• 从“关节Joint”派生的节点允许在链接的实体节点之间创建不同类型的约束。机器人技术中最常用的是铰链HingeJoint关节，它可以为包括轮子在内的旋转电机建模。

• 可以通过将“位置传感器”节点或电机节点分别添加到其设备域来监控或启动“关节Joint”节点。

考虑到这些规则，我们可以开始设计用于为机器人建模的节点层次结构。第一步是确定机器人的哪个部分应该建模为实体节点。
在我们的示例中，这种操作非常明显。该机器人具有4个自由度，与车轮电机相对应。它可以分为五个实体节点：车身和四个车轮。
根据机器人模型的预期应用，在建模时降低自由度可能是获得有效仿真所必需的。例如，在对脚轮进行建模时，实际的方法意味着对2自由度进行建模。但是，如果这种精度对模拟毫无用处，就可以找到一种更有效的方法。例如，将脚轮建模为与地面摩擦系数为零的球体。
第二步是确定哪个实体节点是Robot节点（根节点）。这种选择是任意的，但解决方案通常更容易实现。例如，在人形机器人的情况下，机器人节点通常是机器人胸部，因为机器人对称性有助于关节参数的计算。
在我们的案例中，身体箱显然是更好的选择。该图描绘了机器人的实体节点层次结构。

在场景树的末尾，添加一个Robot节点，该节点具有四个HingeJoint节点，其中一个Solid节点作为端点。请参考此图。将包含长方体几何体的Shape节点添加到Robot节点。将形状的颜色设置为红色。使用形状定义Robot节点的boundingObject字段。框的尺寸为（0.2、0.1、0.05）。向Robot添加一个Physics节点。该图表示定义机器人的所有节点。到目前为止，只实现了根Robot节点的直接子节点。

HingeJoint

轮子的初始位置由“实体”节点的平移域和旋转域定义。
旋转原点（锚点）和旋转轴（轴）由HingeJoint节点的可选HingeJointParameters子项定义。

车轮的半径为0.04，其厚度（气缸的高度）为0.02。对于第一个车轮，应将实心平移定义为（0.05，0.06，0），以定义车身和车轮之间的相对间隙，并将旋转定义为（10 0 1.5708），以使轮缸正确定向。HingeJointParameters锚点也应定义为（0.05、0.06、0），以定义旋转原点（相对于主体）。最后，HingeJointParameters轴应定义旋转轴。在我们的例子中，它是沿着y轴的（所以（0，1，0））。

添加“HingeJointParameters”节点，然后如上所述输入字段值。一些标志显然必须针对其他车轮进行更新。

现在，让我们实现车轮的圆柱体形状。
对于每个铰链关节，有三个字段需要在其中添加节点。

• jointParameters: 添加HingeJointParameters并配置锚点（0.05-0.06 0）和轴（0 1 0）字段。这些值必须根据车轮的位置进行修改。
• device: 添加一个旋转马达，以便能够驱动车轮。根据此图将其名称字段从wheel1更改为wheel4。这些标签将用于引用控制器中的车轮。
• endPoint:在“实体”的子字段中添加“实体”节点，然后添加“形状”节点，最后在“形状”的几何体字段中添加一个圆柱体。圆柱体的半径应为0.04，高度应为0.02。将轮子的颜色设置为绿色。

完成缺失的节点以获得与图中所示相同的结构。不要忘记物理节点。

Sensors

教程4的电子贴纸上使用的传感器与本节中使用的传感器不同。注意它们的测量间隔在0 cm=0和10 cm=1000之间。在此处查找有关lookupTable字段的更多信息。
机器人建模的最后一部分是在机器人上添加两个距离传感器。这可以通过添加两个DistanceSensor节点作为Robot节点的直接子节点来完成。注意，距离传感器沿着正x轴获取其数据。因此，有必要旋转距离传感器，使其x轴指向机器人外部（见图）。

如上所述，添加两个距离传感器。距离传感器与机器人的前矢量成0.3弧度的角度。将图形和物理形状设置为边为0.01[m]的立方体（未变换）。将颜色设置为蓝色。根据此图的标签设置名称字段。

Controller

在前面的教程中，您已经学习了如何设置反馈回路以及如何读取距离传感器值。但是，执行“旋转电机”节点是一项新功能。要对旋转电机进行编程，第一步是包括与RotationalMotor节点相对应的API模块：

#include <webots/motor.h>

然后，要获取RotationalMotor节点的引用，请执行以下操作：

// initialize motors
WbDeviceTag wheels[4];
char wheels_names[4][8] = {
  "wheel1", "wheel2", "wheel3", "wheel4"
};
int i;
for (i = 0; i < 4 ; i++)
  wheels[i] = wb_robot_get_device(wheels_names[i]);

电机可以通过设置其位置、速度、加速度或力来启动。这里我们感兴趣的是设置它的速度。这可以通过将其位置设置为无穷大并限制其速度来实现：

double speed = -1.5; // [rad/s]
wb_motor_set_position(wheels[0], INFINITY);
wb_motor_set_velocity(wheels[0], speed);

实现一个名为four_wheeled_collision_avoidance的控制器，通过距离传感器检测障碍物来移动机器人并避开障碍物。

请注意，DistanceSensor节点的lookupTable字段指示传感器返回的值。为了帮助调试传感器，可以在机器人窗口中实时查看传感器的值。要打开机器人窗口，双击机器人主体，它将在左侧显示一个菜单，其中包含DistanceSensor和RotationalMotor图形。运行模拟以查看演变。
不要忘记设置Robot节点的控制器字段以指示您的新控制器。
与往常一样，本练习的可能解决方案位于教程目录中。

控制器完整代码

#include <webots/distance_sensor.h>
#include <webots/motor.h>
#include <webots/robot.h>

#define TIME_STEP 64

int main(int argc, char **argv) {
  wb_robot_init();
  int i;
  bool avoid_obstacle_counter = 0;
  WbDeviceTag ds[2];
  char ds_names[2][10] = {"ds_left", "ds_right"};
  for (i = 0; i < 2; i++) {
    ds[i] = wb_robot_get_device(ds_names[i]);
    wb_distance_sensor_enable(ds[i], TIME_STEP);
  }
  WbDeviceTag wheels[4];
  char wheels_names[4][8] = {"wheel1", "wheel2", "wheel3", "wheel4"};
  for (i = 0; i < 4; i++) {
    wheels[i] = wb_robot_get_device(wheels_names[i]);
    wb_motor_set_position(wheels[i], INFINITY);
  }
  while (wb_robot_step(TIME_STEP) != -1) {
    double left_speed = 1.0;
    double right_speed = 1.0;
    if (avoid_obstacle_counter > 0) {
      avoid_obstacle_counter--;
      left_speed = 1.0;
      right_speed = -1.0;
    } else { // read sensors
      double ds_values[2];
      for (i = 0; i < 2; i++)
        ds_values[i] = wb_distance_sensor_get_value(ds[i]);
      if (ds_values[0] < 950.0 || ds_values[1] < 950.0)
        avoid_obstacle_counter = 100;
    }
    wb_motor_set_velocity(wheels[0], left_speed);
    wb_motor_set_velocity(wheels[1], right_speed);
    wb_motor_set_velocity(wheels[2], left_speed);
    wb_motor_set_velocity(wheels[3], right_speed);
  }
  wb_robot_cleanup();
  return 0;  // EXIT_SUCCESS
}