02 kinematic redundancy

7/28/2019 02 Kinematic Redundancy

1/55

Robotics 2

Prof. Alessandro De Luca

Robots with

kinematic redundancy


2/55

Redundant robots

Robotics 2 2

direct kinematics of the taskr = f(q)

a robot is (kinematically) redundant for the task ifN>M(more degrees of freedom than strictly needed for executing the task)

r may contain the position and/or the orientation of theend-effector or, more in general, be any parameterizationof the task(even not in the Cartesian workspace)

redundancy is thus a relative concept, i.e., with respectto a given task

f: Q R

task space (dim R = M)joint space (dim Q = N)


3/55

Some tasks and their dimensions

position in the plane position in 3D space orientation in the plane pointing in 3D space position and orientation in 3D space

2

3

1

2

6

a planar robot with N=3 joints is redundant for the task

ofpositioning its E-E in the plane (M=2), but NOT for thetask ofpositioning AND orienting the E-E in the plane (M=3)

TASK [for the end-effector (E-E)] dimension M

Robotics 2 3


4/55

Typical cases of redundant robots

6R robot mounted on a linear track/rail 6-dof robot used for arc welding tasks

task does not prescribe the final roll angle of the welding gun

manipulator on a mobile base dexterous robot hands team of cooperating (mobile) robots humanoid robots...kinematic redundancy is not the only type

redundancy of components (actuators, sensors) redundancy in the control/supervision architecture

Robotics 2 4


5/55

Uses of robot redundancy

avoid collision with obstacles (in Cartesian workspace) or kinematic singularities (injoint space) increase manipulability in specified directions stay within the feasible joint ranges

uniformly distribute/limit joint velocities and/or accelerations minimize energy consumption or needed motion torques optimize execution time increase dependability with respect to faults ...

all objectives should bequantitatively measurable

Robotics 2 5


6/55

DLR robots: LWR-III and Justin

7Rlightweight modular manipulator:elastic joints (HD), joint torque sensing,

13.5 kg weight = payload!!

Justin two-arm upper-body humanoid:

43Ractuated =two arms (27) +torso (3)

+head (2) +two hands (212),45 kg weight

Robotics 2 6


7/55

Video Justin carrying a trailer

Robotics 2 7

motion planning in the configuration space,avoiding Cartesian obstacles and using robot redundancy


8/55

Video Dual-arm redundancy

Robotics 2 8

two 6RComau robots, with one on a linear track (+1P)coordinated 6D motion using the null-space of the robot on the right (N-M=1)

DIS, Uni Napoli


9/55

Videos Self-motion

8RDexter: self-motion withconstant 6D pose of E-E (N-M=2)

6Rrobot with spherical shoulderin compliant tasks for the

Cartesian E-E position (N-M=3)

Robotics 2 9

Nakamuras Lab, Uni Tokyo


10/55

Video Obstacle avoidance

6Rplanar arm moving on a given geometric path for the E-E (N-M=4)

Robotics 2 10


11/55

Videos Mobile manipulators

Magellan + 3R arm (N=6, Nu=5):E-E trajectory control on a circle,

with E-E pointing task (Nu-M=0!!)

Unicycle + 2R planar arm (N=5, Nu=4):E-E trajectory control on a circle,

with pointing task for first link (Nu-M=1)

Robotics 2 11

commands Nu are less than generalized coordinates N!!


12/55

Humanoid robots HRP2

built by Kawada Industries, Inc. for the Humanoid Robotics Project (HRP)sponsored by the Japanese Ministry of Economy, Trade, and Industry

Robotics 2 12

a hyper-redundant system, but with a few non-actuated dofs (at the base!)


13/55

Video Humanoid robot HRP2

JRL CNRS-AIST Joint Research Lab, Toulouse-TsukubaE. Yoshida, C. Esteves, T. Sakaguchi, J.-P. Laumond, and K. Yokoi

Smooth collision avoidance: Practical issues in dynamic humanoid motionIEEE Int. Conf. on Intelligent Robotics and Systems, 2006

Robotics 2 13

whole body motion/navigation avoiding obstacles


14/55

Disadvantages of redundancy

potential benefits should be traded off a greater structural complexity of construction

mechanical (more links, transmissions, ...)

more actuators, sensors, ...

costs

more complicated algorithms for inverse kinematics

and motion control

Robotics 2 14


15/55

Inverse kinematics problem

find q(t) that realizes the task: f(q(t)) = r(t)(at all times t) infinite solutions exist when the robot is redundant

(even for r(t)= r =constant)

the robot arm may have internal displacements that areunobservable during task execution (e.g., in the E-E motion)

these internal displacements can be chosen so as to improve/optimize in some way the system behavior

self-motion: an arm reconfiguration in the joint space thatdoes not change/affect the value of the task variables r

r = constantE-E positionN = 3 > 2 = M

Robotics 2 15


16/55

given r(t) and q(t), t=kTs

Redundancy resolutionvia optimization of an objective function

Local Global

)t(q

given r(t), t [t0, t0+T], q(t0)

(optimization of H(q, ))q (optimization of )H(q,q)d

t 0

t 0 +T

q(t), t [t0, t0 + T]ON-LINE

OFF-LINEq((k+1)Ts) = q(kTs)+ Ts q(kTs)

.

Robotics 2 16

discrete time form

relatively EASY

(a linear problem)

quite DIFFICULT

(TPBV problems arise)


17/55

Local resolution methods

Jacobian-based methods (here, analytic Jacobian in general!)among the infinite solutions, one is chosen, e.g., that minimizes asuitable (possibly weighted) norm

null-space methodsa term is added to the previous solution so as not to affect thetask trajectory execution, i.e., belonging to the null-space (J(q))

task augmentation methodsredundancy is reduced/eliminated by adding S

N-M furtherauxiliary tasks (when S = N-M, the problem has been squared)

three classes of methods for solving r = J(q)q

1

2

3

Robotics 2 17


18/55

Jacobian-based methods

we look for a solution to in the form

q =K(q)r K =

M

N

r = J(q) q

minimum requirement for K: J(q)K(q)J(q) = J(q)

( K = generalized inverse of J)

r J(q)( )

example:if J = [Ja Jb], det(Ja) 0, one such generalized inverse of J is

(actually, this is a stronger right-inverse)

Kr=

Ja1

0

J(q) K(q)r[ ] = J(q)K(q)J(q) q = J(q)q = r

1

J =

N

M

Robotics 2 18


19/55

Pseudoinverse

J# always exists, and is the unique matrix satisfyingJ J# J = J J# J J# = J#

(J J#)T = J J# (J# J)T = J# J

if J is full (row) rank, J# = JT (J JT)-1 ; else, it is computednumerically using the SVD (Singular Value Decomposition) of J

(pinvof Matlab)

the solution minimizes the norm

q = J# (q)r

q 12q 2 = 1

2qTq

... a very common choice: K = J#

Robotics 2 19


20/55

Weighted pseudoinverse

if J is full (row) rank, = W-1 JT (J W-1 JT)-1 the solution minimizes the weighted norm

large weight Wi small qi(e.g., weights can bechosen proportionally to the inverses of the joint ranges)

it is NOT a pseudoinverse (4th relation does not hold ...)

p)q(Jq #w =

q

qWq2

1q

2

1 T2w

=

#

wJ

another choice: K = #wJ

W > 0, symmetric(often diagonal)

.

Robotics 2 20


21/55

SVD and pseudoinverses

the SVD routine of Matlab applied to J provides two orthonormalmatrices UMM and VNN , and a matrix MN of the form

where= rank(J) M, so that their product is J=UT V show that the pseudoinverse of J is always equal to

for any rankof J

show that matrix is obtained by solving the constrained linear-quadratic (LQ) optimization problem (with W>0)

and that the pseudoinverse is a particular case for W=I

min1

2q

w

2s.t. J(q)q r = 0

#

wJ

Robotics 2 21

J#=V

T

#U

1

2

> 0,

+1==

M= 0

=

1

2

M

0Mx(N-M)

singular values of J


22/55

Limits of Jacobian-based methods

no guarantee that singularities are globally avoided duringtask execution despite joint velocities are kept to a minimum, this is a local property

and avalanche phenomena may occur

may/typically lead to non-repeatable solutions cyclic motions in task space do not map to cyclic motions injoint space

after1 tour

qinqfin qin

q(t) = qin + K(q)r()d0

t

Robotics 2 22


23/55

Singularity robustnessDamped Least Squares

induces a robust behavior when crossing singularities, but in thisbasic version it gives always a task error (same as in N=M case)

thus, JDLS is not a generalized inverse K choice of a variable damping factor (q)0 , as a function of the

minimum singular value of J measure of the singularity distance

numerical filtering: introduces damping only in the non-feasibledirections of the task, using a modified SVD/pseudoinverse

minq

2q

2+1

2Jq r

2=H(q)

q= JDLS(q) r = J

T JJT + I( )1

rSOLUTION

unconstrainedminimization

damped leastsquares (DLS)

Robotics 2 23


24/55

Null-space methods

q = J#r + I J#J( ) q0

projection matrix in (J)a particular solution

(here, the pseudoinverse) symmetric idempotent: [I J#J]2 = [I J#J][I J#J]# = [I J#J]

properties of

[I J#J]

general solution of Jq=r. .

all solutions of the associatedhomogeneous equation Jq=0

(self-motions)

.

q =K1 r + I K2J( ) q0K1, K2 generalized inverses of J

(JKiJ=J)

more in general

2

how to choose q0?.

Robotics 2 24


25/55

= JW1JT( )

1Jx0 y( )

xL =L

x

T

=W x x0( )+ JT = 0

L = Jx y = 0

Linear-Quadratic Optimizationgeneralities

minx

1

2x x

0( )T

W x x0( ) =H(x)

(symmetric)0W,Rx N >I

s.t. Jx = y MRy I , (J) = MM x N

( )yJx)x(H),x(LT

+=

T1

0JWxx

=

0yJJWJx T10 =

x = x0 W

1JT JW1JT( )1

Jx0 y( )

M M invertible

Lagrangian

necessaryconditions

0WL2

x >=

+sufficient

conditionfor aminimum

Robotics 2 25


26/55

Linear-Quadratic Optimizationapplication to robot redundancy resolution

minq

12q q0( )

TW q q0( ) =H(q)

q = q0 W1JT JW1JT( )

1 Jq0 r( )

#

WJ

q= JW#r + I JW

# J( )q0projection matrix in

the null-space (J)

J(q) q = r

minimum weighted normsolution (for q0= 0)

.

SOLUTION

PROBLEMq0 is a

privilegedjoint velocity

.

Robotics 2 26


27/55

Projected Gradient (PG)

q = J

#

r + I J#

J( )q0

q1

q2

q3qH

Sq .q-

Sq = {q RN: f(q) = r}I -

for a fixed r-

the choice q0 = q H(q) differentiable objective function

realizes one step of a constrained optimization algorithm

.

while executing the time-varying taskr(t)the robot tries to increase the value ofH(q)

q = (I - J#J)qH.

projectedgradient

Robotics 2 27


28/55

Typical objective functions H(q)

manipulability (maximize the distance from singularities)

joint range (minimize the distance from the mid points of the joint ranges)

obstacle avoidance (maximize the distance from Cartesian obstacles)

Hman(q) = det J(q)JT(q)

Hrange(q) =1

2N

qi qiqM,i qm,i

2

i=1

N

Hobs(q) = minarobotbobstacles

a(q)b2 potential differentiability

issues, since this isa max-min problem

q0 =- qH(q).

Robotics 2 28


29/55

Singularities of planar 3R arm

H(q) = sin2 q2 + sin2 q3

q2

q3-

-

H(q)

q2

q3

- q2

-

q3

iso-level curves of H(q)

for a positioning task

in the plane (M=2),this robot is redundant

it is not Hman,

but has the same minima

independent from q1!

Robotics 2 29

links of equal (unit) length


30/55

Comments on null-space methods

the projection matrix (I J#J) has dimension NN, but only rank N-M(if J is full rank M), with some waste of information

actual (efficient) evaluation of the solution

but the pseudoinverse is needed anyway, and this is computationally

intensive

in principle, the complexity of a redundancy resolution method shoulddepend only from the redundancy degree NM

a constrained optimization method is available, which is known to bemore efficient than the projected gradient (PG) ---at least when the

Jacobian has full rank

q = J#r + I J#J( )q0 = q0 + J# r - Jq0( )

Robotics 2 30


31/55

Reduced Gradient (RG)

if(J(q)) = M, there exists a partition of the set of joints (possibly,after a reordering)

from the implicit function theorem, there exists then a function gf(qa, qb) = r qa= g(r, qb)

the N-M independent variables qb are used to optimize the objectivefunction H(q) (reduced via the use of g to a function of qb only) bythe gradient method

qa = g(r, qb) are then chosen so as to correctly execute the task

q =qa

qb

M

N-MJa(q) =

f

qasuch that is nonsingular

MM

g

qb=

f

qa

1

f

qb= Ja

1(q)Jb(q)with

Robotics 2 31


32/55

Reduced Gradient algorithm

H(q) = H(qa,qb) = H(g(r,qb),qb) = H(qb), with r at current value the reduced gradient (w.r.t. qb only, but still keeping the effects

of this choice into account) is

ALGORITHM

qbH' = Ja

1 Jb( )T

I qH

qb =qbH'step in the gradient direction of

the reduced (N-M)-dim space

satisfaction of the M-dim

task constraintsqa = Ja

1 r Jbqb( )

Jaqa + Jbqb = r

( qbH !! )

Robotics 2 32


33/55

Comparison between PG and RG

Projected Gradient (PG)

Reduced Gradient (RG)

RG is analytically simpler and numerically faster than PG,but requires the search for a non-singular minor (Ja) of therobot Jacobian

ifr = cost & N-M=1 same (unique) direction for q, butRG has automatically a larger optimization step size

else, RG and PG provide always different evolutions

q=

qa

qb

=

Ja1

0

r+

Ja1Jb

I

Ja

1

Jb( )

T

I( )qH

q = J# r + I J#J( )qH

.

Robotics 2 33


34/55

Analytic comparisonPPR robot

q1q2

q3

J = 1 0 s30 1 c

3

= Ja Jb( ) qa = q1q2 qb = q3

PG: q = J#r + I J#J( )qH

J#=

1

1+ 2

1+ 2c3

22s3c3

2s3c3

1+ 2s3

2

s3

c3

I J#

J( )=

1

1+ 2

2s3

2 sym

2

s3c32

c32

s3 c3 1

q =

1 0

0 1

0 0

r +

s3

c3

1

s3 c3 1( )qH

RG: q =Ja1

0

r +

Ja1Jb

I

Ja

1Jb( )T

I( )qH

always < 1!!Robotics 2 34


35/55

Joint range limits

S

G

90 i +90

numerical comparison among pseudoinverse (PS),projected gradient (PG), and reduced gradient (RG) methods

q1

q2q4

task:

E-E linear pathfrom S to G

q =

1 0 0 0

1 1 0 0

1 1 1 0

1 1 1 1

= T

=

1 0 0 0-1 1 0 0

0 -1 1 0

0 0 -1 1

q = T -1 q

90 qi qi1 +90absoluterelativecoordinates

Robotics 2 35

initial configuration


36/55

Numerical resultsminimizing distance from mid joint range

joint 1 joint 2

joint 3 joint 4

Robotics 2 36

upper

limit

steps of numerical simulation


37/55

Numerical resultsobstacle avoidance

pseudoinverse

reduced gradient

q = J# r

xfixedobstacle

H(q) = x sinq4 isin q4 qi( )i=1

3

constrained maximization of

r

r

Robotics 2 37


38/55

Numerical resultsself-motion for escaping singularities

max H(q) = sin2 qi+1 qi( )

i=1

3

steps ofnumerical simulation

r0

RG is faster than PG(keeping the same accuracy on r)

(optimal)this function is actually NOT

the manipulability index,but has the same minima (=0)

Robotics 2 38


39/55

Task augmentation methods

an auxiliary taskis added (task augmentation)fy(q) = y

corresponding to some desirable feature for the solution

a solution is chosen still in the form

or adding a term in the null space of the augmentedJacobian JA

3

S S N-M

rA=J(q)

Jy (q)

q = JA(q)q

q =KA(q) rA

rA=r

y

=

f(q)

fy (q)

N

M+SJA

Robotics 2 39


40/55

Augmenting the task

advantage: better shaping of the inverse kinematic solution

disadvantage: algorithmic singularities are introduced when

(J) = M (Jy) = S but (JA) < M+S

it should always be JT( ) JyT( ) =

difficult to be obtained globally!

rows of J AND rows of Jyare all together

linearly independent

Robotics 2 40


41/55

Augmented taskexample

r(t)

N = 4, M = 2

fy(q) = q4 = /2 (S = 1)last link to be held vertical

absolute joint coordinates

Robotics 2 41


42/55

Extended Jacobian (S = N-M)

square JA: in the absence ofalgorithmic singularities, we can choose

the scheme is then clearly repeatable when fy(q) = 0 corresponds to the necessary (& sufficient) conditions

for constrained optimality of a given objective function H(q), thisscheme guarantees that (local) optimality is preserved during task

execution

in the presence algorithmic singularities, the execution of theoriginal taskas well as of the auxiliary tasks is affected by errors

q = JA1

(q)rA

Robotics 2 42


43/55

Extended Jacobianexample

r(t)

y(t)

MACRO-MICRO manipulator

N = 4, M = 2

r = J q1,q4( )q

y = Jy q1,q2( ) q

JA

=

* *

* 0

44

Robotics 2 43


44/55

Task Priority

we first address the task with highest priority

and then choose v so as to satisfy, if possible, also the secondary(lower priority) task

the general solution for v takes the usual form

r=

J(q)qy = Jy (q)q

q = J#r + (I J#J)v

y = Jy (q)q = JyJ

#r + Jy (I J#J)v = JyJ

# r + Jyv

v = Jy# (y JyJ#r) + (I Jy# Jy )w

w is available for execution of further tasks of lower (ordered) priorities

Robotics 2 44

if the original (primary) task has higher prioritythan the auxiliary (secondary) task


45/55

Task Priority (continue)

substituting the expression of v in

q = J#r + (I J#J) Jy#(y JyJ

#r) + (I J#J)(I Jy# Jy )w

q

Jy#it is always C[BC]# =[BC]#

for an idempotent C matrixpossibly = 0

main advantage: highest priority task no longer affectedby algorithmic singularities

WITHOUTtask priority

WITHtask priority

task 1: follow

task 2: verticalthird link

Robotics 2 45


46/55

Extensions

up to now, redundancy resolution schemes were: defined at first-order differential level (velocity)

it is possible to work in acceleration useful for obtaining smoother motion

allows to include consideration ofdynamics just seen as planning, not control methods

not taking into account errors in task execution it is convenient to use kinematic control

applied to robot manipulators with fixed base can be extended also to wheeled mobile manipulators ... and of course to walking/standing humanoid robots

Robotics 2 46


47/55

Resolution at acceleration level

rewritten in the form

the problem is formally equivalent to the former one,with acceleration in place of velocity commands

for instance, in the null-space method

r=

f(q) r=

J(q)q r = J(q) q+J(q) q

J(q) q = r J(q)q = x

to be chosen given

(at time t)

known q, q

(at time t)

.

q = J#(q) x + I - J#(q)J(q)

( )q0

solution with minimum

acceleration norm

1

2q

2

=qHKD q

needed

to stabilizeself-motions

in the null space(KD > 0)

Robotics 2 47


48/55

Dynamic redundancy resolution schemesas Linear-Quadratic optimization problems

dynamic model of a robot manipulator (more later!)

J(q) q= x (= r J(q)q)

NN symmetric inertia matrix,positive definite for all q

Coriolis/centrifugal vector c(q,q)

+ gravity vector g(q)

minimum torque norm

H1(q) =1

2

2=

1

2qTB2(q)q+nT(q,q)B(q) q+

1

2nT(q,q)n(q,q)

Robotics 2 48

B(q) q+n(q,q) =

input torque vector(provided by the motors)

.

M-dimensionalacceleration task

typical dynamic objectives to be locally minimized at (q,q)

H2(q) =1

2

B2

2=1

2TB-2(q) =

1

2qTq+nT(q,q)B-1(q) q+

1

2nT(q,q)B-2(q)n(q,q)

minimum torque (squared inverse inertia weighted) norm

.


49/55

Dynamic solutions

Robotics 2 49

by applying the general formula of slide #25 for LQ optimizationproblems, checkthat the following closed-form expressions areobtained (in the assumption of full rank Jacobian J)

minimum torque solution

1 = J(q)B-1(q)( )

#r - J(q)q + J(q)B-1(q)n(q,q)( )

minimum (squared inverse inertia weighted) torque solution

2 =B(q)J#(q) r - J(q)q + J(q)B-1(q)n(q,q)( )

try the constrained minimization of the (simple) inverse inertiaweighted norm of the torque ... you get a solution with a leadingJT(q) term: what is its physical interpretation?

good for short trajectorieshowever, for longer trajectories it leads to torque explosion (whipping effect)

good performance in general, to be preferred


50/55

Kinematic control

given a desired M-dimensional task rd(t), in order to recovera task error e = rd r due to initial mismatch or due later to

disturbances inherent linearization error in using the Jacobian (first-order motion) discrete-time implementation

we need to close a feedback loop on task execution, byreplacing (with diagonal matrix gains K > 0 or KP, KD > 0)

where ,

r in velocity-based ...rd+K r

d r( )

rd+K

Drd r( )+KP rd r( )r or in acceleration-based methods

r = f(q)r = J(q)q

Robotics 2 50


51/55

Mobile manipulators

combine coordinates qb of the base + qm of the manipulator use differential map from available commands ub on the

mobile base + um on the manipulator to task output velocity

qb =G(qb)ub

qm =um

r = f(q)(task output, e.g.,the E-E pose)

qb

qm

kinematic

model of thewheeled base

(withnonholonomicconstraints)q=

qbqm

RNI

u=ub

um

RNuI Nu N

Robotics 2 51


52/55

Mobile manipulator Jacobian

most previous results follow by just replacing

r =f(q)

qbqb +

f(q)

qmqm = Jb(q) qb + Jm(q)qm

= Jb(q)G(qb)ub + Jm(q)um = Jb(q)G(qb) Jm(q)( )

ub

um

= JNMM(q)u

r = f(q) = f(qb, qm)

Nonholonomic Mobile Manipulator (NMM)

Jacobian (MNu)

J JNMM q u. (redundancy ifNu-M > 0)

namely, the only

available commandsRobotics 2 52


53/55

Videos Mobile manipulators

Wheeled Justin: base with centeredsteering wheels (Nb=11, Nu=8)

dancing in controlled

but otherwise passive mode

Car-like + 2R planar arm (N=6, Nu=4):E-E trajectory control on a line (Nu-M=2)

with maximization of manipulability

Robotics 2 53

Automatica Fair 2008


54/55

Videos Mobile manipulatorsissues in acceleration control with steering wheels

Car-like (rear-drive speed + front steering) + 3R arm (N=7, Nu=5):E-E trajectory control on a circle (Nu-M=2)

Robotics 2 54

velocity commands of steering wheels do not affect the task velocity

solution: at the task acceleration level, use mixedcommands here: joint (3) and base linear (1)accelerations + steering velocity (1)

without null-space stabilizing term with null-space stabilizing term


55/55

Video Humanoid robot HRP2

Robotics 2 55

a systematic task priority approach, with several simultaneous tasks

IEEE Int. Conf. on Robotics and Automation, 2009

in any order of priorityavoid the obstaclegaze at the object reach the object ...while keeping balance!

all subtasks are locally

expressed by linear

equalities or inequalities(possibly relaxedwhen needed)

02 kinematic redundancy

Documents