Question

（用于上下文）我们要求：定义一个名为move（）的方法。它应该接受一个Movement对象并相应地更新位置。假设线速度以米/秒为单位，角速度以弧度/秒为单位。为了简化问题，请将机器人视为平面上的无量纲点。同样，假定首先评估角速度，以便机器人在向前或向后移动之前先旋转。

import math

class Movement(object):
    """A class describing a TurtleBot's movement

    @param linear_velocity in the range [-0.22, 0.22]
    @param angular_velocity in the range [-2.84, 2.84]
    @param duration in seconds, [0, +inf)
    """

    MAX_LIN_VEL = 5.0
    MAX_ANG_VEL = 2*math.pi

    def __init__(self, linear_velocity=0, angular_velocity=0, duration=1):
        # Check inputs
        self._linear_velocity = linear_velocity
        self._angular_velocity = angular_velocity
        self._duration = duration

        if not abs(linear_velocity) <= self.MAX_LIN_VEL:
            print('Invalid linear_velocity, set to 0')
            self._linear_velocity = 0
        if not abs(angular_velocity) <= self.MAX_ANG_VEL:
            print('Invalid angular_velocity, set to 0')
            self._angular_velocity = 0
        if duration <= 0:
            print('Invalid duration, set to 0')
            self._duration = 0

    def __str__(self):
        return "Movement(linear_velocity=" + str(self._linear_velocity) + ", angular_velocity=" + str(self._angular_velocity) + ", duration=" + str(self._duration) + ")"

    def get_linear_velocity(self):
        return self._linear_velocity

    def get_angular_velocity(self):
        return self._angular_velocity

    def get_duration(self, unit="s"):
        if unit == "ms":
            return self._duration * 1000
        elif unit == "m":
            return self._duration / 60
        else:
            if not unit == "s":
                print("Invalid unit. Unit set to seconds.")
            return self._duration


class MobileRobot:
    def __init__(self, x=0, y=0, angle=0):
        self._x = x
        self._y = y
        self._angle = math.pi/2

    def step_right(self):
        self._x += 1

    def step_left(self):
        self._x -= 1

    def step_up(self):
        self._y += 1

    def step_down(self):
        self._y -= 1

    def get_current_location(self):
        return 'Location = (' + str(self._x) + ','+ str(self._y) + ')'

    def return_to_origin(self):
        self._x= 0
        self._y= 0
        self._angle=  math.pi/2

    def move(self, Movement):
        duration = Movement.get_duration()
        angular_velocity = Movement.get_angular_velocity()
        self._angle += angular_velocity * duration + self._angle

        distance = Movement.get_linear_velocity() * Movement.get_duration()
        self._x += (distance * math.cos(self._angle))
        self._y += (distance * math.sin(self._angle))

print(Movement(0.1))
robot1 = MobileRobot()
move1 = Movement(1,math.pi/2,1)
robot1.move(move1)
print(robot1.get_current_location())

输出应该是：

Movement(linear_velocity=0.1, angular_velocity=0, duration=1)
None
(-1.0, 1.2246467991473532e-16)

但是我得到的是：

Movement(linear_velocity=0.1, angular_velocity=0, duration=1)
Location = (-1.8369701987210297e-16,-1.0)

我绝对困惑为什么x和y互换，而我似乎无法指出错误。

Answer 1

self._angle += angular_velocity * duration + self._angle

这等效于self._angle = self._angle + angular_velocity * duration + self._angle；即除运动指示的旋转之外，先前的值还计算为两次。

Answer 2

旋转太多，导致您在Y轴而不是X轴上移动。

此行是您的问题：

self._angle += angular_velocity * duration + self._angle

应该是：

self._angle += angular_velocity * duration

我没有找到正确的输出位置。 x，y互换。我看不到我在做什么

2 个答案: