I am getting a quaternion from sensor data that is in the coordinate system Y=up, X=right, and Z= backwards.Mine is X=forward, Y=right, Z=up.
So OX=Y, OY=Z and OZ=-X.
I have a function that can convert quaternions into 4by4 matrices, but no idea where to go from here. Any help would be greatly appreciated.
Quaternions in the form of [X, Y, Z, W] are equivalent to axis-angle rotations where W is dependent only on the angle of rotation (but not the axis) and X, Y, Z are the axis of rotation multiplied by sin(Angle/2). Since X, Y, Z have this property, you can just swap and negate them as you would to convert a 3D coordinate between. To convert from your sensor's coordinate system to yours, you can simply do this:
MyQuat.X = -SensorQuat.Z
MyQuat.Y = SensorQuat.X
MyQuat.Z = SensorQuat.Y
MyQuat.W = SensorQuat.W
Assume you have two coordinate systems F1 and F2. For simplicity, assume both have same origin. Now let,
qo_f1 = orientation of frame F1 as seen from frame F2
qo_f2 = orientation of frame F2 is as seen from F1
q_f1 = some quaternion in F1 frame
q_f2 = q_f1 as seen from F2
Then,
q_f2 = qo_f2 * q_f1 * qo_f2.inverse()
Explanation
To rotate anything by quaternion q you just do q*p*q.inverse(). If p is a vector then you first convert it to "fake" quaternion by setting w=0 and x,y,z same as vector. If p is quaternion then you are good to go.
You just Need to Multiply it by q, which is the quaternion that converts points from Coord2 to the Coord1 equivalent.
Explanation:
q2 * point2 * (q2)^(-1) = (q1 * (q^(-1))) * point2 * (q1 * (q^(-1)))^(-1)
= q1 * (q^(-1)) * point2 * q * (q1^(-1)) = q1 * ((q^(-1)) * point2 * q) * (q1^(-1))
= q1 * point1_equivalent * (q1^(-1))
Related
I had attached also the schematic to depict my question.
I need to rotate the vector V with the base point P by an angle and find the new vector V'.
The rotation axis is say for is about a local y axis at point P (which is parallel to global Y axis)
Subsequently, I need to rotate the initial vector V about x axis which is parallel to global Y axis.
The main reason for the rotation is to find the new vector V' at point P. Both the rotations are independent and each of the rotation provides a new V'. I'm programming this in VB.net and output is a double() of new vector V'.
Just apply the two rotations independently (see Wikipedia). The base point does not play any role in this because it is just a constant offset that never changes. If I got your description right, you want the following:
//rotation about y-axis
iAfterRot1 = cos(phi1) * i + sin(phi1) * k
jAfterRot1 = j
kAfterRot1 = -sin(phi1) * i + cos(phi) * k
//rotation about x-axis
iAfterRot2 = iAfterRot1
jAfterRot2 = cos(phi2) * jAfterRot1 - sin(phi2) * kAfterRot1
kAfterRot2 = sin(phi2) * jAfterRot1 + cos(phi2) * kAfterRot1
I have a green vehicle which will shortly collide with a blue object (which is 200 away from the cube)
It has a Kinect depth camera D at [-100,0,200] which sees the corner of the cube (grey sphere)
The measured depth is 464 at 6.34° in the X plane and 12.53° in the Y plane.
I want to calculate the position of the corner as it would appear if there was a camera F at [150,0,0], which would see this:
in other words transform the red vector into the yellow vector. I know that this is achieved with a transformation matrix but I can't find out how to compute the matrix from the D-F vector [250,0,-200] or how to use it; my high-school maths dates back 40 years.
math.se has a similar question but it doesn't cover my problem and I can't find anything on robotices.se either.
I realise that I should show some code that I've tried, but I don't know where to start. I would be very grateful if somebody could help me to solve this.
ROS provides the tf library which allows you to transform between frames. You can simply set a static transform between the pose of your camera and the pose of your desired location. Then, you can get the pose of any point detected by your camera in the reference frame of your desired point on your robot. ROS tf will do everything you need and everything I explain below.
The longer answer is that you need to construct a transformation tree. First, compute the static transformation between your two poses. A pose is a 7-dimensional transformation including a translation and orientation. This is best represented as a quaternion and a 3D vector.
Now, for all poses in the reference frame of your kinect, you must transform them to your desired reference frame. Let's call this frame base_link and your camera frame camera_link.
I'm going to go ahead and decide that base_link is the parent of camera_link. Technically these transformations are bidirectional, but because you may need a transformation tree, and because ROS cares about this, you'll want to decide who is the parent.
To convert rotation from camera_link to base_link, you need to compute the rotational difference. This can be done by multiplying the quaternion of base_link's orientation by the conjugate of camera_link's orientation. Here's a super quick Python example:
def rotDiff(self,q1: Quaternion,q2: Quaternion) -> Quaternion:
"""Finds the quaternion that, when applied to q1, will rotate an element to q2"""
conjugate = Quaternion(q2.qx*-1,q2.qy*-1,q2.qz*-1,q2.qw)
return self.rotAdd(q1,conjugate)
def rotAdd(self, q1: Quaternion, q2: Quaternion) -> Quaternion:
"""Finds the quaternion that is the equivalent to the rotation caused by both input quaternions applied sequentially."""
w1 = q1.qw
w2 = q2.qw
x1 = q1.qx
x2 = q2.qx
y1 = q1.qy
y2 = q2.qy
z1 = q1.qz
z2 = q2.qz
w = w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2
x = w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2
y = w1 * y2 + y1 * w2 + z1 * x2 - x1 * z2
z = w1 * z2 + z1 * w2 + x1 * y2 - y1 * x2
return Quaternion(x,y,z,w)
Next, you need to add the vectors. The naive approach is to simply add the vectors, but you need to account for rotation when calculating these. What you really need is a coordinate transformation. The position of camera_link relative to base_link is some 3D vector. Based on your drawing, this is [-250, 0, 200]. Next, we need to reproject the vectors to your points of interest into the rotational frame of base_link. I.e., all the points your camera sees at 12.53 degrees that appear at the z = 0 plane to your camera are actually on a 12.53 degree plane relative to base_link and you need to find out what their coordinates are relative to your camera as if your camera was in the same orientation as base_link.
For details on the ensuing math, read this PDF (particularly starting at page 9).
To accomplish this, we need to find your vector's components in base_link's reference frame. I find that it's easiest to read if you convert the quaternion to a rotation matrix, but there is an equivalent direct approach.
To convert a quaternion to a rotation matrix:
def Quat2Mat(self, q: Quaternion) -> rotMat:
m00 = 1 - 2 * q.qy**2 - 2 * q.qz**2
m01 = 2 * q.qx * q.qy - 2 * q.qz * q.qw
m02 = 2 * q.qx * q.qz + 2 * q.qy * q.qw
m10 = 2 * q.qx * q.qy + 2 * q.qz * q.qw
m11 = 1 - 2 * q.qx**2 - 2 * q.qz**2
m12 = 2 * q.qy * q.qz - 2 * q.qx * q.qw
m20 = 2 * q.qx * q.qz - 2 * q.qy * q.qw
m21 = 2 * q.qy * q.qz + 2 * q.qx * q.qw
m22 = 1 - 2 * q.qx**2 - 2 * q.qy**2
result = [[m00,m01,m02],[m10,m11,m12],[m20,m21,m22]]
return result
Now that your rotation is represented as a rotation matrix, it's time to do the final calculation.
Following the MIT lecture notes from my link above, I'll arbitrarily name the vector to your point of interest from the camera A.
Find the rotation matrix that corresponds with the quaternion that represents the rotation between base_link and camera_link and simply perform a matrix multiplication. If you're in Python, you can use numpy to do this, but in the interest of being explicit, here is the long form of the multiplication:
def coordTransform(self, M: RotMat, A: Vector) -> Vector:
"""
M is my rotation matrix that represents the rotation between my frames
A is the vector of interest in the frame I'm rotating from
APrime is A, but in the frame I'm rotating to.
"""
APrime = []
i = 0
for component in A:
APrime.append(component * M[i][0] + component * M[i][1] + component * m[i][2])
i += 1
return APrime
Now, the vectors from camera_link are represented as if camera_link and base_link share an orientation.
Now you may simply add the static translation between camera_link and base_link (or subtract base_link -> camera_link) and the resulting vector will be your point's new translation.
Putting it all together, you can now gather the translation and orientation of every point your camera detects relative to any arbitrary reference frame to gather pose data relevant to your application.
You can put all of this together into a function simply called tf() and stack these transformations up and down a complex transformation tree. Simply add all the transformations up to a common ancestor and subtract all the transformations down to your target node in order to find the transformation of your data between any two arbitrary related frames.
Edit: Hendy pointed out that it's unclear what Quaternion() class I refer to here.
For the purposes of this answer, this is all that's necessary:
class Quaternion():
def __init__(self, qx: float, qy: float, qz: float, qw: float):
self.qx = qx
self.qy = qy
self.xz = qz
self.qw = qw
But if you want to make this class super handy, you can define __mul__(self, other: Quaternion and __rmul__(self, other: Quaternion) to perform quaternion multiplication (order matters, so make sure to do both!). conjugate(self), toEuler(self), toRotMat(self), normalize(self) may also be handy additions.
Note that due to quirks in Python's typing, the above other: Quaternion is only for clarity. You'll need a longer-form if type(other) != Quaternion: raise TypeError('You can only multiply quaternions with other quaternions) error handling block to make that into valid python :)
The following definitions are not necessary for this answer, but they may prove useful to the reader.
import numpy as np
def __mul__(self, other):
if type(other) != Quaternion:
print("Quaternion multiplication only works with other quats")
raise TypeError
r1 = self.qw
r2 = other.qw
v1 = [self.qx,self.qy,self.qz]
v2 = [other.qx,other.qy,other.qz]
rPrime = r1*r2 - np.dot(v1,v2)
vPrimeA = np.multiply(r1,v2)
vPrimeB = np.multiply(r2,v1)
vPrimeC = np.cross(v1,v2)
vPrimeD = np.add(vPrimeA, vPrimeB)
vPrime = np.add(vPrimeD,vPrimeC)
x = vPrime[0]
y = vPrime[1]
z = vPrime[2]
w = rPrime
return Quaternion(x,y,z,w)
def __rmul__(self, other):
if type(other) != Quaternion:
print("Quaternion multiplication only works with other quats")
raise TypeError
r1 = other.qw
r2 = self.qw
v1 = [other.qx,other.qy,other.qz]
v2 = [self.qx,self.qy,self.qz]
rPrime = r1*r2 - np.dot(v1,v2)
vPrimeA = np.multiply(r1,v2)
vPrimeB = np.multiply(r2,v1)
vPrimeC = np.cross(v1,v2)
vPrimeD = np.add(vPrimeA, vPrimeB)
vPrime = np.add(vPrimeD,vPrimeC)
x = vPrime[0]
y = vPrime[1]
z = vPrime[2]
w = rPrime
return Quaternion(x,y,z,w)
def conjugate(self):
return Quaternion(self.qx*-1,self.qy*-1,self.qz*-1,self.qw)
I have a straight line in space with an start and end point (x,y,z) and I am attempting to get the angle between this vector and the plane defined by z=0. I am using VB.NET
Here is a picture of the line in my 3d environment (the line I'm intersted in is circled in red) :
It is set to an angle of 70 degrees right now.
You need 2 rays to define an angle.
If you want the angle between a vector and a plane, it is defined for any vector in that plane. However, there is only one minimal value for that, which is the angle between a vector and its projection onto said plane.
Therefore, that minimal value is the one we take when we speak of the angle between a vector and a plane.
This value is also π/2 - the angle between your vector and the the vector that is normal to the plane.You can read more about it all on this site.
With v your vector (thus v.x = end.x - start.x and idem for y and z), n the normal to the plane and a the angle you are looking for, we know from the definition of a scalar product that:
<v,n> = ||v|| * ||n|| * cos(π/2 - a)
We know cos(π/2 - a) = sin(a), and the normal to the z=0 plane is simply the vector n = (0, 0, 1). Thus both the scalar product, v.x * n.x + v.y * n.y + v.z * n.z, and the norm of n, ||n|| = 1, can be simplified a lot. We get the following expression:
sin(a) = v.z / ||v||
Thus finally, the formula by taking the reciprocical of the sine and expliciting the norm of v:
a = Asin(v.z / sqrt( v.x*v.x + v.y*v.y + v.z*v.z ))
According to this documentation the Asin function exists in your System.Math class. It does, however, return the value in radians:
Return Value
Type: System.Double
An angle, θ, measured in radians, such that -π/2 ≤ θ ≤ π/2
-or-
NaN if d < -1 or d > 1 or d equals NaN.
Luckily the same System.Math class contains the value of π so that you can do the conversion:
a *= 180 / Math.PI
I have solid object that is spinning with a torque W, and I want to calculate the force F applied on a certain point that's D units away from the center of the object. All these values are represented in Vector3 format (x, y, z)
I know until now that W = D x F, where x is the cross product, so by expanding this I get:
Wx = Dy*Fz - Dz*Fy
Wy = Dz*Fx - Dx*Fz
Wz = Dx*Fy - Dy*Fx
So I have this equation, and I need to find (Fx, Fy, Fz), and I'm thinking of using the Simplex method to solve it.
Since the F vector can also have negative values, I split each F variable into 2 (F = G-H), so the new equation looks like this:
Wx = Dy*Gz - Dy*Hz - Dz*Gy + Dz*Hy
Wy = Dz*Gx - Dz*Hx - Dx*Gz + Dx*Hz
Wz = Dx*Gy - Dx*Hy - Dy*Gx + Dy*Hx
Next, I define the simplex table (we need <= inequalities, so I duplicate each equation and multiply it by -1.
Also, I define the objective function as: minimize (Gx - Hx + Gy - Hy + Gz - Hz).
The table looks like this:
Gx Hx Gy Hy Gz Hz <= RHS
============================================================
0 0 -Dz Dz Dy -Dy <= Wx = Gx
0 0 Dz -Dz -Dy Dy <= -Wx = Hx
Dz -Dz 0 0 Dx -Dx <= Wy = Gy
-Dz Dz 0 0 -Dx Dx <= -Wy = Hy
-Dy Dy Dx -Dx 0 0 <= Wz = Gz
Dy -Dy -Dx Dx 0 0 <= -Wz = Hz
============================================================
1 -1 1 -1 1 -1 0 = Z
The problem is that when I run it through an online solver I get Unbounded solution.
Can anyone please point me to what I'm doing wrong ?
Thanks in advance.
edit: I'm sure I messed up some signs somewhere (for example the Z should be defined as a max), but I'm sure I'm wrong when defining something more important.
There exists no unique solution to the problem as posed. You can only solve for the tangential projection of the force. This comes from the properties of the vector (cross) product - it is zero for collinear vectors and in particular for the vector product of a vector by itself. Therefore, if F is a solution of W = r x F, then F' = F + kr is also a solution for any k:
r x F' = r x (F + kr) = r x F + k (r x r) = r x F
since the r x r term is zero by the definition of vector product. Therefore, there is not a single solution but rather a whole linear space of vectors that are solutions.
If you restrict the solution to forces that have zero projection in the direction of r, then you could simply take the vector product of W and r:
W x r = (r x F) x r = -[r x (r x F)] = -[(r . F)r - (r . r)F] = |r|2F
with the first term of the expansion being zero because the projection of F onto r is zero (the dot denotes scalar (inner) product). Therefore:
F = (W x r) / |r|2
If you are also given the magnitude of F, i.e. |F|, then you can compute the radial component (if any) but there are still two possible solutions with radial components in opposing directions.
Quick dirty derivation...
Given D and F, you get W perpendicular to them. That's what a cross product does.
But you have W and D and need to find F. This is a bad assumption, but let's assume F was perpendicular to D. Call it Fp, since it's not necessarily the same as F. Ignoring magnitudes, WxD should give you the direction of Fp.
This ignoring magnitudes, so fix that with a little arithmetic. Starting with W=DxF applied to Fp:
mag(W) = mag(D)*mag(Fp) (ignoring geometry; using Fp perp to D)
mag(Fp) = mag(W)/mag(D)
Combining the cross product bit for direction with this stuff for magnitude,
Fp = WxD / mag(WxD) * mag(Fp)
Fp = WxD /mag(W) /mag(D) *mag(W) /mag(D)
= WxD / mag(D)^2.
Note that given any solution Fp to W=DxF, you can add any vector proportional to D to Fp to obtain another solution F. That is a totally free parameter to choose as you like.
Note also that if the torque applies to some sort of axle or object constrained to rotate about some axis, and F is applied to some oddball lever sticking out at a funny angle, then vector D points in some funny direction. You want to replace D with just the part perpendicular to the axle/axis, otherwise the "/mag(D)" part will be wrong.
So from your comment is clear that all rotations are spinning around center of gravity
in that case
F=M/r
F force [N]
M torque [N/m]
r scalar distance between center of rotation [m]
this way you know the scalar size of your Force
now you need the direction
it is perpendicular to rotation axis
and it is the tangent of the rotation in that point
dir=r x axis
F = F * dir / |dir|
bolds are vectors rest is scalar
x is cross product
dir is force direction
axis is rotation axis direction
now just change the direction according to rotation direction (signum of actual omega)
also depending on your coordinate system setup
so ether negate F or not
but this is in 3D free rotation very unprobable scenario
the object had to by symmetrical from mass point of view
or initial driving forces was applied in manner to achieve this
also beware that after first hit with any interaction Force this will not be true !!!
so if you want just to compute Force it generate on certain point if collision occurs is this fine
but immediately after this your spinning will change
and for non symmetric objects the spinning will be most likely off the center of gravity !!!
if your object will be disintegrated then you do not need to worry
if not then you have to apply rotation and movement dynamics
Rotation Dynamics
M=alpha*I
M torque [N/m]
alpha angular acceleration
I quadratic mass inertia for actual rotation axis [kg.m^2]
epislon''=omega'=alpha
' means derivation by time
omega angular speed
epsilon angle
I have two vector pairs (before and after rotation).
before rotation:
[x1,y1,z1]
[x2,y2,z2]
after rotation:
[x1',y1',z1']
[x2',y2',z2']
How to create a quaternion representing this rotation?
In most cases there is no rotation which transforms 2 vectors into 2 other vectors. Here is a simple way to visualize why: a rotation does not change the angle between vectors. If the angle between the 2 vectors before the rotation is different from the angle between the 2 vectors after the rotation, then there is no rotation which meets your criteria.
This said there may be an optimal quaternion with an acceptable error which "almost" rotates your 2 vector pairs. There are a number of algorithms which vary in speed and precision to find such a quaternion. I wrote a fast C++ algorithm for an Arduino application where the speed is critical but the precision is less important.
http://robokitchen.tumblr.com/post/67060392720/finding-a-rotation-quaternion-from-two-pairs-of-vectors
Before rotation: u0, v0. After rotation: u2, v2.
Quaternion q2 = Quaternion::fromTwoVectors(u0, u2);
Vector v1 = v2.rotate(q2.conjugate());
Vector v0_proj = v0.projectPlane(u0);
Vector v1_proj = v1.projectPlane(u0);
Quaternion q1 = Quaternion::fromTwoVectors(v0_proj, v1_proj);
return (q2 * q1).normalized();
If this does not meet the requirements of your own application try to google Wabha's problem.
Well, first you can find the rotation axis using vector-multiplication (cross-multiplication):
axis = v1 x v2;
Then you can compute the rotation angle:
sinA = |axis| / |v1|*|v2|
cosA = v1 . v2 / |v1|*|v2|
Here | | - is vector length operation, and . - is dot-multiplication
And finally, your quaternion is:
Q(w,x,y,z) = (cosA, axis.x * sinA, axis.y * sinA, axis.z * sinA)
I translated marcv81's very helpful blog post into Three.js:
const rotateVectorsSimultaneously = (u0, v0, u2, v2) => {
const q2 = new THREE.Quaternion().setFromUnitVectors(u0, u2);
const v1 = v2.clone().applyQuaternion(q2.clone().conjugate());
const v0_proj = v0.projectOnPlane(u0);
const v1_proj = v1.projectOnPlane(u0);
let angleInPlane = v0_proj.angleTo(v1_proj);
if (v1_proj.dot(new THREE.Vector3().crossVectors(u0, v0)) < 0) {
angleInPlane *= -1;
}
const q1 = new THREE.Quaternion().setFromAxisAngle(u0, angleInPlane);
const q = new THREE.Quaternion().multiplyQuaternions(q2, q1);
return q;
};
Because angleTo always returns a positive value, I manually flip the sign of the angle depending on which side of the u0-v0 plane v1 is on.
A mature solution to this problem is called Triad. Triad is one of the earliest and simplest solutions to the spacecraft attitude determination problem and is extremely efficient computationally.
With Triad, the idea is to replace your paired set of two vectors, with a paired set of three vectors, where the extra vector is generated with a cross-product. By normalizing the vectors, you can solve for a rotation matrix without a matrix inverse or an SVD (as is needed in more general instances of the problem -- see Wahba's Problem)
For full algorithm, see: https://en.wikipedia.org/wiki/Triad_method
You can then convert the solved rotation matrix from Triad to a rotation quaternion:
qw = √(1 + m00 + m11 + m22) /2
qx = (m21 - m12)/( 4 *qw)
qy = (m02 - m20)/( 4 *qw)
qz = (m10 - m01)/( 4 *qw)
In general to make the conversion to quaternion robust, you should consider looking at the matrix trace as discussed here: http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/
Finally, consider an alternative to Triad that directly computes the optimal quaternion called QUEST.
It is fine to find the quaternion from v1 to v2.
The final q = (cos A/2, sin A/2 * axis), where the A is the angle between v1 and v2, axis is the normed axis.
Multiply both side by 2 * cos A/2,
Then we have
2 * cos A/2 *q = (1+cos A, sin A * axis)
(where cos A = dot(v1, v2)/|v1|/|v2| and
axis = cross(v1, v2).normalize() = cross(v1, v2)/|v1|/|v2|/sin A.)
Then 2 * cos A/2 *q = (1+dot(v1, v2)/|v1|/|v2|, cross(v1, v2)/|v1|/|v2|)
Finally q = (1+dot(v1, v2)/|v1|/|v2|, cross(v1, v2)/|v1|/|v2|).normalize()