I am trying to plot a curved path for a robot to follow using the following as a guide: http://rossum.sourceforge.net/papers/CalculationsForRobotics/CirclePath.htm
The code i have does not create a path that ends at the destination. I am expecting the path to curve 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, Rotation Point: Purple
UPDATE:
I have 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 is very interesting and explains the ideas behind the formulas pretty well, althought it lacks the formulas for when the starting position is not at the origin and when the starting angle is not 0.
It took a little while to come up with these formulas, but now it works for every case I could think of. To be able to use the formulas given in the linked article, I used the names of the variables given there. Notice that I also used the notation with t_0 as the starting time, which you just ignored. You can easily remove any instance of t_0 or set t_0 = 0.
The last part of the following code is used for testing and creates a little red turtle that traces the path of the computed arc in the specified direction. The black turtle indicates the goal position. Both turtles are close to each other at the end of the animation, but they are not directly above each other, because I am only iterating over integers and it is possible that t_1 is not an integer.
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 am not sure at which point your code failed, but I hope this works for you. If you intend to use this snippet multiple times in you code consider encapsulating it in a function that takes in the given values as parameters and returns the position function (and if you like rotation function as well).
Related
I would like to check if two polygons built from coordinates touch or overlap. The following example should touch (they came from a simulation), however, the code doesn't detect they touch, hence, crash. Any clue?
import geopy
import geopy.distance
import math
from shapely import geometry
def bearing_calc(a_lat, a_lon, b_lat, b_lon): # a previous position b current position
# print(a)
# a.type
rlat1 = math.radians(a_lat)
# print(a_lat)
rlon1 = math.radians(a_lon)
rlat2 = math.radians(b_lat)
rlon2 = math.radians(b_lon)
dlon = math.radians(b_lon - a_lon)
b = math.atan2(math.sin(dlon) * math.cos(rlat2),
math.cos(rlat1) * math.sin(rlat2) - math.sin(rlat1) * math.cos(rlat2) * math.cos(
dlon)) # bearing calc
bd = math.degrees(b)
br, bn = divmod(bd + 360, 360) # the bearing remainder and final bearing
return bn
# To get a rotated rectangle at a bearing, you need to get the points of the the recatangle at that bearing
def get_rotated_points(coordinates, bearing, width, length):
start = geopy.Point(coordinates)
width = width / 1000
length = length / 1000
rectlength = geopy.distance.distance(kilometers=length)
rectwidth = geopy.distance.distance(kilometers=width)
halfwidth = geopy.distance.distance(kilometers=width / 2)
halflength = geopy.distance.distance(kilometers=length / 2)
pointAB = halflength.destination(point=start, bearing=bearing)
pointA = halfwidth.destination(point=pointAB, bearing=0 - bearing)
pointB = rectwidth.destination(point=pointA, bearing=180 - bearing)
pointC = rectlength.destination(point=pointB, bearing=bearing - 180)
pointD = rectwidth.destination(point=pointC, bearing=0 - bearing)
points = []
for point in [pointA, pointB, pointC, pointD]:
coords = (point.latitude, point.longitude)
points.append(coords)
return points
v1_id = 176
v1_coord_5 = [41.39129840757358, 2.1658667401858738]
v1_coord_4 = [41.391335226146495, 2.1658363842310404]
v1_length = 5 #meters
v1_width = 1.8 #meters
v1_bearing = bearing_calc(v1_coord_4[0], v1_coord_4[1],
v1_coord_5[0], v1_coord_5[1])
v1_points = get_rotated_points(tuple(v1_coord_5), v1_bearing,
v1_width, v1_length)
polygon1 = geometry.Polygon(v1_points)
v2_id = 210
v2_coord_5 = [41.39134056977012, 2.1658847515529804]
v2_coord_4 = [41.39127485461452, 2.165851515431892]
v2_length = 5 #meters
v2_width = 1.8 #meters
v2_bearing = bearing_calc(v2_coord_4[0], v2_coord_4[1],
v2_coord_5[0], v2_coord_5[1])
v2_points = get_rotated_points(tuple(v2_coord_5), v2_bearing,
v2_width, v2_length)
polygon2 = geometry.Polygon(v2_points)
if polygon1.intersection(polygon2).area > 0.0:
print("COLISION")
else:
print("NO COLISION")
I've also tried the function touches
polygon1.touches(polygon2)
Unfortunately, also, false. I don't know if there is a function to check if the two borders touch even in one points, minimally.
If I plot the two polygons, they do not collide, as shown above, however, they should. Is it because the code is not considering the earth surface curvature? How can I adapt the code to consider that factor.
I'm currently working on a section of a program that moves the mouse in an arc.
I'm given three points that define the arc: a starting point p1, a intermediate point on the arc p2 , and the end point p3. I'm also given length of the arc. If length is greater than the actual length of the arc subtended by p1 and p3, then p3 will not be the end point of the arc, but the mouse will continue moving in a circle until it has traveled distance length.
I have worked out the center of the circle (x, y), its radius r, and angle sweeped.
To move the mouse, am hoping to divide angle into smaller intervals each with angle dAngle and moving the mouse between its current position and the new position after sweeping dAngle. What I have in mind is in the pseudocode below:
for i in range(intervals):
x = center.x + r * cos(i * dAngle)
y = center.y + r * sin(i * dAngle)
Move mouse to (x, y)
Sleep 1
However, I've encountered some problems while trying to parametrically find the new point on the arc.
My mouse does not start at p1, but at what I assume is at the point where the line from the mouse to the center and the horizontal line subtends 0 degees, as I haven't factored into the parameters the initial angle. How do I find the initial angle of the mouse?
How do I determine whether to rotate clockwise or anticlockwise, i.e. whether x = center.x + r * cos(i * dAngle) or x = center.x - r * cos(i * dAngle)
If there is a more efficient way of moving in an arc please suggest it.
You can calculate starting angle as
a1 = math.atan2(p1.y-center.y, p1.x-center.x)
then use it in
x = center.x + r * cos(a1 + i * dAngle)
y = center.y + r * sin(a1 + i * dAngle)
About direction - perhaps you can determine direction when arc center is calculated. If no, and arc sweep angle is less than Pi (180 degrees), just find sign of expression
sg = math.sign((p1.x-center.x) * (p3.y-center.y) - (p1.y-center.y) * (p3.x-center.x))
and use it with dAngle
x = center.x + r * cos(a1 + i * sg * dAngle)
similar for y
P.S. note that minus in x = center.x - r * cos(i * dAngle) is wrong way to change direction
I'm trying to create a lookAt function in 2 dimensions using Python, so here's my code right now.
from math import *
def lookAt(segment, originPoint):
segmentCenterPoint = getSegmentCenter(segment)
for i in range(2):
vtx = getVertex(segment, i)
x, y = getVertexCoord(vtx)
# Calculate the rotation angle already applied on the polygon
offsetAngle = atan2(y - segmentCenterPoint.y, x - segmentCenterPoint.x)
# Calculate the rotation angle to orient the polygon to an origin point
orientAngle = atan2(segmentCenterPoint.y - originPoint.y, segmentCenterPoint.x - originPoint.x)
# Get the final angle
finalAngle = orientAngle - (pi / 2)
if offsetAngle >= pi:
offsetAngle -= pi
elif offsetAngle < 0:
offsetAngle += pi
finalAngle += offsetAngle
# Temporary move the point to have its rotation pivot to (0,0)
tempX = x - segmentCenterPoint.x
tempY = y - segmentCenterPoint.y
# Calculate coords of the point with the rotation applied
s = sin(finalAngle)
c = cos(finalAngle)
newX = tempX * c - tempY * s
newY = tempX * s + tempY * c
# Move the point to the initial pivot
x = newX + segmentCenterPoint.x
y = newY + segmentCenterPoint.y
# Apply new coords to the vertex
setVertexCoord(vtx, x, y)
I tried some examples manually and worked well, but when I tried to apply the function on thousands of segments, it seems some segment are not well oriented.
I probably missed something but i don't know what it is. Also, maybe there's a faster way to calculate it ?
Thank you for your help.
EDIT
Here is a visualization to understand better the goal of the lookAt.
The goal is to find A' and B' coordinates, assuming we already know O, A and B ones. ([AB] is the segment we need to orient perpendicularly to the point O)
To find positions of A' and B', you don't need rotate points (and dealt with angles at all).
Find vector OC = segmentCenterPoint - originPoint
Make normalized (unit) vector oc = OC / Length(OC)
Make perpendicular vector P = (-oc.Y, oc.X)
Find CB length lCB
Find A' = C + lCB * P
B' = C - lCB * P
I am new to Python, and currently having a rough time with turtle graphics. This is what I am trying to solve
On Turtellini (the planet where Python turtles live) the
transportation system propels turtles with a giant slingshot. A
particular turtle's original location (x0, y0) is (-180, -100). He is
then shot upward at an initial vertical velocity (vy) of 88 units per
second and a horizontal velocity (vx) of 20 units per second to the
right. He travels for 16 seconds. The acceleration due to gravity (g)
is 11 units per second squared. The the location of the turtle at a
given second (t) is calculated as follows: x = x0 + vx * t and y = y0
+ vy * t - g/2 * t2 . This program is to show how a turtle travels over this period of time.
The output should be like this:
Here is what I should do;
set up the constants (vertical velocity, horizontal velocity,
gravity) and variables (x and y coordinates) set up the turtle by
giving him a proper shape, putting his tail up, moving him to the
initial position, putting his tail down make a loop that repeats for
seconds 1 through 16 inclusive. in each iteration of the loop display
the the values of the x and y variables (in the shell window), move
the turtle to those coordinates, have the turtle stamp his shape,
calculate the new values for the x and y variables after the loop
terminates, move the turtle to the last calculated coordinates,
change his color, and stamp his shape, then wait for a mouse click
My code so far:
import turtle
def main():
wn = turtle.Screen()
turtellini = turtle.Turtle()
t = int(input("Blab blab blab: "))
x0 = -180
y0 = -100
vx = 20
vy = 88
g = 11
x = (float(x0 + vx * t))
y = (float(y0 + vy * t - g / 2 * t**2))
turtellini.color("black")
turtellini.shape("turtle")
turtellini.up()
turtellini.goto(-180,-100)
turtellini.down()
for i in range(1,16,1):
turtellini.stamp()
turtellini.forward(i)
turtellini.right(i)
print(x)
print(y)
if __name__ == "__main__":
main()
I know I am doing bad; but can anyone help me to solve this problem?
You seem to have most of the parts and pieces. The biggest issue I see is you didn't put your x,y calculation in the loop. The loop iteration variable i is really t in your motion equations. Each time you calculate a new x,y you simply move the turtle to that position:
import turtle
from math import pi, atan
x0, y0 = -180, -100 # initial location
vx, vy = 20.0, 88.0 # initial velocity in units per second
travel_time = 16 # seconds
g = 11.0 # acceleration due to gravity in units per second squared
turtellini = turtle.Turtle(shape='turtle', visible=False)
turtellini.penup()
turtellini.radians() # to make turtle compatible with math.atan()
turtellini.setheading(pi / 2) # straight up
turtellini.goto(x0, y0)
turtellini.pendown()
turtellini.showturtle()
turtellini.stamp()
for t in range(1, travel_time + 1):
x = x0 + vx * t
y = y0 + vy * t - g / 2 * t**2
turtellini.goto(x, y)
print(x, y)
angle = atan((vy * t - g * t**2) / (vx * t)) # a guess!
turtellini.setheading(angle)
turtellini.stamp()
turtle.exitonclick()
Unlike the gold standard image, I assumed the turtle was aerodynamic like a bullet and travelled head first through the flight. I don't know, and couldn't quickly find, the formula for the flight angle of a projectile so I guessed from the existing formulas:
I have generated a random point named y0=(a,b) in xy-plane , How can I generate another random point (x,y) 10 steps apart from y0?
note: by 10 steps apart from the firt point I don't mean the Euclidean distance. I mean the number of steps on lattice between the two point (a,b) and (x,y) which is given by |x-a|+|y-b|=10
My attempt(sometimes gives wrong result).
import random
y0=(random.randint(0,50),random.randint(0,50))# here I generated the first point.
y=random.randint(0,50)
# I used the formula |x-a|+|y-b|=10.
x=(10 -abs(y-y0[1]))+y0[0] or x=-(10 -abs(y-y0[1]))+y0[0]
x0=(x,y)
Let's say you have a point (x, y)
create another random point anywhere on the plane: (x1, y2) = (random(), random())
take the vector from your point to the new point: (vx, vy) = (x1-x, y1-y)
get the length l of the vector: l = sqrt(vx * vx + vy * vy)
use l to normalise the vector (so it has a length of 1): (vx, vy) = (vx / l, vy / l)
make the vector 10 steps long: (vx, vy) = (vx * 10, vy * 10)
add it to your original point to get to the desired point: (x1, y2) = (x + vx, y + vy)
voilá :)
from random import random
from math import sqrt
# Deviation
dev = 50
# Required distance between points
l = 10
if __name__ == '__main__':
# First random point
x0, y0 = dev*random(), dev*random()
# Second point
x1 = dev*random()
y1 = y0 + sqrt(l**2 - (x1 - x0)**2)
# Output
print "First point (%s, %s)" % (x0, y0)
print "Second point (%s, %s)" % (x1, y1)
print "Distance: %s" % (sqrt((x1 - x0)**2 + (y1 - y0)**2))
Let's say that your new point (x, y) is on a cercle of radius 10 and center (x0, y0). The random component is the angle.
import math as m
# radius of the circle
r = 10
# create random angle and compute coordinates of the new point
theta = 2*m.pi*random.random()
x = x0 + r*m.cos(theta)
y = y0 + r*m.sin(theta)
# test if the point created is in the domain [[0,50], [0, 50]] (see comments of PM2Ring)
while not ( 0<=x<=50 and 0<=y<=50 ) :
# update theta: add pi/2 until the new point is in the domain (see HumanCatfood's comment)
theta += 0.5*m.pi
x = x0 + r*m.cos(theta)
y = y0 + r*m.sin(theta)
So, you got the formula d=d1+d2=|x-x0|+|y-y0| , for d=10
Let's examine what's going on with this formula:
Let's say we generate a random point P at (0,0)
Let's say we generate y=random.randint(0,50) and let's imagine the value is 50.
What does this mean?
d1=|x-p[0]|=50 and your original formula is d=d1+d2=|x-x0|+|y-y0|, so
that means d2=|y-y0|=10-50 and d2=|y-y0|=-40. Is this possible? Absolutely not! An absolute value |y-y0| will always be positive, that's why your formula won't work for certain random points, you need to make sure (d-d1)>0, otherwise your equation won't have solution.
If you wanted to consider Euclidean distance you just need to generate random points in a circle where your original point will be the center, something like this will do:
import random
import math
def random_point(p, r=10):
theta = 2 * math.pi * random.random()
return (p[0] + r * math.cos(theta), p[1] + r * math.sin(theta))
If you draw a few random points you'll see more and more how the circle shape is created, let's try with N=10, N=50, N=1000:
Now, it seems you need the generated circle to be constrained at certain area region. One possible choice (not the most optimal though) would be generating random points till they meet those constraints, something like this would do:
def random_constrained_point(p, r=10, x_limit=50, y_limit=50):
i = 0
MAX_ITERATIONS = 100
while True:
x0, y0 = random_point(p, r)
if (0 <= x0 <= x_limit and 0 <= y0 <= y_limit):
return (x0, y0)
if i == MAX_ITERATIONS:
return p
i += 1
Once you got this, it's interesting to check what shape is created when you increase more and more the circle radius (10,20,50):
As you can see, your generated random constrained points will form a well_defined subarc.
this code generate a random point xy-plane named y0 then generate another point x0 10 steps apart from y0 in taxi distance .
------- begining of the code--------
import random
y0=(random.randint(0,50),random.randint(0,50))
while True:
y=random.randint(0,50)
x=(10 -abs(y-y0[1]))+y0[0]
if (abs(x-y0[0])+abs(y-y0[1]))==10:
x0=(x,y)
break
abs(x)+abs(y)=10 defines a square, so all you need to do is pick a random value along the perimeter of the square (40 units long), and map that random distance back to your x,y coordinate pair.
Something like (untested):
x = random.randint(-10,9)
y = 10 - abs(x)
if (random.randint(0,1) == 0):
x = -x
y = -y
x = x + y0[0]
y = y + y0[1]
x0=(x,y)
Clipping the x range that way ensures that all points are picked uniformly. Otherwise you can end up with (-10,0) and (10,0) having twice the chance of being picked compared to any other coordinate.