Python – Draw an arc path between two points

I’m trying to draw a curved path for the bot, using the following as a guide: http://rossum.sourceforge.net/papers/CalculationsForRobotics/CirclePath.htm

My code doesn’t create a path that ends with a destination. I expect the path to bend left or right, depending on the quadrant the destination is in (+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)

Start: Blue, End: Red, Spin Point: Purple

Update:
I added code to allow positive and negative x/y values. I have updated the image.

To answer your question, I first read through the article you linked. I think it’s very interesting and explains the idea behind the formula very well, although it lacks the formula when the starting position is not at the origin and the starting angle is not 0.

It took some time to come up with these formulas, but now it works in every situation I can think of. To be able to use the formula given in the linked article, I used the names of the variables given there. Note that I also used a symbol with an t_0 as the start time, which you just ignored. You can easily delete any instance of t_0 or set t_0 = 0.

The last part of the following code tests and creates a baby loggerhead turtle that traces the path of a calculated arc in a specified direction. The black turtle indicates the target location. At the end of the animation, the two turtles are close to each other, but they are not directly above each other because I am just iterating over integers and t_1 may not be integers.

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)))

I’m not sure at what point your code failed, but I hope this works for you. If you plan to use this snippet multiple times in your code, consider wrapping it in a function that takes a given value as an argument and returns a position function (if you like the rotation function as well).