A Unified Framework for Optimal Motion Generation

Deployment of highly redundant robots in real application scenarios require the capability of generating sophisticated behaviours. Many complex movements like getting up from a chair, squatting or even acrobatics have been rarely object of studies so far. Approximately, in the literature we can identify two different approaches for motion generation: whole body trajectory optimization [1], [2] and multi-task coordination techniques [3], [4] [5].

Nevertheless, the aforementioned methods, displays several criticalities. In formulating optimization problems for finding whole-body trajectory, the authors usually employ a open loop description (with or without dynamics) with lesser guar- antees on the final behaviour on the real robot. For multi- task optimization is always required to identify each sub- tasks composing the global behaviour and to hand design the relations among them. Moreover the previous methods are often defined as optimal control problem with quadratic cost functions and they always require an analytical formulation in order to be solved which restricts the range of applications.

In this work we introduce an unified framework that tack- les all the aforementioned limitations and can be used for both trajectory optimization and multi-task coordination. The framework distinctive features to deal with any kind of non- linear cost functions and no need of an explicit mathematical modelling for the optimization problem is inherited from the employment of Black-Box Optimization (BBO) algorithms.

in our framework we choose the Covariance Matrix Adaption Evolution Strategy (CMA-ES) as our optimizer. CMA-ES [6] is a stochastic BBO algorithm that requires little to no tuning to work. BBO is a trial and error approach which need to collect information about the fitness function and the process by performing a sequence of experiments (rollouts) to find an optimal solutions.

Finding an optimal solution requires for the experiment to be performed many times, therefore, for safety reasons, is better to employ a simulation engine in which running the experiment. In CMA-ES basic implementation for each itera- tion K candidates are sampled from a multivariate Gaussian distribution N (π ̄, σ2Σ) (see Fig. 7). We compute the fitness corresponding to each candidate and we keep only the “fittest” samples: we define the Ke candidates, π1:Ke , elites. With this candidates subset we update mean, covariance, and step size of the search distribution N (π ̄, σ2Σ). For the update of σ 2 and Σ both information from the last generation and the previous ones are blended together.

In [7], [8] we used Black Box optimization to tackle the problem of the automatic design of the priorities for multi- task coordination problem. In our framework we adopted a soft task prioritization scheme where each elementary task is modulated by a task priority or task weight function αi(t). Following the scheme of Fig. 1, given nt elementary tasks the final controller is given by: 

Our framework has been used for learning task priorities for a Jaco Arm (Fig. 3) reaching task and for a bimanual task while avoiding an obstacle with an iCub humanoid (Fig. 4) which only the upper body was controlled. In [8] we considered explicitly the problem’s constraints to increase safety and we showed that (1+1)CMA-ES with Constrained Covariance Adaptation [9] has the best performance with respect to a benchmark composed by analytical and robotics problems In [10] we adapted our approach for a trajectory optimiza- tion problem as shown in Fig. 2. Here we introduced a closed loop trajectory constrained optimization that has the property to enhance the feasibility of the resulting optimal trajectory on the real robot. In this work we focused on optimizing the desired task trajectory of the robot Center of Mass (CoM). The CoM Cartesian trajectory is modelled as a weighted sum of normalized Radial Basis Functions (RBFs): 

where p ∗,i com is the desired profile in time of one of the coordinate of the final CoM trajectory, ψk(μk, σk, t) = exp −1/2[(t − μk)/σk] 2 , with fixed mean μk and variance σk of the basis functions, nr is the number of RBFs and πi = (πi1, . . . , πinr ) ⊆ R nP is the set of parameters for each trajectory dimension. In this work we applied our framework to find the right trajectory for achieving a stand-up from a chair task with an iCub humanoid robots (see Fig 5 and 6). As a low level controller for the dynamic balancing of the robot we used the momentum controller introduced in [11].