Pose of the end-effector

Question

I am trying to get the pose of the end-effector. I have attached the picture. I am stuck. I wrote the transformation matrices - I am not sure how to move forward next. How do I find the end-effector pose?.

Answer 1

Looks like a 4-bar slider-crank system with a linear output. Generally, with these closed-chain kinematics, you need to compute the end-effector pose (point P) from two open chains with one open chain starting at link 1 to link 2 and the other open chain starting at link 3 to link 2. Then you need to solve for the generalized coordinates, which from your diagram appear to be $\phi_1$, $\phi_2$, $\phi_3$. $\phi_4$. Personally, I think this selection of generalized coordinates is redundant and incomplete. Redundant because $\phi_4$ can be derived from $\phi_1 + \phi_2 = \phi_3 + \phi_4$. Incomplete because you need one at least one more to describe the linear offset from the end of link 1 to the end of link 3, let's call this $a_L$. So, I think the generalized coordinates of the system should instead be $\phi_1, \phi_2, \phi_3$, and $a_L$,

For the sake of your diagram (and explanation), lets proceed with the fact that the following are unknown: $\phi_1, \phi_2, \phi_3, \phi_4$, and $a_L$.

If $\phi_1$ and $\phi_2$ are known, then the position $P$ from the first open loop chain is: $P_x = a_1 \cos(\phi_1) + a_2 \cos (\phi_2)$

$P_y = a_1 \sin(\phi_1) + a_2 \sin (\phi_2)$

If instead only $a_L, \phi_3,$ and $\phi_4$ are known, then the position $P$ from the second open loop chain is:

$P_x = a_3 \cos(\phi_3) + L \cos (\phi_4) + a_4$, where we add $a_4$ to express point $P$ in the same origin.

$P_y = a_3 \sin(\phi_3) + L \sin (\phi_4)$, where $L = a_2 - a_L $.

Finally, remember that $\phi_4$ has the following property:

$\phi_1 + \phi_2 = \phi_3 + \phi_4$

You have 5 equations (after substituting L) and 5 unknowns ($\phi_1, \phi_2, \phi_3, \phi_4$, and $a_L$.). An analytical solution exists for finding these values in closed-form.

As a final note, the equations I wrote above can be extracted from the transformation matrices, but I think it's clearer to present it this way.

Good luck!

Answer 2

Let's consider four points $A, B, C$ and $D$ and let us consider the parameter $d$, as shown in the figures below. And let us consider $p_e=\begin{Bmatrix}0\\0\\0\\1\end{Bmatrix}$ to be the end-effector location in end-effector's coordinate system.

Now, there are two ways of connecting the origin and the end-effector position, namely $O-A-P$ and $O-C-B-P$.

For the connection $O-A-P$, we can see from the figure that $\overline{OA}$ makes $\psi_1$ with positive x-axis in anticlockwise direction, and $\overline{AP}$ makes $\psi_2$ with $\overline{OA}$ in anticlockwise direction. Therefore the end-effector location in origin's coordinate system $p_o$ is given by

$p_o=R_z(\psi_1) \cdot T_x(a_1) \cdot R_z(\psi_2) \cdot T_x(a_2) \cdot p_e$

$\Rightarrow p_o = \begin{bmatrix}1 & 0 & 0 & 0 \\ 0 & \cos{\psi_1} & -\sin{\psi_1} & 0\\ 0 & \sin{\psi_1} & \cos{\psi_1} & 0\\ 0 & 0 & 0 & 1\end{bmatrix} \begin{bmatrix}1 & 0 & 0 & a_1 \\ 0 & 1 & 0 & 0\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1\end{bmatrix} \begin{bmatrix}1 & 0 & 0 & 0 \\ 0 & \cos{\psi_2} & -\sin{\psi_2} & 0\\ 0 & \sin{\psi_2} & \cos{\psi_2} & 0\\ 0 & 0 & 0 & 1\end{bmatrix} \begin{bmatrix}1 & 0 & 0 & a_2 \\ 0 & 1 & 0 & 0\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1\end{bmatrix} \begin{Bmatrix}0\\0\\0\\1\end{Bmatrix} $

$\Rightarrow p_o = \begin{Bmatrix}a_{1} \cos{\left(\psi_{1} \right)} + a_{2} \cos{\left(\psi_{1} + \psi_{2} \right)}\\a_{1} \sin{\left(\psi_{1} \right)} + a_{2} \sin{\left(\psi_{1} + \psi_{2} \right)}\\0\\1\end{Bmatrix}$

For the connection $O-C-B-P$, we can see from the figure that $\overline{OC}$ makes $0^c$ angle with positive x-axis in anticlockwise direction, $\overline{CB}$ makes $\psi_3$ with $\overline{OC}$ in anticlockwise direction and $\overline{BP}$ makes $\psi_4$ with $\overline{CB}$ in clockwise direction. Therefore the end-effector location in origin's coordinate system $p_o$ is given by

$p_o=R_z(0) \cdot T_x(a_4) \cdot R_z(\psi_3) \cdot T_x(a_3) \cdot R_z(-\psi_4) \cdot T_x(d) \cdot p_e$

$\Rightarrow p_o = \begin{bmatrix}1 & 0 & 0 & a_{4}\\0 & 1 & 0 & 0\\0 & 0 & 1 & 0\\0 & 0 & 0 & 1\end{bmatrix} \begin{bmatrix}\cos{\left(\psi_{3} \right)} & - \sin{\left(\psi_{3} \right)} & 0 & 0\\\sin{\left(\psi_{3} \right)} & \cos{\left(\psi_{3} \right)} & 0 & 0\\0 & 0 & 1 & 0\\0 & 0 & 0 & 1\end{bmatrix} \begin{bmatrix}1 & 0 & 0 & a_{3}\\0 & 1 & 0 & 0\\0 & 0 & 1 & 0\\0 & 0 & 0 & 1\end{bmatrix} \begin{bmatrix}\cos{\left(-\psi_{4} \right)} & - \sin{\left(-\psi_{4} \right)} & 0 & 0\\\sin{\left(-\psi_{4} \right)} & \cos{\left(-\psi_{4} \right)} & 0 & 0\\0 & 0 & 1 & 0\\0 & 0 & 0 & 1\end{bmatrix} \begin{bmatrix}1 & 0 & 0 & d\\0 & 1 & 0 & 0\\0 & 0 & 1 & 0\\0 & 0 & 0 & 1\end{bmatrix} \begin{Bmatrix}0\\0\\0\\1\end{Bmatrix} $

$\Rightarrow p_o = \begin{Bmatrix}a_{3} \cos{\left(\psi_{3} \right)} + a_{4} + d \cos{\left(\psi_{3} - \psi_{4} \right)}\\a_{3} \sin{\left(\psi_{3} \right)} + d \sin{\left(\psi_{3} - \psi_{4} \right)}\\0\\1\end{Bmatrix}$

We have two expressions for $p_o$. Let's equate them.

$\begin{Bmatrix}a_{1} \cos{\left(\psi_{1} \right)} + a_{2} \cos{\left(\psi_{1} + \psi_{2} \right)}\\a_{1} \sin{\left(\psi_{1} \right)} + a_{2} \sin{\left(\psi_{1} + \psi_{2} \right)}\\0\\1\end{Bmatrix}=\begin{Bmatrix}a_{3} \cos{\left(\psi_{3} \right)} + a_{4} + d \cos{\left(\psi_{3} - \psi_{4} \right)}\\a_{3} \sin{\left(\psi_{3} \right)} + d \sin{\left(\psi_{3} - \psi_{4} \right)}\\0\\1\end{Bmatrix}$

From this, we can have two equations:

$a_{1} \cos{\left(\psi_{1} \right)} + a_{2} \cos{\left(\psi_{1} + \psi_{2} \right)}=a_{3} \cos{\left(\psi_{3} \right)} + a_{4} + d \cos{\left(\psi_{3} - \psi_{4} \right)}$

and

$a_{1} \sin{\left(\psi_{1} \right)} + a_{2} \sin{\left(\psi_{1} + \psi_{2} \right)}=a_{3} \sin{\left(\psi_{3} \right)} + d \sin{\left(\psi_{3} - \psi_{4} \right)}$

Given $\psi_1$ and $\psi_3$, there are still three unknowns here, namely $\psi_2$, $\psi_4$ and $d$. But we have only two equations. We need at least one more independent equation in order to solve for the three unknowns for particular values of $\psi_1$ and $\psi_3$.

Let's consider the loop $O-C-B-A-O$. Since it is a loop, the transformation matrices, when multiplied, should produce identity matrix.

We can see from the figure that $\overline{OC}$ makes $0^c$ angle with positive x-axis in anticlockwise direction, $\overline{CB}$ makes $\psi_3$ with $\overline{OC}$ in anticlockwise direction, $\overline{BA}$ makes $\pi-\psi_4$ with $\overline{CB}$ in anticlockwise direction, $\overline{AO}$ makes $\psi_2$ with $\overline{BA}$ in clockwise direction, and $\overline{OC}$ makes $\pi-\psi_1$ with $\overline{AO}$ in anticlockwise direction. Therefore the relation is given by

$T_x(a_4)\cdot R_z(\psi_3)\cdot T_x(a_3)\cdot R_z(\pi-\psi_4)\cdot T_x(d)\cdot R_z(-\psi_2)\cdot T_x(a_1)\cdot R_z(\pi-\psi_1)=I_4$

$\Rightarrow \begin{bmatrix}1 & 0 & 0 & a_{4}\\0 & 1 & 0 & 0\\0 & 0 & 1 & 0\\0 & 0 & 0 & 1\end{bmatrix}\begin{bmatrix}\cos{\left(\psi_{3} \right)} & - \sin{\left(\psi_{3} \right)} & 0 & 0\\\sin{\left(\psi_{3} \right)} & \cos{\left(\psi_{3} \right)} & 0 & 0\\0 & 0 & 1 & 0\\0 & 0 & 0 & 1\end{bmatrix}\begin{bmatrix}1 & 0 & 0 & a_{3}\\0 & 1 & 0 & 0\\0 & 0 & 1 & 0\\0 & 0 & 0 & 1\end{bmatrix}\begin{bmatrix} \cos{\left(\pi - \psi_{4} \right)} & - \sin{\left(\pi - \psi_{4} \right)} & 0 & 0\\\sin{\left(\pi - \psi_{4} \right)} & \cos{\left(\pi - \psi_{4} \right)} & 0 & 0\\0 & 0 & 1 & 0\\0 & 0 & 0 & 1\end{bmatrix}\begin{bmatrix}1 & 0 & 0 & d \\0 & 1 & 0 & 0\\0 & 0 & 1 & 0\\0 & 0 & 0 & 1\end{bmatrix}\begin{bmatrix}\cos{\left(-\psi_{2} \right)} & - \sin{\left(-\psi_{2} \right)} & 0 & 0\\\sin{\left(-\psi_{2} \right)} & \cos{\left(-\psi_{2} \right)} & 0 & 0\\0 & 0 & 1 & 0\\0 & 0 & 0 & 1\end{bmatrix}\begin{bmatrix}1 & 0 & 0 & a_{1}\\0 & 1 & 0 & 0\\0 & 0 & 1 & 0\\0 & 0 & 0 & 1\end{bmatrix}\begin{bmatrix}\cos{\left(\pi-\psi_{1} \right)} & - \sin{\left(\pi-\psi_{1} \right)} & 0 & 0\\\sin{\left(\pi-\psi_{1} \right)} & \cos{\left(\pi-\psi_{1} \right)} & 0 & 0\\0 & 0 & 1 & 0\\0 & 0 & 0 & 1\end{bmatrix}=\begin{bmatrix}1 & 0 & 0 & 0\\0 & 1 & 0 & 0\\0 & 0 & 1 & 0\\0 & 0 & 0 & 1\end{bmatrix}$

$\Rightarrow \begin{bmatrix}\cos{\left(\psi_{1} + \psi_{2} - \psi_{3} + \psi_{4} \right)} & \sin{\left(\psi_{1} + \psi_{2} - \psi_{3} + \psi_{4} \right)} & 0 & - a_{1} \cos{\left(\psi_{2} - \psi_{3} + \psi_{4} \right)} + a_{3} \cos{\left(\psi_{3} \right)} + a_{4} - d \cos{\left(\psi_{3} - \psi_{4} \right)}\\- \sin{\left(\psi_{1} + \psi_{2} - \psi_{3} + \psi_{4} \right)} & \cos{\left(\psi_{1} + \psi_{2} - \psi_{3} + \psi_{4} \right)} & 0 & a_{1} \sin{\left(\psi_{2} - \psi_{3} + \psi_{4} \right)} + a_{3} \sin{\left(\psi_{3} \right)} - d \sin{\left(\psi_{3} - \psi_{4} \right)}\\0 & 0 & 1 & 0\\0 & 0 & 0 & 1\end{bmatrix}=\begin{bmatrix}1 & 0 & 0 & 0\\0 & 1 & 0 & 0\\0 & 0 & 1 & 0\\0 & 0 & 0 & 1\end{bmatrix}$

We can derive many equations from this, but they must be redundant. Let's equate the first row and the first column of LHS and RHS. We get

$\cos{\left(\psi_{1} + \psi_{2} - \psi_{3} + \psi_{4}\right)}=1$

$\Rightarrow \psi_{1} + \psi_{2} - \psi_{3} + \psi_{4}=0$

$\Rightarrow \psi_{3} - \psi_{4} = \psi_{1} + \psi_{2}$

Putting this in the earlier two equations gives

$a_{1} \cos{\left(\psi_{1} \right)} + a_{2} \cos{\left(\psi_{1} + \psi_{2} \right)}=a_{3} \cos{\left(\psi_{3} \right)} + a_{4} + d \cos{\left(\psi_{1} + \psi_{2} \right)}$

$\Rightarrow \left(a_2-d\right)\cos{\left(\psi_1+\psi_2\right)}=a_3 \cos{\left(\psi_3\right)}-a_1\cos{\left(\psi_1\right)}+a_4$

and

$a_{1} \sin{\left(\psi_{1} \right)} + a_{2} \sin{\left(\psi_{1} + \psi_{2} \right)}=a_{3} \sin{\left(\psi_{3} \right)} + d \sin{\left(\psi_{1} + \psi_{2} \right)}$

$\Rightarrow \left(a_2-d\right)\sin{\left(\psi_1+\psi_2\right)}=a_3 \sin{\left(\psi_3\right)}-a_1\sin{\left(\psi_1\right)}$

Squaring and adding the equations gives

$\left[\left(a_2-d\right)\cos{\left(\psi_1+\psi_2\right)}\right]^2+\left[\left(a_2-d\right)\sin{\left(\psi_1+\psi_2\right)}\right]^2=\left[a_3 \cos{\left(\psi_3\right)}-a_1\cos{\left(\psi_1\right)}+a_4\right]^2+\left[a_3 \sin{\left(\psi_3\right)}-a_1\sin{\left(\psi_1\right)}\right]^2$

$\Rightarrow d=a_2\pm \sqrt{a_1^2 +a_3^2 + a_4^2 -2a_1a_3\cos{\left(\psi_1-\psi_3\right)}+a_4\left(a_1\cos{\left(\psi_1\right)}-a_3\cos{\left(\psi_3\right)}\right)}$

And dividing one equation with the other equation gives

$\frac{\left(a_2-d\right)\sin{\left(\psi_1+\psi_2\right)}}{\left(a_2-d\right)\cos{\left(\psi_1+\psi_2\right)}}=\frac{a_3 \sin{\left(\psi_3\right)}-a_1\sin{\left(\psi_1\right)}}{a_3 \cos{\left(\psi_3\right)}-a_1\cos{\left(\psi_1\right)}+a_4}$

$\Rightarrow \psi_2=-\psi_1+\tan^{-1}{\left(\frac{a_3 \sin{\left(\psi_3\right)}-a_1\sin{\left(\psi_1\right)}}{a_3 \cos{\left(\psi_3\right)}-a_1\cos{\left(\psi_1\right)}+a_4}\right)}$

Therefore, we have $\psi_2$ and $d$ in terms of $\psi_1$ and $\psi_3$. And so, we can write all the other unknown variables in terms of $\psi_1$ and $\psi_3$.

Hence, for the position of the end-effector, we have

$p_o = \begin{Bmatrix}a_{1} \cos{\left(\psi_{1} \right)} + a_{2} \cos{\left(\psi_{1} + \psi_{2} \right)}\\a_{1} \sin{\left(\psi_{1} \right)} + a_{2} \sin{\left(\psi_{1} + \psi_{2} \right)}\\0\\1\end{Bmatrix}$

$\Rightarrow p_o = \begin{Bmatrix}a_{1} \cos{\left(\psi_{1} \right)} + a_{2} \cos{\left(\tan^{-1}{\left(\frac{a_3 \sin{\left(\psi_3\right)}-a_1\sin{\left(\psi_1\right)}}{a_3 \cos{\left(\psi_3\right)}-a_1\cos{\left(\psi_1\right)}+a_4}\right)} \right)}\\a_{1} \sin{\left(\psi_{1} \right)} + a_{2} \sin{\left(\tan^{-1}{\left(\frac{a_3 \sin{\left(\psi_3\right)}-a_1\sin{\left(\psi_1\right)}}{a_3 \cos{\left(\psi_3\right)}-a_1\cos{\left(\psi_1\right)}+a_4}\right)} \right)}\\0\\1\end{Bmatrix}$

And for the orientation of the end-effector link, from the figure we can have

$\psi = \psi_1+\psi_2$

$\Rightarrow \psi = \tan^{-1}{\left(\frac{a_3 \sin{\left(\psi_3\right)}-a_1\sin{\left(\psi_1\right)}}{a_3 \cos{\left(\psi_3\right)}-a_1\cos{\left(\psi_1\right)}+a_4}\right)}$