```python
import sympy as s
```


```python
phi,x,y = s.symbols('phi,x,y')
phi_d,x_d,y_d = s.symbols('phi_d,x_d,y_d')
x_i,y_i,beta_i,gamma_i,r_i = s.symbols('x_i,y_i,beta_i,gamma_i,r_i')
q_d = s.symbols('q_d')
```


```python
q_d = s.Matrix([[phi_d],[x_d],[y_d]])
q_d
```




$\displaystyle \left[\begin{matrix}\phi_{d}\\x_{d}\\y_{d}\end{matrix}\right]$




```python
first_trans = s.Matrix([[1,0,0],
                        [0,s.cos(phi),s.sin(phi)],
                        [0,-s.sin(phi),s.cos(phi)]])
first_trans
```




$\displaystyle \left[\begin{matrix}1 & 0 & 0\\0 & \cos{\left(\phi \right)} & \sin{\left(\phi \right)}\\0 & - \sin{\left(\phi \right)} & \cos{\left(\phi \right)}\end{matrix}\right]$




```python
second_trans = s.Matrix([[-y_i,1,0],
                         [x_i,0,1]])
second_trans
```




$\displaystyle \left[\begin{matrix}- y_{i} & 1 & 0\\x_{i} & 0 & 1\end{matrix}\right]$




```python
third_trans = s.Matrix([[s.cos(beta_i),s.sin(beta_i)],
                        [-s.sin(beta_i),s.cos(beta_i)]])
third_trans
```




$\displaystyle \left[\begin{matrix}\cos{\left(\beta_{i} \right)} & \sin{\left(\beta_{i} \right)}\\- \sin{\left(\beta_{i} \right)} & \cos{\left(\beta_{i} \right)}\end{matrix}\right]$




```python
fourth_trans = s.Matrix([[1/r_i,s.tan(gamma_i)/r_i]])
fourth_trans
```




$\displaystyle \left[\begin{matrix}\frac{1}{r_{i}} & \frac{\tan{\left(\gamma_{i} \right)}}{r_{i}}\end{matrix}\right]$




```python
trans = fourth_trans@third_trans@second_trans@first_trans
trans
```




$\displaystyle \left[\begin{matrix}x_{i} \left(\frac{\sin{\left(\beta_{i} \right)}}{r_{i}} + \frac{\cos{\left(\beta_{i} \right)} \tan{\left(\gamma_{i} \right)}}{r_{i}}\right) - y_{i} \left(- \frac{\sin{\left(\beta_{i} \right)} \tan{\left(\gamma_{i} \right)}}{r_{i}} + \frac{\cos{\left(\beta_{i} \right)}}{r_{i}}\right) & - \left(\frac{\sin{\left(\beta_{i} \right)}}{r_{i}} + \frac{\cos{\left(\beta_{i} \right)} \tan{\left(\gamma_{i} \right)}}{r_{i}}\right) \sin{\left(\phi \right)} + \left(- \frac{\sin{\left(\beta_{i} \right)} \tan{\left(\gamma_{i} \right)}}{r_{i}} + \frac{\cos{\left(\beta_{i} \right)}}{r_{i}}\right) \cos{\left(\phi \right)} & \left(\frac{\sin{\left(\beta_{i} \right)}}{r_{i}} + \frac{\cos{\left(\beta_{i} \right)} \tan{\left(\gamma_{i} \right)}}{r_{i}}\right) \cos{\left(\phi \right)} + \left(- \frac{\sin{\left(\beta_{i} \right)} \tan{\left(\gamma_{i} \right)}}{r_{i}} + \frac{\cos{\left(\beta_{i} \right)}}{r_{i}}\right) \sin{\left(\phi \right)}\end{matrix}\right]$




```python
s.simplify(trans)
```




$\displaystyle \left[\begin{matrix}\frac{x_{i} \left(\sin{\left(\beta_{i} \right)} + \cos{\left(\beta_{i} \right)} \tan{\left(\gamma_{i} \right)}\right) + y_{i} \left(\sin{\left(\beta_{i} \right)} \tan{\left(\gamma_{i} \right)} - \cos{\left(\beta_{i} \right)}\right)}{r_{i}} & \frac{- \sin{\left(\beta_{i} + \phi \right)} \tan{\left(\gamma_{i} \right)} + \cos{\left(\beta_{i} + \phi \right)}}{r_{i}} & \frac{\sin{\left(\beta_{i} + \phi \right)} + \cos{\left(\beta_{i} + \phi \right)} \tan{\left(\gamma_{i} \right)}}{r_{i}}\end{matrix}\right]$




```python
s.simplify(trans*(r_i*s.cos(gamma_i))).T
```




$\displaystyle \left[\begin{matrix}x_{i} \sin{\left(\beta_{i} + \gamma_{i} \right)} - y_{i} \cos{\left(\beta_{i} + \gamma_{i} \right)}\\\cos{\left(\beta_{i} + \gamma_{i} + \phi \right)}\\\sin{\left(\beta_{i} + \gamma_{i} + \phi \right)}\end{matrix}\right]$




```python

```

949661e1ed0b08f16cb86d92418989958f205e80
ipynb
Jupyter Notebook
ch13_wheeled_mobile_robot_check_h_i.ipynb
dongilc/symbolic_modern_robotics
2439f26cdc29e73f12a82c7c1e0ac8b4ddc5a353
BSD-2-Clause
ch13_wheeled_mobile_robot_check_h_i.ipynb
dongilc/symbolic_modern_robotics
2439f26cdc29e73f12a82c7c1e0ac8b4ddc5a353
BSD-2-Clause
ch13_wheeled_mobile_robot_check_h_i.ipynb
dongilc/symbolic_modern_robotics
2439f26cdc29e73f12a82c7c1e0ac8b4ddc5a353
BSD-2-Clause
Qwen/Qwen-72B
1. YES
2. YES