I am doing a simulation on a robot that makes circular motion with a center coordinate (x1, y1). The robot measures its position periodically and returns the information. Let's say the returned coordinate is (x3, y3). However, I'm not too sure about the correct way of using atan2 in python to calculate the angles between the current measurement and the last measurement with the center.
so far, I am doing the following, and would like to confirm if it's right:
current_angle = atan2(y3-y1, x3-x1)
last_angle = atan2(y2-y1, x2-x1)
angle_difference = current_angle - last_angle
With the angle_difference, I then calculate the angular velocity = angle_difference / dt
(dt is the time spent between the last measurement and the current measurement)
Almost correct. The formula as given will have trouble when passing across axes between the first and fourth quadrants and the second and third quadrants, so you need to take the minimum of the absolute values of the differences of the angles instead, and then compensate for sign.
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 have the coordinates of 6 points in an image
(170.01954650878906, 216.98866271972656)
(201.3812255859375, 109.42137145996094)
(115.70114135742188, 210.4272918701172)
(45.42426300048828, 97.89037322998047)
(167.0367889404297, 208.9329833984375)
(70.13690185546875, 140.90538024902344)
I have a point as center [89.2458, 121.0896]. I am trying to re-calculate the position of points in python using 4 rotation degree (from 0,90,-90,180) and 6 scaling factor (0.5,0.75,1,1.10,1.25,1.35,1.5).
My question is how can I rotate and scale the abovementioned points relative to the center point and get the new coordinates of those 6 points?
Your help is really appreciated.
Mathematics
A mathematical approach would be to represent this data as vectors from the center to the image-points, translate these vectors to the origin, apply the transformation and relocate them around the center point. Let's look at how this works in detail.
Representation as vectors
We can show these vectors in a grid, this will produce following image
This image provides a nice way to look at these points, so we can see our actions happening in a visual way. The center point is marked with a dot at the beginning of all the arrows, and the end of each arrow is the location of one of the points supplied in the question.
A vector can be seen as a list of the values of the coordinates of the point so
my_vector = [point[0], point[1]]
could be a representation for a vector in python, it just holds the coordinates of a point, so the format in the question could be used as is! Notice that I will use the position 0 for the x-coordinate and 1 for the y-coordinate throughout my answer.
I have only added this representation as a visual aid, we can look at any set of two points as being a vector, no calculation is needed, this is only a different way of looking at those points.
Translation to origin
The first calculations happen here. We need to translate all these vectors to the origin. We can very easily do this by subtracting the location of the center point from all the other points, for example (can be done in a simple loop):
point_origin_x = point[0] - center_point[0] # Xvalue point - Xvalue center
point_origin_y = point[1] - center_point[1] # Yvalue point - Yvalue center
The resulting points can now be rotated around the origin and scaled with respect to the origin. The new points (as vectors) look like this:
In this image, I deliberately left the scale untouched, so that it is clear that these are exactly the same vectors (arrows), in size and orientation, only shifted to be around (0, 0).
Why the origin
So why translate these points to the origin? Well, rotations and scaling actions are easy to do (mathematically) around the origin and not as easy around other points.
Also, from now on, I will only include the 1st, 2nd and 4th point in these images to save some space.
Scaling around the origin
A scaling operation is very easy around the origin. Just multiply the coordinates of the point with the factor of the scaling:
scaled_point_x = point[0] * scaling_factor
scaled_point_y = point[1] * scaling_factor
In a visual way, that looks like this (scaling all by 1.5):
Where the blue arrows are the original vectors and the red ones are the scaled vectors.
Rotating
Now for rotating. This is a little bit harder, because a rotation is most generally described by a matrix multiplication with this vector.
The matrix to multiply with is the following
(from wikipedia: Rotation Matrix)
So if V is the vector than we need to perform V_r = R(t) * V to get the rotated vector V_r. This rotation will always be counterclockwise! In order to rotate clockwise, we simply need to use R(-t).
Because only multiples of 90° are needed in the question, the matrix becomes a almost trivial. For a rotation of 90° counterclockwise, the matrix is:
Which is basically in code:
rotated_point_x = -point[1] # new x is negative of old y
rotated_point_y = point[0] # new y is old x
Again, this can be nicely shown in a visual way:
Where I have matched the colors of the vectors.
A rotation 90° clockwise will than be
rotated_counter_point_x = point[1] # x is old y
rotated_counter_point_y = -point[0] # y is negative of old x
A rotation of 180° will just be taking the negative coordinates or, you could just scale by a factor of -1, which is essentially the same.
As last point of these operations, might I add that you can scale and/or rotated as much as you want in a sequence to get the desired result.
Translating back to the center point
After the scaling actions and/or rotations the only thing left is te retranslate the vectors to the center point.
retranslated_point_x = new_point[0] + center_point_x
retranslated_point_y = new_point[1] + center_point_y
And all is done.
Just a recap
So to recap this long post:
Subtract the coordinates of the center point from the coordinates of the image-point
Scale by a factor with a simply multiplication of the coordinates
Use the idea of the matrix multiplication to think about the rotation (you can easily find these things on Google or Wikipedia).
Add the coordinates of the center point to the new coordinates of the image-point
I realize now that I could have just given this recap, but now there is at least some visual aid and a slight mathematical background in this post, which is also nice. I really believe that such problems should be looked at from a mathematical angle, the mathematical description can help a lot.
I am performing motion tracking of an object, and I am trying to identify the front and back of the object. The object is asymmetrical, which means that the centroid of the contour is closer to the front than the back. Using this information, I am approaching this as follows:
Draw contours of object
Find centroid
centroidx, centroidy = int(moments['m10']/moments['m00']), int(moments['m10']/moments['m00'])
Draw bounding ellipse
cv2.fitEllipse(contour)
Calculate major axis length as follows (and as shown in the figure)
MAx, MAy = int(0.5 * ellipseMajorAxisx*math.sin(ellipseAngle)), int(0.5 * ellipseMajorAxisy*math.cos(ellipseAngle))
Calculate beginning and ending x, y coordinates of the major axis
MAxtop, MAytop = int(ellipseCentrex + MAx), int(ellipseCentrey + MAy)
MAxbot, MAybot = int(ellipseCentrex - MAx), int(ellipseCentrey - MAy)
Identify which of the points is closer to the centroid of the contour
distancetop = math.sqrt((centroidx - MAxtop)**2 + (centroidy - MAytop)**2)
distancebot = math.sqrt((centroidx - MAxbot)**2 + (centroidy - MAybot)**2)
min(distancetop, distancebot)
The problem I am encountering is, while I get the "front" end of the ellipse correct most of the time, occasionally the point is a little bit away. As far as I have observed, this seems to be happening such that the x value is correct, but y value is different (in effect, I think this represents the major axis of an ellipse that is perpendicular to mine). I am not sure if this is an issue with opencv's calculation of angles or (more than likely) my calculations are incorrect. I do realize this is a complicated example, hope my figures help!
EDIT: When I get the wrong point, it is not from a perpendicular ellipse, but of a mirror image of my ellipse. And it happens with the x values too, not just y.
After following ssm's suggestion below, I am getting the desired point most of the time. The point still goes wrong occasionally, but "snaps back" into place soon after. For example, this is a few frames when this happens:
By the way, the above images are after "correcting" for angle by using this code:
if angle > 90:
angle = 180 - angle
If I do not do the correction, I get the wrong point at other times, as shown below for the same frames.
So it looks like I get it right for some angles with angle correction and the other angles without correction. How do I get all the right points in both conditions?
(White dot inside the ellipse is the centroid of the contour, whereas the dot on or outside the ellipse is the point I am getting)
I think your only problem is MAytop. You can consider doing the following:
if ycen<yc:
# switch MAytop and MAybot
temp = MAytop
MAytop = MAybot
MAybot = temp
You may have to do a similar check on the x - scale
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))
I am trying to estimate the value of pi using a monte carlo simulation. I need to use two unit circles that are a user input distance from the origin. I understand how this problem works with a single circle, I just don't understand how I am meant to use two circles. Here is what I have got so far (this is the modified code I used for a previous problem the used one circle with radius 2.
import random
import math
import sys
def main():
numDarts=int(sys.argv[1])
distance=float(sys.argv[2])
print(montePi(numDarts,distance))
def montePi(numDarts,distance):
if distance>=1:
return(0)
inCircle=0
for I in range(numDarts):
x=(2*(random.random()))-2
y=random.random()
d=math.sqrt(x**2+y**2)
if d<=2 and d>=-2:
inCircle=inCircle+1
pi=inCircle/numDarts*4
return pi
main()
I need to change this code to work with 2 unit circles, but I do not understand how to use trigonometry to do this, or am I overthinking the problem? Either way help will be appreciated as I continue trying to figure this out.
What I do know is that I need to change the X coordinate, as well as the equation that determines "d" (d=math.sqrt(x*2+y*2)), im just not sure how.
These are my instructions-
Write a program called mcintersection.py that uses the Monte Carlo method to
estimate the area of this shape (and prints the result). Your program should take
two command-line parameters: distance and numDarts. The distance parameter
specifies how far away the circles are from the origin on the x-axis. So if distance
is 0, then both circles are centered on the origin, and completely overlap. If
distance is 0.5 then one circle is centered at (-0.5, 0) and the other at (0.5, 0). If
distance is 1 or greater, then the circles do not overlap at all! In that last case, your
program can simply output 0. The numDarts parameter should specify the number
of random points to pick in the Monte Carlo process.
In this case, the rectangle should be 2 units tall (with the top at y = 1 and the
bottom at y = -1). You could also safely make the rectangle 2 units wide, but this
will generally be much bigger than necessary. Instead, you should figure out
exactly how wide the shape is, based on the distance parameter. That way you can
use as skinny a rectangle as possible.
If I understand the problem correctly, you have two unit circles centered at (distance, 0) and (-distance, 0) (that is, one is slightly to the right of the origin and one is slightly to the left). You're trying to determine if a given point, (x, y) is within both circles.
The simplest approach might be to simply compute the distance between the point and the center of each of the circles. You've already done this in your previous code, just repeat the computation twice, once with the offset distance inverted, then use and to see if your point is in both circles.
But a more elegant solution would be to notice how your two circles intersect each other exactly on the y-axis. To the right of the axis, the left circle is completely contained within the right one. To the left of the y-axis, the right circle is entirely within the left circle. And since the shape is symmetrical, the two halves are of exactly equal size.
This means you can limit your darts to only hitting on one side of the axis, and then get away with just a single distance test:
def circle_intersection_area(num_darts, distance):
if distance >= 1:
return 0
in_circle = 0
width = 1-distance # this is enough to cover half of the target
for i in range(num_darts):
x = random.random()*width # random value from 0 to 1-distance
y = random.random()*2 - 1 # random value from -1 to 1
d = math.sqrt((x+distance)**2 + y**2) # distance from (-distance, 0)
if d <= 1:
in_circle += 1
sample_area = width * 2
target_area = sample_area * (in_circle / num_darts)
return target_area * 2 # double, since we were only testing half the target