Robotics 2

Robots with kinematic redundancy

Prof. Alessandro De Luca

Redundant robots

direct kinematics of the task r = f(q)

f: Q R
joint space (dim Q = N)

task space (dim R = M)

a robot is (kinematically) redundant for the task if N > M

(more degrees of freedom than strictly needed for executing the task)

r may contain the position and/or the orientation of the end-effector or, more in general, be any parameterization of the task (even not in the Cartesian workspace) redundancy is thus a relative concept, i.e., with respect to a given task
2

Robotics 2

Some tasks and their dimensions

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

dimension M

position in the plane position in 3D space orientation in the plane pointing in 3D space position and orientation in 3D space
a planar robot with N=3 joints is redundant for the task of positioning its E-E in the plane (M=2), but NOT for the task of positioning AND orienting the E-E in the plane (M=3)

2 3 1 2 6

Robotics 2

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

4

Robotics 2

Uses of robot redundancy

avoid collision with obstacles (in Cartesian workspace) or kinematic singularities (in joint 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 be quantitatively measurable

Robotics 2

DLR robots: LWR-III and Justin

Justin two-arm upper-body humanoid: 43R actuated = two arms (27) +torso (3) 7R lightweight modular manipulator: elastic joints (HD), joint torque sensing, +head (2) +two hands (212), 45 kg weight 13.5 kg weight = payload!!
Robotics 2 6

Video Justin carrying a trailer

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

Video Dual-arm redundancy

DIS, Uni Napoli two 6R Comau robots, with one on a linear track (+1P) coordinated 6D motion using the null-space of the robot on the right (N-M=1)
Robotics 2 8

Videos Self-motion
Nakamuras Lab, Uni Tokyo

8R Dexter: self-motion with constant 6D pose of E-E (N-M=2)

Robotics 2

6R robot with spherical shoulder in compliant tasks for the Cartesian E-E position (N-M=3)
9

Video Obstacle avoidance

6R planar arm moving on a given geometric path for the E-E (N-M=4)
Robotics 2 10

Videos Mobile manipulators

commands Nu are less than generalized coordinates N!!

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

Magellan + 3R arm (N=6, Nu=5):

E-E trajectory control on a circle, with E-E pointing task (Nu-M=0!!)
11

Humanoid robots HRP2

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

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

Video Humanoid robot HRP2

whole body motion/navigation avoiding obstacles

E. Yoshida, C. Esteves, T. Sakaguchi, J.-P. Laumond, and K. Yokoi Smooth collision avoidance: Practical issues in dynamic humanoid motion IEEE Int. Conf. on Intelligent Robotics and Systems, 2006
Robotics 2 13

JRL CNRS-AIST Joint Research Lab, Toulouse-Tsukuba

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

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)
N=3>2=M r = constant E-E position

the robot arm may have internal displacements that are unobservable 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 that does not change/affect the value of the task variables r
15

Robotics 2

via optimization of an objective function

Redundancy resolution

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

)) (optimization of H(q, q

Local

Global
t0+T

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

(optimization of

t0

) d ) H(q,q

(t) q

ON-LINE

q(t),t [t0, t0 + T]
OFF-LINE
quite DIFFICULT (TPBV problems arise)

(kTs ) q((k + 1) Ts ) = q(kTs ) + Ts q

discrete time form relatively EASY (a linear problem)

Robotics 2

16

Local resolution methods

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

Jacobian-based methods (here, analytic Jacobian in general!)

among the infinite solutions, one is chosen, e.g., that minimizes a suitable (possibly weighted) norm

null-space methods

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

task augmentation methods

redundancy is reduced/eliminated by adding S N-M further auxiliary tasks (when S = N-M, the problem has been squared)

Robotics 2

17

Jacobian-based methods

= J(q) q in the form we look for a solution to r

J=
N M

= K(q) r q

K=
M

minimum requirement for K: J(q)K(q)J(q) = J(q) ( K = generalized inverse of J)

( J(q)) r

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

example: 1 J if J = [Ja Jb], det(Ja) 0, one such generalized inverse of J is K r = a 0 (actually, this is a stronger right-inverse)
Robotics 2 18

Pseudoinverse
# q = J (q) r

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

J# always exists, and is the unique matrix satisfying J 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 computed numerically using the SVD (Singular Value Decomposition) of J
(pinv of Matlab)

1 2 1 T minimizes the norm q = q q the solution q 2 2

19

Robotics 2

Weighted pseudoinverse
# q = Jw (q) p

another choice: K = J# w

-1 JT (J W-1 JT)-1 if J is full (row) rank, J# = W w

minimizes the weighted norm the solution q

1 2 1 T q w = q Wq 2 2
W > 0, symmetric
(often diagonal)

. large weight Wi small qi (e.g., weights can be chosen proportionally to the inverses of the joint ranges) it is NOT a pseudoinverse (4th relation does not hold ...)
20

Robotics 2

SVD and pseudoinverses

the SVD routine of Matlab applied to J provides two orthonormal matrices UMM and VNN , and a matrix MN of the form
1 =

0 Mx(N-M)

1 2 > 0, +1 = = M = 0

singular values of J

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

J# = V T #U
#

show that matrix J w is obtained by solving the constrained linearquadratic (LQ) optimization problem (with W>0) min 1 q 2 r =0 s .t . J(q)q w 2 and that the pseudoinverse is a particular case for W=I
21

Robotics 2

Limits of Jacobian-based methods

no guarantee that singularities are globally avoided during task execution

despite joint velocities are kept to a minimum, this is a local property and avalanche phenomena may occur cyclic motions in task space do not map to cyclic motions in joint space after 1 tour

may/typically lead to non-repeatable solutions

qin
t

qfin qin
( )d q(t ) = qin + K(q)r

Robotics 2

22

Singularity robustness
Damped Least Squares
2

unconstrained minimization SOLUTION

2 1 min q + Jq r q 2 2
T

) = H(q
T 1

damped least squares (DLS)

= JDLS (q) r = J ( JJ + I) r q

induces a robust behavior when crossing singularities, but in this basic 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-feasible directions of the task, using a modified SVD/pseudoinverse
23

Robotics 2

general solution of J q = r

Null-space methods . .

=Jr + IJ J q 0 q
# #

all solutions of the associated homogeneous equation Jq = 0 (self-motions)

a particular solution projection matrix in (J) (here, the pseudoinverse) symmetric

properties of [I J#J]

more in general

idempotent: [I J#J]2 = [I J#J] [I J#J]# = [I J#J] K1, K2 generalized inverses of J (J Ki J = J)

24

= K1 r + (I K 2 J) q 0 q
Robotics 2

. how to choose q0?

Linear-Quadratic Optimization
generalities

1 T min ( x x 0 ) W ( x x 0 ) = H( x ) x 2

x IR N , W > 0

s.t. J x = y

MxN

(symmetric)

y IR M, (J) = M
Lagrangian

L ( x , ) = H( x ) + T (Jx y )

L necessary x = x 0 W 1 J T xL = = W (x x 0 ) + J T = 0 conditions x + L = Jx y = 0 Jx 0 JW 1 J T y = 0 sufficient 2 xL = W > 0 condition for a 1 T 1 T 1 minimum 1 T 1 x = x 0 W J JW J = JW J (Jx 0 y ) (Jx 0 y )

Robotics 2

M M invertible

25

Linear-Quadratic Optimization
application to robot redundancy resolution

PROBLEM

1 T q 0 ) = H(q ) min (q q0 ) W (q 2 q =r J(q) q

=q 0 W J JW J q
1 T

q0 is a privileged joint velocity

SOLUTION

1 T 1

) (Jq

) r

J# W
# = J# 0 q r + I J ( W J) q W

minimum weighted . norm solution (for q0= 0)

Robotics 2

projection matrix in the null-space (J)

26

Projected Gradient (PG)

# # 0 q= J r + IJ J q

the choice q0 = q H(q) differentiable objective function realizes one step of a constrained optimization algorithm

q3 projected gradient Sq
Robotics 2

while executing the time-varying task r(t) the robot tries to increase the value of H(q) q H for a fixed r

q2

-} Sq = {q IRN: f(q) = r . q = (I - J#J)qH

q1

27

Typical objective functions H(q)

manipulability

(maximize the distance from singularities)

Hman (q) = det J(q) J T (q)

joint range

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

1 N qi qi Hrange (q) = q q 2N i=1 m, i M,i

2

q0 = - qH(q)

obstacle avoidance

(maximize the distance from Cartesian obstacles)

a robot b obstacles

Hobs (q) =

min

a(q) b

potential differentiability issues, since this is a max-min problem

28

Robotics 2

Singularities of planar 3R arm

for a positioning task in the plane (M=2), this robot is redundant

H(q) = sin q2 + sin q3

-

it is not Hman, but has the same minima

iso-level curves of H(q)

H(q)

q3

- q3 -

q2 q3 q2

q2

links of equal (unit) length

Robotics 2

independent from q1!

29

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

= J#r + I J# J q 0 = q 0 + J# (r - Jq 0) q
but the pseudoinverse is needed anyway, and this is computationally intensive

in principle, the complexity of a redundancy resolution method should depend only from the redundancy degree NM a constrained optimization method is available, which is known to be more efficient than the projected gradient (PG) ---at least when the Jacobian has full rank
30

Robotics 2

Reduced Gradient (RG)

if (J(q)) = M, there exists a partition of the set of joints (possibly, after a reordering) MM
qa M q = qb N-M

such that Ja (q) =

f is nonsingular qa

from the implicit function theorem, there exists then a function g

f(qa, qb) = r qa= g(r, qb) 1 f f g 1 = = J with a (q) Jb (q) q qb q a b

the N-M independent variables qb are used to optimize the objective function H(q) (reduced via the use of g to a function of qb only) by method the gradient qa = g(r, qb) are then chosen so as to correctly execute the task
31

Robotics 2

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
q b H' = J Jb

[(

1 a

I qH

( q b H !! )

ALGORITHM

b = q H' q b
a + Jbq b =r Jaq

step in the gradient direction of the reduced (N-M)-dim space satisfaction of the M-dim task constraints

Robotics 2

1 a = J q a (r Jbqb )

32

Comparison between PG and RG

Projected Gradient (PG)

= J# r + I J# J qH q

Reduced Gradient (RG)

1 1 a J J q 1 a a Jb + q = = r J a Jb b 0 q I

((

I qH

RG is analytically simpler and numerically faster than PG, but requires the search for a non-singular minor (Ja) of the robot Jacobian . if r = cost & N-M=1 same (unique) direction for q, but RG has automatically a larger optimization step size else, RG and PG provide always different evolutions

Robotics 2

33

Analytic comparison
PPR robot
1 0 s 3 J = = ( Ja 0 1 c 3
q3 q1 q2 RG:

Jb )

q qa = 1 q2

qb = q3

1 + c 1 2 # J = s 3c3 2 1 + s 3
Robotics 2
2 2 3

1 0 s 3 = 0 1 r + q c3 (s 3 0 0 1

1 1 J J 1 a a Jb = r + q Ja Jb 0 I

((

I qH

c3 1) qH

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

2s 3c3 1 + 2s 2 3 c3

2s 2 3 1 2 I J# J = s 3c3 2 1 + s 3

2c2 3 c3

sym 1
34

always < 1!!

Joint range limits

1 1 q = 1 1 0 1 1 1 0 0 1 1 0 0 = T 0 1

90 i +90

absolute relative coordinates

1 0 0 -1 1 0 = 0 -1 1 0 0 -1 0 0 q = T -1 q 0 1

90 qi qi1 +90
G q2 q1

task: E-E linear path from S to G

initial configuration

q4

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

minimizing distance from mid joint range

Numerical results

joint 1

joint 2

joint 3

joint 4

upper limit

steps of numerical simulation

Robotics 2 36

Numerical results
obstacle avoidance

reduced gradient
= J# r pseudoinverse q

fixed obstacle

constrained maximization of

H(q) = x sinq4 i sin(q4 qi )

i=1
Robotics 2 37

self-motion for escaping singularities

3

Numerical results
max H(q) = sin (qi+1 qi)
i=1 2

(optimal)

this function is actually NOT the manipulability index, but has the same minima (=0)

r0

steps of numerical simulation

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

Robotics 2 38

Task augmentation methods

S

an auxiliary task is added (task augmentation)

fy(q) = y

S N-M

corresponding to some desirable feature for the solution J(q) r f (q) A = = JA (q) q rA = = JA r q M+S y fy (q) Jy (q) N

a solution is chosen still in the form

= K A (q) r A q

or adding a term in the null space of the augmented Jacobian JA

Robotics 2

39

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

T T J J it should always be ( ) ( y)=

difficult to be obtained globally!

Robotics 2

rows of J AND rows of Jy are all together linearly independent

40

Augmented task
example r(t) N = 4, M = 2

absolute joint coordinates

fy(q) = q4 = /2
Robotics 2

(S = 1)

last link to be held vertical

41

Extended Jacobian

(S = N-M)

square JA: in the absence of algorithmic singularities, we can choose

1 A q = JA (q) r

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), this scheme guarantees that (local) optimality is preserved during task execution in the presence algorithmic singularities, the execution of the original task as well as of the auxiliary tasks is affected by errors

Robotics 2

42

Extended Jacobian
example MACRO-MICRO manipulator r(t) y(t) N = 4, M = 2

= J (q1 , q4 ) q r = Jy (q1 , q2 ) q y

* * JA = * 0

44

Robotics 2

43

Task Priority
= J(q)q has higher priority if the original (primary) task r
= Jy (q)q than the auxiliary (secondary) task y

we first address the task with highest priority

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

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

Jy (q)q = = Jy J#r + Jy (I J# J) v = Jy J# r + y Jy v
the general solution for v takes the usual form

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

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

Robotics 2

44

Task Priority

(continue)

substituting the expression of v in q

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

it is always C [BC]# = [BC]# for an idempotent C matrix

J# y

possibly = 0

main advantage: highest priority task no longer affected by algorithmic singularities

task 1: follow task 2: vertical third link

WITHOUT task priority

WITH task priority

Robotics 2

45

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 of dynamics

just seen as planning, not control methods

not taking into account errors in task execution it is convenient to use kinematic control can be extended also to wheeled mobile manipulators ... and of course to walking/standing humanoid robots
46

applied to robot manipulators with fixed base

Robotics 2

Resolution at acceleration level

r = f (q)

= J(q) q r

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

rewritten in the form

to be chosen

= = J(q) q r J(q) q x given (at time t)

known q, q (at time t)

the problem is formally equivalent to the former one, with acceleration in place of velocity commands for instance, in the null-space method

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

solution with minimum 1 2 q acceleration norm 2
Robotics 2

= qH K D q

needed to stabilize self-motions in the null space (KD > 0)

47

Dynamic redundancy resolution schemes

as Linear-Quadratic optimization problems

dynamic model of a robot manipulator (more later!)

+ n(q, q ) = B(q) q
NN symmetric inertia matrix, positive definite for all q

= (= ) J(q) q x r J(q) q
M-dimensional acceleration task

input torque vector (provided by the motors) . Coriolis/centrifugal vector c(q,q) + gravity vector g(q)

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

) = H1 (q 1 2
2

1 T 2 T + 1 nT (q, q ) n(q, q ) = q B (q)q + n (q, q) B(q) q 2 2 minimum torque norm

) = 1 H2 ( q 2
Robotics 2

2 B 2

1 1 T T -1 1 T -2 ) = T B -2 (q) = q q + n (q, q) B (q) q + n (q, q)B (q) n(q, q 2 2 2 minimum torque (squared inverse inertia weighted) norm
48

Dynamic solutions

by applying the general formula of slide #25 for LQ optimization problems, check that the following closed-form expressions are obtained (in the assumption of full rank Jacobian J) minimum torque solution
# - + J(q)B -1(q) n(q, q ) 1 = ( J(q)B -1(q)) r J(q)q

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

minimum (squared inverse inertia weighted) torque solution - + J(q)B -1(q) n(q, q ) = B(q) J# (q) r J(q)q
2

good performance in general, to be preferred

try the constrained minimization of the (simple) inverse inertia weighted norm of the torque ... you get a solution with a leading JT(q) term: what is its physical interpretation?
49

Robotics 2

Kinematic control

given a desired M-dimensional task rd(t), in order to recover a 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, by replacing (with diagonal matrix gains K > 0 or KP, KD > 0)

r r

d + K (rd r ) r

in velocity-based ... or in acceleration-based methods

d + K D (r d r ) + K P (rd r ) r

= J(q)q where r = f(q) , r

Robotics 2

50

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 r = f(q)
kinematic (task output, e.g., model of the the E-E pose) wheeled base (with nonholonomic b = G(qb ) ub constraints) q N

qm

qb q = IR qm
qb

m = um q
Nu N
51

Robotics 2

ub u = IR Nu um

Mobile manipulator Jacobian

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

= f (q) q b + f (q) q m = Jb (q) q b + Jm (q) q m r qb qm ub = Jb (q) G(qb )ub + Jm (q) um = ( Jb (q) G(qb ) Jm (q)) um = JNMM (q) u Nonholonomic Mobile Manipulator (NMM)
Jacobian (MNu)

Robotics 2

most previous results follow by just replacing . (redundancy if Nu-M > 0) J JNMM qu
namely, the only available commands
52

Videos Mobile manipulators

Automatica Fair 2008

Car-like + 2R planar arm (N=6, Nu=4):

E-E trajectory control on a line (Nu-M=2) with maximization of manipulability

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

dancing in controlled but otherwise passive mode
53

Robotics 2

issues in acceleration control with steering wheels velocity commands of steering wheels do not affect the task velocity solution: at the task acceleration level, use mixed commands here: joint (3) and base linear (1) accelerations + steering velocity (1)

Videos Mobile manipulators

without null-space stabilizing term

with null-space stabilizing term

Car-like (rear-drive speed + front steering) + 3R arm (N=7, Nu=5):

Robotics 2

E-E trajectory control on a circle (Nu-M=2)

54

Video Humanoid robot HRP2

a systematic task priority approach, with several simultaneous tasks

in any order of priority avoid the obstacle gaze at the object reach the object ... while keeping balance!

all subtasks are locally expressed by linear equalities or inequalities (possibly relaxed when needed) IEEE Int. Conf. on Robotics and Automation, 2009
Robotics 2 55