Jacobian to determine the linear velocity of each node (DOF 3 RRR robot arm)

Question

I've done the FK and IK for this robot:

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

Answer 1

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.