We present a motion planning algorithm for cases where geometry of the robot cannot be neglected and where its dynamics are governed by non-holonomic constraints. While the two problems are classically treated separately, orientation of the robot strongly affects its possible motions both from the obstacle avoidance and from kinodynamic constraints perspective. We adopt an abstraction based approach ensuring asymptotic completeness. To handle the complex dynamics, a data driven approach is presented to construct a library of feedback motion primitives that guarantee a bounded error in following arbitrarily long trajectories. The library is constructed along local abstractions of the dynamics that enables addition of new motion primitives through abstraction refinement. Both the robot and the obstacles are represented as a union of circles, which allows arbitrarily precise approximation of complex geometries. To handle the geometrical constraints, we represent over- and under-approximations of the three-dimensional collision space as a finite set of two-dimensional "slices" corresponding to different intervals of the robot's orientation space. Starting from a coarse slicing, we use the collision space over-approximation to find a valid path and the under-approximation to check for  potential path non-existence. If none of the attempts are conclusive, the abstraction is refined. The algorithm is applied for motion planning and control of a rover with slipping without its prior modelling.

We propose a sampling-based motion planning algorithm for systems with complex dynamics and temporal logic specifications allowing to tackle sophisticated missions. By complex dynamics we refer to non-holonomy and  disturbance that prevent  implementation  of  an  exact steer function. We instead construct a set of feedback motion primitives guaranteeing bounded state uncertainty (and thus safety) allowing the system to follow an arbitrarily long trajectory without replanning. The motion primitives allow to use A*-based algorithm to provably accomplish the temporal logic mission. We propose a heuristics for the A*-based algorithm via construction of backward trees. We illustrate the approach on several case studies, including simulations of a rover and fixed wing drone. We further show that construction of backward trees allows for faster re-planning compared to the state-of-the-art. 

In this paper, we introduce a hybridization-based feedback control synthesis method for potentially unstable nonlinear continuous-time systems. We construct a discretization and feedback control that ensures dissipation form of incremental input-to-state stability property that a number of abstraction-based control methods rely on or benefit from. This enables the use of these methods also for the case of potentially unstable systems, to achieve a reachability or a temporal logic specification. We furthermore show that the algorithm can also improve abstraction-based methods that do not rely on stability assumptions by reducing the number of constructed abstract states and non-deterministic transitions. We illustrate the benefits of our approach in simulations featuring a cart-pendulum.

Unknown properties of a robot's environment are one of the sources of uncertainty in autonomous navigation. This uncertainty has to be accounted for when modelling robot dynamics. For ground vehicles in particular, terrain structure is one of the main environmental factors that can strongly influence the dynamics. Therefore, to ensure the ability of a robot to safely and efficiently navigate new environments, robust motion planning and control systems are needed. This paper investigates a data-driven approach to planning and control based on construction of robust motion primitives (MPs) and corresponding feedback rules that ensure a bounded error along the planned trajectory. The approach is tested in an exploration scenario in which a robot systematically inspects an area consisting of several terrain types with the aim of recognizing changes in dynamical properties, learning new dynamics models when such changes are detected and recording that information for future use. The advantage of incorporating the collected data into motion planning in multi-terrain environments is illustrated via simulation. 

In this paper, we address the safety of data-driven control for contact-rich manipulation. We propose to restrict the controller’s action space to keep the system in a set of safe states. In the absence of an analytical model, we show how Gaussian Processes (GP) can be used to approximate safe sets. We disable inputs for which the predicted states are likely to be unsafe using the GP. Furthermore, we show how locally designed feedback controllers can be used to improve the execution precision in the presence of modelling errors. We demonstrate the benefits of our method on a pushing task with a variety of dynamics, by using known and unknown surfaces and different object loads. Our results illustrate that the proposed approach significantly improves the performance and safety of the baseline controller.

In this paper, we address the safety and efficiency of data-driven model predictive controllers (DD-MPC) for systems with complex dynamics. First, we utilize safe exploration of dynamical systems to learn an accurate model for the DD-MPC. During training, we use rapidly exploring random trees (RRT) to collect a uniform distribution of data points in the state-input space and overcome the common distribution shift in model learning. This model is also used to construct a tree offline, which at test time is used in the cost function to provide an estimate of the predicted states' distance to the target. Additionally, we show how safe sets can be approximated using demonstrations of exclusively safe trajectories, i.e. positive examples. During test time, the distances of the predicted trajectories to the safe set are used as a cost term to encourage safe inputs. We use a \emph{broken} version of the inverted pendulum problem where the friction abruptly changes in certain regions as a running example. Our results show that the proposed exploration algorithm and the two proposed cost terms lead to a controller that can effectively avoid unsafe states and displays higher success rates than the baseline controllers with models from controlled demonstrations and even random actions.