On the Feedback Linearization of Robots with Variable Joint Stiffness and Other Stories...

Transcription

1 On the Feedback Linearization of Robots with Variable Joint Stiffness and Other Stories... Claudio Melchiorri Dip. di Ingegneria dell Energia Elettrica e dell Informazione G. Marconi Alma Mater Studiorum - Università di Bologna C. Melchiorri (DEI) ADL Surprise Festschrift 1 / 37

2 ADL Surprise Festschrift Other stories Other stories... C. Melchiorri (DEI) ADL Surprise Festschrift 2 / 37

3 ADL Surprise Festschrift Other stories ADL - Unibo Cooperation with ADL started some time at the end of the 80 s... NATO Robotics workshop, Il Ciocco, July 4-6, 1988 (??) Italian workshop on Robotics, Rome, March 30, 1990 C. Melchiorri (DEI) ADL Surprise Festschrift 3 / 37

4 ADL Surprise Festschrift Other stories ADL - Unibo INCONTRO DI STUDIO ROBOTICA ROMA, 30 MARZO 1990 Programma di massima: 1 Parte: Roma 11:30 C. Manes, ``Controllo ibrido'' 12:00 L. Lanari, ``Modellistica e controllo di robot con bracci flessibili'' 12:30 G. Oriolo ``Algoritmi risolutivi della ridondanza'' 2 Parte: Bologna 14:00 R. Zanasi, ``Controllo Binario: simulazione e progettazione assistita'' 14:30 A. Tonielli, C. Rossi, ``Controllo Sliding Mode di motori in corrente alternata con pilotaggio diretto dell'inverter'' 15:00 C. Melchiorri, ``Considerazioni sull'uso di criteri di norma minima nella soluzione di problemi cinematici''. 3 Parte: Napoli 15:30 P. Chiacchio, ``Analisi cinetostatica di bracci cooperanti'' 16:00 B. Siciliano, "Modellistica e controllo di bracci con elementi flessibili'' 16:30 S. Chiaverini, ``Approccio parallelo al controllo di forza e posizione'' C. Melchiorri (DEI) ADL Surprise Festschrift 4 / 37

5 ADL Surprise Festschrift Other stories ADL - Unibo J = a 1s 1 a 2s 12 dc 12 a 2s 12 dc 12 s 12 a 1c 1 + a 2c 12 ds 12 a 2c 12 ds 12 c θ 1 = 20 [deg] θ 2 = 20 [deg] d = 0.5 [m] = 50 [cm] v [m] = [2, 4, 0] T [m/s] = [200, 400, 0] T [cm/s] = v [cm], q = J + v =?? J + [m] = q [m] = J + [m] v [m] = [1/s] [1/s] [m/s] v [m] = J q [m] = 2 [m/s] 4 [m/s] 0 [m/s] J + [cm] = q [cm] = J + [cm] v [cm] = [1/s] [1/s] [cm/s] v [cm] = J q [cm] = 200 [cm/s] 400 [cm/s] 0 [cm/s] C. Melchiorri (DEI) ADL Surprise Festschrift 5 / 37

6 ADL Surprise Festschrift Other stories ADL - Unibo Invariant properties of hybrid position/force control: A. De Luca, C. Manes, and F. Nicolò, 1988, A task space decoupling approach to hybrid control of manipulators, 2nd IFAC Symp. on Robot Control, SYROCO 88, pp , Karlsruhe, F.R.G. C. Melchiorri (DEI) ADL Surprise Festschrift 6 / 37

7 ADL Surprise Festschrift Other stories ADL - Unibo Research projects: Development of an Integrated Mobile Manipulator for Planetary Exploration Tasks, ASI ( ) RAMSETE: Articulated and Mobile Robotics for SErvice and TEchnology, MURST ( ) MISTRAL: Methodologies and Integration of Subsystems and Technologies for Anthropic Robotics and Locomotion, MIUR ( ) MATRICS: Methodologies Applications and Technologies for Robot Interaction Cooperation and Supervision, MIUR ( ) SICURA: Safe Physical Interaction between Robots and Humans, MIUR ( ) C. Melchiorri (DEI) ADL Surprise Festschrift 7 / 37

8 ADL Surprise Festschrift Other stories ADL - Unibo IEEE: Editorial Board, IEEE Transactions on Robotics and Automation ICRA 2007 in Rome C. Melchiorri (DEI) ADL Surprise Festschrift 8 / 37

9 z ADL Surprise Festschrift Other stories z ADL - Unibo Trajectory planning: A. De Luca, L. Lanari, G. Oriolo, Generation and computation of optimal smooth trajectories for robot arms Rapporto DIS 13.88, Università di Roma La Sapienza, Sep A. De Luca, A spline trajectory generator for robot arms, Report RAL 68, Rensselaer Polytechnic Institute, Apr y x y x C. Melchiorri (DEI) ADL Surprise Festschrift 9 / 37

10 ADL Surprise Festschrift Other stories ADL - Unibo Trajectory planning: A. De Luca, L. Lanari, G. Oriolo, Generation and computation of optimal smooth trajectories for robot arms Rapporto DIS 13.88, Università di Roma La Sapienza, Sep A. De Luca, A spline trajectory generator for robot arms, Report RAL 68, Rensselaer Polytechnic Institute, Apr C. Melchiorri (DEI) ADL Surprise Festschrift 10 / 37

11 ADL Surprise Festschrift Other stories ADL - Unibo Il tempo totale di percorrenza della spline è dato da n 1 T = T k = t n t 1 k=1 È possibile impostare un problema di ottimo che minimizza il tempo di percorrenza. Si tratta di calcolare i valori T k in modo da minimizzare T, con i vincoli sulle massime velocità ed accelerazioni ai giunti. Formalmente, il problema si imposta come min Tk tale che T = n 1 k=1 T k q(τ, T k ) < v max τ [0T ] q(τ, T k ) < a max τ [0T ] ed è quindi un problema di ottimo non lineare con funzione obiettivo lineare, risolvibile con tecniche classiche della ricerca operativa. C. Melchiorri (DEI) ADL Surprise Festschrift 11 / 37

12 ADL Surprise Festschrift Other stories ADL - Unibo Si vuole determinare la spline passante per i punti q 1 = 0, q 2 = 2, q 3 = 12, q 4 = 5, che minimizza il tempo di percorrenza totale T rispettando i seguenti vincoli: v max = 3, a max = 2. Occorre risolvere il seguente problema di ottimizzazione non lineare: soggetto ai vincoli riportati di seguito. min T = T 1 + T 2 + T 3 Risolvendo questo problema (p.e. con Optimization Toolbox di MATLAB) si ottengono i seguenti valori: T 1 = , T 2 = , T 3 = , T = sec C. Melchiorri (DEI) ADL Surprise Festschrift 12 / 37

13 ADL Surprise Festschrift Other stories ADL - Unibo Vincoli del problema di ottimizzazione: a 11 v max (velocità iniziale del I tratto v max ) a 21 v max (velocità iniziale del II tratto v max ) a 31 v max (velocità iniziale del III tratto v max ) a a 12T 1 + 3a 13T 2 1 v max (velocità finale del I tratto v max ) a a 22T 2 + 3a 23T 2 2 v max (velocità finale del II tratto v max ) a a 32T 3 + 3a 33T 2 3 v max (velocità finale del III tratto v max ) ) a a 12 ( a 12 3a 13 ) a a 22 ( a 22 3a 23 ) a a 32 ( a 32 3a a 13 ( a 12 3a 13 ) 2 v max (velocità all interno del I tratto v max ) + 3a 23 ( a 22 3a 23 ) 2 v max (velocità all interno del II tratto v max ) + 3a 33 ( a 32 3a 33 ) 2 v max (velocità all interno del III tratto v max ) 2a 12 a max (accelerazione iniziale del I tratto a max ) 2a 22 a max (accelerazione iniziale del II tratto a max ) 2a 32 a max (accelerazione iniziale del III tratto a max ) 2a a 13T 1 a max (accelerazione finale del I tratto a max ) 2a a 23T 2 a max (accelerazione finale del II tratto a max ) 2a a 33T 3 a max (accelerazione finale del III tratto a max ) C. Melchiorri (DEI) ADL Surprise Festschrift 13 / 37

14 ADL Surprise Festschrift Other stories ADL - Unibo Sloshing suppression - movies C. Melchiorri (DEI) ADL Surprise Festschrift 14 / 37

15 ADL Surprise Festschrift Other stories ADL - Unibo Repetitive control & Splines: C. Melchiorri (DEI) ADL Surprise Festschrift 15 / 37

16 ADL Surprise Festschrift Other stories ADL - Unibo C. Melchiorri (DEI) ADL Surprise Festschrift 16 / 37

17 ADL Surprise Festschrift Other stories ADL - Unibo C. Melchiorri (DEI) ADL Surprise Festschrift 17 / 37

18 ADL Surprise Festschrift Other stories ADL - Unibo C. Melchiorri (DEI) ADL Surprise Festschrift 18 / 37

19 ADL Surprise Festschrift Other stories ADL - Unibo C. Melchiorri (DEI) ADL Surprise Festschrift 19 / 37

20 ADL Surprise Festschrift ADL Surprise Festschrift Thanks Alex!! C. Melchiorri (DEI) ADL Surprise Festschrift 20 / 37

21 Variable Joint Stiffness Robot Dynamics Robot Dynamic Model Dynamic Model of Robots with Variable Joint Stiffness On the Feedback Linearization of Robots with Variable Joint Stiffness G. Palli 1, C. Melchiorri 1, A. De Luca 2 1 Dip. di Ingegneria dell Energia Elettrica e dell Informazione G. Marconi Alma Mater Studiorum - Università di Bologna 2 Dip. di Ingegneria Informatica Automatica e Gestionale A. Ruberti Università di Roma La Sapienza C. Melchiorri (DEI) ADL Surprise Festschrift 21 / 37

22 Variable Joint Stiffness Robot Dynamics Robot Dynamic Model Dynamic Model of Robots with Variable Joint Stiffness Robot (+actuators) dynamic equations M(q) q + N(q, q) + K (q θ) = 0 B θ + K (θ q) = τ The diagonal joint stiffness matrix is considered time-variant K = diag{k 1,..., k n }, K = K(t) > 0 Alternative notation K (q θ) = Φk, Φ = diag{(q 1 θ 1 ),..., (q n θ n )}, k = [k 1,..., k n ] T 1 The joint stiffness k can be directly changed by means of a (suitably scaled) additional command τ k k = τ k 2 Alternatively, the variation of joint stiffness may be modeled as a second-order dynamic system k = φ(x, k, k, τ k ) C. Melchiorri (DEI) ADL Surprise Festschrift 22 / 37

23 Variable Joint Stiffness Robot Dynamics Robot Dynamic Model Dynamic Model of Robots with Variable Joint Stiffness Robot (+actuators) dynamic equations M(q) q + N(q, q) + K (q θ) = 0 B θ + K (θ q) = τ The diagonal joint stiffness matrix is considered time-variant K = diag{k 1,..., k n }, K = K(t) > 0 Alternative notation K (q θ) = Φk, Φ = diag{(q 1 θ 1 ),..., (q n θ n )}, k = [k 1,..., k n ] T 1 The joint stiffness k can be directly changed by means of a (suitably scaled) additional command τ k k = τ k 2 Alternatively, the variation of joint stiffness may be modeled as a second-order dynamic system k = φ(x, k, k, τ k ) C. Melchiorri (DEI) ADL Surprise Festschrift 22 / 37

24 Variable Joint Stiffness Robot Dynamics Robot Dynamic Model Dynamic Model of Robots with Variable Joint Stiffness Robot (+actuators) dynamic equations M(q) q + N(q, q) + K (q θ) = 0 B θ + K (θ q) = τ The diagonal joint stiffness matrix is considered time-variant K = diag{k 1,..., k n }, K = K(t) > 0 Alternative notation K (q θ) = Φk, Φ = diag{(q 1 θ 1 ),..., (q n θ n )}, k = [k 1,..., k n ] T 1 The joint stiffness k can be directly changed by means of a (suitably scaled) additional command τ k k = τ k 2 Alternatively, the variation of joint stiffness may be modeled as a second-order dynamic system k = φ(x, k, k, τ k ) C. Melchiorri (DEI) ADL Surprise Festschrift 22 / 37

25 Variable Joint Stiffness Robot Dynamics Robot Dynamic Model Dynamic Model of Robots with Variable Joint Stiffness The input u and the robot state x are: [ ] τ u = R 2n, x = [ q T q T θ ] T T θt R 4n τk In the case of second-order stiffness variation model, the state vector of the robot becomes: x e = [ q T q T θ T θt k T kt ] T R 6n In any case, the goal will be to simultaneously control the following set of outputs [ ] q y = R 2n k namely the link positions (and thus, through the robot direct kinematics, the end-effector pose) and the joint stiffness C. Melchiorri (DEI) ADL Surprise Festschrift 23 / 37

26 Variable Joint Stiffness Robot Dynamics Robot Dynamic Model Dynamic Model of Robots with Variable Joint Stiffness The input u and the robot state x are: [ ] τ u = R 2n, x = [ q T q T θ ] T T θt R 4n τk In the case of second-order stiffness variation model, the state vector of the robot becomes: x e = [ q T q T θ T θt k T kt ] T R 6n In any case, the goal will be to simultaneously control the following set of outputs [ ] q y = R 2n k namely the link positions (and thus, through the robot direct kinematics, the end-effector pose) and the joint stiffness C. Melchiorri (DEI) ADL Surprise Festschrift 23 / 37

27 Variable Joint Stiffness Robot Dynamics Robot Dynamic Model Dynamic Model of Robots with Variable Joint Stiffness The input u and the robot state x are: [ ] τ u = R 2n, x = [ q T q T θ ] T T θt R 4n τk In the case of second-order stiffness variation model, the state vector of the robot becomes: x e = [ q T q T θ T θt k T kt ] T R 6n In any case, the goal will be to simultaneously control the following set of outputs [ ] q y = R 2n k namely the link positions (and thus, through the robot direct kinematics, the end-effector pose) and the joint stiffness C. Melchiorri (DEI) ADL Surprise Festschrift 23 / 37

28 Inverse Dynamics of Variable Stiffness Robots Computing Actuator Commands Dynamic Inversion The motion is specified in terms of a desired smooth position trajectory q = q d (t) and joint stiffness matrix K = K d (t) (or, equivalently, of the vector k = k d (t)) Assuming k = τ k, we have simply τ k,d = k d (t) and only the computation of the nominal motor torque τ d is of actual interest The robot dynamic equation is differentiated twice with respect to time and M(q) q [3] + M(q) q + N(q, q) + K (q θ) + K ( q θ) = 0 M(q) q [4] + 2 M(q) q [3] + M(q) q + N(q, q) + + K ( q θ) + 2 K ( q θ) + K (q θ) = 0 C. Melchiorri (DEI) ADL Surprise Festschrift 24 / 37

29 Inverse Dynamics of Variable Stiffness Robots Computing Actuator Commands Dynamic Inversion The motion is specified in terms of a desired smooth position trajectory q = q d (t) and joint stiffness matrix K = K d (t) (or, equivalently, of the vector k = k d (t)) Assuming k = τ k, we have simply τ k,d = k d (t) and only the computation of the nominal motor torque τ d is of actual interest The robot dynamic equation is differentiated twice with respect to time and M(q) q [3] + M(q) q + N(q, q) + K (q θ) + K ( q θ) = 0 M(q) q [4] + 2 M(q) q [3] + M(q) q + N(q, q) + + K ( q θ) + 2 K ( q θ) + K (q θ) = 0 C. Melchiorri (DEI) ADL Surprise Festschrift 24 / 37

30 Inverse Dynamics of Variable Stiffness Robots Computing Actuator Commands Dynamic Inversion The motion is specified in terms of a desired smooth position trajectory q = q d (t) and joint stiffness matrix K = K d (t) (or, equivalently, of the vector k = k d (t)) Assuming k = τ k, we have simply τ k,d = k d (t) and only the computation of the nominal motor torque τ d is of actual interest The robot dynamic equation is differentiated twice with respect to time and M(q) q [3] + M(q) q + N(q, q) + K (q θ) + K ( q θ) = 0 M(q) q [4] + 2 M(q) q [3] + M(q) q + N(q, q) + + K ( q θ) + 2 K ( q θ) + K (q θ) = 0 C. Melchiorri (DEI) ADL Surprise Festschrift 24 / 37

31 Inverse Dynamics of Variable Stiffness Robots Computing Actuator Commands Dynamic Inversion Reference motor position along the desired robot trajectory Reference motor velocity θ d = q d + K 1 d (M(q d ) q d + N(q d, q d )). θ d = q d + K 1 d Actuators dynamic model inversion ( M(q d )q [3] d + K d K 1 d (M(q d ) q d + N(q d, q d )) θ = B 1 [τ K(θ q)], M(q d ) q d + N(q d, q d ) ). C. Melchiorri (DEI) ADL Surprise Festschrift 25 / 37

32 Inverse Dynamics of Variable Stiffness Robots Computing Actuator Commands Actuator Torques Computation Reference motor torque along the desired trajectory ( τ d = M(q d ) q d + N(q d, q d ) + BK 1 d α d q d, q d, q d, q [3] d, q[4] d, k d, k d, k ) d Some minimal smoothness requirements are imposed q d (t) C 4 and k d (t) C 2 Discontinuous models of friction or actuator dead-zones on the motor side can be considered without problems Discontinuous phenomena acting on the link side should be approximated by a smooth model The command torques τ d can be kept within the saturation limits by a suitable time scaling of the manipulator trajectory C. Melchiorri (DEI) ADL Surprise Festschrift 26 / 37

33 Inverse Dynamics of Variable Stiffness Robots Computing Actuator Commands Actuator Torques Computation Reference motor torque along the desired trajectory ( τ d = M(q d ) q d + N(q d, q d ) + BK 1 d α d q d, q d, q d, q [3] d, q[4] d, k d, k d, k ) d Some minimal smoothness requirements are imposed q d (t) C 4 and k d (t) C 2 Discontinuous models of friction or actuator dead-zones on the motor side can be considered without problems Discontinuous phenomena acting on the link side should be approximated by a smooth model The command torques τ d can be kept within the saturation limits by a suitable time scaling of the manipulator trajectory C. Melchiorri (DEI) ADL Surprise Festschrift 26 / 37

34 Inverse Dynamics of Variable Stiffness Robots Computing Actuator Commands Actuator Torques Computation Reference motor torque along the desired trajectory ( τ d = M(q d ) q d + N(q d, q d ) + BK 1 d α d q d, q d, q d, q [3] d, q[4] d, k d, k d, k ) d Some minimal smoothness requirements are imposed q d (t) C 4 and k d (t) C 2 Discontinuous models of friction or actuator dead-zones on the motor side can be considered without problems Discontinuous phenomena acting on the link side should be approximated by a smooth model The command torques τ d can be kept within the saturation limits by a suitable time scaling of the manipulator trajectory C. Melchiorri (DEI) ADL Surprise Festschrift 26 / 37

35 Inverse Dynamics of Variable Stiffness Robots Computing Actuator Commands Actuator Torques Computation Reference motor torque along the desired trajectory ( τ d = M(q d ) q d + N(q d, q d ) + BK 1 d α d q d, q d, q d, q [3] d, q[4] d, k d, k d, k ) d Some minimal smoothness requirements are imposed q d (t) C 4 and k d (t) C 2 Discontinuous models of friction or actuator dead-zones on the motor side can be considered without problems Discontinuous phenomena acting on the link side should be approximated by a smooth model The command torques τ d can be kept within the saturation limits by a suitable time scaling of the manipulator trajectory C. Melchiorri (DEI) ADL Surprise Festschrift 26 / 37

36 Feedback Linearization Static Feedback Linearization Second-Order Stiffness Model The dynamics of the joint stiffness k is written as a generic nonlinear function of the system configuration k = β(q, θ) + γ(q, θ) τ k Double differentiation wrt time of the robot dynamics M q [4] + 2 M q [3] + M q + N + K ( q B 1 [τ K(θ q)] ) where both the inputs τ and τ k appear + 2 K ( q θ) + Φ (β + γ τ k ) = 0 C. Melchiorri (DEI) ADL Surprise Festschrift 27 / 37

37 Feedback Linearization Static Feedback Linearization Feedback Linearized Model The overall system can be written as [ ] [ q [4] α(xe ) = k β(q, θ) ] [ τ + Q(x e ) τ k where Q(x e ) is the decoupling matrix: [ ] M Q(x e ) = 1 KB 1 M 1 Φ γ(q, θ) 0 n n γ(q, θ) } k Non-Singularity Conditions i > 0 i = 1,..., n γ i (q i, θ i ) 0 By applying the static state feedback [ ] ( [ τ = Q 1 α(xe ) (x e ) β(q, θ) τ k the full feedback linearized model is obtained [ ] [ ] q [4] vq = k v k ] ] [ vq + v k C. Melchiorri (DEI) ADL Surprise Festschrift 28 / 37 ])

38 Feedback Linearization Static Feedback Linearization Feedback Linearized Model The overall system can be written as [ ] [ q [4] α(xe ) = k β(q, θ) ] [ τ + Q(x e ) τ k where Q(x e ) is the decoupling matrix: [ ] M Q(x e ) = 1 KB 1 M 1 Φ γ(q, θ) 0 n n γ(q, θ) } k Non-Singularity Conditions i > 0 i = 1,..., n γ i (q i, θ i ) 0 By applying the static state feedback [ ] ( [ τ = Q 1 α(xe ) (x e ) β(q, θ) τ k the full feedback linearized model is obtained [ ] [ ] q [4] vq = k v k ] ] [ vq + v k C. Melchiorri (DEI) ADL Surprise Festschrift 28 / 37 ])

39 Feedback Linearization Static Feedback Linearization Feedback Linearized Model The overall system can be written as [ ] [ q [4] α(xe ) = k β(q, θ) ] [ τ + Q(x e ) τ k where Q(x e ) is the decoupling matrix: [ ] M Q(x e ) = 1 KB 1 M 1 Φ γ(q, θ) 0 n n γ(q, θ) } k Non-Singularity Conditions i > 0 i = 1,..., n γ i (q i, θ i ) 0 By applying the static state feedback [ ] ( [ τ = Q 1 α(xe ) (x e ) β(q, θ) τ k the full feedback linearized model is obtained [ ] [ ] q [4] vq = k v k ] ] [ vq + v k C. Melchiorri (DEI) ADL Surprise Festschrift 28 / 37 ])

40 Feedback Linearization Dynamic Feedback Linearization Dynamic Feedback Linearization Considering the very simple stiffness variation model k i = τ ki the dynamics of the system becomes: [ ] [ ] [ ] [ ] q M = 1 N 0n n M + 1 Φ τ k 0 n n 0 n n I n n τ k Problem The decoupling matrix of the system is structurally singular Solution Dynamic extension on the input τ k is needed τ k = u k C. Melchiorri (DEI) ADL Surprise Festschrift 29 / 37

41 Feedback Linearization Dynamic Feedback Linearization Dynamic Feedback Linearization Considering the very simple stiffness variation model k i = τ ki the dynamics of the system becomes: [ ] [ ] [ ] [ ] q M = 1 N 0n n M + 1 Φ τ k 0 n n 0 n n I n n τ k Problem The decoupling matrix of the system is structurally singular Solution Dynamic extension on the input τ k is needed τ k = u k C. Melchiorri (DEI) ADL Surprise Festschrift 29 / 37

42 Feedback Linearization Dynamic Feedback Linearization Dynamic Feedback Linearization Considering the very simple stiffness variation model k i = τ ki the dynamics of the system becomes: [ ] [ ] [ ] [ ] q M = 1 N 0n n M + 1 Φ τ k 0 n n 0 n n I n n τ k Problem The decoupling matrix of the system is structurally singular Solution Dynamic extension on the input τ k is needed τ k = u k C. Melchiorri (DEI) ADL Surprise Festschrift 29 / 37

43 Feedback Linearization Dynamic Feedback Linearization Feedback Linearized Model The system dynamics can be then rewritten as: [ ] [ ] [ q [4] α(xe ) τ = + Q(x e ) k where Q(x e ) = 0 n n u k [ ] M 1 KB 1 M 1 Φ 0 n n I n n ] By defining the control law: [ ] ( [ ] [ ]) τ = Q 1 α(xe ) vq (x e ) + uk 0 n n v k we obtain the feedback linearized model: [ ] q [4] = k [ vq v k ] C. Melchiorri (DEI) ADL Surprise Festschrift 30 / 37

44 Feedback Linearization Dynamic Feedback Linearization Feedback Linearized Model The system dynamics can be then rewritten as: [ ] [ ] [ q [4] α(xe ) τ = + Q(x e ) k where Q(x e ) = 0 n n u k [ ] M 1 KB 1 M 1 Φ 0 n n I n n ] By defining the control law: [ ] ( [ ] [ ]) τ = Q 1 α(xe ) vq (x e ) + uk 0 n n v k we obtain the feedback linearized model: [ ] q [4] = k [ vq v k ] C. Melchiorri (DEI) ADL Surprise Festschrift 30 / 37

45 Feedback Linearization Control Strategy Control Strategy A static state feedback in the state space of the feedback linearized system is used: [ ] [ ] vq q [4] v c =, v v f = d k k d [ ] T z d = qd T q d T q d T q [3]T d kd T k d T The state vector z of the feedback linearized system and a suitable nonlinear coordinate transformation are defined: [ ] T z = q T q T q T q [3]T k T k T = Ψ(xe) = q q M 1 [N + Φ k] ] M [ Ṁ 1 M 1 [N + Φ k] + Ṅ + Φ k + Φ k k k C. Melchiorri (DEI) ADL Surprise Festschrift 31 / 37

46 Feedback Linearization Control Strategy Control System Architecture The controller can be then rewritten as: v c = v f + P[z d z] = v f + P[z d Ψ(x e )] where P = [ ] Pq0 P q1 P q2 P q3 0 n n 0 n n 0 n n 0 n n 0 n n 0 n n P k0 P k1 C. Melchiorri (DEI) ADL Surprise Festschrift 32 / 37

47 Simulation of a two-link Planar Manipulator Simulation of a two-link Planar Manipulator Joint positions rad Pos Joint 1 Pos Joint x 10 5 Position errors rad Err Joint 1 Err Joint Joint Stiffness [N m/rad] 0 Stiff Joint 1 Stiff Joint Time s x 10 5 Stiffness errors [N m/rad] Err Joint 1 Err Joint Time s C. Melchiorri (DEI) ADL Surprise Festschrift 33 / 37

48 Application to Antagonistic Variable Stiffness Devices Application to Antagonistic Variable Stiffness Devices Dynamic model of an antagonistic variable stiffness robot By introducing the auxiliary variables M(q) q + N(q, q) + η α η β = 0 B θ α + η α = τ α B θ β + η β = τ β p = θα θ β 2 positions of the generalized joint actuators s = θ α + θ β state of the virtual stiffness actuators F (s) generalized joint stiffness matrix (diagonal) g(q p) strictly monotonically increasing functions (generalized joint displacements) h(q p, s) such that h i (0, 0) = 0 τ = τ α τ β, τ k = τ α + τ β it is possible to write M(q) q + N(q, q) + F (s)g(q p) = 0 2B p + F (s)g(p q) = τ B s + h(q p, s) = τ k C. Melchiorri (DEI) ADL Surprise Festschrift 34 / 37

49 Application to Antagonistic Variable Stiffness Devices Application to Antagonistic Variable Stiffness Devices Dynamic model of an antagonistic variable stiffness robot By introducing the auxiliary variables M(q) q + N(q, q) + η α η β = 0 B θ α + η α = τ α B θ β + η β = τ β p = θα θ β 2 positions of the generalized joint actuators s = θ α + θ β state of the virtual stiffness actuators F (s) generalized joint stiffness matrix (diagonal) g(q p) strictly monotonically increasing functions (generalized joint displacements) h(q p, s) such that h i (0, 0) = 0 τ = τ α τ β, τ k = τ α + τ β it is possible to write M(q) q + N(q, q) + F (s)g(q p) = 0 2B p + F (s)g(p q) = τ B s + h(q p, s) = τ k C. Melchiorri (DEI) ADL Surprise Festschrift 34 / 37

50 Application to Antagonistic Variable Stiffness Devices Application to Antagonistic Variable Stiffness Devices Dynamic model of an antagonistic variable stiffness robot By introducing the auxiliary variables M(q) q + N(q, q) + η α η β = 0 B θ α + η α = τ α B θ β + η β = τ β p = θα θ β 2 positions of the generalized joint actuators s = θ α + θ β state of the virtual stiffness actuators F (s) generalized joint stiffness matrix (diagonal) g(q p) strictly monotonically increasing functions (generalized joint displacements) h(q p, s) such that h i (0, 0) = 0 τ = τ α τ β, τ k = τ α + τ β it is possible to write M(q) q + N(q, q) + F (s)g(q p) = 0 2B p + F (s)g(p q) = τ B s + h(q p, s) = τ k C. Melchiorri (DEI) ADL Surprise Festschrift 34 / 37

51 Application to Antagonistic Variable Stiffness Devices Actual Variable Stiffness Joint Implementations For antagonistic actuated robot with exponential force/compression characteristic (Palli et al. 2007) f i (s i ) = e a s i g i (q i p i ) = b sinh ( c (q i p i ) ) h i (q i p i, s i ) = d [ cosh ( c (q i p i ) ) e a s i 1 ] If transmission elements with quadratic force/compression characteristic are considered (Migliore et al. 2005) f i (s i ) = a 1 s i + a 2 g i (q i p i ) = q i p i h i (q i p i, s i ) = b 1 si 2 + b 2 (q i p i ) 2 For the variable stiffness actuation joint (VSA), using a third-order polynomial approximation of the transmission model (Boccadamo, Bicchi et al. 2006) f i (s i ) = a 1 s 2 i + a 2 s i + a 3 g i (q i p i ) = q i p i h i (q i p i, s i ) = b 1 s 3 i + b 2 (q i p i ) 2 s i + b 3 s i C. Melchiorri (DEI) ADL Surprise Festschrift 35 / 37

52 Application to Antagonistic Variable Stiffness Devices Actual Variable Stiffness Joint Implementations For antagonistic actuated robot with exponential force/compression characteristic (Palli et al. 2007) f i (s i ) = e a s i g i (q i p i ) = b sinh ( c (q i p i ) ) h i (q i p i, s i ) = d [ cosh ( c (q i p i ) ) e a s i 1 ] If transmission elements with quadratic force/compression characteristic are considered (Migliore et al. 2005) f i (s i ) = a 1 s i + a 2 g i (q i p i ) = q i p i h i (q i p i, s i ) = b 1 si 2 + b 2 (q i p i ) 2 For the variable stiffness actuation joint (VSA), using a third-order polynomial approximation of the transmission model (Boccadamo, Bicchi et al. 2006) f i (s i ) = a 1 s 2 i + a 2 s i + a 3 g i (q i p i ) = q i p i h i (q i p i, s i ) = b 1 s 3 i + b 2 (q i p i ) 2 s i + b 3 s i C. Melchiorri (DEI) ADL Surprise Festschrift 35 / 37

53 Application to Antagonistic Variable Stiffness Devices Actual Variable Stiffness Joint Implementations For antagonistic actuated robot with exponential force/compression characteristic (Palli et al. 2007) f i (s i ) = e a s i g i (q i p i ) = b sinh ( c (q i p i ) ) h i (q i p i, s i ) = d [ cosh ( c (q i p i ) ) e a s i 1 ] If transmission elements with quadratic force/compression characteristic are considered (Migliore et al. 2005) f i (s i ) = a 1 s i + a 2 g i (q i p i ) = q i p i h i (q i p i, s i ) = b 1 si 2 + b 2 (q i p i ) 2 For the variable stiffness actuation joint (VSA), using a third-order polynomial approximation of the transmission model (Boccadamo, Bicchi et al. 2006) f i (s i ) = a 1 s 2 i + a 2 s i + a 3 g i (q i p i ) = q i p i h i (q i p i, s i ) = b 1 s 3 i + b 2 (q i p i ) 2 s i + b 3 s i C. Melchiorri (DEI) ADL Surprise Festschrift 35 / 37

54 Conclusions Conclusions The feedforward control action needed to perform a desired motion profile has been computed The feedback linearization problem with decoupled control has been solved taking into account different stiffness variation models The simultaneous asymptotic trajectory tracking of both the position and the stiffness has been achieved by means of an outer linear control loop These results can be easily extended to the mixed rigid/elastic case The proposed approach has been used to model several actual implementation of variable stiffness devices C. Melchiorri (DEI) ADL Surprise Festschrift 36 / 37

55 ADL Surprise Festschrift ADL Surprise Festschrift Thanks Alex!! C. Melchiorri (DEI) ADL Surprise Festschrift 37 / 37