# R36 DERIVATION

**We know the following from our geometry:**
```
1. R0u = R06 * R6g * Rgu 
2. R03 = R03 * R36
3. R6g = I
4. R0u = rotz(yaw) * roty(pitch) * rotx(roll)
5. Rgu = rotz(pi) * roty(-pi/2)
```

**For any matrix recall the following properties:**
```
1. R = I * R = R * I
2. (A * B) * C = A * (B * C)
3. if A = B then A * C = B * C and vice versa
4. if A = B then C * A = C * C and vice versa
5. R * R.inverse() = R.inverse() * R = I 
```

**Note that for rotation matrices**
```
R.inverse() = R.T 
Therefore: R.T * R = R * R.T = I 
```

**From the discussion above, you can do the following:** 
- _Subsitution_  
- - `R0u = (R03 * R36) * I * Rgu`
- _Multiply both sides by `Rgu.inverse() = Rgu.T` at the right_
- - `R0u * Rgu.T = R03 * R36 `
- _Multiply both sides by `R03.inverse() = R03.T` at the left_ 
- - `R03.T * R0u * Rgu.T = R36 `

**We conclude then that:**
```
R36 = R03.T * R0u * Rgu.T
R36 = R03.T * R0g
where:
R0u = rotz(yaw) * roty(pitch) * rotx(roll)
Rgu = rotz(pi) * roty(-pi/2)
```
`



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

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
q1, q2, q3, q4, q5, q6= symbols('q1:7')

R03 = Matrix([
[sin(q2 + q3)*cos(q1), cos(q1)*cos(q2 + q3), -sin(q1)],
[sin(q1)*sin(q2 + q3), sin(q1)*cos(q2 + q3),  cos(q1)],
[        cos(q2 + q3),        -sin(q2 + q3),        0]])

R03T = R03.T
pprint(R03T)
```

    ⎡sin(q₂ + q₃)⋅cos(q₁)  sin(q₁)⋅sin(q₂ + q₃)  cos(q₂ + q₃) ⎤
    ⎢                                                         ⎥
    ⎢cos(q₁)⋅cos(q₂ + q₃)  sin(q₁)⋅cos(q₂ + q₃)  -sin(q₂ + q₃)⎥
    ⎢                                                         ⎥
    ⎣      -sin(q₁)              cos(q₁)               0      ⎦



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

```


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

    ⎡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
RugT = (rotz(pi) * roty(-pi/2)).T
pprint(RugT)
```

    ⎡0   0    1.0⎤
    ⎢            ⎥
    ⎢0  -1.0   0 ⎥
    ⎢            ⎥
    ⎣1   0     0 ⎦



```python
R36 = R03T * R0u * RugT
pprint(R36)
```

    ⎡(1.0⋅sin(α)⋅sin(γ) + sin(β)⋅cos(α)⋅cos(γ))⋅sin(q₂ + q₃)⋅cos(q₁) + (sin(α)⋅sin
    ⎢                                                                             
    ⎢(1.0⋅sin(α)⋅sin(γ) + sin(β)⋅cos(α)⋅cos(γ))⋅cos(q₁)⋅cos(q₂ + q₃) + (sin(α)⋅sin
    ⎢                                                                             
    ⎣                             -(1.0⋅sin(α)⋅sin(γ) + sin(β)⋅cos(α)⋅cos(γ))⋅sin(
    
    (β)⋅cos(γ) - 1.0⋅sin(γ)⋅cos(α))⋅sin(q₁)⋅sin(q₂ + q₃) + 1.0⋅cos(β)⋅cos(γ)⋅cos(q
                                                                                  
    (β)⋅cos(γ) - 1.0⋅sin(γ)⋅cos(α))⋅sin(q₁)⋅cos(q₂ + q₃) - 1.0⋅sin(q₂ + q₃)⋅cos(β)
                                                                                  
    q₁) + (sin(α)⋅sin(β)⋅cos(γ) - 1.0⋅sin(γ)⋅cos(α))⋅cos(q₁)                      
    
    ₂ + q₃)  -1.0⋅(-1.0⋅sin(α)⋅cos(γ) + sin(β)⋅sin(γ)⋅cos(α))⋅sin(q₂ + q₃)⋅cos(q₁)
                                                                                  
    ⋅cos(γ)  -1.0⋅(-1.0⋅sin(α)⋅cos(γ) + sin(β)⋅sin(γ)⋅cos(α))⋅cos(q₁)⋅cos(q₂ + q₃)
                                                                                  
                                           1.0⋅(-1.0⋅sin(α)⋅cos(γ) + sin(β)⋅sin(γ)
    
     - 1.0⋅(sin(α)⋅sin(β)⋅sin(γ) + 1.0⋅cos(α)⋅cos(γ))⋅sin(q₁)⋅sin(q₂ + q₃) - 1.0⋅s
                                                                                  
     - 1.0⋅(sin(α)⋅sin(β)⋅sin(γ) + 1.0⋅cos(α)⋅cos(γ))⋅sin(q₁)⋅cos(q₂ + q₃) + 1.0⋅s
                                                                                  
    ⋅cos(α))⋅sin(q₁) - 1.0⋅(sin(α)⋅sin(β)⋅sin(γ) + 1.0⋅cos(α)⋅cos(γ))⋅cos(q₁)     
    
    in(γ)⋅cos(β)⋅cos(q₂ + q₃)  1.0⋅sin(α)⋅sin(q₁)⋅sin(q₂ + q₃)⋅cos(β) - 1.0⋅sin(β)
                                                                                  
    in(γ)⋅sin(q₂ + q₃)⋅cos(β)  1.0⋅sin(α)⋅sin(q₁)⋅cos(β)⋅cos(q₂ + q₃) + 1.0⋅sin(β)
                                                                                  
                                                         1.0⋅sin(α)⋅cos(β)⋅cos(q₁)
    
    ⋅cos(q₂ + q₃) + 1.0⋅sin(q₂ + q₃)⋅cos(α)⋅cos(β)⋅cos(q₁)⎤
                                                          ⎥
    ⋅sin(q₂ + q₃) + 1.0⋅cos(α)⋅cos(β)⋅cos(q₁)⋅cos(q₂ + q₃)⎥
                                                          ⎥
     - 1.0⋅sin(q₁)⋅cos(α)⋅cos(β)                          ⎦



```python
q1, q2, q3, q4, q5, q6= symbols('q1:7')

R03 = Matrix([
[sin(q2 + q3)*cos(q1), cos(q1)*cos(q2 + q3), -sin(q1)],
[sin(q1)*sin(q2 + q3), sin(q1)*cos(q2 + q3),  cos(q1)],
[        cos(q2 + q3),        -sin(q2 + q3),        0]])

R03T = R03.T

alpha, beta, gamma = symbols('alpha beta gamma')
R0u = rotz(alpha) * roty(beta) * rotx(gamma)

RugT = (rotz(pi) * roty(-pi/2)).T
R36 = R03T * R0u * RugT


roll, pitch, yaw = 0.366, -0.078, 2.561

variables = {
  q1: 1.01249, 
  q2: -0.2758, 
  q3: -0.11568,
  alpha: yaw,
  beta: pitch, 
  gamma: roll
}

R0g = R0u * Rug
R36eval = R36.evalf(subs = variables)

pprint(R36eval)

print(get_spherical_ik(R36eval))
```

    ⎡ 0.724598074610823   -0.686235503719083   0.0635489079820028⎤
    ⎢                                                            ⎥
    ⎢ 0.684428504026748    0.727345057020894   0.0502671950976013⎥
    ⎢                                                            ⎥
    ⎣-0.0807171180481332  0.00707117123884714  0.996711967115533 ⎦
    (0.756897144385541, 0.0808050247662215, 0.00709437915922838)



```python
print(simplify(R03I) == R03.T)
print(Rug == Rug.T)
```

    True
    True



```python
R03 = Matrix([
[sin(q2 + q3)*cos(q1), cos(q1)*cos(q2 + q3), -sin(q1)],
[sin(q1)*sin(q2 + q3), sin(q1)*cos(q2 + q3),  cos(q1)],
[        cos(q2 + q3),        -sin(q2 + q3),        0]])

roll, pitch, yaw = 0.366, -0.078, 2.561
R0u = rotz(yaw)* roty(pitch) * rotx(roll)
Rug = (rotz(pi) * roty(-pi/2)).T

R0u.evalf(subs = {alpha: yaw, beta: pitch, gamma: roll})
R36eval2 = R36.evalf(subs = {q1: 1.01249, q2: -0.2758, q3: -0.11568})
pprint(R36eval2)
```

    ⎡ 0.724598074610823   -0.686235503719083   0.0635489079820028⎤
    ⎢                                                            ⎥
    ⎢ 0.684428504026748    0.727345057020894   0.0502671950976012⎥
    ⎢                                                            ⎥
    ⎣-0.0807171180481332  0.00707117123884723  0.996711967115533 ⎦



```python


def ik(R):
  r12, r13 = R[0,1], R[0,2]
  r21, r22, r23 = R[1,0], R[1,1], R[1,2] 
  r32, r33 = R[2,1], R[2,2]
  q5 = atan2(sqrt(r13**2 + r33**2), r23)
  q4 = atan2(r33, -r13)
  q6 = atan2(-r22, r21)
  return q4.evalf(), q5.evalf(), q6.evalf()

print(ik(R36eval))

print(R36eval)
```

    (1.63446868902482, 1.52050793847535, -0.815787839019348)
    Matrix([[0.724598074610823, -0.686235503719083, 0.0635489079820028], [0.684428504026748, 0.727345057020894, 0.0502671950976013], [-0.0807171180481332, 0.00707117123884714, 0.996711967115533]])



```python
R0g = rotz(yaw) * roty(pitch) * rotx(roll) * Rug
print(R0g)
```

    Matrix([[0.257143295038827, 0.488872082559650, -0.833595473062544], [0.259329420712765, 0.796053601157403, 0.546851822377060], [0.930927267496960, -0.356795110642117, 0.0779209320563015]])



```python
R03T_eval =R03T.evalf(subs = { q1: 1.01249, q2: -0.2758, q3: -0.11568})
pprint(R03T_eval)
pprint(R03T)
```

    ⎡-0.202129925357168  -0.323618808899201  0.924345368248129⎤
    ⎢                                                         ⎥
    ⎢0.489672387232293   0.783986806638987   0.381556863649747⎥
    ⎢                                                         ⎥
    ⎣-0.848153551226034  0.529750463466212           0        ⎦
    ⎡sin(q₂ + q₃)⋅cos(q₁)  sin(q₁)⋅sin(q₂ + q₃)  cos(q₂ + q₃) ⎤
    ⎢                                                         ⎥
    ⎢cos(q₁)⋅cos(q₂ + q₃)  sin(q₁)⋅cos(q₂ + q₃)  -sin(q₂ + q₃)⎥
    ⎢                                                         ⎥
    ⎣      -sin(q₁)              cos(q₁)               0      ⎦



```python

```

9fd46f354a5966a784b4555ce1032401423792b9
ipynb
Jupyter Notebook
playground/r36-eval-playground.ipynb
mithi/arm-ik
e7e87ef0e43b04278d2300f67d863c3f7eafb77e
MIT
2017-07-29T11:40:03.000Z
2022-02-28T14:49:48.000Z
playground/r36-eval-playground.ipynb
mithi/arm-ik
e7e87ef0e43b04278d2300f67d863c3f7eafb77e
MIT
playground/r36-eval-playground.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