A Differentiable Augmented Lagrangian Method for Bilevel Nonlinear Optimization

Robotics: Science and Systems 2019 Freiburg im Breisgau, June 22-26, 2019 A Differentiable Augmented Lagrangian Method for Bilevel Nonlinear Optimization

Benoit Landry, Zachary Manchester and Marco Pavone Department of Aeronautics and Astronautics Stanford University Stanford, CA, 94305 Email: {blandry,zacmanchester,pavone}@stanford.edu

Abstract—Many problems in modern robotics can be addressed by modeling them as bilevel optimization problems. In this work, we leverage augmented Lagrangian methods and recent advances in automatic differentiation to develop a general- purpose nonlinear optimization solver that is well suited to bilevel optimization. We then demonstrate the validity and scalability of our algorithm with two representative robotic problems, namely robust control and parameter estimation for a system involving contact. We stress the general nature of the algorithm and its potential relevance to many other problems in robotics.

I.INTRODUCTION Bilevel optimization is a general class of optimization problems where a lower level optimization is embedded within an upper level optimization. These problems provide a useful framework for solving problems in modern robotics that have a naturally nested structure such as some problems in Figure 1: For our parameter estimation example, a robotic arm motion planning [10, 31], estimation [13] and learning [1, 17]. pushes a box (simulated with five contact points - the yellow However, despite their expressiveness, bilevel optimization spheres - and Coulomb friction) and then estimates the coeffi- problems remain difficult to solve in practice, which currently cient of friction between the box and the ground. This is done limits their impact on robotics applications. by solving a bilevel program using a combination of SNOPT To overcome these difficulties, this work leverages recent and our differentiable solver. Unlike alternative formulations, advances in the field of automatic differentiation to develop the bilevel formulation of this estimation problem scales well a nonlinear optimization algorithm based on augmented La- with the number of samples used because it can easily leverage grangian methods that is well suited to automatic differen- parallelization of the lower problems. tiation. The ability to differentiate our solver allows us to combine it with a second, state-of-the-art nonlinear program solver, such as SNOPT [19] or Ipopt [33] to provide a solution of relying on the KKT conditions (like having to solve the method to bilevel optimization problems. lower problem to optimality for example). We also leverage Similar solution methods can be found in various forms recent advances in automatic differentiation by implement- across many disciplines. However, our approach differenti- ing our solver in the Julia programming language, which ates itself from previous methods in a few respects. First, is particularly well suited for this task. When applying the we keep the differentiable solver general in that it is a method to robotics, unlike a lot of previous work that combines nonlinear optimization algorithm capable of handling both differentiable solvers with stochastic gradient descent in a ma- equality and inequality constraints without relying on projec- chine learning context, we demonstrate how to incorporate our tion steps. Our method also does not depend on using the solver with another state-of-the-art solver that uses sequential Karush–Kuhn–Tucker (KKT) conditions in order to retrieve quadratic programming (SQP) to address problems relevant to the relevant gradients, as described in various work going as robotics. Among other things, this allows us to use our solver far back as [22], and more recently in the context of machine to handle constraints in bilevel optimization problems, not just learning [1]. Instead, closer to what was done for a specialized unconstrained objective functions [13, 14]. physics engine in [14], we carefully implement our solver Statement of Contributions: Our contributions are two fold. without any branching or non-differentiable operations and First, by combining techniques from the augmented La- auto-differentiate through it. We therefore pay a higher price grangian literature and leveraging recent advances in automatic in computational complexity but avoid some of the limitations differentiation, we develop a nonlinear optimization algorithm

1 that is entirely differentiable. Second, we demonstrate the C. Robotics Applications of Bilevel Optimization validity and scalability of the approach on two important problems in modern robotics: robust control and parameter Several groups have explored problems in robotics using estimation for systems with complex dynamics. All of our bilevel programming solution methods. For example [10] code is available at https://github.com/blandry/Bilevel.jl. explores a trajectory optimization problem for a hopping robot. In their work the lower problem consists of rigid body II.RELATED WORK dynamics with contact constraints that uses a special purpose solver (that relies on a projection step) and the upper problem A. Bilevel Optimization is the trajectory optimization which is solved by dynamic Bilevel optimization problems are mathematical programs programming. This work is related to our example described in that contain the solution of other mathematical programs in Section VI, where the upper problem in the bilevel program is their constraints or objectives. In this work we refer to the also a trajectory optimization problem. Trajectory generation primary optimization problem as the upper problem, and the is also addressed in [31], which explores the use of bilevel embedded optimization problem as the lower problem. A optimization in the context of integrated task and motion more detailed definition of the canonical bilevel optimization planning (with a mixed-integer upper problem). problem can be found in Section III. In the machine learning community, there have been at- There have been many proposed solutions to general bilevel tempts at including a differentiable solver inside the learning optimization problems. When the lower problem is convex process (e.g. in an unconstrained minimization method such and regular (with finite derivative), one approach is to di- as stochastic gradient descent). Similar to our parameter rectly include the KKT conditions of the lower problem estimation example described in Section VI-B, the work in the constraints of the upper problem. However except in [13] leverages the approach in [1] for a learning task for special cases, the complementarity aspect of the KKT that includes a linear complementarity optimization problem conditions can yield combinatorial optimization problems that (arising from dynamics with hard contact constraints) as a require enumeration-based solution methods such as branch- lower problem. This work then uses gradient descent to solve and-bound, which can be nontrivial to solve [3, 18, 9, 11, 32]. upper problems related to parameter estimation. Our approach Other proposed solutions to bilevel optimization problems use to the parameter estimation problem differentiates itself from descent methods. These methods express the lower problem [13] by using a different solution method for both the lower solution as a function of the upper problem variables and try to and upper problems. Specifically, we solve the lower problem find a descent direction that keeps the bilevel program feasible. using our proposed augmented Lagrangian method and solve These descent directions can be estimated in various ways, the upper one using a sequential quadratic program (SQP) such as by solving an additional optimization problem [20, 28]. solver. This approach allows us to handle constraints on the Descent methods contain some important similarities to our estimated parameters, which is not possible using gradient proposed method, but are usually more limiting in the types descent like in [13]. Our work also differs in the fact that we of constraints they can handle. Finally, in recent years many use full nonlinear dynamics (instead of linearized dynamics) derivative free methods for bilevel optimization (sometimes and demonstrate our approach on a 3D example (instead of called evolutionary methods) have gained popularity. We refer 2D ones). The authors of [14] also develop a differentiable readers to [12, 29] for a more detailed overview of solution rigid body simulator capable of handling contact dynamics by methods to bilevel optimization problems. developing a special purpose solver and using it in conjunction with gradient descent for various applications. B. Differentiating Through Mathematical Programs Recently there has been a resurgence of interest in the potential for differentiating through optimization problems due III.BILEVEL OPTIMIZATION to the success of machine learning. One of the more successful approaches to date solves the bilevel program’s lower problem The ability to differentiate through a nonlinear optimization using an interior point method and computes the gradient im- solver allows us to address problems in the field of bilevel plicitly [1]. The computation of the gradient leverages results programming. Here, we carefully define the notation related in sensitivity analysis of nonlinear programming [22, 16, 26]. to bilevel optimization. Closest to our proposed method, unrolling methods attempt In bilevel programs, an upper optimization problem is to differentiate through various solution algorithms directly formulated with the result of a lower optimization problem [15, 2, 4]. To the best of our knowledge, unrolling approaches appearing as part of the upper problem’s objective, constraints, are either limited to unconstrained lower optimization prob- or both. In this work we formulate the bilevel optimization lems (which include meta-learning approaches like [17]), or problem using the following definitions, from [29]: the lower problem solver is a special purpose solver that handles its constraints using projection steps, which is similar Definition 1. Bilevel Optimization to what is done in [10] or [14]. A bilevel program is a mathematical program (or optimization problem) that can be written in the form: use a second-order update on the primal (but not dual) variables. The differentiability of these update steps is preserved minimize F (xu, xl) xu∈XU ,xl∈XL through design choices again explained in Section IV-B. The subject to xl ∈ argmin{f(xu, xl): subsequent primal-dual Newton method on the augmented xl∈XL Lagrangian is closely related to SQP. The combination of gi(xu, xl) ≤ 0, i = 1, . . . , m a first-order method with a second-order one is justified in section IV-C. Algorithm 1 contains an overview of the solver hj(xu, xl) = 0, j = 1, . . . , n}, just described, with k·kF representing the Frobenius norm. Gk(xu, xl) ≤ 0, k = 1,...,M, H (x , x ) = 0, m = 1,...,N, m u l Result: x, λ s.t. x ≈ argminf(x); h (x) ≈ 0; g (x) ≤ (1) 0 0 h ← {h ∪ convert(g )} where x represents the decision variables of the upper 0 0 u x, λ ← 0 problem and x represents the decision variables of the lower l while i ≤ num first order iterations do problem. The nonlinear expressions f, g and h represent g ← ∇f(x) − ∇h(x)T λ + c∇h(x)T h(x) the objective and constraints of the lower problem, and the T nonlinear expressions F , G and H represent the objective H ← ∇2f(x) + c∇h(x) ∇h(x) −1 and constraints of the upper problem. Notably, the argmin x ← x − (H + (kHkF + ) · I) g operation in problem 1 returns a set of optima for the lower λ ← λ − c · h(x) problem. c ← α · c Definition 2. Lower Problem Solution i ← i + 1 end We define the set-valued function Ψ(x ) as the solution to the u while j ≤ num second order iterations do following optimization problem: g ← ∇f(x) − ∇h(x)T λ + c∇h(x)T h(x) T Ψ(xu) = argmin{f(xu, xl): H ← ∇2f(x) + c∇h(x) ∇h(x) xl∈XL (2) H ∇hT gi(xu, xl) ≤ 0, i = 1,...,I, K ← ∇h 0 hj(xu, xl) = 0, j = 1,...,J}, U, S, V ← svd(K) where the definition of xu, xl, f, g and h are the same as in −1 Definition 1. Si ← Γ(S)S x x g IV. A DIFFERENTIABLE AUGMENTED LAGRANGIAN ← − VS UT λ λ i h(x) METHOD j ← j + 1 In order to solve bilevel optimization problems, we propose end to implement a simple solver based on augmented Lagrangian Algorithm 1: Differentiable Augmented Lagrangian Method methods. The solver we present carefully combines techniques from the nonlinear optimization community in order to achieve a set of properties that are key in making our solver useful in B. Differentiability the context of bilevel optimization. In our proposed solution to bilevel optimization, an upper solver (like an off-the-shelf SQP In order to keep the solver differentiable, it is important solver) makes repeated calls to a lower solver in order to obtain to avoid any branching in the algorithm. In the context of the optimal value of the lower problem and the corresponding nonlinear optimization this can turn out to be difficult. Indeed, gradients at that solution. These gradients are the partials of most nonlinear optimization methods include a line-search the optimal solution with respect to each parameter of the component which would be difficult to implement without lower problem, which themselves correspond to the variables branching. In order to get around this problem, we strictly use of the upper problem. To achieve this, the key properties that second-order updates on the primal as described above. This the lower solver must have include differentiability, robustness is more computationally demanding than first-order updates, and efficiency. but it gives a useful step-size estimate without requiring a line search. Another complication brought on by using second- A. Solver Overview order updates is that the Hessian of the augmented Lagrangian We now give a quick overview of our solver. First, the function used to compute the second-order updates needs to solver converts inequality constraints to equality constraints be sufficiently positive definite in order to be useful. This by introducing extra decision variables for reasons discussed problem is usually handled by either adding a multiple of in Section IV-B. It then uses the robust first-order augmented identity to the Hessian such that all its eigenvalues are above Lagrangian method to warm-start a full primal-dual Newton some numerical tolerance δ, or by projecting the matrix to method for fast tail convergence. Those initial first-order the space of positive definite matrices using a singular value updates notably discover the constraints active set and actually decomposition [24]. When using the first method, the diagonal correction matrix ∆H with the smallest euclidean norm that stability, implementations of the pseudoinverse computation satisfies λmin(H + ∆H) ≥ δ for some numerical tolerance δ must usually set singular values below some threshold to zero. is given by To keep this important operation continuous, and therefore retain differentiability, we perform this correction using a ∆H∗ = τI, with τ = max(0, δ − λ (H)). (3) min shifted sigmoid function (denoted as Γ in Algorithm 1). Since our first few optimization steps on the primal variables Note that repeated singular values can present a challenge are only used to compute a good initial guess for the dual when computing the gradient of singular value decomposition. variables, the matrix ∆H does not need to be exactly optimal Even though there exists methods to precisely define such (in terms of keeping the steepest descent direction intact). We gradients [25], in practice we found that one of many ways of therefore choose to instead solve for ∆H in a fast and easily preventing to gradients from getting too large (like zeroing differentiable way by using ∆H = (kHkF + δ)I. We prove them) worked well enough for our applications. It is also that this modification makes the Hessian sufficiently positive possible to define the needed gradient using the solution of definite in Theorem 1. the least-squares problem the pseudoinverse is used to solve, but we leave this investigation to future work. Theorem 1 (Conservative Estimate of Required Matrix Mod- Since SVD is computationally expensive, its use is currently ification). Let A be a square, symmetric matrix and δ a non- the main bottleneck of our algorithm. However there exist negative scalar, then ∆A := (kAk +δ)I is a matrix such that F other methods of computing the pseudoinverse, for example, λ (A + ∆A) ≥ δ, where λ is the smallest eigenvalue of min min when a good initial guess is available [5], that offer promising A. directions to improve on this. Proof: First, we know that adding a scalar multiple of A significant source of non-smoothness is often related to the identity to a square matrix increases its eigenvalues by inequality constraints. Here, we get around this problem by in- precisely that scalar amount since troducing shaped slack variables and converting the inequality constraints to equality ones, leveraging the equivalence Av = λv ⇒ (A + δI)v = (λ + δ)v. (4) We also know that for a square Hermitian matrix, the largest g(x) ≤ 0 ⇔ ∃y s.t. g(x) + ξ(y) = 0, (7) singular value is equal to the largest eigenvalue in absolute where ξ(y) is the non-normalized softmax operation value i.e. |λ(A)|max = σmax. Next, since multiplication by orthonormal matrices leaves the Frobenius norm unchanged, log(eky + 1) ξ(y) = , (8) we can show that the Frobenius norm of the matrix A is an k upper bound on its smallest singular value. and k is some parameter that adjusts the stiffness of the T kAkF = kUΣV kF operation.

= kΣkF q C. Robustness = σ2 + ... + σ2 1 n (5) An important aspect of our algorithm is that it must be able ≥ σmax(A) to solve nonlinear optimization problems with potentially bad initial guesses on their solution. We handle this challenge by = |λ(A)|max combining first-order method-of-multipliers steps with primal- ≥ |λ (A)| min dual second-order ones as described in Section IV-A. Indeed, where UΣV T is the singular-value decomposition of A. We first-order method of multipliers based on minimization of can now conclude that the eigenvalues of A + (kAkF + δ)I the augmented Lagrangian have been shown to have slow are at least as large as δ i.e. convergence but to be more robust to bad initial guesses [6]. On the other hand, second order methods have provable λmin(A + (kAkF + δ)I) ≥ δ. (6) quadratic convergence when close to the solution. Our algorithm therefore starts by performing a few update steps of the In the second part of our algorithm we set up the KKT first order method followed by additional steps of a second system corresponding to the augmented Lagrangian with in- order one. equalities treated as equalities. This is effectively sequential quadratic programming. A key challenge here is that the D. Efficiency resulting system can end up being singular, and different An important aspect of our algorithm is that it must be solvers handle this in various ways. In our proposed algo- efficient enough to be called multiple times by an upper solver rithm, we use the Moore-Penrose pseudoinverse to handle the when solving bilevel optimization problems. We therefore potential singularity. The computation of this pseudoinverse leverage recent advances in the field of automatic differen- relies on a singular value decomposition (SVD), which has tiation by implementing our algorithm with state of the art a known derivative [25], and therefore differentiability of the tools: namely the ForwardDiff.jl library [27] and the Julia overall algorithm can be maintained. Note that for numerical programming language [8]. V. VALIDITYOFTHE ALGORITHMONA TOY PROBLEM 1.6 closed form solution WITH KNOWN SOLUTION 1.5 1.4 bilevel solver solution 1.2 In order to show that our approach is capable of recovering 1.0 1.0 0.8 the correct solution for various bilevel optimization problems, 0.5 0.6 we compare the solution returned by our solver with the known 0.4 0.0 closed-form solution of a simple problem. 2 4 6 8 10 0 1 2 3 4 5 0.975 The operations research literature is full of problems that 0.8 can be cast in the bilevel optimization framework. The game 0.7 0.950 0.6 theory literature also contains many such problems, specifi- 0.925 0.5 cally in the context of Stackelberg competitions. Here we take 0.4 0.900 0.3 a simple Stackelberg competition problem from [29]. In this 0.875 problem, two companies are trying to maximize their profit by 0.2 2 4 6 8 10 2 4 6 8 10 producing the right amount of a product that is also being sold 1.15 0.8 by a competitor, and that has a value inversely proportional to 1.10 its availability on a shared market. 0.6 1.05 More specifically, we are interested in optimizing the pro- 0.4 1.00 0.95 duction quantity for the leader, denoted as ql, knowing that 0.2 the competitor (the follower) will respond by optimizing its 0.90 0.0 0.85 own production quantity qf . The price of the product on the 1 2 3 4 5 1 2 3 4 5 market is dictated by a linear relationship P (ql, qf ), and the Figure 2: Comparison of the closed-form solution and the production costs for each company follow quadratic relation- solution returned using our solver for a simple Stackelberg ships Cl(ql) and Cf (qf ). We therefore have the following game with various parameter values. Even though this partic- functions ular problem is an easy problem to solve without sophisticated P (ql, qf ) = α − β(ql + qf ) 2 machinery (a requirement to have a close-form solution avail- Cl(ql) = δlql + γlql + cl (9) able), our approach accurately recovers the optimal solution 2 Cf (qf ) = δf qf + γf qf + cf , which is a useful validation of our algorithm. where α, β, δl, δf , γl, γf , cl and cf are fixed scalars for a given instantiation of the problem. We can now write the bilevel optimization problem as VI.VALIDITYOFTHE ALGORITHMON REPRESENTATIVE ROBOTICS APPLICATIONS maximize P (ql, qf )ql − Cl(ql) ql,qf We now demonstrate how our algorithm can find valid subject to qf ∈ argmax{P (ql, qf )qf − Cf (qf )} (10) solutions to bilevel optimization problems corresponding to qf representative robotics applications. We first combine our ql, qf ≥ 0. differentiable solver with a state-of-the-art nonlinear optimiza- For given parameters, the closed-form solution of the opti- tion solver to recover robust trajectories in the framework mal production rate of leader is given by the expression of trajectory optimization. Next, we show the correctness, but also scalability, of our algorithm by applying it to the ∗ 2(β + δf )(α − γl) − β(α − γf ) ql = 2 . (11) parameter estimation problem of a system with non-analytical 4(β + δf )(β + δl) − 2β dynamics (hard contact with friction). In order to show that our solver can help recover the closed- form solution numerically, we define the following lower A. Robust Control problem When designing controllers or trajectories for robotic sys- Ψ(ql) = argmax{P (ql, qf )qf − Cf (qf ): qf ≥ 0}, (12) tems, taking potential external disturbances into account is qf essential. This challenge is often addressed in the framework which leads to the following bilevel optimization of robust control. There are many ways to design robust maximize P (ql, ψ(qf ))ql − Cl(ql) controllers. Here we show how our solver allows us to ql (13) design controllers that are robust to worst-case disturbances subject to ql ≥ 0. in a straightforward manner by casting the problem as a We then solve the lower problem with our proposed solver bilevel optimization problem. Notably, our approach avoids and the upper one by using SNOPT [19], a state-of-the-art SQP introducing additional decision variables to represent the noise solver. The recovered optimal value for the leader’s production which is often unavoidable with alternative optimization-based amount is shown in figure 2. Even though this is a very approaches to robust control such as [23]. simple problem to solve, it is clear that our proposed solver More specifically, we look at the problem of designing a successfully recovers its solution, which is a valuable sanity robust trajectory in state and input space using a direct method. check of our approach. Usually, trajctory optimization is performed by solving a nonlinear optimization problem in a form similar to the effective area, and therefore drag, of the plate). Together, m these define our worst-case noise model as the solution to the X minimize J(xi, ui) lower problem xi,ui;i=1 ...m i=1 T Ψ(x, u) = argmin{P (x) w : kwk2 ≤ 2, |wz| ≤ 1}, (17) subject to d(xi, ui, xi+1, ui+1) = 0, i = 1, . . . , m − 1, w umin ≤ ui ≤ umax, i = 1, . . . , m − 1, where P (x) computes the normal vector at the center of the (14) flat plate given the current arm configuration x and w is the where x is the state of the system at sample i along the i vector representing the wind strength and direction. Figure 4 trajectory, u the corresponding control input, J some additive i shows the resulting trajectory after including the bilevel noise cost we are interested in minimizing along a trajectory, and model in the objective as m is the total number of samples along the trajectory. The 2 T 2 function d captures the dynamic constraints between two J(xi, ui) = ui + 10(P (xi) Ψ(xi, ui)) . (18) consecutive time samples. This constraint depends on the chosen integration scheme. In the case of backward Euler for Figure 5 is then computed by taking the resulting trajectory example, it would have the form and solving the lower problem to optimality with a state-of- the-art solver, SNOPT, as to avoid the possibility that our d(xi, ui, xi+1, ui+1) := xi+1 − (xi + hf(xi+1, ui+1)), (15) solver is returning inaccurate solutions of the lower problem. where h is the time between sample points i and i + 1. An As can be seen in that plot, potential wind gust disturbance in-depth exposition of direct trajectory optimization is beyond is effectively minimized throughout the trajectory, which is the scope of this paper and we refer the readers to [7] for more achieved by tilting the plate horizontally as it travels from the information. desired initial to final configuration. In order to design a trajectory that is more robust, we Next, figure 6 shows the result of including the noise model propose defining noise affecting the robot as the solution to a as a constraint of the upper problem instead of as an objective ”worst-case” optimization problem. That worst-case noise can then easily be integrated in the trajectory optimization problem Ψ(xi, ui) ≤ 25, i = 9,..., 11. (19) either as part of the objective or the constraints of the original Note that the constraint was only enforced on three samples problem giving variations of the bilevel problem in the middle of the trajectory to show the effect of the m X constraints more clearly. Once again, the resulting trajectory minimize J(xi, ui, Ψ(xi, ui)) xi,ui;i=1 ...m has the desired properties. i=1 subject to d(xi, ui, xi+1, ui+1, Ψ(xi+1, ui+1)) = 0, B. Parameter Estimation i = 1, . . . , m − 1, We now demonstrate the generality of our approach by g(Ψ(xi, ui)) ≤ 0, i = 1, . . . , m − 1, applying it to the significantly different problem of parameter

umin ≤ ui ≤ umax, i = 1, . . . , m − 1, estimation. This application of our algorithm also provides an (16) opportunity to demonstrate its scalability. where Ψ(x, u) is the solution of the lower problem that There are many ways to estimate parameters of a dynamical computes the worst-case noise disturbance. In the general case, system. A common approach is to solve a mathematical this lower problem can take many forms. We offer a specific program with the unknown parameters as decision variables example below. and a dynamics residual evaluated over a observed sequences For our specific example, we applied this technique to the of states in the constraints or the objective of that problem. trajectory optimization problem for a 7 degrees-of-freedom However, some systems, especially ones for which analyt- arm with a large flat plate on its end effector. In this example, ical dynamics are not always available, can bring additional the upper optimization problem contains the arm’s dynamics, challenges to this parameter estimation approach. For example, the start and end configuration constraints and the objective dynamics involving hard contact, unless approximations of the is to minimize the arm’s velocity along the trajectory. Figure contact dynamics are made, are best expressed as solutions to 3 shows the resulting trajectory when nothing with respect optimization problems involving complementarity constraints. to potential disturbances (i.e., Ψ(x, u) = 0) is added to this Given a trajectory for a system that involves hard contact, trajectory optimization. formulating an optimization problem for parameter estimation Next we define a noise model for this example, namely requires not only the parameters as decision variables, but also a simple wind disturbance that has an overall L2 norm the introduction of a set of decision variables for each contact constraint, but also has a smaller absolute value constraint and at each sample in the dataset. Clearly, this approach scales on its vertical component (i.e. vertical wind gust strength is poorly, because the number of decision variables increases more limited than overall wind strength) making the noise with each contact point and each sample added to the dataset. model simple but not trivial. The wind affects the arm the Scaling to the number of samples is also especially important most when it is perpendicular to the plate (which maximizes in the presence of noisy measurements, because in such cases Figure 3: Trajectory that only minimizes joint velocities, with no regards to potential external disturbances.

Figure 4: Trajectory that minimizes joint velocities and worst-case wind gust disturbance using bilevel optimization and our proposed solver. The wind gust has a smaller vertical bound than overall norm bound, and affects the arm proportionally to the effective area of the plate in the free stream.

40 the qualities of the parameter estimates are proportional to the without bilevel robustness objective number of samples included. with bilevel robustness objective Here, we demonstrate how our differentiable solver allows us to address this problem in a more scalable manner by 30 casting it as a bilevel optmization problem and leveraging parallelism. The bilevel problem we propose to solve deﬁnes the contact dynamics in a lower problem, while solving the 20 parameter estimation problem in the upper one. The key worst case disturbance insight here is that even though we reduce the number of decision variables in the upper problem to match the number

10 of parameters, we make the constraint evaluation signiﬁ- 0.0 0.2 0.4 0.6 0.8 time cantly more expensive. However this can be compensated by Figure 5: Comparison of the worst-case disturbance along a parallelizing this constraint evaluation. This then allows our trajectory that was computed without taking disturbances into algorithm to regress parameters using more samples than the account in its objective, and one that did by exploiting the alternative approach. bilevel structure of the problem and our differentiable solver. First, we simulate the resulting trajectory of a 7 degrees of freedom arm pushing on a box that slides across the ﬂoor. Figure 7 shows the types of trajectories the interaction 40 produces. The contact points simulated are the four corners without bilevel robustness constraint with bilevel robustness constraint 35 of the box and the arm’s end effector. The contact dynamics are the widely popular ones introduced in [30], although we

30 do not linearize the dynamics. The rigid body dynamics are computed using the RigidBodyDynamics.jl package [21]. The 25 resulting trajectories are the sample points we use for parameter estimation. Deﬁning M as the well-known manipulator 20 worst case disturbance equation

15 M(q0, v0, u, q, v, Λ) = H(q )(v − v ) + C(q, v) (20) 0.0 0.2 0.4 0.6 0.8 0 0 time + G(q) − B(q, u) − P (q, v, Λ), Figure 6: Comparison of the worst-case disturbance along a where H is the inertial matrix, C captures Coriolis forces, trajectory that was computed without taking disturbances into G captures potentials such as gravity, B maps control inputs account as a constraint, and one that did for the middle three to generalized forces and P maps external contact forces to samples by exploiting the bilevel structure of the problem and generalized forces and Λ are the external contact forces. We our differentiable solver. then formulate the following lower optimization problem Figure 7: Simulation of a 7 degrees of freedom robotic arm pushing a box that slides across the ﬂoor to estimate the friction coefﬁcients.

2 2 Ψ(q0, v0, u, q, v) = argmin {kβk + kcnk : Bilevel Classical Λ={β,λ,cn} µ # samples time (s) scaled time (s) scaled # var .23 6 4.0 1.0 .10 1.0 217 M(q0, v0, u, q, v, Λ) = 0, .19 11 7.5 1.9 .32 3.1 397 λe + D(q)T v ≥ 0, .19 19 12.6 3.2 .96 9.6 685 .23 21 14.2 3.6 1.3 13.0 757 T µcn − 1 β ≥ 0, .19 23 16.4 4.1 1.4 14.0 829 (21) .15 34 23.5 5.9 3.5 35.0 1225 φ(q)cn = 0, .15 44 30.7 7.7 24.55 246.0 1585 T T (λe + D(q) v) β = 0, Table I: Results of estimating the friction coefﬁcients on data T (µcn − 1 β)λ = 0, produced by a simulation with hard contact. Our approach, labeled as ”Bilevel”, scales better with respect to the number of β, cn, λ ≥ 0}, samples than the alternative approach, labeled as ”Classical”, where the components of Λ, namely β, cn and λ are variables that introduces decision variables for the unobserved contact describing the friction and normal forces and D computes the forces in the dataset. contact friction basis. We refer the readers to [30] for a more complete description of the contact dynamics used here. We then solve the following upper optimization problem VII.CONCLUSION

N X 2 In this work, we develop a differentiable general purpose minimize M(qi, vi, ui+1, qi+1, vi+1, Ψ(.))2 µ (22) nonlinear program solver based on augmented Lagrangian i=1 methods. We then apply our solver to a toy example and two subject to 0 ≤ µ ≤ 1, important problems in robotics, robust control and parameter estimation, by casting them as bilevel optimization problem. with (q , v , u , q , v ) as inputs to Ψ using instances of i i i+1 i+1 i+1 In future work, we intend to improve the performance of our our solver running across multiple threads to handle the lower solver by leveraging more advanced optimization techniques problem and SNOPT to take care of the upper one. such as warm starting and advanced parallelization schemes In our specific parallelization scheme, one thread corre- like GPU programming. We also plan to apply our solver to sponds to one sample in the dataset, but other parallelization other problems in robotics that might exhibit a bilevel struc- schemes are also possible. Note that if the samples are noisy, ture, such as imitation learning and trajectory optimization the lower problem is not necessarily feasible. In such cases, through contact. our solver can be interpreted as minimizing the residual of the dynamics. Also note that since we are not using the KKT conditions to retrieve the gradient of the lower problem, not ACKNOWLEDGMENTS solving it to optimality or even feasibility still results in well- The authors of this work were partially supported by the defined gradients. Office of Naval Research, Sea-Based Aviation Program. Table I contains the results of running the estimation approaches for various numbers of samples and different friction REFERENCES coefficients. The important take-away from these results is even though our current implementation of the solver is slower [1] B. Amos and J. Z. Kolter. Optnet: Differentiable opti- than doing the parameter estimation classically for small prob- mization as a layer in neural networks. In Int. Conf. on lems, our approach scales better with the number of samples Machine Learning, 2017. thanks to its straighforward parallelization. Conceptually, this [2] B. Amos, L. Xu, and J. Z. Kolter. Input convex neural is true for the same reasons that it is simple to compute networks. In Int. Conf. on Machine Learning, 2017. losses over samples in parallel when doing gradient descent [3] J. F. Bard and J. E. Falk. An explicit solution to the multi- in machine learning contexts, except in this case the “loss” is level programming problem. Computers & Operations itself a complex nonlinear optimization. Research, 9(1):77–100, 1982. [4] D. Belanger, B. Yang, and A. McCallum. End-to-end [22] A. B. Levy and R. T. Rockafellar. Sensitivity of solutions learning for structured prediction energy networks. In in nonlinear programming problems with nonunique mul- Int. Conf. on Machine Learning, 2017. tipliers. In Recent Advances in Nonsmooth Optimization. [5] A. Ben-Israel and D. Cohen. On iterative computation World Scientific Publishing, 1995. of generalized inverses and associated projections. SIAM [23] J. Lorenzetti, B. Landry, S. Singh, and M. Pavone. Re- Journal on Numerical Analysis, 3(3):410–419, 1966. duced order model predictive control for setpoint track- [6] D. Bertsekas. Constrained optimization and Lagrange ing. In European Control Conference, 2019. Submitted. multiplier methods. Athena Scientific, 2 edition, 1996. [24] J. Nocedal and S. J. Wright. Numerical Optimization. [7] J. T. Betts. Survey of numerical methods for trajectory Springer, second edition, 2006. optimization. AIAA Journal of Guidance, Control, and [25] T. Papadopoulo and M. Lourakis. Estimating the jacobian Dynamics, 21(2):193–207, 1998. of the singular value decomposition: Theory and appli- [8] J. Bezanson, S. Karpinski, V. B. Shah, and A. Edelman. cations. In European Conf. on Computer Vision, 2000. Julia: A fast dynamic language for technical computing, [26] D. Ralph and S. Dempe. Directional derivatives of the 2012. Available at http://arxiv.org/abs/1209.5145. solution of a parametric nonlinear program. Mathemati- [9] W. F. Bialas and M. H. Karwan. Two-level linear cal Programming, 70(1-3):159–172, 1995. programming. Management Science, 30(8):1004–1020, [27] J. Revels, M. Lubin, and T. Papamarkou. Forward-mode 1984. automatic differentiation in julia, 2016. Available at [10] J. Carius, R. Ranftl, V. Koltun, and M. Hutter. Trajectory https://arxiv.org/abs/1607.07892. optimization with implicit hard contacts. IEEE Robotics [28] G. Savard and J. Gauvin. The steepest descent direction and Automation Letters, 3(4):3316–3323, 2018. for the nonlinear bilevel programming problem. Opera- [11] Y. Chen and M. Florian. On the geometric structure tions Research Letters, 15(5):265–272, 1994. of linear bilevel programs: a dual approach. Technical [29] A. Sinha, P. Malo, and K. Deb. A review on bilevel report, Centre De Recherche Sur Les Transports, 1992. optimization: from classical to evolutionary approaches [12] B. Colson, P. Marcotte, and G. Savard. An overview of and applications. IEEE Transactions on Evolutionary bilevel optimization. Annals of Operations Research, 153 Computation, 22(2):276–295, 2018. (1):235–256, 2007. [30] D. Stewart and J. C. Trinkle. An implicit time-stepping [13] F. de Avila Belbute-Peres, K. Smith, K. Allen, J. Tenen- scheme for rigid body dynamics with coulomb friction. baum, and J. Z. Kolter. End-to-end differentiable physics In Proc. IEEE Conf. on Robotics and Automation, 2000. for learning and control. In Conf. on Neural Information [31] M. Toussaint, K. Allen, K. Smith, and J. B. Tenenbaum. Processing Systems, 2018. Differentiable physics and stable modes for tool-use [14] J. Degrave, M. Hermans, J. Dambre, and F. Wyffels. and manipulation planning. In Robotics: Science and A differentiable physics engine for deep learning in Systems, 2018. robotics. In Int. Conf. on Learning Representations, [32] H. Tuy, A. Migdalas, and P. Varbrand. A global optimiza- 2017. tion approach for the linear two-level program. Journal [15] J. Domke. Generic methods for optimization-based of Global Optimization, 3(1):1–23, 1993. modeling. In AI & Statistics, 2012. [33] A. Wachter and L. T. Biegler. On the implementation [16] A. V. Fiacco and Y. Ishizuka. Sensitivity and stability of an interior-point filter line-search algorithm for large- analysis for nonlinear programming. Annals of Opera- scale nonlinear programming. Mathematical Program- tions Research, 27(1):215–235, 1990. ming, 106(1):25–57, 2006. [17] C. Finn, P. Abbeel, and S. Levine. Model-agnostic meta- learning for fast adaptation of deep networks. In Int. Conf. on Machine Learning, 2017. [18] J. Fortuny-Amat and B. McCarl. A representation and economic interpretation of a two-level programming problem. Journal of the Operational Research Society, 32(9):783–792, 1981. [19] P. E. Gill, W. Murray, and M. A. Saunders. SNOPT: An SQP algorithm for large-scale constrained optimization. SIAM Review, 47(1):99–131, 2005. [20] C. D. Kolstad and L. S. Lasdon. Derivative evaluation and computational experience with large bilevel mathematical programs. Journal of Optimization Theory & Applications, 65(3):485–499, 1990. [21] Twan Koolen and contributors. Rigidbodydynam- ics.jl, 2016. URL https://github.com/JuliaRobotics/ RigidBodyDynamics.jl.