绘制两点之间的圆弧路径
绘制两点之间的圆弧路径
提问于 2018-02-04 08:59:39
我正在尝试绘制一条供机器人遵循的曲线路径，使用以下内容作为指南：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正值和负值的代码。我已经更新了图像。
回答 1
Stack Overflow用户
回答已采纳
发布于 2018-02-07 05:15:39
为了回答你的问题，我首先通读了你链接的文章。我认为它非常有趣，并且很好地解释了公式背后的思想，尽管它缺乏起始位置不在原点以及起始角度不为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函数)。
页面原文内容由Stack Overflow提供。腾讯云小微IT领域专用引擎提供翻译支持
原文链接：
https://stackoverflow.com/questions/48603681
复制相关文章
相似问题
腾讯云开发者
