Reference Spreading Hybrid Control Exploiting Dynamic Contact Transitions in Robotics Applications Alessandro Saccon OptHySYS Workshop Trento, January 9-11, 2017
Atlas Robotic Locomotion and Manipulation Durus HRP-2 icub Walkman Amigo Mabel
Research Focus Development of a framework for planning, control, and estimation for robotic systems undergoing intentional physical contact with their surrounding. The aim is enabling dynamic locomotion and manipulation tasks that require exploitation of dynamic contact transitions. The research effort is equally distributed between the theoretical development and the validation of the developed theory via numerical simulations and real-world experiments on existing and ad-hoc-designed physical platforms. PAGE 2
Research focus (cont d) - Dynamic Motion with Dynamic Contact Transitions - Performance (and robustness) - Energy efficiency - Robotic systems with complex kinematics like Tools: Nonlinear and Hybrid Systems Theory (Lyapunov stability, hybrid time domains, ) / Multi-body Dynamics / Nonsmooth Mechanics -- Mechanical Systems with Unilateral Constraints / Numerical Optimal Control PAGE 3
Modeling assumption Mechanical system with unilateral constraints: M(q) q + C(q, q) q + G(q) =S + X subject to X W N,i (q) N,i + W T,i (q) T,i i2i N i2i T complementarity conditions + contact and impact laws See books by: Brogliato / Glocker / Reine... This type of systems can be also casted in the framework of hybrid systems (not 100% equivalent ) Related research by J. Grizzle, A. Ames, A. Forni, A.R.Teel, L.Zaccarian, R. Sanfelice, L.Menini, A.Tornanbè, S.Galeani, B. Brogliato, P.R. Pagilla, N.van de Wouw, From the robotic comunity, L.Sentis, R. Tedrake, E Todorov, PAGE 4 / 25
Reference Spreading Hybrid Control q[m] 2 1.5 1 0.5 0 REFERENCE CLASSICAL REFERENCE SPREADING 0.5 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 q[m] 2 1 0 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 5 q[m] 2 1 0 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 5 u[n] q [m/s] 4 2 0 2 4 6 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 3 3.5 4 4.5 5 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 q [m/s] u[n] 0 5 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 5 0 5 10 15 20 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 q [m/s] u[n] 0 5 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 5 0 5 10 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 Preliminary successful experiments conducted on at 1DOF setup at that time Original idea came from sensitivity analysis / notion of time-triggered linearization A. Saccon, N. van de Wouw, H. Nijmeijer Sensitivity analysis of hybrid systems with state jumps with application to trajectory tracking IEEE Conference on Decision and Control, 2014 PAGE 5
Main collaborators on this research topics Mark Rijnen TU/e Silvio Traversaro IIT Henk Nijmeijer TU/e Nathan van de Wouw TU/e Francesco Nori IIT Herman Bruyninckx KU Leuven René van de Molengraft TU/e Nick Rosielle TU/e
Outline Reference Spreading Hybrid Control: the basics The Actuated Rebounding Pendulum Computing Optimal Tracking Gains Punching the Wall with a Humanoid Robot: Multi-domain Trajectory Tracking
Reference Spreading Hybrid Control
Classic linear feedback + feedforward Hybrid dynamics FLOW x = f x, u, t γ x, u, t 0 JUMP x, = g x., t γ x, u, t = 0 Track a reference state-input signal (α t, μ(t)) with impact times τ 4, τ 5,, τ 7 PAGE 9 / 25
Classic linear feedback + feedforward System dynamics: (flow) x = f x, u, t γ x, u, t 0 (jump) x, = g x., t γ x, u, t = 0 Track a reference state-input signal (α t, μ(t)) with nominal jump times τ 4, τ 5,, τ 7. Classic linear feedback + feedforward u = μ t + K α t x(t) PAGE 10 / 25
Classic linear feedback + feedforward (Drawback 1) Closed-loop and nominal event time mismatch are not considered PAGE 11 / 25
Classic linear feedback + feedforward (Drawback 1) Closed-loop and nominal event time mismatch are not considered (Result) Poor tracking and detrimental inputs during the time mismatch period PAGE 12 / 25
Classic linear feedback + feedforward (Drawback 2) Cannot deal with changing state dimension i.e. with multi-domain hybrid systems stance flight (q k,q h, q k, q h ) 2 R 4 (q k, q k ) 2 R 2 PAGE 13 / 25
Hybrid linear feedback + feedforward Key idea: address the event time mismatch via a different notion of tracking error hybrid time domain: time ( t R) + event counter ( j = 0,1,2, ) define extended reference trajectory PAGE 14 / 25
Hybrid linear feedback + feedforward Key idea: take into account the event time mismatch via a different notion of tracking error first step: partition the reference PAGE 15 / 25
Hybrid linear feedback + feedforward Key idea: take into account the event time mismatch via a different notion of tracking error second step: add a counter for each segment PAGE 16 / 25
Hybrid linear feedback + feedforward Key idea: take into account the event time mismatch via a different notion of tracking error third step: spread the reference by forward and backward integration of the dynamics PAGE 17 / 25
Hybrid linear feedback + feedforward Key idea: take into account the event time mismatch via a different notion of tracking error RESULT: more than one reference at each instant of time PAGE 18 / 25
Hybrid linear feedback + feedforward System dynamics: (flow) x B = f B x B, u, t γ B x B, u, t 0 (jump) x B, = g B x B., t γ B x B, u, t = 0 Track a reference state-input signal (α t, j, μ(t)) with impact times τ 4, τ 5,, τ 7 Apply control input u = μ t + K t, j α t, j x t, j PAGE 19 / 25
The Actuated Rebounding Pendulum
The Actuated Rebounding Pendulum (ARP) PAGE 21
The Actuated Rebounding Pendulum The setup is up and running since a couple of months Model ID, including viscous/dry friction and coeff. or restitution ( v + = - e v - ) PAGE 22
Standard PD 0 1 2 3 4 5 6 7 8 0 1 2 3 4 5 6 7 8 0 1 2 3 4 5 6 7 8 0 1 2 3 4 5 6 7 8 9 10 11 τ [Nm] 0.05 Reference ω 0 n = 2π/0.5 ω n = 2π/1-0.05 ω n = 2π/2-0.1 q [rad] 0.6 0.4 0.2 0 ˆv [rad/s] 5 0-5 -10 0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2 Time [s] PAGE 23
Ref. Spreading 0 1 2 3 4 5 6 7 8 0 1 2 3 4 5 6 7 8 0 1 2 3 4 5 6 7 8 0 1 2 3 4 5 6 7 8 τ [Nm] 0.05 Reference ω 0 n = 2π/0.5 ω n = 2π/1-0.05 ω n = 2π/2-0.1 q [rad] 0.6 0.4 0.2 0 ˆv [rad/s] 5 0-5 -10 0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2 Time [s] PAGE 24
Computing Optimal Tracking Gains
Example: Actuated Rebounding Cart Actuated bouncing mass x = 0 1 0 0 x + 0 1 m u Partially elastic impacts Newton s law of restitution: x, = 1 0 0 e x. m = 1 kg e = 0.95 K = 25 10 PAGE 26
Example: Actuated Rebounding Cart Classic PD Reference Spreading PD PAGE 27
How to tune the feedback gains? First steps Minimize cost on state and input (think to LQR ) BUT closed loop event time are not known in advance Nonlinear hybrid optimal control problem IDEA: Approximate the dynamics about the reference using a hybrid linearization (a TIME TRIGGERED, jumping linear system) Closed loop solution with u = μ(t) + v is approximated by x t, j = αj t, j + z t, j + o z N, v PAGE 28
How to tune the feedback gains? First steps x t, j = αj t, j + z t, j + o z N, v STATE TRIGGERED x = f x, u, t TIME TRIGGERED LIN. z = A t z + B t v x, = g x., t, t = t i z, = G(t)z., t = τ T PAGE 29
The time triggered linearization Linearization jumps at the reference event times linear time-triggered hybrid system PAGE 30 / 36
The time triggered linearization (cont d) Linearization jumps at the reference event times linear time-triggered hybrid system PAGE 31 / 36
The time triggered linearization (cont d) Linearization jumps at the reference event times linear time-triggered hybrid system PAGE 32 / 36
LQR-like Optimal control Optimization problem: Z 1 min X 2 Y zz Qz + v Z Rv N ds + 1 2 z T Z P Z z(t) s.t. z = A t z + B t v t τ T z, = G(t)z. t = τ T z t N = z N PAGE 33
LQR-like Optimal control (cont d) Optimization problem: Z 1 min X 2 Y zz Qz + v Z Rv N ds + 1 2 z T Z P Z z(t) z s.t. z = A t z + B t v t τ T z, = G(t)z. t = τ T t N = z N (SOLUTION) v = R.4 B t Z P t z = K t z P = P Z P = A t Z + PA t PB t R.4 B t Z P + Q P. = Z Z t P, Z t t = T t τ T t = τ T PAGE 34
Optimal control: Actuated Rebounding Cart Feedback gains for example reference (bouncing) K = K 4 K 5 R = 1, Q = 50 0 0 50 56.6 7.1 P Z = 7.1 8.0 K 4 changes sign! Rijnen, Saccon, Nijmeijer, IEEE CDC 2015 PAGE 35
Optimal control: const. vs optimal fdbk gains PAGE 36
Multi-domain Trajectory Tracking
Example: Hopping leg IIT, Genova ADVR group Rijnen, van Rijn, Dallali, Saccon, Nijmeijer, IFAC PSYCO 2016 PAGE 38 / 25
Example: Hopping leg Modeling 2-link robot leg Constrained along slider In flight: 2 DOF ( q h, q i ) In stance: 1 DOF ( q h ) Input torque τ at the knee PAGE 39 / 25
Example: Hopping leg Modeling PAGE 40 / 25
Example: Hopping leg / Ref. Trajectory PAGE 41 / 25
Example: Hopping leg + Classic PD control In flight: K = 200 20 0 0 In stance: K = 200 20 PAGE 42 / 25
Example: Hopping leg + Hybrid PD control In flight: K = 200 20 0 0 In stance: K = 200 20 PAGE 43 / 25
Reference Spreading on a Humanoid Robot Rijnen, de Mooij, Traversaro, Nori, van de Wouw, Saccon, Nijmeijer Control of Humanoid Robot Motions with Impacts IEEE ICRA 2017 (accepted) PAGE 44
Conclusions Classic linear feedback can give poor tracking performance for systems with unilateral constraints Reference spreading control greatly improves tracking performance just by using a different notion of tracking error It works both for periodic and nonperiodic reference Physical experiments has shown that good velocity estimation at impact times is essential to achieve comparable performance with numerical simulations PAGE 45
Conclusions (cont d) Reference spreading can also be applied to multidomain hybrid systems where the state dimension (or the number of active constraints) changes at each event It has been demonstrated on mechanical systems with changing state dimension (multi-domain hybrid systems) such as a hopping robotic leg and a humanoid robot PAGE 46 / 25
Conclusions (cont d) Tuning of feedback gains can in principle be achieved via the use an LQR-like approach, where the timetriggered linearization of the hybrid system is used Surprisingly, the simple case of a bouncing mass, the optimal control problem can lead to negative stiffness gains The time-triggered linearization is a key ingredient in the stability proof