Cartesian Control Analytical inverse kinematics can be difficult - PowerPoint PPT Presentation

Cartesian Control • Analytical inverse kinematics can be difficult to derive • Inverse kinematics are not as well suited for small differential motions • Let’s take a look at how you use the Jacobian to control Cartesian position

Cartesian control y x 2 2 Let’s control the position (not orientation) of y 3 q 3 the three link arm end effector: l y z 3 2 1 z l 2 3 x q x 3 2 1 ( ) ( ) − + − + − z s l c l c c l c l c l c s   l 1 1 1 2 2 3 23 1 2 2 3 23 3 1 23 z   ( ) ( ) 0 q = + − + − J c l c l c s l c l c l c s   1 1 2 2 3 23 1 2 2 3 23 3 1 23 x   0 + 0 l c l c l c   2 2 3 23 3 23 y 0 We can use the same strategy that we used before:      x   q  q x     1 1 − = 1 J = J             y q q y         2 2

Cartesian control  q y x 2 2 y 3 q   3  q δ q l δ x  y z 3 joint ctlr d + + 2 x 1 − 1 J d z l 2 3 x q x 3 2   1 joint position x FK  q z ( q ) sensor l 1 1 z 0 q 1 x 0   q x     y 1 − 1 = J   0     q y     2 However, this only works if the Jacobian is square and full rank… • All rows/columns are linearly independent, or • Columns span Cartesian space, or • Determinant is not zero

Cartesian control 3 x What if you want to control the two- dimensional position of a three-link 3 y manipulator? l 3 l q 2 2 3 x 1 x − − − − − − l s l s l s l s l s l s   1 y ( ) 1 1 2 12 3 123 1 1 2 12 1 1 = J q   q + + + 2 l c l c l c l c l c l c   1 1 2 12 3 123 1 1 2 12 1 1 2 y 0 y   q  q 1  1 x     0 x ( ) l Two equations of three  = J q q 1   0   z 2  y variables each…      q   3 This is an under-constrained system of equations. • multiple solutions • there are multiple joint angle velocities that realize the same EFF velocity.

Generalized inverse 3 x If the Jacobian is not a square matrix (or is not full rank), then the inverse doesn’t 3 y exist… l 3 l q 2 • 3 what next? 2 x 1 x 1 y q 2 2 y 0 y  =  We have: x J q q 1 0 x l 1 0 z # We are looking for a matrix such that: J  #  = q J x  =  x J q

Generalized inverse Two cases: • Underconstrained manipulator (redundant) • Overconstrained Generalized inverse:   x q • for the underconstrained manipulator: given , find any vector that minimizes s.t.  −  x J q   • x q for the overconstrained manipulator: given , find any vector  −  x J q s.t. Is minimized

Jacobian Pseudoinverse: Redundant manipulator 3 x Psuedoinverse definition: (underconstrained) 3  y x Given a desired twist, , find a vector of d  q  =  x d J q l joint velocities, , that satisfies 3 T  l q  =  f ( q ) q q 2 while minimizing 2 3 x 1 x 1 y q 2 Minimize joint velocities 2 y 0 y q = f ( z ) g ( z ) 0 1 Minimize subject to : 0 x l 1 0 z ∇ = λ ∇ f ( z ) g ( z ) Use lagrange multiplier method : z z f ( z ) This condition must be met when is at a minimum = g ( z ) 0 subject to

Jacobian Pseudoinverse: Redundant manipulator ∇ = λ ∇ f ( z ) g ( z ) z z T    = f ( q ) q q 1 Minimize 2    = − = g ( q ) J q x 0 Subject to   T ∇ = f ( q ) q  q (  ∇ = g q ) J  q  T T = λ q J  T q = λ J

Jacobian Pseudoinverse: Redundant manipulator  T q = λ J ( ) λ  T J = q JJ ( ) − 1 JJ T  λ = J q I won’t say why, but if is full rank, then J T JJ is invertible ( ) − 1 JJ T  λ = x  T q = λ J ( ) So, the pseudoinverse calculates the − 1  T T  = q J JJ x vector of joint velocities that  =  ( ) x d J q T JJ satisfies while − 1 # T = J J minimizing the squared magnitude q T   #   = q J x q of joint velocity ( ). • Therefore, the pseudoinverse calculates the least-squares solution.

Calculating the pseudoinverse The pseudoinverse can be calculated using two different equations depending upon the number of rows and columns: ( ) T JJ − 1 # T = J J Underconstrained case (if there are more columns than rows ( m<n )) ( ) − 1 # T T = J J J J Overconstrained case (if there are more rows than columns ( n<m )) # − 1 = J J If there are an equal number of rows and columns ( n=m ) These equations can only be used if the Jacobian is full rank; otherwise, use singular value decomposition (SVD):

Calculating the pseudoinverse using SVD Singular value decomposition decomposes a matrix as follows: Σ For an under-constrained matrix, is a T = Σ J U V diagonal matrix of singular values: σ  0 0 0 0 0 0  1   m × m × n × σ m n n 0 0 0 0 0 0   2   T = σ J U 0 0 0 0 0 0 V 3   0 0 0  0 0 0     σ 0 0 0 0 0 0   n  0 0 0 0  1 σ  1  0 0 0 0 1   σ 2 − # 1 T = Σ J V U   0 0 0 0 1 σ   3 # T = J V 0 0 0  0 U     0 0 0 0 1 σ   n 0 0 0 0 0     0 0 0 0 0  

Properties of the pseudoinverse Moore-Penrose conditions: # # # = J JJ J 1. # = JJ J J 2. T = ( ) # # JJ JJ 3. ( ) T # # = J J J J 4. Generalized inverse: satisfies condition 1 Reflexive generalized inverse: satisfies conditions 1 and 2 Pseudoinverse: satisfies all four conditions ( ) # # = J J Other useful properties of the pseudoinverse: ( ) ( ) T # # T = J J

Controlling Cartesian Position  q    q δ q δ x  joint ctlr # d J + + x d   joint position x FK  q ( q ) sensor Procedure for controlling position: x 1. Calculate position error: err δ = α x x 2. Multiply by a scaling factor: err err #  = α 3. Multiply by the velocity Jacobian pseudoinverse: q J x v err

Controlling Cartesian Orientation How does this strategy work for orientation control? R • Suppose you want to reach an orientation of d R • Your current orientation is c T • = You’ve calculated a difference: R R R cd c d • How do you turn this difference into a desired  # q = ω J angular velocity to use in ?  q   q ω δ q joint ctlr # d J R + + d  R FK  q joint position c ( q ) sensor

Controlling Cartesian Orientation You can’t do this: r • Convert the difference to ZYZ Euler angles: φθψ • Multiply the Euler angles by a scaling factor and # δ = α q J r pretend that they are an angular velocity: φθψ ∂ r φθψ ≠ J Remember that in general: ω ∂ q  q  r  q δ q φθψ joint ctlr # d J R + + d  R FK  q joint position c ( q ) sensor

The Analytical Jacobian 3 x 3 y If you really want to multiply the angular l Jacobian by the derivative of an Euler 3 l q 2 2 3 x angle, you have to convert to the 1 x 1 y “analytical” Jacobian: q 2 ∂ r 2 y φθψ = ( ) 0 y  T r J q A φθψ ω ∂ q q 1 0 x l 1 − 0 s c s   0 z φ φ θ ( )   = = J T r J 0 c s s J   φθψ ω φ φ θ ω For ZYZ Euler A A   1 0 c angles   θ Gimbal lock: by using an analytical Jacobian instead of the angular velocity Jacobian, you introduce the gimbal lock problems we talked about earlier into the Jacobian – this essentially adds “singularities” (we’ll talk more about that in a bit…)

Controlling Cartesian Orientation 3 x The easiest way to handle this Cartesian 3 y orientation problem is to represent the l 3 error in axis-angle format l q 2 3 2 x 1 x 1 y  δ = r k J q q 2 ω 2 y 0 y Axis angle delta rotation q 1 0 x l 1 0 z Procedure for controlling rotation: r 1. Represent the rotation error in axis angle format: err δ = α r r 2. Multiply by a scaling factor: err err 3. Multiply by the angular velocity Jacobian #  = α pseudoinverse: q J r ω err

Controlling Cartesian Orientation Why does axis angle work? • Remember Rodrigues’ formula from before: ( ) ( ) ( ) ( ( ) ) ( ) 2 S k θ = = + θ + − θ R e I S k sin S k 1 cos θ k axis angle ( ) p b  b b = ω p S Compare this to the definition of angular velocity: ( ) t b b S ω ω = R e The solution to this FO diff eqn is: t Therefore, the angular velocity gets integrated into an axis angle representation

Jacobian Transpose Control The story of Cartesian control so far:  =  x J q 1.  #  = q J x 2.