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

Durus Atlas Mabel Amigo iCub Walkman HRP-2

Research Focus

PAGE 2

• Development
• f

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

Research focus (cont’d)

• Dynamic Motion with Dynamic Contact Transitions
• Performance (and robustness)
• Energy efficiency
• Robotic systems with complex kinematics

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

OptHySYS Workshop / Trento, January 9-11, 2017

like

Modeling assumption

• Mechanical system with unilateral constraints:

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

M(q)¨ q + C(q, ˙ q) ˙ q + G(q) = Sτ + X

i∈IN

WN,i(q)λN,i + X

i∈IT

WT,i(q)λT,i

Reference Spreading Hybrid Control

Preliminary successful experiments conducted on at 1DOF setup at that time

• 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

0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 −0.5 0.5 1 1.5 2

q [m]

0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 −6 −4 −2 2 4

˙ q [m/s]

0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 −5 −4.5 −4 −3.5 −3

u [N] 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 1 2 q [m] 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 −5 5 ˙ q [m/s] 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 −20 −15 −10 −5 5 u [N] 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 1 2 q [m] 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 −5 5 ˙ q [m/s] 0.5 1 1.5 2 2.5 3 3.5 4 4.5 5 −10 −5 5 u [N]

REFERENCE CLASSICAL REFERENCE SPREADING

PAGE 5

Original idea came from sensitivity analysis / notion of time-triggered linearization

Main collaborators on this research topics

Herman Bruyninckx KU Leuven Nathan van de Wouw TU/e René van de Molengraft TU/e Mark Rijnen TU/e Francesco Nori IIT Henk Nijmeijer TU/e Silvio Traversaro IIT 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

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 JUMP 𝑦, = g 𝑦., 𝑢 𝛿 𝑦, 𝑣, 𝑢 = 0

• Track a reference state-input signal (𝛽 𝑢 , 𝜈(𝑢))

with impact times 𝜐4, 𝜐5, … , 𝜐7

PAGE 9 / 25

OptHySYS Workshop / Trento, January 9-11, 2017

Classic linear feedback + feedforward

• System dynamics:

(flow) 𝑦̇ = 𝑔 𝑦, 𝑣, 𝑢 𝛿 𝑦, 𝑣, 𝑢 ≥ 0 (jump) 𝑦, = g 𝑦., 𝑢 𝛿 𝑦, 𝑣, 𝑢 = 0

• Track a reference state-input signal

(𝛽 𝑢 , 𝜈(𝑢)) with nominal jump times 𝜐4, 𝜐5, … , 𝜐7.

• Classic linear feedback + feedforward

𝑣 = 𝜈 𝑢 + 𝐿 𝛽 𝑢 − 𝑦(𝑢)

PAGE 10 / 25

OptHySYS Workshop / Trento, January 9-11, 2017

Classic linear feedback + feedforward

(Drawback 1) Closed-loop and nominal event time mismatch are not considered

PAGE 11 / 25

OptHySYS Workshop / Trento, January 9-11, 2017

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

OptHySYS Workshop / Trento, January 9-11, 2017

Classic linear feedback + feedforward

(Drawback 2) Cannot deal with changing state dimension i.e. with multi-domain hybrid systems

PAGE 13 / 25

(qk, ˙ qk) ∈ R2 (qk, qh, ˙ qk, ˙ qh) ∈ R4

stance flight

OptHySYS Workshop / Trento, January 9-11, 2017

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

PAGE 14 / 25

OptHySYS Workshop / Trento, January 9-11, 2017

Hybrid linear feedback + feedforward

PAGE 15 / 25

• 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

Hybrid linear feedback + feedforward

PAGE 16 / 25

• 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

Hybrid linear feedback + feedforward

PAGE 17 / 25

• 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

Hybrid linear feedback + feedforward

PAGE 18 / 25

• 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

Hybrid linear feedback + feedforward

• System dynamics:

(flow) 𝑦̇ B = 𝑔B 𝑦B, 𝑣, 𝑢 𝛿B 𝑦B, 𝑣, 𝑢 ≥ 0 (jump) 𝑦B, = gB 𝑦B., 𝑢 𝛿B 𝑦B, 𝑣, 𝑢 = 0

• Track a reference state-input signal (𝛽 𝑢, 𝑘 , 𝜈(𝑢)) with

impact times 𝜐4, 𝜐5, … , 𝜐7

• Apply control input

𝑣 = 𝜈 𝑢 + 𝐿 𝑢, 𝑘 𝛽 𝑢, 𝑘 − 𝑦 𝑢, 𝑘

PAGE 19 / 25

OptHySYS Workshop / Trento, January 9-11, 2017

The Actuated Rebounding Pendulum

The Actuated Rebounding Pendulum (ARP)

PAGE 21

OptHySYS Workshop / Trento, January 9-11, 2017

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

OptHySYS Workshop / Trento, January 9-11, 2017

Standard PD

PAGE 23

1 2 3 4 5 6 7 8 1 2 3 4 5 6 7 8 1 2 3 4 5 6 7 8 1 2 3 4 5 6 7 8 9 10 11

τ [Nm]

• 0.1
• 0.05

0.05

Reference ωn = 2π/0.5 ωn = 2π/1 ωn = 2π/2

Time [s]

0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2

ˆ v [rad/s]

• 10
• 5

5

˜ q [rad]

0.2 0.4 0.6

OptHySYS Workshop / Trento, January 9-11, 2017

• Ref. Spreading

PAGE 24

1 2 3 4 5 6 7 8 1 2 3 4 5 6 7 8 1 2 3 4 5 6 7 8 1 2 3 4 5 6 7 8

τ [Nm]

• 0.1
• 0.05

0.05

Reference ωn = 2π/0.5 ωn = 2π/1 ωn = 2π/2

Time [s]

0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.8 2

ˆ v [rad/s]

• 10
• 5

5

˜ q [rad]

0.2 0.4 0.6

OptHySYS Workshop / Trento, January 9-11, 2017

Computing Optimal Tracking Gains

OptHySYS Workshop / Trento, January 9-11, 2017

Example: Actuated Rebounding Cart

• Actuated bouncing mass

𝑦̇ = 0 1 0 𝑦 + 1 𝑛 ⁄ 𝑣

• Partially elastic impacts

Newton’s law of restitution:

𝑦, = 1 −𝑓 𝑦. 𝑛 = 1 kg 𝑓 = 0.95 𝐿 = 25 10

PAGE 26

OptHySYS Workshop / Trento, January 9-11, 2017

Example: Actuated Rebounding Cart

Classic PD Reference Spreading PD

PAGE 27

OptHySYS Workshop / Trento, January 9-11, 2017

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

PAGE 28

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 𝑣 = 𝜈(𝑢) + 𝑤 is approximated by

𝑦 𝑢, 𝑘 = 𝛽 J 𝑢, 𝑘 + 𝑨̅ 𝑢, 𝑘 + 𝑝 𝑨N , 𝑤

OptHySYS Workshop / Trento, January 9-11, 2017

How to tune the feedback gains ? First steps

PAGE 29

STATE TRIGGERED TIME TRIGGERED LIN. 𝑦̇ = 𝑔 𝑦, 𝑣, 𝑢 𝑨̇ = 𝐵 𝑢 𝑨 + 𝐶 𝑢 𝑤 𝑦, = 𝑕 𝑦., 𝑢 , 𝑢 = 𝑢𝑗 𝑨,= 𝐻(𝑢)𝑨., 𝑢 = 𝜐T 𝑦 𝑢, 𝑘 = 𝛽 J 𝑢, 𝑘 + 𝑨̅ 𝑢, 𝑘 + 𝑝 𝑨N , 𝑤

OptHySYS Workshop / Trento, January 9-11, 2017

The time triggered linearization

• Linearization jumps at the reference event times

→ linear time-triggered hybrid system

PAGE 30 / 36

OptHySYS Workshop / Trento, January 9-11, 2017

The time triggered linearization (cont’d)

• Linearization jumps at the reference event times

→ linear time-triggered hybrid system

PAGE 31 / 36

OptHySYS Workshop / Trento, January 9-11, 2017

The time triggered linearization (cont’d)

• Linearization jumps at the reference event times

→ linear time-triggered hybrid system

PAGE 32 / 36

OptHySYS Workshop / Trento, January 9-11, 2017

LQR-like Optimal control

• Optimization problem:

min

X

1 2 Y 𝑨Z𝑅𝑨 + 𝑤Z𝑆𝑤

Z N

𝑒𝑡 + 1 2 𝑨 𝑈 Z𝑄Z𝑨(𝑈) s.t. 𝑨̇ = 𝐵 𝑢 𝑨 + 𝐶 𝑢 𝑤 𝑢 ≠ 𝜐T 𝑨, = 𝐻(𝑢)𝑨. 𝑢 = 𝜐T 𝑨 𝑢N = 𝑨N

PAGE 33

OptHySYS Workshop / Trento, January 9-11, 2017

LQR-like Optimal control (cont’d)

• Optimization problem:

min

X

1 2 Y 𝑨Z𝑅𝑨 + 𝑤Z𝑆𝑤

Z N

𝑒𝑡 + 1 2 𝑨 𝑈 Z𝑄Z𝑨(𝑈) s.t. 𝑨̇ = 𝐵 𝑢 𝑨 + 𝐶 𝑢 𝑤 𝑢 ≠ 𝜐T 𝑨, = 𝐻(𝑢)𝑨. 𝑢 = 𝜐T 𝑨 𝑢N = 𝑨N

(SOLUTION)

𝑤 = −𝑆.4𝐶 𝑢 Z𝑄 𝑢 𝑨 = −𝐿 𝑢 𝑨 𝑄 = 𝑄Z 𝑢 = 𝑈 −𝑄̇ = 𝐵 𝑢 Z + 𝑄𝐵 𝑢 − 𝑄𝐶 𝑢 𝑆.4𝐶 𝑢 Z𝑄 + 𝑅 𝑢 ≠ 𝜐T 𝑄.= 𝑎Z 𝑢 𝑄,𝑎 𝑢 𝑢 = 𝜐T

PAGE 34

OptHySYS Workshop / Trento, January 9-11, 2017

Optimal control: Actuated Rebounding Cart

• Feedback gains for example reference (bouncing)

𝐿 = 𝐿4 𝐿5 𝑆 = 1, 𝑅 = 50 50 𝑄Z = 56.6 7.1 7.1 8.0

• 𝐿4 changes sign!
• Rijnen, Saccon, Nijmeijer, IEEE CDC 2015

PAGE 35

OptHySYS Workshop / Trento, January 9-11, 2017

Optimal control: const. vs optimal fdbk gains

PAGE 36

OptHySYS Workshop / Trento, January 9-11, 2017

Multi-domain Trajectory Tracking

Example: Hopping leg

PAGE 38 / 25

IIT, Genova – ADVR group

OptHySYS Workshop / Trento, January 9-11, 2017

Rijnen, van Rijn, Dallali, Saccon, Nijmeijer, IFAC PSYCO 2016

Example: Hopping leg → Modeling

• 2-link robot leg
• Constrained along slider
• In flight: 2 DOF ( 𝑟h, 𝑟i )
• In stance: 1 DOF ( 𝑟h )
• Input torque 𝜐 at the knee

PAGE 39 / 25

OptHySYS Workshop / Trento, January 9-11, 2017

Example: Hopping leg → Modeling

PAGE 40 / 25

OptHySYS Workshop / Trento, January 9-11, 2017

Example: Hopping leg / Ref. Trajectory

PAGE 41 / 25

OptHySYS Workshop / Trento, January 9-11, 2017

Example: Hopping leg + Classic PD control

PAGE 42 / 25

In flight: 𝐿 = 200 20 In stance: 𝐿 = 200 20

OptHySYS Workshop / Trento, January 9-11, 2017

Example: Hopping leg + Hybrid PD control

PAGE 43 / 25

In flight: 𝐿 = 200 20 In stance: 𝐿 = 200 20

OptHySYS Workshop / Trento, January 9-11, 2017

Reference Spreading on a Humanoid Robot

PAGE 44

OptHySYS Workshop / Trento, January 9-11, 2017

Rijnen, de Mooij, Traversaro, Nori, van de Wouw, Saccon, Nijmeijer Control of Humanoid Robot Motions with Impacts IEEE ICRA 2017 (accepted)

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

OptHySYS Workshop / Trento, January 9-11, 2017

PAGE 45

Conclusions (cont’d)

• Reference spreading can also be applied to multi-

domain 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

OptHySYS Workshop / Trento, January 9-11, 2017

PAGE 46 / 25

Conclusions (cont’d)

• Tuning of feedback gains can in principle be achieved

via the use an LQR-like approach, where the ”time- triggered linearization” of the hybrid system is used

• Surprisingly, the simple case of a bouncing mass, the
• ptimal control problem can lead to negative stiffness

gains

• The time-triggered linearization is a key ingredient in the

stability proof

OptHySYS Workshop / Trento, January 9-11, 2017