Motion of Complex Kinematic Robotic Structures Example: Complex Kinematic Chain • Tree structure • Closed kinematic loops So far in this course: only serial robotic structures

2 Sensor Based Robotic Manipulation and Locomotion Tree Structures

Desired values can be assigned for all joints independently.

qA1    qA1  q  qF1  ^      qF 4 

  M (q)q C(q,q)q  g(q) Kinematics and dynamics can be computed in principle identically as for serial manipulators (for example using Euler-Lagrange or Newton-Euler formalism for the dynamics). 3 Sensor Based Robotic Manipulation and Locomotion Closed Kinematic Loops The loop introduces mathematical constraints O between the joints.

Passive joints are necessary in order to minimize the constraint forces which would appear if all joints were active due ^ to positioning inaccuracies.

qa  active joints q    q p  passive joints

Assumptions: • Point contact • The contact point between finger and object does not change.

Under the given assumptions, the contact can be modelled as a virtual passive joint.

4 Sensor Based Robotic Manipulation and Locomotion Forward Kinematics for Closed Loops

TCP1 A3 TCP TCP´2 A (attached to ) A A2 3 A2 3 A2 A3 TCP1  TCP2 active A active A4 A1 4 A1 O passive qa  O passive q  q  usually, only the position of active  p  joints is measured by encoders General procedure: • Breaking the link at a certain joint (typically the output) => a tree structure results • Computation of the geometric model for the tree structure OT (q ,q ), OT (q ,q ) O O TCP1 a p TCP2 a p T (q ,q ) T (q ,q ) TCP1 a p TCP2 a p • From the loop condition (constraint) the passive joints q p are expressed. or • The TCP is expressed by substituting q p TCP1(qa ,qp (qa )) TCP2(qa ,q p (qa )) Remark: inverse kinematics (given TCP, find q) is computed in the known manner for both serial manipulators resulted by breaking the loop. It is generally much easier than direct kinematics,

since 5the two manipulators have simple structures. Sensor Based Robotic Manipulation and Locomotion Example: Four Bar Linkage

qa  TCP1 q  q    q2 3 l A3 TCP q p  2 ´2 A TCP A2 3 l1 active active q q4 A4 1 A1 passive passive 3 4 1 2 Generally: T4 T1 T2 T3'  I Three constraint equations result: 3 corresponding to two translations (q)  0,  R and one rotation in the plane In the simple case of the parallelogram, the solution can be written directly : q4  q1 q  q 4 joints =>The robot has 1 DoF at the TCP 2 1 3 constraints q3  q1 => it follows that only one joint can be actively positioned. 6 Sensor Based Robotic Manipulation and Locomotion Number of DOF in a Loop

For a serial robot: R q  n N links, n 1dof joints (q)  0,  R r constraints for n joints, r constraints => n-r degrees of freedom at TCP

For an arbitrary closed structure with N links, n joints, k DOF/joint: • Planar case: 3 DOF/Link, 3-k constraints/joint n DoF  3(N  n)   ki i1 • Spatial case: 6 DOF/Link, 6-k constraints/joint n (known as Grübler formula) DoF  6(N  n)   ki i1 Example at the whiteboard is valid only for independent constraints

7 Sensor Based Robotic Manipulation and Locomotion Differential Kinematic Model How do I compute the Jacobian? qa  q    q p   v3 A3 By equating the Cartesian velocities at the l2 v3 A separated joint one obtains the differential A2 3 v3  v3 constraints: l1 q1  q3  "velocity v3  v3  J3(q1,q2)  J3(q3,q4) active     A4 constraints" q2  q4  A1 passive q1    q2  Together: J3(q1,q2),  J3(q3,q4)  0   q3  J c0 (q1 ,q2 ,q3 ,q4 )   q4  By introducing the constraints:

Jc (q1)q  0 differential constraints

8 Sensor Based Robotic Manipulation and Locomotion Differential Kinematic Model

One can obtain the equations also by differentiating the constraints:  (q ,q )  0 a p Jc (qa )q  0 (qa,qp ) (qa,qp ) qa   ,   0 differential constraints q   qa q p    p    J c0 q Or:  a  Jca (qa ), Jcp (qa )    0 q p  since J cp ( q a ) is quadratic (number of constraints = number of passive joints) 1 q p  Jcp (qa )Jca (qa )qa (if J cp ( q a ) is not singular) The Cartesian velocity of the open kinematic structure can now be computed in al classical way.

 q1  e.g., for the four bar linkage: v  J , J  J  J q   3 31 32   31 32 1  q1 

9 Sensor Based Robotic Manipulation and Locomotion Dynamics of Closed Kinematic Structures

First, the dynamics of the opened chain (tree structure) 3 is computed with following additional assumptions:  • All joints are actuated. A3 3 A2 A3 • The torques are chosen such that the robot performs the same motions q , q  , q  as for the closed loop. A4 A1   Ba   B  M (q)q C(q,q)q  g(q) B     Bp    a  In reality, we have    and the closed chain is held together by  0  at the virtual separation point the constraint force  .  The forces  are dual to the constraint T velocities v at the separation point: (q)  (q)  v  q       q  q 

Since the real and the virtually separated robot must move identically, it follows:  B (q,q,q)       B (q,q,q) 

10 Sensor Based Robotic Manipulation and Locomotion Dynamics of Closed Kinematic Structures

Remark: In der classical mechanics one obtains this relation through the constrained Euler-Lagrange equation: T d  dL(q,q)  dL(q,q)  (q)          mit L(q,q)  T (q,q) U (q) dt  dq  dq  q 

  B (q,q,q)     T   a   Ba  Jca        0  T    Bp  Jcp  from the second equation:  T  Jcp  Bp T T a  Ba  JcaJcp  Bp  T T T T or simply a  J B  B with JB  I,  JcaJcp  The dynamics of the closed loop can be computed from the dynamics of the tree structure and from the constraint conditions.

11 Sensor Based Robotic Manipulation and Locomotion Dexterous Hands Grasp Types

Precision grasp Pinch grasp Power grasp

13 Sensor Based Robotic Manipulation and Locomotion Opposing Thumb

14 Sensor Based Robotic Manipulation and Locomotion The Complete Finger System

15 Sensor Based Robotic Manipulation and Locomotion Stiffness Control

16 Sensor Based Robotic Manipulation and Locomotion Kinematics of Dexterous Hands

Grasp planning: selection of a grasp, which should be able to:

• Withstand forces acting on the object in arbitrary directions - Force closure grasp • Move the object in arbitrary directions: - Manipulable grasp

In the following, precision grasps are treated

Assumptions: • Contact point on the object is known (or can be measured). • Contact point does not change during motion. • Object and fingers are rigid bodies • Exact geometric model of the object and the hand are available

17 Sensor Based Robotic Manipulation and Locomotion Contact Models

• Point contact without friction: 0 0   zi z Fci O i1 1 Ci Ci1 F    f f R ci 0 ci, ci    normal 0   only force in normal direction 0 • Point contact with Coulomb friction: 1 0 0   0 1 0 0 0 1 Fci    fci, fci  FCci friction cone 0 0 0 0 0 0   0 0 0 R  3 2 2  FCci   f  : f3 R , f1  f2   f3    friction coefficient

18 Sensor Based Robotic Manipulation and Locomotion Contact Models

Soft finger model: 1 0 0 0   0 1 0 0 zi zi1 0 0 1 0 C F O Fci    fci, fci  FCci i ci Ci1 0 0 0 0 0 0 0 0 normal   0 0 0 1 R  4 2 2 FCci  f  : f3 R , f1  f2  f3 , f4   f3  friction coefficient friction coefficient translation rotation The contact can transfer also rotations around the z-axis generally:

Fci  Bci fci, fci  FCci selection matrix friction cone (wrench basis)

19 Sensor Based Robotic Manipulation and Locomotion Grasp Matrix

Contribution of contact force Fci at the coordinate system O: o  f   R 0  pˆoci o ci z F   B f  i zi1 oi    o o  ci ci C Fci O C mo   pˆoci Rci Rci  i f m i1   o o Gi Normal  Ad T B f  G f ocici ci i ci Gci The total wrench in object coordinates is Ad is called the f oci k  1  adjoint matrix   Fo  Gi fci  G1,,Gk     Gfc i1   G  fk  f c or simply: Fo  Gfc, fc  FC grasp matrix 20 Sensor Based Robotic Manipulation and Locomotion Example: Point Contact without Friction

0   0  R o 0  1 F  ci f , f  o oi  o o    ci ci R Rci  xi , yi , zi   pˆ oci Rci Rci  0 0   0 By performing the multiplication one obtains: normal  z  F  i f oi  pˆ z  ci  oci i  lever arm poci  zi The grasp matrix results as:  f1   z1 zk   Fo        Gfc  pˆoc1z1 pˆock zk     fk  G 21 Sensor Based Robotic Manipulation and Locomotion Force Closure Grasp

A grasp is force closure if it can resist any externally applied wrenchFe

Gfc  Fe, fc  FC

Definition: A grasp force f N  f c is called internal force if it generates no net object force

GfN  0, fN  FC z 1 f z2 C c1 O fc2 Example: 1 C2

Theorem (without proof – Murray & al.): A grasp is force closure if and only if G is surjective and if the grasp admits internal forces

22 Sensor Based Robotic Manipulation and Locomotion Evaluation of Force Closure Grasps Generally a difficult problem, depending on the friction cone. (Schlegel, Buss & al.) For point contact without friction, simple solution:

 f1   z1 zk   F   Gf , f R o      c ci   pˆ oc1z1 pˆ ock zk     fk  G  

Theorem ( Murray & al.): Assume point contacts without friction. G i are the columns of the grasp matrix. Following statements are equivalent: 1. The grasp is force closure. p 2. Each object force F o  R can be generated by a positive linear combination of Gi p Fo  Gi fci, Fo R

3. the positive convex hull of all G i contains a vicinity of the origin p=3 in the planar and 6 in the spatial case

23 Sensor Based Robotic Manipulation and Locomotion Example

In the planar case we have  fx  or simply  f   fx   y     0  Fo3   f y  Fo    m   0   z   0    mz   zx1 ,, zxk  and correspondingly the grasp matrix   G3  zy1 ,, zyk      pˆoc1z1 z ,, pˆoc1z1 z 

 a  1 0 1 0   G   0 1 0 1  a b  a b b  

1 0 1 0   G  0 1 0 1 0 0 0 0 grasp convex hull 24 Sensor Based Robotic Manipulation and Locomotion Overview: Generation of Optimal Grasps (Where should the fingers best be placed?) The available fingertip forces are limited. Under these constraints, one would like to generate an "optimal" grasp with respect to possible disturbances.

Problem statement:

1. What can the grasp achieve? Grasp Wrench Space

2. What should the grasp achieve? Task Wrench Space

3. Can it fulfil the task? Quality measure

25 Sensor Based Robotic Manipulation and Locomotion Overview: Grasp Planning (C. Borst et al. 2003)

Input Data

Grasp generation

Rejection of obviously invalid grasps Repetition

Grasp evaluation Quality Force closure measure

Collision test hand configuration

Output of optimal grasp

26 Sensor Based Robotic Manipulation and Locomotion Differential Finger Kinematics

xc2 Duality vector - covector: O Fo  Gfci C1 xo C xc1 2 T T xc  G xo xc1  J1q xc1  G xo H hand surface

By computing finger tip velocity x c over the finger and the object: T Jq  G xo

A grasp is completely described by J , G , FC .

Remark: the velocities of the passive joints are already eliminated by using the selection matrix B. One can therefore compute the object velocity directly from the joint velocity of the fingers. (in contrast to a general parallel kinematics)

27 Sensor Based Robotic Manipulation and Locomotion Manipulability T Jq  G xo

Velocity reachable by Velocities required for omni- the fingers: directional object motions T xc  Im{J} xc  Im{G }

Manipulability: Im{GT }  Im{J} Transformation of forces and velocities:

J GT q xc xo

J T G  fc fo

28 Sensor Based Robotic Manipulation and Locomotion Hand Dynamics

Opening the loop at the contact points, the constraint forces are   f . z1 z2 ci fc1 O fc2 C1 C2 Passive joints: the virtual object joints Active joints: finger joints  Remark: The passive degrees of freedom at the contacts (e.g. rotations for point contact) hand surface are removed by the selection matrix in the grasp matrix. virtual 6 DoF

 Ba passive joint   T 1 T f  M f (q)q C f (q,q)q  g f (q)  J  with q   J G xo differential constraint 0  M o (x)x Co (x, x)x  go (x)  G    Bp By expressing  from the first equation and insertion in the second, the object dynamics results: T Fo  GJ  without proof M (w)x  C(w)x  g(w)  F   o  q  w     xo 

29 Sensor Based Robotic Manipulation and Locomotion Zusatzfolie: Handdynamik

Bemerkung: Die hier abgeleitete Handdynamik gilt unter folgenden Annahmen: 1. Der Griff ist kraftschlüssig und manipulierbar. 2. Die Kontaktkräfte sind innerhalb des Reibkegels. 3. J ist invertierbar (nichtsingulär und nichtredundant)

allgemein: M (w)x C(w)x  g(w)  Fo

1 T 1 T T M (w)  Mo  GJM f J  G  Mo  A M f A T Fo  A  T g(w)  go  A g f T T d C(w, w)  Co  A C f A  A M f A dt mit

AT  GJ T # # 1 T 1 T 1 J  M f J (JM f J ) dynamisch konsistente Pseudoinverse

30 Sensor Based Robotic Manipulation and Locomotion Hand Control

M (w)x C(w)x  g(w)  Fo

All controllers, which have been developed for redundant systems can also be applied for hand control.

For example, the feedback linearization controller:

Fo  M (w)xd  c1e  c2e C(w)x  g(w) e  xd  x Closed loop dynamics: e c1e  c2e  0 The joint torques in the fingers are then: T #   J G Fo To ensure that the contact forces f c stay within the friction cone, the controller must contain additional internal forces: T # T   J G Fo  J fN

31 Sensor Based Robotic Manipulation and Locomotion Generation of Internal Forces

• If the grasp matrix is not quadratic, the question of distributing the object force F o over the finger has to be answered: # # f co G Fo f conot unique, depending on the choice of G

• The internal forces f N have to be chosen such that f c f co fN  FC

Alternative problem formulation: Given the external forces F o , find the minimal contact forces f c such that the contact forces lie in the friction cone: Gfc  Fo, fc  FC

The problem can be formulated as a convex optimization problem and can therefore be solved easily numerically. => unique global minimum! (Schlegl, Buss & al.,’97) • alternative passivity based approach: Generation of internal forces and object forces using potentials and virtual springs

32 Sensor Based Robotic Manipulation and Locomotion Passivity Based Object Impedance Control for Hands

Spring for object movement

V (q)  VS (Hho (q), Hho,d ,ho )

Vk (Hhc (q), Hho (q),hc )

Springs for grasping

V (q)T τ  g(q)   D(q)q q

The force distribution problem is solved here implicitly, by the choice of the springs and their rest lengths.

Passivity-Based Object-Level Impedance Control for a Multi- fingered Hand T. Wimböck, Ch. Ott and G. Hirzinger, IROS 2006.

33 Sensor Based Robotic Manipulation and Locomotion 34 Sensor Based Robotic Manipulation and Locomotion-34- Extension: Two Arm Manipulation

Generalization of the Single Arm Object-Impedance Control Impedance Control

• Introduce an virtual object frame • Control the “grasping” forces via an additional virtual spring. [Natale 2003] H o (H r (q), Hl (q)) • Stiffness matrices must be compatible! • moving the two arms with only one spring connected to this virtual frame.

V (q)  VS (Hl (q), Hl,d ,l )  V (q)  VS (H o (q), H o,d ,o ) VS (H r (q), H r,d , r ) V (H (q), H (q), ) V (H (q), H (q), ) S l r c S l r c V (q)T τ  g(q)   D(q)q C. Ott & T. Wimböck q 35 Sensor Based Robotic Manipulation and Locomotion Two-Armed Manipulation

Generalization of the Single Arm Impedance Control

• Control the “grasping” forces via an additional virtual spring. • Stiffness matrices must be compatible!

V (q)  VS (Hl (q), Hl,d ,l )  VS (H r (q), H r,d , r )

VS (Hl (q), H r (q),c )

C. Ott & T. Wimböck 36 Sensor Based Robotic Manipulation and Locomotion Two-handed Manipulation

Intuitive combination of the two-armed impedance behavior and the object level impedance for the hands

Connect the two-arm impedance with the virtual object frames of the hands, rather than to the end effector frames.

V (q)T τ  g(q)   D(q)q q C. Ott & T. Wimböck 37 Sensor Based Robotic Manipulation and Locomotion 38 Sensor Based Robotic Manipulation and Locomotion-38- Human-Robot-Interaction

Compliant control of the entire robot

Impedance control w D f M v

Rollin’ Justin • 53 active dof Admittance control • 150 kg

39 Sensor Based Robotic Manipulation and Locomotion