Algorithms and Software Solving a Motion Planning Problem for Nonholonomic Five-dimensional Control Systems

In this paper we consider a motion planning problem for nonlinear fifth dimensional systems with two-dimensional linear control. An iterative algorithm based on nilpotent approximation finds the approximate solution of the problem in the classes of piecewise constant and optimal controls. We present implementation of the algorithm in a parallel package for Wolfram Mathematica. The package was applied for the plate-ball problem and for the motion planning problem of car with two trailers on a plain.