DIFFERENTIAL KINEMATICS relationship between joint velocities and end-effector velocities Geometric Jacobian Analytical Jacobian Kinematic singularities Kinematic redundancy Inverse differential kinematics Inverse kinematics algorithms STATICS relationship between end-effector forces and joint torques
GEOMETRIC JACOBIAN T(q) = R(q) p(q) 0 T 1 Goal ṗ = J P (q) q ω = J O (q) q v = [ ] ṗ ω = J(q) q
Derivative of a rotation matrix R(t)R T (t) = I Ṙ(t)R T (t) + R(t)ṘT (t) = O Given S(t) = Ṙ(t)RT (t) S(t) + S T (t) = O Ṙ(t) = S(ω(t))R(t) S = 0 ω z ω y ω z 0 ω x ω y ω x 0
Example R z (α) = cosα sinα 0 sinα cosα 0 0 0 1 S(t) = = αsinα α cosα 0 cosα sinα 0 α cosα α sinα 0 sinα cosα 0 0 0 0 0 0 1 0 α 0 α 0 0 = S(ω(t)) 0 0 0
p 0 = o 0 1 + R 0 1p 1 ṗ 0 = ȯ 0 1 + R0 1ṗ1 + Ṙ0 1 p1 = ȯ 0 1 + R 0 1ṗ1 + S(ω 0 1)R 0 1p 1 = ȯ 0 1 + R 0 1ṗ1 + ω 0 1 r 0 1
Link velocity Linear velocity p i = p i 1 + R i 1 r i 1 i 1,i ṗ i = ṗ i 1 + R i 1 ṙ i 1 i 1,i + ω i 1 R i 1 r i 1 i 1,i = ṗ i 1 + v i 1,i + ω i 1 r i 1,i
Angular velocity R i = R i 1 R i 1 i S(ω i )R i = S(ω i 1 )R i + R i 1 S(ω i 1 i 1,i )Ri 1 i = S(ω i 1 )R i + S(R i 1 ω i 1 i 1,i )R i ω i = ω i 1 + R i 1 ω i 1 i 1,i = ω i 1 + ω i 1,i
ω i = ω i 1 + ω i 1,i ṗ i = ṗ i 1 + v i 1,i + ω i 1 r i 1,i Prismatic joint ω i 1,i = 0 v i 1,i = d i z i 1 ω i = ω i 1 ṗ i = ṗ i 1 + d i z i 1 + ω i r i 1,i Revolute joint ω i 1,i = ϑ i z i 1 v i 1,i = ω i 1,i r i 1,i ω i = ω i 1 + ϑ i z i 1 ṗ i = ṗ i 1 + ω i r i 1,i
Jacobian computation J = j P1... j Pn j O1 j On Angular velocity Joint i prismatic q i j Oi = 0 = j Oi = 0 Joint i revolute q i j Oi = ϑ i z i 1 = j Oi = z i 1
Linear velocity Joint i prismatic q i j Pi = d i z i 1 = j Pi = z i 1 Joint i revolute q i j Pi = ω i 1,i r i 1,n = ϑ i z i 1 (p p i 1 ) j Pi = z i 1 (p p i 1 )
Column of geometric Jacobian [ jpi j Oi ] [ ] zi 1 0 = [ ] zi 1 (p p i 1 ) z i 1 prismatic joint revolute joint z i 1 = R 0 1(q 1 )...R i 2 i 1 (q i 1)z 0 p = A 0 1 (q 1)...A n 1 n (q n ) p 0 p i 1 = A 0 1(q 1 )...A i 2 i 1 (q i 1) p 0
Representation in different frame [ ] ṗt ω t = = J t = [ ] [ ] R t O ṗ O R t ω [ ] R t O O R t J q [ R t O O R t ] J
Three link planar aram
J(q) = [ ] z0 (p p 0 ) z 1 (p p 1 ) z 2 (p p 2 ) z 0 z 1 z 2 p 0 = 0 0 0 p 1 = a 1c 1 a 1 s 1 0 p 2 = a 1c 1 + a 2 c 12 a 1 s 1 + a 2 s 12 0 p = a 1c 1 + a 2 c 12 + a 3 c 123 a 1 s 1 + a 2 s 12 + a 3 s 123 0 z 0 = z 1 = z 2 = 0 0 1
J = a 1 s 1 a 2 s 12 a 3 s 123 a 2 s 12 a 3 s 123 a 3 s 123 a 1 c 1 + a 2 c 12 + a 3 c 123 a 2 c 12 + a 3 c 123 a 3 c 123 0 0 0 0 0 0 0 0 0 1 1 1 J P = [ ] a1 s 1 a 2 s 12 a 3 s 123 a 2 s 12 a 3 s 123 a 3 s 123 a 1 c 1 + a 2 c 12 + a 3 c 123 a 2 c 12 + a 3 c 123 a 3 c 123
Anthropomorphic arm
J = [ ] z0 (p p 0 ) z 1 (p p 1 ) z 2 (p p 2 ) z 0 z 1 z 2 p 0 = p 1 = 0 0 p 2 = 0 z 0 = p = 0 0 1 c 1(a 2 c 2 + a 3 c 23 ) s 1 (a 2 c 2 + a 3 c 23 ) a 2 s 2 + a 3 s 23 z 1 = z 2 = a 2c 1 c 2 a 2 s 1 c 2 a 2 s 2 s 1 c 1 0
J = s 1 (a 2 c 2 + a 3 c 23 ) c 1 (a 2 s 2 + a 3 s 23 ) a 3 c 1 s 23 c 1 (a 2 c 2 + a 3 c 23 ) s 1 (a 2 s 2 + a 3 s 23 ) a 3 s 1 s 23 0 a 2 c 2 + a 3 c 23 a 3 c 23 0 s 1 s 1 0 c 1 c 1 1 0 0 J P = s 1(a 2 c 2 + a 3 c 23 ) c 1 (a 2 s 2 + a 3 s 23 ) a 3 c 1 s 23 c 1 (a 2 c 2 + a 3 c 23 ) s 1 (a 2 s 2 + a 3 s 23 ) a 3 s 1 s 23 0 a 2 c 2 + a 3 c 23 a 3 c 23
Stanford manipulator
J = [ z0 (p p 0 ) z 1 (p p 1 ) z 2 z 0 z 1 0 z 3 (p p 3 ) z 4 (p p 4 ) z 5 (p p 5 ) z 3 z 4 z 5 ] p 0 = p 1 = 0 0 0 p 3 = p 4 = p 5 = c 1s 2 d 3 s 1 d 2 s 1 s 2 d 3 + c 1 d 2 c 2 d 3 p = c 1s 2 d 3 s 1 d 2 + d 6 (c 1 c 2 c 4 s 5 + c 1 c 5 s 2 s 1 s 4 s 5 ) s 1 s 2 d 3 + c 1 d 2 + d 6 (c 1 s 4 s 5 + c 2 c 4 s 1 s 5 + c 5 s 1 s 2 ) c 2 d 3 + d 6 (c 2 c 5 c 4 s 2 s 5 ) z 0 = 0 0 1 z 1 = s 1 c 1 0 z 2 = z 3 = c 1s 2 s 1 s 2 c 2 z 4 = c 1c 2 s 4 s 1 c 4 s 1 c 2 s 4 + c 1 c 4 s 2 s 4 z 5 = c 1c 2 c 4 s 5 s 1 s 4 s 5 + c 1 s 2 c 5 s 1 c 2 c 4 s 5 + c 1 s 4 s 5 + s 1 s 2 c 5 s 2 c 4 s 5 + c 2 c 5
KINEMATIC SINGULARITIES v = J(q) q if J is rank-deficient = kinematic singularities (a) reduced mobility (b) infinite solutions to inverse kinematics problem (c) large joint velocities (in the neighbourhood of singularity) Classification Boundary singularities Internal singularities
Two link planar arm [ ] a1 s J = 1 a 2 s 12 a 2 s 12 a 1 c 1 + a 2 c 12 a 2 c 12 det(j) = a 1 a 2 s 2 ϑ 2 = 0 ϑ 2 = π [ (a 1 + a 2 )s 1 (a 1 + a 2 )c 1 ] T parallel to[ a 2 s 1 a 2 c 1 ] T (components of end-effector velocity non independent)
Singularity decoupling computation of arm singularities computation of wrist singularities
J = [ ] J11 J 12 J 21 J 22 J 12 = [ z 3 (p p 3 ) z 4 (p p 4 ) z 5 (p p 5 ) ] J 22 = [ z 3 z 4 z 5 ] p = p W = p W p i parallel to z i, i = 3, 4, 5 J 12 = [ 0 0 0 ] det(j) = det(j 11 )det(j 22 ) det(j 11 ) = 0 det(j 22 ) = 0
Wrist singularities z 3 parallel to z 5 ϑ 5 = 0 ϑ 5 = π rotations of equal magnitude about opposite directions on ϑ 4 and ϑ 6 do not produce any rotation at the end-effector
Arm singularities Anthropomorphic arm det(j P ) = a 2 a 3 s 3 (a 2 c 2 + a 3 c 23 ) Elbow singularity s 3 = 0 a 2 c 2 + a 3 c 23 = 0 ϑ 3 = 0 ϑ 3 = π
Shoulder singularity p x = p y = 0
ANALYSIS OF REDUNDANCY Differential kinematics v = J(q) q if (J) = r dim ( R(J) ) = r dim ( N(J) ) = n r in general dim ( R(J) ) + dim ( N(J) ) = n
If N(J) where check: q = q + P q a R(P) N(J) J q = J q + JP q a = J q = v q a generates internal motions of the structure
INVERSE DIFFERENTIAL KINEMATICS Nonlinear kinematics equation Differential kinematics equation linear in the velocities Given v(t) + initial conditions = (q(t), q(t)) if n = r q = J 1 (q)v q(t) = t 0 q(ς)dς + q(0) (Euler) numerical integration q(t k+1 ) = q(t k ) + q(t k ) t
Redundant manipulators For a given configuration q, find the solutions q that satisfy v = J q and minimize g( q) = 1 2 qt W q method of Lagrange multipliers g( q, λ) = 1 2 qt W q + λ T (v J q) ( ) T g = 0 q ( ) T g = 0 λ optimal solution q = W 1 J T (JW 1 J T ) 1 v if W = I where q = J v J = J T (JJ T ) 1 is the right pseudo-inverse of J
Use of redundancy g ( q) = 1 2 ( qt q T a )( q q a) like above... g ( q, λ) = 1 2 ( qt q T a )( q q a ) + λ T (v J q) optimal solution q = J v + (I J J) q a
Characterization of internal motions ( ) T w(q) q a = k a q manipulability measure w(q) = det ( J(q)J T (q) ) distance from mechanical joint limits w(q) = 1 2n n ( qi q i i=1 q im q im ) 2 distance from an obstacle w(q) = min p,o p(q) o
Kinematic singularities The previous solutions hold only when J is full-rank If J is not full-rank (singularity) if v R(J) = solution q extracting all linearly independent equations ( physically executable path) if v / R(J) = the system of equations has no solution (path cannot be executed) Inversion in the neighbourhood of singularities det(j) small = q large damped least-squares inverse J = J T (JJ T + k 2 I) 1 where q minimizes g ( q) = v J q 2 + k 2 q 2
ANALYTICAL JACOBIAN p = p(q) φ = φ(q) ṗ = p q q = J P(q) q φ = φ q q = J φ(q) q ẋ = [ ] ṗ φ = [ ] JP (q) J φ (q) q = J A (q) q J A (q) = k(q) q
Angular velocity of Euler angles ZYZ about axes of reference frame as a result of ϕ: [ ω x ω y ω z ] T = ϕ [ 0 0 1 ] T as a result of ϑ: [ ω x ω y ω z ] T = ϑ [ s ϕ c ϕ 0 ] T as a result of ψ: [ ω x ω y ω z ] T = ψ [ c ϕ s ϑ s ϕ s ϑ c ϑ ] T
Composition of elementary angular velocities ω = 0 s ϕ c ϕ s ϑ 0 c ϕ s ϕ s ϑ φ = T(φ) φ 1 0 c ϑ
Physical meaning of ω ω = [ π/2 0 0 ] T 0 t 1 ω = [ 0 π/2 0 ] T 0 t 1 ω = [ 0 π/2 0 ] T 1 < t 2 ω = [ π/2 0 0 ] T 1 < t 2 2 0 ωdt = [ π/2 π/2 0] T
Relationship between analytical Jacobian and geometric Jacobian v = [ ] I O ẋ = T O T(φ) A (φ)ẋ J = T A (φ)j A Geometric Jacobian quantities of clear physical meaning Analytical Jacobian differential quantities in the operational space
INVERSE KINEMATICS ALGORITHMS Kinematic inversion q(t k+1 ) = q(t k ) + J 1 (q(t k ))v(t k ) t drift of solution Closed-Loop Inverse Kinematics (CLIK) algorithm operational space error e = x d x ė = ẋ d ẋ = ẋ d J A (q) q find q = q(e): e 0
Jacobian (pseudo-)inverse Linearization of error dynamics q = J 1 A (q)(ẋ d + Ke) ė + Ke = 0 For a redundant manipulator q = J A (ẋ d + Ke) + (I J A J A) q a
Jacobian transpose q = q(e) without linearizing error dynamics Lyapunov method V (e) = 1 2 et Ke where V (e) > 0 e 0 V (0) = 0 V (e) = e T Kẋ d e T Kẋ = e T Kẋ d e T KJ A (q) q the choice q = J T A(q)Ke leads to V (e) = e T Kẋ d e T KJ A (q)j T A(q)Ke if ẋ d = 0 = V < 0 with V > 0 (asymptotic stability) if N(J T A ) = V = 0 if Ke N(J T A ) q = 0 with e 0 (stuck?)
If ẋ d 0 e(t) bounded (worth increasing norm of K) e( ) 0
Example J T P = 0 0 0 c 1 (a 2 s 2 + a 3 s 23 ) s 1 (a 2 s 2 + a 3 s 23 ) 0 a 3 c 1 s 23 a 3 s 1 s 23 a 3 c 23 null of J T P ν y ν x = 1 tanϑ 1 ν z = 0
Orientation error Position error e P = p d p(q) ė P = ṗ d ṗ Euler angles e O = φ d φ(q) ė O = φ d φ [ ] q = J 1 A (q) ṗd + K P e P φ d + K O e O handy to assign the time history φ d (t) anyhow requires computation of R = [ n s a ] Manipulator with spherical wrist compute q P = R W compute R T W R d = q O (Euler angles ZYZ)
Angle and axis R(ϑ, r) = R d R T orientation error e O = r sinϑ = 1 2 (n n d + s s d + a a d ) ė O = L T ω d Lω where L = 1 2 ( S(nd )S(n) + S(s d )S(s) + S(a d )S(a) ) ė = [ ėp ė O ] = = [ ṗ d J P (q) q L T ω d LJ O (q) q ] [ ] [ ] ṗ d I O L T J q ω d O L [ ] q = J 1 ṗ (q) d + K P e P L ( ) 1 L T ω d + K O e O
Unit quaternion Q = Q d Q 1 orientation error e O = ǫ = η(q)ǫ d η d ǫ(q) S(ǫ d )ǫ(q) [ q = J 1 ṗd + K (q) P e P ω d + K O e O ] quaternion propagation ω d ω + K O e O = 0 η = 1 2 ǫt ω ǫ = 1 2 (ηi S(ǫ)) ω study of stability V = (η d η) 2 + (ǫ d ǫ) T (ǫ d ǫ) V = e T O K Oe O
Second-order algorithms time differentiation of differential kinematics ẍ e = J A (q) q + J A (q, q) q joint accelerations solution q = J 1 A (ẍ (q) e J ) A (q, q) q ë + K D ė + K P e = 0
Comparison among inverse kinematics algorithms Three link planar arm p x p y φ x = k(q) = a 1c 1 + a 2 c 12 + a 3 c 123 a 1 s 1 + a 2 s 12 + a 3 s 123 ϑ 1 + ϑ 2 + ϑ 3 a 1 = a 2 = a 3 = 0.5 m J A = a 1s 1 a 2 s 12 a 3 s 123 a 2 s 12 a 3 s 123 a 3 s 123 a 1 c 1 + a 2 c 12 + a 3 c 123 a 2 c 12 + a 3 c 123 a 3 c 123 1 1 1
q i = [ π π/2 π/2] T rad p di = [ 0 0.5] T m φ = 0 rad desired trajectory p d (t) = [ ] 0.25(1 cosπt) 0.25(2 + sinπt) 0 t 4 φ d (t) = sin π 24 t 0 t 4 MATLAB simulation with Euler numerical integration q(t k+1 ) = q(t k ) + q(t k ) t and t = 1 ms
q = J 1 A (q)ẋ 2 x 10 3 pos error norm 0 x 10 5 orien error 1.5 0.2 [m] 1 0.5 0 0 1 2 3 4 5 [s] [rad] 0.4 0.6 0.8 1 0 1 2 3 4 5 [s]
q = J 1 A (q)(ẋ d + Ke) K = diag{500, 500, 100} 5 joint pos 10 joint vel [rad] 0 1 3 [rad/s] 5 0 1 2 2 5 3 5 0 1 2 3 4 5 [s] 10 0 1 2 3 4 5 [s] 1 x 10 5 pos error norm 0 x 10 8 orien error 0.8 1 [m] 0.6 0.4 [rad] 2 3 0.2 4 0 0 1 2 3 4 5 [s] 5 0 1 2 3 4 5 [s]
free φ (r = 2, n = 3) q = J P (ṗ d + K P e P ) K P = diag{500, 500} 5 x 10 6 pos error norm 0.5 orien 4 [m] 3 2 [rad] 0 0.5 1 0 0 1 2 3 4 5 [s] 1 0 1 2 3 4 5 [s] q = J T P (q)k Pe P K P = diag{500, 500} 0.01 pos error norm 0.5 orien 0.008 [m] 0.006 0.004 [rad] 0 0.5 0.002 0 0 1 2 3 4 5 [s] 1 0 1 2 3 4 5 [s]
q = J P (ṗ d + K P e P ) + (I J P J P) q a KP = diag{500, 500} manipulability measure w(ϑ 2, ϑ 3 ) = 1 2 (s2 2 + s 2 3) ( ) T w(q) q a = k a k a = 50 q 5 1 joint pos 5 1 joint pos [rad] 0 3 2 [rad/s] 0 2 3 5 0 1 2 3 4 5 [s] 5 0 1 2 3 4 5 [s] 5 x 10 6 pos error norm 1 manip 4 [m] 3 2 [rad] 0.95 0.9 1 0 0 1 2 3 4 5 [s] 0.85 0 1 2 3 4 5 [s]
distance from mechanical joint limits 3 ( qi q i ) 2 w(q) = 1 6 i=1 q im q im 2π q 1 2π π/2 q 2 π/2 3π/2 q 3 π/2 ( ) T w(q) q a = k a k a = 250 q [rad] joint 1 pos 6 4 2 0 2 4 6 0 1 2 3 4 5 [s] [rad] joint 2 pos 6 4 2 0 2 4 6 0 1 2 3 4 5 [s] 5 joint 3 pos 2 x 10 4 pos error norm 1.5 [rad] 0 [m] 1 0.5 5 0 1 2 3 4 5 [s] 0 0 1 2 3 4 5 [s]
STATICS Relationship between end-effector forces and moments (forces) γ and joint forces and/or torques (torques) τ with manipulator at equilibrium configuration elementary work associated with torques dw τ = τ T dq elementary work associated with forces dw γ = f T dp + µ T ωdt = f T J P (q)dq + µ T J O (q)dq = γ T J(q)dq elementary displacements virtual displacements δw τ = τ T δq δw γ = γ T J(q)δq
Principle of virtual work the manipulator is at static equilibrium if and only if δw τ = δw γ δq τ = J T (q)γ
Kineto-statics duality N(J) R (J T ) R(J) N (J T ) forces γ N(J T ) not requiring any balancing torques
Physical interpretation of Jacobian transpose CLIK algorithm ideal dynamics τ = q elastic force Ke pulling end-effector towards desired pose in operational space effective only if Ke / N(J T )
Velocity and force transformation [ ṗ2 ω 2 ] = [ I S(r12 ) O I ][ ṗ1 ω 1 ] r 12 = R 1 r 1 12 ṗ 1 = R 1 ṗ 1 1 ṗ 2 = R 2 ṗ 2 2 = R 1 R 1 2ṗ2 2 ω 1 = R 1 ω 1 1 ω 2 = R 2 ω 2 2 = R 1R 1 2 ω2 2
[ ṗ2 2 ω 2 2 ] = [ R 2 1 R1 2S(r1 12 ) ] [ ṗ1 1 O R1 2 ω 1 1 ] v 2 2 = J2 1 v1 1 by virtue of kineto-statics duality γ1 1 = J2 1 T γ2 2 [ f 1 ] [ 1 R 1 ][ 2 O f 2 ] 2 = S(r12)R 1 2 1 R2 1 µ 2 2 µ 1 1
MANIPULABILITY ELLIPSOIDS Velocity manipulability ellipsoid set of joint velocities of constant (unit) norm q T q = 1 redundant manipulator q = J (q)v v T( J(q)J T (q) ) 1 v = 1 Axes eigenvectors u i of JJ T = directions singular values σ i = λ i (JJ T ) = dimensions Volume proportional to w(q) = det ( J(q)J T (q) )
Two link planar arm Manipulability measure w = det(j) = a 1 a 2 s 2 max at ϑ 2 = ±π/2 max at a 1 = a 2 (for given reach a 1 + a 2 )
Velocity manipulability ellipses 1 0.5 [m] 0 0.5 1 0 0.5 1 1.5 2 [m] Singular values [m] 2.5 2 1.5 1 0.5 max min 0 0 0.5 1 1.5 2 [m]
Force manipulability ellipsoid set of joint torques of constant (unit) norm τ T τ = 1 γ T( J(q)J T (q) ) γ = 1 Kineto-statics duality a direction along which good velocity manipulability is obtained is a direction along which poor force manipulability is obtained, and vice versa
Two link planar arm Velocity manipulability ellipses 1 0.5 [m] 0 0.5 1 0 0.5 1 1.5 2 [m] Force manipulability ellipses 1 0.5 [m] 0 0.5 1 0 0.5 1 1.5 2 [m]
Manipulator mechanical transformer of velocities and forces from joint space to operational space transformation ratio along a direction for force ellipsoid α(q) = ( ) 1/2 u T J(q)J T (q)u transformation ratio along a direction for velocity ellipsoid β(q) = (u T( J(q)J T (q) ) 1 u ) 1/2 use of redundant degrees of freedom
Task compatibility of structure along a given direction writing velocity writing plane force throwing in bowling game force velocity throwing direction