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))
Related
When the Rodrigues function is called with a rotation matrix as argument it provides 2 results.
I understand that the first item returned is the vector around which the rotation occurs and that the magnitude of the vector provides the angle of rotation. It seems that it provides a number (in radians) in the range (0,180) degrees for rotations covering (0,360) degrees, therefore there must be a way to determine the sign of the rotation. How do you do that.
As a supplimentary question, I understand that the second result is a Jacobian matrix. How do you use that?
The rotation is always positive, and when it "needs" to be negative (equivalently, closer to 360 than 0 degrees), the vector is simply flipped to the other side, so now it can be positive.
There is the "right hand rule". Right hand grabs vector, thumb pointing along the vector. Fingers indicate positive rotation around vector.
Example: Place your (right) fist on the desk, thumb up. Going +90 degrees is a quarter turn counterclockwise (inward). Going -90 degrees is a quarter turn clockwise (outward)... or +90 degrees with your thumb pointing into the desk.
The Jacobian is a bunch of derivatives, a vector in output-space for each component of the input. It tells you how stable the calculation is, i.e. how easily perturbed the result is, were any of the elements of your input vector to fluctuate by a bit.
Jacobians also show up in robotics. You can use them for inverse kinematics, combined with a solver. Given the Jacobian of your "robot arm", a tool center point, and a target, some math involving a Jacobian tells you what joints to move (a little bit) in which way to get closer to the target. The Jacobian depends on the current pose (i.e. it's not a constant matrix), so you'd recalculate it all the time.
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 a set of approximately 10,000 vectors max (random directions) in 3d space and I'm looking for a new direction v_dev (vector) which deviates from all other directions in the set by e.g. a minimum of 5 degrees. My naive initial try is the following, which has of course bad runtime complexity but succeeds for some cases.
#!/usr/bin/env python
import numpy as np
numVecs = 10000
vecs = np.random.rand(numVecs, 3)
randVec = np.random.rand(1, 3)
notFound=True
foundVec=randVec
below=False
iter = 1
for vec in vecs:
angle = np.rad2deg(np.arccos(np.vdot(vec, foundVec)/(np.linalg.norm(vec) * np.linalg.norm(foundVec))))
print("angle: %f\n" % angle)
while notFound:
for vec in vecs:
angle = np.rad2deg(np.arccos(np.vdot(vec, randVec)/(np.linalg.norm(vec) * np.linalg.norm(randVec))))
if angle < 5:
below=True
if below:
randVec = np.random.rand(1, 3)
else:
notFound=False
print("iteration no. %i" % iter)
iter = iter + 1
Any hints how to approach this problem (language agnostic) would be appreciate.
Consider the vectors in a spherical coordinate system (u,w,r), where r is always 1 because vector length doesn't matter here. Any vector can be expressed as (u,w) and the "deadzone" around each vector x, in which the target vector t cannot fall, can be expressed as dist((u_x, w_x, 1), (u_x-u_t, w_x-w_t, 1)) < 5°. However calculating this distance can be a bit tricky, so converting back into cartesian coordinates might be easier. These deadzones are circular on the spherical shell around the origin and you're looking for a t that doesn't hit any on them.
For any fixed u_t you can iterate over all x and using the distance function can find the start and end point of a range of w_t, that are blocked because they fall into the deadzone of the vector x. The union of all 10000 ranges build the possible values of w_t for that given u_t. The same can be done for any fixed w_t, looking for a u_t.
Now comes the part that I'm not entirely sure of: Given that you have two unknows u_t and w_t and 20000 knowns, the system is just a tad overdetermined and if there's a solution, it should be possible to find it.
My suggestion: Set u_t fixed to a random value and check which w_t are possible. If you find a non-empty range, great, you're done. If all w_t are blocked, select a different u_t and try again. Now, selecting u_t at random will work eventually, yet a smarter iteration should be possible. Maybe u_t(n) = u_t(n-1)*phi % 360°, where phi is the golden ratio. That way the u_t never repeat and will cover the whole space with finer and finer granularity instead of starting from one end and going slowly to the other.
Edit: You might also have more luck on the mathematics stackexchange since this isn't so much a code question as it is a mathematics question. For example I'm not sure what I wrote is all that rigorous, so I don't even know it works.
One way would be two build a 2d manifold (area on the sphere) of forbidden areas. You start by adding a point, then, the forbidden area is a circle on the sphere surface.
While true, pick a point on the boundary of the area. If this is not close (within 5 degrees) to any other vector, then, you're done, return it. If not, you just found a new circle of forbidden area. Add it to your manifold of forbidden area. You'll need to chop the circle in line or arc segments and build the boundary as a list.
If the set of vector has no solution, you boundary will collapse to an empty point. Then you return failure.
It's not the easiest approach, and you'll have to deal with the boundaries of a complex shape over a sphere. But it's guaranteed to work and should have reasonable complexity.
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 have an issue that I can't seem to solve. I have already acquired data from another source and created 2 polynomials that are identical in shape but not in orientation, that is one is rotated x degrees compared to the other, and if you rotate the graph x degrees back they will match.
I have already taken the derivative of both of the graphs at a certain point.
I would like to graph these slopes onto a unit circle on a polar graph, and somehow find the angle difference between these two line segments of slope i and j that extend from the origin.
I'm fairly new to python so I so not know how to begin plotting these in polar or finding a way to determine the angle difference. I know that by hand, you can take the inverse tangent but that will only give you a range from +90 to -90. I would like my number to fall in the range from 0 to 360 for rotation.
Any help is appreciated. If this isn't enough info or if it isn't clear enough I can provide more.