Reference Spreading Hybrid Control Exploiting Dynamic Contact - PowerPoint PPT Presentation

Reference Spreading Hybrid Control Exploiting Dynamic Contact Transitions in Robotics Applications Alessandro Saccon OptHySYS Workshop Trento, January 9-11, 2017

Robotic Locomotion and Manipulation HRP-2 Durus Atlas 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. OptHySYS Workshop / Trento, January 9-11, 2017 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 OptHySYS Workshop / Trento, January 9-11, 2017 PAGE 3

Modeling assumption • Mechanical system with unilateral constraints: X X M ( q )¨ q + C ( q, ˙ q ) ˙ q + G ( q ) = S τ + W N,i ( q ) λ N,i + W T,i ( q ) λ T,i i ∈ I N i ∈ I T subject to 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, … OptHySYS Workshop / Trento, January 9-11, 2017 PAGE 4 / 25

Reference Spreading Hybrid Control REFERENCE CLASSICAL REFERENCE SPREADING 2 2 2 1.5 q [m] q [m] 1 q [m] 1 1 0.5 0 0 0 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 5 5 − 0.5 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 q [m/s] 4 q [m/s] 0 0 2 ˙ q [m/s] ˙ 0 − 2 ˙ − 5 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 − 5 − 4 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 − 6 5 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 5 0 − 3 − 5 0 u [N] − 3.5 u [N] u [N] − 10 − 4 − 5 − 15 − 4.5 − 20 − 5 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 − 10 0 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 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 Silvio Traversaro TU/e Henk Nijmeijer IIT TU/e Nathan van de Wouw TU/e Francesco Nori IIT Herman Bruyninckx KU Leuven Nick Rosielle TU/e René van de Molengraft 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 OptHySYS Workshop / Trento, January 9-11, 2017

Reference Spreading Hybrid Control OptHySYS Workshop / Trento, January 9-11, 2017

Classic linear feedback + feedforward • Hybrid dynamics FLOW 𝑦̇ = 𝑔 𝑦, 𝑣, 𝑢 𝛿 𝑦, 𝑣, 𝑢 ≥ 0 𝑦 , = g 𝑦 . , 𝑢 𝛿 𝑦, 𝑣, 𝑢 = 0 JUMP • Track a reference state-input signal ( 𝛽 𝑢 , 𝜈(𝑢) ) with impact times 𝜐 4 , 𝜐 5 , … , 𝜐 7 OptHySYS Workshop / Trento, January 9-11, 2017 PAGE 9 / 25

Classic linear feedback + feedforward • System dynamics: (flow) 𝑦̇ = 𝑔 𝑦, 𝑣, 𝑢 𝛿 𝑦, 𝑣, 𝑢 ≥ 0 𝑦 , = g 𝑦 . , 𝑢 𝛿 𝑦, 𝑣, 𝑢 = 0 (j ump) • Track a reference state-input signal ( 𝛽 𝑢 , 𝜈(𝑢) ) with nominal jump times 𝜐 4 , 𝜐 5 , … , 𝜐 7 . • Classic linear feedback + feedforward 𝑣 = 𝜈 𝑢 + 𝐿 𝛽 𝑢 − 𝑦(𝑢) OptHySYS Workshop / Trento, January 9-11, 2017 PAGE 10 / 25

Classic linear feedback + feedforward (Drawback 1) Closed-loop and nominal event time mismatch are not considered OptHySYS Workshop / Trento, January 9-11, 2017 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 OptHySYS Workshop / Trento, January 9-11, 2017 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 ) ∈ R 2 q h ) ∈ R 4 ( q k , ˙ ( q k , q h , ˙ q k , ˙ OptHySYS Workshop / Trento, January 9-11, 2017 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 ( 𝑢 ∈ ℝ ) + event counter ( 𝑘 = 0,1,2, … ) • define extended reference trajectory OptHySYS Workshop / Trento, January 9-11, 2017 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 OptHySYS Workshop / Trento, January 9-11, 2017 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 OptHySYS Workshop / Trento, January 9-11, 2017 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 OptHySYS Workshop / Trento, January 9-11, 2017 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 OptHySYS Workshop / Trento, January 9-11, 2017 PAGE 18 / 25

Hybrid linear feedback + feedforward • System dynamics: 𝑦̇ B = 𝑔 B 𝑦 B , 𝑣, 𝑢 𝛿 B 𝑦 B , 𝑣, 𝑢 ≥ 0 (flow) 𝑦 B, = g B 𝑦 B. , 𝑢 𝛿 B 𝑦 B , 𝑣, 𝑢 = 0 (jump) • Track a reference state-input signal ( 𝛽 𝑢, 𝑘 , 𝜈(𝑢) ) with impact times 𝜐 4 , 𝜐 5 , … , 𝜐 7 • Apply control input 𝑣 = 𝜈 𝑢 + 𝐿 𝑢, 𝑘 𝛽 𝑢, 𝑘 − 𝑦 𝑢, 𝑘 OptHySYS Workshop / Trento, January 9-11, 2017 PAGE 19 / 25

The Actuated Rebounding Pendulum

The Actuated Rebounding Pendulum (ARP) OptHySYS Workshop / Trento, January 9-11, 2017 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 - ) OptHySYS Workshop / Trento, January 9-11, 2017 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 Reference 0.05 τ [Nm] ω n = 2 π /0.5 0 ω n = 2 π /1 -0.05 ω n = 2 π /2 -0.1 0.6 q [rad] 0.4 0.2 ˜ 0 5 v [rad/s] 0 -5 ˆ -10 0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2 Time [s] OptHySYS Workshop / Trento, January 9-11, 2017 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 Reference 0.05 τ [Nm] ω n = 2 π /0.5 0 ω n = 2 π /1 -0.05 ω n = 2 π /2 -0.1 0.6 q [rad] 0.4 0.2 ˜ 0 5 v [rad/s] 0 -5 ˆ -10 0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2 Time [s] OptHySYS Workshop / Trento, January 9-11, 2017 PAGE 24

Computing Optimal Tracking Gains OptHySYS Workshop / Trento, January 9-11, 2017

Example: Actuated Rebounding Cart • Actuated bouncing mass 0 𝑦̇ = 0 1 0 𝑦 + 𝑣 ⁄ 1 𝑛 0 • Partially elastic impacts Newton’s law of restitution: 𝑦 , = 1 0 −𝑓 𝑦 . 0 𝑛 = 1 kg 𝑓 = 0.95 𝐿 = 25 10 OptHySYS Workshop / Trento, January 9-11, 2017 PAGE 26

Example: Actuated Rebounding Cart Classic PD Reference Spreading PD OptHySYS Workshop / Trento, January 9-11, 2017 PAGE 27

How to tune the feedback gains ? First steps • Minimize cost on state and input (think to LQR…) Nonlinear hybrid optimal • BUT closed loop event time control problem are not known in advance • IDEA: Approximate the dynamics about the reference using a hybrid linearization (a TIME TRIGGERED, jumping linear system) • Closed loop solution with 𝑣 = 𝜈(𝑢) + 𝑤 is approximated by 𝑦 𝑢, 𝑘 = 𝛽 J 𝑢, 𝑘 + 𝑨̅ 𝑢, 𝑘 + 𝑝 𝑨 N , 𝑤 OptHySYS Workshop / Trento, January 9-11, 2017 PAGE 28