I'm working on a particle filter for an autonomous robot right now, and am having trouble producing expected distance measurements by which to filter the particles. I have an image that I'm using as a map. Each pixel represents a certain scaled area in the enviroment. Space the robot can occupy is white, walls are black, and areas that are exterior to the enviroment are grey.
If you are unfamiliar with what a particle filter is, my python code will create a predetermined number of random guesses as to where it might be (x,y,theta) in the white space. It will then measure the distance to the nearest wall with ultrasonic sensors at several angles. The script will compare these measurements with the measurements that would have been expected at each angle for each guessed location/orientation. Those that most closely match the actual measurements will survive while guesses that are less likely to be right will be eliminated.
My problem is finding the nearest wall AT a given angle. Say the sensor is measuring at 60°. For each guess, I need to adjust the angle to account for the guessed robot orientation, and then measure the distance to the wall at that angle. It's easy enough find the nearest wall in the x direction:
from PIL import Image
#from matplotlib._png import read_png
from matplotlib.pyplot import *
mapp = Image.open("Map.png")
pixels = mapp.load()
width = mapp.size[0]
height = mapp.size[1]
imshow(mapp)
pixelWidth = 5
for x in range(width):
if mapp.getpixel((x, 100)) == (0,0,0,255): #Identify the first black pixel
distance = x*pixelWidth self.x
The problem is that I can't tell the script to search one pixel at a time going at a 60°, or 23°, or whatever angle. Right now the best thing I can think of is to go in the x direction first, find a black pixel, and then use the tangent of the angle to determine how many pixels I need to move up or down, but there are obvious problems with this, mostly having to do with corners, and I can't imagine how many if statements it's going to take to work around it. Is there another solution?
Okay, I think I found a good approximation of what I'm trying to do, though I'd still like to hear if anyone else has a better solution. By checking the tangent of the angle I've actually traveled so far between each pixel move, I can decide whether to move one pixel in the x-direction, or in the y-direction.
for i in range(len(angles)):
angle = self.orientation+angles[i]
if angle > 360:
angle -= 360
x = self.x
y = self.y
x1 = x
y1 = y
xtoy_ratio = tan(angle*math.pi/180)
if angle < 90:
xadd = 1
yadd = 1
elif 90 < angle < 180:
xadd = -1
yadd = 1
elif 180 < angle < 270:
xadd = -1
yadd = -1
else:
xadd = 1
yadd = -1
while mapp.getpixel(x,y) != (0,0,0,255):
if (y-y1)/(x-x1) < xtoy_ratio:
y += yadd
else:
x += xadd
distance = sqrt((y-y1)^2+(x-x1)^2)*pixel_width
The accuracy of this method of course depends a great deal on the actual length represented by each pixel. As long as pixel_width is small, accuracy will be pretty good, but if not, it will generally go pretty far before correcting itself.
As I said, I welcome other answers.
Thanks
Related
I am trying to determine the amount of degrees between two points on a circle. I am having trouble doing so. I have looked at other posts but I cannot seem to figure out how to implement it.
I am trying to get the distance between the two in degrees.
Hopefully the model below gives you a better understanding.
Here is the data chart
def get_angle(x, y):
#75 being the center circle coordinate
(dx, dy) = (75-x, y-75)
angle = degrees(atan2(float(dy), float(dx)))
if angle < 0:
angle += 180
return angle
The values it is returning don't seem right as the values are very similar for some reason. Such as 157 and 120 though it clearly shouldn't return that. I am somewhat new to image processing so I could be looking at it wrong.
Center coordinates: cx, cy
Point coordinates: ax, ay for point A and bc, by for point B
Pseudocode for angle:
dp = dot(A-C, B-C) = (ax-cx)*(bx-cx) + (ay-cy)*(by-cy)
cross = vectorproduct(A-C, B-C) = (ax-cx)*(by-cy) - (ay-cy)*(bx-cx)
angle = degrees(atan2(cross, dp))
if angle < 0:
angle += 180
If angle direction is not what you expect, change sign of cross
I've been trying to make a code with pygame to simulate simple gravity. At the moment, there is only one object (HOM) which is orbiting the sun. However, for reasons unknown to me, whenever I run the code, HOM travels round the sun in an orbit at the start, but then accelerates away from the sun when it reaches ~135 degrees from vertical.
Does anyone know why this is happening and how I can fix it? I have been printing some variables to try and source the problem, but have had no luck so far.
Code:
import pygame,sys,time
from math import *
screen=pygame.display.set_mode((800,600))
G = 5
class Object: #Just an object, like a moon or planet
def __init__(self,mass,init_cds,init_vel,orbit_obj='Sun'):
self.mass = mass
self.cds = init_cds
self.velocity = init_vel
self.accel = [0,0]
self.angle = 0
self.orb_obj = orbit_obj
def display(self):
int_cds = (round(self.cds[0]),round(self.cds[1]))#Stores its co-ordinates as floats, has to convert to integers for draw function
pygame.draw.circle(screen,(255,0,0),int_cds,10)
def calc_gravity(self):
if self.orb_obj == 'Sun':
c_x,c_y = 400,300
c_mass = 10000
else:
c_x,c_y = self.orb_obj.cds
c_mass = self.orb_obj.mass
d_x = self.cds[0]-c_x
d_y = self.cds[1]-c_y
dist = sqrt(d_x**2+d_y**2) #Find direct distance
angle = atan(d_x/d_y) #Find angle
print(d_x,d_y)
print(dist,degrees(angle))
if dist == 0:
acc = 0
else:
acc = G*c_mass/(dist**2) #F=G(Mm)/r^2, a=F/m -> a=GM/r^2
print(acc)
acc_x = acc*sin(angle) #Convert acceleration from magnitude+angle -> x and y components
acc_y = acc*cos(angle)
self.accel = [acc_x,acc_y]
print(self.accel)
self.velocity = [self.velocity[0]+self.accel[0],self.velocity[1]+self.accel[1]] #Add acceleration to velocity
print(self.velocity)
self.cds = (self.cds[0]+self.velocity[0],self.cds[1]+self.velocity[1]) #Change co-ordinates by velocity
print(self.cds)
print('-------------------') #For seperating each run of the function when printing variables
HOM = Object(1000000,(400,100),[10,0]) #The problem planet
clock = pygame.time.Clock()
while True:
for event in pygame.event.get():
if event.type == pygame.QUIT:
pygame.quit()
sys.exit()
screen.fill((0,0,0))
pygame.draw.circle(screen,(255,255,0),(400,300),25)
HOM.display()
HOM.calc_gravity()
clock.tick(30)
pygame.display.flip()
Your main issue has to do with this line:
angle = atan(d_x/d_y) #Find angle
The atan function is very limited in its ability to compute angles because it can't tell the signs of the coordinates you combined in your division. For instance, it will give the same result for atan(1/1) and atan(-1/-1), since both divisions compute the same slope (1).
Instead you should use atan2, and pass the coordinates separately. Since this will let the code see both coordinates, it can pick an angle in the right quadrant of the circle every time.
But there's an even better fix. Instead of computing an angle and then immediately converting it back to a unit vector (by calling sin and cos on it), why not compute the unit vector directly? You already have the original vector's length! Instead of:
acc_x = acc*sin(angle) #Convert acceleration from magnitude+angle -> x and y components
acc_y = acc*cos(angle)
Use:
acc_x = acc * d_x / distance
acc_y = acc * d_y / distance
The d_x / distance and d_y / distance values are the same as the sin and cos values you were getting before (for the angles when they were working correctly), but there's no need for the trigonometry. You can get rid of the line I quoted up top completely!
Note that you might need to reverse the way you're computing d_x and d_y, so that you get a vector that points from the orbiting object towards the object it's orbiting around (instead of pointing the other way, from the center of the orbit towards the orbiting object). I'm not sure if I'm reading your code correctly, but it looks to me like you have it the other way around right now. That means that you were actually getting the wrong results from atan in the cases where your current code was working the way you expected, and the bad behavior (flying off into nowhere) is the code working "correctly" (from a mathematical point of view). Alternatively, you could compute acc to be negative, rather than positive.
As several commenters mentioned, you may have other issues related to your choice of integration algorithm, but those errors are not going to be as large as the main issue with the acceleration angle. They'll crop up as you run your simulation over longer time periods (and try to use larger time steps to make the simulation go faster). Your current algorithm is good enough for an orbit or two, but if you're simulating dozens or hundreds of orbits, you'll start seeing errors accumulate and so you should pick a better integrator.
It is my understanding that the following code
turtle.forward(50)
means moving the turtle on the screen.
Here is a code I am trying to learn:
def forward(distance):
while distance > 0:
if turtle.distance(0,0) > 100:
angle = turtle.towards(0,0)
turtle.setheading(angle)
turtle.forward(1)
distance = distance - 1
I don't really understand how
turtle.forward(1)
distance = distance -1
works here. I understand that if the distance is greater than 100, then the turtle turns back and moves from the end position to the position less than 100. I experimented with it but it is still not clear to me what the code turtle.forward(1) means. Is the turtle supposed to move by 1 pixel? And the final distance is less by another pixel? When I put in different numbers instead of 1, I get strange results. Sorry for asking this question - I am learning very slowly.
Thanks.
turtle.forward(1) is telling the turtle to move forward in the current direction by one pixel.
if turtle.distance(0,0) > 100 checks how far away the turtle is from the origin (center of the pane) and see if it's greater than 100.
angle = turtle.towards(0,0) turtle.setheading(angle) turns the turtle towards the origin
Here's how it comes together:
def forward(distance):
while distance > 0: # loop this while there's still distance
if turtle.distance(0,0) > 100: # check if the distance from the origin is greater that 100
angle = turtle.towards(0,0) # find the angle for the origin from turtle's current location
turtle.setheading(angle) # turns the turtle toward the angle
turtle.forward(1) # move title forward in the current angle by one pixel
distance = distance - 1 # act as a counter for the while loop, so when distance is finally 0, the while loop breaks
The code is kind of contradictory to what it tries to achieve. It looks like the forward function is trying to move towards position (0, 0) in distance number of steps.
def forward(distance):
while distance > 0:
# if the turtle is more than 100 distance away from (0, 0)
# change orientation to face (0, 0)
if turtle.distance(0,0) > 100:
angle = turtle.towards(0,0)
turtle.setheading(angle)
# Make the turtle move one step towards (0, 0)
turtle.forward(1)
# IMO, distance should've been called steps_left
distance = distance - 1
LOGICAL INCONSISTENCY
The code seems like it is trying to make the turtle move towards the origin (0, 0) BUT at the same time it has a counter of sorts called distance - that acts more like steps_allowed_to_take - which might cause the turtle to reach the origin but still attempt to move towards the origin. The turtle.forward(1) in this scenario would actually cause the turtle to move one step away from the origin and in the next loop, the turtle would move back to the origin. Doesn't make sense.
This function is a replacement for turtle.forward() that keeps a turtle that's on a random walk within a circular boundary.
Consider a simplified version of the function:
def forward(distance):
while distance > 0:
turtle.forward(1)
distance = distance - 1
It does exactly what turtle.forward(distance) does, though less efficiently. It moves the turtle forward one pixel at a time until it goes the entire distance, rather then go the whole distance in one motion via turtle.forward(distance). Why? This allows the code to make a decision on each pixel of the move regarding whether the turtle has violated the boundary and adjust its heading:
if turtle.distance(0, 0) > 100:
angle = turtle.towards(0, 0)
turtle.setheading(angle)
Let's embed this function in code to illustrate what it does:
from random import randint
import turtle
def forward(distance):
while distance > 0:
if turtle.distance(0, 0) > 100:
angle = turtle.towards(0, 0)
turtle.setheading(angle)
turtle.forward(1)
distance = distance - 1
boundary = turtle.Turtle(visible=False)
boundary.color("red")
boundary.penup()
boundary.sety(-100)
boundary.pendown()
boundary.circle(100)
turtle.shape("turtle") # default turtle
for _ in range(1000): # akin to while True:
turtle.left(randint(0, 90))
forward(randint(1, 50))
turtle.done()
OUTPUT
I am trying to move a rectangle to a specific point with a specific speed.
however it only works properly if x,y (the point i am trying to move it to) are the same. otherwise, if x was bigger than y it would move at a 45 degree angle until self.location[1]==y(it reached where it needed to be for y), then it would move in a straight for x and vise versa.
i know that i would have to change speed_y so that it was slower. how do i work out what speed i need y to be at in order to get the rectangle to move to location in a straight line no matter what location is?
full function:
def move(self,x,y,speed):
if speed==0:
self.location=[x,y]
else:
speed_x = speed
speed_y = speed
if x > 0 and not self.location[0]==x: # x is positive and not already where it needs to be
if not x == y:
speed_x = something # need to slow down the speed so that it gets to y as the same time as it gets to x
self.speed_x=speed_x
else: self.speed_x=0
if y > 0 and not self.location[1]==y: # same for y
if not x == y:
speed_y = something # need to slow down the speed so that it gets to y as the same time as it gets to x
self.speed_y=speed_y
else: self.speed_y=0
You should set your speeds to a ratio of required distance for each axis.
e.g. if distance to x is half distance to y then speed_x should be half speed_y.
Further example as requested:
distance_x = x-self.location[0]
distance_y = y-self.location[1]
if abs(distance_x) < abs(distance_y):
ratio = distance_x/abs(distance_y)
speed_x = ratio * speed
edit: Reworked the directions of example.
I don't quite get what you are asking for, but see if this helps you:
speed_x = speed*(cos(atan2((y-self.location[1]), (x-self.location[0]))))
speed_y = speed*(sin(atan2((y-self.location[1]), (x-self.location[0]))))
That would "slow" the speed you are given by splitting it in the values needed to get to where you want your box to be at the same time.
Pardon any errors in my english/python, im not native on either one :)
You need mathematical calculations to get new position in every move.
I have a program written for simple 2D collision detection and handling on my current billiards simulation, however it is rubbish am looking for a solution that involves proper physics, i.e. newtons laws of motion for coliisions. what I have so far, which does not conserverve momentum and simplifies physics is
def collide(ball1,ball2):
dx = ball1.x - ball2.x
dy = ball1.y - ball2.y
dist = (dx)**2+(dy)**2
if dist < (ball1.radius + ball2.radius)**2:
tangent = atan2(dy, dx)
angle = 0.5 * pi + tangent
angle1 = 2*tangent - ball1.angle
angle2 = 2*tangent - ball2.angle
speed1 = ball2.speed*e
speed2 = ball1.speed*e
(ball1.angle, ball1.speed) = (angle1, speed1)
(ball2.angle, ball2.speed) = (angle2, speed2)
ball1.x += sin(angle)
ball1.y -= cos(angle)
ball2.x -= sin(angle)
ball2.y += cos(angle)
and what i have to run the collisions is this, where the bounce() is for hitting against the wall
running = True
while running:
background()
for i,ball in enumerate(balls,1):
ball.bounce()
ball.move()
for ball2 in balls[i:]:
collide(ball,ball2)
ball.display()
pygame.display.flip()
i'm still pretty new to this so please change whatever is useless/stupid
I recall I did a simple billiard simulator a while back. As you mentioned this is for educational purposes I will spare you from the whole code (and I don't have to dig around for it too :) )
But basically, I kept track of how long time has elapsed since the last frame. I used this time to find out new positions of each ball given a speed vector of each ball. For collisions in a frame I had to figure out at which exact time two balls would collide, and then apply each collision at that exact time. Pseudo code would look something like:
while running:
frame_time_ms = time elapsed since last frame
collisions = all collisions that will happen during this frame, with the exact time of collision. (1)
while collisions:
collision = first collision in collisions
collision_time_ms = time of collision (1)
move all balls to collision_time_ms
collide the two balls in the collision (2)
collisions = all remaining collisions after the time of collision (1)
move all balls to the end time of the frame
So, you will need to bring back your geometry and physics knowledge to find out the key formulas to:
Given the start and end position of two balls in a frame (or part of
a frame), do they collide, and at which point do they collide.
Remember to include the radius of the balls as well here. This will
give you the time of collision.
Given two balls at exact collision position, how will their new speed vectors look like afterwards. Some hints is to use elastic collisions, and experiment with how elastic it actually is:
http://en.wikipedia.org/wiki/Elastic_collision For bonus points you
could also include rotation of the balls :)
Good luck! =)
Circle on circle collision is simple. Take the center point co ordinance, subtract them to determine the distance between each circle.
Then, if the distance is greater than the sum of both circles radii, than they do not touch. If it is equal they are touching, if it's less they are overlapping.
Simply, if they are touching, have them repel each other. You can do this in a few ways. If you keep track of the directions they move in, have them move in the opposite direction.
As for walls, just use each wall with > < statements. So if the pos of a circle has x coordinates less than the west wall, than it has passed that wall. Again, just have them repel the walls.
Circle Collison is very simple if however you want to do other shapes it will be unbelievably difficult. Unless you just pit circles around those shapes or use pixel perfect collision (this is very high performance demand.)
If you want highly accurate collision of non circles, get a physics engine.