# Help to get robot jacobian from system of implicit equations

I have system of two equations that describes position of robot end-effector ($X_C, Y_C, Z_C$), in accordance to prismatic joints position ($S_A, S_B$):

$S^2_A - \sqrt3(S_A + S_B)X_C = S^2_B + (S_A - S_B)Y_C$

$X^2_C + Y^2_C + Z^2_C = L^2 - S^2_A + S_A(\sqrt3X_C + Y_C)+M(S^2_A+S_BS_A + S^2_B)$

where M and L are constants.

In paper, author states that differentiating this system at given point ($X_C, Y_C, Z_C$) gives the "differential relationship" in form:

$a_{11}\Delta S_A + a_{12}\Delta S_B = b_{11}\Delta X_C + b_{12}\Delta Y_C + b_{13}\Delta Z_C$

$a_{21}\Delta S_A + a_{22}\Delta S_B = b_{21}\Delta X_C + b_{22}\Delta Y_C + b_{23}\Delta Z_C$

Later on, author uses those parameters ($a_{11}, a_{12}, b_{11}...$) to construct matrices, and by multiplying those he obtains Jacobian of the system.

Im aware of partial differentiation, but I have never done this for system of equations, neither I understand how to get those delta parameters.

Can anyone explain what are the proper steps to perform partial differentiation on this system, and how to calculate delta parameters?

EDIT

Following advice given by N. Staub, I differentiated equations w.r.t time.

First equation:

$S^2_A - \sqrt3(S_A + S_B)X_C = S^2_B + (S_A - S_B)Y_C$ $=>$

$2S_A \frac{\partial S_A}{\partial t} -\sqrt3S_A \frac{\partial X_C}{\partial t} -\sqrt3X_C \frac{\partial S_A}{\partial t} -\sqrt3S_B \frac{\partial X_C}{\partial t} -\sqrt3X_C \frac{\partial S_B}{\partial t} = 2S_B\frac{\partial S_B}{\partial t} + S_A\frac{\partial Y_C}{\partial t} + Y_C\frac{\partial S_A}{\partial t} - S_B\frac{\partial Y_C}{\partial t} - Y_C\frac{\partial S_B}{\partial t}$

Second equation:

$X^2_C + Y^2_C + Z^2_C = L^2 - S^2_A + S_A(\sqrt3X_C + Y_C)+M(S^2_A+S_BS_A + S^2_B)$ $=>$

$2X_C \frac{\partial X_C}{\partial t} + 2Y_C \frac{\partial Y_C}{\partial t} + 2Z_C \frac{\partial Z_C}{\partial t} = -2S_A \frac{\partial S_A}{\partial t} + \sqrt3S_A \frac{\partial X_C}{\partial t} +\sqrt3X_C \frac{\partial S_A}{\partial t} + S_A \frac{\partial Y_C}{\partial t} + Y_C \frac{\partial S_A}{\partial t} + 2MS_A \frac{\partial S_A}{\partial t} + MS_B \frac{\partial S_A}{\partial t} + MS_A \frac{\partial S_B}{\partial t} + 2MS_B \frac{\partial S_B}{\partial t}$

then, I multiplied by $\partial t$, and grouped variables:

First equation:

$(2S_A -\sqrt3X_C - Y_C)\partial S_A +(-2S_B -\sqrt3X_C + Y_C)\partial S_B = (\sqrt3S_A +\sqrt3S_B)\partial X_C + (S_A - S_B)\partial Y_C$

Second equation:

$(-2S_A+\sqrt3X_C+Y_C+2MS_A + MS_B)\partial S_A + (MS_A + 2MS_B)\partial S_B = (2X_C-\sqrt3S_A)\partial X_C + (2Y_C-S_A)\partial Y_C + (2Z_C)\partial Z_C$

therefore I assume that required parameters are:

$a_{11} = 2S_A -\sqrt3X_C - Y_C$

$a_{12} = -2S_B -\sqrt3X_C + Y_C$

$a_{21} = -2S_A + \sqrt3X_C + Y_C + 2MS_A + MS_B$

$a_{22} = MS_A + 2MS_B$

$b_{11} = \sqrt3S_A +\sqrt3S_B$

$b_{12} = S_A - S_B$

$b_{13} = 0$

$b_{21} = 2X_C - \sqrt3S_A$

$b_{22} = 2Y_C - S_A$

$b_{23} = 2Z_C$

Now. According to paper, jacobian of the system can be calculated as:

$J = A^{-1} B$, where

$A=(a_{ij})$

$B=(b_{ij})$

so I im thinking right, it means:

$$A = \begin{matrix} a_{11} & a_{12} \\ a_{21} & a_{22} \\ \end{matrix}$$

$$B = \begin{matrix} b_{11} & b_{12} & b_{13} \\ b_{21} & b_{22} & b_{23} \\ \end{matrix}$$

and Jacobian is multiplication of reverse A matrix and B matrix.

Next, author states that Jacobian at given point, where

$X_C = 0$

$S_A=S_B=S$

$Y_C = l_t-\Delta\gamma$

is equal to:

$$J = \begin{matrix} \frac{\sqrt3S}{2S-l_t+\Delta\gamma} & -\frac{\sqrt3S}{2S-l_t+\Delta\gamma}\\ \frac{2(l_t-\Delta\gamma)-S}{6cS-2S+l_t-\Delta\gamma} & \frac{2(l_t-\Delta\gamma)-S}{6cS-2S+l_t-\Delta\gamma} \\ \frac{2Z_C}{6cS-2S+l_t-\Delta\gamma} & \frac{2Z_C}{6cS-2S+l_t-\Delta\gamma} \\ \end{matrix} ^T$$

Everything seems fine. BUT! After multiplicating my A and B matrices I get some monster matrix, that I am unable to paste here, becouse it is so frickin large! Substituting variables for values given by author does not give me proper jacobian (i tried substitution before multiplying matrices (on parameters), and after multiplication (on final matrix)). So clearly Im still missing something. Either Ive done error in differentiation, or Ive done error in matrix multiplication (I used maple) or I dont understand how to subsititue those values. Can anyone point me in to right direction?

EDIT Problem solved! Parameters that I calculated were proper, I just messed up simplification of equations in final matrix. Using snippet from Petch Puttichai I was able to obtain full Jacobian of system. Thanks for help!

Recall that the Jacobian can be interpreted as the matrix which maps the joint velocity to end-effector velocity.

$\dot{x} = J(\theta)~\dot{\theta}$

In your case $x = [x_c,y_c,z_c]$ (prefer lower case for vectors and scalar) and $\theta = [s_A,s_B]$.

If you differentiate the system as suggested you should get the second set of equations with $\Delta s_A, s_A, \Delta s_B, s_B, ...$. To do so you can do the partial time differentiation and multiply your result by $\Delta t$ as for any time dependent variable $u$, $\frac{\partial u}{ \partial t } = \frac{\Delta u }{\Delta t}$.

NOTE : your first equation can be rewritten

$(\sqrt{3} x_c +y_x ) (s_A+s_B) = {s_A}^2 + {s_B}^2 - 2s_A$

which make the distance (the second equation) expressed only from the system ($L,M$) and joint ($s_A,s_B$) parameters.

• Brace me up, couse this is deffinetly the most advanced math problem I have faced. From what I understand x, y, z, Sa and Sb are time dependent variables (they are not constants). – Michał Turkiewicz May 21 '18 at 20:10
• How can I differentiate the system "as suggested"? I know how to perform partial differentiation of function regarding to one of its variables, for example Sa, keeping x,y,z constant (becouse we are differentiating at some given point). How to perform partial time differentiation of such equation? – Michał Turkiewicz May 21 '18 at 20:23
• I don't get your first comment I never assumed $x,y,z$ nor $s_A, s_B$ to be constants. If they are time varying quantities you can differentiate them w.r.t to time, if you don't know how it works by heart there are lost of tables on the net deriving expressions w.r.t. time. – N. Staub May 22 '18 at 6:23

Actually you only got some errors when computing matrix elements.

The matrices A and B should be $$A = \begin{bmatrix}2S_A - \sqrt{3}X - Y & -2S_B - \sqrt{3}X + Y\\ -2S_A + \sqrt{3}X + Y + 2MS_A + MS_B & MS_A + 2MS_B\end{bmatrix}$$ and $$B = \begin{bmatrix}\sqrt{3}S_A + \sqrt{3}S_B & S_A - S_B & 0\\ 2X - \sqrt{3}S_A & 2Y - \sqrt{3}S_A & 2Z\end{bmatrix}.$$ Using the above two matrices should give you the desired result, which can be verified by the following python snippet.

import sympy as sp
# Declare symbolic variables. (I also use r for sqrt(3))
x, y, z, s, sa, sb, m, r = sp.symbols('x, y, z, s, sa, sb, m, r')
A = sp.Matrix([[2*sa - r*x - y, -2*sb - r*x + y],
[-2*sa + r*x + y + 2*m*sa + m*sb, m*sa + 2*m*sb]])
B = sp.Matrix([[r*sa + r*sb, sa - sb, 0],
[2*x - r*sa, 2*y - r*sb, 2*z]])
Ainv = sp.Matrix.inv(A)
res = Ainv.multiply(B)
J = res.subs({sa: s, sb: s, x: 0}) # do the substitution

To print out the element $(i, j)$ of $J$ (indices start from 0), do

print sp.simplify(J[3*i + j])
• Great! I was so close, but i was missing.. simplification. I assumed that program like maple is simplifying by default, and so I tought that values in matrix are in they final, most compact form. Now everything is perfect. I must accept N. Staub's answer, becouse it was the answer to my original problem. But thank You very much for help, and for the snippet. – Michał Turkiewicz May 27 '18 at 21:33
• @MichałTurkiewicz No problem! Yes, you should accept that answer. – Petch Puttichai May 28 '18 at 3:43