Many 3D orientation representations:
Rotation matrix ()
Need numbers Straightfoward computation
Axis-angle ( / )
Singularities Can also represent angular velocity
Quaternions ()
Euler angles ()
Singularities Human understandable
Possible conversion
If a particle rotates around an axis , its velocity is given by :
Here, is an angular velocity. is an unit vector in the direction of the rotation and is the velocity of rotation about that axis.
The cross product is given by:
Which can be rewritten as a matrix product:
The motion of is then given by the first order differential equation:
Which have as solution:
matrix exponential?
Exponential of a matrix is defined as Taylor expansion:
Thanks to some nice properties on *, we have an efficient formula to compute , it is called Rodrigues' formula
* Because
In , the matrix takes and rotates it about the axis.
This is actually a rotation matrix!
Example with Pinocchio
# Rotates 1 radian about z pin.exp3(np.array([0., 0., 1.])) # array([[ 0.54030231, -0.84147098, 0. ], # [ 0.84147098, 0.54030231, 0. ], # [ 0. , 0. , 1. ]])
Here, the input is and the output is .
Conversely, if we take , the logarithm of a rotation matrix, the result will be:
We then found the rotation axis from the matrix!
# 1 radian about z (as computed previously) R = np.array([[ 0.54030231, -0.84147098, 0. ], [ 0.84147098, 0.54030231, 0. ], [ 0. , 0. , 1. ]]) print(pin.log3(R)) # array([0., 0., 1.])
Here, the input is and the output is such that .
Back to the conversions...
If represents the orientation of some object that is currently rotating, we then have:
Because rotation affects equally all unit columns of .
This implies the following identity:
What if a rigid body is rotating and translating ?
We can sum the velocity caused by rotation ( is on the axis) with some linear velocity (which is the same for the whole body).
For another point , we have:
A twist is a 6-dimensional quantity packaging and :
needs a basis to be expressed in also needs a basis, but also a reference point.
We express a twist in a given frame, and expressed in its basis, being the (linear) velocity of its origin.
The linear velocity is then the velocity of the origin of the frame (imagine this point is "fixed" to the body)
In 2D, a rotation + a translation result in a rotation about a different axis
In 3D, the object can also translate along the rotation axis
As a result, the motion can be helicoidal, like a screw. That is why we refer to it as screw theory.
Suppose that we have , a twist expressed in . How to express it in , knowing ?
We now have a convenient way to change the frame of a twist: the adjoint map
It turns out that , where is the following matrix representation of the twist:
We can integrate twists with exponential as well:
Spatial algebra
pin.exp3
pin.log3
pin.exp6
pin.log6
If this DoF rotates by radians, how this will affect ?
First, express the twist in :
Then, express it in :
And integrate it:
We could re-write as
Thus:
We now show explicitly in the equation:
is actually known under the name of screw axis.
A screw axis is a twist that is "unitary". It needs to be multiplied by a scalar to becomes a twist .
For example, can be the twist of a motor rotating by 1 radian.
For a serial robot, forward kinematics is given by:
where:
In a "neutral" configuration, the twist is providing the twist that would create:
We need to account the fact that is in a different frame, because of the configuration of joints that are upper in the kinematics chain.
is what we call the Jacobian, it has dimension , where is the number of degrees of freedom.
It maps a velocity to a spatial twist .
is computed for a given configuration and is actually
The same way is expressed in a frame, so is . We can use the adjoint map to change 's frame.
Jacobian frames
Some typical frames where we want to express the Jacobian:
WORLD
BODY
LOCAL_WORLD_ALIGNED
One way to think of it is by taking the following approximation:
This is an approximation because would also be subject to change during the motion, which is not taken in account in this formula.
Computing target twist
What if we cant to find the twist needed to move the robot from to ?
We have We want
Then
And hence:
Once we have a target twist, we can compute , and we then need to solve:
Which is a linear equation, with naive solution:
Less naive:
--- For a given angular velocity $\omega$, we can note that: $R_{ab} [\omega] R_{ab}^T = [R_{ab} \omega]$ We can see the left hand term as: 1. Expressing a point from $a$ to $b$ 2. Computing its velocity 3. Taking the velocity back to $a$ This is then equivalent to changing $\omega$ from one basis to another.
Figure ?
T_ra = T_ra e^V_a
Singularities
Can also represent angular velocity