Help to get robot jacobian from system of implicit equations

Question

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!

Answer 1

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.

Answer 2

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])