Webots控制两轮差速机器人从一个点到另一个点

新建一个控制器，命名为get_to_goal.py

Webots运行视频如下：

Webots编写控制程序使二轮差速小车导航至目标点

控制器代码如下

from controller import Supervisor
import math
import random

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

#获取电机
motor_right = robot.getDevice("motorRight")
motor_left = robot.getDevice("motorLeft")

#设置电机模式与电机初始速度
motor_right.setPosition(float("inf"))
motor_right.setVelocity(0.0)
motor_left.setPosition(float("inf"))
motor_left.setVelocity(0.0)


name = robot.getFromDef("car")

# 格式化x和z，保留小数点后四位
# robot_x = float("{:.4f}".format(x))
# robot_z = float("{:.4f}".format(z))
# print("X坐标:", robot_x)
# print("Z坐标:", robot_z)


# 假设机器人当前在原点且朝向与y轴负方向一致
CURRENT_ANGLE = -math.pi / 2

# 定义移动速度和角速度
LINEAR_SPEED = 3.14  # 线性速度，单位：米/秒

def get_to_goal(name, target_x, target_z, CURRENT_ANGLE):
    robot_x, robot_z = car_position(name)
    # 目标点与机器人之间的相对角度
    # math.atan2返回值的范围是从 -π 到 π（不包括π），表示的是 (x, y) 点与原点的连线与正x轴之间的夹角（以弧度为单位）
    target_angle = math.atan2(target_z - robot_z, target_x - robot_x)
    print("目标点与机器人之间的相对角度:", target_angle)
    angle_difference1 = target_angle - CURRENT_ANGLE
    # 到达目标点所需的角度差, 未考虑angle_difference1=NaN
    if angle_difference1 > math.pi:
        angle_difference = angle_difference1 - 2 * math.pi
        print("到达目标点所需的角度差:", angle_difference)
    else:
        angle_difference = angle_difference1
        print("到达目标点所需的角度差:", angle_difference)
    rotate_to_target_angle(name, -angle_difference) # 仿真方向与正常方向相反
    move_to_target(name, target_x, target_z)


def car_angle(name):
    rotation = name.getField("rotation")
    WorldRot = rotation.getSFRotation()
    qx, qy, qz, qw = WorldRot
    # theta = 2 * math.acos(qw)
    return qw
def car_position(name):
    trans = name.getField("translation")
    WorldCoord = trans.getSFVec3f()
    x, _, z = WorldCoord
    return x, z

# 首先旋转到目标角度
def rotate_to_target_angle(name, target_angle):
    # 对应仿真左转
    if target_angle - car_angle(name) > 0:
        motor_left.setVelocity(LINEAR_SPEED/8)
        motor_right.setVelocity(-LINEAR_SPEED/8)
        while robot.step(timestep) != -1:
            if car_angle(name) >= target_angle:
                motor_left.setVelocity(0.)
                motor_right.setVelocity(0.)
                return
    # 对应仿真右转  car_angle为负
    if target_angle - car_angle(name) < 0:
        motor_left.setVelocity(-LINEAR_SPEED/8)
        motor_right.setVelocity(LINEAR_SPEED/8)
        while robot.step(timestep) != -1:
            # print("car_angle" + str(car_angle(name)))
            if car_angle(name) <= target_angle:
                print("car_angle" + str(car_angle(name)))
                print("target_angle" + str(target_angle))
                motor_left.setVelocity(0.)
                motor_right.setVelocity(0.)
                return

# 旋转到目标角度后，直线移动到目标点
def move_to_target(name, TARGET_X, TARGET_Y):
    left_speed = LINEAR_SPEED
    right_speed = LINEAR_SPEED

    # 负号对应仿真前进直线移动
    motor_left.setVelocity(-left_speed)
    motor_right.setVelocity(-right_speed)
    while robot.step(timestep) != -1:
        x, z = car_position(name)
        dis = math.sqrt((TARGET_X - x) ** 2 + (TARGET_Y - z) ** 2)
        # print("dis:", dis)
        if dis <= 0.01:
            print("x:" + str(x))
            print("z:" + str(z))
            motor_left.setVelocity(0)
            motor_right.setVelocity(0)
            return
while True:
    x = random.uniform(0, 0.4)
    y = random.uniform(0, 0.4)
    print("x:" + str(x))
    print("y:" + str(y))
    get_to_goal(name,x, y, CURRENT_ANGLE)