Experience-driven optimal motion synthesis in complex and shared environments
Merkt, Wolfgang Xaver
Optimal loco-manipulation planning and control for high-dimensional systems based on general, non-linear optimisation allows for the specification of versatile motion subject to complex constraints. However, complex, non-linear system and environment dynamics, switching contacts, and collision avoidance in cluttered environments introduce non-convexity and discontinuity in the optimisation space. This renders finding optimal solutions in complex and changing environments an open and challenging problem in robotics. Global optimisation methods can take a prohibitively long time to converge. Slow convergence makes them unsuitable for live deployment and online re-planning of motion policies in response to changes in the task or environment. Local optimisation techniques, in contrast, converge fast within the basin of attraction of a minimum but may not converge at all without a good initial guess as they can easily get stuck in local minima. Local methods are, therefore, a suitable choice provided we can supply a good initial guess. If a similarity between problems can be found and exploited, a memory of optimal solutions can be computed and compressed efficiently in an offline computation process. During runtime, we can query this memory to bootstrap motion synthesis by providing a good initial seed to the local optimisation solver. In order to realise such a system, we need to address several connected problems and questions: First, the formulation of the optimisation problem (and its parametrisation to allow solutions to transfer to new scenarios), and related, the type and granularity of user input, along with a strategy for recovery and feedback in case of unexpected changes or failure. Second, a sampling strategy during the database/memory generation that explores the parameter space efficiently without resorting to exhaustive measures---i.e., to balance storage size/memory with online runtime to adapt/repair the initial guess. Third, the question of how to represent the problem and environment to parametrise, compute, store, retrieve, and exploit the memory efficiently during pre-computation and runtime. One strategy to make the problem computationally tractable is to decompose planning into a series of sequential sub-problems, e.g., contact-before-motion approaches which sequentially perform goal state planning, contact planning, motion planning, and encoding. Here, subsequent stages operate within the null-space of the constraints of the prior problem, such as the contact mode or sequence. This doctoral thesis follows this line of work. It investigates general optimisation-based formulations for motion synthesis along with a strategy for exploration, encoding, and exploitation of a versatile memory-of-motion for providing an initial guess to optimisation solvers. In particular, we focus on manipulation in complex environments with high-dimensional robot systems such as humanoids and mobile manipulators. The first part of this thesis focuses on collision-free motion generation to reliably generate motions. We present a general, collision-free inverse kinematics method using a combination of gradient-based local optimisation with random/evolution strategy restarting to achieve high success rates and avoid local minima. We use formulations for discrete collision avoidance and introduce a novel, computationally fast continuous collision avoidance objective based on conservative advancement and harmonic potential fields. Using this, we can synthesise continuous-time collision-free motion plans in the presence of moving obstacles. It further enables to discretise trajectories with fewer waypoints, which in turn considerably reduces the optimisation problem complexity, and thus, time to solve. The second part focuses on problem representations and exploration. We first introduce an efficient solution encoding for trajectory library-based approaches. This representation, paired with an accompanying exploration strategy for offline pre-computation, permits the application of inexpensive distance metrics during runtime. We demonstrate how our method efficiently re-uses trajectory samples, increases planning success rates, and reduces planning time while being highly memory-efficient. We subsequently present a method to explore the topological features of the solution space using tools from computational homology. This enables us to cluster solutions according to their inherent structure which increases the success of warm-starting for problems with discontinuities and multi-modality. The third part focuses on real-world deployment in laboratory and field experiments as well as incorporating user input. We present a framework for robust shared autonomy with a focus on continuous scene monitoring for assured safety. This framework further supports interactive adjustment of autonomy levels from fully teleoperated to automatic execution of stored behaviour sequences. Finally, we present sensing and control for the integration and embodiment of the presented methodology in high-dimensional real-world platforms used in laboratory experiments and real-world deployment. We validate our presented methods using hardware experiments on a variety of robot platforms demonstrating generalisation to other robots and environments.