# Development of Task-Space Nonholonomic Motion Planning Algorithm Based on Lie-Algebraic Method
## 2 Model and Algorithm
 \tag{5}\]  
where control-dependent coefficients \(\mathbf{\alpha}=(\alpha_{1},\alpha_{2},\ldots)^{T}\) pre-multiplying Lie monomials (vector fields) are expressed as  
\[ \begin{split}&\alpha_{1}(t)=\int_{0}^{t}u_{1}(s_{1})ds_{1},\quad \alpha_{2}(t)=\int_{0}^{t}u_{2}(s_{1})ds_{1},\\ &\alpha_{3}(t)=\frac{1}{2}\int_{0}^{t}\int_{0}^{s_{2}}(u_{1}(s_{1 })u_{2}(s_{2})-u_{2}(s_{1})u_{1}(s_{2}))ds_{1}ds_{2},\quad\ldots\end{split}  \tag{6}\]  
and they become more and more complex as the integration is performed over the \(k\)-dimensional simplex, where \(k\) is the degree of a Lie monomial the coefficient corresponds to. It is a common practice to express controls as linear combinations of time-dependent functions [7]  
\[u_{i}(t)=\sum_{j=0}^{N_{i}-1}\phi_{j}(t)p_{i}^{j},\quad i=1,\ldots,m,\quad t \in[0,T], \tag{7}\]  
where \(\phi_{j}(t)\) are elements of any functional basis (polynomials, harmonic functions) and \(N_{i}\) is the number of variables required to describe the \(i\)-th control. Thus, a vector \(\mathbf{p}\) collects all variables \(p_{i}^{j}\) and uniquely describes controls \(\mathbf{u}\) and also energy of controls. Moreover, the shift (5) can be expressed as a function of \(\mathbf{p}\) and resembles forward kinematics (but at a velocity level when considered on a fixed time horizon)  
\[\mathbf{z}(t)(\mathbf{q}_{c},\mathbf{p})=\mathbf{F}(\mathbf{p}). \tag{8}\]  
The shift (8) from the configuration space is mapped into the task-space, and within this space a desired motion towards the goal \(\mathbf{x}_{f}\) can be computed using the Newton algorithm. A complete algorithm to plan a motion of the system (1) with the output function (2), either in a configuration or a task-space, is presented in Algorithm 1. **Step 1. ** Read-in initial data:  
the system (1) together with the output function (2), the initial configuration \(\mathbf{q}_{0}\) and the goal point \(\mathbf{\dot{x}}_{f}\), desired accuracy of reaching the goal \(e\), the initial value of \(\mathbf{p}_{c}\). **Step 2. ** Initialize empty resulting trajectory \(\mathbf{q}_{res}(\cdot)\), and set the current configuration \(\mathbf{q}_{c}\leftarrow\mathbf{q}_{0}\). **Step 3. ** Check the stop condition: iff  
\[\|\mathbf{x}_{f}-\mathbf{k}(\mathbf{q}_{c})\|<e, \tag{9}\]  
then stop computations and output resulting trajectory \(\mathbf{q}_{res}(\cdot)\). Otherwise, progress with Step 4. **Step 4. ** Select a parameterization of controls (7), a time horizon \(T\), and derive mapping (8). **Step 5. ** At a current point in the task-space \(\mathbf{x}_{c}=\mathbf{k}(\mathbf{q}_{c})\), select the planned shift  
\[\Delta\mathbf{x}\leftarrow\xi(\mathbf{x}_{f}-\mathbf{x}_{c}) \tag{10}\]  
by setting a positive coefficient \(\xi\). **Step 6. ** Using the Newton algorithm, Ref. [18] find \(\mathbf{p}^{*}\) that solves the inverse kinematic task  
\[\Delta\mathbf{x}=\mathbf{J}(\mathbf{q}_{c})\mathbf{F}(\mathbf{p}), \tag{11}\]  
where \(\mathbf{J}(\mathbf{q})=\partial\mathbf{k}(\mathbf{q})/\partial\mathbf{q}\) is the Jacobi matrix of the mapping (2). **Step 7. ** Check whether the solution is acceptable:  
\[\|\mathbf{x}_{f}-\mathbf{k}(\mathbf{q}^{*}(T))\|<\|\mathbf{x}_{f}-\mathbf{x}_{c}\|, \tag{12}\]  
where \(\mathbf{q}^{*}(T)\) is the end-point of the trajectory \(\mathbf{q}^{*}(\cdot)\) initialized at \(\mathbf{q}_{c}\) and generated by the system (1) with controls \(\mathbf{u}^{*}(t)\), \(t\in[0,T]\), corresponding to the vector of parameters \(\mathbf{p}^{*}\), cf. Equation (7). **Step 8. ** If Condition (12) is satisfied, then:  
1. append the trajectory \(\mathbf{q}^{*}(\cdot)\) to the resulting trajectory \(\mathbf{q}_{res}(\cdot)\leftarrow\mathbf{q}_{res}(\cdot)\cup\mathbf{q}^{*}(\cdot)\),
2. update the current configuration \(\mathbf{q}_{c}\leftarrow\mathbf{q}^{*}(T)\)
3. and go back to Step 4. Otherwise, decrease the coefficient \(\xi\) and go to Step 5.