Lecture «Robot Dynamics» : Kinematics 3

Transcription

1 Lecture «Robot Dynamics» : Kinematics V lecture: CAB G11 Tuesday 10:15-12:00, every week exercise: HG G1 Wednesday 8:15-10:00, according to schedule (about every 2nd week) office hour: LEE H303 Friday Marco Hutter, Roland Siegwart and Thomas Stastny Robot Dynamics - Kinematics

2 Outline Kinematic control methods Inverse kinematics Singularities, redundancy Multi-task control Iterative inverse differential kinematics Kinematic trajectory control 2

3 What we saw so far Relative pose between frame coordinate frames 3

4 What we saw so far Relative pose between frame coordinate frames [ ] CAB T AB = A r AB 0 T 1 4

5 What we saw so far Relative pose between frame coordinate frames T AB = [ CAB ] A r AB 0 T 1 5

6 What we saw so far Forward kinematics Description of end-effector configuration (position & orientation) as a function of joint coordinates 6

7 What we saw so far Forward kinematics Description of end-effector configuration (position & orientation) as a function of joint coordinates Use the homogeneous transformation matrix 7

8 What we saw so far Forward kinematics Description of end-effector configuration (position & orientation) as a function of joint coordinates {E} Use the homogeneous transformation matrix T IE (q) = [ CIE (q) Ir IE (q) 0 T 1 ] R 4 4 {I} Forward* Kinematics Joint*angles 8

9 What we saw so far Forward kinematics Description of end-effector configuration (position & orientation) as a function of joint coordinates {E} Use the homogeneous transformation matrix T IE (q) = [ CIE (q) Ir IE (q) 0 T 1 Parametrized description [ ] rie (q) x E = = f(q) ϕ IE (q) ] R 4 4 {I} Forward* Kinematics Joint*angles 9

10 What we saw so far Inverse kinematics Description of joint angles as a function of the end-effector configuration Use the homogeneous transformation matrix [ ] CIE (q) T IE (q) = Ir IE (q) 0 T R Parametrized description [ ] rie (q) x E = = f(q) q = f 1 (x ϕ IE (q) E ) {I} {E} Configuration end7effector Inverse* Kinematics Joint*angles 10

11 Closed form solutions Geometric or Algebra Analytic solutions exist for a large class of mechanisms 3 intersecting neighbouring axes (most industrial robots) 11

12 Closed form solutions Geometric or Algebraic Analytic solutions exist for a large class of mechanisms Geometric 3 intersecting neighbouring axes (most industrial robots) Decompose spatial geometry of manipulator into several plane problems and apply geometric laws 12

13 Closed form solutions Geometric or Algebraic Analytic solutions exist for a large class of mechanisms Geometric 3 intersecting neighbouring axes (most industrial robots) Decompose spatial geometry of manipulator into several plane problems and apply geometric laws Algebraic Manipulate transformation matrix equation to get the joint angles TIE = T01 (! 1) T12 (! 2 ) T23 (! 3 ) T34 (! 4 ) T45 (! 5 ) T56 (! 6 ) " (! ) 1 = (! ) (! ) (! ) (! ) (! ) T T T T T T T 01 1 IE " 1 ( T01 (! 1) T12 (! 2 )) TIE = T23 (! 3 ) T34 (! 4 ) T45 (! 5 ) T56 (! 6 ) " 1 ( T01 (! 1) T12 (! 2 ) T23 (! 3 )) TIE = T34 (! 4 ) T45 (! 5 ) T56 (! 6 ) 13

14 Inverse Differential Kinematics We have seen how Jacobians map velocities from joint space to task-space w e = J e0 q In general, we are interested in the inverse problem Simple method: use the pseudoinverse q = J + e0 w e however, the Jacobian might be singular! 14

15 Singularities A singurality is a joint-space configuration q s such that J e0 (q s ) is column-rank deficient the Jacobian becomes badly conditioned w e small desired velocities produce high joint velocities q Singularities can be classified into: boundary (e.g. a stretched out manipulator) easy to avoid during motion planning internal harder to prevent, requires careful motion planning 15

16 Singularities A singurality is a joint-space configuration q s such that J e0 (q s ) is column-rank deficient the Jacobian becomes badly conditioned w e small desired velocities produce high joint velocities q Use a damped version of the Moore-Penrose pseudo inverse q = J T e0(j e0 J T e0 + λ 2 I) 1 w e λ>0,λ R 16

17 Singularities A singurality is a joint-space configuration q s such that J e0 (q s ) is column-rank deficient the Jacobian becomes badly conditioned w e small desired velocities produce high joint velocities q Use a damped version of the Moore-Penrose pseudo inverse q = J T e0(j e0 J T e0 + λ 2 I) 1 w e min w e J e0 q 2 + λ 2 q 2 λ>0,λ R 17

18 Redundancy A kinematic structure is redundant if the dimension of the task-space is smaller than the dimension of the joint-space E.g. the human arm has 7DoF (three in the shoulder, one in the elbow, and three in the wrist) 18

19 Redundancy A kinematic structure is redundant if the dimension of the task-space is smaller than the dimension of the joint-space E.g. the human arm has 7DoF (three in the shoulder, one in the elbow, and three in the wrist) q R 7 19

20 Redundancy A kinematic structure is redundant if the dimension of the task-space is smaller than the dimension of the joint-space E.g. the human arm has 7DoF (three in the shoulder, one in the elbow, and three in the wrist) q R 7 w R 6 20

21 Redundancy A kinematic structure is redundant if the dimension of the task-space is smaller than the dimension of the joint-space E.g. the human arm has 7DoF (three in the shoulder, one in the elbow, and three in the wrist) q R 7 w R 6 J e0 R

22 Redundancy A kinematic structure is redundant if the dimension of the task-space is smaller than the dimension of the joint-space E.g. the human arm has 7DoF (three in the shoulder, one in the elbow, and three in the wrist) q R 7 w R 6 J e0 R 6 7 Redundancy implies infinite solutions q = J + e0 w e 22

23 Redundancy A kinematic structure is redundant if the dimension of the task-space is smaller than the dimension of the joint-space E.g. the human arm has 7DoF (three in the shoulder, one in the elbow, and three in the wrist) q R 7 w R 6 J e0 R 6 7 Redundancy implies infinite solutions q = J + e0 w e + N q 0 N = N (J e0 ) J e0 N = 0 23

24 Redundancy A kinematic structure is redundant if the dimension of the task-space is smaller than the dimension of the joint-space E.g. the human arm has 7DoF (three in the shoulder, one in the elbow, and three in the wrist) q R 7 w R 6 J e0 R 6 7 Redundancy implies infinite solutions q = J + e0 w e + N q 0 N = N (J e0 ) J e0 N = 0 J e0 (J + e0 w e + N q 0 )=w e 24

25 Redundancy A kinematic structure is redundant if the dimension of the task-space is smaller than the dimension of the joint-space E.g. the human arm has 7DoF (three in the shoulder, one in the elbow, and three in the wrist) q R 7 w R 6 J e0 R 6 7 Redundancy implies infinite solutions q = J + e0 w e + N q 0 N = N (J e0 ) J e0 N = 0 One way to compute the nullspace projection matrix N = I J + e0 J e0 J e0 (J + e0 w e + N q 0 )=w e 25

26 Multi-task control Manipulation (as well as locomotion! ) is a complex combination of high level tasks track a desired position ensure kinematic constraints reach a desired end-effector orientation Break down the complexity into smaller tasks Two methods Multi-task with equal priority task i := {J i, wi } Multi-task with Prioritization 26

27 Multi-task control Equal priority Assume that t tasks have been defined The generalised velocity is given by q = J 1.. J nt + } {{ } J w 1.. w n t } {{ } w The pseudo inversion will try to solve all tasks at the same time in an optimal way It is possible to weigh some tasks higher than others J +W = ( JT W J ) 1 JT W W = diag(w 1,...,w m ) 27

28 Multi-task control Prioritization Instead of solving all tasks at once, we can use consecutive nullspace projection to ensure a strict priority We already saw that q = J + 1 w 1 + N 1 q 0 The solution for task 2 should not violate the one found for task 1 ( w 2 = J 2 q = J 2 J + 1 w1 ) + N 1 q 0 This can be solved for Back substituting yields The iterative solution for T tasks is then given by q = n T i=1 q 0 N i q i, with q i =(J i N i ) + (w i J q 0 =(J 2 N 1 ) + ( w2 J 2 J + ) 1 w q = J + 1 w 1 + N 1 (J 2 N 1 ) + ( 1 w2 J 2 J + ) 1 w 1 i 1 k=1 N k q k ) 28

29 Multi-task control Example - single task 3DoF planar robot arm with unitary link lengths Find the generalised velocities, given q t =(π/6,π/3,π/3) T 0ṙ E,t =(1, 1) T l 1 c 1 + l 2 c 12 + l 3 c 123 l 2 c 12 + l 3 c 123 l 3 c 123 IJ e0p = l 1 s 1 l 2 s 12 l 3 c 123 l 2 s 12 l 3 s 123 l 3 s 123 ϕ 1 l 1 e 1 z ϕ 3 l 3 e 3 e l E z 2 z ϕ 2 e 2 z e 3 e E v e x x e 2 x e 1 x l 0 e 0 z = e I z e 0 x = e I x 29

30 Multi-task control Example - single task 3DoF planar robot arm with unitary link lengths Find the generalised velocities, given q t =(π/6,π/3,π/3) T 0ṙ E,t =(1, 1) T l 1 c 1 + l 2 c 12 + l 3 c 123 l 2 c 12 + l 3 c 123 l 3 c 123 IJ e0p = l 1 s 1 l 2 s 12 l 3 c 123 l 2 s 12 l 3 s 123 l 3 s 123 ϕ 1 l 1 e 1 z ϕ 3 l 3 e 3 e l E z 2 z ϕ 2 e 2 z e 3 e E v e x x e 2 x w E = 0 r E = [ ] 1 1 q t = J + E (q t)w E,t = ( [ + [ ] = ]) l 0 e 1 x e 0 z = e I z e 0 x = e I x 30

31 Multi-task control Example - two tasks 3DoF planar robot arm with unitary link lengths Find the generalised velocities, given q t =(π/6,π/3,π/3) T 0ṙ E,t =(1, 1) T IJ e0p = l 1 c 1 + l 2 c 12 + l 3 c 123 l 2 c 12 + l 3 c 123 l 3 c l 1 s 1 l 2 s 12 l 3 c 123 l 2 s 12 l 3 s 123 l 3 s 123 Additionally, we want to fulfill a second task with the same priority as the first, namely that the first and third joint velocities are zero ϕ 1 l 1 e 1 z ϕ 3 l 3 e 3 e l E z 2 z ϕ 2 e 2 z e 3 e E v e x x e 2 x e 1 x l 0 e 0 z = e I z e 0 x = e I x 31

32 Multi-task control Example - two tasks 3DoF planar robot arm with unitary link lengths Find the generalised velocities, given q t =(π/6,π/3,π/3) T 0ṙ E,t =(1, 1) T IJ e0p = Additionally, we want to fulfill a second task with the same priority as the first, namely that the first and third joint velocities are zero w j = [ 0 q = [ JE J j 0 ] T ] + [ ] w E wj J j = [ 1 0 ] l 1 c 1 + l 2 c 12 + l 3 c 123 l 2 c 12 + l 3 c 123 l 3 c l 1 s 1 l 2 s 12 l 3 c 123 l 2 s 12 l 3 s 123 l 3 s 123 l 0 ϕ 3 l 3 e 3 e l E z 2 z ϕ 2 e 2 z e 3 e E v e x x l 1 ϕ 1 e 2 e 1 x z e 1 x e 0 z = e I z e 0 x = e I x 32

33 Prioritized multi-task control Quadrupedal locomotion Main body pose Kinematic limits Foot motion tracking Friction cone constraints Constraint points are not allowed to move 33

34 Prioritized multi-task control Quadrupedal locomotion 34

35 Mapping associated with the Jacobian 35

36 Numerical solutions Inverse differential kinematics Jacobians map joint-space velocities to end-effector velocities χ e = J ea (q) q 36

37 Numerical solutions Inverse differential kinematics Jacobians map joint-space velocities to end-effector velocities χ e = J ea (q) q We can use this to iteratively solve the inverse kinematics problem target configuration χ e, initial joint space guess q 0 1. q q 0 start configuration 2. while χ e χ e (q) tol do while the solution is not reached 3. J ea J ea (q) = χ e q (q) evaluate Jacobian 4. J + ea (J ea(q)) + compute the pseudo inverse 5. χ e χ e χ e (q) find the end-effector configuration error vector 6. q q + J + ea χ e updated the generalized coordinates 37

38 Numerical solutions Inverse differential kinematics Jacobians map joint-space velocities to end-effector velocities χ e = J ea (q) q We can use this to iteratively solve the inverse kinematics problem target configuration χ e, initial joint space guess q 0 1. q q 0 start configuration 2. while χ e χ e (q) tol do while the solution is not reached 3. J ea J ea (q) = χ e q (q) evaluate Jacobian 4. J + ea (J ea(q)) + compute the pseudo inverse 5. χ e χ e χ e (q) find the end-effector configuration error vector 6. q q + J + ea χ e updated the generalized coordinates 38

39 Inverse kinematics Three-link arm example Determine end-effector Jacobian 39

40 Inverse kinematics Three-link arm example Determine end-effector Jacobian 1. Introduce coordinate frames 40

41 Inverse kinematics Three-link arm example Determine end-effector Jacobian 1. Introduce coordinate frames 2. Introduce generalized coordinates 41

42 Inverse kinematics Three-link arm example Determine end-effector Jacobian 1. Introduce coordinate frames 2. Introduce generalized coordinates 3. Determine end-effector position l 0 + l 1 cos(q 1 )+l 2 cos(q 1 + q 2 )+l 3 cos(q 1 + q 2 + q 3 ) 0r 0E (q) = l 1 sin(q 1 )+l 2 sin(q 1 + q 2 )+l 3 sin(q 1 + q 2 + q 3 ) 0 42

43 Inverse kinematics Three-link arm example Determine end-effector Jacobian 1. Introduce coordinate frames 2. Introduce generalized coordinates 3. Determine end-effector position l 0 + l 1 cos(q 1 )+l 2 cos(q 1 + q 2 )+l 3 cos(q 1 + q 2 + q 3 ) 0r 0E (q) = l 1 sin(q 1 )+l 2 sin(q 1 + q 2 )+l 3 sin(q 1 + q 2 + q 3 ) 0 4. Compute the Jacobian 0J ep = q 0 r 0E (q) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( )!# l1sin q1 # l2sin q1+ q2 # l3sin q1+ q2 + q3 # l2sin q1+ q2 # l3sin q1+ q2 + q3 # l3sin q1+ q2 + q3 " $ % = $ l1cos q1 + l2cos q1+ q2 + l3cos q1+ q2 + q3 l2cos q1+ q2 + l3cos q1+ q2 + q3 l3cos q1+ q2 + q3 % $ & % ' 43

44 Inverse kinematics Three-link arm example Iterative inverse kinematics to find desired configuration q i+1 = q i + J + ep (r goal r i ) 44

45 Inverse kinematics Three-link arm example Iterative inverse kinematics to find desired configuration q i+1 = q i + J + ep (r goal r i ) start value 0 q 0 =

46 Inverse kinematics Three-link arm example Iterative inverse kinematics to find desired configuration q i+1 = q i + J + ep (r goal r i ) start value π/2 q 0 =

47 Inverse kinematics Three-link arm example Iterative inverse kinematics to find desired configuration q i+1 = q i + J + ep (r goal r i ) start value π/2 q 0 = π/2 0 Same goal position, multiple solutions joint-space bigger than task-space, redundant system 47

48 Inverse kinematics Iterative methods Let s have a closer look at the joint update rule q i+1 = q i + J + ea χ 48

49 Inverse kinematics Iterative methods Let s have a closer look at the joint update rule q i+1 = q i + J + ea χ Two main issues Scaling if the current error is too large, the error linearization implemented by the jacobian is not accurate enough use a scaling factor 0 <k<1 q i+1 = q i + kj + ea χ unfortunately, this will lead to slower convergence 49

50 Inverse kinematics Iterative methods Let s have a closer look at the joint update rule q i+1 = q i + J + ea χ Two main issues Singular configurations When the Jacobian is rank-deficient, the inversion becomes a badly conditioned problem Use the damped pseudoinverse (Levenberg-Marquardt) q i+1 = q i + J T ea(j ea J T ea + λ 2 I) 1 χ Use the transpose of the Jacobian q i+1 = q i + αj T ea χ For a detailed explanation, check Introduction to Inverse Kinematics with Jacobian Transpose, Pseudoinverse and Damped Least Squares methods, Samuel Buss,

51 Inverse differential kinematics Orientation error 3D rotations are defined in the Special Orthogonal group SO(3) The parametrization affects convergence from start to goal orientation Rotate along shortest path in SO(3): use rotational vectors which parametrize rotation from start to goal χ rotvec = ϕ = C GS ( ϕ) =C GI (ϕ )C T SI(ϕ t ) The update law for rotations will then be q q + k PR J + e0 R ϕ This is NOT the difference between rotation vectors, but the rotation vector extracted from the relative rotation between start and goal 51

52 Trajectory control Position Consider a planned desired motion of the end effector r e(t) ṙ e(t) Let s see how to kinematically control the end-effector position Feedback term r t e = r e(t) r e (q t ) We can design a nonlinear stabilizing controller law q = J + e0 P (ṙ + k PP r t e) If we substitute this into the differential kinematics equation, we get w e = J ep q = ṙ + k PP r t e = ṙ t e + k PP r t e = 0 Stable error dynamics for positive k PP 52

53 Trajectory control Orientation Derivation more involved Final control law similar to the position case q = J + e0 R (ω(t) e + k PR ϕ) Note that we are not using the analytical Jacobian since we are dealing with angular velocities and rotational vectors 53