Efficient Lexicographic Optimization for Prioritized Robot Control and Planning

In this work, we present several tools for efficient sequential hierarchical least-squares programming (S-HLSP) for lexicographical optimization tailored to robot control and planning. As its main step, S-HLSP relies on approximations of the original non-linear hierarchical least-squares programming (NL-HLSP) to a hierarchical least-squares programming (HLSP) by the hierarchical Newton's method or the hierarchical Gauss-Newton algorithm. We present a threshold adaptation strategy for appropriate switches between the two. This ensures optimality of infeasible constraints, promotes numerical stability when solving the HLSP's and enhances optimality of lower priority levels by avoiding regularized local minima. We introduce the solver $\mathcal{N}$ADM$_2$, an alternating direction method of multipliers for HLSP based on nullspace projections of active constraints. The required basis of nullspace of the active constraints is provided by a computationally efficient turnback algorithm for system dynamics discretized by the Euler method. It is based on an upper bound on the bandwidth of linearly independent column subsets within the linearized constraint matrices. Importantly, an expensive initial rank-revealing matrix factorization is unnecessary. We show how the high sparsity of the basis in the fully-actuated case can be preserved in the under-actuated case. $\mathcal{N}$ADM$_2$ consistently shows faster computations times than competing off-the-shelf solvers on NL-HLSP composed of test-functions and whole-body trajectory optimization for fully-actuated and under-actuated robotic systems. We demonstrate how the inherently lower accuracy solutions of the alternating direction method of multipliers can be used to warm-start the non-linear solver for efficient computation of high accuracy solutions to non-linear hierarchical least-squares programs

Sequential Hierarchical Least-Squares Programming for Prioritized Non-Linear Optimal Control

We present a sequential hierarchical least-squares programming solver with trust-region and hierarchical step-filter with application to prioritized discrete non-linear optimal control. It is based on a hierarchical step-filter which resolves each priority level of a non-linear hierarchical least-squares programming via a globally convergent sequential quadratic programming step-filter. Leveraging a condition on the trust-region or the filter initialization, our hierarchical step-filter maintains this global convergence property. The hierarchical least-squares programming sub-problems are solved via a sparse reduced Hessian based interior point method. It leverages an efficient implementation of the turnback algorithm for the computation of nullspace bases for banded matrices. We propose a nullspace trust region adaptation method embedded within the sub-problem solver towards a comprehensive hierarchical step-filter. We demonstrate the computational efficiency of the hierarchical solver on typical test functions like the Rosenbrock and Himmelblau's functions, inverse kinematics problems and prioritized discrete non-linear optimal control

Time-Optimal Control via Heaviside Step-Function Approximation

Least-squares programming is a popular tool in robotics due to its simplicity and availability of open-source solvers. However, certain problems like sparse programming in the $\ell_0$- or $\ell_1$-norm for time-optimal control are not equivalently solvable. In this work, we propose a non-linear hierarchical least-squares programming (NL-HLSP) for time-optimal control of non-linear discrete dynamic systems. We use a continuous approximation of the heaviside step function with an additional term that avoids vanishing gradients. We use a simple discretization method by keeping states and controls piece-wise constant between discretization steps. This way, we obtain a comparatively easily implementable NL-HLSP in contrast to direct transcription approaches of optimal control. We show that the NL-HLSP indeed recovers the discrete time-optimal control in the limit for resting goal points. We confirm the results in simulation for linear and non-linear control scenarios

Eyes on the mind : investigating the influence of gaze dynamics on the perception of others in real-time social interaction

ACKNOWLEDGMENTS This study was partially supported by a grant of the Köln Fortune Program of the Medical Faculty at the University of Cologne to Leonhard Schilbach and by a grant “Other Minds” of the German Ministry of Research and Education to Kai Vogeley. The authors would like to thank Stephanie Alexius and Leonhard Engels for their assistance in data collection.Peer reviewedPublisher PD

Singularity resolution in equality and inequality constrained hierarchical task-space control by adaptive non-linear least-squares

International audienceWe propose a robust method to handle kinematic and algorithmic singularities of any kinematically redundant robot under task-space hierarchical control with ordered equalities and inequalities. Our main idea is to exploit a second order model of the non-linear kinematic function, in the sense of the Newton's method in optimization. The second order information is provided by a hierarchical BFGS algorithm omitting the heavy computation required for the true Hessian. In the absence of singularities, which is robustly detected, we use the Gauss-Newton algorithm that has quadratic convergence. In all cases we keep a least-squares formulation enabling good computation performances. Our approach is demonstrated in simulation with a simple robot and a humanoid robot, and compared to state-of-the-art algorithms

N\mathcal{N}IPM-HLSP: An Efficient Interior-Point Method for Hierarchical Least-Squares Programs

Hierarchical least-squares programs with linear constraints (HLSP) are a type of optimization problem very common in robotics. Each priority level contains an objective in least-squares form which is subject to the linear constraints of the higher priority hierarchy levels. Active-set methods (ASM) are a popular choice for solving them. However, they can perform poorly in terms of computational time if there are large changes of the active set. We therefore propose a computationally efficient primal-dual interior-point method (IPM) for HLSP's which is able to maintain constant numbers of solver iterations in these situations. We base our IPM on the null-space method which requires only a single decomposition per Newton iteration instead of two as it is the case for other IPM solvers. After a priority level has converged we compose a set of active constraints judging upon the dual and project lower priority levels into their null-space. We show that the IPM-HLSP can be expressed in least-squares form which avoids the formation of the quadratic Karush-Kuhn-Tucker (KKT) Hessian. Due to our choice of the null-space basis the IPM-HLSP is as fast as the state-of-the-art ASM-HLSP solver for equality only problems.Comment: 17 pages, 7 figure

Monte-Carlo Tree Search with Prioritized Node Expansion for Multi-Goal Task Planning

Symbolic task planning for robots is computationally challenging due to the combinatorial complexity of the possible action space. This fact is amplified if there are several sub-goals to be achieved due to the increased length of the action sequences. In this work, we propose a multi-goal symbolic task planner for deterministic decision processes based on Monte Carlo Tree Search. We augment the algorithm by prioritized node expansion which prioritizes nodes that already have fulfilled some sub-goals. Due to its linear complexity in the number of sub-goals, our algorithm is able to identify symbolic action sequences of 145 elements to reach the desired goal state with up to 48 sub-goals while the search tree is limited to under 6500 nodes. We use action reduction based on a kinematic reachability criterion to further ease computational complexity. We combine our algorithm with object localization and motion planning and apply it to a real-robot demonstration with two manipulators in an industrial bearing inspection setting

Why we interact : on the functional role of the striatum in the subjective experience of social interaction

Acknowledgments We thank Neil Macrae and Axel Cleeremans for comments on earlier versions of this manuscript. Furthermore, we are grateful to Dorothé Krug and Barbara Elghahwagi for their assistance in data acquisition. This study was supported by a grant of the Köln Fortune Program of the Medical Faculty at the University of Cologne to L.S. and by a grant “Other Minds” of the German Ministry of Research and Education to K.V.Peer reviewedPreprin