I have a rigid body in the shape of a right angle with 3 reference points on it. For each pose of the rigid body I know the 3d coordinates of each of the 3 reference points.
I am trying to find the world view tait-bryan angles of the rigid body for each pose. I believe these are called pitch, yaw, and roll.
I am aware of this method, but I cant get it to work in python.
Heres what I want to try:
First I establish a default pose where the front point is at the origin and the angles are (0,0,0). For each pose I translate all of the coordinates so that the front point is at the origin just like the default pose. Then I treat the other two points as vectors. My goal is to match them up with their corresponding default pose vectors.
I would calculate the quaternion that gets me from the current back point to the default pose back point and apply that quaternion to both the current back and side point.
Next I would find the quaternion that takes me from the newly rotated side point to the default pose side point. The axis of rotation would be the back point vector.
Finally I would take these two quaternions, combine them somehow, then convert the resulting quaternion to tait-bryan angles.
Would this work?
This is not a complete answer, as it only deals with the quaternion part.
If you start with an initial orientation, described using say unit quaternion $\mathbf{q}_0$, you can indeed use quaternions $\mathbf{q}_1$, $\mathbf{q}_2$, and so on to describe the orientation of connected joints: this is exactly how most skeletal models are implemented. The orientation after joint $1$ is $\mathbf{q}_{01} = \mathbf{q}_1 \mathbf{q}_0$, and the orientation after joints $1$ and $2$ is $\mathbf{q}_{012} = \mathbf{q}_2 \mathbf{q}_1 \mathbf{q}_0 = \mathbf{q}_2 ( \mathbf{q}_1 \mathbf{q}_0 )$. Quaternion multiplication is done using the Hamilton product.
You can traverse the chain of joints "up" and "down" by multiplying quaternions. If you use floating-point numbers, you may wish to normalize the quaternions to unit length; it is safe to do and causes no bias.
Let's recap the properties of unit quaternions:
I shall use notation $\mathbf{q} = ( r, i, j, k )$ for an unit quaternion, $r^2 + i^2 + j^2 + k^2$.
The unit quaternion $\mathbf{q}_0 = (1, 0, 0, 0)$ represents no rotation, or original orientation. If $(x, y, z)$ is an unit vector, $x^2 + y^2 + z^2 = 1$, and $2\theta$ is a rotation around that vector, then $\mathbf{q} = (\cos\theta, x \sin\theta, y \sin\theta, z \sin\theta)$ is the unit quaternion describing that rotation. (This makes it very useful for things like knee and elbow joints, where the axis of rotation stays unchanged.)
It is safe to normalize a quaternion to unit length by dividing its components by original $\sqrt{r^2 + i^2 + j^2 + k^2}$; this does not affect the orientation described: $$\mathbf{q}_\text{new} = \frac{\mathbf{q}_\text{old}}{\left\lVert\mathbf{q}_\text{old}\right\rVert} \tag{1}\label{1}$$
Both $\mathbf{q}_+ = (r, i, j, k)$ and $\mathbf{q}_- = (-r, -i, -j, -k)$ describe the exact same orientation; but the rotation is in the opposite direction. This means that when interpolating between two orientations, you'll want to check that their $r$ components have the same sign, and if not, negate all components of one. If you have a function $w(t)$ that goes from $0$ to $1$, then the interpolation from $\mathbf{q}_0$ to $\mathbf{q}_1$ is just $$\mathbf{q} = \frac{\bigl(1 - w(t)\bigr)\mathbf{q}_0 + w(t)\mathbf{q}_1}{\left\lVert \bigl(1 - w(t)\bigr)\mathbf{q}_0 + w(t)\mathbf{q}_1 \right\rVert} \tag{2}\label{2}$$ For $0 \le t \le 1$, I personally prefer $w(t) = 3 t^2 - 2 t^3$, which starts and ends with zero angular velocity; i.e. smooth transition between two static orientations.
In a computer program, for $\eqref{2}$ I use $$\begin{aligned} \lambda_0 &= 1 - w(t) \\ \lambda_1 &= w(t) \\ r_u &= \lambda_0 r_0 + \lambda_1 r_1 \\ i_u &= \lambda_0 i_0 + \lambda_1 i_1 \\ j_u &= \lambda_0 j_0 + \lambda_1 j_1 \\ k_u &= \lambda_0 k_0 + \lambda_1 k_1 \\ n_u &= \sqrt{r_u^2 + i_u^2 + j_u^2 + k_u^2} \\ r &= \frac{r_u}{n_u} \\ i &= \frac{i_u}{n_u} \\ j &= \frac{j_u}{n_u} \\ k &= \frac{k_u}{n_u} \\ \end{aligned}$$
Interpolation is also extremely useful when you have a quaternion $\mathbf{q}_\tau$ representing angular velocity, or rotation occurring in an unit time interval. To find out the change in orientation $\mathbf{q}_0$ in time $\tau$, you simply use $w(t) = \tau$ in above, i.e. $\mathbf{q}_u = (\tau \mathbf{q}_\tau) \mathbf{q}_0$, and finally $\mathbf{q} = \mathbf{q}_u / \lVert\mathbf{q}_u\rVert$. I've used this to e.g. represent the rotation of a vessel in free fall. Each possible "thruster" then has their own quaternion (representing maximum thrust if applied to $\mathbf{q}_\tau$), and is similarly scaled and applied to $\mathbf{q}_\tau$ when the thruster is applied (but now the scalar $\tau$ representing the applied thruster power between $0$ and $1$).
When you have an orientation described by $\mathbf{q}_1$, and you wish to rotate it by $\mathbf{q}_2$ to obtain the combined rotation $\mathbf{q}_{12}$, you do $$\mathbf{q}_{12} = \mathbf{q}_2 \mathbf{q}_1 \tag{3a}\label{3a}$$ via Hamilton product: $$\left\lbrace \begin{aligned} r_{12} &= r_2 r_1 - i_2 i_1 - j_2 j_1 - k_2 k_1 \\ i_{12} &= r_2 i_1 + i_2 r_1 + j_2 k_1 - k_2 j_1 \\ j_{12} &= r_2 j_1 - i_2 k_1 + j_2 r_1 + k_2 i_1 \\ k_{12} &= r_2 k_1 + i_2 j_1 - j_2 i_1 + k_2 r_1 \\ \end{aligned} \right . \tag{3b}\label{3b}$$ Note that the original orientation (or first rotation) is rightmost, and the last rotation leftmost. In other words, when multiplying more than two quaternions, you apply the Hamilton products in the order they are to be applied oldest first, with rotation to be applied on the left ($\mathbf{q}_2$), and the orientation it is applied to on the right ($\mathbf{q}_1$).
To invert a rotation, you simply negate the $r$ component, or the $i$, $j$, and $k$ components. To invert a sequence of rotations, you do that to each of the unit quaternions, then multiply them together, but in the reverse order. For example, if $\mathbf{q}_1^-$ is the inverse of $\mathbf{q}_1$, and $\mathbf{q}_2^-$ is the inverse of $\mathbf{q}_2$, then $\mathbf{q}_{12}^- = \mathbf{q}_1^- \mathbf{q}_2^-$. Compare to $\eqref{3a}$: the order is also reversed.