webots学习笔记——控制器controller学习入门

以下是基于webots2023b版本

1、webots控制器可以简单理解为：通过webots支持的编程语言，编写的机器人控制程序。

2、入口：文件->new->新机器人控制器

3、典型存放位置：

webots-project\my_project\controllers\my_controller111\my_controller111.py

4、控制器模板（以python版本为例）如下：

"""my_controller111 controller."""

# You may need to import some classes of the controller module. Ex:
#  from controller import Robot, Motor, DistanceSensor
from controller import Robot

# create the Robot instance.
robot = Robot()

# get the time step of the current world.
timestep = int(robot.getBasicTimeStep())

# You should insert a getDevice-like function in order to get the
# instance of a device of the robot. Something like:
#  motor = robot.getDevice('motorname')
#  ds = robot.getDevice('dsname')
#  ds.enable(timestep)

# Main loop:
# - perform simulation steps until Webots is stopping the controller
while robot.step(timestep) != -1:
    # Read the sensors:
    # Enter here functions to read sensor data, like:
    #  val = ds.getValue()

    # Process sensor data here.

    # Enter here functions to send actuator commands, like:
    #  motor.setPosition(10.0)
    pass

# Enter here exit cleanup code.

基本编写内容如下：

①导入库或库的模块，常见的如 

from controller import Robot, Motor, DistanceSensor  

其它如math，time，ikpy等等

②创建机器人实例 

robot = Robot()

③获取当前世界world的时间步timestep

timestep = int(robot.getBasicTimeStep())

④获取关节对象，使用getDevices，如

joint1 = robot.getDevice("joint1")  # 直线轴，位置控制  
joint2 = robot.getDevice("joint2")  # 旋转轴1，位置控制  
joint3 = robot.getDevice("joint3")  # 旋转轴2，位置控制  

或者

#  motor = robot.getDevice('motorname')
#  ds = robot.getDevice('dsname')
#  ds.enable(timestep)

⑤主循环Main loop，运行循环直到不满足条件退出循环，如：

while robot.step(timestep) != -1:
    # Read the sensors:读取传感器数据
    # Enter here functions to read sensor data, like:
    #  val = ds.getValue()   给传感器赋值

    # Process sensor data here.处理传感器数据

    # Enter here functions to send actuator commands, like:使用函数发送致动器指令
    #  motor.setPosition(10.0)
    pass

# Enter here exit cleanup code.

5、示例：三轴机械臂简单动作

from controller import Robot, Motor  
import time  
  
# 创建Robot对象  
robot = Robot()  
  
# 获取关节对象  
joint1 = robot.getDevice("joint1")  # 直线轴，位置控制  
joint2 = robot.getDevice("joint2")  # 旋转轴1，位置控制  
joint3 = robot.getDevice("joint3")  # 旋转轴2，位置控制  
  
# 设置关节模式为位置控制  
joint1.setPosition(0.0)  # 初始位置，假设为0米  
joint2.setPosition(0.0)  # 初始位置，假设为0弧度  
joint3.setPosition(0.0)  # 初始位置，假设为0弧度  
  
# 设置关节的速度，这有助于控制移动的速度  
joint1.setVelocity(0.05)  # 设置一个适当的速度值，单位可能是米/秒  
joint2.setVelocity(0.2)  # 设置一个适当的速度值，单位可能是弧度/秒  
joint3.setVelocity(0.2)  # 设置一个适当的速度值，单位可能是弧度/秒  
  
# 目标位置（以米为单位，对于直线轴；以弧度为单位，对于旋转轴）  
target_position_joint1 = 0.03  # 直线轴的目标位置，单位：米  
target_position_joint2 = 1.57  # 旋转轴1的目标位置，单位：弧度（90度）  
target_position_joint3 = 1.57  # 旋转轴2的目标位置，单位：弧度（180度）  
  
# 主循环  
while robot.step(32) != -1:  
    # 移动关节到目标位置  
    joint1.setPosition(target_position_joint1)  
    joint2.setPosition(target_position_joint2)  
    joint3.setPosition(target_position_joint3)  
  
    # 在这里，你可以添加逻辑来更新目标位置，或者根据某些条件改变它们。  
    # 例如，你可以从键盘输入读取新的目标位置，或者根据传感器数据调整它们。  
  
    # 等待直到关节到达目标位置（可选，取决于你的需求）  
    while joint1.getPosition() < target_position_joint1 - 0.01 or joint1.getPosition() > target_position_joint1 + 0.01:  
        time.sleep(0.1)  # 短暂休眠以避免过度占用CPU  
    while joint2.getPosition() < target_position_joint2 - 0.01 or joint2.getPosition() > target_position_joint2 + 0.01:  
        time.sleep(0.1)  
    while joint3.getPosition() < target_position_joint3 - 0.01 or joint3.getPosition() > target_position_joint3 + 0.01:  
        time.sleep(0.1)  
  
# 当仿真结束时，此循环将退出

6、思考（人工提问，大模型作答，需注意甄别答案）

6.1  Q:编写webots控制器程序，除了位置控制模式，还有什么其它控制模式？

A:在Webots中，除了位置控制模式（Position Control），还有多种其他的控制模式可用于电机和传感器。以下是一些常见的控制模式：

①速度控制模式（Velocity Control）: 在这种模式下，电机可以被设置为以特定的速度旋转。速度控制模式通常用于需要平滑运动的应用，如车辆的驱动轮。

WbDeviceTag motor = wb_robot_get_device("motor");
wb_motor_set_velocity(motor, velocity);

②力矩控制模式（Torque Control）: 力矩控制模式允许你直接控制电机的输出力矩，而不是其位置或速度。这种模式适用于需要精确力控制的应用，如抓取操作。

wb_motor_set_torque(motor, torque);

③PWM控制模式（PWM Control）: PWM（脉冲宽度调制）控制模式允许你通过改变电机供电的PWM信号的占空比来控制电机的速度和力矩。这种模式提供了对电机行为的精细控制。

至于PWM（脉冲宽度调制）控制模式，Webots本身并不直接提供PWM控制模式。然而，您可以通过编写自定义控制器代码来实现PWM控制。如果您的硬件支持PWM输出，并且Webots提供了相应的接口，那么您可以在Webots中实现PWM控制。

例如，您可以使用Webots的Motor类来模拟PWM控制，通过快速切换电机的速度或位置来实现类似PWM的效果。以下是一个简单的示例代码：

from controller import Robot, Motor
import time

# 创建机器人实例
robot = Robot()

# 获取电机设备
motor = robot.getDevice('motor_name')

# 设置电机为速度控制模式
motor.setPosition(float('inf'))

# PWM参数
frequency = 50  # 频率，单位为Hz
duty_cycle = 0.5  # 占空比，范围为0到1

# 计算周期时间
period = 1.0 / frequency

# 主循环
while robot.step(int(period * 1000)) != -1:
    # 打开电机
    motor.setVelocity(1.0)
    time.sleep(duty_cycle * period)
    
    # 关闭电机
    motor.setVelocity(0.0)
    time.sleep((1 - duty_cycle) * period)

④编码器控制模式（Encoder Control）: 编码器控制模式通常用于读取电机的旋转编码器值，以便进行反馈控制或计算速度和位移。

double position = wb_motor_get_position(motor);
double velocity = wb_motor_get_velocity(motor);

⑤传感器控制模式: 对于传感器，如红外传感器、超声波传感器、触摸传感器等，通常有读取传感器原始数据的模式，然后根据这些数据进行处理和分析。

double value = wb_distance_sensor_get_value(ds);

⑥PID控制模式: 虽然Webots不直接提供PID控制模式，但你可以在你的控制器程序中实现PID算法来控制电机的位置、速度或力矩。

// 伪代码示例
double error = desired_value - current_value;
double output = Kp * error + Ki *累计_error - Kd * 误差变化率;
wb_motor_set_velocity(motor, output);

在编写Webots控制器程序时，你可以根据机器人的需求和应用场景选择最合适的控制模式。这些控制模式可以单独使用，也可以组合使用，以实现更复杂的控制策略。

6.2 如何理解和使用webots的时间步？

在Webots中，时间步（time step 或 timestep）是一个关键概念，它决定了仿真的更新频率和控制器执行循环的速率。时间步是仿真中最小的时间单位，用于同步仿真中的所有事件，包括物理引擎的更新、设备状态的读取和写入、以及控制器逻辑的执行。

理解和使用Webots的时间步，你需要知道以下几点：

1. 时间步大小： 时间步通常以毫秒为单位。较小的时间步可以提供更平滑的仿真和更高的控制精度，但会增加计算负担，可能导致仿真运行缓慢。较大的时间步可以加快仿真速度，但可能会降低控制精度和仿真的真实性。

2. 控制器循环： 控制器程序通常在一个循环中运行，每次迭代对应一个时间步。在每次迭代中，你可以读取传感器数据、计算控制逻辑，并发送命令给执行器。

3. wb_robot_step函数： 这个函数用于执行一个时间步的仿真。它接受一个参数，即时间步的大小（以毫秒为单位）。调用wb_robot_step后，Webots将更新所有设备的传感器数据，并等待指定的时间步长度，然后再返回控制权给控制器程序。

4. 使用时间步的示例： 以下是一个简单的Webots控制器程序示例，展示了如何使用时间步：

from controller import Robot

# 初始化机器人
robot = Robot()

# 设置时间步大小，单位为毫秒
time_step = 32

# 机器人设备初始化等
# 例如，获取电机和传感器的设备名
motor = robot.getDevice("motor")
ds = robot.getDevice("dsname")

# 确保设备不为空
assert motor and ds

# 启用距离传感器，设置时间步大小
ds.enable(time_step)

# 控制器主循环
while robot.step(time_step) != -1:
    # 读取传感器数据
    # 例如，获取距离传感器的值
    distance = ds.value()

    # 计算控制逻辑
    # 这里只是一个示例，实际控制逻辑会更复杂
    # ...

    # 更新执行器状态
    # 例如，设置电机的目标位置或速度
    # motor.set_position(target_position)
    # motor.set_velocity(target_velocity)

# 清理资源，Webots会自动处理，通常不需要手动清理

在这个Python示例中，我们首先从controller模块导入Robot类。然后，我们创建一个Robot实例，并设置时间步大小。在控制器的主循环中，我们使用robot.step(time_step)来执行一个时间步的仿真，这与C版本中的wb_robot_step(time_step)相对应。

请注意，Webots的Python API提供了一些方便的封装，使得代码更加简洁。例如，robot.getDevice("motor")用于获取名为"motor"的电机设备，而ds.value()用于获取距离传感器的值。

在实际应用中，你需要根据你的机器人模型和需求来编写具体的控制逻辑。上面的代码只是一个基本框架，展示了如何在Python中使用Webots进行仿真控制。

      5.性能考虑： 如果你的控制器逻辑非常复杂，可能需要花费超过一个时间步的时间来完成。在这种情况下，你可能需要优化你的代码，或者使用更快的计算机来运行仿真。

      6.实时仿真： Webots支持实时仿真，这意味着仿真时间与真实世界时间同步。在实时仿真模式下，时间步的大小通常设置为1ms，以确保仿真尽可能接近实时。

通过合理设置和使用时间步，你可以有效地控制仿真的运行速度和质量，同时确保控制器逻辑的正确执行。