The problem I have been attempting to solve is the path finding from a given position to a given goal for a dubins car (no backwards motion, constant velocity) with obstacles. I've attempted to implement a gridless A* algorithm with some simple obstacle avoidance. I expected the generated path to head straight towards the goal, and make minor adjustments to drive around the obstacles that it found. However, as soon as obstacles are introduced to the map, the path instead seems to get stuck at local minimum points of the algorithm's cost function.
The cost function I've implemented is the following:
f(x) = c(x) + g(x)
where c(x) is total travel cost, namely the cumulative cost of moving from node i-1 to i.
Also, g(x) is the cost of the optimal path from the current node to the end goal, which becomes a straight line as it ignores obstacles.
The cost is used as a priority value in a min heap, where each iteration pops the minimum node and generates children nodes. As the children are generated, it is controlled that they are not out of bounds, have not already been visited and are not inside an obstacle. If these controls return false, then the child is added to the heap.
I've attempted introducing a weighting factor k * g(x) to the path cost, hoping that this would "incentivize" the algorithm to move towards the goal instead of getting stuck at a point. However, this merely shifted the minimum point to another location, but still resulted in getting stuck.
I will include my code implementation of the A* algorithm below:
# Description: Pathfinding algorithm, iteratively generates new neighbouring
# nodes and selects the cheapest of these through utilizing a min heap.
# In: Car class object, a node as starting point.
# Out: The finishing node, with attached parent pointers.
def Astar(car, current):
minHeap = [] #initialize heap as list
h.heappush(minHeap, current) #push initial node onto heap
heapCount = 1 #add upon pushes to heap, subtract upon pop
# Iterate through nodes in priority queue
while not ((goal(car, current)) or heapCount == 0):
current = h.heappop(minHeap)
heapCount -= 1
for phi in [-m.pi/4, 0, m.pi/4]: #Full turns or straight are optimal, according to Pontryagins maximum principle
#calculate new values for each phi (steering angle)
xn, yn, thetan = step(car, current.x, current.y, current.theta, phi)
#control feasibility of position
if validCheck(car, xn, yn, current):
#calculate costs for these directives
costC = current.travelled + m.hypot(current.x - xn, current.y - yn) #cost of travel from start position
costG = m.hypot(car.xt - xn, car.yt - yn) #current optimal distance to goal
totalCost = costC + costG
#renew time stamp
newTime = current.time + 0.01
#create child from new data
child = Node(xn, yn, thetan, phi, totalCost, costC, newTime, current)
#push child onto heap
h.heappush(minHeap, child)
heapCount += 1
return current
Note that car is a class which includes certain attributes:
x0 : float: initial x-position [m]
y0 : float: initial y-position [m]
xt : float: target x-position [m]
yt : float: target y-position [m]
xlb : float: minimum x-position [m]
xub : float: maximum x-position [m]
ylb : float: minimum y-position [m]
yub : float: maximum y-position [m]
obs : list: list of tuples for each obstacle obs[i]
It also includes a method step which can generate a new heading angle and position when given a steering angle and previous heading and position.
Any advice or help regarding this problem, why it is occurring and what I can do to improve the path finding would very much be appreciated.
I don't have a solution ready, but an explanation what's going on and and maybe a hint what you can do.
Analysis
The A* algorithm is for graph searching and, given a decent cost function, can greatly reduce the search space when compared with uninformed strategies like BFS. But still, the size of the problem graph matters.
In your code, I see a time increment of 0.01, and I read that as a hint that you are doing very small steps from parent to child nodes. That surely makes sense, to most closely approximating a smooth, non-quantized movement. But at the same time, it results in a huge growth of your problem graph.
Without obstacles, A* will still handle that huge graph gracefully. It will postpone all deviations from the straight line, as their cost will be higher than the node on the straight line. Your heap will grow (have some debug output show you its size...), but most nodes will never be explored further.
With obstacles, the game changes drastically. Let's say, there's an obstacle so that the resulting best path is 1.00 units longer than the straight line. Then A* will explore all nonsense paths, starting from somewhere on the line from start to obstacle, arbitrarily turning left or right until these paths reach an additional length of 1.00. There will be lots of these useless paths, and A* gets stuck in exploring nonsense.
Suggestion
I'd have the A* operate on a higher level.
I guess your obstacles are polygons. So the resulting total path will either ignore an obstacle or touch it at one of its corners. The elements between the touching points will start at a touching point with some heading direction, consist of an initial full-turn part, then a straight part, and then a final full-turn part, and then arrive at the next touching point with some (different) heading (to be honest, I'm not absolutely sure that this turn-straight-turn pattern will really cover all possible situations). Given start and end points and the desired end heading of such an element, you can compute the parts using some geometry, and by the way, check for collisions.
You can't know in advance the optimum heading when passing some touching point, so you'd have to check all possible headings.
I'd take these elements as the steps to be explored by A*.
So, that's how I'd apply A* to your task:
To compute the children of a node, for all other polygon corners and all headings at that corner, compute the element from the parent corner to the other corner, resulting in the given heading there. Check if such an element is geometrically possible and does not collide with some obstacle.
As the cost function, accumulate the length travelled so far, and then add the shortest obstacle-ignoring path to the target. This can either be the straight Pythagorean distance, or a more elaborate distance, taking into account the neccessary initial turn from the current heading to facing the target.
Related
I'm currently trying to develop a to-scale model of the universe using pygame. At the moment, when I'm calculating the x, y positions of the planets w.r.t. the sun, the planets are slowly falling towards the sun, despite only using equations for position based on the distance and angle of the planet (no force).
Here is the code snippet for calculating distance from a given star currently:
def d_obj(self, reference):
x_diff_sq = pow(self.x - reference.pos[0], 2)
y_diff_sq = pow(self.y - reference.pos[1], 2)
return pow(x_diff_sq + y_diff_sq, 0.5)
And then I pass what this function returns into the next function for calculating the position
def move(self, d):
self.theta += self.d_theta
self.x = int(d * math.cos(self.theta)) + total_d/2
self.y = int(d * math.sin(self.theta)) + total_d/2
total_d/2 is a co-ordinate offset and self.d_theta is the rotational period for the given planet.
Each planet has its initial position hard coded and I'm using this to calculate the difference between initial distance and current distance for all of the planets, every tick it is apparent that the planet moves about 1km towards the sun. Is there any way I can attempt to offset this?
I understand that in the scale of things where I'm drawing things in terms of millions of km, I'm just curious what part of these equations is causing the error. I've tried using the '**' operator over pow and after some research online found that pow is better used for powers involving floats.
Should also mention that all calculations are in kilometers, then before drawing, the planets radius and x, y are mapped to the screen from a set distance that is currently around 4 AU.
You're trying to move your planets in circles, right?
In your code, you
Use x and y to calculate distance,
Use delta_theta to calculate new theta,
Use new theta and distance to calculate new x and y.
You don't have to do all that. Instead, you can keep a hardcoded distance and just
Use delta_theta to calculate new theta,
Use new theta and (known) distance to calculate x and y for drawing.
Then your distance will not drift at all.
Side note: If you're planning to keep the planets moving for long times, make sure you keep your theta between 0 and 2*pi, or rounding errors will start kicking in and your theta accuracy will deteriorate.
You're thinking this will make adding moons and asteroids difficult.
Not really!
You can do the same for moons, by noting which planet they belong to, the distance to that planet, delta_theta and initial theta (based on their parent planet).
If you want to start doing ellipses instead of circles, you can change your calculations (use convenient constant orbital elements instead of distance and delta_theta, which will not be constant anymore) to apply Kepler's laws.
You can also add asteroids later. You can keep the Cartesian positions and velocities of the asteroids, and calculate their motion separately, after calculating the motion of all the "trivially" moving objects.
I'm trying to find the closest vertex on a mesh but within a radius. This is to avoid having to loop through all the vertices as that's time consuming. Example, I have 2 shirts with different vertex count and I'm trying to find the closest vertex of vertex1 that's on mesh2's right sleeve on mesh1's right sleeve. I don't want to loop through verts beyond the sleeve as logically no vertices will be close enough. I understand that there is an assumption that there could be other vertices closer, but for the usage I'm looking for, I don't foresee that being an issue.
I have the code to loop through vertices and get the closest point, but if the mesh has a high vert count, it takes a long time, even if it's using the API.
Is there a function in maya that lets you limit vertices based of a radius? Or any tips on how I can write a function that could do that?
You can use the node nearestPointOnMesh. In maya API, you can look for MFnMesh::closestIntersection class that can raycast (Querying of a point is within a mesh maya python api)
vtx = 'shirt1.vtx[0]'
pos = cmds.pointPosition(vtx)
m = 'shirts2'
objShape = cmds.listRelatives(m, ni=True, type='mesh')[0]
node = cmds.createNode('nearestPointOnMesh')
cmds.connectAttr(objShape + ".worldMesh", node + ".inMesh")
cmds.setAttr(node + ".inPosition", type = 'double3', *pos)
target_pos = cmds.getAttr(node + '.position')[0]
face = cmds.getAttr(node + ".nearestFaceIndex")
Question Explanation:
I have a fairly large file(up to 1 million lines) containing data from a mining machine. The data includes the X and Y coordinates of the machine. The machine should only be able to mine to the left (-X direction) or to the right (+X direction) and this occurs in a 'lane'. It should not be able to mine the same area again, but it can change Y-coordinates at any time(i.e, it can change lanes at any time) and skip certain areas. I want to determine in which direction the machine is moving at a given time. For example I want to see where the machine is moving to the left or to the right. Although this sounds simple enough, I am having a bit of trouble.
My attempt:
I am currently finding the sum of 100 values of the difference array of the X-coordinates (the difference array gives the distance the machine has moved). Ideally, if the X-coordinates are decreasing in a 100 point interval, then it should be moving to the left, and if the X-coordinates were increasing in that interval, it should be moving to the right. So if the sum was negative, it will append "0" to an array 100 times. I chose 100 because I thought it would be fairly accurate considering the high number of samples.
# miner_x = array of the x-coordinates of the machine
# miner_x_diff = numpy.diff(miner_x)
# 0 = Negative X direction
# 1 = Positive X direction
diffSample= [sum(miner_x_diff[i:i+100]) for i in range(0, len(miner_x_diff), 100)]
Direction = np.array([])
for value in diffSample:
if value < 0:
for _ in itertools.repeat(None, 100):
Direction = np.append(Direction,0)
else:
for _ in itertools.repeat(None, 100):
Direction = np.append(Direction,1)
# correct size
if Direction.size > miner_x.size:
Direction = Direction[0:Direction.size-(Direction.size-miner_x.size)]
The problem I am having is at the point it changes direction from left to right or vice versa, and this mostly occurs during a lane change. This if course expected, since the sum around that area could be close to 0. But I am not sure how to go about accounting for this change using my current method. Is there a more accurate way to determine the direction of the machine?
An example of a plot of the machines position using one particular data set is shown below (code omitted because it is unnecessary). The yellow points are positions where I had incorrect results occur .In this example there shouldn't be any yellow points. I have not shown the direction color in this example but you can of course see it if I say the start position is at the bottom right.
I will appreciate any help/advice/hints :)
Edit:
Here is a picture of what I want (mind the Paint skills). Note that I don't want to 'see' a straight line, just to know that it is moving in those directions at those points. Green is the start point.
Start by using a smaller sample window. How much time is represented by 100 sample points? The interval should be small enough that you can simply look at the displacement between the initial and final position and have a direction for that time segment. (You do realize that summing the difference of consecutive pairs of numbers is the same as subtracting the first from the last number, don't you?) If you catch the machine at a point where it's turning around, you may get indeterminate data but the samples before and after will show a straight vector.
I'm coding in python. I've created a spiral with my current for loop below. I want to create tiny spiral off-shoots around every 10 degrees. The problem is I don't know how to do that. I'm trying to do something like this:
My question is how would I create the off-shoots? Is this even possible or not?
Any suggestions are greatly appreciated.
import turtle
import math
me = turtle.Turtle()
def miniSpiral():
for i in range(0,360):
x,y = me.position()
me.left(1)
if x%10==0:
x2,y2 = me.forward(((5*i)+5)*math.pi/360)
else:
x2,y2= me.forward(5*math.pi/360)
me.goto((x+x2),(y+y2))
for x2 in range(0,720):
me.left(1)
if x2%10==0:
me.forward(((10*x2)+10)*math.pi/360)
#miniSpiral()
me.forward(10*math.pi/360)
In general, the easiest way to draw a fractal programmatically is to use recursion. Start with the code to draw one "segment" of your fractal. In the image you linked to, that would be one 90-degree piece of the spiral (since that's the distance between the branchings).
Once you have code that can draw one segment, you add a recursive call to it. Give it some parameter (e.g. the initial size), and have the recursive call reduce the value passed on to the next call. Add a base case where a call with that parameter set too small gets skipped (e.g. if size < 1: return) so that the recursion doesn't go on forever.
Now you can add branching. Instead of just one recursive call, make two. You'll need to add some extra logic to move the position of the turtle in between the calls (so the second one starts at roughly the same spot as the first), but that shouldn't be too hard. To make the two branches distinct, vary their initial position or angle, or give them different parameters. In your example image, the "extra" branches all start going the oposite direction from the "main" branch, and they start smaller.
Here's a pseudo-code implementation of the spiral you want (I'm not adding actual turtle code because you seem to be using a different turtle module than the one I have from the standard library):
def spiral(size):
if size < 1: return # base case
draw_segment(size) # this draws a 90 degree piece of the spiral
position = getposition() # save state so we can back up after the first recursive call
angle = getangle()
spiral(size - 1) # main branch of the recursion
penup() # restore state (mostly)
setposition(position)
pendown()
setangle(angle + 180) # we want to start facing the other direction for the second branch
spiral(size - 2) # extra branch of the recursion
You can play around with the details (like how you modify the size for the recursive calls) to suit your tastes or the fractal design you're looking for. For instance, you could multiply the size by some factor (e.g. size * 0.75) rather than subtracting a fixed amount.
Each mini spiral is just a smaller version of the original spiral, as such you can carefully call the original function to make each smaller one.
import turtle
import math
def spiral(steps, left, forward, level):
for step in range(50, steps):
me.left(left)
me.forward(step / forward)
if step % 200 == 0 and level > 0:
x, y = me.position()
heading = me.heading()
spiral((steps * 2) / 3, -left * 1.2, forward * 1.2, level - 1)
me.up()
me.setpos(x, y)
me.setheading(heading)
me.down()
#turtle.tracer(5, 200) # to speed things up
me = turtle.Turtle()
spiral(800, 0.6, 200.0, 4)
Each time spiral() is called, the arguments are modified slightly and the level is reduced by 1. Also the left argument is made negative which has the effect of changing the direction for each sub spiral. Each sub spiral is called after 200 steps. Each sub spiral has 2/3rds of the original steps and so on.... lots of numbers to play with to see how they effect the outcome. When the sub spiral finishes drawing, it jumps back to the point where it started drawing it an continues with the original spiral.
Calling it with a level of 0 would give you a single simple spiral for example.
This would give you the following type of output:
This a draft of a 3D model I’m working with, and I would like to simulate its behaviour using python language. I have been researching on the best implementation for this simulation, but I found nothing that could fit real motion. I have tried analytical solving and failed because of uncertainity of certain parameters (certain errors for arm length) when those were measured.
I want to simulate the motion produced by a revolute joint and transfered to a system which is similar to the one depicted on the scheme.
At a certain time, the system might use the revolute joint and then turn to the following status.
Both status for the system are depicted on the next scheme.
An easy simplification with DH parameters would be:
The important thing is how to calculate the position and the angles of both non-controlled joints so that receptor joint angle (fixed point) can be calculated.
It is not only an inverse kinematics problem. It is necessary to consider the motion restrictions too. The motion must be determined by the revolute joint angle, the lenght of the links and the fixed point position and length.
The red circle in the next image depicts the possible positions for the second non-controlled point.
How would you simulate this motion?
There are one problematic position,
where intersections of two circles (described below)
has one point.
In this situation (we suppose it is planar situation (gravity is perpendicular
to all arm) and static situation) there isn't any force, which move with second non-controlled joint.
In dynamic we choose another solution for next step.
When intersection isn't exist,
that situation dosn't exist
and revolute joint cannot move
to this position.
We obtain (trivialy) motion restrictions when we calculate
all position and determine position where doesn't exist intersection.
Do you obtain end position of non-fixed point directly?
Older ansewer:
Simulate motion:
Calculate position of non controled points for all time between
start position and end position with step delta_t.
Draw step by step each calculated position (for example via Pygame).
Calculate:
First compute position of first non-controlled point (higher)
x_2 = x_1 + l_12 cos(Theta_1),
y_2 = y_1 + l_12 sin(Theta_2),
where X_1(x_1, y_1) is position of revolute point,
X_2(x_2, y_2) is position of first non-controlled point
and l_12 is length between X_1 and X_2
Compute intersection of two circle k_1 and k_2,
where k_1(first non-controlled point, l_23) and k_2(receptor joint, l_34),
where k(center of circle, radius of circle).
Step 2 has two solution.
We choose one of then.
To simulate motion, we must choose
"same solution".
Compute angle from two points:
alpha = math.atan2((y_2-y_1)/(x_2-x_1))