Smooth Transition Between Tasks on a Kinematic Control Level: Application to Self Collision Avoidance for Two Kuka LWR Robots

Smooth Transition Between Tasks on a Kinematic Control Level: Application to Self Collision Avoidance for Two Kuka LWR Robots Tadej Petrič and Leon Žlajpah Abstract A common approach for kinematically redundant robot consist of a definition of several tasks properly combined in priority. However, in some cases the task priority needs to be changed in order to successfully perform the desired task without changing the initial strategy. In this paper we propose a novel method for control of kinematically redundant robots, where we focus on a smooth, continuous transition between the primary and the secondary task. The method is based on a nullspace velocity control algorithm, which is essential for achieving good behaviour of a redundant robotic system. The effectiveness of the proposed system is demonstrated on a robotic system with two Kuka LWR robots. I. INTRODUCTION One of the important issues of the new generation of service and humanoid robots is the kinematic redundancy that they posses [. The kinematic redundancy is characterised by additional degrees of freedom (DOFs) with respect to those required to execute a given task. Most robotic arms mounted on mobile platforms, as well as humanoid robots, are kinematically redundant. A redundant robot has the ability to move the end-effector along the same trajectory using different configurations of the mechanical structure. This provides means for solving sophisticated motion tasks such as avoiding obstacles, avoiding singularities, optimizing manipulability, minimizing joint torques, etc [2. As a consequence, the dexterity of the system is significantly increased, which is essential to accomplish complex tasks [. A common approach of control algorithms for redundant systems in the past decades are acceleration-based redundancy-resolution schemes. Here the appropriate joint accelerations are generated in order to accomplish the primary task and null-space motion as a secondary task [3, [. Velocity null-space control is an appropriate way to control internal motion of redundant manipulator. However, it is also well established that certain acceleration-based control schemes exhibit instabilities [4. An alternative approach is the augmented acobian as introduced in [5, where the secondary task is added to the primary task so as to obtain a square acobian matrix that can be inverted. The main drawback of this technique are the so called algorithmic singularities. They occur when the secondary task causes conflict with the primary task. Khatib deeply investigated the use of second-order inverse kinematic either on torque or acceleration level, starting from [6 to recent task-prioritised humanoid applications [7, [8. T. Petrič and L. Žlajpah are with the Department for automation, biocybernetics and robotics, ozef Stefan Institute, Ljubljana, Slovenia {tadej.petric, leon.zlajpah}@ijs.si In general it is common for task-priority close-loop inverse kinematic algorithms that they do not always allows simple transitions or changing priority levels between the tasks [2, [9. For example, if the task is to move the end effector from point A to point B and if the obstacle is in the desired path, then the robot must adapt its motion properly to successfully perform the desired task, i.e. to avoid the obstacle and get to point B. This can be done by changing the desired trajectory [, [, which in general means changing the strategy. For example, using time-varying artificial potential fields as introduced in [2 or by applying algorithm that can change the priority levels of different tasks. In this paper we propose a novel formulation of the primary and secondary task, so that the desired movement of the robot is in fact a secondary task. The primary task is only observed if we approach a pre-defined threshold. While far form the threshold, our algorithm allows direct control of separate joints. If we approach the threshold, the primary task smoothly takes over and only allows joint control in the null-space of the primary task. The algorithm allows smooth transition in both ways - between observing the primary task and the secondary in the null-space, or just the unconstrained secondary task. This allows unconstrained joint movement while not close to the threshold. We also give a Lyapunovbased stability discussion for the proposed algorithm, by following the work of Antonelli [3. The usefulness of our approach is demonstrated in different scenarios using two Kuka LWR robots. The rest of the paper is organised as follows: In section II we propose a novel algorithm for continuous transition between tasks. In section III the stability of the proposed control algorithm is discussed. In section IV we evaluate the proposed approach by using it for obstacle avoidance tasks. Conclusions and summary are given in section V. II. KINEMATICS The robotic systems under study are serial manipulators. We consider only redundant systems, which have more DOF than needed to accomplish the desired task, i.e. the dimension of the joint space n exceeds the dimension of the task space m, n>m. The difference between n and m will be denoted as the degree of redundancy r, r = n m. Note that by this definition the redundancy is not only a characteristic of the manipulator itself but also of the task, meaning that a non-redundant manipulator may also become redundant for a certain task. By defining q as an n-dimensional vector of joint positions and x as an m-dimensional task variables, then

the forward kinematics relation can be written as x= f (q), () where f is an m-dimensional vector function which maps the joint space variable into task space variables. The corresponding relationship between joint and task velocities is given by as ẋx= f (q) q q= q, (2) where is the m x n configuration dependant acobian matrix. The solution of the above equation for q can be given as a sum of particular and homogenous solution q= q p + q h, (3) where typical requirement is to pursue minimal-norm velocity, leading to the least squares solution q= ẋx+n q n. (4) Here N is the n x n matrix representing the projection into the null-space of, q n is an arbitrary n-dimensional vector of joint velocities and is the Moore-Penrose pseudo inverse of. We denote it by and = T ( T), (5) N=I. (6) Here I is the n x n identity matrix. It can be recognised that the operator N projects a velocity vector in to the nullspace of the acobian matrix. This further corresponds to generating a motion of the robotic system that does not affect the motion of the given task. In robotics this is usually named as internal motion inheriting the nomenclature from original application of those techniques where the primary task was the position of end-effector [3. We denote this solution as the generalized-inverse-based redundancy resolution at the velocity level [4. To avoid the well-known problem of numerical drift, the algorithm is usually implemented as q= (ẋx d + Ke). (7) Here ẋx d is the desired task space velocity, K is a m x m positive-definite matrix and e is the task position error defined as e= x d x, (8) where x d is the desired task space position. For highly redundant system multiple tasks can be arranged in priority. For sake of simplicity, we consider two tasks T a and T b x a = f a (q), (9) x b = f b (q). () For each of the tasks, a corresponding acobian matrices can be defined as a and b, and their corresponding null-space projections N a and N b. Assuming that task T a is the primary task their inverse kinematics can be given as q= aẋx a + N a bẋx b. () In many cases it would be of benefit to have the possibility to change the priority of particular subtasks. Using formulation () this can not be done in a smooth way. Therefore we propose a new definition of the velocity q. The velocity q is now defined as where matrix N a is given by q= aẋx a + N a bẋx b, (2) N a = I λ(x a ) a a, (3) where λ(x a ) is a scalar measure of how active is the primary task T a, projecting the vector x a to the interval [,. When the task T a is active, i.e., some movements are necessary for T a, then λ(x a ) =, and when the task T a is not active, then λ(x a ) =. Consequently, when λ(x a ) = the motion for the task T b is in the null space of T a and λ(x a )=, motion for the task T b is not constrained by T a. Note that λ(x a )= implies also that ẋx a in (2) equals e a =. The proposed algorithm allows smooth transition in both ways, i.e. between observing the task T a and the task T b in null-space of the task T a or just the unconstrained movement of the task T b. III. STABILITY ANALYSIS Lyapunov based stability analysis of our approach is discussed in this section. The derivation is similar to the derivation of the Lyapunov based stability analysis for some popular task-priority close-loop inverse kinematics algorithms [3. By applying basic geometric similarities, the definitions concerning the relationship between two tasks is given first. The two tasks denoted with the subscript a and b are independent if rank ( a) + rank ( b ) = rank ([ a + b), (4) and they are considered as dependent if rank ( a) + rank ( b ) > rank ([ a + b). (5) They will be denoted as orthogonal if a b = O, (6) where O is the m a x m b null matrix. The orthogonal condition implies the independency but not vice versa [3. These three conditions can also be verified by resorting to the transpose of the corresponding acobian instead of the pseudoinverse, i.e. they share the same span. By assuming that the end-effector position is constant (ẋx d = ) and considering eq. (8), we get By stacking both tasks errors as ėe= ẋx= q. (7) e= [ e T a e T b T, (8)

eq. 7 results in [ ėe ėe= a ėe b [ a = b q (9) By resorting to the Lyapunov stability theorem, the error tends asymptotically to zero, if a strictly positive continuously differentiable function V(e) is defined whose time derivate V(e) is negative definite. A possible Lyapunov function candidate is given by V(e)= 2 et e, (2) A Critical point A Actual task path Desired motion x d Obstacle Obstacle x e B Desired task path whose time derivate is V = e T ėe= e T [ a b q, (2) which, by substituting system velocity with (2) results in [ V = e T a ak a e a + N a bk b e b, (22) b and can be rearranged as [ V = e T a ak a a N a bk b b ak a b N a bk b = e T [ M M 2 M 2 M 22 e= (23) e (24) = e T Me. (25) The matrix M is decomposed into sub-matrices M i j of proper dimensions. To apply the Lyapunov stability theorem, one needs to investigate the definiteness of the matrix M, i.e. the definiteness of sub matrix M i j independently. A necessary condition for M to be positive definite is that all sub-matrices on the diagonal are positive definite. First, considering the diagonal sub-matrices, it follows form (23) and (25) that M is obviously positive definite as long as the gain matrix K a >. Next, M 22 is positive definite if the gain matrix K b > and the tasks T a and T b are orthogonal. A sufficient condition for M to be positive definite is given by its eventual lower triangular form [3. If the condition (6) holds, M 2 = O. The sign of the sub-matrices holding the lower triangle is not decisive for the overall identification of M. However, it is worth noticing that M 2 = O if the tasks T b and T a are orthogonal, otherwise it is not determined in sign [3. Overall, our proposed approach leads to a positive definite M, and thus to a strictly negative Lyapunov function, if there exist an orthogonal contrition between the task T a and T b. Under given assumptions, the error, thus tends asymptotically to zero. IV. CASE STUDIES The proposed algorithm can be implemented, as a part of low level control for different robotic tasks, i.e. obstacle avoidance, constraining task or joint space, reflexive behaviour of humanoid robots, stability, etc. In this paper we show how to apply our approach for self collision avoidance (obstacle avoidance). Fig. : Robot motion in presence of some obstacle. In general, the obstacle avoidance strategy is to identify the points on the robotic mechanism which are near the obstacle and assign to them a motion component that moves the points away form it. The motion of the robot is modified, if at least one part of the robot is in the critical distance of the obstacle, i.e. the distance is less than a predefined threshold. We denote this obstacle, with the term active obstacle and a corresponding closest point on the body of the robotic mechanism as the term critical point (see Fig. ). Note that in a bimanual task the right robot is an obstacle for the left one and vice versa. For our proposed approach, it is assumed that the motion of the end effector can be disturbed by any obstacle, which was not the case in [. If such situation occurs, usually the task execution has to be interrupted and the higher level path planing has to be employed to recalculate the desired motion of a end effector. However, if the end-effector path tracking is not essential, we can use the proposed control scheme (2). Consequently, no end-effector path recalculation or higher level path planning is needed. The proposed control allows to smoothly switch the priority of the task, which allows the robot to move away from the obstacle, even if it has to move the end-effector as well. The obstacle avoidance strategy requires only the motion in the direction of the line connecting the critical point with the closest point on the obstacle. In general this is a one dimensional constraint and only one degree of redundancy is needed. Let d be the vector connecting the closest points on the obstacle and robot and let the operational space in A be defined as one-dimensional space in the direction of d [. Then the acobian, which relates the joint space velocities q and the velocity in the direction of d, is calculated as d = n T o. (26) Here is the acobian defined in the Cartesian space and n is the unit vector in the direction of d, given by n = d d. (27)

.8 (λ(e)) n.6.4 n=.2 n=2 n=..2.3.4.5.6.7.8.9 d Fig. 2: Obstacle avoidance proximity gain to the power of n. Parameters are d m =., < d < and n=,2,3... Now the dimension of matrix d is x n, i.e. only one degree of redundancy is required for obstacle avoidance, and the velocity x o is a scalar. Let the primary task T a be the motion in the direction d and the motion of the end-effector be the task T b. This yields a = d, (28) b =. (29) Next let the avoiding velocity ẋx d to be defined as ẋx d = λ(d ) v, (3) where v is a nominal velocity and λ(d ) is defined as ( ) n dm, n=,2,3... d λ(d )= d m d d <d m (3) where n =,2,3... and d m is the critical distance to the obstacle. Then eq. (2) can be rewritten into the form q= d λ(d ) v + N ẋx c. (32) Here ẋx c is the task controller for the end-effector tracking and N is given by N = I λ(d ) d d. (33) Formulation (32) allows unconstrained joint movement, while λ(d ) is close to zero (λ(d ) ). Thus the robot can track the desired task space path, while away form the obstacle. On the other hand, when the robot is close to the obstacle (λ(d ) ), the null space in (33) takes the form N = N, and only allows movement in the null space of the primary task, i.e. obstacle avoidance task. In this case, we can still move the end effector, but the tracking error can increase due to the obstacle avoiding motion. A. Experimental results We applied our algorithm to two Kuka LWR robots as shown in Fig. 3. Our algorithm is used as low-level control to prevent self collision, i.e. collision between the robots themselves. In the first example we show how the left robot smoothly moves away form the right robot, which is moved Fig. 3: Experimental setup for bimanual movement imitation. Proximity detection x e q q d Robot s q n N Forward kinematics Fig. 4: Overall control scheme for obstacle and self-collision avoidance. manually by a human. In the second example we show how to prevent sell collision when the robots are in master-slave configuration and in third example we show how to imitate the movement from a human demonstrator in real time using Microsoft Kinect. The simplified control scheme is shown in Fig. 4. As mentioned previously our algorithm changes the formulation of the primary and the secondary task, so that the desired movement of the robot is in fact a secondary task. The task of collision avoidance is only observed if we approach a pre-defined threshold. While far from the threshold, the algorithm allows direct control of the separate joints ( q n ). If we approach the threshold, the task of collision avoidance smoothly takes over and only allows joint control in the null space projection of this task. Note that q n is in joint space. Fig. 5 shows the results of self collision avoidance, where the task for the left robot was to stay at the desired endeffector position in Cartesian space. The right robot, was guided by hand, i.e. the movement was random. The behaviour of the robot is also shown in the first part of the video (see appendix). In the top plot of Fig. 5 we can see that the closest distance d between both robots never goes over the desired threshold d m =.5. However, in the bottom plot we can see, that the square tracking error e 2 is in some parts significant, especially when the robots are close to each other. The reason for this is that the left robot is moving away form the right robot to prevent collision, even if it will not be able to stay at the desired position in Cartesian space. This means, that the robot would stay at the desired position, while far form the threshold, or the collision avoidance would take over to avoid collision.

d [m e 2.4.2.2. 2 3 4 5 2 3 4 5 Fig. 5: Closest distance between robots, where the left robot is commanded to track the desired position in Cartesian space, and the right robot is moved by a human. The bottom plot shows the squared tracking error e 2 for the left robot. q [rad q3 [rad d [m q2 [rad.4.2.5 5 5 2 25 3.5 5 5 2 25 3 2.5.5 5 5 2 25 3.5.5.5 5 5 2 25 3 q4 [rad 2 5 5 2 25 3 Fig. 6: Closest distance between the robots in the top plot when they are in master-slave configuration. Remaining plots show the first four actual joint angles. The dashed line is the master robot and the solid line is the slave robot. Second scenario shows how both robots avoids each other when in master-slave configuration. The results are shown in Fig. 6, where we can see in the top plot that the closest distance between the robots was always above the desired threshold. Plots from second to the last shows current angles for first four joints for both robots, respectively. We can see q L q R d [m.4.2 q,l [rad q3,l [rad q4,l [rad 2 3 4.5.5.8.5.6 2 4 2 4 q2,l [rad.6.4.2.8 2 4.5.5 2 4.5 2 4 q3,r [rad q4,r [rad q,r [rad q2,r [rad.5 2 4.2.4.6 2 4.5 2 4 Fig. 7: Closest distance between two robots in the top plot. The robots are controlled by a human demonstrator in real time, using Microsoft Kinect. Remaining plots show the desired value for first four joint angles (solid line) and actual values (dashed line) for the left robot in the left-hand side and the right robot in the right-hand side. that the left robot is able to track the right robot while they are not close to each other, i.e. while away form the desired threshold (see the top plot). The behaviour of the robots is also shown in the second part of the video (see appendix). In the third example we show how to imitate the movement with the robotic system in real time. The human motion is captured using the Microsoft Kinect sensor. Microsoft Kinect is based on range camera developed by PrimeSense, which interprets 3D scene information from a continuouslyprojected infrared structured light. By processing the depth image, the PrimeSense application programming interface (API) enables tracking of the users movement in real time. Imitating the motion of the users arm requires some basic understanding of human physiology. The posture of each arm may be described by four angles - three angles in the shoulder joint and one in the elbow. Shoulder joint enables the following motion [5: arm flexion, arm abduction and external rotation. This angles are calculated from the data obtained with Microsoft Kinect. Fig. 7 shows the results of the experiment, where both robots arms were driven in real time by a human demon-

Fig. 8: A sequence of still photos shows the movement of two Kuka LWR robots, while they successfully avoids each other. The desired movement for the robots is imitated in real time using Microsoft Kinect sensor for the tracking. strator using Microsoft Kinect. Again we can see in top plot in Fig. 7, that the distance between both robots is above the selected threshold. The rest of the plots shows the desired and actual angles for the left and right robot. We can see that the actual angles are similar to the desired angles when away from the threshold. While close to the threshold, the robots properly adapt their motion to prevent collision. Fig. 8 shows a sequence for successful self collision avoidance (see also the last part of the video). V. C ONCLUSION In this paper we have shown how we modified prioritised task control at velocity level to implement a soft continuous transition between two different tasks. Higher priority movement only takes over when the desired movement approaches a given threshold, and thus smoothly switches the priority of the tasks and vice versa. We have also shown how to apply the algorithm to two Kuka LWR robot to prevent collision between them. The stability analysis for our approach has been discussed and sufficient condition for asymptotic stability for task tasks errors have been provided, by following the Lyapunov stability theorem. In the future, we would like to implement the algorithm in the lowest level of control of our mechanisms, and try similar behaviours in other scenarios, such as reflexive behaviour for maintain stability of a humanoid robot, or constraining the task or joint space. A PPENDIX : MULTIMEDIA EXTENSION Video: http://abr.ijs.si/upload/3242-robio2.avi R EFERENCES [ L. Zlajpah and B. Nemec, Kinematic control algorithms for on-line obstacle avoidance for redundant manipulators, in Intelligent Robots and Systems, 22. IEEE/RS International Conference on, vol. 2, 22, pp. 898 93 vol.2. [2 L. Sciavicco and B. Siciliano, Modelling and Control of Robot Manipulators (Advanced Textbooks in Control and Signal Processing), 2nd ed., ser. Advanced textbooks in control and signal processing. Springer, an. 25. [3 A. A. Maciejewski and C. A. Klein, Obstacle avoidance for kinematically redundant manipulators in dynamically varying environments, The International ournal of Robotics Research, vol. 4, no. 3, pp. 9 7, 985. [4 K. O Neil, Divergence of linear acceleration-based redundancy resolution schemes, Robotics and Automation, IEEE Transactions on, vol. 8, no. 4, pp. 625 63, aug 22. [5 O. Egeland, Task-space tracking with redundant manipulators, Robotics and Automation, IEEE ournal of, vol. 3, no. 5, pp. 47 475, october 987. [6 O. Khatib, A unified approach for motion and force control of robot manipulators: The operational space formulation, Robotics and Automation, IEEE ournal of, vol. 3, no., pp. 43 53, february 987. [7 L. Sentis,. Park, and O. Khatib, Compliant control of multicontact and center-of-mass behaviors in humanoid robots, Robotics, IEEE Transactions on, vol. 26, no. 3, pp. 483 5, june 2. [8 N. Mansard, O. Khatib, and A. Kheddar, A unified approach to integrate unilateral constraints in the stack of tasks, Robotics, IEEE Transactions on, vol. 25, no. 3, pp. 67 685, june 29. [9 B. Nemec, L. lajpah, and D. Omr?en, Comparison of null-space and minimal null-space control algorithms, Robotica, vol. 25, no. 5, pp. 5 52, 27. [ H. Hoffmann, P. Pastor, D.-H. Park, and S. Schaal, Biologicallyinspired dynamical systems for movement generation: Automatic real-time goal adaptation and obstacle avoidance, in Robotics and Automation, 29. ICRA 9. IEEE International Conference on, may 29, pp. 2587 2592. [ D.-H. Park, H. Hoffmann, P. Pastor, and S. Schaal, Movement reproduction and obstacle avoidance with dynamic movement primitives and potential fields, in Humanoid Robots, 28. Humanoids 28. 8th IEEE-RAS International Conference on, dec. 28, pp. 9 98. [2 O. Khatib, Real-time obstacle avoidance for manipulators and mobile robots, The International ournal of Robotics Research, vol. 5, no., pp. 9 98, 986. [3 G. Antonelli, Stability analysis for prioritized closed-loop inverse kinematic algorithms for redundant robotic systems, Robotics, IEEE Transactions on, vol. 25, no. 5, pp. 985 994, oct. 29. [4 D. N. Nenchev, Redundancy resolution through local optimization: A review, ournal of Robotic Systems, vol. 6, no. 6, pp. 769 798, 989. [5 K. Hayes,. R. Walton, Z. R. Szomor, and G. A. Murrell, Reliability of five methods for assessing shoulder range of motion. The Australian journal of physiotherapy, vol. 47, no. 4, pp. 289 294, 2.