I am trying to create an algorithm to find the fastest path a robot can take between 2 points in terms of time. The robot I would be using would be powered by a drive wheel on each side and have limited acceleration and velocity. I was also hoping to build in some obstacle avoidance so the robot is able to path around obstacles. I am familiar with pathfinding algorithms such as a* that find the fastest path in terms of distance between two points but this algorithm does not always find the fastest path for a robot with bounded acceleration and velocity.
I've been thinking about this for a couple days and honestly I'm not really sure where to start or where to find resources on the topic so any help is appreciated.
Example:
Lets say our robot is 10 units wide and each wheel has a maximum speed of 100 units/second and a maximum acceleration of 10 units/second/second.
At point A (x = 0, y = 0) the velocities of the robots wheels are 50 and 30 for the left and right wheels respectively and the robot is at a 30 degree angle relative to the x-axis. At point B (x = 1000, y = -600) we want the robot to be stationary and have a -75 degree angle relative to the x-axis
What is the most time efficient path for the robot to take from point A to point B given it's starting and ending velocities and headings while also avoiding the obstacles?
Related
I want to generate a couple million 3d points. Every point should have a minimum distance to the nearest neighbor. In the center there should be a ball where no points are generated.
I prototyped in Matlab in 2d and got promising points in the picture below with DistMesh(http://persson.berkeley.edu/distmesh/).
fd = #(p) max( sqrt(sum(p.^2,2)) -100, -(sqrt(sum(p.^2,2))-10) );
fh = #(p) (normcdf((sqrt(sum(p.^2,2))), 0, sqrt(20^2+ 20^2))-0.5)*2;
[p,t] = distmesh( fd, fh, 0.1, [-100,-100;100,100], [],[], 25);
I got 366035 Points in 40sec
Then I tried it in a 3d space but with, 40000 Points after 8 minutes I ran out of RAM (16 GB).
I had something in mind, but don't know what to google. I once read an article where every point had weights acting like springs to other points. Then they simulate all these springs to find a balance. In my case, every point would be a "spring" to the other points with the minimum distance and one "spring" to the center with normal distribution.
Other implementations I tried were an octatree/kd trees and other structures that limit the possible neighbor.
Are there any better implementations or algorithms. Or did I use Matlab wrong, this was the first time I ever used Matlab for prototyping.
I don't even know if it's mathematically feasible, but let's say I have a tower at (Tx, Ty) shooting at a monster located at (Mx(t),My(t)).
The thing is, the path followed by the monster is jagged and swirly, meaning that predictive aiming from a distance based on velocity/direction at -that exact time- would be useless. The monster would've changed directions two times over by the time the bullet reached its target.
To solve this, I have a function to fast forward the monster for (t) frames and get its position as (Mx(t), My(t)) assuming its velocity remains constant. I can get the monster's position at t = 0 (current position) or t = 99999, or anything in between. Think of it as a lookup table.
The hard part is having the turret predictably aim at a position derived from that function.
I have to know t beforehand to know what to put into (Mx(t), My(t)). I have to know (Mx(t), My(t)) to know the distance and calculate the t from that. Is this even possible? Are there alternatives?
Any kind of pseudocode is welcome.
It is, if I understand your problem correctly, basically the transformation for euclidian to polar coordinate system
If you have the relative position x=Mx(t)-Tx and y=My(t)-Ty which is
distance= sqrt(x^2+y^2)
phi = arctan(y/x)
this assumes infinite speed of the shoot. However you can first calculate the approximate time for the projectile using the current position. Calculate the flying time based on the estimated distance and iterate this process.
If convergence speed is to slow (not much difference in projectile/monster speed) You can also use the idea of Divide and conquer.
You calculate the maximum distance the monster runs if it maximises distance. Calculate the distance for now, the max distance and the middle. Then you can decide in which half the monster is and calculate again distance in the middle.
I think everything else would require more information on how you plan to calculate Mx(t) and My(t)
You want the atan2 function.
If the tower is at tx,ty and the monster's predicted position is mx, my then the angle of the tower's gun has to be:
angle = atan2(my - ty, mx - tx)
This should be available in most languages.
Distance is, of course:
square-root((my - ty)^2, (mx - tx)^2)
where ^2 is "squared".
I'm building an application based around finding a "convenient meeting point" given a set of locations.
Currently I'm defining "convenient" as "minimising the total travel distance". This is a different problem from finding the centroid as illustrated by the following example (using cartesian coordinates rather than latitude and longitude for convenience):
A is at (0,0)
B is at (0,0)
C is at (0,12)
The location of minimum total travel for these points is at (0,0) with total travel distance of 12; the centroid is at (0,4) with total travel distance of 16 (4 + 4 + 8).
If the location were confined to being at one of the points, the problem appears to become simpler, but this isn't a constraint I intend to have (unlike, for example, this otherwise similar question).
What I can't seem to do is come up with any sort of algorithm to solve this - suggestions welcomed please!
Here is a solution that finds the geographical midpoint and then iteratively explores nearby positions to adjust that towards the minimum total distance point.
http://www.geomidpoint.com/calculation.html
This question is also quite similar to
Minimum Sum of All Travel Times
Here is a wikipedia article on the general problem you're trying to solve:
http://en.wikipedia.org/wiki/Geometric_median
In a way what you appear to be looking for is the center of mass of a triangle with equal weights at the vertices. That would point to barycentric coordinates.
When going beyond a triangle there are solutions for generalized barycentric coordinates and you could give priorities to persons by modifying the weight of the vertices. What that still would not account for is distances on a real map (can't just travel straight in any direction) but it may be a start?
One option is to define an objective (and gradient) function and use a generic optimization library, such as scipy.optimize. fmin_cg would be a good algorithm to try for your problem. Your objective would be the sum of distances as defined in the "Definition" section of the Geometric median Wikipedia page referenced by hatchet. The argument to your objective function is y.
I'm developing an algorithm that involves inserting intermediary points between GPS coordinates. To be precisely let's assume that I have these 2 coordinates:
25.60593,45.65151
25.60662,45.65164000000001
I want to add extra points between these coordinates (on the same "line"). Is it wise to consider a linear variation (in this case I consider the intermediate points as points on the segment defined by the 2 given coordinates).
This involves actual GPS data obtained from a computed route -> this would mean that the distance is less than 1000 meters between each consecutive shape point (on highway the distance between consecutive points can be quite big due to the fact that it is a straight line).
Is linear variation a good approximation, or are they any other methods? (given the fact that the distance between points is less than 1km)
Thanks,
Iulian
Given that the distance between the points is less than 1 km, AND *ASSUMING* that you're not playing very close to the North or South Pole, linear interpolation is probably good enough.
After linear interpolation is shown in live testing on real data NOT to be good enough, you can try spherical trig interpolation. Math involved.
If both of those fall short, then you will need very specialized expert assistance.
If the you have two points a and b, and the distance from a to b is less than 1km, then the maximimum distance between the midpoint (coordinates obtained by averaging) and the "middle point" (coordinates obtained by going halway along the shortest path between a and b) is tiny (1 micrometer) if a is on the equator, around 5cm at the arctic circle and 25cm at 85 north. If the distance between a and b was up to 10km, then the figures above should be multiplied by 100. These figures were calculated using the GPS ellipsoid, WGS84.
PS You have to be a bit careful averaging longitudes. The average of 179 and -179 is 180, not 0.
I'm looking for an algorithm that would move a point around an arbitrary closed polygon that is not self-crossing in N time. For example, move a point smoothly around a circle in 3 seconds.
1) Calculate the polygon's perimeter (or estimate it if exact time of circling
is not critical)
2) divide the perimieter by the time desired for circling
3) move point around polygon at this speed.
Edit following ire and curses' comment.
Maybe I read the question too literally, I fail to see the difficulty or the points raised by ire_and_curses.
The following describes more specifically the logic I imagine for step #3. A more exact description would require knowing details about the coordinate system, the structure used to describe the polygon, and indication abou the desired/allowed animation refreshing frequency.
The "travellng point" which goes around the polygon would start on any edge of the polygop (maybe on a vertex, as so to make the start and end point more obvious) and would stay on an edge at all time.
From this starting point, be it predetermined or randomly selected), the traveling point would move towards towards a vertex, at the calculated speed. Once there it would go towards the other vertex of the edge it just arrived to, and proceed till it returns to the starting point.
The equations for calculating the points on a given edge are the same that for tracing a polygon: simple trig or even pythagoras (*). The visual effect is based on refreshing the position of the traveling point at least 15 times or so per second. The refresh frequency (or rather its period) can be used to determine the distance of two consecutive points of the animation.
The only less trivial task is to detect the end of a given edge, i.e. when the traveling point needs to "turn" to follow the next edge. On these occasions, a fractional travel distance needs to be computed so that the next point in the animation is on the next edge. Special mention also for extremely short edges, as these may require the fractional distance logic to be repeated (or done differently).
Sorry for such a verbose explanation for a rather straight forward literally ;-) algorithm...
Correction: as pointed out by Jefromi in comment for other response, all that is needed with regard to the tracing is merely to decompose the x and y components of the motion. Although we do need Pythagoras for calculating the distance between each vertex for the perimeter calculation, and we do need to extrapolate because the number of animation steps on an edge is not [necessarily] a integer.
For the record, a circle is not a polygon--it's the limit of a regular polygon as the number of sides go to infinity, but it's not a polygon. What I'm giving you isn't going to work if you don't have defined points.
Assuming you have your polygon stored in some format like a list of adjacent vertices, do a O(n) check to calculate the perimeter by iterating through them and computing the distance between each point. Divide that by the time to get the velocity that you should travel.
Now, if you want to compute the path, iterate back through your vertices again and calculate from your current position where your actual position should be on the next timestep, whatever your refresh time step may be (if you need to move down a different edge, calculate how much time it would take to get to the end of your first edge, then continue on from there..). To travel along the edge, decompose your velocity vector into its components (because you know the slope of the edge from its two endpoints).
A little code might answer this with fewer words (though I'm probably too late for any votes here). Below is Python code that moves a point around a polygon at constant speed.
from turtle import *
import numpy as nx
import time
total_time = 10. # time in seconds
step_time = .02 # time for graphics to make each step
# define the polygone by the corner points
# repeat the start point for a closed polygon
p = nx.array([[0.,0.], [0.,200.], [50.,150.], [200.,200.], [200.,0.], [0.,0.]])
perim = sum([nx.sqrt(sum((p[i]-p[i+1])**2)) for i in range(len(p)-1)])
distance_per_step = (step_time/total_time)*perim
seg_start = p[0] # segment start point
goto(seg_start[0], seg_start[1]) # start the graphic at this point
for i in range(len(p)-1):
seg_end = p[i+1] # final point on the segment
seg_len = nx.sqrt(sum((seg_start-seg_end)**2))
n_steps_this_segment = int(seg_len/distance_per_step)
step = (seg_end-seg_start)/n_steps_this_segment # the vector step
#
last_point = seg_start
for i in range(n_steps_this_segment):
x = last_point + step
goto(x[0], x[1])
last_point = x
time.sleep(step_time)
seg_start = seg_end
Here I calculated the step size from the step_time (anticipating an graphics delay) but one could calculate the step size, from whatever was needed, for example, the desired speed.