Question

我正试图让一个机器人转向面对另一个机器人，基于它们在全局坐标图上的相应坐标。

这是我写的代码，但它似乎根本不起作用：

def calcAngleToCoords(self, curAngle, curPosition, targPosition):
    retVal = False

    if type(curPosition) is list and type(targPosition) is list:
        x_1, y_1 = curPosition
        x_2, y_2 = targPosition
        # Sets origin coordinate to zero
        x_2 = x_2 - x_1
        y_2 = y_2 - y_1

        radius = math.sqrt(y_2 ** 2 + x_2 ** 2) # Pythagorean Thereom, a^2 + b^2 = c^2 | Radius = c, y_2 = a, x_2 = b
        angle = curAngle * (math.pi / 180)

        x_1 = radius * math.cos(angle)
        y_1 = radius * math.sin(angle)

        turnArc = math.atan( (y_1 - y_2) / (x_2 - x_1) ) * (180 / math.pi)

        retVal = turnArc
        # TODO: Check to see if the angle is always clockwise.
    else:
        raise TypeError("Invalid parameter types. Requires two lists.")

    return(retVal)

任何人都可以告诉我一个更好的方法来做这个或我做错了吗？这是我正在进行的项目，截止日期即将到来，所以任何帮助都会受到赞赏！

Answer 1

无需计算半径

    x_2 = x_2 - x_1
    y_2 = y_2 - y_1
    angle = curAngle * (math.pi / 180)
    dx = math.cos(angle)
    dy = math.sin(angle)
    turnArc = math.atan2(x_2 * dy - y_2 * dx,  x_2 * dx + y_2 * dy ) * (180 / math.pi)

注意使用atan2函数，返回-pi和pi之间的角度。它正确地确定旋转方向并找到最短的转弯。如果只需要顺时针方向旋转，如果turnArc为负，则加180;

获得转弯角度到坐标地图上的位置？

1 个答案: