Robotics 2

Hybrid Force/Motion Control

Prof. Alessandro De Luca

Natural and artificial constraints

!!

contact/interaction between the robot and a purely geometric (rigid and frictionless) environment naturally constrains the end-effector motion in ideal conditions (robot and environment perfectly rigid, frictionless contact), one can define in the task space two sets of generalized directions selected so that
!!

!!

!!

end-effector motion is feasible in a set of k directions natural constraints (where the environment cannot react with forces/torques) on force and motion contact reaction forces/torques arise in a set of 6-k directions imposed by the task (where the environment bars any end-effector motion)

!!

these sets of directions are mutually orthogonal (and complementary, namely they cover the 6D task/Cartesian space) and are characterized by a suitable task frame RFt (typically attached to the robot end-effector) for general interaction tasks, position and orientation of the task frame will be time-varying the particular way task execution should be performed can be expressed in terms of artificial constraints that specify desired values (to be imposed by the control law) for the velocities, in the k directions feasible for motion, and for the forces, in the remaining 6-k directions feasible for contact reaction
2

!!

!!

Robotics 2

Task frame and constraints

z

- example 1

RFt
y x
v = linear velocity ! = angular velocity F = force M = moment

task: slide the cube along the guide natural (geometric) constraints vy = v z = 0 6-k=4 !x = !z = 0 k=2 Fx = My = 0 artificial constraints (to be imposed by the control law) Fy = Fy,des = 0 Mx = Mx,des = 0, Mz = Mz,des = 0 Fz = Fz,des !y = !y,des = 0 vx = vx,des
3

6-k=4 k=2

Robotics 2

Selection of directions
z

- example 1
0& 0( ( 0( # vx & # vx & )% ( = T )% ( ( 0 $" y ' $" y ' ( 1( 0( '

RFt
y
"0 x $1 $ " F % $0 $M ' = $ 0 # & $ $0 $0 #
Robotics 2

#1 %0 % # v & %0 %" ( = % 0 $ ' % %0 %0 here, constant $

0 0 0% 0 0 0 ' "! F " Fy % ' $ y% $F ' 1 0 0 ' Fz ' z $ ' $ ' ( = Y ( ' 0 1 0 $Mx ' $Mx ' ' $ ' $M ' 0 0 0 ' # Mz & # z& 0 0 1' &

TTY = 0
constraint forces/torques cannot perform work
4

Task frame and constraints

z RFt(") z0 x y RF0 y0

- example 2

task: turning a crank (free handle) natural constraints vx = v z = 0 !x = !y = 0 Fy = Mz = 0 artificial constraints Fx = Fx,des = 0, Fz = Fz,des = 0 Mx = Mx,des = 0, My = My,des = 0 vy = vy,des !z = !z,des = 0

"

x0

RF0

Rz(")

RFt(")

Robotics 2

Selection of directions
z RFt(") z0 x y RF0

example 2
0& 0( ( 0( # vy & #vy & * = T ( ) ) * %" ( 0( % "z ( $ ' $ z' ( 0( 1( '

"

#0 %1 y0 % 0 T # v & #R () ) 0 &% 0 = %0 ( % ( R T () ) '% 0 x0 $ " ' $ 0 % %0 %0 $ 0 0 1 0 0 0 0 0 0 1 0 0 0% 0 ' " Fx % " Fx % ' $ ' $F ' 0 ' Fz $ ' = Y (( ) ) $ z ' ) 0 ' $Mx ' $ Mx ' ' $ ' $M ' 1 ' #My & # y& 0' &

"1 $0 $ ! 0 T " F % " R (( ) 0 %$ 0 $0 '=$ '$ 0 T M 0 R ( ( ) # & # &$ $0 $0 #

Robotics 2

TT(")Y(") = 0

Task frame and constraints

- example 3

task: insert a screw in a bolt

x y
p = screw pitch

RFt

natural constraints (partial) vx = v y = 0 !x = !y = 0

artificial constraints (abundant) Fx = Fx,des = 0, Fy = Fy,des = 0 Mx = Mx,des = 0, My = My,des = 0 vz = vz,des , !z = !z,des = (2#/p)vz,des Fz = Fz,des , Mz = Mz,des (Fz,des )
7

the screw proceeds along and around the z-axis, but not in an independent way! (1 dof) accordingly, Fz and Mz cannot be independent
Robotics 2

the force/torque direction is orthogonal to that of motion!

Selection of directions

example 3
2) p

#v& %" ( = 0 0 1 0 0 $ '

* vz = T * vz

k=1

! p

Y: such that TTY = 0

Fz = " 2# Mz p

RFt

z
"F % $M ' # &

6-k=5

the columns of T and Y do not necessarily coincide with Cartesian directions (columns of the identity matrix) generalized directions

"1 $0 $0 =$ $0 $0 $0 #

0 0 0 0 % " Fx % " Fx % $ ' 1 0! 0 0 ' $ F ' Fy ' 2) ' $ y ' $ 0 0 0 ( p ' * $M ' = Y * $M ' x x 0 1 0 0 ' $ ' $ ' M M y' $ y' 0 0 1 0 ' $ $M ' $M ' 0 0 0 1 ' # z & # z& &
8

Robotics 2

Frames of interest
y0 x0 yt

example 4
ye

planar motion of a 2R robot in contact with a surface (M=2)

xt xe

environment surface

"! task frame RFt used for an independent definition of the hybrid reference values (here: tvx,des [k=1] and tFy,des [M-k=1]) and for computing the errors driving the feedback control law "! sensor frame RFe (here = RF2) where the force eF = (eFx,eFy) is measured "! base frame RF0 in which the end-effector velocity is expressed (here, 0v = (0vx,0vy) of O2), when computed using robot Jacobian and joint velocities all quantities should then be expressed (rotated) in the same reference frame: the task frame!
Robotics 2 9

Parameterization of hybrid tasks

a description of robot-environment contact type: it defines the task frame

#v& k = T ( s ) ) s s " IR %" ( $ ' parameterizes "F % ! M$k " # IR = Y ( s ) ( ) $M ' # & parameterizes

contact forces/torques do not perform work on E-E displacements

E-E free motion

TT(s)Y(s) = 0

the generalized directions contact forces/torques of the task frame depend in general on s (i.e., on the E-E pose in the previous first three examples, ! in the environment) and in! general, it is M=6

robot dynamics

"F % + S(q, q )q + g(q) = u + J (q)$ ' B(q) q #M &

T

Robotics ! 2

#v& robot = J(q)q kinematics %" ( $ '

10

Hybrid force/velocity control

!!

control objective: to impose the desired evolution to the parameters s of motion and to parameters $ of force s(t) sd(t), $(t) $d(t) control law is designed in two steps
1.! exact linearization and decoupling in the task frame by feedback closed-loop model

!!

& # as & #s % (=% ( $ " ' $ a" '

2.! (linear) design of as and a$ so as to impose the desired dynamic behavior to the errors es = sd s and e$ = $d $

"! assumptions: N = M (=! 6, usually), J(q) out of singularity (+ TTY = 0)

Robotics 2

. Note: in simple cases, $ and s are just single components of F or M and of v or !; accordingly, Y and T will be simple 0/1 selection matrices

11

Feedback linearization in task space

= T(s) " s J(q) " q
"q "s + J = T" + T J" q s
#s #q = J"1 T # + T "J q s

!
"1

"F % T B(q)q + S(q, q)q + g(q) = u + J (q)$ ' = u + J T (q) Y (s ) ( ) M& # ! !

' $ s "1 (s )s " + S(q, q )q + g(q) = u J(q)q ) + B(q) J (q) T (B(q)J (q)T(s) "J (q)Y(s))& % #(
T

nonsingular N x N matrix under the assumptions made

linearizing and $ as ' "1 " Jq + S* q +g u = (BJ T " J Y )& ) + BJ Ts decoupling a % #( control law
"1 T

!
Robotics 2

& # as & #s %" ( = %a ( $ ' $ "'

s has relative degree 2

M k $ has relative degree 0

12

Stabilization with as and a$

as usual, it is sufficient to apply linear control techniques (on each single input-output scalar channel)

d + K D (s d " s ) + K P (sd " s) as = s

s + K De s + K Pes = 0 e es = sd " s # 0
here a" = "d could be just enough, but there would be then no force error (and so, poor robustness)

!
!

a " = " d + K I % ( " d # " ) d$ ! ! = # + K I " # = 0 " " #

.

& (#d $ #)d% ' 0

!
!
Robotics 2

we need measures of s, s and $!

!
13

Filtering position and force measures

. . s, s obtained from measures of q and q, equating the descriptions of the end-effector pose and velocity from the robot side (direct and differential kinematics) and from the environment side (function of s) example z0
"L cos s % $ ' 0 r = 0f (q) = $ L sin s ' $ ' y0 0 # &

s = ATAN2( 0 fy (q),0 fx (q))

r L
!

s="
x0

=T J(q) " q !(s) " s

= T # (s)J(q) " q s

! end-effector ! $ obtained from force/torque measures at the

"F % $ M ' = Y (s ) ( ) # &
Robotics 2

#F & " = Y (s ) % ( $M '

#

pseudoinverses of tall matrices with full column rank, e.g., (TTT)-1TT (or weighted)
14

Block diagram of the hybrid controller

q, q
d + K Ds d + K Ps d s
+ _ + _ + _

as
KI s + a$

!+

$d

task-space feedback linearization

robot u in contact with the environment

"F % $M ' # &

s, s
!

!
$ s filtering of measures s

M-k force control loops

k motion control loops

KD

KP

limit cases k=M (free motion): no force control loops, only motion ! k=0 (frozen robot end-effector): no motion control loops, only force
Robotics 2 15

Block diagram of hybrid control

simpler case of 0/1 selection matrices
rotation matrix into the task space

. $ and s are just single components of F (or M) and of v (or !) Y and T are replaced by 0/1 selection matrices: and I-
Robotics 2 16

First experiments with hybrid control

video MIMO-CRF robot (DIS, Laboratorio di Robotica, 1991)

Robotics 2

video

17

in force and velocity measurements 1.! presence of friction at the contact

#! there is a reaction force component in the free motion directions that opposes motion (in case of Coulomb friction, the tangent force intensity depends also from the applied normal force...)

Sources of inconsistency

2.! compliance in the robot structure and/or at the contact

#! a (small) displacement may result also directions that are nominally constrained by the environment
NOTE: however,

if the geometry of the environment at the contact is known with precision, task inconsistencies due to 1. and 2. on the measures of s and $ are automatically filtered out through the pseudo-inversion of the matrices T and Y

3.! uncertainty on the environment geometry at the contact

(can be reduced/eliminated by real time estimation processes driven by external sensors: vision, but also force!)
18

Robotics 2

Identification of an unknown surface

how difficult is to identify the unknown profile of the environment surface, using information from velocity and force measurements at the contact? 1.! normal = nominal direction of measured force
... in the presence of contact motion with friction, the measured torque is slightly rotated from the actual normal by an (unknown) angle % Fn F
Ft = " Fn = tg # " Fn

2.! tangent = nominal direction of measured velocity

... compliance in the robot structure (joints) and/or at ! the contact may lead to a computed velocity that has a small component along the actual normal to the surface vt v vn ! 0 for approaching the unknown surface and for recovering contact (in case of loss), the robot uses a simple exploratory logic
19

3.! mixed method (sensor fusion) with RLS

a.! tangent direction is estimated in a recursive way from position measurements b.! friction angle is estimated in a recursive way using the current estimate of the tangent and from force measurements
Robotics 2

Position-based estimation of the tangent

(for a circular surface traced at constant speed)

[degrees]

Robotics 2

time [sampling intervals]

20

Force-based estimation of the tangent

(for the same circular surface traced at constant speed)

[degrees]

Robotics 2

time [sampling intervals]

21

Difference between estimated tangents

differences are in the order of 7-8... but which one is correct? Better results are obtained with some kind of sensor fusion!

[degrees]

Robotics 2

time [sampling intervals]

22

Reconstructed surface profile

identification with a RLS (Recursive Least Squares) method, which continuously updates the coefficients of two quadratic polynomials fitting locally the unknown contour through data fusion from both force and position/velocity measurements

[m]

this is the reconstructed contour of a cinema film reel (of radius = 17 cm)

Robotics 2

[m]

23

Normal force
regulated to 20 N during simultaneous motion and identification
peaks correspond to grooves on the surface contour

[N]

time [sampling intervals]

Robotics 2 24

Contour identification and hybrid control

performed simultaneously

MIMO-CRF robot (DIS, Laboratorio di Robotica, 1992)

Robotics 2 25

Contour identification and hybrid control

video

Robotics 2

26

Robotized deburring of car windshields

!!

!!

!!

car windshields with sharp edges and tolerances due to fabrication and excess of gluing material (PVB=Polyvinyl butyral) between glass layers robot end-effector follows a preprogrammed path, despite the small errors w.r.t. the nominal windshield profile, thanks to the passive compliance of the deburring work tool contact force between blades and work piece can be independently controlled by a pneumatic actuator in the work tool

!!

!! !!

the deburring robotic worktool contains in particular: two blades for cutting the exceeding plastic material (PVB), the first actuated, the second passively pushed by a spring a load cell for measuring the 1D applied force on-board control system
27

Robotics 2

Model of the deburring work tool

physical mathematical

Robotics 2

for a stability analysis (based on a linear model and on root locus techniques) of a force control loop in a single direction, in the presence of multiple masses/springs as in this present case, see Eppinger & Seering, IEEE CSM, 1987 (material in the course web site)

28

Summary through videos

compliance control (active Cartesian stiffness control without F/T sensor)

impedance control (with F/T sensor)

force control (realized as external loop providing the reference to an internal position loop)

hybrid force/position control

Robotics 2

COMAU robot c/o Universit di Napoli (1994)

29