12_Inverse Diff Kin Statics

7/28/2019 12_Inverse Diff Kin Statics

1/35

Robotics 1

Inverse differential kinematics

Statics

Prof. Alessandro De Luca

Robotics 1 1


2/35

Inversion of differential kinematics

find the joint velocity vector that realizes a desired end-effector generalized velocity (linear and angular)

problems near a singularity of the Jacobian matrix (high q) for redundant robots (no standard inverse of a rectangular matrix)

v = J(q) q.

q = J-1(q) v.

J square andnon-singular

in these cases, more robust inversion methods are needed

.

generalized velocity

Robotics 1 2


3/35

Behavior near a singularity

problems arise only whencommanding joint motion byinversion of a given Cartesianmotion task

figure shows a linear Cartesiantrajectory for a planar 2R robot

there is a sudden increase ofthe displacement/velocity of thefirst joint near 2=- (end-effector close to the origin),despite the required Cartesiandisplacement is small

q = J

-1(q) v.

Robotics 1 3


4/35

Simulation resultsplanar 2R robot in straight line Cartesian motion

q = J-1(q) v.

Robotics 1 4

a line from right to left, at =170 angle with x-axis,executed at constant speed v=0.6 m/s for T=6 s

start

end

regular case


5/35


Robotics 1 5

path at=170

regular

case

q1

q2

onlyintegration

error!!(10-10)


6/35


q = J-1(q) v.

Robotics 1 6


close to singular case

startend


7/35


Robotics 1 7

path at=178

close tosingular

case

q1

q2

increasedintegration

error

(210-9)

largepeakofq1

.


8/35


q = J-1(q) v.

Robotics 1 8


close to singular casewith joint velocity saturation at Vi=300/s

startend


9/35


Robotics 1 9

path at=178

close tosingular

case

q1

q2

actualposition

error!!(6 cm)

saturatedvalueofq1

.


10/35

Damped Least Squares method

inversion of differential kinematics as an optimization problem function H = weighted sum of two objectives (minimum error on

achieved end-effector velocity and minimum joint velocity)

= 0 when far enough from a singularity with > 0, there is a (vector) error(=v Jq) in executing the

desired end-effector velocity v(check that !), butthe joint velocities are always limited (damped)

JDLS can be used both for m = n and for m < n

equivalent expressions, but this one is more convenient in redundant robots!

Robotics 1 10

JDLS

= Im+ J J

T( )1

v

.


11/35


q = JDLS(q) v.

Robotics 1 11

a line from right to left, at =179.5 angle with x-axis,executed at constant speed v=0.6 m/s for T=6 s

a comparison of inverse and damped inverse Jacobian methodseven closer to singular case

startend

q = J-1(q) v.

startend

somepositionerror ...


12/35


q = JDLS(q) v.

Robotics 1 12

q = J-1(q) v.

here, a very fastreconfiguration of

first joint ...

a completely different inverse solution,while crossing the region

close to the folded singularity

path at=179.5


13/35


q = JDLS(q) v.

Robotics 1 13

q = J-1

(q) v.

extremely largepeakvelocityof first joint!!

very smoothjoint motionwith limited

joint velocities!

q1

q2


14/35


q = JDLS(q) v.

Robotics 1 14

q = J-1

(q) v.

minimumsingularvalue of

JJT

and I+JJT

some actualerror (25 mm)at singularitycrossing, laterrecovered by

feedback action

(v v+Ke)

increased

integrationerror(310-8)

they only differwhen damping

factor is non-zero

damping factoris chosennon-zero

only close tosingularity!


15/35

Use of the pseudo-inverse

such that

solution pseudo-inverse of J

a constrained optimization (minimum norm) problem

if , the constraint is satisfied ( is feasible) else where minimizes the error

orthogonal projection of on

Robotics 1 15


16/35

Properties of the pseudo-inverse

if rank = m = n:

if = m < n:

it is the unique matrix that satisfies the four relationships

it always exists and is computed in general numericallyusing the SVD = Singular Value Decomposition of J(e.g., with the MATLAB function pinv)

Robotics 1 16


17/35

Numerical example

Jacobian of 2R arm with l1 = l2 = 1 and q2 = 0 (rank = 1)

x

y

l1l2

is the minimum norm

joint velocity vector that

realizes q1

Robotics 1 17


18/35

General solution for m


19/35

Velocity manipulability

in a given configuration, we wish to evaluate how effectiveis the mechanical transformation between joint velocities andend-effector velocities how easily can the end-effector be moved in the various directions

of the task space

equivalently, how far is the robot from a singular condition we consider all end-effector velocities that can be obtained

by choosing joint velocity vectors ofunit norm

taskvelocitymanipulability ellipsoid (JJT)-1 if = m

Robotics 1 19

Rem: the core matrix of the ellipsoidequation vTA-1 v=1 is the matrix A!


20/35

Manipulability ellipsoidin velocity

direction of principal axes:(orthogonal) eigenvectorsassociated to

i

length of principal (semi-)axes:singular values of J (in its SVD)

in a singularity, the ellipsoidloses a dimension

(for m=2, it becomes a segment)

proportional to the volume of theellipsoid (for m=2, to its area)

planar 2R arm with unitary links

Robotics 1 20

w=

detJJ

T=

ii=1

m

0


21/35

best posture for manipulation

(similar to a human arm!)

Manipulability measure

planar 2R arm with unitary links: Jacobian J is square

Robotics 1 21

det JJT

( )= detJ

detJ

T= detJ =

ii=1

2

2 r r

max at 2=/2 max at r=2

1(J)

2(J)

full isotropy is never obtainedin this case, since it always 12


22/35

Statics - Transformation of forces

1

2

i

n

F

3

= vector of forces/torques at the joints F = vector of forces/torques exerted from the robot end-effector to the

environment (equal and opposite to the reaction forces/torques from

the environment to the robot end-effector)

which is the relation between and Fin a static equilibrium(i.e., with the robot remaining at rest)?

environment

Robotics 1 22


23/35

Statics - Equivalent formulations

1

2

i

n

F

3

which should be provided by the motors at the joints so that the robotend-effector applies F to the environment?

which at the joints balances a -F exerted from the environment? which is the force/torque felt at the joints in the presence of a -F

exerted at the robot end-effector?

in a given configuration

Robotics 1 23


24/35

Virtual displacements and work

infinitesimal (or virtual, i.e., satisfying all possible

constraints imposed on the system) displacementsat an equilibrium

without kinetic energy variation (zero acceleration)without dissipative effects (zero velocity)

the virtual work is the work done by all forces/torquesacting on the system for a given virtual displacement

dq1

dq2dqi

dqn

dq3

dp dt

= Jdq

Robotics 1 24


25/35

Principle of virtual work

the sum of the virtual works done by allforces/torques acting on the system = 0

1dq1

2dq2dp dt

= - FT Jdq3dq3 idqi

ndqn- F

- FT

principle ofvirtual work

Robotics 1 25


26/35

Duality between velocity and force

velocity(or displacement )

in thejoint space

generalized velocity(or E-E displacementin the Cartesian space

generalized forcesat the Cartesian E-E

forces/torquesat thejoints

J(q)

JT(q)

the singular configurationsfor the velocity map are the same

as those for the force map(J) = (JT)

)

Robotics 1 26


27/35

Dual subspaces of velocity and forcedefinitions

Robotics 1 27


28/35

Dual subspaces of velocity and forcepictorial view

J(q)

JT(q)

all are linear subspaces: origins always belong to the defined sets!

Robotics 1 28


29/35

Velocity and force singularitieslist of possible cases

= rank(J) = rank(JT

) min(m,n)

1. < m

2. = m

1. < n

2. = n

1. det J = 0

2. det J 0

m

n

Robotics 1 29


30/35

Example of singularity analysis

planar 2R arm with genericlink lengths l1 and l2 J(q) =

-l1

s1

-l2

s12

-l2

s12

l1c1+l2c12 l2c12det J(q) = l1l2s2

singularity at q2= 0 (arm straight)

singularity at q2= (arm folded)

J =-(l1+l2)s1 -l2s1(l1+l2)c1 l2c1

J =

(J) = (JT) =

(JT) = (J) =

-s1c1

c1s1

l1+l2

l2

l2

-(l1+l2)

1

0

l2-l1

l2

(J) and (JT

) as above

(J) =(JT) =

(JT)

(J)

(l2-l1)s1 l2s1

-(l2-l1)c1 -l2c1

0

1(for l1= l2 , )

l2

-(l2-l1) (for l1= l2 , )

Robotics 1 30


31/35

Force manipulability

in a given configuration, evaluate how effective is themechanical transformation between joint torques and end-effector forces how easily can the end-effector apply generalized forces (or

balance applied ones) in the various directions of the task space

in particular, in a singular configuration, there are directions in thetask space where an external force/torque is balanced by the robotwithout the need of any joint torque

we consider all end-effector forces that can be applied (orbalanced) by choosing joint torque vectors ofunit norm

taskforcemanipulability ellipsoid

same directions of the principal

axes of the velocity ellipsoid, butwith semi-axes ofinverse lengths

Robotics 1 31


32/35

Velocity and force manipulabilitya dual comparison

Robotics 1 32

planar 2R arm with unitary links

area det JJT( ) =1(J) 2(J)

area det JJT( )

1

=

1

1(J)

1

2(J)

note:

velocity and forceellipsoids have herea different scale

for a better view

Cartesian actuation task (highjoint-to-tasktransformation ratio):

preferred velocity (or force) directions are those where the ellipsoid stretches

Cartesian control task (low transformation ratio = high resolution):

preferred velocity (or force) directions are those where the ellipsoid shrinks


33/35

Velocity and force transformations

the same reasoning made for relating end-effector to joint forces/torques (static equilibrium + principle of virtual work) holds true alsofor relating forces and torques applied at different places of a rigidbodyand/or expressed in different reference frames

relation among generalized velocities

relation among generalized forces

Robotics 1 33


34/35

Example 1: 6D force/torque sensor

RFB

RFA

f

m

frame ofmeasure for the forces/torques(attached to the wrist sensor)

frame ofinterest for evaluatingforces/torques in a task

with environmental contact

JBA

JBAT

Robotics 1 34


35/35

Example 2: Gear reduction at joints

motor

transmission elementwith motion reduction ratio N:1

link

umm.

u.

m.

.

= N

u = Num

Robotics 1 35

another simple application

of the principle of virtual work!

Documents

12_Inverse Diff Kin Statics