# 足先（エンドエフェクタ）のジョイント角度ベクトルによるヤコビ行列の導出

制御したいもの -> 足先の2次元位置 + 足先の地面となす角度  

足先の状態変数を次のようにおく．  




```python
import sympy as sy
from sympy import pi, cos, sin, tan
from IPython.display import display
```


```python
# 式を定義
q1 = sy.Symbol("q1")
q2 = sy.Symbol("q2")
q3 = sy.Symbol("q3")

qvec = sy.Matrix([[q1, q2, q3,]]).T


l1 = sy.Symbol("l1")
l2 = sy.Symbol("l2")
l3 = sy.Symbol("l2")
l4 = sy.Symbol("l2")



D1 = sy.Matrix([
        [cos(q1), -sin(q1), 0],
        [sin(q1), cos(q1), 0],
        [0, 0, 1],
    ])

D2 = sy.Matrix([
        [1, 0, l1],
        [0, 1, 0],
        [0, 0, 1],
    ])

D3 = sy.Matrix([
        [cos(q2), -sin(q2), 0],
        [sin(q2), cos(q2), 0],
        [0, 0, 1],
    ])

D4 = sy.Matrix([
        [1, 0, l2],
        [0, 1, 0],
        [0, 0, 1],
    ])


theta = 270/180*pi
D5 = sy.Matrix([
        [cos(theta), -sin(theta), 0],
        [sin(theta), cos(theta), 0],
        [0, 0, 1],
    ])

D6 = sy.Matrix([
        [1, 0, l3],
        [0, 1, 0],
        [0, 0, 1],
    ])

D7 = sy.Matrix([
        [cos(q3), -sin(q3), 0],
        [sin(q3), cos(q3), 0],
        [0, 0, 1],
    ])

D8 = sy.Matrix([
        [1, 0, l4],
        [0, 1, 0],
        [0, 0, 1],
    ])
```


```python
# 足先（エンドエフェクター）の位置
D_end = D1 * D2 * D3 * D4 * D5 * D6 * D7 * D8
D_end = sy.simplify(D_end)

phi = sy.Symbol('phi')
phi = q1 + q2 + q3 -2*pi

ee = sy.Matrix([
    [D_end[0, 2]],
    [D_end[1, 2]],
    [phi]
])
display(ee)
```


$\displaystyle \left[\begin{matrix}l_{1} \cos{\left(q_{1} \right)} + \sqrt{2} l_{2} \sin{\left(q_{1} + q_{2} + \frac{\pi}{4} \right)} + l_{2} \sin{\left(q_{1} + q_{2} + q_{3} \right)}\\l_{1} \sin{\left(q_{1} \right)} - \sqrt{2} l_{2} \cos{\left(q_{1} + q_{2} + \frac{\pi}{4} \right)} - l_{2} \cos{\left(q_{1} + q_{2} + q_{3} \right)}\\q_{1} + q_{2} + q_{3} - 2 \pi\end{matrix}\right]$



```python
jacobi = ee.jacobian(qvec)
jacobi = sy.simplify(jacobi)
```

結果を表示


```python
display(jacobi)
```


$\displaystyle \left[\begin{matrix}- l_{1} \sin{\left(q_{1} \right)} + \sqrt{2} l_{2} \cos{\left(q_{1} + q_{2} + \frac{\pi}{4} \right)} + l_{2} \cos{\left(q_{1} + q_{2} + q_{3} \right)} & l_{2} \left(\sqrt{2} \cos{\left(q_{1} + q_{2} + \frac{\pi}{4} \right)} + \cos{\left(q_{1} + q_{2} + q_{3} \right)}\right) & l_{2} \cos{\left(q_{1} + q_{2} + q_{3} \right)}\\l_{1} \cos{\left(q_{1} \right)} + \sqrt{2} l_{2} \sin{\left(q_{1} + q_{2} + \frac{\pi}{4} \right)} + l_{2} \sin{\left(q_{1} + q_{2} + q_{3} \right)} & l_{2} \left(\sqrt{2} \sin{\left(q_{1} + q_{2} + \frac{\pi}{4} \right)} + \sin{\left(q_{1} + q_{2} + q_{3} \right)}\right) & l_{2} \sin{\left(q_{1} + q_{2} + q_{3} \right)}\\1 & 1 & 1\end{matrix}\right]$


ipynb
212a3fa30863e311568a0c512775734a8cafe94e
Jupyter Notebook
a1d717f58ab6a6b1e688dd5747c79d82eca98193
MIT
YoshimitsuMatsutaIe/quadruped
sy/calc_jacobian.ipynb
a1d717f58ab6a6b1e688dd5747c79d82eca98193
MIT
YoshimitsuMatsutaIe/quadruped
sy/calc_jacobian.ipynb
a1d717f58ab6a6b1e688dd5747c79d82eca98193
MIT
YoshimitsuMatsutaIe/quadruped
sy/calc_jacobian.ipynb