1
$\begingroup$

I've done the FK and IK for this robot: enter image description here

On those I seem to be able to get to the positions I want on the simulator so I'm assuming nothing is wrong with my FK and IK calculations. I've tried to calculate the Jacobian to later determine the speed at which each actuator needs to go while inputting a linear speed for the endpoint. I assume I can calculate that by inverting the Jacobian matrix and multiplying it to the speed I want, getting a 1x3 matrix with the speeds for each joint. However, the Jacobian matrix I get isn't square (6x3) and even if I only consider the linear speed part (top part which is square), that part isn't possible to invert (rank of it is lower than it's length).

Here's the parameters that constitute the Jacobian matrix that I calculated:

$$Z^0_0 = \begin{bmatrix}0 \\ 0 \\ 1\end{bmatrix}$$

$$Z^0_1 = \begin{bmatrix}\sin(\theta_1) \\ -\cos(\theta_1) \\ 0\end{bmatrix}$$

$$Z^0_2 = \begin{bmatrix}\cos(\theta_1)*\sin(\theta_2)+\cos(\theta_2)*\sin(\theta_1) \\ \sin(\theta_1)*\sin(\theta_2)-\cos(\theta_1)*\cos(\theta_2) \\ 0\end{bmatrix}$$

$$O^0_0 = \begin{bmatrix}0 \\ 0 \\ 0\end{bmatrix}$$

$$O^0_1 = \begin{bmatrix}0 \\ 0 \\ d_1\end{bmatrix}$$

$$O^0_2 = \begin{bmatrix}d_2*\cos(\theta_2) \\ d_2*\sin(\theta_2) \\ d_1\end{bmatrix}$$

$$O^0_3 = \begin{bmatrix}d_3*\cos(\theta_3)+d_2*\cos(\theta_2)*\cos(\theta_3)-d_2*\sin(\theta_2)*\sin(\theta_3) \\ d_3*\sin(\theta_3)+d_2*\cos(\theta_2)*\sin(\theta_3)+d_2*\cos(\theta_3)*\sin(\theta_2) \\ d_1\end{bmatrix}$$

$$J = \begin{bmatrix} Z^0_0\times(O^0_3 - O^0_0) & Z^0_1\times(O^0_3 - O^0_1) & Z^0_2\times(O^0_3 - O^0_2) \\ Z^0_0 & Z^0_1 & Z^0_2\end{bmatrix}$$

$$ \dot{q}(t) = \begin{bmatrix} \dot{q_1} \\ \dot{q_2} \\ \dot{q_3} \end{bmatrix}$$

What am I doing wrong?

EDIT: Added the DH parameters and used MathJax to better show the matrixes.

\begin{array} {|r|r|}\hline link & a_i & \alpha_i & d_i & \theta_i \\ \hline 1 & 0 & -90º & d1 & \theta_1* \\ \hline 2 & d2 & 0 & 0 & \theta_2* \\ \hline 3 & d3 & 0 & 0 & \theta_3* \\ \hline \end{array}

EDIT2: Added the transformation matrixes for each joint.

$$ H^1_0 = \begin{bmatrix} \cos(\theta_1) & 0 & \sin(\theta_1) & 0 \\ \sin(\theta_1) & 0 & -\cos(\theta_1) & 0 \\ 0 & 1 & 0 & d_1 \\ 0 & 0 & 0 & 1\end{bmatrix} $$

$$ H^2_1 = \begin{bmatrix} \cos(\theta_2) & -\sin(\theta_2) & 0 & d_2*\cos(\theta_2) \\ \sin(\theta_2) & \cos(\theta_2) & 0 & d_2*\sin(\theta_2) \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1\end{bmatrix} $$

$$ H^3_2 = \begin{bmatrix} \cos(\theta_3) & -\sin(\theta_3) & 0 & d_3*\cos(\theta_3) \\ \sin(\theta_3) & \cos(\theta_3) & 0 & d_3*\sin(\theta_3) \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1\end{bmatrix} $$

$$ H^2_0 = H^2_1 * H^1_0 $$ $$ H^3_0 = H^3_2 * H^2_1 * H^1_0 $$

From $ H^n_0 $ I'm getting $ Z_n $ and $ O^n_0 $ which are:

$$Z_n =\begin{bmatrix}H^n_0(1,3) \\ H^n_0(2,3) \\ H^n_0(3,3) \end{bmatrix} , O^n_0 = \begin{bmatrix}H^n_0(1,4) \\ H^n_0(2,4) \\ H^n_0(3,4) \end{bmatrix}$$

$\endgroup$
  • $\begingroup$ As drawn, your arm is in a singular configuration. Shoulder and elbow will both move the end effector up, but no joint can extend it further (because the elbow is hyperextended, placing the forearm inline with the upper arm). Also, your notation is transposed from common notation, which would have a 3x1 speed vector and a 6x3 Jacobian. $\endgroup$ – RLH Jun 5 at 2:06
  • $\begingroup$ "...your notation is transposed from common notation..." - you mean that it is correct and the jacobian matrix, that indeed is 6x3 I wrote it wrong, is impossible to invert so I cannot set the speeds for each joint given a end effector's speed? $\endgroup$ – diogofd8 Jun 5 at 3:24
  • $\begingroup$ Your joint velocity vector should be 3x1, and your Jacobian 6x3. It would help us to help you if you explained what your “parameters that constitute the Jacobian” are, use the MathJax feature of the site to write them out in a more readable form, and write out the Jacobian as well. $\endgroup$ – RLH Jun 5 at 13:12
  • $\begingroup$ Sorry, this is my 1st post here and I had no idea how to use MathJax before. Did some googling and came out with the edit. Is it better now? The O's are the axis distance from the joint to the origin axis O^0_0 while the Z's are the first 3 rows of the 3rd column of the T matrix. I've added also the DH parameters $\endgroup$ – diogofd8 Jun 5 at 14:14
  • $\begingroup$ That helps a lot. You can use syntax like “d_{2}” and “\sin{\theta_{1}” to make the terms in the arm more readable. $\endgroup$ – RLH Jun 5 at 14:45
1
$\begingroup$

Your axes and link vectors seem off. The axes $z_{1}$ and $z_{2}$ should be parallel, and your $O^{0}_{2}$ and $O^{0}_{3}$ don’t seem to be accounting for all the rotations that occur prior to the link.

Note that as drawn, your arm is in a singular configuration. Shoulder and elbow will both move the end effector up, but no joint can extend it further (because the elbow is hyperextended, placing the forearm inline with the upper arm). This means that if you try taking the inverse of the position block of the Jacobian, you’ll find it to be rank deficient.

———

Edit: I’m not a fan of DH for this. A better procedure would be:

  1. Identify your reference configuration.

  2. Take the local definition for each joint axis as its axis direction in the reference configuration.

  3. Take the local link vector as its vector in the reference configuration.

  4. Convert each rotation axis into a rotation matrix. If you’ve chosen a good reference configuration, the resulting matrices will be canonical $R_x$, $R_{y}$, or $R_{z}$ matrices.

  5. Get the net rotations along the chain by taking the cumulative product of the rotation matrices working left to right as you go down the chain.

  6. Get the link vectors by multiplying the net rotations into the corresponding local link vectors.

  7. Get the link endpoints by taking the cumulative sum of the link vectors.

  8. Get the rotation axes by multiplying the net rotations into the corresponding local link axes.

If you want to get one step fancier, you can build joint and link transformation matrices by augmenting the rotation matrices with a zero translation vector and the link transformation matrices with an identity rotation matrix. You can then take the left-to-right cumulative product to get the transform to each point on the chain.

| improve this answer | |
$\endgroup$
  • $\begingroup$ I've edited the main post with information on how I'm getting Zn and O^n_0, I saw it being done like that on a youtube video about "rocotics jacobian matrix" but I'm now unsure if I am doing it right, could you please point me towards my mistake? Are the DH parameters wrong and because of it I'm generating the wrong matrixes and snowballing the problem into a singularity or am I just doing the Jacobian incorrectly? If so, how can I fix it? $\endgroup$ – diogofd8 Jun 5 at 15:47
  • $\begingroup$ It looks like you’re multiplying the H matrices in the reverse order of what they should be. It also looks like you’re extracting the third columns of your rotation blocks to get your joint axes, which doesn’t seem right, because they’re not all parallel. As a side note, I’m not a fan of DH, because it gets you into weird representations. For this problem it would be much simpler to write out the joint vectors and joint axes directly. $\endgroup$ – RLH Jun 5 at 16:24
  • $\begingroup$ on this webside link they multiply the H matrixes the same way I do (3*2*1). Also, the same matrixes are able to determine correctly the position of the robot using direct kinematics and to determine correctly the angles of the joints using inverse. Is there a way to always obtain the right positions and angles while having the wrong transformation matrixes? All I want to know is how to make the Jacobian for this scenario because it seems that I'm doing it wrong. $\endgroup$ – diogofd8 Jun 5 at 17:00
  • $\begingroup$ I don’t know why they’re using that order. I’ve listed the procedure that I give to my students who aren’t working with full transformation matrices. If you get that working, it can help debug any more sophisticated approach you want to implement. $\endgroup$ – RLH Jun 5 at 17:29

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.