To control a robotic arm, I have a controller with 6 dimensions (x, y, z position, and roll, pitch, yaw rotation). I am using a position (x, y, z) and quaternion (x, y, z, w) to represent the desired position and orientation of the robot's gripper.
I am using some libraries that people might not be familiar with (namely, geometry_msgs from ROS), so here is what I'm doing in pseudocode:
while(true):
new_position = last_position + (joy.x, joy.y, joy.z)
new_quaternion = last_quaternion * quaternionFromRPY(joy.roll, joy.pitch, joy.yaw)
// (Compute inverse kinematics using position and orientation, then send to robot arm)
last_position = new_position
last_quaternion = new_quaternion
delay(dt)
I am able to set the x, y, and z of the position part of the pose just fine. Technically, I am also able to set the quaternion just fine as well, but my problem is that controlling the quaternion is extremely non-intuitive, since rotation is not commutative. i.e., if I rotate 180 degrees about the x-axis, then the control of the rotation about the z-axis gets inverted.
Is there any way that I can control the rotation from a sort of "global reference frame," if that makes any sense? I would like it so that if I twist the joystick clockwise about z, the pose will rotate accordingly, instead of "sometimes rotating clockwise, sometimes rotating counterclockwise."
Isn't this just as simple as exchanging the order of the factors in the quaternion product?
A unit quaternion q transforms a vector v in local coordinates to the rotated vector q*v*q' in global coordinates. A modified quaternion a*q*b (a, b also unit quaternions) transforms v as
a*(q*(b*v*b')*q')*a',
where the b part can be interpreted as rotation in local coordinates and the a part as rotation in global coordinates.
So to apply a rotation in global coordinates only, set b=1, i.e., leave it out, and put the desired rotation in the a factor.
Related
I'm working with quaternions and I have a little problem.
I have an object (The violet line) and a quaternion relative to the rotation axis (Black line), and I want to convert this quaternion in local space, so that the rotation axis become the object.
I need to calculate the roll of the object (Which will be the Y rotation in local space that I will convert to axis angle) and then calculate the X and Z rotation in Axis Angle.
Here is a scheme that I drew to a better understanding of the question :
To understand you can think of your shoulder, when you move your arm you have the X and Z which will determine the forearm position and the Y which will determine the rotation of your elbow.
Do not hesitate to ask for clarifications, since it can be hard to understand what I'm searching for.
Is there a formula or an algorithm that I can use to do so ?
Is there 3D programmers who worked with quaternions and who can clarify me on the subject with an algorithm or just words?
You are looking for a quaternion q, such that qjq'=n, where n is the imaginary unit quaternion representing the axis of the object. This has a standard solution in terms of the product jn, essentially the square root.
If
jn=c+s*e, e imaginary unit, c²+s²=1, s>=0
then
q = sqrt(0.5*(1+c)) + sqrt(0.5*(1-c))*e
so compute
p=j*n // condition is n is imaginary unit
c=real(p)
e=imag(p)
s=abs(e)
if(s>0) e=e/s else e=j
s=sqrt(0.5*(1-c))
c=sqrt(0.5*(1+c))
q=c+s*e
See also https://stackoverflow.com/a/23414774/3088138
I use quaternions for rotations in OpenGL engine.Currently , in order to create rotation matrix for x ,y and z rotations I create a quaternion per axis rotation.Then I multiply these to get the final quaternion:
void RotateTo3(const float xr ,const float yr ,const float zr){
quat qRotX=angleAxis(xr, X_AXIS);
quat qRotY=angleAxis(yr, Y_AXIS);
quat qRotZ=angleAxis(zr, Z_AXIS);
quat resQuat=normalize(qRotX * qRotY * qRotZ);
resQuat=normalize(resQuat);
_rotMatrix= mat4_cast(resQuat);
}
Now it's all good but I want to create only one quaternion from all 3 axis angles and skip the final multiplication.One of the quat constructors has params for euler angles vector which goes like this:
quat resQuat(vec3(yr,xr,zr))
So if I try this the final rotation is wrong.(Also tried quat(vec3(xr,yr,zr)) ) .Isn't there a way in GLM to fill the final quaternion from all 3 axis in one instance ?
Now , one more thing:
As Nicol Bolas suggested , I could use glm::eulerAngleYXZ() to fill a rotation matrix right away as by his opinion it is pointless to do the intermediate quaternion step.. But what I found is that the function doesn't work properly , at least for me .For example :
This :
mat4 ex= eulerAngleX(radians(xr));
mat4 ey= eulerAngleY(radians(yr));
mat4 ez= eulerAngleZ(radians(zr));
rotMatrix= ex * ey * ez;
Doesn't return the same as this :
rotMatrix= eulerAngleYXZ(radians(yr),radians(xr),radians(zr));
And from my comparisons to the correct rotation state ,the first way gives the correct rotations while the second wrong .
Contrary to popular belief, quaternions are not magical "solve the Gimbal lock" devices, such that any uses of quaternions make Euler angles somehow not Euler angles.
Your RotateTo3 function takes 3 Euler angles and converts them into a rotation matrix. It doesn't matter how you perform this process; whether you use 3 matrices, 3 quaternions or glm::eulerAngleYXZ. The result will still be a matrix composed from 3 axial rotations. It will have all of the properties and failings of Euler angles. Because it is Euler angles.
Using quaternions as intermediaries here is pointless. It gains you nothing; you may as well just use matrices built from successive glm::rotate calls.
If you want to do orientation without Gimbal lock or the other Euler angle problems, then you need to stop representing your orientation as Euler angles.
In answer to the question you actually asked, you can use glm::eulerAngleYXZ to compute
Do you mean something like this:
quat formQuaternion(double x, double y, double z, double angle){
quat out;
//x, y, and z form a normalized vector which is now the axis of rotation.
out.w = cosf( fAngle/2)
out.x = x * sinf( fAngle/2 )
out.y = y * sinf( fAngle/2 )
out.z = z * sinf( fAngle/2 )
return out;
}
Sorry I don't actually know the quat class you are using, but it should still have some way to set the 4 dimensions. Source: Quaternion tutorial
eulerAngleYXZ gives one possible set of euler angles which, if recombined in the order indicated by the api name, will yield the same orientation as the given quaternion.
It's not a wrong result - it's one of several correct results.
Use a quaternion to store your orientation internally - to rotate it, multiply your orientation quat by another quat representing the amount to rotate by, which can be built from angle/axis to achieve what you want.
I have a start point in 3D coordinates, e.g. (0,0,0).
I have the direction I am pointing, represented by three angles - one for each angle of rotation (rotation in X, rotation in Y, rotation in Z) (for the sake of the example let's assume I'm one of those old logo turtles with a pen) and the distance I will travel in the direction I am pointing.
How would I go about calculating the end point coordinates?
I know for a 2D system it would be simple:
new_x = old_x + cos(angle) * distance
new_y = old_y + sin(angle) * distance
but I can't work out how to apply this to 3 dimensions
I suppose another way of thinking about this would be trying to find a point on the surface of a sphere, knowing the direction you're pointing and the sphere's radius.
First of all, for positioning a point in 3D you only need two angles (just like you only needed one in 2D)
Secondly, for various reasons (slow cos&sin, gimbal lock, ...) you might want to store the direction as a vector in the first place and avoid angles alltogether.
Anyway, Assuming direction is initially z aligned, then rotated around x axis followed by rotation around y axis.
x=x0 + distance * cos (angleZ) * sin (angleY)
Y=y0 + distance * sin (Anglez)
Z=z0 + distance * cos (angleZ) * cos (angleY)
Based in the three angles you have to construct the 3x3 rotation matrix. Then each column of the matrix represents the local x, y and z directions. If you have a local direction you want to move by, then multiply the 3x3 rotation with the direction vector to get the result in global coordinates.
I made a little intro to 3D coordinate transformations that I think will answer your question.
3D Coordinates
First, it is strange to have three angles to represent the direction -- two would be enough. Second, the result depends on the order in which you turn about the respective axes. Rotations about different axes do not commute.
Possibly you are simply looking for the conversion between spherical and Cartesian coordinates.
I need to calculate the 2 angles (yaw and pitch) for a 3D object to face an arbitrary 3D point. These rotations are known as "Euler" rotations simply because after the first rotation, (lets say Z, based on the picture below) the Y axis also rotates with the object.
This is the code I'm using but its not working fully. When on the ground plane (Y = 0) the object correctly rotates to face the point, but as soon as I move the point upwards in Y, the rotations don't look correct.
// x, y, z represent a fractional value between -[1] and [1]
// a "unit vector" of the point I need to rotate towards
yaw = Math.atan2( y, x )
pitch = Math.atan2( z, Math.sqrt( x * x + y * y ) )
Do you know how to calculate the 2 Euler angles given a point?
The picture below shows the way I rotate. These are the angles I need to calculate.
(The only difference is I'm rotating the object in the order X,Y,Z and not Z,Y,X)
This is my system.
coordinate system is x = to the right, y = downwards, z = further back
an object is by default at (0,0,1) which is facing backward
rotations are in the order X, Y, Z where rotation upon X is pitch, Y is yaw and Z is roll
Here are my working assumptions:
The coordinate system (x,y,z) is such that positive x is to the right, positive y is down, and z is the remaining direction. In particular, y=0 is the ground plane.
An object at (0,0,0) currently facing towards (0,0,1) is being turned to face towards (x,y,z).
In order to accomplish this, there will be a rotation about the x-axis followed by one around the y-axis. Finally, there is a rotation about the z-axis in order to have things upright.
(The terminology yaw, pitch, and roll can be confusing, so I'd like to avoid using it, but roughly speaking the correspondence is x=pitch, y=yaw, z=roll.)
Here is my attempt to solve your problem given this setup:
rotx = Math.atan2( y, z )
roty = Math.atan2( x * Math.cos(rotx), z )
rotz = Math.atan2( Math.cos(rotx), Math.sin(rotx) * Math.sin(roty) )
Hopefully this is correct up to signs. I think the easiest way to fix the signs is by trial and error. Indeed, you appear to have gotten the signs on rotx and roty correct -- including a subtle issue with regards to z -- so you only need to fix the sign on rotz.
I expect this to be nontrivial (possibly depending on which octant you're in), but please try a few possibilities before saying it's wrong. Good luck!
Here is the code that finally worked for me.
I noticed a "flip" effect that occurred when the object moved from any front quadrant (positive Z) to any back quadrant. In the front quadrants the front of the object would always face the point. In the back quadrants the back of the object always faces the point.
This code corrects the flip effect so the front of the object always faces the point. I encountered it through trial-and-error so I don't really know what's happening!
rotx = Math.atan2( y, z );
if (z >= 0) {
roty = -Math.atan2( x * Math.cos(rotx), z );
}else{
roty = Math.atan2( x * Math.cos(rotx), -z );
}
Rich Seller's answer shows you how to rotate a point from one 3-D coordinate system to another system, given a set of Euler angles describing the rotation between the two coordinate systems.
But it sounds like you're asking for something different:
You have: 3-D coordinates of a single point
You want: a set of Euler angles
If that's what you're asking for, you don't have enough information. To find the Euler angles,
you'd need coordinates of at least two points, in both coordinate systems, to determine the rotation from one coordinate system into the other.
You should also be aware that Euler angles can be ambiguous: Rich's answer assumes the
rotations are applied to Z, then X', then Z', but that's not standardized. If you have to interoperate with some other code using Euler angles, you need to make sure you're using the same convention.
You might want to consider using rotation matrices or quaternions instead of Euler angles.
This series of rotations will give you what you're asking for:
About X: 0
About Y: atan2(z, x)
About Z: atan2(y, sqrt(x*x + z*z))
I cannot tell you what these are in terms of "roll", "pitch" and "yaw" unless you first define how you are using these terms. You are not using them in the standard way.
EDIT:
All right, then try this:
About X: -atan2(y, z)
About Y: atan2(x, sqrt(y*y + z*z))
About Z: 0
Talking about the rotation of axes, I think step 3 should have been the rotation of X'-, Y''-, and Z'-axes about the Y''-axis.
I'm trying to make a triangle (isosceles triangle) to move around the screen and at the same time slightly rotate it when a user presses a directional key (like right or left).
I would like the nose (top point) of the triangle to lead the triangle at all times. (Like that old asteroids game).
My problem is with the maths behind this. At every X time interval, I want the triangle to move in "some direction", I need help finding this direction (x and y increments/decrements).
I can find the center point (Centroid) of the triangle, and I have the top most x an y points, so I have a line vector to work with, but not a clue as to "how" to work with it.
I think it has something to do with the old Sin and Cos methods and the amount (angle) that the triangle has been rotated, but I'm a bit rusty on that stuff.
Any help is greatly appreciated.
The arctangent (inverse tangent) of vy/vx, where vx and vy are the components of your (centroid->tip) vector, gives you the angle the vector is facing.
The classical arctangent gives you an angle normalized to -90° < r < +90° degrees, however, so you have to add or subtract 90 degrees from the result depending on the sign of the result and the sign of vx.
Luckily, your standard library should proive an atan2() function that takes vx and vy seperately as parameters, and returns you an angle between 0° and 360°, or -180° and +180° degrees. It will also deal with the special case where vx=0, which would result in a division by zero if you were not careful.
See http://www.arctangent.net/atan.html or just search for "arctangent".
Edit: I've used degrees in my post for clarity, but Java and many other languages/libraries work in radians where 180° = π.
You can also just add vx and vy to the triangle's points to make it move in the "forward" direction, but make sure that the vector is normalized (vx² + vy² = 1), else the speed will depend on your triangle's size.
#Mark:
I've tried writing a primer on vectors, coordinates, points and angles in this answer box twice, but changed my mind on both occasions because it would take too long and I'm sure there are many tutorials out there explaining stuff better than I ever can.
Your centroid and "tip" coordinates are not vectors; that is to say, there is nothing to be gained from thinking of them as vectors.
The vector you want, vForward = pTip - pCentroid, can be calculated by subtracting the coordinates of the "tip" corner from the centroid point. The atan2() of this vector, i.e. atan2(tipY-centY, tipX-centX), gives you the angle your triangle is "facing".
As for what it's relative to, it doesn't matter. Your library will probably use the convention that the increasing X axis (---> the right/east direction on presumably all the 2D graphs you've seen) is 0° or 0π. The increasing Y (top, north) direction will correspond to 90° or (1/2)π.
It seems to me that you need to store the rotation angle of the triangle and possibly it's current speed.
x' = x + speed * cos(angle)
y' = y + speed * sin(angle)
Note that angle is in radians, not degrees!
Radians = Degrees * RadiansInACircle / DegreesInACircle
RadiansInACircle = 2 * Pi
DegressInACircle = 360
For the locations of the vertices, each is located at a certain distance and angle from the center. Add the current rotation angle before doing this calculation. It's the same math as for figuring the movement.
Here's some more:
Vectors represent displacement. Displacement, translation, movement or whatever you want to call it, is meaningless without a starting point, that's why I referred to the "forward" vector above as "from the centroid," and that's why the "centroid vector," the vector with the x/y components of the centroid point doesn't make sense. Those components give you the displacement of the centroid point from the origin. In other words, pOrigin + vCentroid = pCentroid. If you start from the 0 point, then add a vector representing the centroid point's displacement, you get the centroid point.
Note that:
vector + vector = vector
(addition of two displacements gives you a third, different displacement)
point + vector = point
(moving/displacing a point gives you another point)
point + point = ???
(adding two points doesn't make sense; however:)
point - point = vector
(the difference of two points is the displacement between them)
Now, these displacements can be thought of in (at least) two different ways. The one you're already familiar with is the rectangular (x, y) system, where the two components of a vector represent the displacement in the x and y directions, respectively. However, you can also use polar coordinates, (r, Θ). Here, Θ represents the direction of the displacement (in angles relative to an arbitary zero angle) and r, the distance.
Take the (1, 1) vector, for example. It represents a movement one unit to the right and one unit upwards in the coordinate system we're all used to seeing. The polar equivalent of this vector would be (1.414, 45°); the same movement, but represented as a "displacement of 1.414 units in the 45°-angle direction. (Again, using a convenient polar coordinate system where the East direction is 0° and angles increase counter-clockwise.)
The relationship between polar and rectangular coordinates are:
Θ = atan2(y, x)
r = sqrt(x²+y²) (now do you see where the right triangle comes in?)
and conversely,
x = r * cos(Θ)
y = r * sin(Θ)
Now, since a line segment drawn from your triangle's centroid to the "tip" corner would represent the direction your triangle is "facing," if we were to obtain a vector parallel to that line (e.g. vForward = pTip - pCentroid), that vector's Θ-coordinate would correspond to the angle that your triangle is facing.
Take the (1, 1) vector again. If this was vForward, then that would have meant that your "tip" point's x and y coordinates were both 1 more than those of your centroid. Let's say the centroid is on (10, 10). That puts the "tip" corner over at (11, 11). (Remember, pTip = pCentroid + vForward by adding "+ pCentroid" to both sides of the previous equation.) Now in which direction is this triangle facing? 45°, right? That's the Θ-coordinate of our (1, 1) vector!
keep the centroid at the origin. use the vector from the centroid to the nose as the direction vector. http://en.wikipedia.org/wiki/Coordinate_rotation#Two_dimensions will rotate this vector. construct the other two points from this vector. translate the three points to where they are on the screen and draw.
double v; // velocity
double theta; // direction of travel (angle)
double dt; // time elapsed
// To compute increments
double dx = v*dt*cos(theta);
double dy = v*dt*sin(theta);
// To compute position of the top of the triangle
double size; // distance between centroid and top
double top_x = x + size*cos(theta);
double top_y = y + size*sin(theta);
I can see that I need to apply the common 2d rotation formulas to my triangle to get my result, Im just having a little bit of trouble with the relationships between the different components here.
aib, stated that:
The arctangent (inverse tangent) of
vy/vx, where vx and vy are the
components of your (centroid->tip)
vector, gives you the angle the vector
is facing.
Is vx and vy the x and y coords of the centriod or the tip? I think Im getting confused as to the terminology of a "vector" here. I was under the impression that a Vector was just a point in 2d (in this case) space that represented direction.
So in this case, how is the vector of the centroid->tip calculated? Is it just the centriod?
meyahoocomlorenpechtel stated:
It seems to me that you need to store
the rotation angle of the triangle and
possibly it's current speed.
What is the rotation angle relative to? The origin of the triangle, or the game window itself? Also, for future rotations, is the angle the angle from the last rotation or the original position of the triangle?
Thanks all for the help so far, I really appreciate it!
you will want the topmost vertex to be the centroid in order to achieve the desired effect.
First, I would start with the centroid rather than calculate it. You know the position of the centroid and the angle of rotation of the triangle, I would use this to calculate the locations of the verticies. (I apologize in advance for any syntax errors, I have just started to dabble in Java.)
//starting point
double tip_x = 10;
double tip_y = 10;
should be
double center_x = 10;
double center_y = 10;
//triangle details
int width = 6; //base
int height = 9;
should be an array of 3 angle, distance pairs.
angle = rotation_angle + vertex[1].angle;
dist = vertex[1].distance;
p1_x = center_x + math.cos(angle) * dist;
p1_y = center_y - math.sin(angle) * dist;
// and the same for the other two points
Note that I am subtracting the Y distance. You're being tripped up by the fact that screen space is inverted. In our minds Y increases as you go up--but screen coordinates don't work that way.
The math is a lot simpler if you track things as position and rotation angle rather than deriving the rotation angle.
Also, in your final piece of code you're modifying the location by the rotation angle. The result will be that your ship turns by the rotation angle every update cycle. I think the objective is something like Asteroids, not a cat chasing it's tail!