```python
import sympy
import numpy
from math import cos, sin

import sympybotics
from sympybotics._compatibility_ import exec_

pi = sympy.pi
q = sympybotics.robotdef.q

a1, a2, d3, d4 = sympy.symbols('a1, a2, d3, d4')
```


```python
scara = sympybotics.robotdef.RobotDef(
    'SCARA - Spong',
    [( 0, a1,  0, q),
     (pi, a2,  0, q),
     ( 0,  0,  q, 0),
     ( 0,  0, d4, q)],
    dh_convention='standard')
```


```python
scara_geo = sympybotics.geometry.Geometry(scara)
scara_kin = sympybotics.kinematics.Kinematics(scara, scara_geo)

cos, sin = sympy.cos, sympy.sin
q1, q2, q3, q4 = sympy.flatten(scara.q)
```


```python
T_spong = sympy.Matrix([
    [(-sin(q1)*sin(q2) + cos(q1)*cos(q2))*cos(q4) + (sin(q1)*cos(q2) + sin(q2)*cos(q1))*sin(q4), 
     -(-sin(q1)*sin(q2) + cos(q1)*cos(q2))*sin(q4) + (sin(q1)*cos(q2) + sin(q2)*cos(q1))*cos(q4), 
     0, a1*cos(q1) - a2*sin(q1)*sin(q2) + a2*cos(q1)*cos(q2)],
    [(sin(q1)*sin(q2) - cos(q1)*cos(q2))*sin(q4) + (sin(q1)*cos(q2) + sin(q2)*cos(q1))*cos(q4),
     (sin(q1)*sin(q2) - cos(q1)*cos(q2))*cos(q4) - (sin(q1)*cos(q2) + sin(q2)*cos(q1))*sin(q4),
     0, a1*sin(q1) + a2*sin(q1)*cos(q2) + a2*sin(q2)*cos(q1)],
    [0, 0, -1, -d4 - q3],
    [0, 0, 0, 1]])

print(scara_geo.T[-1])

assert (scara_geo.T[-1] - T_spong).expand() == sympy.zeros(4)
```

    Matrix([[(-sin(q1)*sin(q2) + cos(q1)*cos(q2))*cos(q4) + (sin(q1)*cos(q2) + sin(q2)*cos(q1))*sin(q4), -(-sin(q1)*sin(q2) + cos(q1)*cos(q2))*sin(q4) + (sin(q1)*cos(q2) + sin(q2)*cos(q1))*cos(q4), 0, a1*cos(q1) - a2*sin(q1)*sin(q2) + a2*cos(q1)*cos(q2)], [(sin(q1)*sin(q2) - cos(q1)*cos(q2))*sin(q4) + (sin(q1)*cos(q2) + sin(q2)*cos(q1))*cos(q4), (sin(q1)*sin(q2) - cos(q1)*cos(q2))*cos(q4) - (sin(q1)*cos(q2) + sin(q2)*cos(q1))*sin(q4), 0, a1*sin(q1) + a2*sin(q1)*cos(q2) + a2*sin(q2)*cos(q1)], [0, 0, -1, -d4 - q3], [0, 0, 0, 1]])



```python
J_spong = sympy.Matrix(
    [[-a1*sin(q1) - a2*sin(q1)*cos(q2) - a2*sin(q2)*cos(q1), -a2*sin(q1)*cos(q2) - a2*sin(q2)*cos(q1), 0, 0],
     [a1*cos(q1) - a2*sin(q1)*sin(q2) + a2*cos(q1)*cos(q2), -a2*sin(q1)*sin(q2) + a2*cos(q1)*cos(q2), 0, 0],
     [0, 0, -1, 0],
     [0, 0, 0, 0],
     [0, 0, 0, 0],
     [1, 1, 0, -1]])

print(scara_kin.J[-1])

assert (scara_kin.J[-1] - J_spong).expand() == sympy.zeros(6, 4)
```

    Matrix([[-a1*sin(q1) - a2*sin(q1)*cos(q2) - a2*sin(q2)*cos(q1), -a2*sin(q1)*cos(q2) - a2*sin(q2)*cos(q1), 0, 0], [a1*cos(q1) - a2*sin(q1)*sin(q2) + a2*cos(q1)*cos(q2), -a2*sin(q1)*sin(q2) + a2*cos(q1)*cos(q2), 0, 0], [0, 0, -1, 0], [0, 0, 0, 0], [0, 0, 0, 0], [1, 1, 0, -1]])


f6b3041bb766b9bbaa0ed90aa47979114dbfcf65
ipynb
Jupyter Notebook
test_scara_dh_sym_geo_kin.ipynb
jenskober/SymPyBotics
3842759562b22d2f901e947dc0047ca48ff631b2
BSD-3-Clause
test_scara_dh_sym_geo_kin.ipynb
jenskober/SymPyBotics
3842759562b22d2f901e947dc0047ca48ff631b2
BSD-3-Clause
test_scara_dh_sym_geo_kin.ipynb
jenskober/SymPyBotics
3842759562b22d2f901e947dc0047ca48ff631b2
BSD-3-Clause
Qwen/Qwen-72B
1. YES
2. YES