# Development of Task-Space Nonholonomic Motion Planning Algorithm Based on Lie-Algebraic Method
## 3 Simulations  
In order to evaluate Algorithm 1, some tests were performed using two models of mobile robots depicted in Figure 1. The unicycle is described by equations  
\[\mathbf{\dot{q}}= \begin{bmatrix}\dot{q}_{1}\\ \dot{q}_{2}\\ \dot{q}_{3}\end{bmatrix} = \begin{bmatrix}\cos(q_{3})&0\\ \sin(q_{3})&0\\ 0&1\end{bmatrix} \mathbf{u}= \begin{bmatrix}\cos(q_{3})\\ \sin(q_{3})\\ 0\end{bmatrix} u_{1}+ \begin{bmatrix}0\\ 0\\ 1\end{bmatrix} u_{2}=\mathbf{X}(\mathbf{q})u_{1}+\mathbf{Y}(\mathbf{q})u_{2}. \tag{14}\]  
The configuration vector \(\mathbf{q}\) is composed of position \((q_{1},q_{2})\) and orientation \(q_{3}\) of the vehicle on a plane, while \(u_{1},u_{2}\) denote controls interpreted as angular and linear velocities. The model of a kinematic car is given as  
\[\mathbf{\dot{q}}= \begin{bmatrix}L\cos(q_{3})\cos(q_{4})&0\\ L\sin(q_{3})\cos(q_{4})&0\\ \sin(q_{4})&0\\ 0&1\end{bmatrix} \mathbf{u}= \begin{bmatrix}L\cos(q_{3})\cos(q_{4})\\ L\sin(q_{3})\cos(q_{4})\\ \sin(q_{4})\\ 0\end{bmatrix} u_{1}+ \begin{bmatrix}0\\ 0\\ 0\\ 1\end{bmatrix} u_{2}=\mathbf{X}(\mathbf{q})\,u_{1}+\mathbf{Y}(\mathbf{q})\,u_{2}, \tag{15}\]  
where configuration \(\mathbf{q}\) has got an extra component \(q_{4}\) describing the steering wheel angle. For simplicity, it was assumed that \(L=1\). Both models are nonholonomic ones because generators \(\mathbf{X},\mathbf{Y}\) supplemented with the Lie bracket \([\mathbf{X},\mathbf{Y}]\) for the unicycle  
\[[\mathbf{X},\mathbf{Y}]= \begin{bmatrix}\sin(\mathbf{q}_{3})\\ -\cos(\mathbf{q}_{3})\\ 0\end{bmatrix} , \tag{16}\]  
and  
\[[\mathbf{X},\mathbf{Y}]= \begin{bmatrix}L\cos(\mathbf{q}_{3})\sin(\mathbf{q}_{4})\\ L\sin(\mathbf{q}_{3})\sin(\mathbf{q}_{4})\\ -\cos(\mathbf{q}_{4})\\ 0\end{bmatrix} ,\ \ [\mathbf{X},[\mathbf{X},\mathbf{Y}]]= \begin{bmatrix}-L\sin(\mathbf{q}_{3})\\ L\cos(\mathbf{q}_{3})\\ 0\\ 0\end{bmatrix} ,\ \ [\mathbf{Y},[\mathbf{X},\mathbf{Y}]]=\mathbf{X}. \tag{17}\]  
for the kinematic car, satisfy Condition (4). For both models, the position on the \(xy\) plane was selected as the output function  
\[\mathbf{k(q)}=(q_{1},q_{2})^{T}. \tag{18}\]  
If a robot is inscribed in a circle of sufficient radius, its orientation does not influence the possibility of collisions with obstacles. For the kinematic car another output function was used, coupling position on the \(xy\) plane and orientation of the vehicle  
\[\mathbf{k(q)}=(q_{1},q_{2},q_{3})^{T}, \tag{19}\]  
In practice, the orientation of the steering axle is not relevant. In order to check the motion planning Algorithm 1 within a configuration space, the identity output function was selected for both mobile robots. In all tested cases, an initial configuration was set to the vector composed of zeroes only  
\[\mathbf{q}_{0}=\mathbf{0}. \tag{20}\]  
In the tests, a side-way motion maneuver on the \(xy\)-plane was planned for both models and all output functions. The maneuver is challenging as it requires a motion along higher (than generators \(\mathbf{X,Y}\)) degree vector fields. Depending on a dimension of a task-space, goal points were selected as  
\[\mathbf{x}_{f}=(0,\delta),\quad\mathbf{x}_{f}=(0,\delta,0),\quad\delta\in\{0.05,0.1,0. 2,0.5,0.7,1\}. \tag{21}\]  
Any single task was run using three parameterizations of controls. Each control was composed of a few very first items of the orthonormal Fourier basis  
\[u_{1} =p_{1}^{1}\rho+p_{1}^{2}\sqrt{2}\rho\sin(\omega t)+p_{1}^{3}\sqrt {2}\rho\cos(\omega t), \tag{22}\] \[u_{2} =p_{2}^{1}\rho+p_{2}^{2}\sqrt{2}\rho\sin(\omega t)+p_{2}^{3}\sqrt {2}\rho\cos(\omega t)\] (23) \[u_{1} =p_{1}^{1}\rho+p_{1}^{2}\sqrt{2}\rho\sin(\omega t),\quad u_{2}=p _{2}^{1}\rho+p_{2}^{3}\sqrt{2}\rho\cos(\omega t)\] (24) \[u_{1} =p_{1}^{1}\rho+p_{1}^{3}\sqrt{2}\rho\cos(\omega t),\quad u_{2}=p _{2}^{1}\rho+p_{2}^{2}\sqrt{2}\rho\sin(\omega t) \tag{25}\]  
where \(\rho=\sqrt{1/T}\) and \(\omega=2\pi/T\). Later on, it was assumed that \(T=1\).