# Determine derivatives of angular sequence to rotation matrix

Peter Corke 2021

SymPy code to evaluate the mappings from rotation angle sequences to rotation matrix, then compute the derivatives to determine the mapping from angular rates to angular velocity.

Adjust the next cell to choose the particular angle sequence.


```python
from spatialmath.base import *
from sympy import *
```


```python
angle_names = ('alpha', 'beta', 'gamma')
func = eul2r

#angle_names =  ('phi', 'theta', 'psi')
#func = lambda Gamma: rpy2r(Gamma, order='xyz')
```

Define a symbol for time


```python
t = symbols('t')
```

Define arrays of symbols for the angles, angle as function of time, time derivative of angle as a function of time


```python
angle = []  # names of angles, eg. theta
anglet = []  # angles as function of time, eg. theta(t)
angled = []  # derivative of above, eg. d theta(t) / dt
angledn = []  # symbol to represent above, eg. theta_dot
for i in angle_names:
    angle.append(symbols(i))
    anglet.append(Function(i)(t))
    angled.append(anglet[-1].diff(t))
    angledn.append(i + '_dot')
```

Compute the rotation matrix


```python
R = Matrix(func(anglet))
R
```




$\displaystyle \left[\begin{matrix}- \sin{\left(\alpha{\left(t \right)} \right)} \sin{\left(\gamma{\left(t \right)} \right)} + \cos{\left(\alpha{\left(t \right)} \right)} \cos{\left(\beta{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)} & - \sin{\left(\alpha{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)} - \sin{\left(\gamma{\left(t \right)} \right)} \cos{\left(\alpha{\left(t \right)} \right)} \cos{\left(\beta{\left(t \right)} \right)} & \sin{\left(\beta{\left(t \right)} \right)} \cos{\left(\alpha{\left(t \right)} \right)}\\\sin{\left(\alpha{\left(t \right)} \right)} \cos{\left(\beta{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)} + \sin{\left(\gamma{\left(t \right)} \right)} \cos{\left(\alpha{\left(t \right)} \right)} & - \sin{\left(\alpha{\left(t \right)} \right)} \sin{\left(\gamma{\left(t \right)} \right)} \cos{\left(\beta{\left(t \right)} \right)} + \cos{\left(\alpha{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)} & \sin{\left(\alpha{\left(t \right)} \right)} \sin{\left(\beta{\left(t \right)} \right)}\\- \sin{\left(\beta{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)} & \sin{\left(\beta{\left(t \right)} \right)} \sin{\left(\gamma{\left(t \right)} \right)} & \cos{\left(\beta{\left(t \right)} \right)}\end{matrix}\right]$



Compute its time derivative


```python
Rdot = Matrix(R).diff(t)
Rdot
```




$\displaystyle \left[\begin{matrix}- \sin{\left(\alpha{\left(t \right)} \right)} \cos{\left(\beta{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \alpha{\left(t \right)} - \sin{\left(\alpha{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \gamma{\left(t \right)} - \sin{\left(\beta{\left(t \right)} \right)} \cos{\left(\alpha{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \beta{\left(t \right)} - \sin{\left(\gamma{\left(t \right)} \right)} \cos{\left(\alpha{\left(t \right)} \right)} \cos{\left(\beta{\left(t \right)} \right)} \frac{d}{d t} \gamma{\left(t \right)} - \sin{\left(\gamma{\left(t \right)} \right)} \cos{\left(\alpha{\left(t \right)} \right)} \frac{d}{d t} \alpha{\left(t \right)} & \sin{\left(\alpha{\left(t \right)} \right)} \sin{\left(\gamma{\left(t \right)} \right)} \cos{\left(\beta{\left(t \right)} \right)} \frac{d}{d t} \alpha{\left(t \right)} + \sin{\left(\alpha{\left(t \right)} \right)} \sin{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \gamma{\left(t \right)} + \sin{\left(\beta{\left(t \right)} \right)} \sin{\left(\gamma{\left(t \right)} \right)} \cos{\left(\alpha{\left(t \right)} \right)} \frac{d}{d t} \beta{\left(t \right)} - \cos{\left(\alpha{\left(t \right)} \right)} \cos{\left(\beta{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \gamma{\left(t \right)} - \cos{\left(\alpha{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \alpha{\left(t \right)} & - \sin{\left(\alpha{\left(t \right)} \right)} \sin{\left(\beta{\left(t \right)} \right)} \frac{d}{d t} \alpha{\left(t \right)} + \cos{\left(\alpha{\left(t \right)} \right)} \cos{\left(\beta{\left(t \right)} \right)} \frac{d}{d t} \beta{\left(t \right)}\\- \sin{\left(\alpha{\left(t \right)} \right)} \sin{\left(\beta{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \beta{\left(t \right)} - \sin{\left(\alpha{\left(t \right)} \right)} \sin{\left(\gamma{\left(t \right)} \right)} \cos{\left(\beta{\left(t \right)} \right)} \frac{d}{d t} \gamma{\left(t \right)} - \sin{\left(\alpha{\left(t \right)} \right)} \sin{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \alpha{\left(t \right)} + \cos{\left(\alpha{\left(t \right)} \right)} \cos{\left(\beta{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \alpha{\left(t \right)} + \cos{\left(\alpha{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \gamma{\left(t \right)} & \sin{\left(\alpha{\left(t \right)} \right)} \sin{\left(\beta{\left(t \right)} \right)} \sin{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \beta{\left(t \right)} - \sin{\left(\alpha{\left(t \right)} \right)} \cos{\left(\beta{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \gamma{\left(t \right)} - \sin{\left(\alpha{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \alpha{\left(t \right)} - \sin{\left(\gamma{\left(t \right)} \right)} \cos{\left(\alpha{\left(t \right)} \right)} \cos{\left(\beta{\left(t \right)} \right)} \frac{d}{d t} \alpha{\left(t \right)} - \sin{\left(\gamma{\left(t \right)} \right)} \cos{\left(\alpha{\left(t \right)} \right)} \frac{d}{d t} \gamma{\left(t \right)} & \sin{\left(\alpha{\left(t \right)} \right)} \cos{\left(\beta{\left(t \right)} \right)} \frac{d}{d t} \beta{\left(t \right)} + \sin{\left(\beta{\left(t \right)} \right)} \cos{\left(\alpha{\left(t \right)} \right)} \frac{d}{d t} \alpha{\left(t \right)}\\\sin{\left(\beta{\left(t \right)} \right)} \sin{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \gamma{\left(t \right)} - \cos{\left(\beta{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \beta{\left(t \right)} & \sin{\left(\beta{\left(t \right)} \right)} \cos{\left(\gamma{\left(t \right)} \right)} \frac{d}{d t} \gamma{\left(t \right)} + \sin{\left(\gamma{\left(t \right)} \right)} \cos{\left(\beta{\left(t \right)} \right)} \frac{d}{d t} \beta{\left(t \right)} & - \sin{\left(\beta{\left(t \right)} \right)} \frac{d}{d t} \beta{\left(t \right)}\end{matrix}\right]$



Get angular velocity vector in terms of angles and angle rates


```python
omega = Matrix(vex(Rdot * R.T))
```

For each element of this 3x1 matrix get the coefficients of each angle derivative


```python
A = Matrix.zeros(3,3)
for i in range(3):
    e = omega[i,0].expand()
    for j in range(3):
        A[i, j] = e.coeff(angled[j])
```

The result is a 3x3 matrix. Mapping from angle rates to angular velocity. We subsitute angle as a function of time to plain angle, then simplify.


```python
A = trigsimp(A.subs(a for a in zip(anglet, angle)))
```

Compute the inverse and simplify.  Mapping from angular velocity to angle rates.


```python
Ai = trigsimp(A.inv())
```

Render as code


```python
pycode(A).replace('ImmutableDenseMatrix', 'np.array')
```




    'np.array([[0, -math.sin(alpha), math.sin(beta)*math.cos(alpha)], [0, math.cos(alpha), math.sin(alpha)*math.sin(beta)], [1, 0, math.cos(beta)]])'




```python
pycode(Ai).replace('ImmutableDenseMatrix', 'np.array')
```




    'np.array([[-math.cos(alpha)/math.tan(beta), -math.sin(alpha)/math.tan(beta), 1], [-math.sin(alpha), math.cos(alpha), 0], [math.cos(alpha)/math.sin(beta), math.sin(alpha)/math.sin(beta), 0]])'



Compute the time derivative of `Ai`, from angular acceleration to angle acceleration


```python
Ai = Ai.subs(a for a in zip(angle, anglet))
```


```python
Ai_dot = trigsimp(Ai.diff(t).subs(a for a in zip(angled, angledn)).subs(a for a in zip(anglet, angle)))
Ai_dot
```




$\displaystyle \left[\begin{matrix}\frac{\alpha_{dot} \sin{\left(\alpha \right)}}{\tan{\left(\beta \right)}} + \frac{\beta_{dot} \cos{\left(\alpha \right)}}{\sin^{2}{\left(\beta \right)}} & - \frac{\alpha_{dot} \cos{\left(\alpha \right)}}{\tan{\left(\beta \right)}} + \frac{\beta_{dot} \sin{\left(\alpha \right)}}{\sin^{2}{\left(\beta \right)}} & 0\\- \alpha_{dot} \cos{\left(\alpha \right)} & - \alpha_{dot} \sin{\left(\alpha \right)} & 0\\- \frac{\alpha_{dot} \sin{\left(\alpha \right)} + \frac{\beta_{dot} \cos{\left(\alpha \right)} \cos{\left(\beta \right)}}{\sin{\left(\beta \right)}}}{\sin{\left(\beta \right)}} & \frac{\alpha_{dot} \cos{\left(\alpha \right)} - \frac{\beta_{dot} \sin{\left(\alpha \right)} \cos{\left(\beta \right)}}{\sin{\left(\beta \right)}}}{\sin{\left(\beta \right)}} & 0\end{matrix}\right]$




```python
pycode(Ai_dot).replace('ImmutableDenseMatrix', 'np.array')
```




    'np.array([[alpha_dot*math.sin(alpha)/math.tan(beta) + beta_dot*math.cos(alpha)/math.sin(beta)**2, -alpha_dot*math.cos(alpha)/math.tan(beta) + beta_dot*math.sin(alpha)/math.sin(beta)**2, 0], [-alpha_dot*math.cos(alpha), -alpha_dot*math.sin(alpha), 0], [-(alpha_dot*math.sin(alpha) + beta_dot*math.cos(alpha)*math.cos(beta)/math.sin(beta))/math.sin(beta), (alpha_dot*math.cos(alpha) - beta_dot*math.sin(alpha)*math.cos(beta)/math.sin(beta))/math.sin(beta), 0]])'




```python

```

74068f1fdf5999066b7375c94f1f12843a3ce06f
ipynb
Jupyter Notebook
symbolic/angvelxform.ipynb
jungr-ait/spatialmath-python
140d499e733ed9775762df90d36e4b2c4c2fc6eb
MIT
2020-04-24T02:49:36.000Z
2022-03-31T16:13:38.000Z
symbolic/angvelxform.ipynb
jungr-ait/spatialmath-python
140d499e733ed9775762df90d36e4b2c4c2fc6eb
MIT
2020-05-21T04:13:33.000Z
2022-02-15T12:46:17.000Z
symbolic/angvelxform.ipynb
jungr-ait/spatialmath-python
140d499e733ed9775762df90d36e4b2c4c2fc6eb
MIT
2020-05-06T11:22:55.000Z
2022-03-21T14:15:16.000Z
Qwen/Qwen-72B
1. YES
2. YES