```python
from sympy import symbols, cos, sin, pi, simplify, pprint, tan, expand_trig, sqrt, trigsimp, atan2
from sympy.matrices import Matrix
```


```python
def rotx(q):

  sq, cq = sin(q), cos(q)

  r = Matrix([
    [1., 0., 0.],
    [0., cq,-sq],
    [0., sq, cq]
  ])
    
  return r


def roty(q):

  sq, cq = sin(q), cos(q)

  r = Matrix([
    [ cq, 0., sq],
    [ 0., 1., 0.],
    [-sq, 0., cq]
  ])
    
  return r


def rotz(q):

  sq, cq = sin(q), cos(q)

  r = Matrix([
    [cq,-sq, 0.],
    [sq, cq, 0.],
    [0., 0., 1.]
  ])
    
  return r
```


```python
alpha, beta, gamma = symbols('alpha beta gamma', real = True)
r = rotz(alpha) * roty(beta) * rotx(gamma)
```


```python
pprint(simplify(r))
```

    ⎡1.0⋅cos(α)⋅cos(β)  -1.0⋅sin(α)⋅cos(γ) + sin(β)⋅sin(γ)⋅cos(α)  1.0⋅sin(α)⋅sin(
    ⎢                                                                             
    ⎢1.0⋅sin(α)⋅cos(β)  sin(α)⋅sin(β)⋅sin(γ) + 1.0⋅cos(α)⋅cos(γ)   sin(α)⋅sin(β)⋅c
    ⎢                                                                             
    ⎣   -1.0⋅sin(β)                 1.0⋅sin(γ)⋅cos(β)                         1.0⋅
    
    γ) + sin(β)⋅cos(α)⋅cos(γ)⎤
                             ⎥
    os(γ) - 1.0⋅sin(γ)⋅cos(α)⎥
                             ⎥
    cos(β)⋅cos(γ)            ⎦



```python
for m in [0, 1, 2]:
  for n in [0, 1, 2]: 
    
    print()
    print("R" + str(m) + str(n))
    pprint(r[0, 1])
    print()
```

    
    R00
    -1.0⋅sin(α)⋅cos(γ) + sin(β)⋅sin(γ)⋅cos(α)
    
    
    R01
    -1.0⋅sin(α)⋅cos(γ) + sin(β)⋅sin(γ)⋅cos(α)
    
    
    R02
    -1.0⋅sin(α)⋅cos(γ) + sin(β)⋅sin(γ)⋅cos(α)
    
    
    R10
    -1.0⋅sin(α)⋅cos(γ) + sin(β)⋅sin(γ)⋅cos(α)
    
    
    R11
    -1.0⋅sin(α)⋅cos(γ) + sin(β)⋅sin(γ)⋅cos(α)
    
    
    R12
    -1.0⋅sin(α)⋅cos(γ) + sin(β)⋅sin(γ)⋅cos(α)
    
    
    R20
    -1.0⋅sin(α)⋅cos(γ) + sin(β)⋅sin(γ)⋅cos(α)
    
    
    R21
    -1.0⋅sin(α)⋅cos(γ) + sin(β)⋅sin(γ)⋅cos(α)
    
    
    R22
    -1.0⋅sin(α)⋅cos(γ) + sin(β)⋅sin(γ)⋅cos(α)
    



```python
# beta
sin_beta = -r[2, 0]
cos_beta = simplify(sqrt(r[0, 0]**2 + r[1, 0]**2))

print()
pprint(sin_beta / cos_beta)
```

    
    1.0⋅sin(β)
    ──────────
     │cos(β)│ 



```python
# gamma
sin_gamma = r[2, 1]
cos_gamma = r[2, 2]

pprint(sin_gamma / cos_gamma)
```

    1.0⋅sin(γ)
    ──────────
      cos(γ)  



```python
# alpha 
sin_alpha = r[1, 0]
cos_alpha = r[0, 0]

pprint(sin_alpha / cos_alpha)
```

    1.0⋅sin(α)
    ──────────
      cos(α)  



```python
q4, q5, q6 = symbols('q4:7', real = True)
```


```python
R36 = Matrix([
  [-sin(q4)*sin(q6) + cos(q4)*cos(q5)*cos(q6), -sin(q4)*cos(q6) - sin(q6)*cos(q4)*cos(q5), -sin(q5)*cos(q4)],
  [                           sin(q5)*cos(q6),                           -sin(q5)*sin(q6),          cos(q5)],
  [-sin(q4)*cos(q5)*cos(q6) - sin(q6)*cos(q4),  sin(q4)*sin(q6)*cos(q5) - cos(q4)*cos(q6),  sin(q4)*sin(q5)]])

pprint(R36)
```

    ⎡-sin(q₄)⋅sin(q₆) + cos(q₄)⋅cos(q₅)⋅cos(q₆)  -sin(q₄)⋅cos(q₆) - sin(q₆)⋅cos(q₄
    ⎢                                                                             
    ⎢             sin(q₅)⋅cos(q₆)                             -sin(q₅)⋅sin(q₆)    
    ⎢                                                                             
    ⎣-sin(q₄)⋅cos(q₅)⋅cos(q₆) - sin(q₆)⋅cos(q₄)  sin(q₄)⋅sin(q₆)⋅cos(q₅) - cos(q₄)
    
    )⋅cos(q₅)  -sin(q₅)⋅cos(q₄)⎤
                               ⎥
                   cos(q₅)     ⎥
                               ⎥
    ⋅cos(q₆)   sin(q₄)⋅sin(q₅) ⎦



```python
for m in [0, 1, 2]:
  for n in [0, 1, 2]: 
    
    print()
    print("R" + str(m) + str(n))
    pprint(R36[0, 1])
    print()
```

    
    R00
    -sin(q₄)⋅cos(q₆) - sin(q₆)⋅cos(q₄)⋅cos(q₅)
    
    
    R01
    -sin(q₄)⋅cos(q₆) - sin(q₆)⋅cos(q₄)⋅cos(q₅)
    
    
    R02
    -sin(q₄)⋅cos(q₆) - sin(q₆)⋅cos(q₄)⋅cos(q₅)
    
    
    R10
    -sin(q₄)⋅cos(q₆) - sin(q₆)⋅cos(q₄)⋅cos(q₅)
    
    
    R11
    -sin(q₄)⋅cos(q₆) - sin(q₆)⋅cos(q₄)⋅cos(q₅)
    
    
    R12
    -sin(q₄)⋅cos(q₆) - sin(q₆)⋅cos(q₄)⋅cos(q₅)
    
    
    R20
    -sin(q₄)⋅cos(q₆) - sin(q₆)⋅cos(q₄)⋅cos(q₅)
    
    
    R21
    -sin(q₄)⋅cos(q₆) - sin(q₆)⋅cos(q₄)⋅cos(q₅)
    
    
    R22
    -sin(q₄)⋅cos(q₆) - sin(q₆)⋅cos(q₄)⋅cos(q₅)
    



```python
#q4 

sin_q4 = R36[2, 2]
cos_q4 =  -R36[0, 2]
pprint(sin_q4 / cos_q4)
```

    sin(q₄)
    ───────
    cos(q₄)



```python
#q5 

sin_q5 = sqrt(R36[0, 2]**2 + R36[2, 2]**2) 
cos_q5 = R36[1, 2]
pprint(trigsimp(sin_q5 / cos_q5))
```

    │sin(q₅)│
    ─────────
     cos(q₅) 



```python
#q6

sin_q6 = -R36[1, 1]
cos_q6 = R36[1, 0] 
pprint(sin_q6 / cos_q6)
```

    sin(q₆)
    ───────
    cos(q₆)



```python
def get_last_three_angles(R):
    
  sin_q4 = R[2, 2]
  cos_q4 =  -R[0, 2]
    
  sin_q5 = sqrt(R[0, 2]**2 + R[2, 2]**2) 
  cos_q5 = R[1, 2]
    
  sin_q6 = -R[1, 1]
  cos_q6 = R[1, 0] 
  
  q4 = atan2(sin_q4, cos_q4)
  q5 = atan2(sin_q5, cos_q5)
  q6 = atan2(sin_q6, cos_q6)
    
  return q4, q5, q6
```


```python
R = Matrix([
    [0.724598074610823,  -0.686235503719083,   0.0635489079820028], 
    [0.684428504026748,   0.727345057020894,   0.0502671950976013], 
    [-0.0807171180481332, 0.00707117123884714, 0.996711967115533]
])

print("q4: ", -1.507 + 3.14)
# q4 = 1.6344652724032285, q5 =1.5205000259943031, q6 = -0.81578130619968381

q4, q5, q6 = get_last_three_angles(R)
print("\n q4:", q4, "\n q5:", q5, "\n q6:", q6)

print("q4:", q4.evalf())
```

    q4:  1.6330000000000002
    
     q4: -1.50712396456497 + pi 
     q5: 1.52050793847535 
     q6: -0.815787839019348
    q4: 1.63446868902482



```python

```

d88e59a8802330ad2e8b0bb853b3e1df0c4b74c9
ipynb
Jupyter Notebook
playground/euler-angles.ipynb
mithi/arm-ik
e7e87ef0e43b04278d2300f67d863c3f7eafb77e
MIT
2017-07-29T11:40:03.000Z
2022-02-28T14:49:48.000Z
playground/euler-angles.ipynb
mithi/arm-ik
e7e87ef0e43b04278d2300f67d863c3f7eafb77e
MIT
playground/euler-angles.ipynb
mithi/arm-ik
e7e87ef0e43b04278d2300f67d863c3f7eafb77e
MIT
2017-10-27T13:30:21.000Z
2022-02-10T10:08:42.000Z
Qwen/Qwen-72B
1. YES
2. YES