I have a 2DOF (z,y axes) stabilization system that needs to maintain the orientation of the end-effector.
Suppose the system is attached to a 3DOF rotating platform and has some fixed joints at the end of the rotation chain. We have no control over the 3DOF platform or the fixed joints, but we can measure the angles. I need to find the joint angles of the 2DOF stabilization system such that the end-effector $x$ vector matches the desired setpoint vector.
In this case, the position does not matter and the system only needs to stabilise the end effector's orientation. The forward kinematics of the end effector can be written as:
$$R_{plat} \underbrace{R_z(\psi) R_y(\theta)}_{\textrm{2DOF system}}R_{fixed} = R_{end} $$
Where $R_{end}$ is the end-effector rotation matrix. $R_{plat}$ is the rotation matrix of the 3DOF (z-y'-x'') platform. $R_{fixed}$ is the fixed z and y joints at the end of the rotation sequence with known angles. $R_z(\psi)$ and $R_y(\theta)$ are the rotations of the 2DOF stabilisation system.
Note that this is what I'm using:
$$ R_{z}(\psi )= \begin{bmatrix} \cos{\psi} & -sin{\psi} & 0 \\ sin{\psi} & cos{\psi} & 0 \\ 0 & 0 & 1 \end{bmatrix}$$
$$ R_{y}(\theta )= \begin{bmatrix} \cos{\theta}&0&\sin{\theta}\\0&1&0\\-sin{\theta}&0&\cos{\theta}\end{bmatrix}$$
$$ R_{x}(\phi )= \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos{\phi} & -sin{\phi} \\ 0 & \sin{\phi} & cos{\phi} \end{bmatrix}$$
I only need the $x$ vector of the LHS to match the RHS, therefore I need to solve for $\psi$ and $\theta$ such that the equation below holds true.
$$R_{plat} \underbrace{R_z(\psi) R_y(\theta)}_{\textrm{2DOF system}}R_{fixed}\begin{bmatrix}1 \\ 0 \\ 0 \end{bmatrix} = R_{end}\begin{bmatrix}1 \\ 0 \\ 0 \end{bmatrix} $$
How can I solve for $\psi$ and $\theta$?
I know this is possible as I am able to get the $x$ vectors to match by brute forcing (see image below for example of what I need to achieve).
Here's what I have tried and doesn't work
I tried rearranging the equation to solve for $R_z(\psi) R_y(\theta)$ like this:
$$R_z(\psi) R_y(\theta) = R_{plat}^{-1} R_{end} R_{fixed}^{-1}$$
However, this does not work as it required three joints to achieve the rotation of the RHS. If I solve for $\psi = \textrm{atan2}(R_{21}, R_{11})$ and $\theta = \sin^{-1}(-R_{31})$, the resulting rotation below is what I got, which is not quite right.
Thank you so much for taking your time to read and answer!!
Best Answer
You have the following equation:
$ R_{plat} R_z(\psi) R_y (\theta) R_{fixed} e_1 = R_{end} e_1 $
where $e_1 =[1, 0, 0]^T $
Premultiplying both sides by $R_{plat}^T $ , you get
$ R_z(\psi) R_y (\theta) R_{fixed} e_1 = R_{plat}^T R_{end} e_1 $
Let's introduce two vectors $v$ and $w$ as follows
$v = R_{fixed} e_1 $ and $ w = R_{plat}^T R_{end} e_1 $, then
$ R_z(\psi) R_y (\theta) v = w $
Finally, premultiply both sides by $R_z(\psi)^T $, then
$ R_y ( \theta) v = R_z(\psi)^T w \hspace{48pt}(*)$
Note that vectors $v$ and $w$ are unit vectors, but are otherwise arbitrary. We're now looking for $\theta$ and $\psi$, that will satisfy equation $(*)$. The left hand side is a rotation of vector $v$ about the $y$ axis while the right hand side is a rotation of $w$ about the $z$ axis. These two cones will intersect (and thus equation (*) will have a solution) if the acute angles $\phi_1$ and $\phi_2$ add up to a value that is greater than $90^\circ$, where $\phi_1$ is the acute angle between the $y$ axis and $v$, and $\phi_2$ is the acute angle between the $z$ axis and $w$. That is,
$\phi_1 = \cos^{-1} | v_y | $ and $\phi_2 =\cos^{-1} | w_z | $
If $\phi_1 + \phi_2 \gt \dfrac{\pi}{2} $ then we will have two solutions for $\psi$ and $\theta$. (The $\dfrac{\pi}{2}$ comes from the fact that the $y$ and $z$ are pependicular to each other).
Otherwise, no solutions are possible.
Assuming that the condition above is satisfied, then the intersection of the two cones, satisfies the property that,
$ k^T (R_y(\theta) v) = \cos(\phi_2) $
Where $k$ is the unit vector along the $z$ axis. This equation is a scalar trigonometric equation and can be solved for $\theta$. There will be two solutions, and for each, the corresponding $\psi$ can be computed from equation $(*)$.
To see how to solve this equation, recall that
$R_y(\theta) = \begin{bmatrix} \cos \theta && 0 && \sin \theta \\ 0 && 1 && 0 \\ -\sin \theta && 0 && \cos \theta \end{bmatrix} $
As a sum of matrices, $R_y(\theta) $ is equal to
$R_y(\theta) = A_0 + A_1 \cos \theta + A_2 \sin \theta $
where
$A_0 = \begin{bmatrix} 0 && 0 && 0 \\ 0 && 1 && 0 \\ 0 && 0 && 0 \end{bmatrix}$
$A_1 = \begin{bmatrix} 1 && 0 && 0 \\ 0 && 0 && 0 \\ 0 && 0 && 1 \end{bmatrix}$
$A_2 = \begin{bmatrix} 0 && 0 && 1 \\ 0 && 0 && 0 \\ -1 && 0 && 0 \end{bmatrix} $
So now our equation becomes:
$ k^T (A_0 + A_1 \cos \theta + A_2 \sin \theta ) v = \cos(\phi_2) $
which simplifies to
$ (k^T A_1 v) \cos \theta + (k^T A_2 v) \sin \theta = \cos(\phi_2) - k^T A_0 v $
Note that $k^T A_0 = [0,0,0] $ and $k^T A_1 = [0, 0, 1] $ and $k^T A_2 = [-1, 0, 0] $
Thus the equation simplifies further to
$v_z \cos \theta - v_x \sin \theta = \cos(\phi_2) = | w_z | $
which is very easy to solve.