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

Let’s control the position (not orientation) of the three link arm end effector:

1

q

2

q

3

q

x y z

1

z

1

x

1

y

2

z

2

x

2

y

3

z

3

y

3

x

1

l

2

l

3

l

( ) ( ) ( ) ( )

          + − + − + − + − + − =

23 3 23 3 2 2 23 1 3 23 3 2 2 1 23 3 2 2 1 23 1 3 23 3 2 2 1 23 3 2 2 1

c l c l c l s c l c l c l s c l c l c s c l c l c l c c l c l s J

      =      

2 1

q q J y x           =      

−

y x J q q    

1 2 1

We can use the same strategy that we used before:

However, this only works if the Jacobian is square and full rank…

Cartesian control

1 −

J

+ ) (q FK 

d

x  x  q  x  δ q  δ +

d

q  q 

joint ctlr joint position sensor

1

q

2

q

3

q

x y z

1

z

1

x

1

y

2

z

2

x

2

y

3

z

3

y

3

x

1

l

2

l

3

l

      =      

−

y x J q q    

1 2 1

• All rows/columns are

linearly independent, or

• Columns span

Cartesian space, or

• Determinant is not zero

What if you want to control the two- dimensional position of a three-link manipulator?

Cartesian control

x y

1

q

z

2

q

3

q

x

1

y

1

y

2

x

2

x

3

y

3

1

l

2

l

3

l

( )

      + + + − − − − − − =

1 1 12 2 1 1 123 3 12 2 1 1 1 1 12 2 1 1 123 3 12 2 1 1

c l c l c l c l c l c l s l s l s l s l s l s l q J

( )

          =      

3 2 1

q q q q J y x     

Two equations of three variables each… This is an under-constrained system of equations.

• multiple solutions
• there are multiple joint angle velocities that realize the

same EFF velocity.

If the Jacobian is not a square matrix (or is not full rank), then the inverse doesn’t exist…

• what next?

Generalized inverse

x y

1

q

z

2

q

3

q

x

1

y

1

y

2

x

2

x

3

y

3

1

l

2

l

3

l

q J x   =

We are looking for a matrix such that:

#

J

We have:

x J q  

#

= q J x   =

Two cases:

• Underconstrained manipulator (redundant)
• Overconstrained

Generalized inverse

Generalized inverse:

• for the underconstrained manipulator: given , find any vector

that minimizes s.t.

• for the overconstrained manipulator: given , find any vector

s.t. Is minimized

q J x   − q  x  x  q  q J x   −

This condition must be met when is at a minimum subject to Psuedoinverse definition: (underconstrained) Given a desired twist, , find a vector of joint velocities, , that satisfies while minimizing x y

1

q

z

2

q

3

q

x

1

y

1

y

2

x

2

x

3

y

3

1

l

2

l

3

l

q J xd   = q  q q q f

T 

  = ) (

d

x 

Jacobian Pseudoinverse: Redundant manipulator

Use lagrange multiplier method:

) ( ) ( z g z f

z z

∇ = ∇ λ

Minimize subject to :

) (z f ) ( = z g ) ( = z g ) (z f

Minimize joint velocities

q q q f

T 

 

2 1

) ( = ) ( = − = x q J q g    ) ( ) ( z g z f

z z

∇ = ∇ λ

T q

q q f  



= ∇ ) ( J q g

q

= ∇ ) ( 



J q

T T

λ =  λ

T

J q = 

Minimize Subject to

Jacobian Pseudoinverse: Redundant manipulator

So, the pseudoinverse calculates the vector of joint velocities that satisfies while minimizing the squared magnitude

• f joint velocity ( ).
• Therefore, the pseudoinverse

calculates the least-squares solution.

λ

T

J q = 

( )λ

T

JJ q J = 

( )

q J JJ T 

1 −

= λ

( )

x JJ T 

1 −

= λ

I won’t say why, but if is full rank, then is invertible

J

T

JJ

λ

T

J q = 

( )

x JJ J q

T T

 

1 −

=

( )

1 # −

=

T T JJ

J J x J q  

#

= q J xd   = q qT  

Jacobian Pseudoinverse: Redundant manipulator

Calculating the pseudoinverse

The pseudoinverse can be calculated using two different equations depending upon the number of rows and columns:

( )

1 # −

=

T T JJ

J J

Underconstrained case (if there are more columns than rows (m<n))

( )

T T

J J J J

1 # −

=

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;

• therwise, use singular value decomposition (SVD):

Calculating the pseudoinverse using SVD

Singular value decomposition decomposes a matrix as follows:

T

V U J Σ =

T

U V J

n

                      =

1 1 1 1 #

3 2 1

σ σ σ σ

 For an under-constrained matrix, is a diagonal matrix of singular values:

m m× n n× n m× Σ

T

U V J

1 # −

Σ =

T n

V U J                 =

3 2 1

σ σ σ σ 

Properties of the pseudoinverse

( )

J J =

# #

( ) ( )

# # T T

J J =

Moore-Penrose conditions:

J J JJ =

#

# # #

J JJ J = ( )

# #

JJ JJ

T =

( )

J J J J

T # #

=

Generalized inverse: satisfies condition 1 Reflexive generalized inverse: satisfies conditions 1 and 2 Pseudoinverse: satisfies all four conditions 1. 2. 3. 4. Other useful properties of the pseudoinverse:

#

J

+ ) (q FK 

d

x  x  q  x  δ q  δ +

d

q  q 

joint ctlr joint position sensor

Controlling Cartesian Position

Procedure for controlling position:

• 1. Calculate position error:
• 2. Multiply by a scaling factor:
• 3. Multiply by the velocity Jacobian pseudoinverse:

err

x

err err

x x α δ =

err v

x J q α

#

= 

Controlling Cartesian Orientation

#

J

+ ) (q FK 

d

R

c

R q  ω q  δ +

d

q  q 

joint ctlr joint position sensor

How does this strategy work for orientation control?

• Suppose you want to reach an orientation of
• Your current orientation is
• You’ve calculated a difference:
• How do you turn this difference into a desired

angular velocity to use in ?

d

R

c

R

ω

#

J q = 

d T c cd

R R R =

Controlling Cartesian Orientation

#

J

+ ) (q FK 

d

R

c

R q  q  δ +

d

q  q 

joint ctlr joint position sensor

You can’t do this:

• Convert the difference to ZYZ Euler angles:
• Multiply the Euler angles by a scaling factor and

pretend that they are an angular velocity:

φθψ

α δ r J q

#

=

φθψ

r

φθψ

r

Remember that in general:

q r J ∂ ∂ ≠

φθψ ω

The Analytical Jacobian

x y

1

q

z

2

q

3

q

x

1

y

1

y

2

x

2

x

3

y

3

1

l

2

l

3

l

If you really want to multiply the angular Jacobian by the derivative of an Euler angle, you have to convert to the “analytical” Jacobian: 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…)

( )

q J r T q r

A



ω φθψ φθψ =

∂ ∂

( )

ω θ θ φ φ θ φ φ ω φθψ

J c s s c s c s J r T J

A A

          − = = 1

For ZYZ Euler angles

x y

1

q

z

2

q

3

q

x

1

y

1

y

2

x

2

x

3

y

3

1

l

2

l

3

l

The easiest way to handle this Cartesian

• rientation problem is to represent the

error in axis-angle format

q J rk 

ω

δ =

Controlling Cartesian Orientation

Axis angle delta rotation Procedure for controlling rotation:

• 1. Represent the rotation error in axis angle format:
• 2. Multiply by a scaling factor:
• 3. Multiply by the angular velocity Jacobian

pseudoinverse:

err

r

err err

r r α δ =

err

r J q α

ω #

= 

Controlling Cartesian Orientation

Why does axis angle work?

• Remember Rodrigues’ formula from before:

( )

( ) ( ) ( ) ( ) ( )

θ θ

θ θ

cos 1 sin

2

− + + = = k S k S I e R

k S k

axis angle

( ) p

S p

b b b

ω = 

Compare this to the definition of angular velocity: The solution to this FO diff eqn is:

( )t

S t b

b

e R

ω ω =

Therefore, the angular velocity gets integrated into an axis angle representation

Jacobian Transpose Control

q J x   =

The story of Cartesian control so far: 1. 2.

x J q  

#

=

Jacobian Transpose Control

Here’s another approach:

err T err x

x e

2 1

= ( ) q x x q e

T err

∂ ∂ − = ∂ ∂

T

q e q         ∂ ∂ − ← α  ( )

T T err

q x x q       ∂ ∂ = α 

( )

err T

x q x q ∂ ∂ = α 

( )

err T v

x J q α = 

Start with a squared position error function (assume the poses are represented as row vectors) Gradient descent: take steps proportional to in the direction of the negative gradient.

x x x

ref err

− =

Position error:

α

• rientation error: axis angle orientation of reference pose in

the current end effector reference frame:

Jacobian Transpose Control

The same approach can be used to control orientation:

( )

ref curr T

k J q

ω

α = 

ref currk

Jacobian Transpose Control

So, evidently, this is the gradient of that

• Jacobian transpose control descends a squared

error function.

• Gradient descent always follows the steepest

gradient

err T err x

x e

2 1

=

( )

err T x

J q = 

Jacobian Transpose v Pseudoinverse

What gives?

• Which is more direct? Jacobian pseudoinverse or

transpose?

ξ

T

J q =  ξ

#

J q = 

• r

They do different things:

• Transpose: move toward a reference pose as quickly as

possible

• One dimensional goal (squared distance meteric)
• Pseudoinverse: move along a least squares reference twist

trajectory

• Six dimensional goal (or whatever the dimension of the

relevant twist is)

The pseudoinverse moves the end effector in a straight line path toward the goal pose using the least squared joint velocities.

• The goal is specified in terms of the

reference twist

• Manipulator follows a straight line path in

Cartesian space

d

x The transpose moves the end effector toward the goal position

• In general, not a straight line path in

Cartesian space

• Instead, the transpose follows the gradient

in joint space

d

x

Jacobian Transpose v Pseudoinverse

Up until now, we’ve used the Jacobian in the twist equation,

Using the Jacobian for Statics

q J = ξ

Interestingly, you can also use the Jacobian in a statics equation:

w J T = τ

Cartesian wrench:

        = m f w

force moment (torque) Joint torques

Using the Jacobian for Statics

It turns out that both wrenches and twists can be understood in terms of a representation of displacement known as a screw.

• Therefore, you can calculate work by integrating the dot

product:

( )

            = ⋅ + ⋅ =

∫ ∫

m f v m f v W

T

ω ω q W

T 

∫

= τ

Work in Cartesian space Work in joint space Conservation of energy:

            = ∫

∫

m f v q

T T

ω τ 

Using the Jacobian for Statics

Incremental work (virtual work)

            = ω τ v m f q

T T 

q J m f q

T T

        = τ J m f

T T

      = τ       = m f J T τ w J T = τ w J T = τ q J = ξ

vs Wrench-twist duality:

Note that twist can be represented in different reference frames:

      = ω ξ

b b b

v       = ω ξ

k k k

v

1 2

ω ω

b b

=

Consider two reference frames attached to the same rigid body:

1

T

b 2

T

b 12 1 1 2

r v v

b b b

× + = ω

Twist: converting between reference frames

Twist: converting between reference frames

1 2

ω ω

b b

=

1

T

b 2

T

b 12 1 1 2

r v v

b b b

× + = ω

( )

            − =      

1 1 12 2 2

ω ω

b b b b

v I r S I v

( )

                  −       =       ω ω

1 1 1 1 12 2 2 2 2

v R R I r S I R R v

b b T b T b

( )

            − =       ω ω

1 1 1 2 12 1 1 2 1 2 2 2

v R r S R R v

Twist in frame 2 Twist in frame 1

Wrench can also be represented in different reference frames:

      = m f w

b b b

      = m f w

k k k 1

T

b 2

T

b

Wrench: converting between reference frames

Use the virtual work argument to derive the relationship:

1

T

b 2

T

b

Wrench: converting between reference frames

            =            

1 1 1 1 1 1 1 1 2 2 2 2 2 2 2 2

ω ω v m f v m f

T T

( )

            =             −      

1 1 1 1 1 1 1 1 1 1 1 1 1 2 12 1 1 2 1 2 2 2 2 2

ω ω v m f v R r S R R m f

T T

( )

T T

m f R r S R R m f       =       −      

1 1 1 1 1 2 12 1 1 2 1 2 2 2 2 2

( )

            =      

2 2 2 2 2 1 2 1 12 1 2 1 1 1 1 1

m f R R r S R m f

Use a 6-axis load cell bisecting the second link to calculate wrenches at the end effector (the tip of the last link)

Converting wrenches: Example

6 axis load cell

3

l 2

2

l

3

q

eff

y

          − = 1

3 3 3 3

c s s c Rsensor

eff

                − − = 2 2

3 2 3 2 3

s l c l l rsensor

eff

eff

x

sensor

y

sensor

x

Converting wrenches: Example

( )

              =        

sensor sensor sensor sensor sensor eff sensor eff sensor eff eff sensor eff eff eff eff eff

m f R R r S R m f

,

6 axis load cell

3

l 2

2

l

3

q

eff

x

eff

y

sensor

x

sensor

y                               − − − − + − =        

sensor sensor sensor sensor eff eff eff eff

m f s l c l c l s l c s c l l s c s l c s s c m f 1 2 2 2 2 1

2 3 2 2 3 2 3 3 3 3 3 3 3 2 3 3 3 3 2 3 3 3 3