Mobile Manipulation: Force Control

Transcription

1 Mobile Manipulation: Force Control Mike Stilman Robotics & Intelligent GT Georgia Institute of Technology Atlanta, GA February 19, Force Control Strategies Logic Branching Continuous Force Control Direct Feedback Position/Velocity Feedback Position Control & Force Control Impedance Control (Classic & Revised) Hybrid Control 2 1

2 Logic Branching Brief Description: Eecute specified position/force commands Switch behavior on perceived input Motivation: Handle Uncertainty Achieve Very Low Tolerances Ernst 61, Baber 73, Inoue 74 3 Peg In Hole Case 1: Loose Fit Inoue Suppose our positioning error is <.068 z 4 2

3 Peg In Hole Case 1: Loose Fit Inoue Suppose our positioning error is <.068 z 1 2 1) Position Control X 2) PUSH-INTO Fz = Insert Force F = Peg In Hole Case 2: Loose Fit with Uncertainty Inoue 74 Our positioning error is <.068 but Starting position is uncertain (±.1 ) z 6 3

4 Peg In Hole Case 2: Loose Fit with Uncertainty Inoue 74 Our positioning error is <.068 but Starting position is uncertain (±.1 ) ) Z = Z- Until Fz > 0 z ) DROP-INTO X = X Fz = Small Contact Force F = Small Sliding Force Until F > Threshold 3) PUSH-INTO 7 Peg In Hole Case 3: Close Fit Inoue 74 Positioning error >.01 and Starting position is uncertain (±.1 ).01 1 z 8 4

5 Peg In Hole Case 3: Close Fit Inoue 74 Positioning error >.01 and Starting position is uncertain (±.1 ) ) Position Control θ to.1 2) DROP-INTO z Peg In Hole Case 3: Close Fit Inoue 74 Positioning error >.01 and Starting position is uncertain (±.1 ) ) Position Control θ to.1 2) DROP-INTO z 3 4 3) Torque θ until τ > threshold 4) PUSH-INTO 10 5

6 Continuous Strategies Discrete branching: Advantageous for handling Uncertainty/Tolerances Very useful as part of a system Could be more time efficient Continuous Strategies: Coordination of multi-ais motions Responds to continuously changing force-torque information Achieves forces with greater precision Nevins 73, Whitney 77 Raibert & Craig 81, Mason 81, Khatib Continuous Force Control Feed-forward: 12 6

7 Continuous Force Control Feed-forward: τ = J T F How do we do Feedback? 13 Force-based Feedback Control f d e f J T e τ K F τ Arm f F/T Sensor τ = K F J T (f f d ) Is K F in Joint Space or Workspace? Will it work well? Whitney

8 Force-based Control (Linearization) f d e f J T e τ K F τ G Arm θ f F/T Sensor τ = G K F J T (f f d ) 15 Force-based Control (Feed-forward) f d J T e f J T e τ K F τ G Arm θ f F/T Sensor τ = J T f d G K F J T (f f d ) Similar to Volpe

9 Force-based Control (Feed-forward Term) f d J T e f J T e τ K F τ G Arm θ f F/T Sensor τ = J T f d G K F J T (f f d ) Non-zero steady-state error Can be oscillatory Similar to Volpe Options Integral Control Z t τ = J T f d G K FI J T (f f d )dt 0 Increase Feed-Forward Term Volpe

10 Options Integral Control Z t τ = J T f d G K FI J T (f f d )dt 0 Increase Feed-Forward Term Volpe Position-based Force Control Workspace dynamics: Workspace dynamics with contact: Ḡ(q) =f Ḡ(q) =f f E 20 10

11 Position-based Force Control Workspace dynamics: Workspace dynamics with contact: Ḡ(q) =f Ḡ(q) =f f E Generic Position Controller: f = Ḡ(q) K p( d ) Controlled System Dynamics: Ḡ(q) =Ḡ(q) K p ( d )f E 21 Position-based Force Control Workspace dynamics: Workspace dynamics with contact: Ḡ(q) =f Ḡ(q) =f f E Generic Position Controller: f = Ḡ(q) K p( d ) Controlled System Dynamics: Ḡ(q) =Ḡ(q) K p ( d )f E K p d - K p ( d )=f E d = Kp 1 f E 22 11

12 Position-based Force Control: Feedback Position Control: d Kp e F C J T τ C Kin. G Arm θ 23 Position-based Force Control: Feedback f d 1 s K If K p d e F C F/T Sensor J T τ C Kin. G Arm θ f Force Control K lf K p f e 24 12

13 Position Control Force Control Impedance Control Continuous relationship between position/force Simulates behavior of a simple mechanical system Hybrid Control Selection Matri identifies directions for position/force Allows for precise positioning and force control 25 Impedance Control How would the robot respond if its dynamics were actually: M d ẍ D d ẋ e K d e = f E e =( r ) Position tracking when force = 0 Compliance when force > 0 Restoration to tracking when force is removed 26 13

14 Impedance Control How would the robot respond if its dynamics were actually: M d ẍ D d ẋ e K d e = f E e =( r ) Position tracking when force = 0 Compliance when force > 0 Restoration to tracking when force is removed ẍ = M 1 d (f E D d ẋ e K d e ) 27 Impedance Control How would the robot respond if its dynamics were actually: M d ẍ D d ẋ e K d e = f E e =( r ) Position tracking when force = 0 Compliance when force > 0 Restoration to tracking when force is removed ẍ = M 1 d (f E D d ẋ e K d e ) Notice the similarity to workspace computed torque: τ = J T ( M(M 1 d (f E D d ẋ e K d e )) Cẋ Ḡ f E ) 28 14

15 Position-based Impedance Control M d ẍ D d ẋ e K d e = f E e =( r ) 1) Simulate the system response to f E ẍ(t t) = M 1 d (f E D d ẋ(t)k d (t)) ẋ(t t) = ẋ(t) ẍ(t) t (t t) = (t) ẋ(t) t 2) Track simulated (possibly ẋ ) 29 Hybrid Control Task Constraint: Restriction on the freedom of motion of the manipulator Cannot move in some direction Can control forces/moments in that direction Degrees of freedom are coordinates in a task frame = 1 n T F t Task frame is a transformed world frame F t = T 0 t F 0 F 0 F t 30 15

16 Hybrid Control A motion constraint is described by a selection matri: S = s 1 Sẋ = 0 s n Coordinates Cartesian Fied Ais (Roll, Pitch Yaw) R t B = R(z t, φ)r(y t, θ)r( t, ψ) z y F t S RPY = I[ s s y s z s ψ s θ s φ ] T Alternative Coordinates: 31 Eamples of Constraints [ ] [ ] Parameterized [ ] Parameterized [ ] 32 16

17 Hybrid Control d f d Motion Control Force Control R t R t S I-S R t T R t T Arm f Force Motion 33 Best of Both Worlds Use task frame to set a center of compliance for impedance control Use Impedance Control (not motion control) in hybrid system Vary the parameters of Impedance Control according to direction 34 17

18 Summary We looked at: Logic Branching Continuous Force Control (Force and Position Based) Hybrid Postion/Force Strategies Your goal: Think about how these strategies can help you accomplish the task Which subtasks require which type of control? 35 18