2
, Simultaneous Contact, Gait and Motion Planning for Robust Multi-Legged Locomotion via Mixed-Integer Convex Optimization Bernardo Aceituno-Cabezas 1 , Carlos Mastalli 2 , Hongkai Dai 3 , Michele Focchi 2 , Andreea Radulescu 2 , Darwin G. Caldwell 2 , Jos´ e Cappelletto 1 , Juan C. Grieco 1 , Gerardo Fern´ andez-L´ opez 1 , and Claudio Semini 2 1 Mechatronics Research Group, Sim´ on Bol´ ıvar University, Caracas, Venezuela 2 Dynamic Legged Systems Lab., Department of Advanced Robotics, Istituto Italiano di Tecnologia, Genova, Italy 3 Toyota Research Institute, Los Altos, California, USA , Introduction Reasoning about contacts and motions simultaneously is crucial for generating complex whole-body behaviors. We propose a mixed- integer convex formulation to plan simultaneously contact locations, gait transitions and motion, in a computationally efficient fashion. Approach Overview Optimal Plan Goal State Convex terrain segmentation Current State Command Simultaneous Contact and Motion Planner Whole-body Controller Trajectory Optimization We formulate a convex trajectory optimization: min r ,k o ,p l ,λ l g T + N X k =1 g (k ) (1) The running cost g (k ) maximizes the stability of the motion, while seeking for the fastest and smoothest gait: g (k )= k ¨ r k k Q v + kλ l ,k k Q F + q u U k + q t t k - q α α k , (2) 1 Minimize the CoM acceleration ¨ r. 2 Minimize the contact forces magnitude kλk. 3 Minimize the upper bound of quadratic terms U =(u - , u + ). 4 Maximize the stability margin α. 5 Minimize the execution time. The terminal cost g T biases the plan towards its goal (r G ): g T = kr T - r G k Q g (3) In practice, we add a small cost to k ˙ k k Q k in order to generate smoother motions. Whole-body Control Reference CoM acceleration (¨ r r ∈ R 3 ) and body angular acceleration ( ˙ ω r b ∈ R 3 ) through a virtual model : ¨ r r =¨ r d + K r (r d - r )+ D r (˙ r d - ˙ r ), ˙ ω r b =˙ ω d b + K θ e (R d b R T b )+ D θ (ω d b - ω b ), (4) where K r , D r , K θ , D θ ∈ R 3×3 are positive-definite diagonal matrices of propor- tional and derivative gains, respectively. We formulate the tracking problem using QP with the generalized accelerations and contact forces as decision variables, x = [¨ q T , λ T ] T ∈ R 6+n +3n l : x * = arg min x g err (x)+ kxk W ; Ax = b, d < Cx < ¯ d (5) g err (x)= ¨ r - ¨ r r ˙ ω b - ˙ ω r b S (6) The equality constraints Ax = b encodes dynamic consistency, stance condition and swing task. The inequality constraints d < Cx < ¯ d encode friction, torque, and kinematic limits [1]. We map the optimal solution x * into desired feed-forward joint torques τ * ff ∈ R n : τ * ff = M T bj M j ¨ q * + h j - J T cj λ * (7) These are summed with the joint PD torques (i.e. feedback torques τ fb ) to form the desired torque command τ d : τ d = τ * ff + PD (q d j , ˙ q d j ), (8) which is sent to a low-level joint-torque controller. Simultaneous Contact and Motion Planning A. Centroidal Dynamics m¨ r ˙ k = m g + ∑ n l l =1 λ l ∑ n l l =1 (p l - r ) × λ l , (9) I CoM position r I Angular Momentum k I Contact force of end-effector λ l I Position of end-effector p l where p l - r can be described as bilinear function [2] and decomposed as: ab = u + - u - 4 u + ≥ (a + b) 2 u - ≥ (a - b) 2 . (10) B. Gait Sequence A gait matrix [3] T ∈{0, 1} N f ×N t where T ij = 1 means that the robot will move to the i th contact location at the j th time-slot. I number of contacts N f I number of time slots N t Each contact location is reached once: ∑ N t j =1 T ij =1 , ∀i =1, .., N f . C. Contact Location We constrain the contacts to lie within one of N r convex safe contact surfaces (each represented as a polygon R = {c ∈ R 3 |A r c ≤ b r }). After assigning the contact to swing, we optimize the contact locations f = (f x , f y , f z ,θ ) and we assign this contact to one of the N r using a binary matrix: N r X r=1 H i r =1, H i r ⇒ A r f i ≤ b r (11) We approximate the kinematic limits as a bounding box with respect to the CoM: f i - r T (i ) + L i cos(θ i + φ i ) sin(θ i + φ i ) ≤ d lim , (12) I CoM position at i transition r T (i ) I Diagonal of the bounding box d lim I Distance from the trunk of leg L i (the trigonometric functions are decomposed in piecewise linear functions) [4]. D. End-effector Trajectories We define γ (j , t ) as swing reference trajectory, connected to adjacent contacts, where: I j indicates the time-slot I t ∈ [1,..., N k ] all the knots per time-slot The leg reaches the contact position f i at the end of the j slot: T ij ⇒ p l (i )γ (j ,N k ) = f i , (13) where l (i ) is the leg number for the i th contact. To constrain that the leg remains stationary when there is no transition: X i ∈C (l ) T ij =0 ⇒ p l γ (j ,t ) = p l γ (j ,1) ∀t ∈ [2,..., N k ], (14) where C (l ) are the contact indexes assigned to the l th leg. We ensure kinematic feasibility by constraining the CoM position with respect to the end-effectors (bounding box constraint): d - < r j - ∑ n l l =1 p lj n l < d + (15) https://dls.iit.it/
