Robotics 2 Robots with kinematic redundancy

Transcription

1 Robotics 2 Robots with kinematic redundancy Prof. Alessandro De Luca

2 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 of a robot is thus a relative concept, i.e., it holds with respect to a given task Robotics 2 2

3 Some tasks and their dimensions TASKS [for the end-effector (E-E)]! position in the plane! position in 3D space! orientation in the plane! pointing in 3D space! position and orientation in 3D space dimension M 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) Robotics 2 3

4 Typical cases of redundant robots! 6R robot mounted on a linear track/rail! for positioning and orienting its end-effector in 3D space! 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 manipulators (or 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 Uses of robot redundancy! avoid collision with obstacles (in Cartesian space)! or kinematic singularities (in joint space)! stay within the admissible joint ranges! increase manipulability in specified directions! 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 5

6 DLR robots: LWR-III and Justin 7R LWR-III lightweight manipulator: elastic joints (HD), joint torque sensing, 13.5 kg weight = payload!! Justin two-arm upper-body humanoid: 43R actuated = two arms (2!7) + torso (3 * ) + head (2) + two hands (2!12), 45 kg weight Robotics 2 * = one joint is dependent on the motion of the other two 6

7 Justin carrying a trailer video motion planning for DLR Justin robot in the configuration space, avoiding Cartesian obstacles and using robot redundancy Robotics 2 7

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

9 Motion cueing from redundancy video Max Planck Institute for Biological Cybernetics, Tübingen a 6R KUKA KR500 mounted on a linear track (+1P) with a sliding cabin (+1R), used as an emulation platform for dynamic human perception (N-M=2) Robotics 2 9

10 Self-motion video video 8R Dexter: self-motion with constant 6D pose of E-E (N-M=2) Nakamura s Lab, Uni Tokyo 6R robot with spherical shoulder in compliant tasks for the Cartesian E-E position (N-M=3) Robotics 2 10

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

12 video Mobile manipulators N u available commands < N generalized coordinates!! video Unicycle + 2R planar arm (N=5, N u =4): E-E trajectory control on a circle, with pointing task for first link (N u -M=1) Magellan + 3R arm (N=6, N u =5): E-E trajectory control on a circle, with E-E pointing task (N u -M=0!!) Robotics 2 12

13 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 13

14 HRP2 humanoid robot in action video whole body motion/navigation avoiding obstacles JRL CNRS-AIST Joint Research Lab, Toulouse-Tsukuba 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 14

15 Disadvantages of redundancy! potential benefits should be traded off against! 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 15

16 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 at the task level (e.g., not contributing to E-E motion)! these joint displacements can be chosen so as to improve/optimize in some way the behavior of the robotic system! self-motion: an arm reconfiguration in the joint space that does not change/affect the value of the task variables r! solutions are mainly sought at differential level (e.g., velocity) Robotics 2 16

17 . Redundancy resolution via optimization of an objective function! Local! Global given r(t) and q(t), t = kt s given r(t), t " [t 0, t 0 +T], q(t 0 ) optimization of H(q, q! ) optimization of H(q, q )d" t 0 + T # t 0 q (kt s ) ON-LINE q(t), t " [t 0, t 0 + T] q((k +1)T s ) = q(kt s ) + T s " q (kt s ) discrete-time form OFF-LINE relatively EASY (a LQ problem) quite DIFFICULT (nonlinear TPBV problems arise) Robotics 2 17

18 1 Local resolution methods three classes of methods for solving r = J(q) q! Jacobian-based methods (here, analytic Jacobian in general!) among the infinite solutions, one is chosen, e.g., that minimizes a suitable (possibly weighted) norm! 2 null-space methods a term is added to the previous solution so as not to affect execution of the task trajectory, i.e., belonging to the null-space N(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 18

19 Jacobian-based methods we look for a solution to J = 1 N M r = J(q) q q = K(q) r K = in the form minimum requirement for K: J(q)K(q)J(q) = J(q) ( K = generalized inverse of J) M N " r #$( J(q) ) J(q) [ K(q) r ] = J(q)K(q)J(q) q = J(q) q = r example: if J = [J a J b ], det(j a ) # 0, one such generalized inverse of J is K r = (actually, this is a stronger right-inverse) # % $ J "1 a 0 & ( ' Robotics 2 19

20 Pseudoinverse! 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 # = J T (J J T ) -1 ; else, it is computed numerically using the SVD (Singular Value Decomposition) of J (pinv of Matlab)! the pseudo-inverse joint velocity is the only that minimizes the 1 norm q 2 2 = 1 2 task error norm q = J # (q) r q T q... a very common choice: K = J # among all joint velocities that minimize the 1 r 2 " J(q) q 2 { (q)}! if the task is feasible ( r "Im J ), there will be no task error Robotics 2 20

21 Weighted pseudoinverse q = J # w (q) r another choice: K = # J w! if J is full (row) rank, = W -1 J T (J W -1 J T ) -1 q! the solution minimizes the weighted norm 1 2 T 2 q! = w 1 2 q! # J w Wq! W > 0, symmetric (often diagonal)! large weight W i small q i (e.g., weights can be chosen proportionally to the inverse of the joint ranges)! it is NOT a pseudoinverse (4th relation does not hold...) but shares similar properties. Robotics 2 21

22 Singular Value Decomposition (SVD)! the SVD routine of Matlab applied to J provides two orthonormal matrices U M!M and V N!N, and a matrix # M!N of the form $ # 1 & # " = & 2 & & %! # M 0 Mx(N-M) where = rank(j) " M, so that their product is ' ) ) ) J = U " V T " 1 # " 2 # # " $ > 0, " $+1 = = " M = 0 ) singular values of J (! the columns of U are eigenvectors of JJ T (associated to its nonnegative eigenvalues $ i2 ), the columns of V are eigenvectors of J T J! the last N - columns of V are a basis for the null space of J Jv i = " i u i (i =1,,#) Jv i = 0 (i = " +1,,N) Robotics 2 22

23 Computation of pseudoinverses! show that the pseudoinverse of J is equal to for any rank of J J # = V " # U T "# = # J w & 1 ( # ( 1 (! ( ( ( ( ( ( ' 0 (N%M)xM ) # $ *! show that matrix is obtained by solving the constrained linearquadratic (LQ) optimization problem (with W>0, symmetric, and assuming J of full rank) min 1 q 2 2 w s.t. J(q) q " r = 0 and that the pseudoinverse is a particular case for W=I! show that a weighted pseudoinverse of J can be computed by SVD/ pinv as J a = J W -1/2 = W -1/2 pinv(j a ) # J w Robotics 2 23

24 unconstrained minimization of a suitable objective function SOLUTION Singularity robustness Damped Least Squares (DLS) min q µ 2 q r 2 " J q 2 = H( q ) q = J DLS (q) r = J T! induces a robust behavior when crossing singularities, but in the basic version gives always a task error e = µ 2 JJ T + µ 2 I M r! thus, J DLS is not a generalized inverse K ( JJ T + µ 2 I ) "1 M r ( ) "1! using SVD of J=U % V T J DLS = V" DLS U T with " DLS = compromise between large joint velocity and task accuracy (as in N=M case) % # diag{ i # 2 i + µ } ( ' * ' 2 * ' &!&' diag{0} * ' * ' * ' 0 (N-M)x$ 0 (N-M)x(N-$) * & )! choice of a variable damping factor µ 2 (q) $ 0, as a function of the minimum singular $ m (q) value of J measure of distance to singularity! numerical filtering: introduces damping only/mostly in non-feasible directions for the task (see Maciejewski and Klein, J of Rob Syst, 1988) Robotics 2 24

25 Behavior of DLS solution q 1 2µ % 1/# for # $ 0 PINV " & ' 0 for # = 0 DLS " # # 2 + µ 2 µ " a. comparison of joint velocity norm with PINV (pseudoinverse) or DLS solutions in a direction of a singular vector u, when the associated singular value $ 0 PINV goes to infinity (and then is 0 at $ = 0) DLS peaks a value of 1/2µ at $ = µ (and then smoothly goes to 0...) ' b. graphical interpretation of the damping effect (here M=N=2, for simplicity) J 1 q = r 1 q 2 one equality constraint minimum norm solution q 1 q 2 J 1 q = r 1 two equality constraints exact (unique) solution J 2 q = r 2 q 1 q 2 J 1 q = r 1 J 2 q = r 2 approximate (damped) solution two (almost dependent) equality constraints H( q ) = µ2 q q 1 r 2 - J exact (ill-conditioned) solution Robotics 2 25 q 2

26 Numerical example of DLS solution planar 2R arm, unit links, close to a singular configuration (stretched): q 1 =45, q 2 =1.5 ) " q DLS =.472 % $ ' #.055& (µ 2 = 10-3 ) (& ( µ) " q DLS =.133 % $ ' #.066& (µ 2 = 10) " r = -1/ 2 % $ ' # 1/ 2& (J) [rad/s] # q * 1& = % ( $ "1' iso-level curves of H [rad/s] # q * 1& = % ( $ "1' exact solution (µ=0) H = µ2 q r 2 - J q 2 µ q e % H min Robotics 2 26

27 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 only a local property and avalanche phenomena may occur! typically lead to non-repeatable motion in the joint space! cyclic motions in task space do not map to cyclic motions in joint space after 1 tour q in q fin # q in t q(t) = q in + # K(q) r (")d" Robotics

28 Drift with Jacobian pseudoinverse! a 7R KUKA LWR4 robot moves in the vicinity of a human operator! we command a cyclic Cartesian path (only in position, M=3), to be repeated several times using the pseudoinverse solution! (unexpected) collision of a link occurs during the third cycle... video Robotics 2 28

29 a particular solution (here, the pseudoinverse) in (J T ) 2 Null-space methods.. general solution of J q = r even more in general q = K 1 r + I "K 2 J q = J # r + ( I " J # J) q 0 ( ) q 0... but with less nice properties! orthogonal. projection of q 0 in )(J) symmetric K 1, K 2 generalized inverses of J (J K i J = J) all solutions of the associated homogeneous equation Jq = 0 (self-motions) idempotent: [I J # J] 2 = [I J # J] [I J # J] # = [I J # J].. J # r orthogonal to [I J # J]q 0 properties of projector [I J # J]. how do we choose q 0? Robotics 2 29.

30 Geometric view on Jacobian null space in the space of velocity commands Jq = r Jq = r Jq = 0 q 2 J # r minimum norm solution final solution final solution (closest to ) q 0...its projection the null space (I - J # J) q 0 q 2 generic/preferred joint velocity.... q 0 null space correction q 1 J # r q 1 subspace ker {J} Jq = 0 a correction is added to the original pseudoinverse (minimum norm) solution i) which is in the null space of the Jacobian ii) and possibly satisfies additional criteria or objectives Robotics 2 30

31 necessary conditions + sufficient condition for a minimum Linear-Quadratic Optimization generalities 1 min x ( 2 x " x 0) T W( x " x 0 ) = H(x) s.t. J x = y T M x N " x L = #L #x " % L = Jx $ y = 0 = W( x $ x 0 ) + J T % = 0 " = ( JW #1 J T ) #1 ( Jx 0 # y) M!M invertible ( Jx! y) x!i R N, W > 0 (symmetric) M y!i R, &(J) = M L(x, ") = H(x) + " Lagrangian (with multipliers *) 2! x L = W > 0 T! 1 x x = x 0 " W "1 J T = Jx0! JW J "! y = T x " 1 T 0 " W J 0! ( JW "1 J T ) "1 ( Jx 0 " y) Robotics 2 31

32 PROBLEM Linear-Quadratic Optimization application to robot redundancy resolution 1 min q ( q 2 " q 0 ) T W q " J(q) q = r ( ) = H( q 0 q ). q 0 is a privileged joint velocity SOLUTION q = q 0 " W "1 J T ( ) ( JW "1 J T ) "1 J q 0 " r # J W q = J W# r + ( I " J # W J) q 0 minimum weighted. norm solution (for q 0 = 0) projection matrix in the null-space )(J) Robotics 2 32

33 projected gradient Projected Gradient (PG) q = J # r + ( I " J # J) q 0. the choice q 0 = + q H(q)! differentiable objective function realizes one step of a constrained optimization algorithm S q q 3 + q H. q - while executing the time-varying task r(t) the robot tries to increase the value of H(q) for a fixed r - q 2 N-dimensional S q = {q " I R N : f(q) = r - }. q = (I - J # J)+ q H (I - J # J)+ q H = 0 is a necessary condition q 1 of constrained optimality Robotics 2 33

34 Typical objective functions H(q)! manipulability (maximize the distance from singularities) H man (q) = det J(q)J T (q)! joint range (minimize the distance from the mid points of the joint ranges) q i " [q m,i,q M,i ] q i = q M,i + q m,i 2 H range (q) = 1 2N N ) i=1 q i " q i q M,i " q m,i! obstacle avoidance (maximize the minimum distance to Cartesian obstacles) # % $ & ( ' 2. q 0 = - + q H(q) also known as clearance H obs (q) = min a"robot b"obstacles a(q) #b 2 potential difficulties due to non-differentiability (this is a max-min problem) Robotics 2 34

35 Singularities of planar 3R arm the robot is redundant for a positioning task in the plane (M=2) H(q) = sin 2 q 2 + sin 2 q 3 unconstrained maxima of H(q) -, it is not H man but has the same minima iso-level curves of H(q) H(q) -, q 2, q 3 q 3, -, q 3, -, q 2, links of equal (unit) length q 2 independent from q 1! Robotics 2 35

36 Minimum distance computation in human-robot interaction LWR4 robot with a finite number of control points a(q) (8, including the E-E) a Kinect sensor monitors the workspace giving the depth of points b on obstacles that are fixed or moving (like a human) distances in 3D (and then the clearance) are computed in this case as min { } a" control points b " human body a(q) #b 2 Robotics 2 36

37 Comments on null-space methods! the projection matrix (I J # J) has dimension N!N, but only rank N-M (if J is full rank M), with some waste of information! actual (efficient) evaluation of the solution q = J # r + ( I " J # J) q 0 = q 0 + J # r - J but the pseudoinverse is needed anyway, and this is computationally intensive (SVD in the general case)! in principle, the complexity of a redundancy resolution method should depend only from the redundancy degree N M! 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 q 0 ( ) Robotics 2 37

38 Decomposition of joint space! if &(J(q)) = M, there exists a decomposition of the set of joints (possibly, after a reordering) M! M " q = q a % $ # q ' M b & N-M such that J a (q) = " f " q a! from the implicit function theorem, there exists then a function g f(q a, q b ) = r q a = g(r, q b ) is nonsingular with " g $ = # " f " q & b %" q a ' ) ( #1 " f = # J #1 a (q) J " q b (q) b! the N-M variables q b can be selected independently (e.g., they are used for optimizing an objective function H(q), reduced via the use of g to a function of q b only)! q a = g(r, q b ) are then chosen so as to correctly execute the task Robotics 2 38

39 Reduced Gradient (RG)! H(q) = H(q a,q b ) = H(g(r,q b ),q b ) = H (q b ), with r at current value! the Reduced Gradient (w.r.t. q b only, but still keeping the effects of this choice into account) is " q b H' = 0 " q b H' = #( J #1 a J ) T b I! algorithm [ ] " q H ( " # q b H!! ) is a compact (i.e., N-M dimensional) necessary condition of constrained optimality q b = " q b H' J a q a + J b q b = r "1 q a = J a ( r " J b ) q b step in the gradient direction of the reduced (N-M)-dim space satisfaction of the M-dim task constraints Robotics 2 39

40 Comparison between PG and RG! Projected Gradient (PG)! Reduced Gradient (RG) q = " q a % $ # q ' = " J a $ b & # 0 q = J # r + ( I " J # J)# q H (1 % ' " r + (J a $ & # I (1 J b % ' ( J a & ( ( (1 J ) T ) b I ) H q! RG is analytically simpler and numerically faster than PG, but requires the search for a non-singular minor (J a ) 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 methods provide always different evolutions Robotics 2 40

41 Analytic comparison PPR robot! q 1 q 2 q 3 # J = 1 0 "!s 3 & % $ 0 1!c ( = J a J b 3 ' RG: q = " 1 0% $ 0 1 ' $ # 0 0 ' & r + " $ $ # ( )!s 3 (!c 3 1 " q a = q 1 % $ # q ' q b = q 3 2 & q = J "1 # a & % ( $ 0 # r + "J a % ' $ I "1 J b % '(!s 3 (!c 3 1)) q H ' & ( ( "1 J b ) T I) ) qh & ( " J a ' PG: q = J # r + ( I " J # J)# q H J # = 1 1 +! 2 1 +! 2 2 # c 3! 2 s 3 c 3 & %! 2 s 3 c 3 1 +! 2 2 ( s 3 % $ "!s 3!c ( 3 ' ( I " J # J) = 1 1 +! 2! 2 2 # s 3 sym& % "! 2 s 3 c 3! 2 2 ( c 3 % $!s 3 "!c 3 1 ( ' always < 1!! Robotics 2 41

42 Joint range limits " % $ ' q = $ ' ( = T( $ ' $ # ' & absolute relative coordinates # & % ( " = % ( q = T -1 q % ( % $ ( ' "90 # $ i # +90 "90 # q i " q i"1 # +90 q 2 G S task: E-E linear path from S to G initial configuration q 4 q 1 numerical comparison among pseudoinverse (PS), projected gradient (PG), and reduced gradient (RG) methods Robotics 2 42

43 Numerical results minimizing distance from mid joint range joint 1 joint 2 joint 3 joint 4 upper limit steps of numerical simulation Robotics 2 43

44 Numerical results obstacle avoidance reduced gradient pseudoinverse q = J # r r r fixed obstacle x constrained maximization of H(q) = x sinq 4 " 3 # i=1! i sin( q 4 " q i ) Robotics 2 44

45 Numerical results self-motion for escaping singularities (optimal) max H(q) = 3 # i=1 sin 2 ( q i+1 " q i ) this function is actually NOT the manipulability index, but has the same minima (=0) r - 0 steps of numerical simulation RG is faster than PG (keeping the same accuracy on r) Robotics 2 45

46 3 Task augmentation methods! an auxiliary task is added (task augmentation) S f y (q) = y S. N-M corresponding to some desirable feature for the solution " r A = r $ % # y ' = & " f(q) % $ # f y (q) ' & r A =! a solution is chosen still in the form q = K A (q) r A " J(q) % $ # J y (q) ' q = J A (q) q & or adding a term in the null space of the augmented Jacobian J A Robotics 2 46 J A N M+S

47 Augmenting the task! advantage: better shaping of the inverse kinematic solution! disadvantage: algorithmic singularities are introduced when &(J) = M &(J y ) = S but &(J A ) < M+S it should always be "( J T ) # "( T J ) y = $ difficult to be obtained globally! rows of J AND rows of J y are all together linearly independent Robotics 2 47

48 Augmented task example r(t) N = 4, M = 2 absolute joint coordinates f y (q) = q 4 =,/2 (S = 1) last link to be held vertical Robotics 2 48

49 Extended Jacobian (S = N-M)! square J A : in the absence of algorithmic singularities, we can choose! the scheme is then repeatable q = J "1 A (q) r A! provided no singularities are encountered during a complete task cycle*! when the N-M conditions f y (q) = 0 correspond to necessary (& sufficient) conditions for constrained optimality of a given objective function H(q) (see RG method, slide #37), this scheme guarantees that constrained optimality is locally preserved during task execution! in the vicinity of algorithmic singularities, the execution of both the original task as well as the auxiliary task(s) are affected by errors (when using a DLS inversion) there exists an unexpected phenomenon in some 3R manipulators having generic kinematics: the robot may sometimes perform a pose change after a full cycle, even if no singularity has been encountered during motion (see J. Burdick, Mech. Mach. Theory, 30(1), 1995) Robotics 2 49

50 Extended Jacobian example MACRO-MICRO manipulator r(t) N = 4, M = 2 y(t) ( ) ( ) r = J q 1, q 4 y = J y q 1,q 2 q q " J A = * * % $ #* 0& ' 4!4 Robotics 2 50

51 Task Priority if the original (primary) task r = J(q) q has higher priority than the auxiliary (secondary) task r 2 = J 2 (q) q! we first address the task with highest priority q = J # r + (I " J # J)v = J # r +Pv! and then choose v so as to satisfy, if possible, also the secondary (lower priority) task r 2 = J 2 q = J 2 J # r + J 2 (I " J # J)v = J 2 J # r + (J 2 P)v the general solution for v takes the usual form v = (J 2 P) # ( r 2 " J 2 J # r ) + (I " (J 2 P) # (J 2 P))w w is available for execution of further tasks of lower (ordered) priorities Robotics 2 51

52 Task Priority (continue) " substituting the expression of v in q q = J # r +P(J 2 P) # ( r 2 " J 2 J # r ) +P(I " (J 2 P) # (J 2 P))w P [BP] # = [BP] # when matrix P is ( J 2 P) # possibly = 0 idempotent and symmetric " main advantage: highest priority task is ideally no longer affected by algorithmic singularities (damping is only in the secondary task) task 1: follow task 2: vertical third link WITHOUT task priority WITH task priority Robotics 2 52

53 A general task priority formulation! consider a large number p of tasks to be executed at best and with strict priorities by a robotic system having many dofs! everything should run efficiently in real time, with possible addition, deletion, swap, or reordering of tasks! a recursive formulation that reduces computations is convenient k-th task projector in the null-space of k-th task ( )! projector in the null-space of the augmented Jacobian of the first k tasks stack of first k tasks augmented Jacobian of first k tasks Robotics 2 53

54 Recursive solution with priorities - 1! start with the first task and reformulate the problem so as to provide always a solution, at least in terms of minimum error norm Robotics 2 54

55 Recursive solution with priorities - 2 prioritized solution up to task k-1 set of all solutions up to task k-1 LQ problem for k-th task initialization recursive formula (Siciliano, Slotine: ICAR 1991) prioritized solution up to task k correction needed when the solution up to task k-1 is not satisfying also task k over the steps, the search set is progressively reduced Robotics 2 55

56 Recursive solution with priorities properties and implementation! the solution considering the first k tasks with their priority satisfies also ( does not perturb ) the previous k-1 tasks since = (Maciejewski, Klein: IJRR 1985): check the four defining properties of a pseudoinverse! recursive expression also for the null-space projector (Baerlocher, Boulic: IROS 1998): for the proof, see Appendix A! in case the k-th task is not compatible with the previous ones (algorithmic singularity), use DLS instead of # in k-th solution... Robotics 2 56

57 A list of extensions (some still on-going research)! up to now, only basic redundancy resolution schemes! defined at first-order differential level (velocity)! it is possible to work in acceleration! useful for obtaining smoother motion! allows including the consideration of dynamics! seen within a planning, not a control perspective! take into account and recover errors in task execution by using kinematic control schemes! applied to robot manipulators with fixed base! extend to wheeled mobile manipulators! tasks specified only by equality constraints! add also linear inequalities in a complete QP formulation! in particular for humanoid robots in multiple complex tasks! consider hard limits in joint/command space Robotics 2 57

58 Resolution at acceleration level r = f(q) r = J(q) q! rewritten in the form J(q) q = r " J (q) q! = x to be chosen given (at time t) the problem is formally equivalent to the previous one, with acceleration in place of velocity commands! for instance, in the null-space method r = J(q) q + J (q) q. known q, q (at time t) q = J # (q) x + ( I - J # (q)j(q) ) q 0 solution with minimum acceleration norm 1 2 q 2 = " qh #K D q needed to damp/stabilize self-motions in the null space (K D > 0) Robotics 2 58

59 Dynamic redundancy resolution as Linear-Quadratic optimization problems! dynamic model of a robot manipulator (more later!) B(q) q + n(q, q ) = " J(q) q = x (= r " J (q) q ) N!N symmetric inertia matrix, positive definite for all q input torque vector (provided by the motors). Coriolis/centrifugal vector c(q,q) + gravity vector g(q) M-dimensional acceleration task! typical dynamic objectives to be locally minimized at (q,q) H 1 ( q ) = 1 2 " 2 = 1 2 q T B 2 (q) q + n T (q, q )B(q) q nt (q, q )n(q, q ). minimum torque norm H 2 ( q ) = 1 2 " 2 B #2 = 1 2 " T B -2 (q)" = 1 2 q T q + n T (q, q )B -1 (q) q nt (q, q )B -2 (q)n(q, q ) minimum torque (squared inverse inertia weighted) norm Robotics 2 59

60 Three dynamic solutions! by applying the general formula for LQ optimization problems ( ), check that the following closed-form expressions are obtained (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 )) good for short trajectories (in fact, it is still only a local solution!) for longer trajectories it may lead to a torque explosion (whipping effect) minimum (squared inverse inertia weighted) torque solution " 2 = B(q)J # (q) r - J (q) q + J(q)B -1 (q) n(q, q ) ( ) good performance in general, to be preferred ( ) in slide #31! try also the constrained minimization of the (simple) inverse inertia weighted norm of the torque... a solution with a leading J T (q) term is obtained: what is its physical interpretation? " 3 = J T (q)( J(q)B -1 (q)j T (q)) # ( r - J (q) q + J(q)B -1 (q) n(q, q )) Robotics 2 60

61 Kinematic control! given a desired M-dimensional task r d (t), in order to recover a task error e = r d r due to initial mismatch or due 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 K P, K D > 0) r d + K r d "r r in velocity-based... r r d + K D where r = f(q), r = J(q) q ( ) ( r d " r ) + K P ( r d "r) or in acceleration-based methods Robotics 2 61

62 Mobile manipulators! coordinates: q b of the base and q m of the manipulator! differential map: from available commands u b on the mobile base and u m on the manipulator to task output velocity q m r = f(q) (task output, e.g., the E-E pose) " q = q % b $ ' ( I R N # & q m q b = G(q b )u b q m = u m kinematic model of the wheeled base (with nonholonomic constraints) q b " u = u % b $ ' ( R N u I # & u m N u " N Robotics 2 62

63 Mobile manipulator Jacobian r = f(q) = f(q b, q m ) r = " f(q) q " q b + " f(q) q m = J b " q b (q) q b + J m (q) q m m ( ) u b = J b (q)g(q b )u b + J m (q)u m = J b (q)g(q b ) J m (q) = J NMM (q)u Nonholonomic Mobile Manipulator (NMM) Jacobian (M!N u )! most previous results follow by just replacing. J J NMM q u (redundancy if N u -M > 0) namely, the available velocity commands Robotics 2 63 # % $ u m & ( '

64 video Mobile manipulators Automatica Fair 2008 video car-like + 2R planar arm (N=6, N u =4): E-E trajectory control on a line (N u -M=2) with maximization of J NMM manipulability wheeled Justin: base with centered steering wheels (N b =3+4!2, N u =8) dancing in controlled but otherwise passive mode Robotics 2 64

65 Mobile manipulators issues in acceleration control with steering wheels! velocity commands of steering wheels do not affect the task velocity! solution: at the task acceleration level, using mixed commands! here 5: joint (3) and base linear (1) accelerations + steering velocity (1) video video without null-space stabilizing term with null-space stabilizing term car-like (rear-drive speed + front steering) + 3R arm (N=7, N u =5): E-E trajectory control on a circle (N u -M=2) Robotics 2 65

66 Quadratic Programming (QP) with equality and inequality constraints! minimize a quadratic objective function (typically positive definite, like when using norms of vectors) subject to linear equality and inequality constraints, all expressed in terms of joint velocity commands solution set, with only equality constraints within a given convex set QP complete formulation solution set, with only inequality constraints (non-negative) slack variables (possibly, also with prioritization of constraints) Robotics 2 66

67 Equality and inequality linear constraints feasible convex area (from inequalities) active inequality constraint NO exact solution here higher priority lower priority solution if ineq ineq inequality constraint inequality solution if ineq ineq minimum norm solution feasible convex area any priority order gives the same final solution equality constraint inequality equality (top priority) set of possible minimum error solutions if equality inequalities equality inequality... inequalities equality NO exact solution here inequality inequality feasible convex area Robotics 2 67

68 Equality and Inequality Tasks for the high-dof humanoid robot HRP2! a systematic task priority approach, with several simultaneous tasks video in any order of priority avoid the obstacle gaze at the object reach the object... while keeping balance! IEEE Int. Conf. on Robotics and Automation (ICRA) 2009 all subtasks are locally expressed by linear equalities or inequalities (possibly relaxed when needed) on joint velocities Robotics 2 68

69 Inclusion of hard limits in joint space Saturation in the Null Space (SNS) method! robot has limited capabilities: hard limits on joint ranges and/or on joint motion or commands (max velocity, acceleration, torque)! represented as box inequalities that can never be violated (at most, active constraints or saturated commands) kept separated from stack of tasks! (equality) tasks are usually executed in full (with priorities, if desired), but can be relaxed (scaled) in case of need (i.e., when robot capabilities are used at their limits)! saturate one overdriven joint command at a time, until a feasible and better performing solution is found Saturation in the Null Space = SNS! on-line decision: which joint commands to saturate and how, so that this does not affect task execution! for tasks that are (certainly) not feasible, SNS embeds the selection of a task scaling factor that preserves task direction with minimal scaling contains saturated joint scaling diagonal velocities Robotics 2 factor 0/1 matrix 69

70 Geometric view on SNS operation in the space of velocity commands NO exact solution here = = hard bounds (box inequality constraints) hard bounds (box inequality constraints) the total correction to the original pseudoinverse solution is always in the null space of the Jacobian (up to task scaling, if present) Robotics 2 70

71 Illustrative example - 1 consider a 4R robot with equal links of unitary length task: end-effector Cartesian position manipulator configuration differential map task Jacobian desired Cartesian velocity commanded joint velocity velocity limits Robotics 2 71

72 current configuration associated Jacobian Illustrative example - 2 desired end-effector velocity direct (velocity =) task scaling? saturating only the most violating velocity? Robotics 2 72

73 Joint velocity bounds joint space limits joint velocity bounds conversion: control sampling (piece-wise constant velocity commands) + max feasible velocities and decelerations to stay/stop within the joint range smooth velocity bound anticipates the reaching of a hard limit Robotics 2 73

74 SNS at velocity level Algorithm 1 initialization W : diagonal matrix with (j,j) element = 1 if joint j is enabled = 0 if joint j is disabled : vector with saturated velocities in correspondence of disabled joints s : current task scale factor s * : largest task scale factor so far Robotics 2 74

75 SNS at velocity level Algorithm 1 compute the joint velocity with initialized values check the joint velocity bounds compute the task scaling factor and the most critical joint if a larger task scaling factor is obtained save the current solution disable the most critical joint by forcing it at its saturated velocity Robotics 2 75

76 SNS at velocity level Algorithm 1 check if the task can be accomplished with the remaining enabled joints if NOT use the parameters that allow the largest task scaling factor and exit repeat until no joint limit is exceeded Robotics 2 76

77 Task scaling factor Algorithm 2 yields the best task scaling factor for the most critical joint in the current joint velocity solution Robotics 2 77

78 Simulation results 7-dof KUKA LWR IV [deg] [deg/s] [deg/s] [ms] Robotics 2 78

79 Simulation results for increasing V requested task move the end-effector through six desired Cartesian positions along linear paths with constant speed V Neglecting Constraint Task Scaling task redundancy degree = 7 3 = 4 robot starts at the configuration [deg] SNS approach Robotics 2 79

80 Experimental results KUKA LWR IV with hard joint-space limits IEEE Transactions on Robotics 2015 video Robotics 2 80

81 Variations of the SNS method SNS at the acceleration command level + consideration of multiple tasks with priority IEEE IROS 2012 video Robotics 2 81

82 Global solution for repeatable motion video (IEEE ICRA 2013)! for cyclic tasks: a bidirectional probabilistic search in the reduced space of extra degrees of freedom (dimension N-M)! including also collision avoidance (or other task constraints)! experiment with KUKA LWR4 at DIAG Robotics Lab (Dec 2012) video Robotics 2 82

83 Bibliography - 1! R. Cline, Representations for the generalized inverse of a partitioned matrix, J. SIAM, pp , 1964! T.L. Boullion, P. L. Odell, Generalized Inverse Matrices, Wiley-Interscience, 1971! A. Maciejewski, C. Klein, Obstacle avoidance for kinematically redundant manipulators in dynamically varying environments, Int. J. of Robotics Research, vol. 4, no. 3, pp , 1985! A. Maciejewski, C. Klein, Numerical filtering for the operation of robotic manipulators through kinematically singular configurations, J. of Robotic Systems, vol. 5, no. 6, pp , 1988! Y. Nakamura, Advanced Robotics: Redundancy and Optimization, Addison-Wesley, 1991! B. Siciliano, J.J. Slotine, A general framework for managing multiple tasks in highly redundant robotic systems, 5th Int. Conf. on Advanced Robotics, pp , 1991! P. Baerlocher, R. Boulic, Task-priority formulations for the kinematic control of highly redundant articulated structures, IEEE/RSJ Int. Conf. on Intelligent Robots and Systems, pp , 1998! P. Baerlocher, R. Boulic, An inverse kinematic architecture enforcing an arbitrary number of strict priority levels, The Visual Computer, vol. 6, no. 20, pp , 2004! A. Escande, N. Mansard, P.-B. Wieber, Fast resolution of hierarchized inverse kinematics with inequality constraints, IEEE Int. Conf. on Robotics and Automation, pp , 2010! O. Kanoun, F. Lamiraux, P.-B. Wieber, Kinematic control of redundant manipulators: Generalizing the taskpriority framework to inequality task, IEEE Trans. on Robotics, vol. 27, no. 4, pp , 2011! A. Escande, N. Mansard, P.-B. Wieber, Hierarchical quadratic programming, (including software), 26 Dec 2012 Robotics 2 83

84 Bibliography - 2! A. De Luca, G. Oriolo, The reduced gradient method for solving redundancy in robot arms, Robotersysteme, vol. 7, no. 2, pp , 1991! A. De Luca, G. Oriolo, B. Siciliano, Robot redundancy resolution at the acceleration level, Laboratory Robotics and Automation, vol. 4, no. 2, pp , 1992! A. De Luca, L. Lanari, G. Oriolo, Control of redundant robots on cyclic trajectories, IEEE Int. Conf. on Robotics and Automation, pp , 1992! A. De Luca, G. Oriolo, Reconfiguration of redundant robots under kinematic inversion, Advanced Robotics, vol. 10, n. 3, pp , 1996! A. De Luca, G. Oriolo, P. Robuffo Giordano, Kinematic control of nonholonomic mobile manipulators in the presence of steering wheels, IEEE Int. Conf. on Robotics and Automation, pp , 2010! F. Flacco, A. De Luca, O. Khatib, Motion control of redundant robots under joint constraints: Saturation in the null space, IEEE Int. Conf. on Robotics and Automation, pp , 2012! F. Flacco, A. De Luca, O. Khatib, Prioritized multi-task motion control of redundant robots under hard joint constraints, IEEE/RSJ Int. Conf. on Intelligent Robots and Systems, pp , 2012! F. Flacco, A. De Luca, Optimal redundancy resolution with task scaling under hard bounds in the robot joint space, IEEE Int. Conf. on Robotics and Automation, pp , 2013! F. Flacco, A. De Luca, "Fast redundancy resolution for high-dimensional robots executing prioritized tasks under hard bounds in the joint space, IEEE/RSJ Int. Conf. on Intelligent Robots and Systems, pp , 2013! F. Flacco, A. De Luca, O. Khatib, Control of redundant robots under hard joint constraints: Saturation in the null space, IEEE Transactions on Robotics, vol. 31, no. 3, pp , 2015! M. Cefalo, G. Oriolo, M. Vendittelli, "Planning safe cyclic motions under repetitive task constraints, IEEE Int. Conf. on Robotics and Automation, pp , 2013 Robotics 2 84

85 Appendix A - Recursive Task Priority proof of recursive expression for null-space projector! proof based on a result on pseudoinversion of partitioned matrices (Cline: J. SIAM 1964)! (i)! (i) + (ii) Q.E.D.! (ii)! if k-th task is scalar (Greville formula) Robotics 2 85 =

86 Experimental results KUKA LWR IV with joint-space limits + elbow and wrist Cartesian constraints IEEE ICRA 2012 video Robotics 2 86