```python
import symbolic_modern_robotics as smr
import sympy as s
s.init_printing()
```


```python
# unit rotation axis
omega_hat = s.Matrix([[0],[0],[1]]) 
theta = s.pi/4 
# 3-vector form  of angular velocity
omega = omega_hat*theta 
omega
```




$\displaystyle \left[\begin{matrix}0\\0\\\frac{\pi}{4}\end{matrix}\right]$




```python
# get Unit axis of Rotation and theta from 3-vector of exponential coordinates 
smr.AxisAng3(omega) 
```




$\displaystyle \left( \left[\begin{matrix}0\\0\\1.0\end{matrix}\right], \  0.785398163397448\right)$




```python
# get so3(skew-symmetric matrix) from 3-vector of exponential coordinates 
so3 = smr.VecToso3(omega)
so3
```




$\displaystyle \left[\begin{matrix}0 & -0.785398163397448 & 0\\0.785398163397448 & 0 & 0\\0 & 0 & 0\end{matrix}\right]$




```python
# get 3-vector of exponential coordinates from so3(skew-symmetric matrix)
omega = smr.so3ToVec(so3)
omega
```




$\displaystyle \left[\begin{matrix}0\\0\\0.785398163397448\end{matrix}\right]$




```python
# get SO3(Rotation matrix) from so3(skew-symmetric matrix)
R = smr.MatrixExp3(so3)
R
```




$\displaystyle \left[\begin{matrix}0.707106781186548 & -0.707106781186547 & 0\\0.707106781186547 & 0.707106781186548 & 0\\0 & 0 & 1.0\end{matrix}\right]$




```python
# get so3(skew-symmetric matrix) from SO3(Rotation matrix)
so3 = smr.MatrixLog3(R)
so3
```




$\displaystyle \left[\begin{matrix}0 & -0.785398163397448 & 0\\0.785398163397448 & 0 & 0\\0 & 0 & 0\end{matrix}\right]$




```python
# 6-vector form twist, V 
V = s.Matrix([0,0,0,1,2,3])
V
```




$\displaystyle \left[\begin{matrix}0\\0\\0\\1\\2\\3\end{matrix}\right]$




```python
# get se3(twist 4x4 matrix) from 6-vector spatial velocity, V
se3 = smr.VecTose3(V)
se3
```




$\displaystyle \left[\begin{matrix}0 & 0 & 0 & 1.0\\0 & 0 & 0 & 2.0\\0 & 0 & 0 & 3.0\\0 & 0 & 0 & 0\end{matrix}\right]$




```python
V = smr.se3ToVec(se3)
```


```python
S, theta_dot = smr.AxisAng6(V)
S, theta_dot
```




$\displaystyle \left( \left[\begin{matrix}0\\0\\0\\0.267261241912424\\0.534522483824849\\0.801783725737273\end{matrix}\right], \  3.74165738677394\right)$




```python
# get 6-vector spatial velocity, V from se3(twist 4x4 matrix)
V = smr.se3ToVec(se3)
V
```




$\displaystyle \left[\begin{matrix}0\\0\\0\\1.0\\2.0\\3.0\end{matrix}\right]$




```python
T = smr.MatrixExp6(se3)
T
```




$\displaystyle \left[\begin{matrix}1 & 0 & 0 & 1.0\\0 & 1 & 0 & 2.0\\0 & 0 & 1 & 3.0\\0 & 0 & 0 & 1\end{matrix}\right]$




```python
se3 = smr.MatrixLog6(T)
se3
```




$\displaystyle \left[\begin{matrix}0 & 0 & 0 & 1.0\\0 & 0 & 0 & 2.0\\0 & 0 & 0 & 3.0\\0 & 0 & 0 & 0\end{matrix}\right]$




```python
help(smr.so3ToVec)
```

    Help on function so3ToVec in module symbolic_modern_robotics:
    
    so3ToVec(so3mat)
        ###### Rotation Matrix
        # Calculate 3-vector from so3
    



```python

```


```python

```

e9e61d9aaf1938285f71eceae26ce39d7e26a3f4
ipynb
Jupyter Notebook
.ipynb_checkpoints/Example_1_smr-checkpoint.ipynb
dongilc/symbolic_modern_robotics
2439f26cdc29e73f12a82c7c1e0ac8b4ddc5a353
BSD-2-Clause
.ipynb_checkpoints/Example_1_smr-checkpoint.ipynb
dongilc/symbolic_modern_robotics
2439f26cdc29e73f12a82c7c1e0ac8b4ddc5a353
BSD-2-Clause
.ipynb_checkpoints/Example_1_smr-checkpoint.ipynb
dongilc/symbolic_modern_robotics
2439f26cdc29e73f12a82c7c1e0ac8b4ddc5a353
BSD-2-Clause
Qwen/Qwen-72B
1. YES
2. YES

