Momentum-centric whole-body control and kino-dynamic motion generation for floating-base robots Alexander Herzog The Movement Generation and Control Group (Ludovic Righetti)
Conflicting tasks & constraints task vs task weights physical constraint vs task hierarchies position task x CoP inequality constraint
Balancing in Single Support [Herzog, Righetti et al., IROS, 2014] feedback in task space no joint stabilization consistent optimization problem QP Cascades [Kanoun et al., 2011, Saab et al., 2013] 1kHz x des x Task Hierarchy M q + h = S T + J T c 1., torque limits 2.admissible contact forces, acceleration limits 3.swing leg, posture and momentum control 4.Regularization on forces joint pos. & vel. tracking momentum tracking www.youtube.com/watch?v=jmj3uv2q8xg push force
Momentum-based control as a LQR problem ḣ = apple I 3 3 0 3 3... [x cog x i ] I 3 3... + apple mg 0 (linear) (angular) mẋ com = h lin Interaction forces as the control input Desired performance min apple apple xcom h T Q apple xcom h + T R Linearization of the dynamics to compute optimal feedback control = K apple xcog h + k(x ref, h ref ) Use control law to define desired closedloop behavior of the momentum task apple I 3 3 0 3 3... [x i x cog ] I 3 3... + K apple xcog h k =0
Balance control with LQR momentum control [Herzog et al., AuRo 2016] Original momentum task LQR Design LQR significantly improves performance No need for re-tuning when changing contact conditions www.youtube.com/watch?v=jmj3uv2q8xg
strict prioritization between tasks and constraints 1kHz task feedback loop(s) LQR feedback design for momentum control pure feed forward torques, no joint stabilization
Gait control with passive ankles torque control passive ankles controlling CoP requires upper body motion Our strategy: control gait with stepping no explicit CoM control hierarchical inverse dynamics
Step Timing Adjustment: A Step Toward Generating Robust Gaits [WeBT2.1] Repeat after each foot-step: Specify bounded, nominal stepping frequency, width and length Repeat in a feedback-loop online at 1kHz: Optimize step duration and location Re-compute swing foot trajectories to adjust for changing swing durations and landing locations. update nominal stepping schedule optimize step timing & location online adapt swing foot trajectory Robot Task Hierarchy Robot [M. Khadiv, A. Herzog, A. Moosavian, L. Righetti; Step Timing Adjustment: A Step Toward Generating Robust Gaits; Humanoids 2016]
Step Timing Adjustment: A Step Toward Generating Robust Gaits [WeBT2.1] Repeat after each foot-step: Specify bounded, nominal stepping frequency, width and length Repeat in a feedback-loop online at 1kHz: Optimize step duration and location Re-compute swing foot trajectories to adjust for changing swing durations and landing locations. update nominal stepping schedule optimize step timing & location online M q + h = S T + J T c adapt swing foot trajectory 1., 2. contact constraints, CoM height Task Hierarchy Robot 3. swing leg control 4. Regularization on posture 5. Regularization Robot on forces [M. Khadiv, A. Herzog, A. Moosavian, L. Righetti; Step Timing Adjustment: A Step Toward Generating Robust Gaits; Humanoids 2016]
Step Timing Adjustment: A Step Toward Generating Robust Gaits [WeBT2.1] push: 290 N * 0.3 s Push recovery over different directions https://www.youtube.com/watch?v=rvbcfm8v_dy [13] A. Herdt et al. Online walking motion generation with automatic footstep placement Advanced Robotics, vol. 24, no. 5-6, pp. 719 737, 2010. [M. Khadiv, A. Herzog, A. Moosavian, L. Righetti; Step Timing Adjustment: A Step Toward Generating Robust Gaits; Humanoids 2016]
Problem formulation: kino-dynamic planning Given a desired sequence of contacts compute optimal trajectories for: - contact forces (+ locations) - center of mass - angular momentum - whole-body motion Consistent with the dynamics of the robot contact forces M(q) q + h(q, q) =S T + J T c (q) joint + robot base accelerations torques (under) actuation 2 S, q 2 S q Constraints on admissible contact forces and motions
Decomposition of the dynamics Equations of motion M(q) q + h(q, q) =S T + J T c (q) [Herzog et al., IROS 2016]
Decomposition of the dynamics Equations of motion M u q + h u = J T u M a q + h a = + J T a Unactuated Actuated [Herzog et al., IROS 2016]
Decomposition of the dynamics Unactuated dynamics = evolution of momentum Linear momentum Angular momentum Equations of motion M u q + h u = J T u M a q + h a = + J T a Unactuated Actuated Actuated dynamics = M a q + h a J T a Equivalent to a manipulator Any combination of motions and contact forces will satisfy actuated dynamics (ignoring actuation limits) [Herzog et al., IROS 2016]
Decomposition of the dynamics Unactuated dynamics = evolution of momentum Linear momentum Angular momentum Decomposition of optimal control problem: Equations of motion M u q + h u = J T u M a q + h a = + J T a Unactuated 1 momentum and contact forces planning II kinematics planning Actuated Actuated dynamics = M a q + h a J T a Equivalent to a manipulator Any combination of motions and contact forces will satisfy actuated dynamics (ignoring actuation limits) [Herzog et al., IROS 2016]
Problem formulation: kino-dynamic planning Kinematic optimization Dynamic optimization min. q(t), (t) J q (q) J ( ) + Cost describing the task to achieve s.t. x CoM (q) =r x e (q) =c e apple H(q) q + Ḣ(q) q = ḣ = Mg + P P e f e e (apple e +(c e r) f e ) Dynamic constraint q 2 S q, 2 S Contact force and Kinematic constraints The only variables connecting dynamics optimization to the kinematics optimization are - Center of Mass r(t) - Contact locations ce(t) - Linear+angular momentum h(t)
Problem formulation: kino-dynamic planning Kinematic optimization Dynamic optimization min. q(t), (t) J q (q) J ( ) + Cost describing the task to achieve s.t. x CoM (q) =r x e (q) =c e apple H(q) q + Ḣ(q) q = ḣ = Mg + P P e f e e (apple e +(c e r) f e ) Dynamic constraint q 2 S q, 2 S Contact force and Kinematic constraints - Decompose kino-dynamic optimization into: kinematic optimization problem (q(t)) dynamic optimization problem (contact forces λ(t)) - Solve two better structured problems iteratively and enforce that both problems agree on r(t),c(t),h(t)
Algorithm alternating kinematics and dynamics optimizations min. q(t) J q (q) {z } Task Cost + H(q) q h 2 + x CoM (q) r 2 + x e (q) c e 2 {z } Consistency with Force Optimization s.t. q 2 S q Kinematic optimization Typically converges after 2 iterations r, c, h x CoM (q), x e (q), H(q) q min. r,c,h, J (r, c, h, ) {z } Task Cost + h h 2 + r r 2 + c e c e 2 {z } Consistency with Motion Optimization r, c, h r, c, h [Herzog et al., IROS 2016] apple Mg + P s.t. ḣ = P e f e e (apple e +(c e r) f e ), r, c 2 S Dynamic optimization
Optimal control of multi-contact behavior Kinematic plan is not physically possible https://www.youtube.com/watch?v=nkp5z9mfrrw [Herzog et al., IROS 2016]
Optimal control of multi-contact behavior After alternate optimization of kinematics and dynamics => physically consistent motion https://www.youtube.com/watch?v=nkp5z9mfrrw [Herzog et al., IROS 2016]
Structure of the dynamics optimal control problem min. r,c,h, J (r, c, h, ) {z } Task Cost + h h 2 + r r 2 + c e c e 2 {z } Consistency with Motion Optimization apple Mg + P s.t. ḣ = P e f e e (apple e +(c e r) f e ), r, c 2 S optimal control with contact forces and location as control input (f,k,c) and center of mass and momentum as states Problem is non-convex due to the cross product Structure of optimization problem leads to efficient solvers 1. dynamics can be written as difference of convex function 2. problem can be expressed in sparse sequential form that scales linearly with time [Herzog et al., IROS 2016]
Difference of convex functions [Herzog et al., IROS 2016] Nonlinearity comes from cross product (non-convex operator) [a b] i = apple a b T H i apple a b,i=1, 2, 3 Can we compute the convex part of the cross product? H i = V i (D +,i D,i )V T i = H + i H i, with D +,i, D,i non-negative, diagonal H + i, H Useful i positive to definite solve the optimization problem by iterative convex approximations (e.g. using a Sequential apple apple C 0 [(Ca) (Db)] i =( 0 D x)t (H + C 0 i H i ) 0 D x = x T ( E T H + i E ET H i E )x apple apple a C 0 x = b, E = 0 D E T H + i E, ET H i E positive definite Quadratic Program algorithm)
Difference of convex functions [Herzog et al., IROS 2016] apple a [a b] i = b T apple a H i,i=1, 2, 3 b H i = V i (D +,i D,i )Vi T = H + i H i, with D +,i, D,i non-negative, diagonal H + i, H i positive definite Cross product written using quadratic form Hi for each component apple apple C 0 [(Ca) (Db)] i =( 0 D x)t (H + C 0 i H i ) 0 D x = x T ( E T H + i E ET H i E )x apple apple a C 0 x = b, E = 0 D E T H + i E, ET H i E positive definite
Difference of convex functions [Herzog et al., IROS 2016] [a b] i = apple a b T H i apple a b,i=1, 2, 3 H i = H + i H i H + i > 0 H i > 0 H i = V i (D +,i D,i )V T i = H + i H i, with D +,i, D,i non-negative, diagonal H + i, H i positive definite [r f] i =[(Cx) (Dx)] i apple apple C 0 =( x) T (H + C 0 0 D i H i )( x) 0 D {z } {z } E E = x T (E T H + i E {z } p.s.d E T H i E)x {z } p.s.d Hi can be written as a difference of positive definitive matrices Decomposition of cross product can be precomputed offline for fixed vectors (independent of actual problem)
Difference of convex functions [Herzog et al., IROS 2016] We can decompose the nonlinearity into a difference of positive definite matrices - convex part of the problem can be trivially constructed without any matrix decomposition [a b] i = apple a b T H i apple a b - Order of magnitude speed of up positive compared definitive to off-the- matrices [r f] i =[(Cx) shelf SQP (Dx)] solvers i like SNOPT apple apple C 0 =( x) T (H + C 0 0 D i H i )( x) 0 D {z } {z } of the E problem that leads to E optimization times < 1s = x T (E T H + i E {z } p.s.d,i=1, 2, 3 H i = V i (D +,i D,i )V T i = H + i H i, with D +,i, D,i non-negative, diagonal H + i, H i positive definite E T H i E)x {z } p.s.d H i = H + i Decomposition leads to new and fast algorithms Hi can be written as a difference H i H + i > 0 H i > 0 - Decomposition can be used for a convex relaxation Decomposition of cross product can be precomputed offline for fixed vectors (independent of actual problem)
Decomposition We decompose the original problem into kinematic trajectory optimization optimal control with controls f,c and states r,h dynamics can be constructed as difference of convex functions (no matrix decomposition) expressed in sparse sequential form Plans are dynamically feasible; can be controlled with hierarchical inverse dynamics [Herzog et al 2015]
A Convex Model of Humanoid Momentum Dynamics for Multi-Contact Motion Generation [ThPoS.8] min. r,l,c,f s.t. J(r, l, c, f)+k kk 2 2 3 A t 4 r t l t 5 + a t =0 f t k t = X e (c t,e r t ) f t,e r (CoM) f (force) = X i x T Q + i x xt Q i x x T = r l f c T motion generation ~1s c (contact location) [B. Ponton, A. Herzog, S. Schaal, L. Righetti; A Convex Model of Humanoid Momentum Dynamics for Multi-Contact Motion Generation; Humanoids 2016]
A Convex Model of Humanoid Momentum Dynamics for Multi-Contact Motion Generation [ThPoS.8] min. r,l,c,f s.t. J(r, l, c, f)+k X i 2 3 A t 4 r t l t f t 5 + a t =0 x T Q + i x xt Q i xk 2 r (CoM) f (force) motion generation ~1s c (contact location) [B. Ponton, A. Herzog, S. Schaal, L. Righetti; A Convex Model of Humanoid Momentum Dynamics for Multi-Contact Motion Generation; Humanoids 2016]
A Convex Model of Humanoid Momentum Dynamics for Multi-Contact Motion Generation [ThPoS.8] min. r,l,c,f s.t. J(r, l, c, f)+ X kx T Q + i xk2 + kx T Q i xk 2 i A t 2 4 r t l t f t 3 5 + a t =0 r (CoM) f (force) motion generation ~1s c (contact location) [B. Ponton, A. Herzog, S. Schaal, L. Righetti; A Convex Model of Humanoid Momentum Dynamics for Multi-Contact Motion Generation; Humanoids 2016]
A Convex Model of Humanoid Momentum Dynamics for Multi-Contact Motion Generation [ThPoS.8] min. r,l,c,f s.t. J(r, l, c, f)+ X ks + k 2 + ks k 2 i A t 2 4 r t l t f t 3 5 + a t =0 x T Q + i x apple s+,x T = r l f c T x T Q i x apple s r (CoM) f (force) motion generation ~1s c (contact location) [B. Ponton, A. Herzog, S. Schaal, L. Righetti; A Convex Model of Humanoid Momentum Dynamics for Multi-Contact Motion Generation; Humanoids 2016]
Conclusion Whole-body control with hierarchical inverse dynamics balancing with momentum control decomposed whole-body motion generation We can identify structure in the force optimization favorable for structured optimization
The Movement Generation and Control Group Ludovic Righetti Nicholas Rotella Alexander Herzog Brahayam Ponton [ThPoS.8] Miroslav Bogdanovic John Rebula Sean Mason Majid Khadiv [WeBT2.5] [WeBT2.1] Funding