# Pose of the end-effector

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?.

• Welcome to Robotics Stack Exchange. Please help provide more details about what you're looking for help doing. Your diagram and matrices are not annotated enough to understand the mechanism enough to exactly know what you're doing. And you need to clarify what conventions you're using for your notation. I expect that you're doing this following an external reference, but remember that we who are reading this don't have that extra context, your question needs to be self contained with all the necessary information to help you. Nov 2 '21 at 8:51
• In attition to @Tully's comments, it looks like you're using DH params to describe a linkage that's grounded at two locations to find the position of point P? If so, this is an incorrect application for DH parameters. Your diagram looks like a type of slider-crank mechanism. Nov 3 '21 at 17:57

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!

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)}$$