Question

我正在尝试使用以下内容作为指南为机器人绘制曲线路径：http://rossum.sourceforge.net/papers/CalculationsForRobotics/CirclePath.htm

我所拥有的代码不会创建一个以目的地结尾的路径。我希望路径向左或向右弯曲，具体取决于目的地所在的象限（+ x + y，+ x-y，-x + y，-x-y）

import math
start = [400,500]
dest = [200,300]
speed = 10
startangle = 0
rc =0
rotv =0
rads =0

def getPos(t):
    ang = (rotv*t)+rads
    x = start[0] - rc * math.sin(rads) + rc * math.sin(rotv*(t)+rads)
    y = start[1] + rc * math.cos(rads) - rc * math.cos(rotv*(t)+rads)
    return (int(x),int(y), ang)

dx = dest[0] - start[0]
dy = dest[1] - start[1]
rads = math.atan2(-dy,dx)
rads %= 2*math.pi
distance = (dx**2 + dy**2)**.5  #rg
bangle = 2*rads
rc = distance /(2 * math.sin(rads))
if rads > (math.pi/2):
    bangle = 2*(rads-math.pi)
    rc= -rc
if rads < -(math.pi/2):
    bangle = 2*(rads+math.pi)
    rc= -rc
pathlength = rc * bangle
xc = start[0] - rc * math.sin(rads)
yc = start[1] + rc * math.cos(rads)
rotcenter = [xc,yc]
traveltime = pathlength/speed
rotv = bangle/traveltime
for p in range(int(traveltime)):
    pos = getPos(p)

开始：蓝色，结束：红色，旋转点：紫色

更新：我添加了代码以允许正负x / y值。我已经更新了图片。

Answer 1

要回答您的问题，我首先阅读您关联的文章。我认为它非常有趣并且很好地解释了公式背后的想法，但是它缺少当起始位置不在原点并且起始角度不是0时的公式。

花了一点时间来提出这些公式，但现在它适用于我能想到的每一个案例。为了能够使用链接文章中给出的公式，我使用了那里给出的变量的名称。请注意，我还使用带有t_0的符号作为开始时间，您只是忽略了它。您可以轻松删除t_0的任何实例或设置t_0 = 0。

以下代码的最后一部分用于测试并创建一个小红龟，它跟踪指定方向上计算弧的路径。黑乌龟表示目标位置。两个海龟在动画结束时彼此靠近，但它们并不直接在彼此之上，因为我只是迭代整数而且t_1可能不是整数。

from math import pi, hypot, sin, cos, atan2, degrees

def norm_angle(a):
    # Normalize the angle to be between -pi and pi
    return (a+pi)%(2*pi) - pi

# Given values
# named just like in http://rossum.sourceforge.net/papers/CalculationsForRobotics/CirclePath.htm
x_0, y_0 = [400,500] # initial position of robot
theta_0 = -pi/2      # initial orientation of robot
s = 10               # speed of robot
x_1, y_1 = [200,300] # goal position of robot
t_0 = 0              # starting time

# To be computed:
r_G = hypot(x_1 - x_0, y_1 - y_0)        # relative polar coordinates of the goal
phi_G = atan2(y_1 - y_0, x_1 - x_0)
phi = 2*norm_angle(phi_G - theta_0)      # angle and 
r_C = r_G/(2*sin(phi_G - theta_0))       # radius (sometimes negative) of the arc
L = r_C*phi                              # length of the arc
if phi > pi:
    phi -= 2*pi
    L = -r_C*phi
elif phi < -pi:
    phi += 2*pi
    L = -r_C*phi
t_1 = L/s + t_0                        # time at which the robot finishes the arc
omega = phi/(t_1 - t_0)                # angular velocity           
x_C = x_0 - r_C*sin(theta_0)           # center of rotation
y_C = y_0 + r_C*cos(theta_0)

def position(t):
    x = x_C + r_C*sin(omega*(t - t_0) + theta_0)
    y = y_C - r_C*cos(omega*(t - t_0) + theta_0)
    return x, y

def orientation(t):
    return omega*(t - t_0) + theta_0

#--------------------------------------------
# Just used for debugging
#--------------------------------------------
import turtle

screen = turtle.Screen()
screen.setup(600, 600)
screen.setworldcoordinates(0, 0, 600, 600)

turtle.hideturtle()
turtle.shape("turtle")
turtle.penup()
turtle.goto(x_1, y_1)
turtle.setheading(degrees(orientation(t_1)))
turtle.stamp()
turtle.goto(x_0, y_0)
turtle.color("red")
turtle.showturtle()
turtle.pendown()
for t in range(t_0, int(t_1)+1):
    turtle.goto(*position(t))
    turtle.setheading(degrees(orientation(t)))

我不确定你的代码在哪一点上失败了，但我希望这对你有用。如果您打算在代码中多次使用此代码段，请考虑将其封装在一个函数中，该函数将给定值作为参数并返回position函数（如果您还需要rotation函数）。

绘制两点之间的弧形路径

1 个答案: