Philippe Fraisse

Sylvain Miossec

Katja

Joven Romain Joris

Vincent Kévin

Stéphane Adrien David

Marion, Thomas Alejandro Guillaume

TLP Thread-Level Parallelism ZMP Zero-Moment Point

Keywords: motion planning, humanoid robotics, parallel computing, GPGPU, nonlinear optimization. Résumé planification de mouvement, robotique humanoïde, calcul parallèle, GPGPU, optimisation non linéaire SQP Sequential Quadratric Programming

Je souhaiterais tout d'abord remercier mon directeur de thèse, Abderrahmane Kheddar, pour son soutien, ses conseils, et sa confiance tout au long de ces années de thèse. Merci de m'avoir accueilli au JRL en stage, de m'avoir proposé de poursuivre en thèse, et de m'avoir donné les moyens de réussir. Le temps que j'ai passé au Japon a été une vraie chance, et une expérience enrichissante. Je le remercie vivement pour cette opportunité.

Je tiens aussi à remercier mon co-encadrant de thèse, Adrien Escande. Son expertise en optimisation m'aura énormément aidé au cours de mes recherches. Ses conseils avisés, ses relectures minutieuses et nos discussions enrichissantes ont clairement fait la différence.

Un énorme merci aux rapporteurs, Florent Lamiraux et Dinesh Manocha, pour leur relecture et leurs retours précieux. Je tiens aussi à remercier

] and a piecewise-constant control (computation done with CasADi). . . . 1.5 Discretization may lead to constraint violations. These pictures come from [El Khouri 13]: when generating dynamic motions (e.g. shelves scenario for the top row, martial arts scenario for the bottom row), the discrete constraint evaluations (points on the curves) led to violations of the ZMP constraint. The solution was then to use a reduced support polygon (green area in the right column) so that the ZMP trajectory remains in the actual support polygon (green area in the left column). . . . . . . . . . . . . . . . . . . . . . . . . .

1.6

Comparison of min-max bounds of time-grid evaluations and polynomial approximations. In both cases, there is no guarantee of finding the true bounds. The Taylor approximations centered at mid-intervals have their highest bounding errors when extrema are reached on interval boundaries, and the error can be reduced by increasing the polynomial order of the approximation, or reducing the size of the intervals. Time-grid bounding errors can be reduced by increasing the discretization's granularity. While the geometry, kinematics and dynamics depend on q(t) (plus f (t) for the dynamics), the kinematics (resp. the dynamics) may reuse data from the geometry (resp. the geometry and the kinematics). .

2.2 B-splines: error between extrema from the convex hull and actual extrema . .

List of Figures xv

2.3 Topology of a system as returned by lstopo on Linux. The CPU, its caches (L1, L2, L3), cores and threads can be seen on the left, and the peripherals connected to it through PCIe buses are on the right (e.g. the GPU labeled card0). 2.4 Transfer time for different types of memory (pageable or pinned) as well as transfer directions (to and from the GPU) for a GeForce GT 650M with PCIe 2. While the transfer time is nearly constant (∼10µs) for small transfer sizes, it increases linearly after 8kB. Then, maximum throughput is reached (bandwidth limit). Overall, pinned (or page-locked) host memory is more efficient than the default pageable memory, i.e. memory that can be swapped out. This is due to the fact that when transferring data to or from the GPU, pageable memory is first copied to pinned memory, and that extra copy can be bypassed. However, allocation and deallocation of pinned memory is more expensive. Note that this only concerns host memory, as device memory is automatically page-locked. 2.5 Overview of the global computation involved in the optimization process. . . . The method revolves around the idea of Articulated-Body Inertia (ABI) [Featherstone 83], the equivalent inertia of a set of "assembled" bodies connected by joints, and is the basis for the DCA method [Featherstone 99a]. The first assembly-disassembly pass computes the kinematics, and the second handles the system kinetics. . . . .

Real-time GPU simulation of a cloth model consisting of 65536 particles

with more than 130 000 distance constraints and a few thousand contact constraints [ neighboring control points that influence the curve on T i . . . . . . . . . . . . 3.12 Resulting structure of the Jacobian matrix for a given scenario: log of the amplitude on the left, actual sparsity structure of the matrix on the right. For clarity, we only consider a portion of an actual Jacobian matrix. The sparsity can be further refined if properties of the constraints are taken into account.

Note that here, the optimization parameters are ordered per joints, that is first the control points of the first joint, then the control points of the second joint, etc. for each solver. The optimal solution is f (x * ) = -√ 3, where x * = (0, √ 3). . 4.9 PyQt GUI used to load and replay motions in RViz. . . . . . . . . . . . . . . 4.10 Memory layout used for data structures. Our templated implementation supports both single-and double-precision floating points, as performance of GPU code may greatly vary depending on the floating-point type used. . . . . . . .

4.11

Signed ZMP distance to the support polygon. The ZMP remains within the support polygon if this signed distance is negative. . . . . . . . . . . . . . . 4.12 Snapshots from whole-body motion planning with the HRP-2 humanoid robot. 4. 13 [Wieber 02]: a contact force f can be split into a normal component f n and a tangential component f t (on the left). To ensure a static contact, these contact forces have to remain in a cone whose angle depends on the friction coefficient µ (in the middle). To simplify the problem, it is common to consider a linear (convex polyhedral) approximation [ Hauser 14] (on the right). . over time in the single-support case. Constraint limit is 1. . . . . . . . . . . . 5.11 Normalized angular distance α(t)/µ of contact forces to the cone of friction over time in the double-support case. Constraint limit is 1. . . . . . . . . . . Yakub 13]. r is the reference trajectory, ũ represents the future inputs, y the output of the system, ỹ the predicted outputs, and ẽ the predicted error. . . . . . . . . . . . 130 6.2 Influence of model predictive control on the control sequence. Each MPC step computes the control u over an horizon T (e.g. T = 3∆t), but only the beginning of it is actually applied to the system, as successive MPC steps (every ∆t) improve the control sequence based on system feedback. . . . . . . . . . 131 6.3 Let us consider P(x i , s(t i ), t ≥ t i ). As we constrain the joint states at t = t i , we may obtain large discontinuities δ q(t i + ∆t) for the new commands sent at t i + ∆t (in red). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 134 6.4 Let us consider P(x i , s(t i ), t ≥ t i + ∆t). Since we assume that joint trajectories are strictly followed, we can limit the discontinuities of the new commands sent at t i + ∆t (in red) by taking the future state as a constraint of the problem. 134 6.5 Architecture of the MPC framework. . . . . . . . . . . . . . . . . . . . . . . 136 6.6 On the first row: initial splines and constraints before optimization. On the second row: splines after first optimization, and additional state constraint. On the third row: reoptimization after a single iteration. On the fourth row: full reoptimization. Satisfied constraints are in green, violated ones are in red. . . 139 6.7 Scenario considered: HRP-2 needs to put a ball (initially in its left hand) into a target box. That box may change during the motion. The robot is standing on non-coplanar surfaces. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 143 6.8 Constraint violation during the replanning process, for computation with ( , ) and without ( , ) a change in the task, for different computation times ∆t. A maximum of 5 NLP iterations per MPC steps was used. Each point describes the global constraint violation after an MPC step. Parts with constant violations are due to the fact that the constraint violation tolerance is reached, and no change in the environment was observed. . . . . . . . . . . . . . . . . 145 6.9 Constraint violation during the replanning process, for computation with ( , ) and without ( , ) a change in the task, for different acceptable constraint violations. Here, ∆t = 250 ms, for 5 NLP iterations per MPC step. 146 6.10 Constraint violation during the replanning process, for different perturbation feedback times (i.e. times at which the task was updated). Here, ∆t = 100 ms, for 5 NLP iterations per MPC step. . . . . . . . . . . . . . . . . . . . . . . . 147 6.11 Influence on joint state constraints on discontinuities in the control sequence. The task is updated at t = 2.5 s. Only considering joint position constraints (in green) leads to nonsmooth controls during the replanning phase (C 0 trajectories). Taking joint velocity constraints into account helps reduce such discontinuities (C 1 trajectories, in red). . . . . . . . . . . . . . . . . . . . . . 6.12 Snapshots from the initial trajectory (top row) and the replanned trajectory List of Tables 1. 1 Comparison of different optimization-based motion planning methods. Note that computation times depend on the problems solved in these papers as well as the computing hardware used at that time. This still gives a rough idea of whether these methods can be used for control or simply offline motion generation. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

Block diagram of a model predictive controller as presented in [

2.1

List of latencies for the Intel i7-6700 (Skylake), 4.0 GHz (with Turbo Boost), 14 nm, 16 GB RAM (dual DDR4-2400 CL15). Source: 7-cpu.com. . . . . . . 4.8 Timings (total and average per iteration) for the HRP-2 scenarios with both single-and double-precision floats, for each GPU, and without any cost function. This includes geometry, kinematics, dynamics (albeit not required), constraint evaluations and their gradient counterpart, and time spent in the solver. Note that an Ipopt iteration may not always involve a constraint Jacobian query, so for two different scenarios, even if the number of iterations is similar, the actual number of Jacobian evaluations may be different. . . . . . 105 4.9 Comparison between the CPU and GPU implementations for several key features.107 4.10 Average time per iteration in ms for the parallel CPU version (for multiple thread counts) and our GPU version (for single-and double-precision floats), with associated speedup relative to the single-threaded CPU version. The scenario considered has n ≈ 300, with geometry and kinematics constraints (m ≈ 2000), and 15 time intervals. Computation on the CPU is done with double-precision floating numbers. This table gives an idea of the speedup to expect for the core computation on different systems, and we expect an even better speedup once highly parallelizable collision constraints are added. . . . 111

Theoretical table defining which

6.1 Comparison of the number of Ipopt iterations until an acceptable solution is found for different scaling methods, with and without a jerk cost function.

indicates that Ipopt did not converge before 300 iterations, and * that it converged to a point of local infeasibility. . . . . . . . . . . . . . . . . . . . 142 D.1 Evolution of Tesla GPUs. While the hypothetical peak performance has kept on increasing over the years, some of the underlying architecture properties have changed inconsistently. For example, Fermi GPUs featured few SMs with many cores, while the more recent architectures such as Pascal provide more SMs with less core per SM. . . . . . . . . . . . . . . . . . . . . . . . . . . . 184

Nomenclature Superscripts Ẋ time derivative of X.

i X X (vector or tensor) is expressed in the i-th link's coordinate system X i Y i Z i .

X skew-symmetric matrix associated with X (cf. Sec. A.1).

X T transposed vector or tensor.

Subscripts

X i X (vector or tensor) is related to the i-th link.

Other Symbols

S i static rotation of the i-th link.

R i λ (i) rotation of the i-th link w.r.t. the λ (i)-th link.

Q free free space.

Θ i rotation from the i-th link to the world frame R g .

Q obs a configuration space obstacle.

Q configuration space.
f Vector of contact forces expressed in R g .

f i i-th contact force (related to the s(i)-th contact surface) expressed in R g .
g acceleration of the gravity.

Γ i joint force exerted on the i-th link by the i -1-th link.

i ωi angular acceleration of the i-th link expressed in R i .

s(i) f i i-th contact force (related to the s(i)-th contact surface) expressed in R s(i) .

i J i inertia matrix of the i-th link, expressed in R i .

i k i vector from the i-th link's center of mass to its parent joint expressed in R i (cf. Fig. 3.7).

i l i vector from the i-th link's center of mass to one of its children joint expressed in R i (cf. Fig. 3.7).

IN i resultant torque applied to the i-th link.

i T c i momentum due to contact forces, expressed in R i .

i T i joint torque applied to the i-th joint, expressed in R i .

i ω i angular velocity of the i-th link expressed in R i .

J

Jacobian matrix of the contact points. m i mass of the i-th link.

M inertia matrix of the robot.

M i mass matrix of the i-th link. q joint trajectories.

R g world frame (Galilean reference frame).

R i i-th link's frame (local frame). τ joint torques.
T c i momentum due to contact forces, expressed in R g .

i T ext i momentum due to external forces, expressed in R i .

T ext i momentum due to external forces, expressed in R g .

T i joint torque applied to the i-th joint, expressed in R g .

W i

Weight matrix of the i-th contact force for the Lagrange multiplier method.

x i i-th link's center of mass, expressed in R g .

Acronyms / Abbreviations

Introduction

In 1920, Karel Čapek introduced the word robot in his science-fiction play, "R.U.R.". In Czech, robota means forced labor or hard work. Nearly a century later, the play became a reality to some extent: a large variety of robotic systems have been created, from highly specialized machines designed for a single repetitive task, to highly versatile robots which can adapt to more complex scenarios. Alike computers, robotic systems are entering our daily environments: factories, hospitals, malls, schools, or even homes1 . Modern robotic systems are annexing human proxemics, i.e. our personal space where safety is paramount. Evolving in such unstructured environments and toward human interaction requires integrated sensing of the surroundings, as well as relatively fast and safe decisional responses. By being able to operate close to (or with) humans, many applications are envisioned, such as collaborative work with humans [ Bussy 12], daily home services 2 , or home assistance for frail persons 3 .

Our environments are shaped according to our locomotion and motion capabilities, or according to the tools that we can conceive and use: doors that we can open, stairs that we can climb, cars that we can drive, etc. Therefore, anthropomorphic robots that mirror us may evolve unimpeded and can be more easily embodied: this is what makes humanoid robots an interesting question. In March 2011, the Fukushima disaster (and others before) revealed how important robots can be. Indeed, hundreds of workers were deployed on-site despite the presence of ionizing radiations, since some of the disaster-response tasks could only be performed by humans. It is still unclear how many of these workers will have severe health problems in the coming years as a result of this exposure. Following that incident, from 2012 to 2015, international teams of scientists competed and tested their skills in the DARPA Robotics Challenge (DRC). The objective was to spur development of semi-autonomous robots able to accomplish complex tasks in unsafe human-built environments, so that one day, we no longer need to put human lives in jeopardy when physical intervention in dangerous situations is required. The competition featured 8 tasks such as ingress, driving and egress from a car, opening a door, manipulating tools, opening a valve, walking over rubbles and climbing stairs (see Fig. 1). The DRC finals highlighted how difficult it is for state-of-the-art robots to accomplish tasks that a human operator could fulfill in a matter of seconds or a few minutes. Gathering information from sensors, processing that information to get a comprehensive representation of the situation (see Fig. 2), and deciding how to react based on this input can take several minutes for robotic systems. If an error happens, the robot is often unable to recognize it. If it does, then it is frequently unable to recover from it properly. Teleoperation is not always possible either, since communication is not easy or even possible if the robot is far away from the ground station, or needs to enter a building. The industry is also interested in robotics, and no longer simply for automated chain assemblies featuring fixed-base robotic arms. In 2015, Airbus Group and a consortium of European research institutes under the coordination of the CNRS launched the COMANOID project4 . One of the objectives is "to investigate the deployment of humanoid robots solutions in Airbus assembly operations that are laborious and inaccessible for wheeled platforms". Here, humanoid robots should accomplish tasks that are less-valued for human employees, and that wheeled robots are unable to perform due to the obstructed environment. Once again, safety is essential as robots will share the workspace with human workers.

Contribution and plan

The work presented in this thesis involves three distinct fields:

• robot motion planning: to generate desired motions to achieve the task objectives, • numerical optimization: solving optimization problems where we want to minimize a cost function subject to a set of constraints, • parallel computing: to reduce the overall computation time.

In the first chapter (Chap. 1), we will introduce a brief state of the art in motion planning, and present concisely parallel computing architectures used for high-performance computing, such as Graphics Processing Units (GPUs).

In Chap. 2, we present a parallelization analysis of the previous work [Lengagne 13]. This study shows that while the previous implementation tackled the parallelization at the highest level, the lower layers of the computation are also good candidates for parallelization. However, due to the reliance on recursive computation and automatic differentiation for the whole computation, the library developed previously is not suitable as a basis for our work.

The second contribution involves the parallel computation of the dynamics (Chap. 3). Our optimization-based approach to motion planning implies that we need to evaluate constraints and their gradients. Thus, the gradients of the core robotics computation (geometry, kinematics, dynamics) need to be computed in parallel. We developed our method with our target processing architecture in mind, namely GPUs.

The following chapter introduces the actual GPU implementation of our optimization problem (Chap. 4). This work relies on RobOptim, our C++ optimization framework used to write the optimization problem (cost function, constraints), and CUDA, NVIDIA's generalpurpose GPU computing API. Our results show important speedups compared to the previous CPU-based parallel planner.

A proper handling of contact forces is essential when considering multi-contact motion planning, leading to our next contribution (Chap. 5): we propose a new parametrization of contact forces suited for optimization-based motion planning with continuous constraint satisfaction.

Finally, we investigate the extension of our method to model predictive control, and present our findings (Chap. 6). This last part is not implemented yet on a real humanoid platform as more computation optimizations are still needed.

The following chapter concerns the brief state-of-the-art part of this thesis.

Chapter 1

State of the art 1.1 Motion planning

Autonomous motion generation tools for humanoid robotics represent a truly challenging area [Kuffner 03]. When considering the full dynamics problem with all the constraints involved (joint limits, collision avoidance, balance, contacts, mission-related tasks etc.), the computation becomes extremely complex, thus traditionally unfit for real-time applications.

Yet, the results obtained in [ Lengagne 13] show the kind of high-quality motions that can be generated with these techniques. These methods can be used to solve a wide range of practical problems, for instance one may be looking for the maximum mass that a humanoid robot can carry across a warehouse, from a trolley to a high-up shelf. In that example, the problem to solve is tightly coupled with the motion to generate, and it may be difficult or impossible to solve with methods relying on heavy approximations. Another practical example is to find the dynamic limits of a robot: [Koch 14] shows extremely promising results to that end. However, the problem with these approaches is that computation times can easily reach 24 hours for a multi-contact motion lasting over 10 seconds, which makes them unusable in fast-changing environments (e.g. with unknown dynamic obstacles to avoid).

Still, as motion planning methods have seen their computation times reduced, and as control algorithms have seen their supported features improved (e.g. non-coplanar contacts), the historical gap between planning and control is slowly shrinking. The robotics community roots for a unified method that would merge the high-level abstraction of motion planning with the robustness and speed of control, thus supporting whole-body multi-contact dynamic motions in a reliable way.

Path planning

[ Latombe 91] presents a detailed analysis of the motion planning problem. First, the configuration q of a rigid body A is defined as the specification of the position and orientation of a frame attached to it w.r.t. a fixed Cartesian coordinate system. We note Q the configuration space, that is the vector space defined as:

Q = {q | q is a valid configuration of the body} (1.1)
An articulated system is made of several rigid objects A 1 , . . . , A n called links or bodies, connected by joints. These joints constrain the movements of its connected bodies. For that kind of system, the configuration space is a subset of the composite configuration space of the links A i , constrained by the joints. Then, Latombe defines a path from an initial configuration q init to a final configuration q final as the continuous map τ : [0, 1] → Q, with τ(0) = q init and τ(1) = q final , which is the standard definition of a path in a topological space. Thus, the motion planning problem can be expressed as a path planning problem, i.e. find a path τ across the configuration space Q such that mission constraints are satisfied, e.g. joint limits, collision avoidance, positions of contacts etc. The difficulty of the motion planning problem lies in these constraints. Collision avoidance is often treated specifically, and we define configuration space obstacles Q obsi as the sets of configurations q such that the robot intersects with obstacles in the environment. Thus, we can define the free space Q free as:

Q free = Q \ i Q obsi (1.2)
Q free is the set of configurations q such that the robot is not in collision with the workspace. Thus, finding a collision-free path is equivalent to finding a path in Q free .

However, a path planner only solves the problem of finding collision-free paths in the environment: it does not directly account for kinodynamic constraints, that is constraints on velocities, accelerations, forces and torques.

Properties

A wide range of approaches have been proposed to generate robotic motions, such as potential fields, sampling-based methods, optimization-based methods etc. All of these methods have strengths and limitations, and which one to use largely depends on the kind of application and constraints involved (complexity of the dynamic model, computation time, safety etc.).

We can distinguish planning methods based on their properties [

Potential fields

When planning a geometric path through an environment, the objective is to go from a starting point to a given destination without colliding with the environment. A possible solution to this problem involves an artificial potential field composed of an attractive potential that pulls the robot towards the target, and a repulsive potential pushing the robot away from the obstacles. Thus, the first task is to generate the potential field. Then, the robot follows a steepest descent through the potential field until it reaches the global minimum (i.e. its target destination). This made real-time collision avoidance possible for simple robots (manipulators or mobile robots) during the 1980s [Khatib 86], when computational power was much lower than nowadays. Another notable example is contact planning for humanoid robots (see Fig. 1.1).

The main problem with potential functions is that they may have a lot of local minima, i.e. solutions that are locally optimal, but not necessarily globally optimal. A possible solution could be to construct a convex navigation function (i.e. with a unique minimum), but this is only feasibly when obstacles have simple shapes and/or the configuration space is small.

The idea was then to rely on a randomized path planner (RPP) [Latombe 91] that executes a series of gradient descents followed by random motions once it reaches a minimum. Randomized planners are not deterministic, i.e. different executions may lead to different solutions and computation times may vary. Furthermore, the planner is unable to detect if the problem is impossible, so a limit on the execution time has to be enforced. Yet, the method is probabilistically resolution-complete, that is if there exists a valid path, the probability of finding such a path converges toward 1 when the execution time grows toward infinity.

Attractive potential

Repulsive potential Solution q nal q init Figure 1.1 Potential fields can also be used for contact planning. This image and details on the subject can be found in [ Escande 08].

Such randomized planners can also be used without potential functions, by solely depending on a large set of samples in the configuration space.

Sampling-based methods

Sampling-based methods rely on the following idea: sample the configuration space for valid configurations, create a roadmap through these samples, and find the shortest path from an initial configuration to a target configuration. These methods are extremely popular: often simpler to implement, they scale particularly well, since increasing the number of samples enhances the quality of the solution or reduces the time required to find a feasible one. As a result, these methods are the ones featured in the motion planning library of ROS 1 , which interfaces with OMPL 2 , an open-source motion planning library implementing randomized motion planners. We can distinguish geometric planners that only return paths that account for geometric and kinematic constraints, from control-based planners. While the paths of the former have to be made dynamically feasible in a post-processing step, the latter relies on state propagation to obtain kinodynamic trajectories, that is paths that satisfy velocity, acceleration, force and torque constraints.

We will now present some of the most common sampling-based algorithms.

Probabilistic Roadmap (PRM) The PRM methods [Kavraki 96; Geraerts 04] operate as follows:

1. Learning phase: build a graph, called roadmap, by repeating the following steps:

• Construction step: pick a random sample in the configuration space Q until you find a collision-free sample.

• Expansion step: use a local planner to connect the sample to its nearest neighbors in the roadmap. Only collision-free links between nodes are kept.

2. Query phase: given an initial and final configuration, add them to the roadmap thanks to the local planner, and find the shortest path through the roadmap (e.g. with Dijkstra algorithm). These methods are simple to implement and good candidates for parallelization. However, they face the following challenges:

• Sampling the configuration space efficiently is not straightforward, especially in the presence of narrow passages [ Saha 05]. • Collision checking is expensive and takes most of the computation time. A fast collisionchecking library is essential to make the best of PRM methods [Pan 12a]. • Connecting samples may be difficult for nonholonomic systems. An overview of some of the common issues is available in Fig. 1.2.

Rapidly exploring Random Trees (RRT) These methods were introduced in [LaValle 98] to handle nonholonomic constraints (including dynamics). The idea is to build the search tree incrementally in order to cover uniformly the whole configuration space. The starting configuration serves as the root of the tree, which is then expanded with new random samples. A second tree could also be expanded from the final configuration simultaneously, and both would be connected as soon as they are visible from each other (see Fig. 1.3). RRTs are special cases of rapidly exploring dense trees (RDT), i.e. methods that aim at obtaining a dense tree-covering of the configuration space.

Similarly to PRM methods, RRT planners can struggle with narrow passages, and improved tree-building strategies have been devised to improve their efficiency for such cases [ Zhang 08]. Start and end configurations are in blue, collision-free samples of the roadmap are in red, the initial tree is in black, and the collision-free paths (if any) are in green. A bidirectional scheme was used (trees grow from both the start and end configurations, and connect when possible).

Randomized planners such as PRM or RRT usually require an extra smoothing step, since generated motions are often jerky [Pan 12b]. Furthermore, though the generated trajectories only take the geometry into account, it is possible to integrate the velocities to obtain kinodynamic trajectories [ Pham 13].

Optimization-based methods

These methods model the motion planning problem as an optimization problem: a cost function that needs to be minimized, a possible set of constraints that need to be satisfied, and the unknowns are the trajectories or their parametrization. The cost function can be the energy consumption, the time required to execute the motion, the smoothness of the motion, or any other criterion relevant to the mission (including combinations of such criteria).

The problem and an initial guess are then sent to an optimization solver which will iteratively try to solve the problem. Once it converges, the solution is returned to the user. The properties of the problem (linearity, convexity, gradient continuity, equality or inequality constraints) will determine the kind of solvers that can be used, and the theoretical properties of the solution that can be obtained.

Simplified models Motion planning involves nonlinear dynamic models and equations which are expensive to handle. For real-time/interactive applications, for instance when we want to be able to cope with external disturbances, dealing with a simplified model allows to remarkably decrease computation times, down to the order of milliseconds.

This idea has been used with great success for walking pattern generators: by using a 3D linear inverted pendulum, an algorithm for real-time walking pattern generation was developed [Kajita 02]. This approximation imposes some strong restrictions:

• z = 0, thus a completely flat or constant-slope ground (although extended work added support for moderate unknown slopes and roughness [ Nishiwaki 12]), • it assumes a zero variation of angular momentum about the CoM (Center of Mass), which is a strong limit when trying to achieve complex dynamic motions.

Another notable example is contact-invariant optimization [Mordatch 12]: by using simplified physics and contact models, computation is greatly simplified, and impressive results were obtained in simulation. For this method, constraints are treated as penalties to the cost function, and are thus not guaranteed to be satisfied. Moreover, porting the work to an actual humanoid robot proves to be challenging: it was tested on a HRP-2 humanoid robot with constant feet contacts [Koenemann 15], yet the approach still needs to be validated on walking robots.

Convex optimization These methods rely on problems involving simplified convex models that can be reliably and quickly solved. A convex optimization problem is defined by: min

x f(x)
Subject to:

g i (x) ≤ 0 i ∈ 1, . . . , m h j (x) = 0 j ∈ 1, . . . , p
with f and g i convex functions, and h j affine functions. These problems admit a unique (global) minimum, and solvers are guaranteed to converge. The main drawback lies in the convex approximation: only parts of the global problem may be suitable for this kind of simplifications, and overapproximations may excessively limit the range of available motions.

This approach was successfully used for motion planning with collision checking [Schulman 14], and for generating smooth interpolating time-optimized trajectories on contact submanifolds given initial geometric trajectories [ Hauser 14]. Computation times are often much lower than their non-convex counterparts, which makes this method a good candidate for interactive applications.

Optimal control Optimal control aims at finding state and control trajectories of dynamic systems that minimize a cost function over a finite time horizon [0, T ]. If we note u the control variables, and x the state variables, the optimal control problem (OCP) can be written:

min u(t),x(t) J = T 0 l x(t), u(t),t dt (1.3) Subject to: ẋ(t) = g x(t), u(t) , t ∈ [0, T ] (1.4) x(t 0 ) = x 0 (1.5) 0 ≤ h x(t), u(t) , t ∈ [0, T ] (1.6)
with l the running cost, x 0 the initial state, g represents the dynamics of the system, and h the constraints of the problem.

The control u is discretized on a time grid over [0, T ] based on a chosen basis function (e.g. linear function). Thus, we obtain for instance a piecewise linear control parametrized by a finite set of optimization parameters. Then, a resolution method such as direct multiple shooting is used (see Fig. 1.4). The system trajectories x are further parametrized as a series of initial value problems. Additional constraints enforce the continuity between shooting intervals. We obtain a large structured nonlinear problem that can be solved efficiently with a sequential quadratic programming (SQP) solver.

The application of this method to humanoid robotics is being actively researched, and the approach was successfully validated on actual robots with complex multi-contact motions [Koch 14; Kudruss 15]. In [Kudruss 15], joint trajectories q are not generated by the method. Instead, it returns CoM and end-effector trajectories that are sent to the Stack of Tasks (SoT) that handles the generalized inverse kinematics. A distinction can be made in the nonlinear case: Nonlinear Model Predictive Control (NMPC) uses nonlinear dynamics systems, hence drastically increasing the complexity of the resolution process compared to the linear case. By avoiding convex approximations of the model, a wider range of solutions can be found, at the cost of computation time. Specific measures can be taken to try and limit the increased computational burden, especially when dealing with similar consecutive optimization problems [Bock 07].

The inverse problem, known as Inverse Optimal Control, aims at finding the optimization criterion that can generate a given solution for a given dynamics system. Thus, by analyzing the locomotion of humans, this method has been used successfully to generate natural locomotion paths for humanoid robots [Mombaur 10].

As the final method that we will introduce is the one we will be investigating in depth, it requires its own subsection. A summary comparison of the different optimization-based motion planning methods is also available in Table 1.1.

Optimization-based whole-body motion planning

The general motion generation problem can be formulated as the following optimization problem: min

q(t),τ(t), f (t) C (q(t), τ(t), f (t))
(1.7) Subject to: c eq (q, q, q, τ, f ) = 0 (1.8) c ineq (q, q, q, τ, f ) ≤ 0 (1.9)

c teq q(t d ), q(t d ), q(t d ), τ(t d ), f (t d ) = 0 (1.10)
where C is a cost function, e.g. global energy consumption [ Miossec 06], motion duration [ Lengagne 07], torque change [Uno 89], jerk [Flash 85] etc. c eq describes equality constraints, c ineq describes inequality constraints, and c teq describes discrete equality constraints (only active at specific times). In short, we are trying to find joint trajectories, joint torques and contact forces that will minimize our cost function while satisfying a set of equality or inequality constraints.

This method is quite different from the previous ones, since it is not suited for large geometric planning problems. However, paired with a high-level planner (e.g. sample-based) or posture generator [ Lengagne 13], these methods can provide efficient motions in large-scale scenarios.

Semi-infinite programming As explained in [ Lengagne 13], since the solution to eq. (1.7) is a set of continuous time functions that fulfill a set of continuous constraints throughout the whole motion (that can be seen as an infinite set of discrete values), the problem can be treated as an infinite programming (IP) problem.

To make this problem solvable, we need to reduce the complexity of the problem by choosing a class of continuous functions that can be parameterized by discrete values. Thus, we transform the IP problem into a semi-infinite programming (SIP) problem by choosing B-splines (see Sec. A.3) to parameterize both joint trajectories and force trajectories.

Though the constraints considered are continuous with respect to time, it is common to use further approximations by discretizing the problem: constraints are only evaluated on a predefined time-grid whose granularity influences both the quality of the approximation and the computation times involved. The main issue is that there are no guarantee to satisfy the constraints over the motion, since constraints could be violated between two time instants (see Fig. 1 [El Khouri 13]: when generating dynamic motions (e.g. shelves scenario for the top row, martial arts scenario for the bottom row), the discrete constraint evaluations (points on the curves) led to violations of the ZMP constraint. The solution was then to use a reduced support polygon (green area in the right column) so that the ZMP trajectory remains in the actual support polygon (green area in the left column).

In [Lengagne 10a], the choice was made to use polynomial approximations rather than time-grid approximations after torque constraints were violated when executing motions on the robot. This led to more expensive computations, but an increased safety regarding constraint satisfaction (see Fig. 1 In both cases, there is no guarantee of finding the true bounds. The Taylor approximations centered at mid-intervals have their highest bounding errors when extrema are reached on interval boundaries, and the error can be reduced by increasing the polynomial order of the approximation, or reducing the size of the intervals. Time-grid bounding errors can be reduced by increasing the discretization's granularity. Here the time grid is set to half of the spline's intervals.

Properties

The general approach behind this method makes it easy to had extra constraints. As it deals with time trajectories of joint positions as an input, obtaining time derivatives is straightforward (e.g. polynomial derivation), while other methods cannot rely on an exact direct computation. Another goal of this work is to use it for human-centric studies. Some early work showed how it can be used to study leg impairments and disabilities while using a humanoid robot as a mannequin [ Lengagne 11]: this was achieved thanks to constraints on the knee joint, and on the feet contact forces.

Limits

Nonlinear solvers are double-edged swords: since they accept any nonlinear constraint, it is easy to add constraints that will lead to a poor convergence rate. For example, great care needs to be taken to ensure that the gradients are not null when reaching the optimum, else the nonlinear solver may have difficulties converging. Moreover, these methods only return a local minimum, and checking whether that minimum is the true optimum is at best laborious, and usually impossible [Bobrow 88].

As for the motion planning framework itself, while it generated smooth and efficient motions, these were executed without any real feedback loop that could account for perturbations, such as the ankle flexibilities of the HRP-2 humanoid robot, or changes in the tasks considered.

Also, the polynomial computation limits the scope of possible computations: dividing polynomials or computing their square root is more expensive and not as straightforward as the scalar equivalents.

Evaluating continuous constraints for the full robot model proved to be a challenging tasks from a computational point of view. This method is known to be particularly slow and unusable in any real-time applications, and may only be used to generate motions offline that can be replayed in an open-loop manner. As a result, we considered taking advantage of recent parallel processing architectures to improve computation times.

Parallel processor architectures

In this section, we will explain why parallel architectures became predominant in today's computers, and introduce a few concepts essential for their understanding. Then, we will present key architectures and the role they can play in high-performance computing.

Concepts

Moore's Law In 1965, Gordon E. Moore, soon-to-be co-founder of Intel Corporation, predicted that the number of transistors per chip would double every year [ Moore 65]: "The complexity for minimum component costs has increased at a rate of roughly a factor of two per year. Certainly over the short term this rate can be expected to continue, if not to increase. Over the longer term, the rate of increase is a bit more uncertain, although there is no reason to believe it will not remain nearly constant for at least 10 years".

His prediction was revised in 1975 for a doubling every two years, and this "law" remained accurate for several decades (see Fig. 1.7).

Other metrics need to be closely monitored, such as the cost per transistor (see Fig. 1.8), the performance per watt, and the design cost. These metrics may better reflect the trends and challenges driving the semiconductor industry.

State of the art

Method

Continuous constraints

Computation time

Freeflyer Contacts q τ T Validated on robots

[Bobrow 88] ✗ ∼min ✗ ✗ Generated ✓ ✓ ✓ [Miossec 06] ✗ ∼min ✓ ✗ Generated ✓ ✗ ✓ [Lengagne 13] ✓ ∼min/hours ✓ Given/Optimized Generated ✓ ✓ ✓ [Hauser 14] ✗ ∼ms/s ✓ Generated Given ✗ ✓ ✓ [Park 14] ✗ ∼s ✓ Generated Generated ✓ ✗ ✗ [Kudruss 15] ✗ ∼min/hours ✓ Given/Optimized ✗ ✓ ✗ ✓ [Koenemann 15] ✗ ∼ms ✓ Generated Generated ✓ ✓
Fixed feet only Table 1.1 Comparison of different optimization-based motion planning methods. Note that computation times depend on the problems solved in these papers as well as the computing hardware used at that time. This still gives a rough idea of whether these methods can be used for control or simply offline motion generation. Still, this exponential growth cannot continue indefinitely, since transistors are slowly reaching their limits: miniaturization to the atomic level [Fuechsle 12], clock rates, instruction-level parallelism, speed of data communication limited by the speed of light, cooling requirements etc. As a result, the current single-core architectures reached their limits, and the companies are slowly moving towards a new "More than Moore" strategy [ Waldrop 16]. The semiconductor industry is looking for alternative materials that could lead to semiconductors running at higher frequencies and lower voltage, or even different approaches (e.g. "3D" processors, IBM's "electronic blood"), but physical limits are still there. Meanwhile, a possible solution is parallel computing.

Amdahl's Law When dealing with parallel algorithms and architectures, the objective is to make a given computation faster by efficiently splitting up the work between different computing entities. An important metric is then the speedup, that is a throughput or latency improvement w.r.t. the original sequential algorithm. If we note S the speedup, n the number of threads used, and σ the proportion of the program that is strictly serial, Amdahl's law [Amdahl 67] states that:

S(n) = 1 σ + 1 n (1 -σ ) (1.11)
Thus, theoretically, making the computation faster involves increasing n and/or decreasing σ . In practice, n is either limited by the parallel architecture (e.g. number of cores on the CPU) or the dimension of the problem (for instance, process these 1000 data points resulting from an experiment). As for decreasing σ , this often requires rethinking the algorithms and data structures used, since the traditional state-of-the-art serial algorithms are unsuitable (σ → 1), which is often due to data dependencies in the computation pipeline. Similarly, if a system contains multiple components, Amdahl's law also shows that optimization to a given component is only as good as its participation to this system, i.e. if that component accounts for 10% of the global computation, then at best you can reduce the global computation done by 10%.

In practice, more processors may not just lead to faster computation, since this may not be the only metric considered. For the methods presented in Subsec. 1.1.4, the quality of the solution often scales with the number of processors used, since more samples can be processed simultaneously [ Park 13]. In this situation, Gustafson's law is preferred [Gustafson 88], since it accounts for this kind of scenario.

Computer architectures can be classified according to Flynn's taxonomy [Flynn 72]:

Single Instruction Single Data (SISD) a single stream of instructions is applied to a single data stream (e.g. single-core CPU).

Single Instruction Multiple Data (SIMD) a single stream of instructions is applied to multiple data streams (e.g. array processor, GPUs, CPUs with SIMD vector registers).

Multiple Instructions Single Data (MISD) multiple streams of instructions are applied to a single data stream.

Multiple Instructions Multiple Data (MIMD) multiple streams of instructions are applied to multiples data streams (e.g. many-core CPUs). While these parallel architectures may have less theoretical limits than the classical singlecore CPU chips, they are bound by practical limitations. For instance, MIMD architectures are limited by the number of independent instructions in the code: two instructions cannot be executed in parallel if the second instruction depends on results from the first instruction.

As for SIMD operations on CPUs, recent hardware supports the AVX instruction set (which stands for "Advanced Vector Extensions"). With AVX-512, 32 512-bit vector registers are available. Intel could keep on increasing the size of these registers, but over time less programs will be able to benefit from this theoretical increased throughput.

Similarly, programs can benefit from multi-core CPUs if computationally expensive tasks can be divided into several independent threads, or if multiple different programs need to run simultaneously.

We will now see some of the parallel architectures commonly used.

Multi-core processors

A multi-core processors is a processor that contains multiple independent processing units (cores). It makes multithreading (running multiple threads concurrently) and multiprocessing (running multiple processes concurrently) possible.

The first multi-core processors were developed in the 1980s, but they only became widespread when the performance growth of single-core processors started to slow down. Nowadays, even smartphones or mobile consoles contain multi-core processors, increasing the scope of applications that can be supported.

Multi-core processor architectures may greatly vary: number of cores, inter-core communication methods (e.g. shared memory, message passing) etc. These properties depend on the targeted applications. For instance, Intel also set its eyes on the High-Performance Computing (HPC) market, with its Xeon Phi "many-core" coprocessors (see Fig. 1.9): the second generation of their product is built using up to 72 cores with 4 threads per core. This kind of architecture is designed for highly parallel scientific computation, thus unsuited for traditional desktop applications.

Graphics Processing Unit (GPU)

A common task in any desktop application is to display images, for instance when playing a video or inside interactive games. The typical operations involve processing large data sets through a graphics pipeline, thus in a SIMD manner. As a result, dedicated specialized graphics chips were manufactured to efficiently tackle that task, and these are usually called Graphics Processing Units (GPUs).

Flynn's taxonomy defined in Subsec. 1.2.1 can be extended with NVIDIA's Single Instruction Multiple Threads (SIMT) GPUs: multiple processors have each multiple threads that act like SIMD processors. It provides a trade-off between SIMD's efficiency and simultaneous multithreading's flexibility. Though GPUs were initially designed for graphics and video rendering, after the advent of programmable shaders (programs that compute rendering effects on the GPU), general-purpose computing on GPUs (GPGPU) became possible. The biggest obstacle to GPGPU remains the complexity of the design and implementation of efficient solutions, and a lot of effort is being put in the main GPGPU frameworks, namely OpenCL and CUDA, to make GPGPU more accessible. Companies like NVIDIA are also eyeing the embedded system market: with an increasing need for data processing in autonomous systems (e.g. self-driving car), having powerful energy-efficient highly parallel processors can help increase responsiveness and reliability. In 2014, the Jetson TK1 development board was released: it features a 2.32GHz ARM quad-core Cortex-A15 CPU with a Tegra K1 GPU, and according to the official description, it targets applications in "computer vision, robotics, medicine, security and automotive". A notable example is given by Google with Project Tango, a tablet that embeds a Tegra K1 GPU, and "combines 3D motion tracking with depth sensing to give your mobile device the ability to know where it is and how it moves through space". Even though computer-intensive tasks such as real-time large-scale dense visual SLAM [ Meilland 13] may not be suitable for that embedded GPU (or only after enormous code optimization efforts), it still gives an idea of what is coming in the next few years.

Today's smartphones could have been built 10 years ago if modern batteries existed back then: mobile CPUs and small mechanical hard drives were available, but energy consumption was a strong limiting factor. Thus, reducing energy consumption thanks to low-power computing is a key to success for the embedded system community. The Jetson TX1, the TK1's successor, draws 10 W for a computing power similar to that of a 300 W desktop from a few years ago. In order to exploit its potential, the software needs to be able to use its GPU at its fullest.

Field-Programmable Gate Array (FPGA)

A FPGA is a configurable integrated circuit. It contains an array of programmable logic blocks (logic gates, combinational logic) connected via programmable interconnects. The idea is that such circuits can be reprogrammed a posteriori to adapt to new scenarios or fix bugs, and could even be used as a developing platform before manufacturing a traditional application-specific integrated circuit (ASIC). Thus, FPGAs provide a lot of flexibility w.r.t. what they can achieve. This makes them suitable for a wide range of applications from digital signal processing to computer vision. They can be reprogrammed as parallel hardware (e.g. SIMD), based on the problem that needs to be solved. However, FPGAs have been known to be slower and less energy-efficient, though the gap with ASICs is getting smaller [ Kuon 07]. These parallel architectures are not necessarily mutually exclusive. In 2014, Intel unveiled a new Xeon CPU containing a integrated FPGA. This way, the chip can be customized to specific workloads, which could increase performance in high-performance computing scenarios, and the same hardware could be easily repurposed for different applications.

High-performance computing

The main challenge for today's software developers is to exploit the computational power of the available processors to their fullest potential. This often implies redesigning existing algorithms and data structures, which presupposes a good understanding of the underlying architecture. Problems may not all benefit from current parallel processors, and this observation is not limited to these architectures: even hypothetical future computing systems, such as quantum computers, may only be useful for highly specific problems (cryptography, optimization), and their applications to common software is still unclear. Yet, a large number of scientific fields involve parallelizable algorithms, and for such problems, parallel processing architectures have brought noticeable speedups since their introduction. Let us take the example of supercomputers, that is systems with considerable computational capacities for general-purpose computing. Supercomputers are used for complex scientific computation such as weather forecasting, molecular modeling, physical simulations etc. These systems are extremely expensive to design, create and operate, thus the trends observed for such systems give a good idea of the technologies that increase computation throughput at a lower cost. Fig. 1.12 shows the percentage of supercomputers that rely on accelerators (e.g. Intel Xeon Phi, GPUs), or more specifically GPUs. On that ranking, supercomputers are sorted by theoretical throughput, e.g. how many floating point operations can be done per second. Since their introduction a few years ago, accelerators (including GPUs) are increasingly used for highly parallel general-purpose computing thanks to their number-crunching capabilities, and over time more supercomputers rely extensively on these accelerators. Another essential metric is the energy efficiency, often measured as the performance per watt, since the operating cost of supercomputers often outshines the initial buying price. For that aspect, GPUs prove their meaningfulness, as can be seen on Fig. 1.13.

Towards simpler parallelization

Some projects, such as OpenACC, even aim for high-level abstractions able to transparently dispatch computation to any CPU and/or GPU available. This can be seen as an extension of OpenMP to heterogeneous CPU/GPU systems. While this does make things a lot easier for "simple" problems, the developer's experience is still essential to achieve good performance. ). Data can be obtained at www. green500.org. The peak in November 2011 appears to be due to IBM's GPU-powered iDataPlex DX360M3 clusters that entered the market that year. New languages have also been designed for this specific purpose, such as Cray's Chapel. For this language, the objective is to provide a higher level of expression for the user, and a clean separation between algorithms and data structures.

While high-level frameworks and development tools simplify the developer's work, the resulting software may use more resources than actually needed, which becomes a problem when an embedded system or portable device with limited resources/battery is targeted (e.g. smartphones).

Application to our context

The emergence of parallel architectures greatly benefited scientific fields, such as linear algebra [Courtecuisse 09], fluid dynamics [ Rustico 14], machine learning [ Bekkerman 11], or even robotics [Tasora 11; Pan 12a; Park 14].

In [ Lengagne 13], the time-interval loop is parallelized over OpenMP CPU threads, and since there were more time intervals than CPU cores available (8 cores), this means that the timings listed already reflect a full usage of the CPU (if we ignore any possible code optimization). Each thread still relied on automatic differentiation and sequential algorithms, and the only way to achieve any good speedup on the CPU would be to completely redesign the whole lower-level computation. Thus, the following question was raised: "By considering these issues, it could be interesting to design a new solver that can use the computational efficiency of the GPGPU and exploit the sparse properties of the problem under study."

Conclusion

In this chapter, we have defined the motion planning problem, and presented a range of methods able to generate robot motions under different hypotheses. While sampling-based methods are the most popular to generate collision-free paths, post-processing steps are necessary to obtain smooth and viable motions that can be safely run on the robot. Alternative methods such as optimization-based motion planning can handle more complex problems, but do so at the cost of much higher computation times.

We have also introduced some existing parallel computing architectures that can help software developers and researchers accelerate their calculations. Thanks to reduced energy consumption, such computing platforms are slowly entering the embedded system market, and robotics is starting to truly benefit from it, notably for computer vision that relies on expensive image processing.

Chapter 2 Problem formulation and multi-level parallelism

The previous chapter already introduces a concise explanation of the existing work in the domain of planning and general purpose graphic processing units. As the main issue that we try to challenge is the computation time of the optimization-based motion planning method, it is important to understand the choices that are made in this thesis. We first detail the problem formulation, and introduce the polynomial approximation of the constraints over time intervals. Then, we introduce several key performance-related concepts, and we present the core idea of our approach, that is the several levels of parallelism that will serve as a basis for our highly parallel library.

Problem Formulation

First, we model and formulate the planning problem independently from the solver. Problem formulation means defining the optimization problem, i.e. the cost function and the constraints that all depend on the optimization parameters. Cost and constraints are to be evaluated over given time intervals at each solver iteration for a given value of the optimization variables supplied by the solver. The evaluation tells us which constraints are violated or not, and by how much.

Robot definition

A robot is composed of a set of rigid objects, called links or bodies, assembled by joints. We consider robots with tree-like structures, i.e. the graph where the bodies are the nodes and the joints are the edges, is a tree. This is called a kinematic tree. For a robot with a fixed base, the base is chosen as the root of the tree. For robots without a fixed base, we consider only movements where at any time, at least one body is in contact with a rigid environment (no flight phase). The root can hence be transferred from a contacting body to another contacting body during the entire motion. For example, for a humanoid robot, when a foot is on the floor, this foot is chosen to be the root. We note n the number of joints (so that there are n + 1 bodies).

Equation of Motion

The motion of a robot can be described by a function q of the time, where q(t) is the vector of the joint parameters (here angular values) at time t, which is called the configuration.

Not all functions q are admissible though. They must obey physics laws which, in robotics, are expressed by the Equation of Motion (EoM) and contact with friction. For a robot without a fixed base, yet with a fixed body, the EoM writes (see also [Chrétien 15]):

M r (q) M j (q) q + B r (q, q) B j (q, q) = 0 I τ + J ⊺ r (q) J ⊺ j (q) f (2.1)
where M is the inertia of the robot, B represents the contribution of the gravity and the effect of speed (Coriolis forces, etc.), τ is the vector of joint torques, f is the vector obtained by stacking all forces f k (k ∈ [1, n f ]) applied on the robot at points p k (including contact forces) and J is the Jacobian matrix of all points p k , obtained by stacking the matrices ∂ p k ∂ q . The upper part of this equation (subscript r, for robot) is directly the Euler-Newton laws expressing that the acceleration and change of angular rate of the robot, seen as a single rigid object, are function of the external forces. It does not appear for fixed-base robots. The lower part (subscript j, for joint) relates the inertia and external forces to the joint torques. Some forces f k are further restricted by friction laws (we use Coulomb's laws) to live in a subset F , i.e. we have:

f ∈ F (2.2)
where F is the product of the Coulomb cones of friction at each contact point.

Cartesian quantities

At configuration q(t), the position and orientation of the i-th body w.r.t. a global world frame are given by a vector x i (q(t)) and an orientation matrix Θ i (q(t)), so that a point with coordinates p in the body's frame has coordinates Θ i (q(t))p + x i (q(t)) in the world frame. The spatial and angular velocities (resp. accelerations) of the body are denoted by the vectors ẋ(q(t)) and ω i (q(t)) (resp. ẍi (q(t)) and ωi (q(t))), so that the speed of the same point is ω i (q(t)) × p + ẋi (q(t)) (and the acceleration ωi (q(t)) × p + ẍi (q(t))), where × denotes the cross product. For the sake of clarity, we drop the dependency in q and simply write x i (t),

Θ i (t), etc.
We denote by G(t) (geometry) the set of all x i (t) and Θ i (t), and by K(t) (kinematics) the set of all their first and second derivatives.

General formulation

The problem we are interested in can be written as follows min.

q, f ,τ h(q(t), f (t), τ(t), G(t), K(t)) (2.3) s.t. eq. (2.1) c i (q(t), f (t), τ(t), G(t), K(t)) ≥ 0 ∀t ∈ I i , ∀i = 1 • • • m
with h and c i real-valued functions, m the number of constraints, I i ⊆ [0, T ] the time interval (possibly a single instant {t i }) on which the i-th constraint must be verified and T the total duration of the movement. Whenever we want to express an equality constraint c = 0, we replace it by c + ε ≥ 0 and εc ≥ 0 with ε small enough to have a good approximation but not too much to avoid over-constraining the resolution of the problem (ε is typically a fraction of the precision we want to achieve, e.g. 0.1mm on a position constraint). This is for instance used to define geometric contact conditions. Note that the unknowns of this problem are functions of time.

Functions h and c i are written in their most generic way here. Even though G and K are functions of q, we make them appear explicitly to emphasize their role as intermediate computation quantities. Each function does not need to depend on all q, f τ, G and K.

Cost and constraints

The cost functions that are usually considered are:

• the jerk, for smoothness: T 0 ... q 2 , • the energy, for efficiency: T 0 τ 2 . Minimizing the total motion time T is considered in [ Lengagne 13], and other costs are possible.

As for constraints, we can distinguish several types. First, we use constraints on intrinsic robot's limitations:

• joint positions q -≤ q(t) ≤ q + , • joint speeds q-≤ q(t) ≤ q+ , • joint torques τ -≤ τ(t) ≤ τ + .
Furthermore, to translate that f ∈ F , we consider:

µ 2 k f 2 k,n -f k,t 2 ≥ 0 (2.4)
where f k,n and f k,t are the normal and tangential part of the force f k , and µ k the friction coefficient. All of these constraints write as c(q) ≥ 0, c(τ) ≥ 0, and c( f ) ≥ 0.

Then we have constraints on the bodies positions c(G) ≥ 0, typically requiring a point with coordinates p in the i-th body to be at a position p des in the world, i.e. Θ i (q(t))p + x i (q(t)) = p des , or that the distance δ (b i ,b j ) (t) between bodies i and j is greater than a safety margin, to avoid collision. Similar constraints c(K) ≥ 0 can be devised. Constraints on body position and velocity are combined to translate high-level tasks.

Finally, we can consider global constraints on the robot:

• position of the Center of Mass (CoM) x g (t),

• velocity of the CoM ẋg (t),

• acceleration of the CoM ẍg (t),

• Center of Pressure (CoP) at some contacts. Some of these constraints are used to ensure the robot satisfies a stability criterion, e.g. for walking, the CoP should remain within the robot's support polygon.

Parametrization of variables

The above optimization problem is not solvable because its variables live in a function space of infinite dimension. To make its resolution tractable, we need to restrain the search space by approximating it. We do so by using parametrized functions. The choice of parametrization must be made carefully or there will not be any solution verifying eq. (2.1) at all time. First, we can see from the lower part of eq. (2.1) that τ can be directly obtained from q and f . This is called inverse dynamics. We can therefore remove it from the variables together with this part of the equation. Second, we will see in Chap. 5 how to parametrize q and f to satisfy the upper part of eq. (2.1): any parametrization can be chosen for q and a sub-part f of f , the rest of f is deduced from q, f and the upper part of eq. (2.1).

Figure 2.1 Variable dependency graph. While the geometry, kinematics and dynamics depend on q(t) (plus f (t) for the dynamics), the kinematics (resp. the dynamics) may reuse data from the geometry (resp. the geometry and the kinematics).

We choose uniform B-Splines as parametrization for q and f : for each element y i = q i or y i = fi , N control points are chosen and

y i (t) = N-1 ∑ j=0 p i, j B j,K (t) (2.5)
where B j,K is a basis function of degree K and p i, j the associated control point.

All quantities q, f τ, G and K depend on t and these parameters p i, j (see also Fig. 2.1), thus the problem writes min.

p h(p,t) (2.6) s.t. c i (p,t) ≥ 0 ∀t ∈ I i , ∀i = 1 • • • m
This problem is called a Semi-Infinite Program (SIP), because while the search space is of finite dimension, the number of constraints is infinite (each c i for any given t is a constraint). Furthermore, since we are dealing with nonlinear constraints and cost functions, this problem is also a Nonlinear Program (NLP).

Computation over time intervals

Various possibilities exist to tackle SIP, the most classical one being to enforce the constraints only at discrete instants t i [ Miossec 06]. However, between two such instants, constraints may be violated [ Lengagne 07]. A trade-off must be made between the number of instants (the closer the instants are, the smaller the violation is) and the computation time due to the evaluation of constraints at each instant.

A possible alternative involves constraining trajectories rather than instantaneous values, which is the approach followed in [Lengagne 10a].

Tight inequality constraint satisfaction

Most constraints (e.g. joint limits, link positions, etc.) can be written as a set of inequality constraints ∀t, g(q(t), f (t)) ≥ 0. q(t) and f (t) being parametrized functions of the time, such constraints are eventually expressed as

∀t ∈ [0, T ] , g(x,t) ≥ 0 (2.7)
where x is the vector of all parameters.

In this section, we explain how we handle general inequality constraints to satisfy them continuously over a time interval. We successively reformulate eq. (2.7) to ultimately obtain a finite set of constraints over x only.

For the sake of simplicity, we consider hereafter that g is real-valued. The following developments extend readily to vector-valued functions by considering their elements one by one. First, we rewrite eq. (2.7) as min

t∈[0,T ] g(x,t) ≥ 0 (2.8)
Therefore, we get rid of the time dependency, but end up having to compute the global minimum of a function over an interval, which is intractable in the general case. We then cut the time interval

[0, T ] in N small intervals [t i-1 ,t i ] with 0 = t 0 < t 1 < . . . < t N = T
, where the t i are expressed as fixed fractions of T and further rewrite eq. (2.7) as N constraints:

min

t∈[t i-1 ,t i ] g(x,t) ≥ 0, i = 1 . . . N (2.9)
Note that both reformulations are equivalent to the original.

If the interval i is small enough, we can accurately approximate g on it by a low-order Taylor expansion gi around the middle point (t i-1 + t i )/2. Since gi (t) is a polynomial in t, we can apply root-finding techniques to its derivative to locate its extrema and thus its global minimum over [t i-1 ,t i ]. We note t min i (x) the corresponding minimizer. It is a very good estimate of the minimizer of g over [t i-1 ,t i ] and we define g min i (x) = g(x,t min i (x)). The inequality constraint eq. (2.7) is ultimately rewritten as

g min i (x) ≥ 0, i = 1 . . . N (2.10)
This last reformulation is not equivalent to eq. (2.7) anymore, but is an approximation of it that can be made as accurate as necessary by adjusting the sizes of the intervals and the order of the Taylor expansion, to the expense of the computational speed. For example, we often take time intervals of about 0.2s and Taylor expansion of order 5. This last choice let us use analytical formulae to find the roots of the derivative of gi . Together with the interval size, we get a maximum inaccuracy of magnitude 10 -6 between g and gi . Finding the roots can also be done numerically for higher-order polynomials [ Bartoň 07].

The present constraint handling is an improvement over [ Lengagne 13], where the constraints are also split into small intervals and a Taylor expansion is used, but then gi is transformed into an equivalent B-spline (see Appendix A.3.2), with coefficients b i, j , and the constraint is rewritten ∀(i, j) b i, j ≥ 0, using the fact that a B-spline lies in the convex hull of its control points (a property also used in simpler settings in [Bobrow 01]). This last step can be overly conservative (see Fig. 2.2). Our approach is slightly more expensive, but approximates the constraint more closely, giving more freedom to solve the problem. The fastest state-of-the-art solvers not only require to evaluate the constraints but also their derivatives w.r.t. the optimization parameters. For the above constraint, the derivation goes as follows:

Error on max

Error on min

∂ ∂ x g min i (x) = ∂ ∂ x gi (x,t min i (x) = ∂ gi ∂ x (x,t min i ) + ġi (x,t min i ) ∂t min i ∂ x (2.11)
where ġi is the derivative of gi w.r.t. its time dependency. We face 3 possibilities:

• t min i lies strictly in the interval [t i-1 ,t i ] so that we have ġi (x,t min i ) = 0 since t min i is a critical point. • t min i is on the boundaries of the interval, yet ġi (x,t min i ) = 0 (this is a pathological case).
• t min i is on the boundaries of the interval and ġi (x,t min i ) = 0, meaning that t min i would change if we move the boundaries: t min i is independent of x and thus

∂t min i ∂ x = 0.
The second term of eq. (2.11) is then always 0 and we simply compute

∂ ∂ x g min i (x) = ∂ gi ∂ x (x,t min i ) (2.12)
As usual with pointwise minimum, the issue of continuity must be considered: t min i (x) is not a continuous function of x as it can jump from one minimum to another when a local minimum becomes the global minimum. However, t min i (x) is piecewise C 0 , and from eq. (2.12) we get that g min i (x) is piecewise C 1 . Most solvers require that functions are at least C 1 (everywhere), but they are robust to derivative discontinuities as long as they do not occur at the optimum (a case which can occur in robotics, see for example [Escande 14b]). Our early experiments did not show any problem with this discontinuity. We however implemented and tested an alternative solution which gets rid of most of the problem by considering not only gi (x,t min i ) ≥ 0 but also all the constraints gi (x,t c ) ≥ 0 for all "critical" points t c : all (local) minimizers and maximizers of gi over interval i as well as the real part of complex roots of ġi . The possible robustness increase is not worth the additional computational burden, but we can use this solution as a backup if we encounter a problem one day.

Equality constraints

Equality constraints can be difficult to handle when dealing with trajectories. In the scope of this work, we rely on two approaches:

1. Treat the equality constraint as two inequality constraints with a given tolerance ε, i.e.

g(x) = g ref is treated as -ε ≤ g(x) -g ref ≤ ε.
The problem is that the constraint will not be exactly satisfied, and it may lead to an over-constrained problem. This is the approach we use for our equality constraints on positions, velocities, etc. 2. Use a different parametrization for the problem, e.g. if the initial problem contains:

∀x ∈ X , g(x) = 0 ∀x ∈ X , h(x) ≤ 0 we instead consider: ∀x ∈ X , h(x) ≤ 0 where X = {x ∈ X | g(x) = 0}.
While this approach can be more complex to integrate to an existing optimization framework, it ensures that the equality constraint is always (exactly) satisfied. For more details and an example of this approach, see Chap. 5.

Computational complexity

Compared to the time-grid evaluation, the polynomial approximation leads to more expensive computation: every operation involves polynomial vectors or matrices instead of simpler scalar matrices. This partially explains the difference in computation times between the previous timegrid optimizer [ Miossec 06] and the early work on the polynomial CPU optimizer [Lengagne 10a], and justifies the following OpenMP parallelization. Note that both implementations relied on Ipopt [Wächter 06] for the optimization process.

On the other hand, the number of constraints for the polynomial approach can be much lower: a single continuous constraint can replace multiple discrete constraints, thus decreasing the number of rows of the Jacobian matrix. Still, only 13 evaluation points were considered in [ Miossec 06], while 10 time intervals were handled in [Lengagne 10a] for a similar problem.

As for comparison of the reported computation times, the second paper does not report computation times of the constraint evaluations (including gradients) independently from Ipopt's computation, so a sound and fair comparison is not possible. Indeed, the limitedmemory BFGS method used in Ipopt can have a large impact on the computation time, and it largely depends on the settable number of most recent iterations taken into account.

Performance-related concepts

In order to have a computer program run as fast as possible, a good understanding of some key concepts is necessary before even designing the underlying data structures. Thus, we will introduce notions or libraries that are common when tackling performance issues.

Sequential bottlenecks

In software engineering, a bottleneck is a part of the code that severely limits the global performance of a program. When trying to optimize a given program, the first step is to track down those bottlenecks, and then to optimize them to increase the global throughput. These bottlenecks can be due to the complexity of the algorithm (e.g. the bubble sort's O(n 2 ) complexity), disk input/output (especially with older hard disk drives), etc. Bottlenecks need to be studied relatively to the rest of the software. Let us consider a simple program that only consists in a loop over two GPU functions (or kernels) f1 and f2, and that loop needs to be evaluated a thousand times. If f1 takes 1s, and f2 takes 1ms, then f1 is likely to be where we should focus our efforts, since 99.9% of the time is spent there. If f1 takes 10 µs and f2 takes 10 ns, the exact same conclusion can be reached, since the relative importance is similar.

For example, when executing a CUDA kernel, there is an overhead of 9 µs for synchronous calls, and 3 µs for asynchronous calls (measured on a GeForce GT 650M GPU). While this appears negligible, if the program is supposed to take the same order of magnitude for its execution, this overhead becomes a problem, and a potential bottleneck. For comparison, a direct call to an empty CPU function has an overhead in the order of the nanosecond on an Intel Core i7, so between 3 and 4 orders of magnitude faster compared to the GPU equivalent.

OpenMP

The implementation of the work presented in [ Lengagne 13] relies on OpenMP to parallelize the computation done on each time interval (direct computation, gradient evaluations). OpenMP is a multi-platform shared-memory parallel programming API in C/C++ and Fortran. It provides an easy way to parallelize loops over multiple threads, for instance: std :: vector < Model > models ( n ); // Compute the models in parallel # pragma omp parallel for for ( int i = 0; i < n ; i ++) models [ i ]. compute ();

Listing 2.1 Simple example of OpenMP usage.

While this does make writing a parallel program easy, and help to achieve a good speedup with a minimum of changes to the code for simple problems, in the case of the previous CPU implementation, only the time-interval loop was parallelized, and further parallelization was impossible without a complete redesign of the whole library.

A similar approach is being actively researched for heterogeneous CPU/GPU systems: OpenACC (which stands for Open Accelerators) aims at extending OpenMP to support systems with various accelerators. The objective is to dispatch computation to the available accelerators, whether these are CPUs or GPUs, and reduce the complex work that needs to be done by the developers.

Data locality

Although writing a parallelized program is simple with an API such as OpenMP, obtaining good performance is another issue. For instance, to achieve optimal performance, avoiding cache misses is crucial. A cache miss refers to a failed attempt at fetching or writing data from/to a fast memory cache. If some data is not available in the fast cache hierarchy, it has to be retrieved from the slower main memory (see Table 2.1), which induces a slowdown in the execution since the thread stalls until the data is available. Short stalls can usually be hidden by the compiler thanks to out-of-order executions, that is the processor reschedules instructions based on the availability of the data.

An essential concept is the locality principle (or "locality of reference") [ Denning 05]. It distinguishes multiple types of data locality: Spatial locality we access some pieces of data stored contiguously, or even better linearly, in memory,

Temporal locality we access the same data repeatedly in a short time.

If data is properly organized, i.e. data that needs to be processed simultaneously is stored contiguously, a single fetch operation can be used for multiple pieces of data, thus reducing the overall delay induced by these operations. If some data is reused shortly after being loaded, it is likely to still be available in a fast cache.

A common textbook example is matrix multiplications. Depending on the memory layout and the order of the loops used in the computation, a large performance gain can be obtained by properly handling caches. In the first version (Listing 2.2), a cache miss is observed during each iteration of the inner loop when fetching b[k][j]. Thus, if the matrix size N is too large compared to the sizes of the caches, b[k][j] will no longer be in the cache when it is next required, leading to a fetch from slower cache or RAM.

// " Slow " matrix multiplication for ( int i = 0; i < N ; ++ i ) for ( int j = 0; j < N ; ++ j )

{ // [ not in cache ] // | // v double v = c [ i ][ j ]; // Inner loop for ( int k = 0; k < M ; ++ k ) { // c [ i ][ j ] = c [ i ][ j ] + a [ i ][ k ] * b [ k ][ j ] // // [ in cache ] [ not in cache ] // | | // v v v = v + a [ i ][ k ] * b [ k ][ j ]; } // [ not in cache ] // | // v c [ i ][ j ] = v ; }
Listing 2.2 Matrix multiplication with lots of cache misses in the inner loop.

On the other hand, the version presented in Listing 2.3 ensures that b[k][j] remains in the cache, leading to less cache misses, and thus higher performance.

For these two basic versions, computation with N=5000,M=5000 took respectively 390 and 86 seconds, showing the impact that caching may have even on such simple examples (execution 4.5 times faster by just inverting two loops). An improved version would involve loop tiling, that is iterating other blocks of the matrix to improve spatial and temporal locality. This shows how important caching can be when trying to achieve good performance, and how data locality needs to be carefully taken into account when designing high-performance software dealing with a large data set. // Faster matrix multiplication for ( int i = 0; i < N ; ++ i ) While this is also a problem in single-threaded programs, the impact of cache misses can be greatly amplified on shared-memory multiprocessors (more communication required). Avoiding cache misses requires properly organizing the memory layout of the data structures, and adapted algorithms that will use efficient memory access patterns.

for ( int k = 0; k < M ; ++ k ) // < --+ { // | // [ not in cache ] // | // | // | // v // | inverted loops double v = a [ i ][ k ]; // | // | // Inner loop // | for ( int j = 0; j < N ; ++ j ) // < --+ { // c [ i ][ j ] = c [ i ][ j ] + a [ i ][ k ] * b [ k ][ j ] // // [ in cache ] [ in cache ] // | | | // v v v c [ i ][ j ] = c [ i ][ j ] + v * b [ k ][ j ]; } } Listing 2.3

Thread synchronization, barriers and atomic operations

In a multithreaded program, it is common for threads to work collaboratively on the same data. Since all of the threads may not run at the exact same speed, synchronization steps are required to ensure the proper execution of the program, else we face the following data hazards: In these 3 cases, a change in the execution order of the instructions leads to different results. These behaviors are called race conditions.

Read
When writing parallel algorithms, parts of the computation may require some previously computed data. Thus, we need to ensure that all computation on that data is done before using it: this is done by a specific synchronization method called barrier. When reaching a barrier, a thread has to stop and wait until all of the other threads have reached the same point. Such synchronizations are strong constraints on the execution of the program, and can thus reduce its performance. Therefore, these synchronizations need to be used sparingly.

A related concept is atomic operations. On his blog1 , Jeff Preshing gives the following definition:

"An operation acting on shared memory is atomic if it completes in a single step relative to other threads. When an atomic store is performed on a shared variable, no other thread can observe the modification half-complete. When an atomic load is performed on a shared variable, it reads the entire value as it appeared at a single moment in time. Non-atomic loads and stores do not make those guarantees."

They are more efficient than the classical mutual exclusion approach, but are more limited and can still have a strong impact on performance if not used in the right context. For instance, if trying to compute the sum of a vector in parallel, smart reduction algorithms should be used instead of simply letting each thread call atomic adds for their assigned elements of the vector.

A word on system topology

Components of a computer cooperate by exchanging data and operating on it. Depending on the organization of the components, the kind of data buses used, or the complexity of the tasks that components need to solve, communication itself may have a noticeable influence on the overall performance.

Such performance considerations may happen within the multi-core CPU itself, as expressed by the issues of data locality in the previous section. Nowadays, most Intel multi-core CPUs support Hyper-Threading, that is for each physical CPU core, the system sees multiple virtual cores that share caches (see Fig. 2.3) and will share some workload. When developing a multithreaded application on the CPU, concepts such as processor affinity (whether to bind a thread to one or more processors) may help improve performance by taking into account this underlying architecture. For instance, virtual threads of the same physical core will compete for the shared execution units, thus reducing the throughput compared to threads of different physical cores.

Another possible bottleneck when computing on the GPU is the data transmission through the PCI Express (PCIe) bus between CPU and GPU (see Fig. 2.3). Any data copied to and from the GPU goes through this bus, which has limited bandwidth and induces a limited overhead (usually a fraction of a microsecond). There has been extensive work by chipset manufacturers such as Intel: the PCIe controller is now part of the integrated circuit, drastically reducing latencies. Bandwidth on the other hand can be an issue: when gigabytes of data need to be processed, memory transfers may become the main bottleneck (see Fig. 2.4), and the only solution is to reduce the copy requirements, or try to hide the latencies by overlapping computation and transfers.

Parallelizable pipeline

In order to reduce computation times as much as possible, it appears essential to minimize the sequential parts of the optimization process. A high-level parallelism, involving independent Figure 2.3 Topology of a system as returned by lstopo on Linux. The CPU, its caches (L1, L2, L3), cores and threads can be seen on the left, and the peripherals connected to it through PCIe buses are on the right (e.g. the GPU labeled card0). time intervals, can ease the convergence of the solver, thus potentially reducing the computation time. This is the approach that was followed in [ Lengagne 13], and parallelization was done on multi-core CPUs. When considering highly parallel architectures (e.g. GPUs), two problems arise:

• the number of time intervals will be much lower than what a GPU can handle (hundreds or thousands of cores), • on each time interval, calculation is computationally expensive: contact forces, dynamics, collision detection, gradient evaluations, while dealing with polynomial matrices and vectors.

We can imagine increasing significantly the number of time intervals, but in that case the quantity of data handled by the solver could become too large, and the GPU's memory could be saturated. Moreover, this may not reduce the global computation time: although the finer granularity could help the solver converge in less iterations, each optimization step would be a lot more expensive to compute.

Reducing the time taken at each iteration implies solving these sequential bottlenecks. These parts of the process involve robotics computation and gradient evaluations. Finding efficient algorithms and data structures able to deal with these low-level computations is therefore essential. 10 -6 10 -5 10 -4 10 -3 10 -2 10 -1 10 0 10 1 10 2 10 3 10 4 10 -3 Figure 2.4 Transfer time for different types of memory (pageable or pinned) as well as transfer directions (to and from the GPU) for a GeForce GT 650M with PCIe 2. While the transfer time is nearly constant (∼10µs) for small transfer sizes, it increases linearly after 8kB. Then, maximum throughput is reached (bandwidth limit). Overall, pinned (or page-locked) host memory is more efficient than the default pageable memory, i.e. memory that can be swapped out. This is due to the fact that when transferring data to or from the GPU, pageable memory is first copied to pinned memory, and that extra copy can be bypassed. However, allocation and deallocation of pinned memory is more expensive. Note that this only concerns host memory, as device memory is automatically page-locked.

The main objective of this work is thus to obtain a GPU pipeline that uses as much GPU computing power as possible while avoiding bottlenecks. Additionally, each part of the optimization process has to be computed on the GPU so as to limit slow data transfers between CPU and GPU through the PCI Express bus.

Overview of the computation graph

A graph of the computation is presented in Fig. 2.5. We can distinguish several distinct steps in this process:

Forward kinematics/inverse dynamics computation

Given the joint trajectories q(t), contact frames and contact forces f (t), we need to compute the positions, orientations, velocities, accelerations of the links, as well as the joint torques τ(t).

Collision detection

Knowing the positions of all the links at any time t, we need to evaluate the minimum distances between pairs of links.

Joint constraints evaluation

Joint positions, speeds and torques must remain between hardware-imposed bounds.

Tasks-related constraints evaluation

Extra constraints may be required to properly describe the tasks: position of contact frames, additional joint constraints etc.

Cost function evaluation

This describes the function that we will try to minimize. This could for instance be the energy consumption, the total execution time, the jerk of the joints trajectories, etc.

Optimization solver iteration

Depending on the current optimization parameters, constraints and cost function, the solver deduces a new set of optimization parameters. If an optimal solution has been found, the process is stopped. Else, we continue with these new parameters.

It is important to keep in mind that the gradients of all these physical parameters are required for the computation of the constraint Jacobian matrix. Moreover, since these parameters are represented as polynomials, evaluating constraints comes down to finding polynomial bounds on given time intervals.

The computation is also split between the CPU and the GPU. For example, building the optimization problem (cost function, constraints) and the actual optimization solver run on the CPU, while the evaluation of the robot computation, of the constraints and their Jacobian matrix is done on the GPU. A global overview of this organization is presented on Fig. 2.6. P, q 0 (t)

Compute dynamics

Mission contact frames q k (t) Evaluate joint limits q, q, q,τ Collision avoidance Environment q obstacles Evaluate task constraints q, q, q,τ tasks Evaluate cost function q,τ Optimization solver

F,C,δ F,δC,δ 2 F,δ 2 C Optimal solution? q f (t),T f q k+1 (t) yes no q k (t)

!" !" !" !" !" !"

! "#$ %! %&' %&' %&' %&' %! !'' '' '' ''

!"

! " (% (%! %&' %!''( ;( ;( ;( ;( ;( ;( ;( ;( ;(

Main ideas

To speedup the computation needed to evaluate the constraints, we consider the following axes of parallelization:

Data-independent parallelism Parts of the computation can be evaluated in parallel and fully independently, i.e. two distinct parallel evaluations will not share any data at any point. This includes:

• Evaluations over time intervals,

• Evaluations of gradients w.r.t. different p i ,

• Evaluations of constraints of the same type.

For this kind of parallelism, the synchronization issues presented in Subsec. 2.3.4 are irrelevant, since at this abstraction level, threads for different gradient evaluations do not operate on the same data. Note that the previous work only tackled the first point, that is parallel evaluations over time intervals, thanks to OpenMP (cf. Subsec. 2.3.2).

Data-dependent parallelism

Parts of the computation may rely on parallel algorithms using shared data. In our case, we have a model-based parallelism that exploits the properties of the kinematic tree describing the robot. This implies that shared data accesses must be thoroughly examined to avoid race conditions. Moreover, since we are at the lowest abstraction level, data locality (presented in Subsec. 2.3.3) needs to be carefully studied, as the whole computation relies on these low-level operations. We can thus distinguish a high-level data-independent parallelism from a low-level datadependent parallelism (see Fig. 2.7). These different levels can be studied separately. Moreover, though the scope or the individual speedup of each parallelization approach may be limited, the cumulative speedup is what actually matters.

Data-independent parallelism

Each interval can be treated individually. This was already accounted for in [ Lengagne 13] where each CPU thread handles a different interval. This is a direct result of Sec. 2.2.

The same observation applies to the gradients w.r.t. the optimization parameters. Indeed, all the optimization parameters (spline control points) are independent from each other. Thus, if N int is the number of time intervals, K the order of the B-Splines, n the number of joints; we need to evaluate N int (K + 1)n gradients. Hence, the number of gradients that need to be evaluated grows linearly with N int , and they can be computed separately as well.

We designed our time intervals as follows: if we consider a B-spline of order K with N cp control points, we can split the curve into N int = N cp -K intervals (a.k.a. segments or bays) and each control point only influences K + 1 intervals. Note that in order to refine the polynomial approximation, our implementation can either increase the number of control points or (equivalently) split these time intervals further, which would, in both cases, increase the number of constraints applied to the problem.

Data-dependent parallelism

The development of efficient parallel dynamics algorithms for robotics applications has been extensively investigated during the last decades [Featherstone 99a; Featherstone 99b; Yamane 06; Yamane 09]. Current trends exploit multi-core and many-core architectures to reduce the computation time [Tasora 11; Bhalerao 12; Park 14]. The parallel methods usually provide good speedup for long kinematic chains, but in the case of actual robots, even complex ones, the number of joints rarely exceeds 50 (e.g. humanoid HRP-2: 32 or up to 43 joints with the hands), split over multiple chains, which makes the speedup with parallel methods not substantial. Thus, we decided to use methods that would allow for as much of the computation to be run in parallel, with a minimum quantity of synchronization steps. The choices that have been made regarding the computation of the dynamics are presented in Chap. 3.

Conclusion

Given the specific requirements of this nonlinear optimization-based motion planning approach, we have been able to highlight parts of the optimization pipeline that can greatly benefit from parallel computing architectures such as GPUs. The previous work already considered the parallelization of the time-interval loop, but there are many other parts of the pipeline that can be parallelized in order to greatly reduce the overall computational time. A presentation of the algorithms, data structures, and implementation choices can be found in the next chapters.

Chapter 3 Optimization-friendly parallel computation of the dynamics

The dynamics of a robot is defined as the relationship between the forces acting on the system (gravity, contact forces, Coriolis effects) and the accelerations they produce. We can distinguish the forward dynamics, i.e. the problem of finding accelerations knowing the forces, from the inverse dynamics, which is finding the torques and forces knowing the accelerations. In our context, since our optimization parameters are the joint trajectories given as splines, joint accelerations are known, and we need to find the forces and torques acting upon the robot.

Computing the dynamics of a robot can be an expensive task, even more so when dealing with a complex robot model. Since these calculations are at the center of our problem, being able to compute them as fast as possible is a key element to achieve better performance.

Available methods

Computing the inverse dynamics of articulated bodies is an intrinsically recursive task [Featherstone 87; Lee 05]. In our case, we are dealing with the dynamics of the humanoid robot HRP-2, which consists in 43 bodies, or 29 for a simplified model with fixed hands and neck. Several solutions can be considered:

• a pre-computation/generation step of optimized code for a given robot model using symbolic computation, • a parallel computation scaling with the number of bodies and contacts, • a parallel computation scaling with the number of time intervals, • a higher-level approach (solver) allowing us to launch several optimization computations simultaneously,

• a hybrid approach based on some of these ideas.

We also need to take into account our target computing architecture, i.e. GPUs, and the properties of our approach: (i) polynomial computation (B-spline parametrization), and (ii) gradient evaluations required.

Symbolic computation of the dynamics

Symbolic computation deals with mathematical expressions rather than numerical data. For example, we can obtain a mathematical expression of the dynamics, and it can then be evaluated for different times t.

This method is a powerful tool as the mathematical expressions that we obtain can then be simplified, while a traditional numerical algorithm may contain redundant computations. Then, most symbolic computation framework can generate efficient C code for the evaluation of these expressions. Indeed, we can easily obtain independent functions used to compute every relevant parameter (position, velocity, acceleration, torque. . . ) for every body/joint in terms of the robot configuration and external forces. However, the code generation needs to take into account a large set of outputs when generating the computation graph: intermediate data computed to evaluate the torques may also be required when computing the accelerations, and it should not be computed twice.

Additionally, this approach is mostly MIMD-friendly. These independent functions can be computed in parallel on MIMD architectures: every computing unit would be in charge of a given function. A similar approach was chosen in [Zomaya 92]: they managed to divide the dynamics computation in subtasks, and these subtasks are computed independently on a transputer array (MIMD). However, efficiently applying this method to SIMD architectures is not straightforward. Still, turning GPUs into MIMD-capable hardware thanks to a set of compiler, assembler and interpreter is being actively researched [Dietz 10].

Parallel methods scaling with the number of bodies

The most common approach is the one that deals with the different bodies in parallel [ Yamane 09]. For scenarios with a lot of moving parts, this approach increases the complexity of the scenes that can be processed in real time. With simple objects (e.g. grains of sand), these methods make it possible to simulate millions of objects much faster [Tasora 09; Tasora 11] (see Fig. 3.1). When dealing with more complex models, other works coupled these methods with the simplification of the dynamics to obtain even better speedups [Redon 05]. The main objective is usually interactivity, that is real-time simulation allowing the user to interact with the scene. This implies a discrete parametrization of the simulation which is updated -in parallel-at each time step. The methods that follow this approach often deal with instantaneous evaluations that need to be done at interactive rates, while in our case, the parameters we are dealing with are time polynomials, not scalar values.

As for the computation of the dynamics for articulated bodies, many parallel algorithms have been developed during the last two decades:

• Constraint Force Algorithm (CFA) [ Fijany 95],

• One problem arises: in order to be efficient, a GPU has to launch up to thousands of threads in parallel that execute the same operations. In Table 3.1, a GPU puts us on the last row, so the number of bodies N B needs to be at least as high as the number of processors N P ,

Assembly Disassembly

Figure 3.2 Assembly-disassembly phases allow the resolution of the motion equations in a recursive and parallel manner [ Bhalerao 12]. The method revolves around the idea of Articulated-Body Inertia (ABI) [Featherstone 83], the equivalent inertia of a set of "assembled" bodies connected by joints, and is the basis for the DCA method [Featherstone 99a]. The first assembly-disassembly pass computes the kinematics, and the second handles the system kinetics.

which is not even close in our case. Furthermore, a detailed study of a DCA-like algorithm, DCA-ABA [Bhalerao 12], teaches us that on a many-core architecture, the communication cost between threads can be more expensive than the speedup provided by the parallelization. If the objective is to have a large speedup compared to the sequential algorithms, Bhalerao et al. inform us that the number of processors has to be lower than N B 8 .

Alternative methods for the parallel computation of the dynamics

Other methods which are either less popular or more recent provide a way to compute the dynamics in parallel. Although these methods may not be adapted for the control of the robot, the GPU pipelines presented or the data structures chosen provide a real scientific benefit.

For example, the state-time formulation [ Anderson 05] is based on the idea that we can use time as a state variable of the problem for discrete approaches, and thus the method scales both spatially and temporally.

We can also mention position-based dynamics [Müller 07] which involves the use of the positions of objects, not their forces or velocities. This method is consequently a really good candidate for real-time interactivity. The user can manipulate objects of the scene, without the need for several integration/derivation steps. They concentrate their effort on deformable objects, but conclude on the possibility to use their work for the simulation of rigid bodies.

The impulse-based dynamic method [Bayer 09], developed for GPGPU, is particularly adapted to articulated bodies with a large number of links. It is therefore an interesting technique for the simulation of deformable objects, such as cloth, that can be simulated with numerous interconnected parts (see Fig. 3.3). The thinner the mesh, the better the simulation.

Nonrecursive inverse dynamics

The algorithms presented in Subsec. 3.1.2 (e.g. DCA) often reach a O(log(n)) complexity, where n is the number of bodies, thanks to a parallel recursive approach. This implies an imbalance when distributing the computation to the parallel processors. For example, for a kinematic chain of length n, the first pass of the parallel algorithm handles n/2 virtual bodies, the second one n/4, etc. Thus, these recursive formulations may not be the best candidates for computation on the GPU in our context, as individual evaluations would remain particularly expensive.

The method presented in [Zhang 98] provides a way to compute the inverse dynamics on n parallel processors. Since the paper does not answer all of our questions (expression of velocities and accelerations, mobile free-flyer, differentiation of the equations, contacts etc.), it is necessary to go into the details and work on the missing parts. Moreover, the nonrecursive property highlighted in the title is slightly misleading: the computation of the inverse dynamics of an articulated body is intrinsically recursive because of joint equality constraints. It is nevertheless possible to compute a large set of temporary parameters in parallel before using a parallel prefix sum, without relying on high-level recursive functions.

Parallel computation with prefix sums

For each stage of the computation, we will favor parallel methods that reduce synchronization steps as much as possible while allowing the GPU to compute kernels (GPU functions) in parallel. The objective is to have most of the computation done in parallel with fully independent kernels, and the rest is made of prefix operations that can be computed in a logarithmic parallel approach. Note that we only deal with 30 or 40 bodies, and the longest chain of the kinematic tree is not likely to contain more than half of the bodies, so the parallel speedup is not as important as for prefix sums over large vectors. algorithms exist when dealing with large vectors (see Fig. 3.5), especially when targeting a specific computing architecture.

Prefix sums are useful when dealing with kinematic chains. For example, if we want to compute the positions of the bodies of a 2D 4-link kinematic chain (x i , y i ) ∀i ∈ {0, 1, 2, 3} (see Fig. 3.6), we have:

x 0 = a y 0 = b x 1 = x 0 + L 0 cos(θ 0 ) = a + L 0 cos(θ 0 ) y 1 = y 0 + L 0 sin(θ 0 ) = b + L 0 sin(θ 0 ) x 2 = x 1 + L 1 cos(θ 0 + θ 1 ) = a + L 0 cos(θ 0 ) + L 1 cos(θ 0 + θ 1 ) y 2 = y 1 + L 1 sin(θ 0 + θ 1 ) = b + L 0 sin(θ 0 ) + L 1 sin(θ 0 + θ 1 ) x 3 = x 2 + L 2 cos(θ 0 + θ 1 + θ 2 ) = a + L 0 cos(θ 0 ) + L 1 cos(θ 0 + θ 1 ) + L 2 cos(θ 0 + θ 1 + θ 2 )
y 3 = y 2 + L 2 sin(θ 0 + θ 1 + θ 2 ) = b + L 0 sin(θ 0 ) + L 1 sin(θ 0 + θ 1 ) + L 2 sin(θ 0 + θ 1 + θ 2 )
where (a, b) is the position of the root body. This can be rewritten with the following prefix sums:

Θ = [θ 0 , θ 0 + θ 1 , θ 0 + θ 1 + θ 2 ] = prefix_sum([θ 0 , θ 1 , θ 2 ]) [x 0 , x 1 , x 2 , x 3 ] = prefix_sum([a, L 0 cos(Θ 0 ), L 1 cos(Θ 1 ), L 2 cos(Θ 2 )]) [y 0 , y 1 , y 2 , y 3 ] = prefix_sum([b, L 0 sin(Θ 0 ), L 1 sin(Θ 1 ), L 2 sin(Θ 2 )])
where θ i is the angle representing the relative orientation of the i-th link w.r.t. its predecessor, and Θ i its absolute orientation w.r.t. the world frame.

The only issue is that these algorithms only tackle the case of chain dependencies, while we deal with a tree structure. Thus, when dealing with kinematic trees that involve multiple branches, these algorithms cannot be used as such and need to be adapted to branching. A

y x y 0 x 0 y 1 x 1 y 2 x 2 y 3 x 3 θ 0 L 0 θ 1 L 1 θ 2 L 2 Figure 3.6 Example kinematic chain.
simple suboptimal possibility (if the number of branches is limited) involves processing subbranches in a bottom-up approach, which may require a few passes that are run sequentially.

Finally, a requirement of this approach is that values need to be represented in the same frame (e.g. world frame), so that simple additions/multiplications are possible without any expensive frame transformation.

Notations and overview

We will be using the following coordinate system:

• R i = X i Y i Z i describes the coordinate system of the i-th link, while x i y i z i designates the coordinate system of the i-th joint. The origin of X i Y i Z i is located at the center of mass of the link. Links are numbered from 0 to n -1. • R g = X g Y g Z g is the world frame (Galilean reference frame).

• i X indicates that the vector or tensor is expressed in the link's coordinate system X i Y i Z i .

• X i indicates that the vector or tensor is related to the i-th link.

• X ⊺ indicates that the vector or tensor is transposed. • X indicates that this is the skew-symmetric matrix associated with X (see Sec. A.1).

• n is the number of joints (hence n + 1 is the number of bodies).

The algorithm follows the following steps:

1. Determine the position and orientation of the free-flyer. 2. Compute the geometry: position and orientation of all the links w.r.t. the world frame. 3. Compute the forward kinematics (velocity and accelerations).

Position and orientation of the free-flyer

We consider robots with tree-like structures, i.e. the graph where the bodies are the nodes and the joints are the edges is a tree. This is called a kinematic tree. To position the robot's bodies in the scene, we need to know:

• the position and orientation of one body of the robot in the world frame,

• the relative positions and orientations of the other bodies w.r.t. that specific body. That root body is commonly referred to as the free-flyer, and it usually coincides with the root of the kinematic tree. For humanoid robots, this is often the waist link. In the previous work [Lengagne 10b], two options were considered:

1. Let the solver find the free-flyer trajectory.

2. Use the joint trajectories and the contact frames to compute the free-flyer trajectory.

The parametrization of the first choice made the problem overconstrained, and thus did not provide good results. As a result, the second option was used. In our case, we do not rely on a constant free-flying body. Instead, we choose it among the bodies in contact with the environment. Since we know the positions and orientations of the contacts a priori, we can virtually treat the robot as a fixed-base robot. This implies that we do not support ballistic phases (i.e. phases when no links are in contact with the environment), and that different phases of the motion will involve different computation graphs. Nonetheless, this approach greatly simplifies the computation.

Geometry

Since we know the (fixed) free-flyer frame and the joint trajectories q(t) on each interval, we can proceed with the computation of the kinematics and dynamics of the system. The first step is the computation of the geometry, that is the spatial positions x i (t) and orientations Θ i (t) of all the bodies of the robot. For these values, we made the choice to track the positions of the centers of mass, i.e. x i (t) = ----→ OG i (t), and the Θ i (t) matrix actually contains the rotation from the local body frame to the global frame, since in practice most of the transformations are done in that direction.

That part of the computation is intrinsically recursive. If we look at Fig. 3.7, we get the following holonomic constraint:

--→ OG i + - → k i = ---→ OG i-1 + - → l i (3.2) x i (t) + Θ i (t) i k i (t) = x i-1 (t) + Θ i-1 (t) i-1 l i (t) i = 1, . . . , n (3.3)
Let us write this a bit differently, by clearly distinguishing the relation between body orientations. Let λ (i) be the parent index of the i-th body. The preceding joint index is i -1, and we can write the following recursive formulation for the body orientations:

Θ 0 = R ⊺ 0 (3.4) Θ i (t) = Θ λ (i) (t)S ⊺ i-1 R ⊺ (q i-1 (t)) (3.5)
where R 0 is the orientation of the root body, R(q i (t)) is the rotation matrix associated with the i-th joint, S i is a static orientation shift associated with the i-th joint. The transpose operators express the fact that Θ i rotates vectors from the local frame to the global frame.

For this recursion, we can use parallel prefix multiplications. In this case, the operation is particularly expensive since it involves multiplications of 3 × 3 polynomial matrices if dealing with rotation matrices to represent rotations (e.g. large temporary buffers required).

Similarly, for the body positions:

x i (t) = x λ (i) (t) + d i (Θ λ (i) (t), Θ i (t)) (3.6) = x λ (i) (t) + Θ λ (i) (t) i l i -Θ i (t) i k i (3.7) = x λ (i) (t) + l i (t) -k i (t) (3.8)
where d i is the vector from x λ (i) to x i in the world frame that depends on the link orientations and their respective geometric parameters. Thus, if we unroll the recursion of eq. (3.8):

x i (t) = x 0 + ∑ j∈Λ(i) l j (t) -k j (t) (3.9) = x 0 + ∑ j∈Λ(i) lk j (t) (3.10) with lk j (t) = Θ λ (i) (t) i l i -Θ i (t) i k i (3.11)
where Λ(i) is the set of all the parent bodies of the i-th link. Here, we can precompute the lk j (t) terms in parallel, and then apply a parallel prefix sum. This time, the parallel prefix operation does not involve expensive temporary buffers. Note that x 0 is constant w.r.t. time, since we assume the root body fixed on a given interval.

Kinematics

The next step involves the computation of the velocities and accelerations of the bodies. Methods that deal with instantaneous states may need to rely on local derivation schemes (e.g. ẋ(t i ) = x(t i )-x(t i-1 ) t i -t i-1 ). In our case, since we deal with time trajectories expressed as time polynomials, obtaining the time derivatives is as simple as a polynomial derivation. However, this can reduce the precision of the computation. Hence, the polynomial degree used for the computation should account for these successive derivations. Spatial velocities and accelerations are simply given by:

ẋi (t) = dx i dt (t) (3.12) ẍi (t) = d ẋi dt (t) (3.13)
Let ω i and ωi denote respectively the angular velocities and accelerations expressed in the world frame. If we use the following formulae and the skew-symmetric notations (see Appendix A.1):

∀x ∈ R 3 , ∀i ∈ {0, . . . , n}, ∃Θ i ∈ SO(3),    Θ i i x = x Θ⊺ i = ω i (3.14)
We obtain:

i ω i = Θ ⊺ i Θ⊺ i i = 0, . . . , n (3.15) i ˙ ω i = ( Θ⊺ i ) 2 + Θ ⊺ i Θ⊺ i i = 0, . . . , n (3.16)
Again, i ˙ ω i can be computed with a simple polynomial derivation instead.

Dynamics

Now that we know the acceleration of each body, we can determine the resultant of the external forces applied to the bodies f i thanks to the second law of motion:

f i (t) = m i ẍi (t) i = 0, . . . , n (3.17)
where m i is the mass of the i-th link. That link is subject to the following external forces:

• Force exerted by the parent link λ (i):

T λ (i)/i K i = -→ F λ (i)/i -→ M λ (i)/i k i
• Force exerted by the children link(s) µ(i):

T µ(i)/i L i = -→ F µ(i)/i -→ M µ(i)/i l i • Contact forces: T contact i G i = -→ F c i -→ M c i G i • Weight: P i G i = m i - → g - → 0 G i • Other external forces: T ext i G i = -→ F ext i -→ M ext i G i
Note: some parts of the robot can be connected to two bodies or more. For these bodies, there are multiple µ(i) links. For the sake of readability without loss of generality, we assume that there is only one such link in the following equations, as this does not invalidate the approach.

The second law of motion gives us:

D(R i /R g ) G i = T λ (i)/i K i + T µ(i)/i L i + T contact i G i + P i G i + T ext i G i -→ A (R i /R g ) -→ δ G i (R i /R g ) G i = -→ F λ (i)/i -→ M λ (i)/i K i + -→ F µ(i)/i -→ M µ(i)/i L i + -→ F c i -→ M c i G i + m i - → g - → 0 G i + -→ F ext i -→ M ext i G i
where D(R i /R g ) is the resultant wrench of the i-th link, with -→ A (R i /R g ) the acceleration of the system, and -→ δ G i (R i /R g ) the applied torque about G i . We can express the moments at G i :

T λ (i)/i K i = -→ F λ (i)/i -→ M λ (i)/i + --→ G i K i ∧ -→ F λ (i)/i G i T µ(i)/i L i = -→ F µ(i)/i -→ M µ(i)/i + --→ G i L i ∧ -→ F µ(i)/i G i
Thus we get:

-→ A (R i /R g ) -→ δ G i (R i /R g ) G i = -→ F λ (i)/i -→ M λ (i)/i + --→ G i K i ∧ -→ F λ (i)/i G i + -→ F µ(i)/i -→ M µ(i)/i + --→ G i L i ∧ -→ F µ(i)/i G i + -→ F c i -→ M c i G i + m i - → g - → 0 G i + -→ F ext i -→ M ext i G i (3.18)
The relation on the resultant gives us:

-→ A (R i /R g ) = m i - → ẍi = -→ F λ (i)/i + -→ F µ(i)/i + -→ F c i + m i - → g + -→ F ext i
According Newton's third law (reciprocal actions):

-→ F µ(i)/i = - -→ F i/µ(i)
Which implies:

m i - → ẍi = -→ F λ (i)/i - -→ F i/µ(i) + m i - → g + -→ F c i + -→ F ext i ∑ k∈{i}∪M(i) m k - → ẍk = ∑ k∈{i}∪M(i) ( -→ F λ (k)/k - -→ F k/µ(k) ) + ∑ k∈{i}∪M(i) (m k - → g + -→ F c k + -→ F ext k ) ∑ k∈{i}∪M(i) m k - → ẍk = -→ F λ (i)/i + ∑ k∈{i}∪M(i) (m k - → g + -→ F c k + -→ F ext k )
where M(i) is the set of all the (recursive) children bodies of the i-th link.

The sum of forces such as weight, contact forces and other external forces exerted on bodies ∈ M(i) corresponds to all the external forces exerted on S M(i) , the system formed by µ(i) and subsequent bodies. Moreover, if there is a joint branch on the body i, the formula remains the same (sum of the forces applied to the bodies of the branches). Thus, we can deduce the joint force exerted on link i by parent link λ (i):

Γ i = -→ F λ (i)/i = ∑ k∈{i}∪M(i) (m k ẍk -(m k g + f k + F ext k )) i = 1, . . . , n (3.19) = ∑ k∈{i}∪M(i) IF k (3.20)
Once more, Γ i can be computed with a parallel prefix sum, once the IF k values have been computed independently in parallel. Then, if we consider the moments of eq. (3.18), we get:

-→ δ G i (R i /R 0 ) = -→ M λ (i)/i + --→ G i K i ∧ -→ F λ (i)/i + -→ M µ(i)/i + --→ G i L i ∧ -→ F µ(i)/i + -→ M c i + -→ M ext i = -→ M λ (i)/i + --→ G i K i ∧ -→ F λ (i)/i - -→ M i/µ(i) - --→ G i L i ∧ -→ F i/µ(i) + -→ M c i + -→ M ext i (3.21)
According to [Leimanis 65; Hemami 82], the resultant torque applied to a rigid body (expressed in R 0 ) is:

-→ δ G i (R i /R 0 ) = IN i = Θ i ( i J i i ωi -f i ( i ω i )) (3.22)
with:

f i ( i ω i ) =    i w i2 i w i3 (J i2 -J i3 ) i w i3 i w i1 (J i3 -J i1 ) i w i1 i w i2 (J i1 -J i2 )    (3.23)
If we translate this with the matrix notation, with T i = -→ M λ (i)/i the joint torque applied to joint i, and if we use the skew-symmetric operator X for the cross product (see Sec. A.1), we get:

IN i = T i + Θ i i k i Θ ⊺ i Γ i -T µ(i) -Θ i i l i Θ ⊺ i Γ µ(i) + T c i + T ext i T i = T µ(i) + IN i -Θ i i k i Θ ⊺ i Γ i + Θ i i l i Θ ⊺ i Γ µ(i) -T c i -T ext i T i = T µ(i) + IN i -IK i + IL i -T c i -T ext i T i = ∑ k∈{i}∪M(i) (IN k -IK k + IL k -T c k -T ext k ) = ∑ k∈{i}∪M(i) HT k (3.24)
Optimization-friendly parallel computation of the dynamics with:

IK i = Θ i i k i Θ ⊺ i Γ i (3.25) IL i = Θ i i l i Θ ⊺ i Γ µ(i) (3.26) HT i = IN i -IK i + IL i -T c i -T ext i (3.27)
Then, we can deduce the torques applied to the actuators:

τ i = Θ ⊺ i T i • i Z i (3.28) i Z i = (0, 0, 1) ⊺ (3.29)
A summary graph of the computation is available in Fig. 3.8.

Inverse Dynamics

Differentiation of the equations

Since the resolution of the optimization problem involves a gradient-based solver, it is necessary to compute the gradients (and possibly the Hessian matrix) of the multiple problem variables (positions, velocities, accelerations, torques, etc.) as they intervene in the computation of the cost function and the constraints.

The Jacobian matrix J ∈ R m×n is the matrix that contains the first-order partial derivatives of the problem's constraints g i , where m is the output size of the constraints, and n the input size of the problem (optimization parameters p j ):

J(p) = ∂ g i ∂ p j (p) (3.30)
In our case, since our optimization parameters are B-spline control points that have a local influence, the Jacobian matrix is notably sparse (i.e. it contains a majority of zeros).

The differentiation of the equations is particularly expensive, because it adds n calculations of this type for the gradient (and n 2 for the Hessian), with n the number of control points. The analytical expression of the gradients is a source of complexity. It is thus common to rely on either finite differences (FD) or automatic differentiation (AD). However, FD suffers from a lack of precision, which can be critical to convergence. Additionally, both FD or AD can be quite expensive computationally [ Lengagne 13], even though GPU-based AD is being actively investigated [ Kozikowski 13].

Also, let us consider the time derivatives Ẋ and Ẍ of an expression X(q(p,t)), whose gradients are to be computed w.r.t. the parameters p k of p. Let us suppose we want to have a gradient pipeline relying on gradients w.r.t. q j , that we convert to gradients w.r.t. p k at the very end.

The chain rule gives us:

∂ X ∂ p k (t) = n j -1 ∑ j=0 ∂ X ∂ q j ∂ q j ∂ p k = ∂ X ∂ q ∂ q ∂ p k
By Schwarz's theorem (see Subsec. A.2.1), we have:

∂ Ẋ ∂ p k (t) = ∂ ∂t ∂ X ∂ p k = ∂ X ∂ q ∂ 2 q ∂t∂ p k ∂ Ẍ ∂ p k (t) = ∂ 2 ∂t 2 ∂ X ∂ p k = ∂ X ∂ q ∂ 3 q ∂t 2 ∂ p k
The current CPU library delegates the Hessian computation to Ipopt, which uses a quasi-Newton method to compute the approximation of the second-order partial derivatives 1 . One question remains: should we compute the exact Hessian (more robust code, less iterations) or should we use its approximation (Hessian computation possibly faster)? While the idea of this project is to rethink the computation from a GPU perspective, and though the Hessian computation could also benefit from SIMD architectures, we will not cover the exact computation of the Hessian. This is nonetheless a possibility.

Since the gradient of the end values depends on the gradient of intermediate values, we need to go through the previous algorithm and apply the gradient operator at every step. Let us start with the rotation matrix from link λ (i) to link i, i.e. R i

λ (i) = R i λ (i) (q i (P i ,t))
, where P i is the vector containing the control points of q i :

∂ R i λ (i) ∂ p j =      ∂ R i λ (i) ∂ q i ∂ q i ∂ p j if p j ∈ P i 0 otherwise ∂ S i ∂ p j = 0 (static orientation) (3.31)
Since q i is a K-th order B-spline defined by m = K + 1 control points:

q i (P i ,t) = m ∑ k=1 b k,K (t) p k p k ∈ P i ∂ q i ∂ p j = m ∑ k=1 b k,K (t) ∂ p k ∂ p j =    b j,K (t) if p j ∈ P i 0 otherwise (3.32)
We can deduce: ∂ q i : this only involves the derivatives of cos(q i ) and sin(q i ).

∂ R i λ (i) ∂ p j =      ∂ R i λ (i) ∂ q i b j,K (t)
Next is the gradient of the rotation matrix from the local frame to the global frame, ∂ Θ i ∂ p j . The main difficulty here lies in the kinematic tree structure: the orientation of the link located at a node of the tree depends on all the relative rotations of its predecessors in the tree (cf. eq. (3.5)).

For the sake of conciseness, let us merge the static orientations S i to R(q i ). R j i is the rotation from body i to body j:

∂ Θ i ∂ p j (q) = ∂ ( i ∏ k=0 R λ (k) k ) ∂ p j = i ∑ k=1 (R 0 1 • • • R λ (λ (k)) λ (k) ∂ (R λ (k) k ) ∂ p j R k µ(k) • • • R λ (i) i ) =      R 0 1 • • • R λ (λ (k)) λ (k) ∂ R λ (k) k ∂ q k b j,K (t)R k µ(k) • • • R λ (i) i if p j ∈ P k with k ∈ Λ(i) 0 otherwise
Thus, we need to compute a sum of multiple products of polynomial matrices, which is particularly expensive, especially if long kinematic chains are involved. If we write the recursive analytical expression of the gradient of Θ i :

∂ Θ i ∂ p j (q) = ∂ Θ λ (i) ∂ p j (q) R(q i-1 ) + Θ λ (i) (q) ∂ R λ (i) i ∂ p j (q i-1 ) (3.33)
We can rewrite this expression as follows:

γ i = γ λ (i) α i-1 + β i (3.34) with:              α i = R(q i ) β i = Θ λ (i) (q) ∂ R λ (i) i ∂ p j (q i-1 ) γ i = ∂ Θ i ∂ p j (q)
By unrolling the recursion of eq. (3.34) for a simple chain (Θ ⊺ 0 describes the constant orientation of the root link, and the links are part of the same chain starting from the root), we get:

γ 0 = 0 γ 1 = β 1 γ 2 = β 1 α 1 + β 2 γ 3 = β 1 α 1 α 2 + β 2 α 2 + β 3 γ 4 = β 1 α 1 α 2 α 3 + β 2 α 2 α 3 + β 3 α 3 + β 4 . . .
In the case of a tree, this generalizes into:

∂ Θ i ∂ p j (q) = γ i = len(b i )-2 ∑ j=1 β b i [ j] len(b i )-2 ∏ k= j α b i [k] + β i , ∀i ∈ [1, n] (3.35)
where b i is the kinematic chain from the root to the i-th link, and len(b i ) its length.

Let B = β i and Π α = π α [ j, i] where π α [i, j] = ∏ len(b i )-2 k= j α b i [k]
. Both of these matrices can be computed in parallel: B in O(1), and Π α in O(log 2 n).

For a simple kinematic chain, Π α can be represented as a dense upper-triangular matrix. In the general case, the columns of Π α do not reach the diagonal when branching is involved (cf. Fig. 3.9). Moreover, Π α is to be evaluated and stored only once per time interval, while B is to be computed for each time interval as well as each control point active on the time interval.

According to Schwarz's theorem:

∂ Θi ∂ p j = ∂ ∂ p j ∂ Θ i ∂t = ∂ ∂t ∂ Θ i ∂ p j (3.36)
The gradient of Θi can be obtained by a simple time derivation of ∂ Θ i ∂ p j . The same goes for Θi : Now that the gradients of the orientation matrices are known, we can compute the gradient of the spatial position (and its derivatives), based on its expression in eq. (3.10):

∂ Θi ∂ p j = ∂ ∂ p j ∂ Θi ∂t = ∂ ∂t ∂ Θi ∂ p j (3.
∂ x i ∂ p j = ∑ k∈Λ(i) ∂ lk k ∂ p j (3.38)
As a result, this can precomputed with a parallel prefix sum, after the following gradients have been computed independently in parallel:

∂ lk i ∂ p j = ∂ l i ∂ p j - ∂ k i ∂ p j = ∂ Θ λ (i) ∂ p j (t) i l i - ∂ Θ i ∂ p j (t) i k i (3.39)
We can then deduce

∂ ẋi ∂ p j and ∂ ẍi ∂ p j : ∂ ẋi ∂ p j = ∂ ∂ p j ∂ x i ∂t = ∂ ∂t ∂ x i ∂ p j (3.40) ∂ ẍi ∂ p j = ∂ ∂ p j ∂ ẋi ∂t = ∂ ∂t ∂ ẋi ∂ p j (3.41)
The expression of ∂ f i ∂ p j is thus directly:

∂ f i ∂ p j = m i ∂ ẍi ∂ p j (3.42)
We can now find

∂ Γ i ∂ p j and ∂ T i ∂ p j
, thanks to eq. (3.20) and eq. (3.24):

Γ i = ∑ k∈{i}∪M(i) IF k ∂ Γ i ∂ p j = ∑ k∈{i}∪M(i) ∂ IF k ∂ p j (3.43) T i = ∑ k∈{i}∪M(i) HT k ∂ T i ∂ p j = ∑ k∈{i}∪M(i) ∂ HT k ∂ p j (3.44)
Both depend on a parallel prefix sum and the precomputation of the following gradients:

∂ IF i ∂ p j = ∂ f k ∂ p j - ∂ f k ∂ p j + ∂ F ext k ∂ p j (3.45) ∂ HT i ∂ p j = ∂ IN k ∂ p j - ∂ IK k ∂ p j + ∂ IL k ∂ p j - ∂ T c k ∂ p j - ∂ T ext k ∂ p j (3.46)
with:

∂ IK i ∂ p j = Θ i i k i Θ ⊺ i ∂ Γ i ∂ p j + ∂ Θ i ∂ p j i k i Θ ⊺ i + Θ i i k i ∂ Θ i ∂ p j ⊺ Γ i (3.47) ∂ IL i ∂ p j = Θ i i l i Θ ⊺ i ∂ Γ µ(i) ∂ p j + ∂ Θ i ∂ p j i l i Θ ⊺ i + Θ i i l i ∂ Θ i ∂ p j ⊺ Γ µ(i) (3.48) ∂ IN i ∂ p j = ∂ Θ i ∂ p j i J i i ωi -f i ( i ω i ) + Θ i i J i ∂ i ωi ∂ p j - ∂ f i ∂ p j ( i ω i ) (3.49) ∂ f i ∂ p j =         ∂ i w i2 ∂ p j i w i3 + i w i2 ∂ i w i3 ∂ p j (J i2 -J i3 ) ∂ i w i3 ∂ p j i w i1 + i w i3 ∂ i w i1 ∂ p j (J i3 -J i1 ) ∂ i w i1 ∂ p j i w i2 + i w i1 ∂ i w i2 ∂ p j (J i1 -J i2 )         (3.50) ∂ i ω i ∂ p j = ∂ Θ i ∂ p j ⊺ Θ⊺ i + Θ ⊺ i ∂ Θi ∂ p j ⊺ (3.51)
The last step is then:

∂ τ i ∂ p j = ∂ Θ i ∂ p j ⊺ T i + Θ ⊺ i ∂ T i ∂ p j • i Z i (3.52) i Z i = (0, 0, 1) ⊺ (3.53)

Sparsity of the Jacobian matrix

We have seen in Subsec. 2.1.5 the kind of constraints that we need to handle to generate robot motions. Since we are dealing with independent time intervals and B-spline control points have a local influence on the curve (see Fig. 3.10), the Jacobian matrix is largely sparse. However, one control point may influence multiple contiguous time intervals. The number of time intervals that a control point influences is linked to the degree of the B-spline. Thus, for each control point, we only need to compute its gradient on a limited number of time intervals, as it is going to be null on every other time interval (see Fig. If we assume that the splines have constant properties (e.g. degree) throughout the motion, and if we use uniform B-splines, then the number of control points that influence a given time interval is constant.

Furthermore, the sparsity of the Jacobian matrix may also be due to the constraints themselves. For instance if the root of the robot is the left foot, and we constrain the position of the right foot, only the joints that appear in the kinematic chain linking these two bodies will influence the constraint. Yet, if we only consider the time intervals to filter the control points, several "logical" zeros will be treated as floating-point zeros. While this should not have a major influence on the result, it increases the computation time and memory requirements. An example of this behavior can be seen on Fig. 3.12.

Conclusion

In this chapter, we presented the basis of our GPU-based dynamics simulator whose task is to evaluate the optimization problem's constraints and their gradients. Our objective is to leverage the high dimensionality of the problem as well as its sparsity structure to reduce the computation time required. We have shown how the geometry, kinematics and dynamics equations can be computed in parallel to limit the number of synchronizations steps. Our approach also tackles the analytical computation of the gradients, drastically reducing the computational burden compared to the previous CPU-based version that relied on automatic differentiation. Figure 3.12 Resulting structure of the Jacobian matrix for a given scenario: log of the amplitude on the left, actual sparsity structure of the matrix on the right. For clarity, we only consider a portion of an actual Jacobian matrix. The sparsity can be further refined if properties of the constraints are taken into account. Note that here, the optimization parameters are ordered per joints, that is first the control points of the first joint, then the control points of the second joint, etc.

We now need to see how the algorithms and methods presented here can be implemented efficiently on the GPU, and evaluate how fast these methods may run on the actual hardware.

Chapter 4

Software implementation 4.1 Introduction

GPUs are increasingly used for scientific computing thanks to their number-crunching capabilities, and GPU manufacturers like NVIDIA or AMD know that there is a real market to conquer. Video processing is not the only field where GPUs are extensively exploited: they have been used successfully for fluid dynamics [ Rustico 14], bioinformatics [Schatz 07], machine learning [Abadi 15], or robot motion planning [ Park 14]. As more autonomous systems will embed a GPU, e.g. for video processing, being able to use that extra computing power may prove to be crucial for complex real-world applications.

Development on a GPU differs from the traditional CPU approaches: capabilities, bottlenecks, and intrinsic limits are different. GPU manufacturers are thus trying to provide libraries and frameworks that will ease the development while still providing the best performance possible. For scientists interested in porting their applications to the GPU, two standards are available: NVIDIA's CUDA, or OpenCL, its open alternative. Thus, the first choice that one faces is which standard to use.

In this chapter, we first introduce CUDA, the framework we used for the core computation running on the GPU, and present the nonlinear optimization framework that we rely on to write the motion-planning optimization program. Then, we show the results of our parallel implementation with scenarios involving the HRP-2 humanoid robot.

CUDA

Why CUDA over OpenCL

CUDA, which stands for Compute Unified Device Architecture, is a parallel computing API created by NVIDIA for its GPUs. While it is freely available, the code is proprietary and closed-source.

On the other hand, OpenCL, the Open Computing Language, is an open standard, maintained by the Khronos Group, a consortium of technology companies that also handles other open standards such as OpenGL, COLLADA, or the new Vulkan API. It also targets heterogeneous systems that may contain CPUs, GPUs, or even FPGAs. This makes it more complex than a pure GPGPU API such as CUDA, and the actual implementations are done by each company (e.g. Intel, AMD).

CUDA provides some profiling tools, such as the NVIDIA Visual Profiler (NVVP), that make code optimization and debugging easier. Being closed-source and proprietary implies that users depend entirely on NVIDIA for bug fixes, and when bugs are found, the user might need to wait for several months until the fixed version is actually released. While this may sound like a detail, throughout the work presented in this thesis, we reported 7 different issues, ranging from compiler crashes to wrong implementations of the C++ standard. Twice as many were found but not reported for lack of time to provide simple reproducible codes. NVIDIA developers always replied quickly and fixed the errors, but we had to wait up to 6 months to obtain the actual fixed CUDA libraries. Meanwhile, the user needs to find ways to avoid the issues, which often means reimplementing parts of the code until the problem disappears. Still, open alternatives may not always lead to a better outcome. While a simple bug we reported to GCC developers was fixed in 24 hours, two more complex issues reported to Clang developers are still opened 2 years later, possibly for lack of a simple self-contained reproducible code. Thus, using OpenCL may not have helped for that specific issue.

Yet, from a pure performance perspective, no standard is better than the other. Final performance depends more on the underlying hardware and the end user's implementation than the actual API used. Both APIs also share the same goals. A subset of C++11/14 features is being slowly added (starting with CUDA 7.0 and OpenCL 2.1) to make GPU code easier to develop and closer to what C++ developers would expect. This situation is similar to the one game developers face when they need to choose between Microsoft's DirectX and OpenGL. In the end, it mostly depends on what your collaborators use, what the platforms and libraries you plan on using support, and the kind of GPUs at your disposal. A simple subjective comparison of both frameworks in presented in the choice was made to use CUDA, since all workstations and laptops used NVIDIA GPUs, and some of our collaborators had worked with NVIDIA's framework.

Architecture and vocabulary

Since the global GPU architecture was presented in Chap. 1, we will focus on the specific CUDA architecture and its related terminology. First, we will distinguish the host, i.e. the CPU, from the device, i.e. the GPU.

A GPU is composed of an array of multiprocessors, called streaming multiprocessor (SM). Their number varies depending on the GPU (from 2 to 16 for the GPUs at our disposal). With CUDA, the code is executed in groups of 32 threads called warps. This implies that threads of a same warp either execute the same instruction or wait. This sets the minimum granularity of kernel executions. Then, warps are regrouped into thread blocks. Threads of the same block have access to a common shared memory, and they are all executed on the same multiprocessor. Finally, thread blocks form a grid. An overview of this hierarchy is available in Fig. 4.1.

We call kernel a function running on the GPU. A kernel can only read device memory. When a kernel is executed over a grid, the blocks of the grid are distributed to the available multiprocessors, and multiple blocks can execute concurrently on the same multiprocessor.

We can then make a parallel between the thread hierarchy and the memory hierarchy on the GPU. Indeed, the following memory types are available (see Fig. 4.2):

Global memory (DRAM): this is the "staging area" where data copied from/to the host is stored. When a "2GB GPU" is advertised, this is the memory referred to. Any thread from any block or grid can access it in a kernel, but accesses are notably slow. Recent GPUs provide cached accesses to global memory.

Grid

... Shared memory: faster than global memory, shared memory is tied to a specific block: threads of a block can use it to exchange data with each other. However, the quantity of shared memory per multiprocessor is limited.

Registers: the fastest available memory. Both the total number of registers per multiprocessor, and the maximum number of registers per thread are limited.

Local memory: if too many registers are needed to run a given kernel, global memory may be used in complement to the registers, drastically reducing performance (see Subsubsec. 4.2.5 on register spilling below).

Constant/texture memory: more specific memory is available, for instance for fixed readonly data. Loading such data with a special memory access mode can be more efficient than simply loading it from global memory. For instance, texture memory can benefit from a dedicated cache optimized for data locality.

A comparison of the memories' relative speed is available in Table 4.2.

The properties and respective quantity of available memory depends on the Compute Capability (CC) of the GPU. Finally, threads may share data with each other, but under some conditions:

• All threads may share data thanks to global memory. Although this is the slowest solution, global memory is persistent through the lifetime of the program, and it is available in large quantity (e.g. few GBs). • Threads from the same block can exchange data through shared memory. Even though the quantity of shared memory is limited, and its usage may decrease the number of blocks that can be processed simultaneously on the GPU, it is usually much faster than global memory. However, shared memory is not persistent (it is tied to a specific kernel).

• Threads from the same warp may share memory thanks to a shuffle instruction (see Subsubsec. 4.2.5 on processor instructions below), although this is not as straightforward as typical shared memory.

CUDA and C++

When dealing with complex data structures and processes, plain Fortran/C code may be difficult to maintain and extend for the developers. NVIDIA has been trying to support C++ to lift part of the developers' burden, but only a subset of the C++ features are supported.

First, Runtime Type Information (RTTI), that is the possibility to determine the type of a variable at runtime with mechanisms such as dynamic_cast or type_info, is not supported. Virtual methods are not directly supported either: the virtual function table cannot simply be copied from the host to the device, which forces the developers to design workarounds (e.g. policy-based design with template programming). Another missing feature is exceptions, and asserts were only recently added.

Still, C++11 constructs are being slowly added to CUDA: auto, static_assert, variadic templates, lambda functions, rvalue references, range-based for loops, etc. However when the code developed for this thesis was first compiled with C++11 enabled, we ended up findings errors in the compiler: it broke backward compatibility with C++03, which prevented us from using any of the new features.

Thus, we obtain a support for C++ stripped down from some of its useful and powerful features that define the language. Avoiding these features may be a good thing for performance (e.g. no virtual table), but a bad one for the developer, as it is likely to increase the complexity of the code base. C++ developers should not expect to develop exactly as they would on the CPU, but the newly supported C++11 constructs and higher-level C++ wrappers such as Thrust help alleviate these differences. Note that OpenCL shares the same issues.

However, new features are not always compatible with older hardware. For example, CUDA 6.0 provided a unified memory model to simplify development: one of the key benefit is the automatic copy of nested structured data between CPU and GPU, which is something we use extensively, but one of our GPUs was not supported, so we continued using our ad-hoc solution.

Community

The big data and machine learning communities have been important contributors to the GPGPU computing ecosystem: treating large data sets or training deep neural networks are time consuming tasks at which GPUs excel. Projects such as TensorFlow [Abadi 15], Torch [Collobert 11], Theano [Bastien 12] or Caffe [Jia 14] all try to exploit the computing power of heterogeneous computing platforms (multi-core CPUs and many-core GPUs) to accelerate their computation, and they all support CUDA.

The contributions are not limited to high-level software libraries: Google engineers are working on an open-source GPGPU compiler called gpucc [ Wu 16], an alternative to NVIDIA's CUDA compiler, nvcc. The Google team explained some of the key motivations for such an endeavor: performance tuning, language features, and bug turnaround times. Their objective is to have a compiler based on LLVM, and their early results even show improvements in terms of compilation time and runtime performance for some benchmarks. Their work is being slowly integrated into clang, and may become a solid alternative to NVIDIA's proprietary compiler.

Performance-related considerations

When implementing any code on a parallel architecture, the kind of memory you use, the way you use it, or how you organize your workload may dramatically decrease the performance of your program. Based on official coding guidelines, advices given among the GPGPU community, and personal experience, here are some of the things that need to be considered when preparing that endeavor:

• Avoid register spilling,

• Avoid uncoalesced memory accesses,

• Avoid synchronizations and reductions,

• Avoid branching,

• Avoid shared memory,

• Use adapted processor instructions.

Here, we use the CUDA vocabulary, but these advices may also be valid for different architectures.

Register spilling

Registers represent the fastest memory available on the GPU. When writing a kernel, a typical simple scenario involves the following process:

1. Load data from global memory to thread-local memory, 2. Process data and save the result to thread-local memory, 3. Copy the result back to global memory. That way, we minimize the number of loads from and stores to global memory, since such operations imply an important penalty on the overall performance of the kernel.

Still, each thread only has access to a limited quantity of register memory depending on the Compute Capability of the GPU (cf.

Uncoalesced memory accesses

When multiple threads of a warp access some memory, e.g. data stored in global memory, we want to minimize the number of loads that will be done, since this can slow the whole kernel down. Indeed, data is loaded by lines that are aligned on memory, so the best-case scenario is when we have sequential aligned memory loads (cf. Fig. 4.3).

Address 0 128

Thread index 0 31 The causes of uncoalesced memory access can be:

• memory access is not sequential (cf. Fig. 4.4),

• memory access is sparse,

• memory access is unaligned (cf. Fig. 4.5).

Address 0 128

Thread index 0 31 For caching transactions, two 128-byte L1-cache lines will be requested. For non-caching transactions, the same happens but with 32-byte L2-cache lines, so an extra L2-cache line will be requested.

Between a coalesced and an uncoalesced kernel, performance can vary by a large factor (up to 6 times slower with early Tesla hardware). This depends on the load granularity (cache line size) and the type of cache that will be hit. For instance, Fermi and Kepler architectures support two types of caching schemes when loading from global memory:

• Full caching (L1 and L2): try to hit in L1, then L2, then global memory. The granularity is 128-byte cache lines. This is the default behavior. • L2-only: try to hit L2, then global memory. This time, the granularity is 32-byte cache lines. This can be more efficient when dealing with random access patterns that nearly never hit the L1 cache. In our case, this mode is likely to slow the computation down since our access patterns are for the most part regular.

Shared memory

Similarly to register memory, there exists hardware limits on how much shared memory can be used inside a thread block (cf. Table 4.4). The main limit is on the quantity of shared memory that can be used per multiprocessor, which means that if the quantity of shared memory used scales with the number of blocks involved (which is likely to be the case), then this leads to a maximum number of blocks that can be processed simultaneously by the multiprocessor. Therefore, shared memory usage needs to be carefully analyzed to maximize throughput. Shared memory is divided into memory modules of equal size called "banks" that can be accessed simultaneously. If we have load or store operations on multiple addresses that involve n shared memory banks, then the bandwidth will be n times higher than for a single memory bank. However, if multiple threads request different data located in the same memory bank, the accesses are serialized, leading to a limited bandwidth.

Compute Capability (CC)

1.0 1.1 1.2 1.3 2.x 3.0 3.5 5.0

Processor instructions

Some complex operations can be efficiently handled by the processor if they are part of its instructions set, for instance:

Fused Multiply-Add (FMA) res ← (x × y) + z. This operation is commonly used in the accumulation of products, which is for instance the case when evaluating polynomials or multiplying matrices.

Shuffle

This instruction provides a way to exchange a variable between threads within a warp without requiring shared memory. This can be used in reduction algorithms (e.g. sum of values stored in thread-local memory).

Funnel Shift

Concatenate two 32-bit registers into one 64-bit register, shift that value left or right, then return the most or least significant 32 bits. This can be used for memory copies between misaligned buffers using aligned loads and stores, rotate (circular shift) operations etc. The compiler may be able to detect when one of these instructions can be used and optimize the generated code accordingly (especially for FMA which is easy to detect and improves floating-point precision), but once again, the developer can make use of these instructions explicitly to improve performance drastically.

Loop unrolling

Unrolling loops can have a huge positive impact on performance, as demonstrated in [Volkov 10]. The idea is that we can hide arithmetic and memory latencies, that is the time taken to achieve a given operation, by overlapping multiple operations for each thread. This is referred to as Instruction-Level Parallelism (ILP), while the classical multithread parallelism is called Thread-Level Parallelism (TLP). A common case where this is easily applicable is in loops: instead of having n threads do 1 independent instruction each, we can have for instance n/4 threads do 4 independent instructions each. Thus, an efficient GPU implementation should exploit at best both TLP and ILP.

In our case, such loops can happen when adding or multiplying matrices, polynomials, when looping over bodies, etc. Since we know the lengths of these loops at compile-time, we can explicitly ask the compiler to unroll those loops. Now that we have seen what GPU/CUDA programming entails, we will present the library that we used to write our optimization problem on the CPU.

RobOptim

Since we treat the nonlinear solver as a blackbox that we could implement later on, we wanted to implement the optimization problem in such a way that would make the comparison of different solvers simple and efficient. To that end, we decided to use RobOptim 1 .

This open-source C++ framework was created and developed at CNRS-LAAS and CNRS-AIST JRL, and although it has been mainly used in the context of humanoid robotics, the core library deals with generic optimization problems [ Moulard 13].

RobOptim helps users write optimization problems with cost functions and constraints in a simple way, independently of the solver used. It also provides tools such as finite-difference computation, logging, optimization callbacks, plotting capabilities, etc. The goal is to provide a highly efficient framework (i.e. with minimum overhead) to simplify development without sacrificing performance. RobOptim relies on Eigen2 for linear algebra (matrix computation), with both dense and sparse matrix computation supported.

In this section, we will quickly present the framework as well as the contributions that were made during the scope of this thesis.

Architecture

RobOptim consists of 3 distinctive layers (see Fig. 4 For complex problems, it can be extremely difficult to know which solver is best suited to obtain a solution quickly and reliably, thus being able to easily test and tune multiple solvers can really help developers. Similarly, if one wants to know if one's solver can compete with off-the-shelf solvers for a specific problem, all that needs to be done is create a RobOptim plugin for that solver, and run the same code with the other supported solvers [ Moulard 14].

f (x) = ln(1 + x 2 0 ) -x 1 s.t. (1 + x 2 0 ) 2 + x 2 1 = 4

Solver tuning

Most nonlinear optimization solvers can be tuned to improve their performance when solving specific problems. For example, Ipopt provides tens of parameters that can be modified, and some of them can have a huge impact on the convergence of the solver. Properly tuning such parameters requires a good understanding of the solver, of the problem being solved, and can be a time-consuming endeavour if the resolution of the problems takes minutes or hours. Solvers such as KNITRO provide an integrated tuner: the user supplies a list of parameters and associated values to test, and the solver returns the associated computation times, final cost and constraint violation. However, since KNITRO may also choose several parameters automatically during the initialization, the tuning feature did not provide any particular benefit.

Contributions

During the course of this thesis, we have contributed extensively to the RobOptim framework. Indeed, RobOptim was mostly used for trajectory optimization, and a dedicated toolbox was released for that specific purpose. However, in our case, we are dealing with a peculiar optimization scheme with different types of optimization parameters (e.g. joint control points, contact forces control points, etc.), a large number of parameters (several hundreds) and constraints (several thousands) while trying to satisfy possibly nonsmooth equality and equality constraints. Moreover, we are aiming for interactive scenarios, thus performance is essential. Our problem and long-term goal were different compared to the problems solved by initial RobOptim users, which justified improvements to the framework itself.

Sparse matrix and block support

Recent changes to the Eigen library allow us to pass expressions (e.g. relative to blocks of matrices) to functions without evaluating them, thus reducing the quantity of (costly) temporary evaluations. RobOptim now benefits from these new features for increased performance.

Moreover, when building a sparse matrix, we need to generate a temporary vector of triplets (row, col, value) (elements of the matrix). This operation can be expensive, and the temporary data is not necessary if the pattern of the sparse matrix is already known. Thus, sparse computation in RobOptim's plugins can now operate as follows:

1. Initialization (first iteration): allocate matrices, evaluate matrices once to obtain their immutable sparse patterns. 2. Following iterations: no allocation, update the sparse matrices in-place, then copy the nonzeros to the solver's data. This assumes that the sparse pattern does not change over time, which is an assumption made by the underlying solvers as well.

In order to do this, we now provide helper functions that can either copy or update a block of a sparse matrix. This can also be useful in user libraries: in one of our problems, the update methods turned out to be over 40 times faster than the simple copy, which initially led to a bottleneck of the computation on the CPU. This is especially useful when we assemble our problem's Jacobian matrix.

Scaling

An essential part of the resolution of a constrained optimization problem is its scaling. Indeed, solvers rely on the gradient values at each iteration to know which direction to take. If the gradients vary by a large margin between different constraints (e.g. 10 -3 vs 10 3 ) for a given iterate, the solver may not be able to converge. The idea is thus to scale the constraints and optimization parameters such that gradients are all in the same range, (e.g. ∼1 with a min-max range of 1000).

While solvers such as KNITRO or Ipopt can provide a scaling based on the starting point, providing one's own scaling parameters can have a large impact on performance. RobOptim provides a way to store scaling parameters that can later be used by the nonlinear solver (only supported by the Ipopt plugin for now). Users can try to set the scaling parameters manually, but it is difficult as we are not trying to scale the constraint values, but their gradients. Thus, we wrote a ScalingHelper class that takes a RobOptim problem and can compute scaling parameters with a given method for some optimization parameters. A comparison of convergence rates for different scaling methods will be presented in Chap. 6 (see Sec. 6.4).

Another possibility could be to add a "self-scaling" term to our cost function, that is scaling parameters are part of the optimization parameters, and we try to minimize a well-defined cost. The initial scaling parameters could be set to the ones provided by our current methods. The drawback here is the computational burden: while the problem's Jacobian matrix can be easily extended to account for the extra optimization parameters (new columns would simply contain a diagonal matrix of the constraint evaluations), the gradient of the extra cost may be expensive to evaluate if it relies on individual constraint gradients (thus the gradient of the cost involves second-order partial derivatives).

Callbacks

RobOptim now supports user-defined iteration callbacks, that is callbacks called by the plugin at each new iterate (if the nonlinear solver supports it). Callbacks provide a lot of flexibility for the user. We encountered the following use cases: providing a way for the user to gracefully terminate the optimization (e.g. after pressing Ctrl+C once, or when a specific criterion is satisfied), plotting in real-time the cost function and the constraint violation, keeping track of the best "candidate" according to a user-defined heuristic, logging intermediate data, etc.

Analyzing results

Another practical concern involves the analysis of the optimization result returned by the solver. The ResultAnalyzer class provides checks for the KKT conditions, the linearly independent constraint qualification, and the absence of null gradients for constraints in the active set (see Sec. B.1 in the appendix). This can help users track and solve convergence issues.

Python bindings

For those who want to quickly test a simple problem, Python bindings have also been developed: performance will be slower compared to a pure C++ implementation because of the Python wrapper overhead, but it simplifies development thanks to the high-level scripting capabilities of the Python language. For now, only dense matrices are supported.

Missing features

The main missing feature of RobOptim is proper variable handling. For instance, constraints often only involve a specific set of parameters, and the user should not need to know how the argument vector is organized to be able to easily extract the relevant variables, of fill the associated columns of the Jacobian matrix.

A possible solution involves the work that was done to add manifold support to RobOptim [Brossette 15]. While the core library only considers R n with n the input size of the problem, the new manifold extension allows to write optimization problems over the Cartesian product of elementary manifolds (e.g. R k , SO(3) etc.). Although it lacks in user-friendliness compared to the core library, it still allowed to write a full posture generator relying on PGSolver, a new solver supporting manifolds explicitly, without having to track each manifold component manually from the global argument vector.

Framework overview

Now that we have presented the libraries used for GPU computing and the optimization problem, we can introduce our motion planning framework, which involves the following layers:

1. Low-level polynomial computation (GPU), 2. Kernels for robot computation (GPU), 3. Optimization problem: creation and resolution (CPU), 4. Post-processing tools (CPU). In this section, we will quickly explain what each part achieves and how it connects with the rest of the framework.

Low-level GPU polynomial math library

As we explained in Chap. 3, the underlying computation involves polynomial vectors and matrices. Our CUDA implementation is heavily templated on the multi-dimensional array sizes, since we assume the kinematic tree's size is known at compile time. Indeed, robot models rarely change, and if they do (more specifically if the number of links changes), a simple recompilation of the library is required. By knowing the size of these arrays at compile time, we can let the compiler unroll the loops.

The library contains kernels for vector and polynomial computations, with operations such as addition or multiplication for vector and matrices, or the evaluation polynomial bounds. This was developed solely to satisfy the requirements we have for the implementation of the dynamics algorithms, and optimized for our specific use case.

GPU polynomial dynamics library

Once the polynomial API is available, we can implement the dynamics equations presented in Chap. 3. Since this represents the core of the computation, this is where most of the code optimization and benchmarking takes place. To this end, the NVIDIA Visual Profiler proved to be a valuable tool to pinpoint bottlenecks in the computation. When testing our framework on different platforms involving different generations of GPUs, we noticed that some parts of the code may perform quite differently with different GPU architectures (e.g. Fermi vs Kepler), and micro optimization becomes more complex. One should not expect a given GPU code to be as optimized on any GPU, since it may hit different bottlenecks depending on the GPU architecture's properties.

The dynamic computation involves two pipelines: one for the direct computation (evaluation of the constraints), and one for the gradient computation (evaluation of the gradients of the constraints). The gradient computation mirrors the direct pipeline, and some of the kernels are nearly identical when only additions are involved. The main drawback of this approach is its development complexity: gradients have to be implemented manually as CUDA kernels, which supposes that the developers has some experience with CUDA.

A more automatic approach can now be envisioned, since more high-level optimizationrelated GPGPU libraries are now available, such as Google's TensorFlow used for machine learning [Abadi 15]. These libraries would still need to be adapted (e.g. TensorFlow does not implement the convolution mode required for the fixed-size polynomial matrix computation), but since they feature automatic as-needed gradient computation, this would ease development and testing. On the other hand, it is impossible to know the impact this would have on the overall performance compared to our current implementation. TensorFlow and related libraries are designed and optimized for machine learning applications, and as far as we know not for any real-time applications, while we deal with an uncommonly peculiar problem.

Optimization problem

We use RobOptim to write and solve the optimization problem: we use the core library to implement the constraints, and the joint trajectories are parametrized with RobOptim Trajectory's cubic B-splines. We implemented the functions as sparse functions for improved performance. Thus we obtain a sparse constrained nonlinear optimization problem, which can be solved with the supported sparse solvers: Ipopt, KNITRO and NAG.

Since the constraints relying on GPU data should be evaluated together (they depends on the same raw data provided by the GPU library), we designed a constraint pool to ensure that they are processed together (some solvers, such as CFSQP, can query constraints individually). We also have some cost functions and constraints that can be evaluated directly and efficiently on the CPU: jerk cost (quadratic function), joint limits (quadratic functions), and joint fixed states at a given time (linear functions). This is due to the nature of the B-splines: such trajectories are linear w.r.t. their control points. Solvers such as NAG or KNITRO can also take into account the constraints properties (linear, quadratic, or simply nonlinear).

Configuration

The description of a scenario is provided as a YAML file indicating the robot used, which constraints to consider, which solver parameters to forward to RobOptim, constraint bounds, contact sequences etc. The robot model has to be provided as a URDF file, and robot contact surfaces are given as RSDF files3 .

Our objective is to support the output from our posture generator [ Escande 13], that is given a sequence a robot postures and associated contacts with the environment, we want to build the optimization problem that will generate the associated dynamic motion. While the core implementation of this feature is developed, the (semi-)automatic mapping of contact configurations to time intervals remains to be done.

Analysis and GUI

Once the optimization process converges, we export the resulting joint trajectories and the associated problem configuration. We then rely on ROS scripts that load the results and generate a ROS bag containing the full motion. To ease this process, a PyQt GUI was developed (see Fig. 4.9). The motion is displayed in RViz, the ROS 3D visualization tool. There, physics (i.e. collisions, stability. . . ) is not handled: this only gives a visual confirmation that the generated motion achieves what was expected geometrically. For actual physics simulation, please refer to the work done in Chap. 6 (see Subsec. 6.3.4). Figure 4.9 PyQt GUI used to load and replay motions in RViz.

Implementation choices

The main data involved in the computation is multi-dimensional arrays of floating-point scalars (float and double are supported). The dimensions involved are:

1. control points (for gradient computation), 2. time intervals, 3. 3D matrices or vectors, 4. polynomials (vector of coefficients), 5. robot bodies. For a 3D vector, the size of such array is thus 3 n cp n int n poly n bodies . As for the order of the indices, i.e. the memory layout chosen, this has a direct impact on the performance, since this will influence memory coalescing (see Subsubsec. 4.2.5). As gradients w.r.t. different control points are all independent, and computation over different time intervals are independent as well, we chose the layout presented in Fig. 4.10. Thus, if we consider a multi-dimensional array ar in C, data is accessed by ar

[grad_idx][int_idx][xyz_idx][poly_idx][body_idx],
where grad_idx is the index of the control point related to that gradient evaluation, int_idx the index of the time interval, xyz_idx the index in the 3D vector, poly_idx the polynomial coefficient's index, and body_idx the index of the associated robot body. Our memory access patterns for threads reflect these choices: threads from the same warp will access contiguous memory. We typically use thread indices (elements of a block) to represent body indices, and block indices (elements of a grid) represent time intervals and control points for gradient computation. The only parts of the computation where data needs to be exchanged between threads are reduction steps, and these involve data related to bodies of the same time interval (and same control point in case of gradient evaluations). Thus, shared memory can be used for such reduction steps. These choices are not always optimal: this currently limits us to a rather static run strategy for the GPU, which limits achievable occupancy. We plan on lifting the restrictions as soon as we can use stable high-level tensor libraries that still allow low-level optimization.

Meanwhile, our approach leads to a large number of blocks (intervals, control points per interval) and a limited number of threads per blocks (links). Thus, our software is likely to perform better on GPUs with a large number of SMs and less cores per SM. If we look at the evolution of Tesla GPUs for instance (see Appendix Table D.1), the newest Tesla P100 seems to be a perfect candidate, as it features a larger number of SMs (56, compared to the 14 of our own Tesla C2070) and a limited number of cores per SM (64, compared to the previous Tesla K20's 192).

Constraint formulations

Since we are mostly dealing with nonlinear constraints over time intervals, the way we write our constraints is hampered by the limits of polynomial computation. For example, polynomial square roots or divisions are not straightforward (see Appendix A.4), these operations are expensive and rely on extra approximations, so they should be avoided when possible. In this section, we give a quick overview of some supported constraints and the chosen formulation for the GPU implementation.

Constraints over time intervals

As most of our constraints impact full time intervals, we first focused our efforts on these constraints. We explained earlier that the previous work relied on the bounds of the equivalent B-splines [Lengagne 10a], which led to faster computation but was too conservative, while we evaluate exact polynomial bounds (see Subsec. 2.2.1).

For now, these constraints consider full time intervals, but they could be applied to a given range of their respective time interval. Indeed, the underlying computation remains identical (the same control points are impacted), however the final bound evaluation is done on the provided range rather than the full interval.

Link position constraint A link's spatial trajectory is given by a 3D polynomial vector (x, y, z). Constraining that link to a given reference position

(x ref , y ref , z ref ) simply writes (x -x ref , y -y ref , z -z ref ) = 0 (which is still a polynomial expression).
Link orientation constraint Being able to constrain the orientation of the robot's links is essential for motion planning, for instance if we want to fix a link to a given contact frame. A possible formulation involves evaluating the angular errors of the actual frame (u, v, w) w.r.t. the reference frame (x, y, z) [Miossec 06]:

                       θ 1 (q) = atan u ⊥(xz) .z u ⊥(xz) .x = 0 θ 2 (q) = atan u ⊥(xy) .y u ⊥(xy) .x = 0 θ 3 (q) = atan v ⊥(yz) .z v ⊥(yz) .y = 0 (4.1)
where u ⊥(xz) denotes the projection of u on the plane (xz), and '.' the dot product. When dealing with scalar values, this approach is simple, avoids singularities, and proved to be suitable to the optimization process. However, in our case, (u(t), v(t), w(t)) is a polynomial vector, which implies that we would need to evaluate:

1. the result of a polynomial division, 2. the atan of that polynomial, 3. the maximum over the time interval of the resulting polynomial.

Finally, we would need to evaluate the gradient of that formulation as well. Thus, we instead used a linear approximation around 0 (the optimum):

       θ1 (q) = u ⊥(xz) .z = 0 θ2 (q) = u ⊥(xy) .y = 0 θ3 (q) = v ⊥(yz) .z = 0 (4.2)
Note that this formulation is invalid in degenerate cases: if we start in a configuration with u = y, we get θ1 (q) = 0 even though the constraint is not satisfied. We also investigated an alternative distance on SO(3) [Huynh 09]: we can for instance consider

d(t) = ||I 3 -R 1 R ⊺ 2 || F ,
= ∑ n-1 i=0 ∑ m-1 j=0 a 2 i, j .
The problem is that when the error reaches 0, we end up with gradient discontinuities, or a null gradient if we consider the squared norm. Another possibility would be to constrain 3 virtual vertices on the constrained link. The drawback of that approach is that we obtain linearly dependent gradients. Thus, improvements can still be made for the formulation of this constraint: finding a formulation that satisfies our requirements (polynomial computation of its gradient on the GPU) while providing good gradient properties is not straightforward.

CoM position constraint Since we compute the CoM trajectory over time, we use the same approach as for the link position constraints.

ZMP position constraint

We compute the ZMP (Zero-Moment Point) trajectory ZMP(t) = (ZMP x (t), ZMP y (t)) as follows [Hirukawa 06]: where m is the total mass of the robot, (x G (t), y G (t), z G (t)) the trajectory of its center of mass G, g = (0, 0, -9.81) the gravitational acceleration, z 0 the height of the ground, and L the angular momentum of the robot w.r.t. the center of mass. Then the ZMP constraint over the i-th time interval is: max

ZMP x = x G - z G -z 0 zG -g ẍG -Ly (4.3) ZMP y = y G - z G -z 0 zG -g ÿG + Lx (4.4)

ZMP

t∈T i d j (t) < 0 ∀ j (4.5)
where d j (t) is the distance from the ZMP trajectory to the segment s j (see Fig. 4.11) of the support polygon. That distance is negative if the ZMP remains within its support polygon, and is positive otherwise. For now we only considered and tested the ZMP constraint on flat (xy) planes.

Constraints at a given time

We also want to be able to set discrete constraints, i.e. g(t d ) ≤ 0 or h(t d ) = 0, where t d is a given time instant.

Link position/velocity constraint at a given time These constraints are configured by providing a link id, a reference position/velocity, a time interval, and a relative time t d on that interval. Then, we can evaluate the position/velocity of the link as a scalar 3D vector, and simply use the Euclidean distance (or any other norm on R 3 ).

Results

Scenario 1 (start) Scenario 1 (end) Scenario 2 (start) Scenario 2 (end)

Scenario 3 (start) Scenario 3 (end)

Figure 4.12 Snapshots from whole-body motion planning with the HRP-2 humanoid robot.

We assess our implementation using motion planning with a humanoid robot. In the following examples, our optimization parameters are the control points of the uniform cubic B-splines used to parametrize the joint trajectories.

Tests are conducted on different NVIDIA GPUs whose proprieties are summarized in Table 4.6. We also compare performance of the optimization process when dealing with singleor double-precision floating-point numbers. Finally, we compare our results with a similar optimization problem solved with the previous parallel CPU library.

Scenarios

Our test scenarios are made with the HRP-2 humanoid robot with motions lasting several seconds. We use constraints described in Subsec. 2.1.5 without a cost function to evaluate the performance of our library. We also froze some joints (e.g. hands, neck) since there is no manipulation done. We end up with a total number of 28 joints (29 bodies). The position of the main contact link is given (right foot).

Scenario 1 The robot starts in the half-sitting configuration with its right hand horizontal (must be kept so all the time); then lowers its CoM below a given threshold (while shifting it above the support polygon of its other foot); then gets back to the original configuration. All of it is done under joint position and speed limits.

Scenario 2 Same as previous one, except that instead of constraining the right hand, a constraint to keep its two feet on the ground is used (i.e. additional equality constraints on the second foot for position and orientation). Scenario 3 The robot achieves a kick motion with a minimum velocity of 1m/s, while keeping its CoM above its right foot, keeping its right hand fixed in position and orientation under joint position and speed limits. 4.7 Scenarios with the HRP-2 humanoid model (29 bodies). We use cubic B-splines for the parametrization of joint trajectories. 10 intervals were chosen for the first and second scenarios, and 15 for the third one. n represents the input size of the problem (number of optimization parameters), and m represents the output size (number of constraints).

Model

Scenarios

Timings

The reported timings are the time spent in the optimization process, i.e. we clock the call to RobOptim's solve() method. The initialization time is not included, since the allocations and preprocessing are done once and for all. Thus, timings include GPU computation, memory transfers between host and device, solver data filling and computation done in the NLP solver (Ipopt). Even though computing the inverse dynamics is not mandatory (since there is no contact forces), we still included its computation to give a fair order of the computation time we GeForce GTX Titan Black 0.680 (0.036) 0.766 (0.048) Table 4.8 Timings (total and average per iteration) for the HRP-2 scenarios with both single-and double-precision floats, for each GPU, and without any cost function. This includes geometry, kinematics, dynamics (albeit not required), constraint evaluations and their gradient counterpart, and time spent in the solver. Note that an Ipopt iteration may not always involve a constraint Jacobian query, so for two different scenarios, even if the number of iterations is similar, the actual number of Jacobian evaluations may be different.

expect, even though adding contact forces will increase the number of optimization parameters, resulting in a more expensive gradient computation for the inverse dynamics.

Three different setups are considered:

• Desktop with a Tesla C2070 GPU and an Intel Xeon W5590 CPU @ 3.33GHz,

• Laptop with a GeForce GT 650M GPU and an Intel Core i7-3740QM CPU @ 2.70GHz,

• Desktop with a GeForce GTX Titan Black GPU and an Intel Core i7-4930K CPU @ 3.40GHz.

On the laptop running the optimization with the GeForce GT 650M GPU, computation time (excluding the solver) for a typical iteration of the second scenario is classified as follows:

• Direct computation: 9.27%

• Gradient computation: 75.27% • Filling RobOptim's sparse Jacobian matrix: 14.71% • Others: 0.75%

We reached the point where simply fetching data copied back from the GPU in compact form and filling the global sparse Jacobian with it is no longer negligible w.r.t. the total computation time. If we also take into account the average time spent in the solver at each iteration, we get:

• Time spent in the parallel simulator: 91.69% • Time spent in the solver (Ipopt): 8.31%

Although we can reduce the number of nonzeros in the Jacobian matrix by taking the kinematic tree into account, or optimize the sparse Jacobian filling process, time spent inside the NLP solver is out of our control for we are using off-the-shelf solvers.

Since historically, GPUs have been used for computer graphics were high precision is rarely required, most GPUs will have a much higher throughput in single precision than in double precision. The theoretical ratio for peak performance FP64/FP32 may vary from 1/2 to 1/32 depending on the hardware. This difference is explained by a varying number of cores dedicated to double-precision computation, which is often lower for cards designed for the gaming industry. Thus, we made the choice of supporting both, since a higher precision may lead to a faster convergence as seen with the third scenario in Table 4.8 (NLP solvers deal with double-precision floats), but a lower precision allows for faster computation on the GPU. The performance gap can be seen more clearly in Fig. 4.14, especially for the GeForce GT 650M.

We disabled ECC (error-correcting code) support on the Tesla card, since this feature is not available on the other cards. It is not that relevant in this context: computation lasts a few seconds, and the solver may be able to recover from most single-bit errors should they happen. Still, if we manage to reduce computation time even more and start merging our planning with low-level control, it would be worth reconsidering ECC errors that could potentially lead to critical failures on the robot.

The previous parallel CPU version presented in [ Lengagne 13] supported more features (e.g. contact forces, collision detection), so we are currently unable to make a direct comparison for the resolution of full problems. For similar scenarios however (no constraint on forces or torques, no cost function, no collision detection, same number of intervals), a timing comparison between the CPU and GPU versions is available in Table 4.10. We summarize average timings per iteration to compare the computational load independently from the optimization process itself. For all the CPUs considered, the maximum number of processors available was lower than the number of time intervals. While the main objective of this work was to use the power of GPU computing to accelerate the computation, different choices have also been made when Moreover, running our code on different architectures revealed how difficult choosing the best implementation strategy for a given kernel is. For instance, some accumulations provide acceptable performance on the GT 650M card with atomic operations on global memory, but the Tesla C2070 had a better throughput when relying on temporary shared memory. Also, when venturing off the beaten tracks, it is not uncommon to find errors in the dependencies of one's project. In our case, since our method is not usually ported to GPUs, we encountered several bugs in the CUDA compiler itself, and the long release cycle of proprietary software can then become an extra problem that needs to be dealt with.

Conclusion

Our work lays outside the box of usual GPGPU problems. It paves the way for a full-fledged GPU multi-contact motion planning library dedicated to complex robotic systems. By leveraging the high dimensionality as well as its sparsity structure, and formulating the problem at best of what could be parallelized, we managed to compute in a matter of seconds or less continuous whole-body trajectories of our motion planning on the GPU. There is still room for improvements regarding the computation times. Ideally, the solver would also be part of the GPU and even customized to robotic problems to gain higher factors of performance.

Our follow-up work involves integrating contact forces and their parametrization to the optimization problem and the GPU pipeline. Since the sequence of contacts is found by a contact planner [ Escande 13] or given by the user, the numbers of contacts and related parameters are known a priori. Plus, we design the problem such that any change in the contact configuration happens on interval boundaries, allowing us to integrate them in the current framework. The next chapter will discuss the issue of integrating contact forces in the problem formulation. Chapter 5

Optimization-friendly computation of contact forces

Previous GPU simulations of simple humanoid motion planning did not consider contact forces as they require a peculiar attention and are of prime importance in our group. In this chapter, we will first introduce the previous method used for the computation of contact forces in the context of continuous nonlinear optimization-based motion planning. Then, we will propose a new parametrization that allows us to optimize over both joint trajectories and contact forces. Finally, we will present our current results with examples involving the HRP-2 humanoid robot.

State of the art

Properly handling contact forces is an important part of the optimization process when generating robot motions. Without contacts, a robot cannot generate the displacement required to fulfill its mission (see Fig. 5.1). Contact forces modify the dynamics of the robot and allow it to move while keeping its balance.

We have defined in Sec. 2.1 the formulation of the problem. If we consider a discrete set of forces to describe the contacts (see Fig. 5.2), we get N p contact forces, and we can rewrite eq. (2.1) as in [Wieber 02; Lengagne 13]:

M r (q) M j (q) q + B r (q, q) B j (q, q) We want to avoid any sliding during the contact phase, hence every contact has to fulfill Coulomb's law (see Fig. 5.3). Let us rewrite eq. (2.4) as:

= 0 I τ + N p ∑ i=1 J ⊺ r,i (q) J ⊺ j,i (q) f i ( 5
∀i ∈ [1, N p ], ∀t ∈ [0, T ]    f n i (t) > 0 f t i (t) 2 ≤ µ 2 i f n i (t) 2
(5.2)

In order to ensure the proper balance of the robot, we are looking for contact forces that verify eq. (5.2) and compensate dynamics effects. By extracting the first part of eq. (5.1) (unactuated part), we get:

M r (q)q + B r (q, q) = N p ∑ i=1 J ⊺
r,i (q) f i (5.3) Figure 5.3 Coulomb's law [Wieber 02]: a contact force f can be split into a normal component f n and a tangential component f t (on the left). To ensure a static contact, these contact forces have to remain in a cone whose angle depends on the friction coefficient µ (in the middle). To simplify the problem, it is common to consider a linear (convex polyhedral) approximation [Hauser 14] (on the right).

that we can rewrite: d r (q, q, q) = J ⊺ r (q) f (5.4)

Here, we can compute d r (q, q, q) and J ⊺ r (q), and we need to find f such that contact forces remain within their friction cones. In [Lengagne 10b], two approaches to compute these contact forces are presented:

1. let the solver handle the computation of the contact forces, which are treated as optimization parameters, 2. deduce contact forces from joint trajectories q.

The first method led to an overconstrained problem due to its parametrization, and it was thus decided to use the second approach. The problem is that eq. (5.4) is likely to be underdetermined. Indeed, the matrix J r = [J r,1 , J r,2 , • • • , J r,N p ] is not necessarily square, depending on the number of contact points considered.

Since there is an infinity of forces f satisfying eq. (5.4), the proposed approach relies on the resolution of the following quadratic program (QP):

min. f 1 2 N p ∑ i=1 β i α i f t i 2 + f n i 2 (5.5) s.t. d r (q, q, q) = N p ∑ i=1 J ⊺ r,i (q) f i
where α i is used to regulate the importance of the tangential force with respect to the normal force, and β i is used to balance the repartition of the forces. If ∀i, β i = α i = 1, we end up with the pseudo-inverse problem, and the Euclidean norm of the forces is minimized. To obtain contact forces closer to the normal direction, we can take (for instance) ∀i, α i = 10. Thus, for each time interval, we need to solve this quadratic program. We can make two observations. First, this approach does not guarantee that contact forces remain inside their respective cones of friction. This can only be verified a posteriori, and the optimization process needs to be restarted with a different value for α i in the hope of eventually finding valid contact forces. Second, the computation involves the computation of the inverse of a 6 × 6 polynomial matrix Ω:

f i = W -1 i J r,i (q)Ω -1 d r (q, q, q) (5.6)
where

W i = diag(β i α i , β i α i , β i ).
This part of the computation could prove to be expensive. Indeed, for every contact, multiple forces have to be computed, and for each force, a 6 × 6 polynomial matrix has to be inverted. We can approach the matrix inversion problem with multiple methods, knowing that Ω is a square symmetric real matrix, for instance a Gauss-Jordan inversion, or an adequate decomposition (e.g. LU or LDLT). These methods all require a stable implementation of polynomial divisions (see Sec. A.4 in the Appendix).

Once f is known, we can finally compute the joint torques thanks to eq. (5.1). As for the differentiation of f i , since with this method, f i only depends on the joint control points q, the analytical gradients can be obtained by using the results of the dynamics gradient computation, but its computation is particularly expensive.

Optimization-friendly parametrization

The previous approach has two drawbacks: (i) the cone of friction constraints are not guaranteed to be satisfied. They have to be checked upon termination of the trajectory optimization and in case of failure, a new optimization must be started with new weights. (ii) Nothing prevents the solver from converging to a trajectory q for which no feasible forces exist.

The two above problems are addressed by [Hauser 14] where f (t) is found by solving a linear program which includes a discretized version of eq. (2.2) as constraints, and uses a slack variable to measure the possible infeasibility of a given q(t) to drive the overall optimization to a feasible solution. However, feasible forces are only found at sampling times.

To the best of our knowledge, there is no method which finds forces that satisfy eq. (2.2) continuously over a time interval. The main contribution of this chapter is to propose one. We also discuss the problem of parametrization in the presence of equality constraints, a problem that was empirically observed in part, but not explained, in the 2D case in [Lengagne 10b].

Figure 5.4 Contact parameters Thus, our objective has been to find a contact forces model that can satisfy eq. (5.4) while allowing the optimization solver to find adequate contact forces.

Equality constraints and parametrization

Consider the 2-link arm in 2D shown in Fig. 5.5, and suppose that we want to constrain its end effector to stay on the line x = 1. At any instant, its configuration must satisfy the constraint cos(q 1 ) + cos(q 2 ) = 1. If we chose to parametrize q 1 and q 2 by polynomials of t, it is easy to show that the only solutions to the satisfaction of this constraint over a time interval are q 1 (t) = c and q 2 (t) = cos -1 (1 -cos(c)), for -π/2 ≤ c ≤ π/2, i.e. constant solutions. Clearly, this is not satisfying as, physically, the robot has the freedom to move its end effector. But here despite the constraint being one-dimensional and while the robot has only 2 degrees of freedom, there is no possible movement in the null space of the constraint. This is due to the choice of the parametrization: the chosen parametric functions cannot correctly describe a movement along the constraint. Note that with any other choice of (piecewise) polynomial parametrization (splines, Lagrange polynomials...), the same problem occurs. This problem can be generalized: given a constraint f (x(t), y(t)) = 0 over an interval, with

x ∈ R n , y ∈ R m and f a n-valued function, a poor choice of parametrization will lead to only constant solutions, despite the constraint letting m degrees of freedom to move.

In certain cases, it is possible to solve the constraint explicitly, i.e. write y(t) = φ (x(t)). Then, we only need to parametrize x with any parametrization, and replace y by the composed expression. In our simple example above, we would have q 2 (t) = cos -1 (1 -cos(q 1 )), and the constraint would be satisfied whatever the parametrization of q 1 is, provided we add constraints to enforce that -π/2 ≤ q 1 (t) ≤ π/2 over the interval. In the general case however, such a solution is out of reach.

In the trajectory optimization problem, two main types of equality constraints appear: the FED and the Cartesian positions of given bodies (generally the hands and feet) over time intervals. In the next section, we show how we chose to parametrize the variables to avoid the issue caused by the FED and one position constraint. For the remaining equality constraints, the problem is still open and, for want of a better alternative, we transform a constraint g = 0 into -ε ≤ g ≤ ε. ε should not be too small, to avoid impeding the convergence, but not too big either, to approximate correctly the original equality. A typical value we take on the translation part of a position constraint is 10 -4 m.

FED-compatible force parametrization

For robots with a free-flyer but otherwise fully actuated joints, such as humanoid robots, the configuration space writes Q = SE(3) × Q ′ , where Q ′ is the space of joint configurations, and the FED (see eq. (5.1)) can be further decomposed accordingly as:

M rr M r j M jr M j j qr q j + n r n j = 0 I τ + J ⊺ rr J ⊺ r j J ⊺ jr J ⊺ j j f r f j (5.7)
with r and j denoting the underactuated free-flyer part (r stands for "root") and internal joints part.

We restrict to movements where the robot has always at least one body f fixed and in contact with its environment (in particular we do not consider ballistic phases). We consider this body as the root for kinematics and dynamics models. Since contacting bodies can change over time, we might need to choose different roots, and thus different representations, over the movement duration.

Body r is fixed, so that qr = 0 and eq. (5.7) becomes:

M r j M j j q j + n r n j = 0 I τ + J ⊺ rr J ⊺ r j J ⊺ jr J ⊺ j j f r f j (5.8)
This approach has several advantages: it reduces the number of variables, spares us from working with quantities in SE(3), and solves exactly a position constraint for one body. It was also adopted in [ Lengagne 13]. Note that the forces applied on body r still appear.

τ is uniquely defined by the lower part of eq. (5.8). Therefore we do not need to parametrize it and can perform an immediate variable reduction. On the contrary, q and f are non-trivially related through the upper part of the equation, so that they must be parametrized carefully if we want eq. (5.8) to be verified at all times while allowing non-constant solutions.

The upper part of eq. (5.8) is an expression of the 6 equations of Euler-Newton, up to a multiplication by a nonsingular matrix depending on the choice of frame and point to express the forces and moments [ Wieber 05]. If the moments in the Euler equations are written at a fixed point, J rr is independent of q. Furthermore, it is full row rank provided the contact points of body r are not collinear, which can be assumed for body r to remain fixed. These two properties are pivotal in the following derivations.

Regrouping all the terms on the left, and insisting on the dependencies w.r.t q, the upper part of eq. (5.8) can be written: d r (q, q, q) = J ⊺ rr f r + J ⊺ r j (q) f j (5.9)

We now show that f = f ⊺ r f ⊺ j ⊺ can be written as: f = Y d r (q, q, q) + Z(q) f (5.10) with Y d r (q, q, q) a particular solution to eq. (5.9), Z(q) a basis of the null space of J rr J r j ⊺ , and f a vector in this null space.

Because J ⊺ rr is full rank, its QR decomposition is:

J ⊺ rr Π 1 Π 2 = Q R 1 R 2 (5.11)
with Q a 6 × 6 orthogonal matrix and R 1 6 × 6 full rank and thus invertible upper-triangular matrix. Π 1 Π 2 is a permutation matrix and R 2 has no particular properties.

Denoting f 1 = Π ⊺ 1 f r and f 2 = Π ⊺ 2 f r , eq. (5.9) is rewritten:

Q ⊺ d r (q, q, q) = R 1 f 1 + R 2 f 2 + Q ⊺ J ⊺ r j (q) f j (5.12)
which can be solved for f 1 :

f 1 = R -1 1 Q ⊺ d r (q, q, q) -J ⊺ r j (q) f j -R -1 1 R 2 f 2 (5.13) Then f = Π 1 f 1 + Π 2 f 2 f j = Π 1 R -1 1 Q ⊺ 0 d r (q, q, q) + Π 2 -Π 1 R -1 1 R 2 -Π 1 R -1 1 J ⊺ r j (q) 0 I f 2 f j (5.14)
from which we deduce Y , Z and f .

Note that only d r and the upper-right part of Z depend on the joint trajectories q. Other quantities can be precomputed once and for all, for a given choice and position of a root body r. This is what makes our approach tractable.

Choosing any parametrization of q and f , eq. (5. 14) gives us a parametrization of f satisfying eq. (5.8) automatically. Note that if k is the total number of contact points considered, then f is of size 3k -6. In the following, we use B-splines for q and f .

Application to gradient-based optimization

As this parametrization is used in the context of our gradient-based optimization, we need to consider two things: (i) how to formulate the cone of friction constraints, (ii) how to compute the gradient of this algorithm.

As we support nonlinear constraints, we do not need to rely on a discretization of the cones of friction as seen on Fig. 5.3, and can directly consider eq. (5.2) applied to a time interval T j :

∀T j ⊂ T, ∀i ∈ [1, N p ]      min T j f n i (t) > 0 min T j µ 2 i f n i (t) 2 -f t i (t) 2 ≥ 0
(5.15)

These constraints only require the evaluation of f i (t), and their gradients rely on

∂ f i ∂ p j (t)
and ∂ f i ∂ fi, j (t) as well, as contact forces depend on both joint control points p j , and force control points fi, j . Let us assume that we know J r (q), d r , ∂ J r (q) ∂ p j and ∂ d r ∂ p j (the details of the related mechanics computation are available in Appendix C). From eq. (5.14), we deduce:

∂ f i ∂ p j = Π 1 R -1 1 Q ⊺ 0 ∂ d r ∂ p j (q, q, q) +    0 -Π 1 R -1 1 ∂ J ⊺ r j ∂ p j (q) 0 0    f (5.16) ∂ f i ∂ fi, j = 0 -Π 1 R -1 1 J ⊺ r j (q) 0 0 ∂ f ∂ fi, j
(5.17)

Once these gradients are evaluated, the gradients of both cone of friction constraints and the inverse dynamics pipeline (for the computation of torque trajectories) can be computed.

Examples Description

The following examples were implemented with the RobOptim framework using Python bindings, and the resolution was achieved thanks to its Ipopt plugin. The model used here is the HRP-2 humanoid robot. In order to isolate our approach within the whole optimization problem, we fix the trajectory of the robot and only optimize on the forces parameters. The free-flyer part of the FED and the cone of friction constraints seen in eq. (2.2) need to be satisfied continuously over the course of the motion. We do not consider torque limits and thus We consider two different examples: in the first one (Fig. 5.6), a single contact surface is considered, and the robot simply needs to lower its center of mass while keeping a constant orientation for its right wrist; in the second example (Fig. 5.7), a double-support motion is considered, and the robot also lowers its CoM from its initial half-sitting posture. Here, we use 3k -6 cubic B-splines as a parametrization of f , and their control points will form the optimization parameter vector. The contact points are taken at the corner of the feet. Hence we have k = 4 in the first case and k = 8 in the second.

Results

In both cases, the solver was able to find contact forces control points such that the continuous constraints are satisfied throughout the motion in a few iterations.

In Fig. 5.8 and Fig. 5.9, we can indeed see that the contact forces (expressed in the contact surface frame) are on the right side of the contact surfaces (i.e. ∀t ∈ [0, T ] , f z (t) ≥ 0), while Fig. 5.10 and Fig. 5.11 confirm that the contact forces remain within the cones of friction (i.e. ∀t ∈ T, α(t)/µ < 1 where α is the angle of the force with the normal to the contact and µ is the friction coefficient). The FED is satisfied up to numerical errors i.e. at any instant t, d r -J ⊺ rr J ⊺ r j f has no components greater in absolute value than 10 -13 . Note that time intervals are visible on the figures.

The examples were generated without any regard to contact forces. If we slightly modify these movements to make them impossible from a contact forces perspective, the solver will fail to converge, either after explicitly flagging the problem as infeasible, or after reaching the user-defined iteration limit. This is because the trajectory is imposed. However, because our approach uses explicitly the forces as optimization variables, it makes it possible to write tractable optimization problems with both trajectory and forces as variables. This is mathematically immediate and implementation is underway.

Currently, most of the constraints computation is done in Python. While this made it easy to prototype and validate the ideas presented here, it yields long computation times (especially due to the polynomial matrix manipulation in NumPy): for the double-support example, with N = 11 intervals, 360 variables, 176 constraints, the whole optimization process takes about 15 minutes. However, based on our experience on porting such problems to C++ and in particular of exploiting with GPGPU the important inherent parallelization potential of constraints evaluation in such problems, we anticipate to solve the whole optimization problem with both trajectory and forces in a few seconds. This estimate comes from the current computation times of our GPU-based motion planning library: the joint trajectories used in these examples were generated in less than a second on a laptop.

Conclusion

In this chapter, we presented a parametrization of contact forces designed for the resolution of optimization programs that need to ensure the continuous satisfaction of constraints for robotics problems. The method relies on the QR decomposition of the free-flyer's Jacobian matrix, and allows to choose any parametrization of contact forces that best suit the problem being solved.

In future work, we plan to implement this method in our GPU-based motion planning library to efficiently treat contact forces and torque constraints in addition to the kinematics and simple dynamics constraints that it currently handles. Assuming this, we will investigate next the potential of implementing a whole-body model predictive control for multi-contact motion.

Chapter 6

Application to Model Predictive Control

Introduction

No matter the sophistication of the robot embedded sensors, they will be subject to noise, and may behave differently depending on external conditions. The dynamic and kinematic models that we use are also not perfect (from the sensory measurement viewpoint). Finally, the physical parameters of the environment where the robot evolves may be unknown, uncertain, and the task parameters may vary due to changes of the environment. These discrepancies can be seen as many 'perturbations' to the model we use for the planning. As a result, a robot must be able to absorb perturbations by quickly adapting its motion, and the control must be robust to some extent. This is essential to ensure the motion's integrity and the completion of the assigned tasks. This problem can be difficult to solve because of the high-dimensionality of the dynamic systems considered, and the subsequent reaction-time constraints to recover from wrong or idealistic predictions.

These observations apply to humanoid robotics, and a real-time whole-body multi-contact motion planning method that can solve this problem is often presented as the ultimate goal of humanoid control. However, the main limitation of such approaches is the computation time: most methods would take from minutes to hours to return a valid solution, while real-time constraints force us to compute trajectories in -at most-a few control cycles (one cycle being ∼5 ms). Thus, large approximations (up to reducing the entire robot to a mass model centered at the CoM) are made on the model or the equations, which drastically limits the scope of feasible motions.

In this chapter, we see how our GPU-based approach can be used for such purpose. We present the associated computation framework, as well as early simulation results.

Model Predictive Control

State of the art

Model predictive control (MPC) is a feedback control method that involves the real-time optimization of a dynamic process. This is an iterative finite-horizon optimization approach suitable for systems subject to disturbances (see Fig. 6.1). As the whole-body model we are dealing with is nonlinear, our work only considers nonlinear model predictive control (NMPC).

In practice, a model predictive controller computes trajectories on a finite (or infinite) time Yakub 13]. r is the reference trajectory, ũ represents the future inputs, y the output of the system, ỹ the predicted outputs, and ẽ the predicted error. MPC has been used extensively for chemical engineering [Diehl 02], but we are more interested in its application to robot control. MPC is usually applied to problems with simplified models [Kajita 03; Audren 14; Brasseur 15] as dealing with the full nonlinear robot model can lead to expensive computations, unsuitable for real-time control.

Yet, whole-body model predictive control is actively studied, with the notable example of contact-invariant optimization [Mordatch 12]. This method is able to quickly optimize contact surfaces and motion trajectories together, to generate a wide range of behaviors from high-level input (including multi-robot motions). As the computation runs in near real-time (∼50 ms) on a many-core CPU, it can be used as a model preview controller when coupled with a fast physics engine [ Erez 13]. Still, several questions have been raised regarding this method and its application to robot control. Indeed, the various relaxations lead to an unconstrained Figure 6.2 Influence of model predictive control on the control sequence. Each MPC step computes the control u over an horizon T (e.g. T = 3∆t), but only the beginning of it is actually applied to the system, as successive MPC steps (every ∆t) improve the control sequence based on system feedback. tractable optimization problem. It relies on a simplified physics model (e.g. massless limbs), the feasibility of the contact forces is handled as a penalty term in the cost function and not a "true" constraint, and its reliability for highly dynamic motions is unclear. While promising experiments have been conducted on a HRP-2 robot [Koenemann 15], so far no walking motion has been demonstrated on a robot (feet stayed in contact with the ground).

Concerning our optimization approach, since the previous CPU-based work described in Chap. 1 involved long computation times, motions were generated offline and replayed in an open-loop manner on the robot. Attempts were made to replan motions online [ Lengagne 09]: the replanning of a kicking motion with only the leg joints was considered (12 DoFs on the HOAP-3 robot), leading to a parameter vector x ∈ R 61 . The offline computation took roughly 2 hours. Even though such computation times were greatly reduced later on in that project, the idea was to precompute the full motion, and run another optimization pass online with extra constraints that account for changes in the scenario's configuration, e.g. different positions of the target ball. Yet, even a single iteration of the global optimization process was slow, so a linear approximation of the constraints was computed. For the replanning constraints, a single parameter was adapted, namely the collision location along the x-axis, and only a subset of the optimization parameters were considered. Thus, the linear approximation of the constraints allowed for a replanning process that only took a second. Yet, this approach may be at best tedious to adapt to other scenarios, or even impossible for more complex configurations.

Problem formulation

We will note ∆t the computation time taken for one MPC step (supposed constant). In practice, this should be an upper-bound of the actual computation time. If we consider the i-th MPC step, that problem involves:

• the start time of that step t i ,

• an initial vector of optimization parameters x i ,

• the state of the robot and environment at t i , s(t i ),

• the (future) time of the new control sequence t i + ∆t, and its duration d i . The start of that sequence is constrained based on the robot's expected state (see Subsec. 6.2.3). Thus we will note P(x i , s(t i ), t ≥ t i + ∆t) the i-th MPC step such that the control sequence is computed for t ≥ t i + ∆t, knowing s(t i ) and x i .

Additionally, the planner has some key properties. First, the idea is to limit to a minimum number of NLP iterations the MPC steps (a single solver iteration if possible): based on the results of Chap. 4, we assume that with recent GPUs, computation can be run under 50 ms for similar scenarios, and this computation time is likely to decrease with new and future GPU architectures.

Second, we assume that perturbations are limited. Unless the computation is fast enough to allow several iterations to be run, or if multiple MPC steps are acceptable to account for changes, only limited changes to the tasks can be handled.

Our approach shares the following properties and objectives with the usual NMPC problems in the context of optimal control [Diehl 02]:

• we are dealing with a large, sparse nonlinear problem,

• we rely on a model to predict the future system output (up to a few seconds),

• the solution of the previous optimization problem can be exploited for the new problem, • we target real-time control applications,

• our end goal is to handle a receding prediction horizon of at least a few seconds.

Replanning constraints

Properly handling discontinuities in the control sequence is a common issue for these approaches. Indeed, we want to ensure that joint position commands are at least C 1 , to keep the robot's motion smooth. Let us consider the problem P(x i , s(t i ), t ≥ t i ). Here we only constrain the joints state at t = t i , therefore the commands sent at t i + ∆t are likely to be discontinuous (see Fig. 6.3). In our case, i.e. a robot controlled in position with very high gains, we can make the assumption that the robot will closely follow the given joint position commands. As a result, future joint states are known: they are directly given by our joint splines. Thus, we can constrain the future state of the robot at t i + ∆t, and considering P(x i , s(t i ), t ≥ t i + ∆t) will reduce discontinuities in the commands (see Fig. 6.4). A similar approach is done in [Koenemann 15]. A state update consists in using the current actual state of the system, while state prediction implies relying on the hypothetical future state to help limit future discontinuities in the control sequence. A comparison between control sequences generated with and without state updates is done, and for now they decided to disable state updates as it led to more discontinuities in the control. Thus, they rely on the integration of the dynamics to define the hypothetical current state, and still use a state prediction to further limit discontinuities. Yet, it is unclear why state updates led to worse continuity, and they will investigate the issue in future work.

Algorithm

The planner, whose role is to solve the MPC problem presented in Subsec. 6.2.2, operates as follows:

1. Initialization step: we start by computing the optimal trajectories for the full scenario, i.e. P(x 0 , s(0), t ≥ 0), where x 0 is the vector of initial control points, and s(0) the initial state of the scene (robot and environment). This is what was presented in Chap. 4. 2. Start executing the motion: the generated sequence of joint positions is sent to the controller. 3. While the motion has not finished executing:

• Get the current state of the environment (e.g. position of a target) s(t i ).

• Update the optimization problem: P(x i-1 , s(t i ), t ≥ t i + ∆t), where x i-1 is the previous optimal optimization parameters.

• Solve the problem given a maximum number of iterations. Store the new optimization parameters x i .

• Generate a sequence of joint positions q(t), and send them to the robot controller. The first element in this sequence corresponds to q(t i + ∆t).

The full algorithm is detailed in Alg. 1.

Framework

This section introduces the MPC framework that we developed to test our approach. A summary graph of the components is available in Fig. 6.5.

Figure 6.3 Let us consider P(x i , s(t i ), t ≥ t i ). As we constrain the joint states at t = t i , we may obtain large discontinuities δ q(t i + ∆t) for the new commands sent at t i + ∆t (in red).

Figure 6.4 Let us consider P(x i , s(t i ), t ≥ t i + ∆t). Since we assume that joint trajectories are strictly followed, we can limit the discontinuities of the new commands sent at t i + ∆t (in red) by taking the future state as a constraint of the problem. Update t to the current simulation time 12: end while

Summary of periodic rates and durations

Our framework involves multiple periodic or iterative components, each associated with different rates or computation times. It is therefore essential to clearly distinguish them:

1. Robot control loop, running every 5 ms. 2. Simulator loop, running every 2 ms. 3. NLP iterations, each of them taking δt (e.g. 50 ms). 4. MPC loop, running every ∆t = mδt, where m is the maximum number of NLP iterations per MPC step. 5. Motion time T (a few seconds).

Additionally, we can control the simulation time for testing purpose, as explained below in Subsec. 6.3.4.

OpenRTM

OpenRTM is an implementation of the RT (Robotics Technology) middleware. In that middleware, all the elements are RT components (RTC) that provide ports to communicate with each other thanks to the CORBA communication model.

In our framework, we can distinguish several periodic RT components (see Fig. 6.5):

• Robot Controller: the bridge between the planner and the actual robot (simulated or actual).

• Replanner/Predictive Controller: high-level component that generates the joint trajectories. This component may rely on a posture generator to generate key postures and associated contacts with the environment. • Pause Simulator: component required to control the Choreonoid simulation. For each of them, the onExecute() function needs to be implemented, and it will be called periodically based on the component's rate (e.g. 500 Hz).

Robot controller

This RT component buffers a vector of joint position commands q (control sequence), and at each iteration, sends the current joint position q(t) to the actual robot. If the stabilizer is enabled, it can also provide the waist position/orientation and the ZMP to the robot. Joint accelerations can also be used as a control sequence, and in that case, Euler integrations need to be done. All related computations (e.g. forward kinematics) are done with RBDyn1 .

Choreonoid

Choreonoid is "an integrated robotics GUI environment" developed at AIST2 . We use it as a physics simulator as it can handle HRP-2's ankle flexibilities, which is essential to have a simulation as close to the reality as possible [Vaillant 16]. This simulator was for example used by the AIST-NEDO team during the DRC finals, and was the imposed simulator for the Japan Virtual Robotics Challenge3 (JVRC).

In order to test our ideas, we need to simulate how long each MPC step takes. That way, we can estimate how faster our implementation would need to be to satisfy real-time constraints, or how the framework would behave with a faster GPU or an improved convergence rate. This can help us determine where we should focus our efforts regarding the replanning steps. In order to achieve this, we need to control the simulation time. As this feature was not available in the simulator, we had to add our own RT component for that purpose.

As a result, we can easily simulate different convergence rates (i.e. how many MPC steps are required before a solution to the updated problem is found), or how long these steps take.

Predictive controller (replanner)

This RT component represents the core of this framework: it handles the creation, update and resolution of the optimization problem, as seen in Alg. 1. It is also in charge of sending the control sequence to the robot controller. Finally, it also controls the simulation, i.e. when to pause or resume the simulator, depending on the user-defined rates.

Thus, the component's implementation is split into distinct modes:

idle: the planner is waiting for new instructions (e.g. a new scenario). While in this state, the robot controller keeps on playing the last commands. Once a query is received, the component's mode is set to running.

running: the optimization problem is being solved. During that step, we freeze the simulation time. Once a solution is found, we resume the simulation, and the component enters the waiting mode. waiting: the planner waits for the simulator to reach a given time. This allows us to simulate a specific computation time, in order to evaluate how the system behaves with different computing architectures. Once the given time is reached, the mode is set to commands.

commands: the planner sends the generated control sequence to the controller. Once this is done, the component goes back to running mode if a new MPC step can be computed, or to idle mode if the end of the motion is reached.

Improved initialization and scaling

Starting point with key postures

As we are dealing with joint trajectories parametrized as B-splines, constraints on the robot's joint states at a given time t are linear equality constraints that can be easily satisfied by the optimization solver. These are stored as constant sparse matrices for fast evaluations. Thus, it is possible to quickly optimize the initial joint trajectories if key joint states are known a priori.

To obtain valid trajectories w.r.t. joint limits, we need to enforce joint position and velocity constraints over time intervals as well.

In our framework, this step is done during the initialization. While building the full motion planning problem, we run this optimization after adding the CPU joint constraints, but before adding the constraints evaluated on the GPU.

Let us consider a simple example where we want to minimize the jerk of two splines s i (t) while constraining their state at given times. The initial problem consists in the following constraints for each spline:

• a bound constraint on s i (t) for t ∈ [0.02, 0.10]:

s i ≤ s i (t) ≤ s i ,
• a bound constraint on ṡi (t) for t ∈ [0.62, 0.70]: ṡi ≤ ṡi (t) ≤ ṡi . Then, we proceed as follows:

1. We start from random optimization parameters (see first row of Fig. 6.6), and none of the constraints are satisfied. 2. After a first optimization, we obtain two minimum-jerk splines satisfying the constraints on s(t) and ṡ(t) (see second row of Fig. 6.6). 3. Then, we get their state at t = 0.32, i.e. the values of s i (t), ṡi (t), and si (t), add small perturbations to this state (to simulate external disturbance to the system), and set it as new equality constraints. If we optimize again, starting from the previous optimal trajectories but with these new equality constraints, we get after one iteration a spline satisfying the new state constraint, but not minimizing the cost function, since the local minima changed (see third row of Fig. 6.6). 4. Finally, we let the optimization process finish its course, hence minimizing the cost function of the new problem (see last row of Fig. 6.6). If we only consider these instantaneous joint state constraints, without any cost function, the problem can be solved in a single iteration. Here, the computation is fully done on the CPU as the short computation times do not justify running the calculation on the GPU. If we add a jerk cost function (quadratic), we can increase the convergence tolerance if computation time needs to be limited during this initialization step. In practice, the time taken by this optimization (∼100ms to reach the optimum solution) might be negligible compared to the rest of the computation, and may help reduce the overall computation time of the full problem, as it puts the optimization solver closer to a valid solution, compared to constant or random initial trajectories. In a few specific cases however, for instance when the constant trajectories initially satisfy the fixed-contact constraint throughout the motion, this interpolation should be avoided.

Influence of scaling

Up until now, we focused our efforts on the per-iteration computation, that is the evaluation of the cost function, the constraints, and their respective gradients. Although we rely on external off-the-shelf nonlinear solvers, we can still influence their convergence rates by properly scaling Figure 6.6 On the first row: initial splines and constraints before optimization. On the second row: splines after first optimization, and additional state constraint. On the third row: reoptimization after a single iteration. On the fourth row: full reoptimization. Satisfied constraints are in green, violated ones are in red. our problem. The objective of the scaling process is to have gradient coefficients of the same order of magnitude, so that the optimization process is not only driven by the constraints with the highest gradient values. Note that parts of the optimization may be invariant to scaling (e.g. the Newton steps), but the overall optimization process behaves differently for different scaling factors (initialization, globalization strategy, etc.).

There are 3 things that we can scale:

1. the optimization parameters x = [x 0 , . . . , x n-1 ], 2. the cost function f (x), 3. the constraints g

i (x), i ∈ [0, . . . , m -1].
Let us first consider the scaling of the optimization parameters. If we note s x i = 0 the scaling factor of x i , and xi the associated scaled parameter, we get: (x 0 , . . . , xn-1 ) = (s x 0 x 0 , . . . , s x n-1 x n-1 ) =⇒ (x 0 , . . . , x n-1 ) = x0 s x 0 , . . . , xn-1

s x n-1 (6.1)
where x is the new vector of optimization parameters. We deduce, for any differentiable function h (e.g. f or g i ):

h(x 0 , . . . , x n-1 ) = h x0 s x 0 , . . . , xn-1 s x n-1 = h(x 0 , . . . , xn-1 ) (6.2)
We can then differentiate this equation:

∂ h ∂ xi (x) = ∂ h ∂ x i (x) ∂ x i ∂ xi = 1 s x i ∂ h ∂ x i (x) (6.3)
Scaling the cost function or constraints (e.g. noted u(x)) is directly:

ũ(x) = s u u(x) (6.4) ∂ ũ ∂ x i (x) = s u ∂ u ∂ x i (x) (6.5)
In the previous chapters, we relied on Ipopt's automatic gradient-based scaling [ Wächter 06]. The default algorithm (which provided the best results in practice) works as follows:

s ob j = min 1, g max ||∇ x f (x 0 )|| ∞ (6.6) s g i = min 1, g max ||∇ x g i (x 0 )|| ∞ i ∈ [0, . . . , m -1] (6.7)
where s ob j is the scaling of the objective function and s g i the scaling of the i-th constraint. As a result, all gradients components are at most g max at x 0 (starting point), where g max is set by the user.

To see if we could improve the convergence rate of the optimization with an improved scaling method, we followed the approach presented in [ Miossec 16]. The idea is to find the scaling factors s x i and s g j that minimize the following cost:

C s (s x 0 , . . . , s x n-1 , s g 0 , . . . , s g m-1 ) = 1 n n-1 ∑ k=0 log 1 s x k ∂ f ∂ x k 2 + 1 m 1 n m-1 ∑ j=0 n-1 ∑ k=0 log s g j s x k ∂ g j ∂ x k 2 (6.8)
This minimization problem can be solved analytically, under the constraint of a minimum gradient value (e.g. 10 -9 ), and of a range of acceptable gradient values (e.g. 10 3 ). The details of the resolution can be found in the related paper.

We assume that the cost function is already scaled. We implemented this method in the RobOptim framework, and tested it with our problem solved by Ipopt. It scales the problem based on the starting point. Thus, if one of the constraints has a null gradient at that point, it will not be scaled, and a similar observation can be made for Ipopt's gradient-based scaling.

For simple problems, we obtained convergence rates up to twice as fast compared to Ipopt's default gradient-based scaling method (see Table 6.1). However, for more complex problems, Ipopt's scaling sometimes provided better results. Note that without any scaling, our problem does not converge to a solution in most scenarios. Moreover, for both scaling methods, the scaling parameters are optimized for the starting point, and may not be adequate for the rest of the optimization if the gradients change drastically.

Results

In this section, we will present some of our early results. Note that the robot's stabilizer is disabled, so the robot follows the given joint trajectories closely due to its high gains.

Also, the receding time horizon is not truly supported yet. If T is the total time of the mission, then our horizon's size is d i = T -(t i + ∆t), as the final time of the motion is currently fixed, and we assume we can handle a time horizon of size T . Future work should consider d i = d, where d is a constant time horizon.

Scenario

While the tests made with splines in Sec. 6.4 show that a quick handling of perturbations on joint trajectories is possible, the high-gain position control of our robots does not justify such a feedback loop. On the other hand, the environment may change over time or the robot can drift, hence the constraints describing the robot's tasks may evolve. Our objective is to investigate updates on these tasks, while keeping the same optimization problem. To test our approach, we consider the following scenario, inspired from [ Lengagne 13]: the robot needs to put a ball into a box located under a table. To achieve this motion without falling, the robot needs to take an extra contact with the table (see Fig. 6.7). We also added non-coplanar contacts with the ground.

Thus, we consider the following constraints:

• joint limits (position and velocity),

• fixed contacts (both feet on the ground, and the right wrist on the table),

• position of the left hand at the middle of the motion (above the target box),

• state constraint during updates: we need to enforce the future joint state to limit discontinuities in the control sequence. During the execution of the motion, we modify the position of the target box (from the red to the green box), and expect the robot to correct its left-hand trajectory in time.

Computation times

We investigated different delays in the replanning loop, by simulating multiple computation times to account for different hypothetical computing hardware. Since results from Chap. 4 hint at a good speedup when testing our GPU framework on recent high-end GPUs, we can imagine improved speedups with future GPUs.

Let n be the number of NLP iterations required to converge to an acceptable solution. If each MPC step supports a maximum of m NLP iterations, then at least n/m MPC steps are likely to be required. A better scaling or formulation of the constraints can help decrease n, while faster evaluation of the optimization problem can increase m. The computation time of an MPC step ∆t is given by ∆t ≈ mδt where δt is the average computation time of a single NLP iteration.

We can see in Fig. 6.8 the impact that a faster MPC engine can have, as we compare the constraint violation over MPC steps for different computation times (∆t = 50 and 250 ms). For example, we can start executing the initial full trajectory sooner, as that trajectory can still be slowly improved over time (see decreasing constraint violation from t = 0 to 1 s). When a perturbation is detected (on that figure, at t = 1 s), having a faster replanner means two things.

First, the initial trajectory might be better if limited perturbations were observed. In that example, 2 MPC steps were required for ∆t = 50 ms, while the slower replanner with ∆t = 250 ms required 6 steps. To confirm this observation, we made a similar comparison with identical computation times (∆t = 250 ms) but with different solutions to the initial optimization problem thanks to different tolerances for the acceptable constraint violation. The result can be seen on Fig. 6.9: we observe the same improvement to the MPC convergence as on Fig. 6.8, but here with an identical ∆t.

Second, finding a valid trajectory for that new problem can be done faster. If the perturbations concern an incoming task, a faster replanner is more likely to find adequate joint trajectories in time for the task's successful execution. On Fig. 6.10, we compare the replanning process when changing the feedback times of the task change. For that scenario, with ∆t = 100 ms, we manage to find new trajectories even if the change to the task is reported 500 ms before its execution time. Yet, a minimum time frame for the replanning process is difficult to evaluate beforehand, as it depends on the actual scenario being solved, and the amplitude of the perturbation.

Resulting control sequences

To see the changes made to the joint trajectories, we plotted the generated control sequence for some of the joints, see Fig. 6.11. We compared 3 trajectories:

• the initial trajectory for the non-perturbed problem, • the result of the replanning process, with only joint position constraints at each t i , • the result of the replanning problem, with both joint position and velocity constraints taken into account.

Only considering joint positions for the replanning constraints leads to nonsmooth trajectories (C 0 only), while constraining joint velocities as well leads to C 1 trajectories. As these constraints are evaluated directly with the previous joint splines, and since they are implemented as sparse linear constraints, they have a negligible influence on the global computation time.

Snapshots of the resulting motions for both the initial and perturbed problems are available in Fig. 6.12. The image in the middle of the sequence corresponds to the time of the task's execution (t = 3 s), and we can see that the left hand reaches for the expected box for both motions.

Conclusion and future work

In this chapter, we have presented our early results on the usage of our GPU motion planner for model predictive control. We have shown that the optimization problem can be updated online to account for changes in the environment. While several MPC steps may be required to find a set of new valid trajectories, our early tests show that the new optimization problem may be solved quickly for limited changes to the initial constraints. The task is updated at t = 2.5 s. Only considering joint position constraints (in green) leads to nonsmooth controls during the replanning phase (C 0 trajectories). Taking joint velocity constraints into account helps reduce such discontinuities (C 1 trajectories, in red).

Still, this application of our framework shows that further improvements need to be made regarding the building and update of optimization problems, as these steps also need to be optimized for interactive scenarios. For example, if we detect a change in the box task too late, we may want to delay the execution of the task automatically, which is not supported yet. We also want to validate our approach with an actual receding prediction horizon of a few seconds.

Moreover, our approach does not guarantee convergence as we are still dealing with nonlinear optimization problems describing the whole-body motion generation. Indeed, we are not using a hierarchical optimization solver [Escande 14a], and constraints are all treated together. Ill-conditioned constraints not only have a negative influence on the convergence of the initial resolution, but can also limit the convergence of the replanning process.

During our tests, we used the same nonlinear solver as for the full motion generation, i.e. Ipopt. However, interior-point solvers may not be the best candidates for such replanning applications. Indeed, they require a strictly feasible starting point, which may not be the case for the solution of the previous optimization problem, and may thus lead to a slower early convergence of the solver. Instead, relying on a SQP solver for the replanning steps may help increase the convergence rate when limiting the number of NLP iterations. The HQP solver4 may be a good candidate, as it supports large sparse problems and is open source.

Finally, the energy consumption remains an open question. While whole-body MPC may provide improvements in terms of robot control, its computational burden may be too important for current embedded systems with limited power and energy, and current MPC frameworks with simplified models may be more appropriate.

Conclusion

This thesis proposed a new approach to accelerate the computation of optimization-based whole-body motion planning by leveraging the computing power of GPUs.

We first analyzed the parallelization opportunities of our polynomial-based optimization pipeline. The previous work relied on CPU parallelization to accelerate the computation, but the result was still limited to offline computation as the whole process took from minutes to hours. Indeed, it only considered parallelizing over time intervals. Thus, we took into account the inner loops as well, especially the independent gradient evaluations and model-based parallelism. Moreover, we decided to use a more accurate evaluation method for the bounds of the polynomials, as the previous approach was too conservative.

Then, we analyzed the computation of the inverse dynamics specifically. Indeed, this part represents the core of the computation. As it deals with polynomials and its gradient needs to be computed as well, great care needs to be taken when designing the algorithm and data structures for it to run efficiently on the GPU. We chose to have as many fully independent functions as possible, while synchronization steps are modeled as parallel prefix operations.

In order to validate our ideas, we implemented our highly parallel dynamics and constraints evaluator on the GPU with CUDA. The sparse optimization problem is written with the RobOptim framework. We compared the performance of our library with different GPUs and observed speedups of 1 to 2 orders of magnitude compared to the previous CPU framework.

We also proposed a new parametrization of contact forces suitable for optimization-based whole-body motion planning. Our approach allows to choose any parametrization of contact forces that best suit the problem being solved. For example, we use the same B-spline parametrization for joint trajectories and contact forces, and our method guarantees that the fundamental equation of dynamics is always satisfied.

Finally, we investigated the extension of our work to model predictive control. The fast computation allowed us to integrate the planner to a real-time framework, both as a planner for the full trajectory and also as a replanner that can handle small disturbances to the tasks. The early results obtained recently are promising, but improvements need to be made to our framework to truly investigate real-time scenarios. Besides, the scaling method that we now use can drastically decrease the number of iterations required to find a solution. This shows that enhancements can be made to the optimization problem and on the solver's side, independently from the GPU code.

Aside from performance improvements to the existing implementation, future work may involve the full integration of the contact forces parametrization to the GPU framework, and the implementation of efficient collision avoidance constraints on the GPU. Force, torque and collision constraints are currently missing, and would allow safer motion executions on the robot. A faster planner will also allow us to improve the reactivity of the MPC framework, and possibly to add actual sensors to the feedback loop.

Nonlinear solvers themselves can benefit from GPU computing [GadeNielsen 14]. We could thus consider having our own optimization solver, tailored to our problem, that can directly exploit the Jacobian matrix computed on the GPU. This would remove gradient data transfers between the CPU and GPU, and could possibly lead to faster solver computation for our large-scale problems.

∂ 2 f ∂ x i ∂ x j (a 1 , . . . , a n ) = ∂ 2 f ∂ x j ∂ x i
(a 1 , . . . , a n ) Theorem A.2.2 Gradient of the inverse of a square matrix Let A(x 1 , . . . , x n ) be a square matrix depending on real parameters x i ∈ I i ⊆ R. We suppose that all the components of A are differentiable, and that A(x 1 , . . . , x n ) is invertible for all x i ∈ I i . Thus:

∂ A -1 ∂ x i = -A -1 ∂ A ∂ x i A -1 Proof AA -1 = l 1 n A ∂ A -1 ∂ x i + ∂ A ∂ x i A -1 = 0 n ∂ A -1 ∂ x i = -A -1 ∂ A ∂ x i A -1

A.3 B-splines

A B-spline is a linear combination of basis functions, which can be represented as:

S(t) = m ∑ i=1 b i,K (t)p i (A.1)
A B-spline is defined by m control points and K, the order of the basis functions. It can be computed thanks to Cox-de Boor's recursive formula1 :

∀i ∈ [0, m] b i,0 (t) =    1 if t i ≤ t ≤ t i+1 0 else ∀i ∈ [0, m] b i,K (t) = t -t i t i+K -t i b i,K-1 (t) + t i+K+1 -t t i+K+1 -t i+1 b i+1,K-1 (t) (A.2)
where t i is a knot of the knot vector which respects the following propriety: ∀i, t i ≤ t i+1 . These control points can be of any dimension. In this thesis, we deal with 1D B-splines for joint They are first-order necessary conditions of optimality. Since they can be computed numerically, they were implemented in the RobOptim framework for post-optimization analysis.

B.2 Constraint qualifications

A constraint qualification is a propriety of the feasible set Ω that guarantees the KKT conditions will hold at a given x. Some examples are presented in this section. We added checks for the qualifications that can be evaluated numerically to the RobOptim framework.

LCQ

The linearity constraint qualification (LCQ) holds at x if ∀i ∈ E ∪ (I ∩ A (x)), c i is affine in a neighborhood of x, where A (x) is the finite set of indices of active constraints at x.

LICQ

The linear independent constraint qualification (LICQ) holds at x if the set of active constraint gradients is linearly independent [Nocedal 06] (chap. 12). This implies that these gradients are not null. This can be checked easily by computing the rank of the Jacobian matrix of the active constraints.

CRCQ

The constant rank constraint qualification (CRCQ) holds at x if there exists a neighborhood of x in which the rank of each subset of the gradients of E ∪ (I ∩ A (x)) is constant. with x 0 = (-2, 1), x * = (0.5, 0.25) and f (x * ) = 0.25.

For this problem, with the given starting point, Ipopt converges to a local minimum f (x i ) = 3.98206 with x i = (-0.990968, 0.995474), no matter how we tune its parameters.

The solution returned is located on the boundary of one of the constraints (cf. Fig. B.1), and represents a local minimum in that constrained area. A workaround would be to use a better starting point, which could be done thanks to a multistart implementation: we solve the problem multiple times for different starting points, and we keep the best solution. Still, we have no guarantee on whether we will find the global minimum, and a good strategy needs to be found for the choice of those starting points. One could also argue that finding the actual global minimum is not always necessary: if the cost function represents the average power required to achieve the mission, sparing a few watts at the cost of drastically increased computation time is probably not worth it. We can compute the resultant wrench itself:

-→ A (S /R G ) = m - → a C (C.4) -→ δ C (S /R G ) = d dt - → σ C (S /R G ) (C.5)
where -→ a C is the acceleration of the center of mass, -→ δ C the global torque exerted on the system, and -→ σ C the angular momentum.

We also have the following properties:

- → σ C (S /R G ) = N links -1 ∑ i=0 - → σ C (S i /R G ) (C.6) - → σ C (S i /R G ) = - → σ X i (S i /R G ) + -→ CX i × m i - → v i (S i /R G ) (C.7) - → σ X i (S i /R G ) = T i I i -→ Ω i (C.8)
where S i denotes a part of the system (e.g. i-th link), T i transformation from the i-th link's local frame to the inertial frame, I i moment of inertia tensor of the i-th link, and -→ Ω i the angular velocity of the i-th link. Thus:

- → σ C (S /R G ) = N links -1 ∑ i=0 (T i I i -→ Ω i + -→ CX i × m i - → OX i ) (C.9)
Supposing there are no other external forces acting upon the system, the equality in the first part of the wrenches (forces) leads to:

m( - → a C -- → g ) = N c -1 ∑ λ =0 -→ F c λ (C.10)
Concerning the resultant moment, if we also assume that the external and contact moments are null:

-→ δ C (S /R G ) + --→ X 0 C × m( - → a C -- → g ) = N c -1 ∑ λ =0 --→ X 0 P λ × -→ F c λ (C.11)
If we further note -→ r i, j = --→ X i P j the vector from the center of mass of the i-th contact link to its j-th contact point, we get:

-→ δ C (S /R G ) + --→ X 0 C × m( - → a C -- → g ) = N c 0 -1 ∑ λ =0 -→ r 0,λ × -→ F c λ + K ∑ k=1 N c k -1 ∑ λ =0 ( --→ X 0 X k + -→ r k,λ ) × -→ F c λ (C.12)
where N c k is the number of contact points for the k-th contact link, and K is the total number of contact links. Thus, the first part of the contact Jacobian J rr (part related to the free-flyer) is independent of q, whereas the rest depends on q because of the -→ x k (q) = --→ X 0 X k term. In matrix form, with everything expressed in the inertial frame, this can be written as:

d r (q) = J ⊺ r (q) f (C.13)
= m(a C (q)g) δ C (q) + m c(q)(a C (q)g) (C.14)

δ C (q) = σC (q) (C.15) σ C (q) = N links -1 ∑ i=0 m i ( x i (q)c(q))ẋ i (q) + T i (q)I i Ω i (q) (C.16) J ⊺ r (q) = J ⊺ rr J ⊺ r j (q) (C.17 where c(q) = ----→ X 0 C(q) and f k contains all of the contact forces acting upon link k. Note that this gives us a better expression for J ⊺ r (q) if the trajectories of the links' centers of masses are known. Computing the gradient of J ⊺ r (q) and f is simply:

∂ d r ∂ p j (q, q, q) = ∂ J ⊺ r ∂ p j (q) f + J ⊺ r (q) ∂ f ∂ p j (C.20) =     m ∂ a C ∂ p j (q) ∂ δ C ∂ p j (q) + m c(q) ∂ a C ∂ p j (q) + m ∂ c ∂ p j (q)(a C (q) -g)     (C.21) ∂ δ C ∂ p j (q) = ∂ σC ∂ p j (q) (C.22) ∂ σ C ∂ p j (q) = N links -1 ∑ i=0 m i ∂ x i ∂ p j (q) - ∂ c ∂ p j (q) ẋi (q) (C.23) + m i x i (q) -c(q) ∂ ẋi ∂ p j (q) + ∂ T i ∂ p j (q)I i Ω i (q) + T i (q)I i ∂ Ω i ∂ p j (q) ∂ J ⊺ r ∂ p j (q) = 0 ∂ J ⊺ r j ∂ p j (q) (C.24) =     0 3×3 0 3×3 λ ∈[[0,N c 0 -1]]    0 3×3 ∂ x k ∂ p j (q) + ∂ T k ∂ p j (q)r k,λ    λ ∈[[0,N c k -1]] k∈[[1,K]]     (C.25) ∂ f ∂ p j = 0 ∂ f k∈[[1,K]] ∂ p j ⊺ (C.26)
Moreover, contact forces can be expressed either in the inertial frame or the local frames. If we use the latter, we can express the cone of friction constraints much more easily. Since here the rest is expressed in the inertial frame, we simply need to add a transformation matrix T (from the local link frames to the inertial frame) to eq. (C.13): d r (q) = J ⊺ r (q)T (q) f (C.27) T (q) = diag T 0 T k (q) (C.28) J ⊺ r (q)T (q) = J ⊺ rr T 0 J r j (q) ⊺ diag(T k (q)) (C.29)

where T k is the orientation matrix from the k-th link's frame to the inertial frame, and f is given in the local frames. Hence, the QR decomposition of eq. (5.11) can be done on J ⊺ rr T 0 instead, leading to the same results.

Introduction1

  State of the art 1.1 Motion planning . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.1.1 Path planning . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.1.2 Properties . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.1.3 Potential fields . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.1.4 Sampling-based methods . . . . . . . . . . . . . . . . . . . . . . . . 1.1.5 Optimization-based methods . . . . . . . . . . . . . . . . . . . . . . 1.1.6 Optimization-based whole-body motion planning . . . . . . . . . . . 1.2 Parallel processor architectures . . . . . . . . . . . . . . . . . . . . . . . . .

2. 6

 6 Optimization process . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2.7 Two levels of parallelism available: a high-level parallelism applied to independent time intervals, and a low-level parallelism applied to the underlying model. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.1 Example of GPU-based simulation: 1.1 million moving bodies in a box [Tasora 11]. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.2 Assembly-disassembly phases allow the resolution of the motion equations in a recursive and parallel manner [Bhalerao 12]

  Bayer 09]. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.4 Naive parallel prefix sum [Harris 07]. This method is not work-efficient: it performs O(n log 2 n) additions, while the sequential algorithm only performs O(n) additions. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.5 Work-efficient parallel prefix sum [Harris 07]. This method involves two phases: an up-sweep and a down-sweep phase. It performs O(n) operations (additions, swaps). Additionally, the sum of all the elements is obtained after the first phrase. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.6 Example kinematic chain. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.7 Coordinate system chosen . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.8 Computation pipeline for the dynamics. Triple arrows indicate parallel reductions, and the operator used is indicated in parentheses. Data in circles is either constant or external input. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3.9 Parallel filling of Π α for the simplified HRP-2 model (cf. Fig. 4.13). Each color represents a different computation step in the logarithmic algorithm (including the initialization). 4 recursions are enough to fill the matrix. . . . . . . . . . . 3.10 B-spline control points have a local influence on the curve. . . . . . . . . . . 3.11 When computing the gradients for a time interval T i , one only needs to consider

4. 1

 1 CUDA hierarchical thread organization. . . . . . . . . . . . . . . . . . . . . 4.2 CUDA memory hierarchy. . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.3 Coalesced scenario: sequential aligned memory access. All threads access one cache line. This is the ideal configuration. . . . . . . . . . . . . . . . . . . . 4.4 Uncoalesced scenario: non-sequential aligned memory access. In this case, more recent architectures (e.g. the one used by the Tesla K20) can still combine transactions to alleviate this problem. . . . . . . . . . . . . . . . . . . . . . . 4.5 Uncoalesced scenario: sequential unaligned memory access. For caching transactions, two 128-byte L1-cache lines will be requested. For non-caching transactions, the same happens but with 32-byte L2-cache lines, so an extra L2-cache line will be requested. . . . . . . . . . . . . . . . . . . . . . . . . 4.6 RobOptim architecture. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.7 Hock-Schittkowski problem 7 . . . . . . . . . . . . . . . . . . . . . . . . . 4.8 Evolution of the cost during the optimization process of problem 7 (see Fig. 4.7)

  . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.4 Contact parameters . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.5 A simple robot with two unit-length links, whose end effector must stay on the line x = 1. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.6 Single-support motion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.7 Double-support motion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5.8 Evolution of the contact forces (expressed in the contact surface frame) over time in the single-support case. . . . . . . . . . . . . . . . . . . . . . . . . . 5.9 Evolution of the contact forces (expressed in the contact surface frames) over time in the double-support case. . . . . . . . . . . . . . . . . . . . . . . . . 5.10 Normalized angular distance α(t)/µ of contact forces to the cone of friction

  (bottom row). The 4th image of each row shows the position of the left hand at the time the ball is expected to be dropped in the box. . . . . . . . . . . . . . A.1 B-spline: influence of knot vectors. Note that non-uniform knot vectors have been generated randomly, and are not identical on Fig. A.1c and Fig. A.1d. . . A.2 B-spline: convex hull (red: B-spline, green: control polygon, blue: convex hull) B.1 Results for Hock-Schittkowski problem 16 with Ipopt: the circle represents the starting point, the square the final Ipopt result, the cross shows where the actual global minimum is, and the black line shows the convergence of Ipopt. The grayed-out area shows where the constraints are violated, and the colors represent the values of the cost function f (x). Given the large range of cost values involved, high values of the cost function are printed with the same color.177

  algorithm to use depending on the number of bodies N B and the number of processors N P [Featherstone 99a]. . . . . . . . . 4.1 Subjective comparison of strengths and drawbacks of CUDA and OpenCL. . 4.2 Comparative speed (equivalent number of instructions) of the different types of memory. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.3 Register and local memory properties for each Compute Capability (CC). MP stands for multiprocessor. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.4 Shared memory properties for each Compute Capability (CC). MP stands for multiprocessor. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.5 Solvers currently supported by RobOptim. "Global" indicates whether the solver finds a global or local solution, "constrained" whether it supports constraints, and "sparse" whether sparse matrices are supported. . . . . . . . . . 4.6 NVIDIA GPUs considered for these tests: mobile GPUs, older Tesla cards, and more recent high-end hardware. . . . . . . . . . . . . . . . . . . . . . . . . 4.7 Scenarios with the HRP-2 humanoid model (29 bodies). We use cubic B-splines for the parametrization of joint trajectories. 10 intervals were chosen for the first and second scenarios, and 15 for the third one. n represents the input size of the problem (number of optimization parameters), and m represents the output size (number of constraints). . . . . . . . . . . . . . . . . . . . . . .

Figure 1

 1 Figure 1 HRP-2Kai completing tasks at the DRC finals, in 2015. Credits: DARPA.

Figure 2

 2 Figure 2 Point cloud generated by Carnegie Mellon University's CHIMP robot: millions of points are gathered to generate a 3D representation of its surroundings. Credits: DARPA.

Figure 1 . 2

 12 Figure 1.2 Overview of PRM behaviors. Start and end configurations are in blue, collision-free samples of the roadmap are in red, the initial roadmap is in black, and the collision-free paths (if any) are in green.

Figure 1 . 3

 13 Figure1.3 Overview of RRT behaviors. Start and end configurations are in blue, collision-free samples of the roadmap are in red, the initial tree is in black, and the collision-free paths (if any) are in green. A bidirectional scheme was used (trees grow from both the start and end configurations, and connect when possible).

Figure 1 . 4

 14 Figure 1.4 Example of direct multiple shooting with a Van der Pol oscillator [Vassiliadis 99] and a piecewise-constant control (computation done with CasADi).

  Figure1.7 Moore's Law: number of transistors per (multi-)processor and processor frequencies (clock rates) over the years for a large set of CPUs (logarithmic scale). While the number of transistors has kept on increasing, clock rates have stagnated for the last decade due to heating reasons.

Figure 1 . 9

 19 Figure 1.9 Circuit board containing an Intel Xeon Phi CPU (image credits: Intel Corporation).

Figure 1 .

 1 Figure 1.10 NVIDIA's GeForce GTX Titan Black: 2880 stream processors, 7.1 billion transistors, and a theoretical single-precision throughput of 5.1 TFlops (image credits: bit-tech.net).

Figure 1

 1 Figure 1.11 FPGA architecture [Manohar 09]: logic blocks (LB), connection blocks (CB) and switch blocks (SB).

Figure 1 . 12

 112 Figure1.12 Percentage of Top 50/Top 500 supercomputers relying on GPUs, or more generally accelerators (GPUs, Intel Xeon Phi, etc.). Data can be obtained at www.top500.org.

Figure 2

 2 Figure 2.2 B-splines: error between extrema from the convex hull and actual extrema

  After Write (RAW) : a thread (T2) reads data (c) written by another thread (T1). T1: c ← a + b T2: d ← c + b Write After Read (WAR) : a thread (T1) writes to some memory (c) after another thread (T2) read from it. T1: d ← c + b T2: c ← a + b Write After Write (WAW) : two threads (T1 and T2) write to the same memory (c). T1: c ← a + b T2: c ← d + e

  data size (MB) Transfer time (ms) Time per memory transfer Pageable (Host to Device) Pinned (Host to Device) Pageable (Device to Host) Pinned (Device to Host)

Figure 2 . 5

 25 Figure 2.5 Overview of the global computation involved in the optimization process.

  $ %! %&' %!''( )*+,"-.+/0( 1/2345(( #(6(#"#($(( (5/>/D(A/,*@D.+/01( !/01D5."0D(43.2@.+/01(!" !" !" !" !" !" !" !" !" !" !" '( '( '(

Figure 2

 2 Figure 2.6 Framework organization between CPU and GPU. The computation is split between the CPU (building the problem, optimization solver, matrices assembly) and the GPU (core computation, constraint evaluations, gradient computation). The background patterns indicate different time intervals T k , and the background colors refer to different constraint types.

t

  Figure2.7 Two levels of parallelism available: a high-level parallelism applied to independent time intervals, and a low-level parallelism applied to the underlying model.

Figure 3 . 1

 31 Figure 3.1 Example of GPU-based simulation: 1.1 million moving bodies in a box [Tasora 11].

  Divide-and-Conquer Algorithm (DCA) [Featherstone 99a; Featherstone 99b; Mukherjee 07; Malczyk 12; Bhalerao 12] (see Fig. 3.2), which is a parallel version of the Articulated-Body Algorithm (ABA) [Featherstone 83], • Hybrid Direct-Iterative Algorithm (HDIA) [Anderson 99], • Assembly-Disassembly Algorithm (ADA) [Yamane 06], • For load balancing: Automatic Scheduling [Yamane 07]. N B = 10 N B = 100 N B = 1000 N P

Figure 3 .

 3 Figure 3.3 Real-time GPU simulation of a cloth model consisting of 65536 particles with more than 130 000 distance constraints and a few thousand contact constraints [Bayer 09].

A

  Figure 3.4 Naive parallel prefix sum [Harris 07]. This method is not work-efficient: it performs O(n log 2 n) additions, while the sequential algorithm only performs O(n) additions.

Figure 3 . 7

 37 Figure 3.7 Coordinate system chosen

Figure 3 . 8

 38 Figure 3.8 Computation pipeline for the dynamics. Triple arrows indicate parallel reductions, and the operator used is indicated in parentheses. Data in circles is either constant or external input.

  Figure 3.9 Parallel filling of Π α for the simplified HRP-2 model (cf. Fig. 4.13). Each color represents a different computation step in the logarithmic algorithm (including the initialization). 4 recursions are enough to fill the matrix.

  3.11).

Figure 3 .

 3 Figure 3.10 B-spline control points have a local influence on the curve.

Figure 3 .

 3 Figure 3.11 When computing the gradients for a time interval T i , one only needs to consider neighboring control points that influence the curve on T i .

Figure 4

 4 Figure 4.1 CUDA hierarchical thread organization.

Figure 4

 4 Figure 4.2 CUDA memory hierarchy.

Figure 4 . 3

 43 Figure 4.3 Coalesced scenario: sequential aligned memory access. All threads access one cache line. This is the ideal configuration.

Figure 4 . 4 Figure 4 . 5

 4445 Figure 4.4 Uncoalesced scenario: non-sequential aligned memory access. In this case, more recent architectures (e.g. the one used by the Tesla K20) can still combine transactions to alleviate this problem.

Figure 4 . 7 Figure 4 . 8

 4748 Figure 4.7 Hock-Schittkowski problem 7

Figure 4 .

 4 Figure 4.10 Memory layout used for data structures. Our templated implementation supports both single-and double-precision floating points, as performance of GPU code may greatly vary depending on the floating-point type used.

Figure 4 .

 4 Figure 4.11 Signed ZMP distance to the support polygon. The ZMP remains within the support polygon if this signed distance is negative.

Figure 4 .

 4 Figure 4.13 Simplified kinematic tree associated with the examples (root = right foot): hand and head joints are fixed.

  Figure 4.14 Comparison of average time per iteration (in ms), for different GPUs and different scenarios. Detailed data is available in Table 4.8.

Figure 4 .

 4 Figure 4.16 Comparison of generated joint trajectories with no cost function (left) and a minimum-jerk cost function (right). That cost function aims at smoothing the joint trajectories, and hence the actual robot motions.

Figure 5 . 1 Figure 5 . 2

 5152 Figure 5.1 In order to create a displacement, a robot needs at least one support point and a change in posture [Wieber 00].

Figure 5

 5 Figure 5.5 A simple robot with two unit-length links, whose end effector must stay on the line x = 1.

Figure 5

 5 Figure 5.6 Single-support motion

Figure 5 . 8

 58 Figure 5.8 Evolution of the contact forces (expressed in the contact surface frame) over time in the single-support case.

Figure 5 . 9

 59 Figure 5.9 Evolution of the contact forces (expressed in the contact surface frames) over time in the double-support case.

Figure 5 .

 5 Figure5.10 Normalized angular distance α(t)/µ of contact forces to the cone of friction over time in the single-support case. Constraint limit is 1.

Figure 5 .

 5 Figure 5.11 Normalized angular distance α(t)/µ of contact forces to the cone of friction over time in the double-support case. Constraint limit is 1.

Algorithm 1

 1 MPC algorithm. t ∈ [0, T ] describes the relative motion time, and get_state gets the state of the robot and the environment at a given time. Input: mission description D, robot R, environment E, starting point x0 # Initialization: solve full problem 1: Get initial state: s0 = get_state(E,R,0) 2: Initialize problem: pb = Problem(D,R,x0,s0,0) 3: Compute initial control sequence: c0 = pb.solve() 4: Send initial control sequence c0 to the controller 5: Start chrono: t = 0 # Replanning phase 6: while t < T -∆t do 7: Get current state: s = get_state(E,R,t) 8: Update optimization problem: pb.update(s,t+∆t) 9: Compute new control sequence: c = pb.solve() 10: Send new control sequence c to the controller 11:

Figure 6 . 5

 65 Figure 6.5 Architecture of the MPC framework.

Figure 6 . 7

 67 Figure 6.7 Scenario considered: HRP-2 needs to put a ball (initially in its left hand) into a target box. That box may change during the motion. The robot is standing on non-coplanar surfaces.

  Figure 6.8 Constraint violation during the replanning process, for computation with ( , ) and without ( , ) a change in the task, for different computation times ∆t. A maximum of 5 NLP iterations per MPC steps was used. Each point describes the global constraint violation after an MPC step. Parts with constant violations are due to the fact that the constraint violation tolerance is reached, and no change in the environment was observed.

Figure 6 .

 6 Figure 6.11 Influence on joint state constraints on discontinuities in the control sequence.The task is updated at t = 2.5 s. Only considering joint position constraints (in green) leads to nonsmooth controls during the replanning phase (C 0 trajectories). Taking joint velocity constraints into account helps reduce such discontinuities (C 1 trajectories, in red).

150

  Figure 6.12 Snapshots from the initial trajectory (top row) and the replanned trajectory (bottom row). The 4th image of each row shows the position of the left hand at the time the ball is expected to be dropped in the box.

  Figure A.1 B-spline: influence of knot vectors. Note that non-uniform knot vectors have been generated randomly, and are not identical on Fig. A.1c and Fig. A.1d.

Figure A. 2

 2 Figure A.2 B-spline: convex hull (red: B-spline, green: control polygon, blue: convex hull)

Figure B. 1

 1 Figure B.1 Results for Hock-Schittkowski problem 16 with Ipopt: the circle represents the starting point, the square the final Ipopt result, the cross shows where the actual global minimum is, and the black line shows the convergence of Ipopt. The grayed-out area shows where the constraints are violated, and the colors represent the values of the cost function f (x). Given the large range of cost values involved, high values of the cost function are printed with the same color.

  λ λ ∈[[0,N c 0 -1]] 3×3 x k (q) + T k (q)r k,λ λ ∈[[0,N c k -1]] k∈[[1,K]]   (C.18) f = f 0 f k∈[[1,K]] ⊺ (C.19)

  Graphics Processing Unit (GPU) . . . . . . . . . . . . . . . . . . . . 1.2.4 Field-Programmable Gate Array (FPGA) . . . . . . . . . . . . . . . 1.3 High-performance computing . . . . . . . . . . . . . . . . . . . . . . . . . . 1.3.1 Towards simpler parallelization . . . . . . . . . . . . . . . . . . . . 1.3.2 Application to our context . . . . . . . . . . . . . . . . . . . . . . . 1.4 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

	1	HRP-2Kai completing tasks at the DRC finals, in 2015. Credits: DARPA. . .

1.2.1 Concepts . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.2.2 Multi-core processors . . . . . . . . . . . . . . . . . . . . . . . . . . 1.2.3 2 Problem formulation and multi-level parallelism 2.1 Problem Formulation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2.1.1 Robot definition . . . . . . . . . . . . . . . . . . . . . . . . . . . . 2 Point cloud generated by Carnegie Mellon University's CHIMP robot: millions of points are gathered to generate a 3D representation of its surroundings. Credits: DARPA. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.1 Potential fields can also be used for contact planning. This image and details on the subject can be found in [Escande 08]. . . . . . . . . . . . . . . . . . . 1.2 Overview of PRM behaviors. Start and end configurations are in blue, collisionfree samples of the roadmap are in red, the initial roadmap is in black, and the collision-free paths (if any) are in green. . . . . . . . . . . . . . . . . . . . . 1.3 Overview of RRT behaviors. Start and end configurations are in blue, collisionfree samples of the roadmap are in red, the initial tree is in black, and the collision-free paths (if any) are in green. A bidirectional scheme was used (trees grow from both the start and end configurations, and connect when possible). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.4 Example of direct multiple shooting with a Van der Pol oscillator [Vassiliadis 99

  Here the time grid is set to half of the spline's intervals. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.7 Moore's Law: number of transistors per (multi-)processor and processor frequencies (clock rates) over the years for a large set of CPUs (logarithmic scale). While the number of transistors has kept on increasing, clock rates have stagnated for the last decade due to heating reasons. . . . . . . . . . . . . . . .13 Percentage of energy-efficient Top 50/Top 500 supercomputers relying on GPUs, or more generally accelerators (GPUs, Intel Xeon Phi, etc.). Data can be obtained at www.green500.org. The peak in November 2011 appears to be due to IBM's GPU-powered iDataPlex DX360M3 clusters that entered the market that year. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

1.8 Cost per transistor: cost per 100 million gates w.r.t. the semiconductor manufacturing process node. Note the shift after 28 nm semiconductors. This indicates that the price of CPUs may rise, since fitting more transistors will increase the price. Source: International Business Strategies Inc. . . . . . . . 1.9 Circuit board containing an Intel Xeon Phi CPU (image credits: Intel Corporation). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.10 NVIDIA's GeForce GTX Titan Black: 2880 stream processors, 7.1 billion transistors, and a theoretical single-precision throughput of 5.1 TFlops (image credits: bit-tech.net). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.11 FPGA architecture [Manohar 09]: logic blocks (LB), connection blocks (CB) and switch blocks (SB). . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.12 Percentage of Top 50/Top 500 supercomputers relying on GPUs, or more generally accelerators (GPUs, Intel Xeon Phi, etc.). Data can be obtained at www.top500.org. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12.1 Variable dependency graph.

  Simplified kinematic tree associated with the examples (root = right foot): hand and head joints are fixed. . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4.14 Comparison of average time per iteration (in ms), for different GPUs and different scenarios. Detailed data is available in Table 4.8. . . . . . . . . . . 4.15 Comparison of speedups between the CPU version (with multiple threads used) and the GPU version (with single-and double-precision floats). For each machine, the speedup is computed w.r.t. the single-thread CPU version on that same machine. Data is available in Table 4.10. . . . . . . . . . . . . . . . . . 4.16 Comparison of generated joint trajectories with no cost function (left) and a minimum-jerk cost function (right). That cost function aims at smoothing the joint trajectories, and hence the actual robot motions. . . . . . . . . . . . . . Surface contacts can be modeled as point contacts [Abe 07]. . . . . . . . . . 5.3 Coulomb's law

5.1 In order to create a displacement, a robot needs at least one support point and a change in posture [Wieber 00]. . . . . . . . . . . . . . . . . . . . . . . . . . 5.2

  Cheng 04], such as: Completeness: a planning algorithm is complete if it is guaranteed to find a solution if it exists, or to report failure otherwise (in finite time),

	Discretization completeness: a planning algorithm is discretization complete if as the gran-
	ularity of the discretization becomes finer, the probability to find a solution increases,
	asymptotically approaching 1,
	Resolution completeness: a planning algorithm is resolution complete if given a discretized
	problem, it is guaranteed to find a solution if it exists, or to report failure otherwise (in
	finite time),

Asymptotic completeness: a planning algorithm is asymptotically complete if it reports a solution in finite time if it exists, but may run forever otherwise, Global/local: a planning algorithm is global if it is able to find the global solution/optimum of a problem. If it can get stuck in local solutions/minima, the planner is said to be local, and is likely to return suboptimal paths.

Discretization may lead to constraint violations. These pictures come from

  .5).

		0.15									0.15
									ZMP									ZMP
									CoM									CoM
		0.10									0.10
		0.05									0.05
	x										x
		0.00									0.00
		-0.05									-0.05
		0.20 -0.10	0.15	0.10	0.05	0.00	-0.05	-0.10	-0.15	-0.20	0.20 -0.10	0.15	0.10	0.05	0.00	-0.05	-0.10	-0.15	-0.20
						y									y
		0.15									0.15
									ZMP									ZMP
									CoM									CoM
		0.10							CoP		0.10							CoP
		0.05
											0.05
	x	0.00									x
											0.00
		-0.05
											-0.05
		-0.10
		0.20 -0.15	0.15	0.10	0.05	0.00	-0.05	-0.10	-0.15	-0.20	0.20 -0.10	0.15	0.10	0.05	0.00	-0.05	-0.10	-0.15	-0.20
						y									y
	Figure 1.5

  .6).

							B-spline
							Taylor approxima on
							Taylor approx. bounds
							Time-grid evalua ons
							Time-grid bounds
	t = 0 0	t 1	t 2	t 3	t 4	t 5	t 6	t = T 7
	Figure 1.6 Comparison of min-max bounds of time-grid evaluations and polynomial approxima-
	tions.

  Cost per transistor: cost per 100 million gates w.r.t. the semiconductor manufacturing process node. Note the shift after 28 nm semiconductors. This indicates that the price of CPUs may rise, since fitting more transistors will increase the price. Source: International Business Strategies Inc.

					Cost per transistor
	Cost per 100M gates ($)	2 3 4	4.01	2.82	1.94	1.28	1.42	1.55	1.31
		1
			90	55	45/40	28	20	16/14	10
				Semiconductor manufacturing process (nm)
	Figure 1.8

  Matrix multiplication with less cache misses in the inner loop.

	Cache Size	Context	Latencies
	L1	64 KiB (per core)	Cache hit (via pointer) Cache hit (via complex address)	4 cycles 5 cycles
	L2	256 KiB (per core) Cache hit	12 cycles
			Cache hit, unshared line	42 cycles
	L3	8192 KiB (shared)	Cache hit, shared line w/ another core	65 cycles
			Cache hit, shared line modified in another core	75 cycles
	RAM		Local	42 cycles + 51 ns
	Miscellaneous	Branch misprediction	16-17 cycles
	Table 2.1 List of latencies for the Intel i7-6700 (Skylake), 4.0 GHz (with Turbo Boost), 14 nm,
	16 GB RAM (dual DDR4-2400 CL15). Source: 7-cpu.com.

Table 3 .

 3 1 Theoretical table defining which algorithm to use depending on the number of bodies N B and the number of processors N P[Featherstone 99a].

  if p j ∈ P i

	Now we need to evaluate	∂ R i λ (i)
		0	otherwise

1 See: http://www.coin-or.org/Ipopt/documentation/node31.html

Table 4

 4 Subjective comparison of strengths and drawbacks of CUDA and OpenCL.

	.1. Thus,

  Table 4.3). If the user tries to use more memory than what is available in register memory, a block of global memory called local memory is used to compensate the missing memory, and that local memory leads to slow load and store operations. The compiler is able to optimize register usage, but is limited to what the developer programmed in his kernel, thus great care needs to be put into avoiding unnecessary register usage. Register and local memory properties for each Compute Capability (CC). MP stands for multiprocessor.

	Compute Capability (CC)	1.0 1.1 1.2 1.3 2.x 3.0 3.5 5.0 5.2 6.0
	Number of 32-bit registers per MP	8k	16k 32k	64k
	Maximum number of 32-bit registers per thread	128	63	255
	Amount of local memory per thread	16 kB	512 kB
	Table 4.3

Table 4

 4

	5.2	6.0

.4 Shared memory properties for each Compute Capability (CC). MP stands for multiprocessor.

  .6): 1. Solver layer: the RobOptim plugins build an abstraction layer around existing nonlinear solvers such as CFSQP, Ipopt or KNITRO. 2. Core layer: the computational model with useful tools, operators and decorators that operate on functions. 3. Application layer: high-level applications built with the core layer, such as the trajectory toolbox, the bounding capsule generator, the posture generator (PG), etc. Applications can transparently call any of the supported solvers to solve optimization problems.

		Solver layer
	CFSQP	Ipopt	KNITRO	. . .
		Core layer
	Function	Problem	Logger	. . .
		Application layer
	Trajectory	Capsule	PG	. . .
		Figure 4.6 RobOptim architecture.
		min x

  where R 1 and R 2 are two rotation matrices, and ||.|| F is the Frobenius norm, i.e. if A is a n-by-m matrix, ||A|| F

Table 4 .

 4 6 NVIDIA GPUs considered for these tests: mobile GPUs, older Tesla cards, and more recent high-end hardware.

		Tesla C2070	GeForce GT 650M	GeForce GTX Titan Black
	Microarchitecture	Fermi	Kepler
	Compute Capability (CC)	2.0	3.0	3.5
	Number of Shared Multiprocessors (SMs)	14	2	15
	Number of cores/SM	32		192
	Memory (MiB)	5375	2048	6144
	GPU max clock rate (MHz)	1147	835	980
	32-bit registers per thread	63		255
	Maximum amount of shared memory per MP (KB)		48

  This result brings usFigure 4.15 Comparison of speedups between the CPU version (with multiple threads used) and the GPU version (with single-and double-precision floats). For each machine, the speedup is computed w.r.t. the single-thread CPU version on that same machine. Data is available in Table 4.10. closer to real-time and hence to considering closed-loop whole-body model predictive control.

			Average speedup w.r.t. single-thread CPU version, for different machines
		70
	Average speedup for one iteration	10 20 30 40 50 60
		0	Desktop	Laptop	Server
			CPU (1 thread)	CPU (6 threads)	CPU (12 threads)
			CPU (2 threads)	CPU (8 threads)	GPU (double)
			CPU (4 threads)	CPU (10 threads)	GPU (float)

Table 4 .

 4 10 Average time per iteration in ms for the parallel CPU version (for multiple thread counts) and our GPU version (for single-and double-precision floats), with associated speedup relative to the single-threaded CPU version. The scenario considered has n ≈ 300, with geometry and kinematics constraints (m ≈ 2000), and 15 time intervals. Computation on the CPU is done with double-precision floating numbers. This table gives an idea of the speedup to expect for the core computation on different systems, and we expect an even better speedup once highly parallelizable collision constraints are added.

	Computer	Desktop (Tesla C2070) Avg. (ms/iter.) Speedup Avg. (ms/iter.) Speedup Avg. (ms/iter.) Speedup Laptop (GT 650M) Server (GTX Titan Black)
	CPU (1 thread)	2715	1.0	2178	1.0	2235	1.0
	CPU (2 threads)	1747	1.6	1144	1.9	1255	1.8
	CPU (4 threads)	1011	2.7	712	3.1	697	3.2
	CPU (6 threads)	-	-	654	3.3	521	4.3
	CPU (8 threads)	-	-	602	3.6	515	4.4
	CPU (10 threads)	-	-	-	-	485	4.6
	CPU (12 threads)	-	-	-	-	479	4.7
	GPU (double)	94	28.9	172	12.7	48	46.6
	GPU (single)	54	50.3	96	22.7	36	62.1

  T (called horizon), based on a given model, and only the beginning of these trajectories are applied to the system, as the predictive controller is called every ∆t seconds, and ∆t ≪ T (see Fig.6.2).

	Disturbance		MPC		r(t)
	System	y(t)	Model	ỹ(t) -	+
	ũ(t)				ẽ(t)
			Optimizer
			Cost, constraints
	Figure 6.1 Block diagram of a model predictive controller as presented in [

Table 6 .

 6 ScenarioCost No scaling Ipopt scaling This method 1 Comparison of the number of Ipopt iterations until an acceptable solution is found for different scaling methods, with and without a jerk cost function. -indicates that Ipopt did not converge before 300 iterations, and * that it converged to a point of local infeasibility.

	Lower CoM (1 contact)	Zero Jerk	5 16	5 11	4 9
	Lower CoM (2 contacts)	Zero Jerk	--	19 17	12 16
	Lower CoM (non-coplanar)	Zero Jerk	80 27	64 22	31 20
	Table	Zero Jerk	93* -	84 -	51 50
	Table (non-coplanar)	Zero Jerk	70 -	70 62	28 48
	Kick	Zero Jerk	188* 22	21 34	26 13

A robot in every home, Scientific American, January

2 www.robohow.eu 3 www.projetromeo.com

www.comanoid.eu

Robot Operating System: www.ros.org

Open Motion Planning Library: ompl.kavrakilab.org

See Jeff Preshing's blog post: "Atomic vs. Non-Atomic Operations".

http://roboptim.net

http://eigen.tuxfamily.org

See https://gite.lirmm.fr/idh/rsdf

https://github.com/jorisv/RBDyn

http://choreonoid.org/en

http://www.jvrc.org/en/index.html

http://hqp.sourceforge.net

For more detailed information: http://www.cl.cam.ac.uk/teaching/1999/AGraphHCI/SMAG/ node4.html

Acknowledgements

Synchronizations and reductions

We presented in Subsec. 2.3.4 the notion of synchronization and barriers for multithreaded programs. In our case, this is particularly important for reduction algorithms, such as the prefix sums used in the dynamics (see Subsec. 3.2.1). For such algorithms, a key component is a shared buffer available to all the threads involved. In the context of a CUDA kernel, if we assume that all these threads are part of the same block, shared memory can be used. If shared memory is used, synchronizations can be done over all the threads in a block with the __syncthreads() function. Omitting synchronizations where data hazards are possible leads to undefined behavior, and luckily NVIDIA's cuda-memcheck tools can detect such programming errors.

Branching

Branching typically happens with conditional blocks. Since the smallest unit that a warp scheduler can schedule contains 32 threads, if at least one thread has to follow a branch, then the whole warp will follow it and threads that do not need to execute the branch will be idle. Thus, for if then else blocks (see List. 4.1), if threads of a warp are split between the two branches, both branches will be executed for the warp. As a consequence, in order to increase the performance of the code, intra-wrap branching needs to be avoided. A solver-independent parameter tuner could be implemented as a RobOptim extension: the user provides parameters to optimize with associated sets or ranges of values for a given solver, a RobOptim problem, and a meta cost function involving the computation time, the problem's constraint violation and cost. Then RobOptim would return the optimal set of parameters. However, this should not replace a good understanding of the underlying solver.

Supported solvers

RobOptim currently supports multiple nonlinear solvers that differ on several aspects:

• their type of optimization method (e.g. SQP, interior point), • whether they look for a global or local solution, • whether they solve constrained problems, • whether they implement sparse-matrix support, • their license. 4.5 Solvers currently supported by RobOptim. "Global" indicates whether the solver finds a global or local solution, "constrained" whether it supports constraints, and "sparse" whether sparse matrices are supported.

A list of the supported solvers in available in

In order to see how each solver fared with respect to one another, we made a benchmark comparing them in [Moulard 14]. This simply gives a rough idea of what to expect of different nonlinear solvers with a wide range of simple problems. In order to have a "fair" comparison, we used the default parameters of each solver, however some of these solvers supported intermediate logging, which implies that the computation times reported could probably all be reduced thanks to adequate solver parametrization and equivalent feature support. it came down to the implementation. For a comparison of key features between the CPU and GPU implementations, refer to Table 4 4.9 Comparison between the CPU and GPU implementations for several key features.

As for more complex scenarios involving n ≈ 2000 optimization parameters and m ≈ 30000 constraints, scaling was an issue for the CPU version, since it took several hours to compute. Even though this represents highly challenging problems, we still expect to run the computation with our GPU approach in (at most) a matter of minutes for such extreme scenarios. A lot of effort was also spent over the years to optimize the performance of the CPU library (e.g. avoiding costly allocations), which can still be done for our GPU implementation.

Cost function

While the example highlighted here did not rely on any cost function to ease the comparison with the previous CPU library, our work currently supports the minimum-jerk cost function. A comparison of generated joint trajectories can be seen on Fig. 4.16.

Limitations

The main drawback of our approach (compared to the previous CPU version) lies in the difficulties relative to the low-level GPU implementation of the dynamics pipeline, and the analytical gradient implementation. New constraints can now directly exploit gradients already implemented, but larger extensions of this work will still require some experience with CUDA. International workshops

Academic contributions

We note X the skew-symmetric matrix associated with X. It is defined as:

We have the following property:

This operator can be used to compute the cross product:

These notations are used in rigid body mechanics via tensor calculus.

A.2 Theorems

Theorem A.2.1 Schwarz's theorem -symmetry of second derivatives

If f : R n → R has continuous second partial derivatives at any given point in R n , say, (a 1 , . . . , a n ), then for 1 ≤ i, j ≤ n: trajectories. However in this section, figures involve 2D B-splines as the influence of their control points is easier to visualize.

A.3.1 Knot vector

The knot vector has the power of changing the shape of the B-spline by modifying its basis functions (the basis functions depend on the spacing between the knots). We can distinguish 3 main types of knot vectors:

Non uniform

This is the general case, the only constraint is ∀i, t i ≤ t i+1 .

Uniform

Knots are uniformly spaced: ∀i, t i+1t i = α ∈ R. For instance: [0, 0.25, 0.5, 0.75, 1.0].

Open uniform

The knots are uniformly spaced, except at the ends where k knots are equal.

(A.5) For instance: [0, 0, 0, 0, 1, 2, 3, 4, 4, 4, 4] (for k = 4 and 7 control points).

Let us consider the following uniform knot vector: [1, 2, ..., K + (n + 1)], i.e. ∀i,t i = i. Thus, eq. (A.2) becomes:

For a given order K, basis functions are only translated versions of a same function. As a result, a uniform B-spline cannot be modified via its knot vector (equidistant knots), but we can:

• move the control points: this is the main idea behind the use of B-splines.

• multiply the number of control points: if we put two control points one above the other, the curve will go closer to this point (we increase its weight). • modify the order: if we increase the order of the B-spline K, we increase the continuity of the curve at the knots, the smoothness of the curve, and the curve gets further away from its control polygon. • make both ends meet: we can make a closed loop. If we have M control points, we only need to repeat the first K points.

Note: a uniform B-spline does not reach its extreme control points (start and end of the control polygon).

Open-uniform knot vector If we are not interested in a closed B-spline, and if we want the curve to reach its first and last control points, we need to use open-uniform knot vectors. The only difference with a uniform knot vector is that there are K equal knots at its ends. This type of B-spline should be chosen over the uniform type if we do not need a closed curve. Note: an open uniform B-spline with n + 1 = K -1 control points is a K-th order Bézier curve.

Non-uniform knot vector This type of knot vector gives an additional degree of freedom for the parameterization of B-splines. By changing the spacing between knots, we can modify the shape of the curve, but these modifications appear insignificant compared to the other possible actions. The other property of non-uniform B-spline is about continuity (see A.3.2): by multiplying knots, one can adjust the continuity of the curve at the knots.

A.3.2 Properties

The choice of B-splines is not arbitrary: these functions have interesting properties that we will present, which explains the general enthusiasm towards this class of function [Vaz 03; Lengagne 10a; Lee 05].

Local influence of control points Moving a control point only changes the local behavior of the curve, and not its global behavior as for Bézier curves.

Convex hull A B-spline is entirely contained in the convex hull of its control points. Indeed, basis functions are defined by:

This provides a very fast way to compute an upper and a lower bound of a B-spline. If we model the joint parameters as B-splines, we can for instance test for the violation of joint constraints efficiently (but inaccurately).

Moreover, if we take into account the locality property, we obtain the following feature: for any section of a B-spline, that section is only influenced by a subset of the control points, and that section of the spline is entirely contained inside the convex hull of this subset. This implies that we can also evaluate the bounds of only parts of the splines.

Affine invariance Applying a translation, rotation or scaling transformation on the control points has the same effect as applying these transformations on the computed B-spline curve.

Matrix representation Since we will be using polynomials with their matrix representations, we will require a similar matrix representation for B-splines. We will write:

where B contains the polynomial parameters of the basis functions, and the p i are the control points of the B-spline. This matrix is computed only once during the initialization phase.

"Conversion" of a polynomial to a B-spline It is also possible to "convert" a polynomial to a B-spline, in order to quickly get an estimate of its bounds. Let P(t) be a polynomial defined as:

We are looking for the control points p 0 , ..., p N such that:

Which gives us:

B being invertible (the b i,K (t) functions form a basis of a vector space), we have:

Derivative of a B-spline The derivative of a K-th order B-spline defined by m control points is a B-spline of K -1 order defined by m -1 control points. Indeed, by deriving the recursive Cox-de Boor formula from eq. (A.2) with respect to time, we get: 14) with:

where t i always represents the i-th coordinate of the knot vector. Ṡ(t) is an actual B-spline, and we can also evaluate its bounds as previously by evaluating its convex hull (A.3.2):

Ṡ ≤ r i ≤ S (A.16)

A.4 Polynomial division

Let P 1 and P 2 be scalar polynomials, with P 2 = 0. Dividing P 1 by P 2 consists in finding polynomials Q and R such that:

The rest R will be a measure of the error of the division process. Since we deal with fixed-size polynomials, the error can be large enough to negatively influence the rest of the computation.

If we consider the Taylor approximation at 0:

Thus, for a fixed-size polynomial of degree n:

As before, the approximation is only valid on a short interval centered on 0, and the difference with the actual result may be important for some pairs of polynomials if the interval is not short enough.

Appendix B Optimization

Let us consider the following optimization problem [Nocedal 06,Chapter 12]:

where x is a vector of optimization parameters, f (cost function) and the functions c i (constraints) are smooth real-valued functions on a subset of R n . E denotes the finite set of equality constraint indices, and I the finite set of inequality constraint indices. We define the feasible set Ω as the set of optimization parameters x that satisfy the constraints.

B.1 Optimality conditions

The Lagrangian of the general optimization problem is defined as:

Given a local solution x * to the optimization problem of eq. (B.1), if we suppose that the functions f and c i are continuously differentiable, and that x * satisfies some regularity conditions, then there exists a Lagrange multiplier vector λ * = [λ * i ] i∈E ∪I such that the Karush-

MFCQ

The Mangasarian-Fromovitz qualification (MFCQ) holds at x if:

Usually, the weaker the qualification gets, the harder it is to verify it, and it is satisfied more often. In our case, LICQ is the only qualification that we can compute numerically, without requiring any specific knowledge of the underlying problem.

B.3 A word on local minima

When dealing with nonconvex optimization problems, finding the global minimum of a cost function can be a difficult task for the optimization solver. Most solvers implement local optimization methods that only guarantee convergence towards a local minimum. Let us consider the 16th problem from the Hock-Schittkowski test suite [Hock 80]:

Rigid body dynamics for contact forces

If we call R g the inertial frame, C the center of mass of the robot, P i the contact points, and consider S = {Robot} as our system, the external forces acting upon the robot are:

• Contact forces:

• Weight:

Thus, if we choose to make the computation in the free-flyer frame R 0 (e.g. main contact link), transforming moments to the center of mass of the free-flyer X 0 , we get: