# Determine derivative of Jacobian from angular velocity to exponential rates

Peter Corke 2021

SymPy code to deterine the time derivative of the mapping from angular velocity to exponential coordinate rates.


```python
from sympy import *
```

A rotation matrix can be expressed in terms of exponential coordinates (also called Euler vector)

$
\mathbf{R} = e^{[\varphi]_\times} 
$
where $\mathbf{R} \in SO(3)$ and $\varphi \in \mathbb{R}^3$.

The mapping from angular velocity $\omega$ to exponential coordinate rates $\dot{\varphi}$ is

$
\dot{\varphi} = \mathbf{A} \omega
$

where $\mathbf{A}$ is given by (2.107) of [Robot Dynamics Lecture Notes, Robotic Systems Lab, ETH Zurich, 2018](https://ethz.ch/content/dam/ethz/special-interest/mavt/robotics-n-intelligent-systems/rsl-dam/documents/RobotDynamics2018/RD_HS2018script.pdf)


$
\mathbf{A} = I_{3 \times 3} - \frac{1}{2} [v]_\times + [v]^2_\times \frac{1}{\theta^2} \left( 1 - \frac{\theta}{2} \frac{\sin \theta}{1 - \cos \theta} \right)
$
where $\theta = \| \varphi \|$ and $v = \hat{\varphi}$

We simplify the equation as

$
\mathbf{A} = I_{3 \times 3} - \frac{1}{2} [v]_\times + [v]^2_\times \Theta
$

where
$
\Theta = \frac{1}{\theta^2} \left( 1 - \frac{\theta}{2} \frac{\sin \theta}{1 - \cos \theta} \right)
$

We can find the derivative using the chain rule

$
\dot{\mathbf{A}} = - \frac{1}{2} [\dot{v}]_\times + 2 [v]_\times [\dot{v}]_\times \Theta + [v]^2_\times \dot{\Theta}
$

We start by defining some symbols


```python
Theta, theta, theta_dot, t = symbols('Theta theta theta_dot t', real=True)
```

We start by finding an expression for $\Theta$ which depends on $\theta(t)$


```python
theta_t = Function(theta)(t)
```


```python
Theta = 1 / theta_t ** 2 * (1 - theta_t / 2 * sin(theta_t) / (1 - cos(theta_t)))
Theta
```




$\displaystyle \frac{1 - \frac{\theta{\left(t \right)} \sin{\left(\theta{\left(t \right)} \right)}}{2 \left(1 - \cos{\left(\theta{\left(t \right)} \right)}\right)}}{\theta^{2}{\left(t \right)}}$



and now determine the derivative


```python
T_dot = Theta.diff(t)
T_dot
```




$\displaystyle - \frac{2 \left(1 - \frac{\theta{\left(t \right)} \sin{\left(\theta{\left(t \right)} \right)}}{2 \left(1 - \cos{\left(\theta{\left(t \right)} \right)}\right)}\right) \frac{d}{d t} \theta{\left(t \right)}}{\theta^{3}{\left(t \right)}} + \frac{- \frac{\theta{\left(t \right)} \cos{\left(\theta{\left(t \right)} \right)} \frac{d}{d t} \theta{\left(t \right)}}{2 \left(1 - \cos{\left(\theta{\left(t \right)} \right)}\right)} - \frac{\sin{\left(\theta{\left(t \right)} \right)} \frac{d}{d t} \theta{\left(t \right)}}{2 \left(1 - \cos{\left(\theta{\left(t \right)} \right)}\right)} + \frac{\theta{\left(t \right)} \sin^{2}{\left(\theta{\left(t \right)} \right)} \frac{d}{d t} \theta{\left(t \right)}}{2 \left(1 - \cos{\left(\theta{\left(t \right)} \right)}\right)^{2}}}{\theta^{2}{\left(t \right)}}$



which is a somewhat complex expression that depends on $\theta(t)$ and $\dot{\theta}(t)$.

We will remove the time dependency and generate code


```python
T_dot = T_dot.subs([(theta_t.diff(t), theta_dot), (theta_t, theta)])
```


```python
pycode(T_dot)
```




    '(-1/2*theta*theta_dot*math.cos(theta)/(1 - math.cos(theta)) + (1/2)*theta*theta_dot*math.sin(theta)**2/(1 - math.cos(theta))**2 - 1/2*theta_dot*math.sin(theta)/(1 - math.cos(theta)))/theta**2 - 2*theta_dot*(-1/2*theta*math.sin(theta)/(1 - math.cos(theta)) + 1)/theta**3'



In order to evaluate the line above we need an expression for $\theta$ and $\dot{\theta}$.  $\theta$ is the norm of $\varphi$ whose elements are functions of time


```python
phi_names = ('varphi_0', 'varphi_1', 'varphi_2')
phi = []  # names of angles, eg. theta
phi_t = []  # angles as function of time, eg. theta(t)
phi_d = []  # derivative of above, eg. d theta(t) / dt
phi_n = []  # symbol to represent above, eg. theta_dot
for i in phi_names:
    phi.append(symbols(i, real=True))
    phi_t.append(Function(phi[-1])(t))
    phi_d.append(phi_t[-1].diff(t))
    phi_n.append(i + '_dot')
```

Compute the norm


```python
theta = Matrix(phi_t).norm()
theta
```




$\displaystyle \sqrt{\varphi_{0}^{2}{\left(t \right)} + \varphi_{1}^{2}{\left(t \right)} + \varphi_{2}^{2}{\left(t \right)}}$



and find its derivative


```python
theta_dot = theta.diff(t)
theta_dot
```




$\displaystyle \frac{\varphi_{0}{\left(t \right)} \frac{d}{d t} \varphi_{0}{\left(t \right)} + \varphi_{1}{\left(t \right)} \frac{d}{d t} \varphi_{1}{\left(t \right)} + \varphi_{2}{\left(t \right)} \frac{d}{d t} \varphi_{2}{\left(t \right)}}{\sqrt{\varphi_{0}^{2}{\left(t \right)} + \varphi_{1}^{2}{\left(t \right)} + \varphi_{2}^{2}{\left(t \right)}}}$



and now remove the time dependenices


```python
theta_dot = theta_dot.subs(a for a in zip(phi_d, phi_n))
theta_dot = theta_dot.subs(a for a in zip(phi_t, phi))
theta_dot
```




$\displaystyle \frac{\varphi_{0} \varphi_{0 dot} + \varphi_{1} \varphi_{1 dot} + \varphi_{2} \varphi_{2 dot}}{\sqrt{\varphi_{0}^{2} + \varphi_{1}^{2} + \varphi_{2}^{2}}}$



which is simply the dot product over the norm.

4a43f1b4cf427658e64be73bb6f403f3ec4e74ec
ipynb
Jupyter Notebook
symbolic/angvelxform_dot.ipynb
dbkmgm/spatialmath-python
8d48e5a21334f9ceac4f549f194c79afaa22a5d7
MIT
symbolic/angvelxform_dot.ipynb
dbkmgm/spatialmath-python
8d48e5a21334f9ceac4f549f194c79afaa22a5d7
MIT
symbolic/angvelxform_dot.ipynb
dbkmgm/spatialmath-python
8d48e5a21334f9ceac4f549f194c79afaa22a5d7
MIT
Qwen/Qwen-72B
1. YES
2. YES