I am trying to implement Inverse Kinematics on a 2D arm(made up of three sticks with joints). I am able to rotate the lowest arm to the desired position. Now, I have some questions:
How can I make the upper arm move alongwith the third so the end point of the arm reaches the desired point. Do I need to use the rotation matrices for both and if yes can someone give me some example or an help and is there any other possibl;e way to do this without rotation matrices???
The lowest arm only moves in one direction. I tried google it, they are saying that cross product of two vectors give the direction for the arm but this is for 3D. I am using 2D and cross product of two 2D vectors give a scalar. So, how can I determine its direction???
Plz guys any help would be appreciated....
Thanks in advance
Vikram
I'll give it a shot, but since my Robotics are two decades in the past, take it with a grain of salt.
The way I learned it, every joint was described by its own rotation matrix, defined relative to its current position and orientation. The coordinate of the whole arm's endpoint was then calculated by combining the rotation matrices together.
This achieved exactly the effect you are looking for: you could move only one joint (change its orientation), and all the other joints followed automatically.
You won't have much chance in getting around matrices here - in fact, if you use homogeneous coordinates, all joint calculations (rotations as well as translations) can be modeled with matrix multiplications. The advantage is that the full arm position can then be described with a single matrix (plus the arm's origin).
With this transformation matrix, you can tackle the inverse kinematic problem: since the transformation matrix' elements will depend on the angles of the joints, you can treat the whole calculation 'endpoint = startpoint x transformation' as a system of equations, and with startpoint and endpoint known, you can solve this system to determine the unknown angles. The difficulty herein lies that the equation may not be solvable, or that there are multiple solutions.
I don't quite understand your second question, though - what are you looking for?
Instead of a rotation matrix, the rotation can be represented by its angle or by a complex number of the unit circle, but it's the same thing really. More importantly, you need a representation T of rigid body transformations, so that you can write stuff like t1 * t2 * t3 to compute the position and orientation of the third link.
Use atan2 to compute the angle between the vectors.
As the following Python example shows, those two things are enough to build a small IK solver.
from gameobjects.vector2 import Vector2 as V
from matrix33 import Matrix33 as T
from math import sin, cos, atan2, pi
import random
The gameobjects library does not have 2D transformations, so you have to write matrix33 yourself. Its interface is just like gameobjects.matrix44.
Define the forward kinematics function for the transformation from one joint to the next. We assume the joint rotates by angle and is followed by a fixed transformation joint:
def fk_joint(joint, angle): return T.rotation(angle) * joint
The transformation of the tool is tool == fk(joints, q) where joints are the fixed transformations and q are the joint angles:
def fk(joints, q):
prev = T.identity()
for i, joint in enumerate(joints):
prev = prev * fk_joint(joint, q[i])
return prev
If the base of the arm has an offset, replace the T.identity() transformation.
The OP is solving the IK problem for position by cyclic coordinate descent. The idea is to move the tool closer to the goal position by adjusting one joint variable at a time. Let q be the angle of a joint and prev be the transformation of the base of the joint. The joint should be rotated by the angle between the vectors to the tool and goal positions:
def ccd_step(q, prev, tool, goal):
a = tool.get_position() - prev.get_position()
b = goal - prev.get_position()
return q + atan2(b.get_y(), b.get_x()) - atan2(a.get_y(), a.get_x())
Traverse the joints and update the tool configuration for every change of a joint value:
def ccd_sweep(joints, tool, q, goal):
prev = T.identity()
for i, joint in enumerate(joints):
next = prev * fk_joint(joint, q[i])
q[i] = ccd_step(q[i], prev, tool, goal)
prev = prev * fk_joint(joint, q[i])
tool = prev * next.get_inverse() * tool
return prev
Note that fk() and ccd_sweep() are the same for 3D; you just have to rewrite fk_joint() and ccd_step().
Construct an arm with n identical links and run cnt iterations of the CCD sweep, starting from a random arm configuration q:
def ccd_demo(n, cnt):
q = [random.uniform(-pi, pi) for i in range(n)]
joints = [T.translation(0, 1)] * n
tool = fk(joints, q)
goal = V(0.9, 0.75) # Some arbitrary goal.
print "i Error"
for i in range(cnt):
tool = ccd_sweep(joints, tool, q, goal)
error = (tool.get_position() - goal).get_length()
print "%d %e" % (i, error)
We can try out the solver and compare the rate of convergence for different numbers of links:
>>> ccd_demo(3, 7)
i Error
0 1.671521e-03
1 8.849190e-05
2 4.704854e-06
3 2.500868e-07
4 1.329354e-08
5 7.066271e-10
6 3.756145e-11
>>> ccd_demo(20, 7)
i Error
0 1.504538e-01
1 1.189107e-04
2 8.508951e-08
3 6.089372e-11
4 4.485040e-14
5 2.601336e-15
6 2.504777e-15
In robotics we most often use DH parameters for the forward and reverse kinematics. Wikipedia has a nice introduction.
The DH (Denavit-Hartenberg) notation is part of the solution. It helps you collect a succinct set of values that describe the mechanics of your robot such as link length and joint type.
From there it becomes easier to calculate forward kinematics. The first think you have to understand is how to translate a coordinate frame from one place to another coordinate frame. For example, given your robot (or the DH table of it), what is the set of rotations and translations you have to apply to one coordinate frame (the world for example) to know the location of a point (or vector) in the robot's wrist coordinate frame.
As you may already know, homogeneous transform matrices are very useful for such transformations. They are 4x4 matrices that encapsulate rotation and translation. Another very useful property of those matrices is that if you have two coordinate frames linked and defined by some rotation and translation, if you multiply the two matrices together, then you just need to multiply your transformation target by the product of that multiplication.
So the DH table will help you build that matrix.
Inverse kinematics is a bit more complicated though and depends on your application. The complication arises from having multiple solutions for the same problem. The greater the number of DOF, the greater the number of solutions.
Think about your arm. Pinch something solid around you. You can move your arm to several locations in the space and still keep your pinching vector unchanged. Solving the inverse kinematics problem involves deciding which solution to choose as well.
Related
I'm looking for an appropriate algorithm that reliably predicts input coordinates given by a digital stylus (e.g. Surface Pen). In my application, I'm given the coordinates of the input as x and y coordinates on the screen (together with their timestamps).
Given a list of events ordered chronologically, my aim is to find the coordinate of the "next" events.
The simple algorithm I came up with is using weighted average. Here is some code that illustrates it (in real code I use more than just 4 points):
class Point{
double x;
double y;
double timestamp;
}
// this contains 4 points
// in chronological order
vector<Point> points;
// predict using simple weighted average
// the subtraction operation is simply subtraction on x and y coordinates
Point diff1 = points[1]-points[0];
Point diff2 = points[2]-points[1];
Point diff3 = points[3]-points[2];
// add a weighted difference to the last point we have
Point predictedPoint = points[3] + diff1 * 0.2 + diff2 * 0.3 + diff3 * 0.5;
This doesn't work as well as I would like to as often the predicted input is not very accurate. So I would like to find something better that estimates the next input based on previous points (it'd be nice if this accounted for the velocity of inputs as well).
Unfortunately not a go to answer but merely some solution directions to consider.
One option is to plot the coordinates as a function of time and calculate the trendline (see e.g. https://math.stackexchange.com/questions/204020/what-is-the-equation-used-to-calculate-a-linear-trendline)
In your case the X axis would be the time component and the Y axis either the X or Y coordinate, so you'll end up with 2 functions. Take T=0 for the first timespan and the rest of the X axis in seconds.
This solution will only work with somewhat linear movements (maybe a zigzag) only. The issue with this approach however is that when let's say you draw half a circle the next predicted point will try to make an S curve because you average only known points.
Another option would be to differentiate the slope. Lets say you want to draw a hexagon, then the slope of your first line will be 0deg, second line 60deg, 3rd line 120deg, 4th 180deg, 5th 270deg, 6th=1St 360=0deg. In this case your X,Y coordinates go all over the place but the difference in slope is every time 60deg. Same applies to drawing a circle but the steps are infinitely smaller (this is actually one of the methods to calculate pi)
Now if you would just calculate the slope over the actual X,Y coordinates you'll basically end up with the same kind of algorithm as in your initial code. Basically you want to calculate the acceleration (using same method as described in the beginning of this answer), see e.g. https://blog.prepscholar.com/acceleration-formula-equation, and then calculate the trendline over the acceleration.
But also this solution is not fool proof. If your pencil trace is a zigzag then both your direction and acceleration will go nuts on every corner and your prediction will make no sense.
My advice would be to record a number of strokes, plot the result in a number of graphs and try some different formula's and methods to see which gives the best result.
I am using matlab's built in function called Procrustes to see the rotation translation and scale between two images. But, I am just using coordinates of the brightest points in the image and rotating these coordinates about the center of the image. Procrustes compares two matrices and gives you the rotation, translation, and scale. However, procrustes only works correctly if the matrices are in the same order for comparison.
I am given an image and a separate comparison coordinate matrix. The end goal is to find how much the image has been rotated, translated, and scaled compared to the coordinate matrix. I can just use Procrustes for this, but I need to correctly order the coordinates found from the image to match the order in the comparison coordinate matrix. My thought was to compare the distance between every possible combination of points in the coordinate matrix and compare it to the coordinates that I find in the picture. I just do not know how to write this code due to the fact if there is n coordinates, there will be n! possible combinations.
Just searching for the shortest distance is not so hard.
A = rand(1E4,2);
B = rand(1E4,2);
tic
idx = nan(1,1E4);
for ct = 1:size(A,1)
d = sum((A(ct,:)-B).^2,2);
idx(ct) = find(d==min(d));
end
toc
plot(A(1:10,1),A(1:10,2),'.r',B(idx(1:10),1),B(idx(1:10),2),'.b')
takes half a second on my PC.
The problems can start when two points in set A are matched to the same location in set B.
length(unique(idx))==length(idx)
This can be solved in several ways. The best (imho) is to determine a probability that point B matches with point A based on the distance (usually something that decreases exponentially), and solve for the most probable situation.
A simpler method (but more error prone) is to remove the matched point from set B.
Why hello thar. I have a 3D model whose starting angles along its 3 axis are known. Those angles are transformed into a Direction Cosine Matrix using this pattern (information found here) :
New angles values are obtained as time goes by, corresponding to an update of the model's orientation. To take those new values into consideration, I update the DCM with those in this way :
newDCM = oldDCM * newanglesDCM
What I want and how I do it : Now the tricky part. I actually only want the Y component of the rotation matrix. In order to apply a motion on my model, I need it to be flatten so the motion vector doesn't go either in the air or down on the ground. To do so I simply pull the 3 angles back from the rotation matrix and create a new one with the angles [0 Y 0].
The problem I have : When rotation are applied to the model, the DCM is updated. As a motion is detected, I multiply the motion vector [0 Yvalue 0] to the DCM that has been flatten (according to the previous explanation). The result is great when the instantaneous rotations have null or close to null values in X and Z components. But as soon as the model is in a situation where X and Z have significant values, the "orientation" of the model's motion is wrong. If I apply rotations that make it return into a "only Y" situation, it starts being good again.
What could go wrong : Either my Direction Cosine Matrix is wrong, or the technique I use to flatten the matrix is just completely stupid.
Thanks for your help, I'd really appreciate if you could give me a hand on this !
Greg.
EDIT : Example as requested
My model has 3 axis X,Y and Z. This defines the XYZ convention when the model is at rest. At starting point t0, I know the angles dAx, dAy and dAz that allow me to rotate the model from its original configuration to the one it is in at t0. If that bugs you, let's say that the model is at rest at t0, it doesn't matter.
I create the DCM just like explained in the image (let it be an identity matrix if it started at rest).
Every now and then rotations are applied to the model. Those rotations also are made of a dAx, dAy and dAz. Thus I update the rotation matrix (DCM) by multiplying the old one by the newly generated one : newDCM = oldDCM * newanglesDCM.
Now let's say I want the model to move, on the grid, from a point to another. Imagine the grid to be a street for example. Whether the model is oriented towards the sky, on a side or in front of it, I want the motion to be the same : alonside the road and not elevating in the air or diving into the ground.
If I kept the rotation matrix as it is, applying a [0 Y 0] rotation WOULD make it go somewhere I don't want it to. Thus I try to find my old original XZ frame, by flattening the DCM. Then I still have the Y component so I know where in the street the model is moving.
Imagine a character, whose head is the model and who is walking outside. If he looks to a building's window and walks, he won't walk in the air right up to the window - he'll walk to the feet of the building. That is exactly what I want to do :D
What you have to do is equate the elements two rotation matrices
E_1 = Rx(dθx)Ry(dθy)Rz(dθz)
and
E_2 = Rx(dφx)Rz(dφz)Ry(dφy)
in an element by element basis. Find two elemetns that contain only sin(dφy) and cos(dφy) and divide them to get tan(dφy)=... in terms of dθx, dθy and dθz.
I tried to do this with the DCM given but I cannot replicate the sequence of rotations to get what you have. My E_1 above is similar but some signs are different. In my example that I did I got the following expressions
dφy=atan( tan(dθy)/cos(dθz) )
dφz=atan( (cos(dθy)*sin(dθz))/(cos(dθy)*cos(dθz)*cos(dφy)+sin(dθy)*sin(dφy)) )
dφx=atan( (cos(dθx)*sin(dθy)*sin(dθz)+sin(dθx)*cos(dθz))/(cos(dθx)*cos(dθz)- ...
You have to work out your own relationships based on the sequences you use.
Note: that once dφy is known the above E_1 = E_2 equality becomes
Rx(dφx)Rz(dφz) = Rx(dθx)Ry(dθy)Rz(dθz)Ry(-dφy)
which is solved for dφz and then you have
Rx(dφx) = Rx(dθx)Ry(dθy)Rz(dθz)Ry(-dφy)Rz(-dφz)
for dφx.
If I have 2 sets of points I can rotate one around with Procrustes analysis to align one with the other.
But suppose these 2 sets of points each are attached to images and I would like to rotate the images as well. Is there any way I can also rotate the image, instead of rotating just the points? The tutorial there uses a dot product for rotation (solve u, s, v = svd(p1', p2) and then do p2 . v . u', p' is transposed p)
However that doesn't tell me what the angle between the images is.
The page on wikipedia calculates an angle between each pair of points I think.
Maybe what I'm asking is impossible? If I rotate the first set of points to align it with the first, can't I also rotate the respective images by an angle as well? Point being, which angle is that?
I noticed that v . u' gives me a 2 x 2 matrix which seems to be the rotation matrix (there's a wikipedia page but I can't link there due to posting priviledges). I got the sin and cos of the third and first elements and then used arctan2, but the results I'm getting are kind of weird. I know they have to be transformed from radians but I'm not convinced what I'm doing is right. Trying the rotation it gives me on gimp makes it seem like it's not what I want, but I'll test some more.
It seems like your approach is mostly correct. Two things which come to mind:
1) The paper you linked to (Procrustes analysis) involves a translation and a scaling in addition to rotation. Depending on whether or not those operations are also performed on your images, you may end up with strange results that don't appear to match.
2) I think you may be overcomplicating your angle calculation. v * u' appears to be the correct rotation matrix, but I believe the correct angle only requires one of the matrix entries in the 2x2 matrix. For instance, just use acos() of the first matrix entry. As you've noticed, this will (depending on the program) give you an answer in radians which you'll have to convert to degrees if you want to try out the rotation in gimp, etc.
Hope this helps!
So first of all I have such image (and ofcourse I have all points coordinates in 2d so I can regenerate lines and check where they cross each other)
(source: narod.ru)
But hey, I have another Image of same lines (I know thay are same) and new coords of my points like on this image
(source: narod.ru)
So... now Having points (coords) on first image, How can I determin plane rotation and Z depth on second image (asuming first one's center was in point (0,0,0) with no rotation)?
What you're trying to find is called a projection matrix. Determining precise inverse projection usually requires that you have firmly established coordinates in both source and destination vectors, which the images above aren't going to give you. You can approximate using pixel positions, however.
This thread will give you a basic walkthrough of the techniques you need to use.
Let me say this up front: this problem is hard. There is a reason Dan Story's linked question has not been answered. Let provide an explanation for people who want to take a stab at it. I hope I'm wrong about how hard it is, though.
I will assume that the 2D screen coordinates and projection/perspective matrix is known to you. You need to know at least this much (if you don't know the projection matrix, essentially you are using a different camera to look at the world). Let's call each pair of 2D screen coordinates (a_i, b_i), and I will assume the projection matrix is of the form
P = [ px 0 0 0 ]
[ 0 py 0 0 ]
[ 0 0 pz pw]
[ 0 0 s 0 ], s = +/-1
Almost any reasonable projection has this form. Working through the rendering pipeline, you find that
a_i = px x_i / (s z_i)
b_i = py y_i / (s z_i)
where (x_i, y_i, z_i) are the original 3D coordinates of the point.
Now, let's assume you know your shape in a set of canonical coordinates (whatever you want), so that the vertices is (x0_i, y0_i, z0_i). We can arrange these as columns of a matrix C. The actual coordinates of the shape are a rigid transformation of these coordinates. Let's similarly organize the actual coordinates as columns of a matrix V. Then these are related by
V = R C + v 1^T (*)
where 1^T is a row vector of ones with the right length, R is an orthogonal rotation matrix of the rigid transformation, and v is the offset vector of the transformation.
Now, you have an expression for each column of V from above: the first column is { s a_1 z_1 / px, s b_1 z_1 / py, z_1 } and so on.
You must solve the set of equations (*) for the set of scalars z_i, and the rigid transformation defined R and v.
Difficulties
The equation is nonlinear in the unknowns, involving quotients of R and z_i
We have assumed up to now that you know which 2D coordinates correspond to which vertices of the original shape (if your shape is a square, this is slightly less of a problem).
We assume there is even a solution at all; if there are errors in the 2D data, then it's hard to say how well equation (*) will be satisfied; the transformation will be nonrigid or nonlinear.
It's called (digital) photogrammetry. Start Googling.
If you are really interested in this kind of problems (which are common in computer vision, tracking objects with cameras etc.), the following book contains a detailed treatment:
Ma, Soatto, Kosecka, Sastry, An Invitation to 3-D Vision, Springer 2004.
Beware: this is an advanced engineering text, and uses many techniques which are mathematical in nature. Skim through the sample chapters featured on the book's web page to get an idea.