Related
I would like to find the curvature at a given point on a 3D b-spline.
I believe that I want to use the derivatives of the spline at that point to calculate the curvature at that point but I do not understand how.
I have defined a 3D bspline (taken from SolidWorks (where it came from is not relevant )) in geomdl. I can evaluate any point on the spline using Curve.evaluate_single(). This appears to work correctly. I have verified the returned points against the SolidWorks model so I assume I have implemented the bspline in nurbs correctly.
I need to find the radius at a various points along this curve. From my Google searching I think that I want to use Curve.derivatives() to calculate the instantaneous curvature at that point. But I do not understand how to get the results of Curve.derivatives() into a radius.
So below is the result of Curve.derivatives(SomePointOnPath,4):
[ [74.66019681782404, 131.77035668055586, 19.88498274391211],
[-2719.7097781710354, -598.8099790539873, -711.5032638750225],
[-5384.519486543373, 1273.8662545231637, 19431.220551950217],
[93757.48746982217, -22247.397114396095, 31343.52746776864],
[0.0, 0.0, 0.0]]
By taking points a small step either side of this point I have calculated the radius at this point to be about 409 ( Solving for radius of a circle given 3 points )
I do not understand what the results from Curve.derivatives() are telling me.
(the first tuple is the coordinates of the point, beyond that I am lost)
I expect the radius at this particular point to be about 409.
My fundamental question is two parts:
What are the results telling me. What do they mean.
How do I use these result to calculate the radius at this point.
The radius of curvature of a curve can be computed as |C'|^3/|C' X C"| where C' and C" are the first and the 2nd derivative vectors, X is the cross product operator and |.| is the vector's magnitude. So, you will need the first and the 2nd derivatives at that point to compute the radius of curvature.
Curve.derivatives(SomePointOnPath,4) returns the first 4 derivatives of the curve and the 0-th derivative is the position of the point on the curve. Therefore, the first derivative is [-2719.7097781710354, -598.8099790539873, -711.5032638750225] and the 2nd derivative is [-5384.519486543373, 1273.8662545231637, 19431.220551950217]. So, we can use these two vectors to do our computation and obtain a radius of curvature 408.9176414, which is close to your estimated value 409.
On a side note, you can simply pass '3' as the 2nd argument to Curve.derivatives() as the 3rd derivatvie vector has no use in the computation of radius of curvature.
Let's say I have a contour shape defined by two functions x(p) and y(p), where p is the distance traveled along the perimeter of the shape, normalized between 0 and 1. For example, a unit circle about the origin would be defined as x(p) = sin(2 * pi * p) and y(p) = cos(2 * pi * p).
A simple test to determine if a point is within that circle would be to calculate the point's distance from the origin, and then detect if that distance is less than or equal to 1.
But what if my shape has multiple crossings, and is much more complicated than a circle?
There exists a point in polygon test for a discrete shape defined by a set of points. That algorithm is pretty easy to find, since it's used in a lot of places. But if I don't want to use a discrete definition of my shape, what algorithm can I use to determine the winding number, with the assumption that the shape is defined at compile time?
Generalizing the point in polygon test, you can find all solutions of y(p)=0 such that x(p)>0 and use the parity of their numbers.
In the case of the circle, cos(2πp)=0 for p=(k+1/2)π, and only one value of p in range [0,1) makes sin(2πp)>0.
So far so good if you can solve the y equation analytically. Otherwise you will need a reliable numerical solver, able to spot all roots. An alternative is to flatten the curve (draw it as a polyline, ensuring a maximum deviation tolerance), and apply the polygon algorithm.
For the sake of a second example, let us consider Pascal's limaçon with equation r = 0.5 + cos Θ, and some test point along X.
y = (0.5 + cos Θ) sin Θ = 0
for Θ=0, 2π/3, π, 4π/3. The corresponding abscissas are 1.5, 0, 0.5 and 0.
You can conclude that the interior points on the X axis are between 0.5 and 1.5 (also at 0, in a degenerate way).
If you know a point that is inside (or outside) the shape and the shape is smooth and non-intersecting, then you can connect the known point to any other point and count the number of times it crosses the boundary. An even number of times means the unknown point is similarly inside (or outside). An odd number means the opposite.
This is based on the Jordan Curve Theorem. As I recall, winding number is needed for the proof of the theorem, but the application is quite easy.
How about computing the total curvature of the closed curve (assuming it is everywhere differentiable), then dividing it by 2PI? See Wiki's Total Curvature page here.
I am working on a project using an Arduino that needs to calculate the area of a polygon made up of many points. I use surveyor's theorem,
But the points are in random order, not (counter)clockwise. Some make lines that cross, and they make polygons like a bow-tie or hourglass, which don't work for the surveyor's theorem, so I need to sort them in (counter)clockwise order. what is the easiest way to do this?
You don't need to find the convex hull. Just use the area formula from a bunch of points ordered counterclockwise:
http://en.wikipedia.org/wiki/Polygon#Area_and_centroid
float totalArea = 0.0;
for(i=0; i<N; i++) {
float parallelogramArea = (point[i].x*point[i+1].y - point[i+1].x*point[i].y)
float triangleArea = parallelogramArea / 2.0;
totalArea += triangleArea;
}
// or divide by 2 out here for efficiency
The area formula comes from taking each edge AB, and calculating the (signed) area between the edge and the origin (triangle ABO) by taking the cross-product (which gives you the area of a parallelogram) and cutting it in half (factor of 1/2). As one wraps around the polygon, these positive and negative triangles will overlap, and the area between the origin and the polygon will be cancelled out and sum to 0, while only the area inside remains. This is why the formula is called the Surveyor's Formula, since the "surveyor" is at the origin; if going counterclockwise, positive area is added when going left->right and negative area is added when going right->left, from the perspective of the origin.
The mathematical formula is given below, but does not provide the intuition behind it (as given above):
edit (after question has been changed)
There is absolutely no way to "get their order" without additional assumptions, e.g. "the polygon is convex".
If the polygon is concave, it becomes nearly impossible in the general case without lots of extra assumptions (proof: consider a point which lies within the convex hull, but whose neighbors do not; there are many possible valid polygons you can construct using that point, its neighbors, and their neighbors).
If the polygon is convex, all you need to do is sort by the angle from some arbitrary point inside the polygon (e.g. centroid of three arbitrary points).
You could find the center of gravity (cx,cy) of the points and then calculate the angles of the points relative to (cx,cy).
angle[i] = atan2(y[i]-cy, x[i]-cx) ;
Then sort the points by angle.
Just beware that a random set of points does not describe a single unique polygon. So this method will just give you one of the possible polygons, and not necessarily the polygon you would have obtained if you had manually connected the dots.
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.
Finding the angle between two vectors is not hard using the cosine rule. However, because I am programming for a platform with very limited resources, I would like to avoid calculations such as sqrt and arccos. Even simple divisions should be limited as much as possible.
Fortunately, I do not need the angle per se, but only need some value that is proportional to said angle.
So I am looking for some computationally cheap algorithm to calculate a quantity that is related to the angle between two vectors. So far, I haven't found something that fits the bill, nor have I been able to come up with something myself.
If you don't need the actual euclidean angle, but something that you can use as a base for angle comparisons, then changing to taxicab geometry may be a choice, because you can drop trigonometry and it's slowness while MAINTAINING THE ACCURACY (or at least with really minor loosing of accuracy, see below).
In main modern browser engines the speedup factor is between 1.44 - 15.2 and the accuracy is nearly the same as in atan2. Calculating diamond angle is average 5.01 times faster than atan2 and using inline code in Firefox 18 the speedup reaches factor 15.2. Speed comparison: http://jsperf.com/diamond-angle-vs-atan2/2.
The code is very simple:
function DiamondAngle(y, x)
{
if (y >= 0)
return (x >= 0 ? y/(x+y) : 1-x/(-x+y));
else
return (x < 0 ? 2-y/(-x-y) : 3+x/(x-y));
}
The above code gives you angle between 0 and 4, while atan2 gives you angle between -PI and PI as the following table shows:
Note that diamond angle is always positive and in the range of 0-4, while atan2 gives also negative radians. So diamond angle is sort of more normalized. And another note is that atan2 gives a little more precise result, because the range length is 2*pi (ie 6.283185307179586), while in diamond angles it is 4. In practice this is not very significant, eg. rad 2.3000000000000001 and 2.3000000000000002 are both in diamond angles 1.4718731421442295, but if we lower the precision by dropping one zero, rad 2.300000000000001 and 2.300000000000002 gives both different diamond angle. This "precision loosing" in diamond angles is so small, that it has some significant influence only if the distances are huge. You can play with conversions in http://jsbin.com/bewodonase/1/edit?output (Old version: http://jsbin.com/idoyon/1):
The above code is enough for fast angle comparisons, but in many cases there is need to convert diamond angle to radians and vice verca. If you eg. have some tolerance as radian angles, and then you have a 100,000 times loop where this tolerance is compared to other angles, it's not wise to make comparisons using atan2. Instead, before looping, you change the radian tolerance to taxicab (diamond angles) tolerance and make in-loop comparisons using diamond tolerance and this way you don't have to use slow trigonometric functions in speed-critical parts of the code ( = in loops).
The code that makes this conversion is this:
function RadiansToDiamondAngle(rad)
{
var P = {"x": Math.cos(rad), "y": Math.sin(rad) };
return DiamondAngle(P.y, P.x);
}
As you notice there is cos and sin. As you know, they are slow, but you don't have to make the conversion in-loop, but before looping and the speedup is huge.
And if for some reason, you have to convert diamond angle to radians, eg. after looping and making angle comparisons to return eg. the minimum angle of comparisons or whatever as radians, the code is as follows:
function DiamondAngleToRadians(dia)
{
var P = DiamondAngleToPoint(dia);
return Math.atan2(P.y,P.x);
}
function DiamondAngleToPoint(dia)
{
return {"x": (dia < 2 ? 1-dia : dia-3),
"y": (dia < 3 ? ((dia > 1) ? 2-dia : dia) : dia-4)};
}
Here you use atan2, which is slow, but idea is to use this outside any loops. You cannot convert diamond angle to radians by simply multiplying by some factor, but instead finding a point in taxicab geometry of which diamond angle between that point and the positive X axis is the diamond angle in question and converting this point to radians using atan2.
This should be enough for fast angle comparisons.
Of course there is other atan2 speedup techniques (eg. CORDIC and lookup tables), but AFAIK they all loose accuracy and still may be slower than atan2.
BACKGROUND: I have tested several techniques: dot products, inner products, law of cosine, unit circles, lookup tables etc. but nothing was sufficient in case where both speed and accuracy are important. Finally I found a page in http://www.freesteel.co.uk/wpblog/2009/06/encoding-2d-angles-without-trigonometry/ which had the desired functions and principles.
I assumed first that also taxicab distances could be used for accurate and fast distance comparisons, because the bigger distance in euclidean is bigger also in taxicab. I realized that contrary to euclidean distances, the angle between start and end point has affect to the taxicab distance. Only lengths of vertical and horizontal vectors can be converted easily and fast between euclidean and taxicab, but in every other case you have to take the angle into account and then the process is too slow (?).
So as a conclusion I am thinking that in speed critical applications where is a loop or recursion of several comparisons of angles and/or distances, angles are faster to compare in taxicab space and distances in euclidean (squared, without using sqrt) space.
Have you tried a CORDIC algorithm? It's a general framework for solving polar ↔ rectangular problems with only add/subtract/bitshift + table, essentially doing rotation by angles of the form tan-1 (2-n). You can trade off accuracy with execution time by altering the number of iterations.
In your case, take one vector as a fixed reference, and copy the other to a temporary vector, which you rotate using the cordic angles towards the first vector (roughly bisection) until you reach a desired angular accuracy.
(edit: use sign of dot product to determine at each step whether to rotate forward or backward. Although if multiplies are cheap enough to allow using dot product, then don't bother with CORDIC, perhaps use a table of sin/cos pairs for rotation matrices of angles π/2n to solve the problem with bisection.)
(edit: I like Eric Bainville's suggestion in the comments: rotate both vectors towards zero and keep track of the angle difference.)
Back in the day of a few K of RAM and machines with limited mathematical capabilities I used lookup tables and linear interpolation. The basic idea is simple: create an array with as much resolution as you need (more elements reduce the error created by interpolation). Then interpolate between lookup values.
Here is an example in processing (original dead link).
You can do this with your other trig functions as well. On the 6502 processor this allowed for full 3D wire frame graphics to be computed with an order of magnitude speed increase.
Here on SO I still don't have the privilege to comment (though I have at math.se) so this is actually a reply to Timo's post on diamond angles.
The whole concept of diamond angles based on the L1 norm is most interesting and if it were merely a comparison of which vector has a greater/lesser w.r.t. the positive X axis it would be sufficient. However, the OP did mention angle between two generic vectors, and I presume the OP wants to compare it to some tolerance for finding smoothness/corner status or sth like that, but unfortunately, it seems that with only the formulae provided on jsperf.com or freesteel.co.uk (links above) it seems it is not possible to do this using diamond angles.
Observe the following output from my Asymptote implementation of the formulae:
Vectors : 50,20 and -40,40
Angle diff found by acos : 113.199
Diff of angles found by atan2 : 113.199
Diamond minus diamond : 1.21429
Convert that to degrees : 105.255
Rotate same vectors by 30 deg.
Vectors : 33.3013,42.3205 and -54.641,14.641
Angle diff found by acos : 113.199
Diff of angles found by atan2 : 113.199
Diamond minus diamond : 1.22904
Convert that to degrees : 106.546
Rotate same vectors by 30 deg.
Vectors : 7.67949,53.3013 and -54.641,-14.641
Angle diff found by acos : 113.199
Diff of angles found by atan2 : 113.199
Diamond minus diamond : 1.33726
Convert that to degrees : 116.971
So the point is you can't do diamond(alpha)-diamond(beta) and compare it to some tolerance unlike you can do with the output of atan2. If all you want to do is diamond(alpha)>diamond(beta) then I suppose diamond is fine.
The cross product is proportional to the angle between two vectors, and when the vectors are normalized and the angle is small the cross product is very close to the actual angle in radians due to the small angle approximation.
specifically:
I1Q2-I2Q1 is proportional to the angle between I1Q1 and I2Q2.
The solution would be trivial if the vectors were defined/stored using polar coordinates instead of cartesian coordinates (or, 'as well as' using cartesian coordinates).
dot product of two vectors (x1, y1) and (x2, y2) is
x1 * x2 + y1 * y2
and is equivilent to the product of the lengths of the two vectors times the cosine of the angle between them.
So if you normalize the two vectors first (divide the coordinates by the length)
Where length of V1 L1 = sqrt(x1^2 + y1^2),
and length of V2 L2 = sqrt(x2^2 + y2^2),
Then normalized vectors are
(x1/L1, y1/L1), and (x2/L2, y2/L2),
And dot product of normalized vectors (which is the same as the cosine of angle between the vectors) would be
(x1*x2 + y1*y2)
-----------------
(L1*L2)
of course this may be just as computationally difficult as calculating the cosine
if you need to compute the square root, then consider using the invsqrt hack.
acos((x1*x2 + y1*y2) * invsqrt((x1*x1+y1*y1)*(x2*x2+y2*y2)));
The dot product might work in your case. It's not proportional to the angle, but "related".