Webots+ROS学习记录(3)——四轮移动机器人

一、四轮移动机器人模型

本教程的目的是从头创建你的第一个机器人。这个机器人将由一个身体、四个轮子和两个距离传感器组成。结果如图1所示。图2从俯视图显示了机器人的坐标关系。
图1图二-代表机器人身体和轮子的坐标系是相同的方向。它们的+x向量(红色)定义了机器人的左侧，它们的+y向量(绿色)定义了机器人的顶部，它们的+z向量(蓝色)定义了机器人的前部。但是距离传感器的方向不同，它们的+x向量表示传感器的方向

二、步骤

a) 新建一个仿真

  1. 新建一个工程，确保你的新world里面有地面、墙壁和照明（ground, the walls and the lighting）保存world文件为4_wheels_robot.wbt。

  2. 在给出创建机器人模型的规则之前，需要一些定义。

  • 包含Solid节点及其所有派生节点的集合称为solid节点。类似的定义也适用于Device, Robot, Joint 和 Motor节点。机器人模型的主要结构是一个实体节点连接在一起的树。Joint节点用于在其父节点和子节点之间添加一个(或多个)自由度(DOF)。Joint节点的直接父节点和子节点都是实节点。从Joint派生的节点允许在链接的solid节点之间创建不同类型的约束。
  • 在我们的例子中：机器人有4自由度，对应于车轮电机。它可以分为五个实体节点:身体和四个轮子。

b) 将机器人分成实体节点

  1. 在场景树的最后，添加一个具有四个HingeJoints节点的Robot节点，该节点有一个Solid子节点
    图3

  2. 向robot节点添加包含Box geometry的shape节点。将形状的颜色设置为红色。使用该形状还可以定义机器人节点的boundingObject字段。Box的尺寸是(0.1,0.05,0.2)。为机器人添加一个Physics节点。图4表示定义机器人的所有节点。到目前为止，只实现了根robot节点的直接子节点。图4

c) 创建HingeJoints

  1. 添加一个HingeJointParameters节点。车轮的初始位置由实体节点的translation和rotation确定。而旋转原点(anchor)和旋转轴(axis)则由可选的铰节点的HingeJointParameters子节点定义。对于第一个轮子，实体平移应定义为(0.06,0,0.05)，以便车身与轮子之间有相对间隙。HingeJointParameters-anchor也应定义为(0.06,0,0.05)来定义旋转原点(相对于body)。最后，HingeJointParameters-
    axis在我们的例子中它沿着x轴，设为所以(1,0,0)。

  2. 完成缺失的节点，得到与图4所示相同的结构。将Transform节点Rotation设置为(0,0,1,Pi/2)。圆柱体的半径为0.04，高度为0.02。把轮子的颜色设为绿色。

  3. 为了能够驱动车轮，在每个铰链关节上添加一个RotationalMotor，并将它们的name字段设置为“wheel1”（注意不是DEF）。name命名之后，可以在控制器中直接使用。

d) 添加Sensors

  1. 如前所述，添加两个距离传感器。距离传感器与机器人前矢量成角度为0.3
    [rad]。将它们的类型字段设置为“sonar”。将它们的图形和物理形状设置为边长为0.01的立方体(不是transform)[m]。设置它们的颜色为蓝色。同样的，给他们设置name字段。

e) 编写控制器

  1. 要对旋转电机进行编程，第一步是包含与RotationalMotor节点对应的API模块: #include <webots/motor.h>

  2. 然后初始化RotationalMotor

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]);
}；
  1. 可以通过设定电机的位置、速度、加速度或力来驱动电机(参见参考手册)。这里我们感兴趣的是设定它的速度。
double speed = -1.5; // [rad/s] 

wb_motor_set_position(wheels[0], INFINITY); 

wb_motor_set_velocity(wheels[0], speed);
  1. 注意，DistanceSensor节点的lookupTable字段指示传感器返回哪些值(参见参考手册)。

v. 参考程序如下：

#include <webots/robot.h>

#include <webots/motor.h>

 

#define TIME_STEP 64

 

int main(int argc, char **argv) {
  wb_robot_init();
  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]
  for (i=0; i<4 ; i++)
  {
    wb_motor_set_position(wheels[i], INFINITY);
    wb_motor_set_velocity(wheels[i], speed);
  }
  /* main loop
   * Perform simulation steps of TIME_STEP milliseconds
   * and leave the loop when the simulation is over
   */
while (wb_robot_step(TIME_STEP) != -1) {
    /*
     * Read the sensors :
     * Enter here functions to read sensor
data, like:
     * double val = wb_distance_sensor_get_value(my_sensor);
     */
    /* Process sensor data here */
    /*
     * Enter here functions to send actuator
commands, like:
     * wb_motor_set_position(my_actuator,
10.0);
     */
  };
  /* Enter your cleanup code here */
  /* This is necessary to cleanup webots
resources */
  wb_robot_cleanup();
  return 0;
}

结果