Abstract:
A system and method for providing multiple priority impedance control for a robot manipulator where impedance laws are realized simultaneously and with a given order of priority. The method includes a control scheme for realizing a Cartesian space impedance objective as a first priority while also realizing a joint space impedance objective as a second priority. The method also includes a control scheme for realizing two Cartesian space impedance objectives with different levels of priority. The method includes instances of the control schemes that use feedback from force sensors mounted at an end-effector and other instances of the control schemes that do not use this feedback.

Description:
STATEMENT REGARDING FEDERALLY SPONSORED RESEARCH OR DEVELOPMENT 
       [0001]    The invention described herein may be manufactured and used by or for the U.S. Government for U.S. Government (i.e., non-commercial) purposes without the payment of royalties thereon or therefore. 
     
    
     BACKGROUND OF THE INVENTION 
       [0002]    1. Field of the Invention 
         [0003]    This invention relates generally to a system for controlling the impedance of a robotic manipulator and, more particularly, to a scheme for multiple prioritized operational space impedances at different points on a robotic manipulator. 
         [0004]    2. Discussion of the Related Art 
         [0005]    Arms and manipulators are essential to many robotic tasks. For example, they are essential to robotic assembly tasks. Many robot arms and manipulators designed for factory use are stiff, high-impedance arms that operate using position control systems. However, the ability to control forces applied by the robot on the environment is also useful in assembly tasks. In particular, it is useful to be able to control the impedance of the manipulator, i.e., the relationship between applied environmentally forces and manipulator position. 
         [0006]    Active impedance control of robot arms and manipulators has been studied and different approaches have been proposed. One of the earliest approaches was a strategy for controlling end-effector stiffness in Cartesian coordinates. Assuming the existence of a low-level torque controller, this approach calculated the joint torque corresponding to a Cartesian space error given a desired Cartesian space end-effector stiffness. This can be considered to be a system for achieving a desired linear zero-order impedance. This approach was later expanded into a system for controlling second-order linear impedance. In this approach, the control system attempts to adjust not only the stiffness, but also the manipulator damping and the inertia. 
         [0007]    Another body of work relevant to the current invention is multiple-priority velocity and acceleration control. It is well known that because a robot manipulator has enough actuated degrees of freedom, a manifold of joint configurations may exist that place the end effector in the same position and orientation. This property is known as the kinematic redundancy of the manipulator and has been used primarily to enable robots to achieve multiple velocity or acceleration objectives at once. For example, with a seven degree-of-freedom (DOF) humanoid manipulator, it is possible to place the manipulator end-effector in a particular pose while also adjusting the position of the elbow as desired. Typically, position objective are assigned priorities with lower-priority position objective operating subject to higher-priority objectives. 
       SUMMARY OF THE INVENTION 
       [0008]    In accordance with the teachings of the present invention, a system and method are disclosed for providing multiple priority impedance control for a robot manipulator where multiple impedance laws are realized simultaneously and with a given order of priority. The method includes a control scheme for realizing a Cartesian space impedance objective as a first priority while also realizing a joint space impedance objective as a second priority. The method also includes a control scheme for realizing two Cartesian space impedance objectives with different levels of priority. The method includes instances of the control schemes that use feedback from force sensors mounted at an end-effector and other instances for the control schemes that do not use this feedback. 
         [0009]    Additional features of the present invention will become apparent from the following description and appended claims, taken in conjunction with the accompanying drawings. 
     
    
     
       BRIEF DESCRIPTION OF THE DRAWINGS 
         [0010]      FIG. 1  is a block diagram of a system for providing Cartesian impedance with a subordinate joint space impedance task; 
           [0011]      FIG. 2  is a block diagram of a system for providing Cartesian impedance with a subordinate Cartesian space impedance task; and 
           [0012]      FIG. 3  is a flow chart diagram showing hierarchical impedance control. 
       
    
    
     DETAILED DESCRIPTION OF THE EMBODIMENTS 
       [0013]    The following discussion of the embodiments of the invention is directed to a system and method for providing hierarchical impedance control of a robotic system is merely exemplary in nature, and is in no way intended to limit the invention or its applications or uses. 
         [0014]    The embodiments of the invention are described in terms of the following generalized equation of motion for a robot manipulator with n revolute joints: 
         [0000]        A{umlaut over (q)}+η=τ   a +τ  (1) 
         [0000]    Where A is the n×n manipulator inertia matrix, {umlaut over (q)} is an n-vector of manipulator joint accelerations, τ is an n-vector of torques resulting from externally applied loads and τ a  is a vector of actuator torques. 
         [0015]    η describes the sum of friction, coriolis, centrifugal and gravitational torques as: 
         [0000]      η= t+c+g   (2) 
         [0000]    Where t is the vector of frictional forces, c is a vector of manipulator centrifugal and coriolis forces and g is a vector of manipulator gravity forces. The dependence of A, c and g on manipulator configuration is implicitly assumed. 
         [0016]    It is frequently useful to design controllers defined in operational space coordinates rather than in the joint space. The term “operational space” may refer to any coordinate system mapped onto by the joint space. Frequently, the operational space coordinates of most interest are considered to be the space of Cartesian positions and orientations of a point resolution (POR) on the manipulator. Another alternative is an operational space coordinate system that described a system state in relation to a scalar potential function in the joint space. When operational space coordinates describe the Cartesian pose, then the pose of the POR as an element of SE that will be represented in exponential coordinates (a six-vector where the first three numbers described position and the last three numbers describe orientation using the axis-angle representation). The Cartesian velocity of the POR will be represented as a twist and the acceleration as the derivative of twist. Similarly, loads in Cartesian space will be written as wrenches (six-vector that concentrates a force and a moment). 
         [0017]    The POR Jacobian J is an important quantity that relates joint velocities {dot over (q)} to Cartesian twists at the POR, {dot over (x)}:{dot over (x)}=J{dot over (q)}. Using a virtual work argument, the POR Jacobian can also be shown to relate wrenches applied to the POR by external sources f to the joint torques τ that gives: τ=J T f. The acceleration of the POR is calculated by solving equation (1) for {umlaut over (x)}: 
         [0000]        {umlaut over (x)}=JA   −1 (τ−τ a +η)+ {dot over (J)}{dot over (q)}   (3) 
         [0018]      FIG. 1  is a block diagram of a system  10  for providing Cartesian impedance with a subordinate joint space impedance task and  FIG. 2  is a block diagram of a system  20  for providing Cartesian impedance with a subordinate Cartesian space impedance task which will be used in the analysis below. From the systems  10  and  20  it is clear that the space of actuator torques that solve JA −1 τ a =0 result in zero POR accelerations. Therefore, any actuator torque, τ a =ANγ*, results in null space accelerations with free variable. Note that this matrix, AN, performs a similar function as the inertia-weighted null space. 
         [0019]    Each embodiment can be described in terms of one of the two block diagrams in the systems  10  and  20  where in either case different terms substitute for the G and H blocks. 
         [0020]    Based on the above analysis, the first-order impedance embodiments of this invention are realized by the following control law. Allow a change of variables such that a new control variable τ′ specifies actuator torques according to: 
         [0000]      τ a =η−τ′  (4) 
         [0021]    Recall that η is the sum of frictional, coriolis centrifugal, and gravitational torques. Then the Cartesian control law can be specified as: 
         [0000]    
       
         
           
             
               
                 
                   
                     
                       τ 
                       l 
                     
                     = 
                     
                       
                         
                           J 
                           T 
                         
                          
                         
                           f 
                           * 
                         
                       
                       + 
                       
                         AN 
                          
                         
                             
                         
                          
                         
                           γ 
                           * 
                         
                       
                       + 
                       
                         A 
                          
                         
                           J 
                           . 
                         
                          
                         
                           q 
                           . 
                         
                       
                     
                   
                    
                   
                     
 
                   
                    
                   
                     Where 
                      
                     
                       : 
                     
                   
                 
               
               
                 
                   ( 
                   5 
                   ) 
                 
               
             
             
               
                 
                   
                     f 
                     * 
                   
                   = 
                   
                     
                       
                         B 
                         c 
                       
                        
                       
                         
                           x 
                           ~ 
                         
                         . 
                       
                     
                     + 
                     
                       
                         K 
                         c 
                       
                        
                       
                         x 
                         ~ 
                       
                     
                     + 
                     
                       f 
                       des 
                     
                   
                 
               
               
                 
                   ( 
                   6 
                   ) 
                 
               
             
           
         
       
     
         [0022]    Equation (6) is a desired Cartesian space first order linear impedance, and γ* is a free null space variable. B c  and K c  are the Cartesian space damping and stiffness matrices and f des  is a desired reference wrench. This control law requires measurement of the Cartesian velocity and pose error 
         [0000]    
       
         
           
             
               
                 x 
                 ~ 
               
               . 
             
              
             
                 
             
              
             and 
              
             
                 
             
              
             
               x 
               ~ 
             
           
         
       
     
         [0000]    defined with respect to velocity and pose references {dot over (x)}* and x*. As expected, γ* may be set arbitrarily without changing the acceleration of force applied by the POR. Substituting equations (2), (3) and (4) back into equation (1) gives: 
         [0000]        Λ{umlaut over (x)}+f*=ΛJA   −1 τ  (7) 
         [0023]    For an arbitrary value of γ* where Λ=(JA −1 J T* ) −1  is the Cartesian space passive manipulator inertia. When forces are applied only to the POR, then equation (7) reduces to: 
         [0000]        Λ{umlaut over (x)}+f*=f   (8) 
         [0024]    In one embodiment, the controller realizes a second-priority joint space impedance while also realizing the first-priority first order Cartesian space impedance. The second-priority impedance is: 
         [0000]    
       
         
           
             
               
                 
                   
                     
                       
                         A 
                          
                         
                           q 
                           ¨ 
                         
                       
                       + 
                       
                         τ 
                         * 
                       
                     
                     = 
                     τ 
                   
                    
                   
                     
 
                   
                    
                   
                     Where 
                      
                     
                       : 
                     
                   
                 
               
               
                 
                   ( 
                   9 
                   ) 
                 
               
             
             
               
                 
                   
                     τ 
                     * 
                   
                   = 
                   
                     
                       B 
                        
                       
                         
                           q 
                           ~ 
                         
                         . 
                       
                     
                     + 
                     
                       K 
                        
                       
                         q 
                         ~ 
                       
                     
                     + 
                     
                       τ 
                       des 
                     
                   
                 
               
               
                 
                   ( 
                   10 
                   ) 
                 
               
             
           
         
       
     
         [0025]    B and K are the joint space damping and stiffness matrices, respectively, and τ des  is a desired external torque. 
         [0000]    
       
         
           
             
               
                 q 
                 ~ 
               
               . 
             
              
             
                 
             
              
             and 
              
             
                 
             
              
             
               q 
               ~ 
             
           
         
       
     
         [0000]    are the joint velocity and position errors resured with respect to the joint velocity and position references {dot over (q)}* and q* respectively. Note that this is really just a first order impedance specification because the passive inertia is left unchanged. 
         [0026]    Since the passive inertia of the manipulator remains unchanged, it is possible to select the γ* that minimizes the joint acceleration error or the joint torque error. First, consider minimizing the joint acceleration error. The acceleration required by the second-priority joint impedance is: 
         [0000]        {umlaut over (q)}=A   −1 (τ−τ*)  (11) 
         [0027]    The acceleration realized by the first-priority control law is: 
         [0000]        {umlaut over (q)}=A   −1   τ−A   −1   J   T   f*−Nγ*   (12) 
         [0028]    The γ* that minimizes the different between {umlaut over (q)}′ and {umlaut over (q)} is: 
         [0000]    
       
         
           
             
               
                 
                   
                     
                       
                         
                           γ 
                           min 
                           * 
                         
                         = 
                         
                           
                             
                               arg 
                                
                               
                                   
                               
                                
                               min 
                             
                             γ 
                           
                            
                           
                              
                             
                               
                                 
                                   q 
                                   ¨ 
                                 
                                 ′ 
                               
                               - 
                               
                                 q 
                                 ¨ 
                               
                             
                              
                           
                         
                       
                     
                   
                   
                     
                       
                         = 
                         
                           
                             
                               arg 
                                
                               
                                   
                               
                                
                               min 
                             
                             γ 
                           
                            
                           
                              
                             
                               
                                 N 
                                  
                                 
                                     
                                 
                                  
                                 γ 
                               
                               + 
                               
                                 
                                   A 
                                   
                                     - 
                                     1 
                                   
                                 
                                  
                                 
                                   J 
                                   T 
                                 
                                  
                                 
                                   f 
                                   * 
                                 
                               
                               - 
                               
                                 
                                   A 
                                   
                                     - 
                                     1 
                                   
                                 
                                  
                                 
                                   τ 
                                   * 
                                 
                               
                             
                              
                           
                         
                       
                     
                   
                   
                     
                       
                         = 
                         
                           
                             
                               NA 
                               
                                 - 
                                 1 
                               
                             
                              
                             
                               ( 
                               
                                 
                                   τ 
                                   * 
                                 
                                 - 
                                 
                                   
                                     J 
                                     T 
                                   
                                    
                                   
                                     f 
                                     * 
                                   
                                 
                               
                               ) 
                             
                           
                           . 
                         
                       
                     
                   
                 
               
               
                 
                   ( 
                   13 
                   ) 
                 
               
             
           
         
       
     
         [0029]    Therefore, the multi-priority control law that minimizes second-priority acceleration error is: 
         [0000]      τ′=( I−ANA   −1 ) J   T   f*+ANA   −1   τ*+A{dot over (J)}{dot over (q)}   (14) 
         [0030]    It is also possible to select the γ* that minimizes the error in joint torques resulting from externally applied forces. The torque required to realize the second priority joint impedance is: 
         [0000]      τ′= A{umlaut over (q)}+τ*   (15) 
         [0031]    The torque applied to the environment by the first priority control law is: 
         [0000]      τ= A{umlaut over (q)}+J   T   f*+ANγ*   (16) 
         [0032]    The value of γ* that minimizes the second-priority torque error is: 
         [0000]    
       
         
           
             
               
                 
                   
                     
                       
                         
                           γ 
                           min 
                         
                         = 
                         
                           
                             
                               arg 
                                
                               
                                   
                               
                                
                               min 
                             
                             γ 
                           
                            
                           
                              
                             
                               
                                 τ 
                                 ′ 
                               
                               - 
                               τ 
                             
                              
                           
                         
                       
                     
                   
                   
                     
                       
                         = 
                         
                           
                             
                               arg 
                                
                               
                                   
                               
                                
                               min 
                             
                             γ 
                           
                            
                           
                              
                             
                               
                                 τ 
                                 * 
                               
                               - 
                               
                                 
                                   J 
                                   T 
                                 
                                  
                                 
                                   f 
                                   * 
                                 
                               
                               - 
                               
                                 AN 
                                  
                                 
                                     
                                 
                                  
                                 
                                   γ 
                                   * 
                                 
                               
                             
                              
                           
                         
                       
                     
                   
                   
                     
                       
                         = 
                         
                           
                             
                               ( 
                               AN 
                               ) 
                             
                             + 
                           
                            
                           
                             ( 
                             
                               
                                 τ 
                                 * 
                               
                               - 
                               
                                 
                                   J 
                                   T 
                                 
                                  
                                 
                                   f 
                                   * 
                                 
                               
                             
                             ) 
                           
                         
                       
                     
                   
                 
               
               
                 
                   ( 
                   17 
                   ) 
                 
               
             
           
         
       
     
         [0033]    Therefore, the multi-priority control law that minimizes the second-priority torque error is: 
         [0000]      τ′=( I−A ( AN ) + ) J   T   f*+A ( AN ) +   τ*+A{dot over (J)}{dot over (q)}   (18) 
         [0034]    In another embodiment, the controller realizes a second-priority Cartesian impedance while also realizing the first-priority first order Cartesian space impedance. Since the first-priority impedance remains the same as it was in the previous embodiment, this embodiment re-uses the control law of equation (5). γ* must be chosen to optimize manipulator dynamics for the second-priority Cartesian impedance. The second priority impedance is: 
         [0000]      Λ 2   {umlaut over (x)}   2   +f*   2 =Λ 2   J   2   A   −1 τ  (19) 
         [0000]    Where Λ 2 =(J 2 AJ 2   T ) −1  is the passive inertia of the manipulator, J 2  is the Jacobian matrix, and f 2  is the force applied at the second POR by an external source. f* 2  is: 
         [0000]    
       
         
           
             
               
                 
                   
                     f 
                     2 
                     * 
                   
                   = 
                   
                     
                       
                         B 
                         2 
                       
                        
                       
                         
                           
                             x 
                             ~ 
                           
                           . 
                         
                         2 
                       
                     
                     + 
                     
                       
                         K 
                         2 
                       
                        
                       
                         
                           
                             x 
                             ~ 
                           
                           ¨ 
                         
                         2 
                       
                     
                   
                 
               
               
                 
                   ( 
                   20 
                   ) 
                 
               
             
           
         
       
     
         [0035]    Note that when forces are applied at the second POR only, equation (20) becomes: 
         [0000]      Λ 2   {umlaut over (x)}   2   +f*   2   =f   2   (21) 
         [0036]    As with the second-priority joint space impedance law, the passive inertia of the manipulator remains unchanged because the first-priority impedance is first order. The space of second-POR accelerations that can be realized by the first priority Cartesian impedance is found using equation (12): 
         [0000]        {umlaut over (x)}   2   =J   2   A   −1   τ−J   2   A   −1   J   T   f*−J   2   Nγ*+{dot over (J)}   2   {dot over (q)}   (22) 
         [0037]    The second-POR acceleration required by the second-priority Cartesian impedance in equation (19) is: 
         [0000]        {umlaut over (x)}′   2   =J   2   A   −1   τ−J   2   A   −1   J   2   T   f*   2   (23) 
         [0038]    The γ* that minimizes the difference between {umlaut over (x)} 2  and {umlaut over (x)}′ 2  is: 
         [0000]    
       
         
           
             
               
                 
                   
                     
                       
                         
                           γ 
                           min 
                         
                         = 
                         
                           
                             
                               arg 
                                
                               
                                   
                               
                                
                               min 
                             
                             γ 
                           
                            
                           
                              
                             
                               
                                 
                                   x 
                                   ¨ 
                                 
                                 2 
                                 ′ 
                               
                               - 
                               
                                 
                                   x 
                                   ¨ 
                                 
                                 2 
                               
                             
                              
                           
                         
                       
                     
                   
                   
                     
                       
                         = 
                         
                           
                             argmin 
                             γ 
                           
                            
                           
                              
                             
                               
                                 
                                   
                                     
                                       
                                         J 
                                         2 
                                       
                                        
                                       
                                         A 
                                         
                                           - 
                                           1 
                                         
                                       
                                        
                                       
                                         J 
                                         T 
                                       
                                        
                                       
                                         f 
                                         * 
                                       
                                     
                                     + 
                                     
                                       
                                         J 
                                         2 
                                       
                                        
                                       N 
                                        
                                       
                                           
                                       
                                        
                                       
                                         γ 
                                         * 
                                       
                                     
                                     - 
                                   
                                 
                               
                               
                                 
                                   
                                     
                                       
                                         J 
                                         2 
                                       
                                        
                                       
                                         A 
                                         
                                           - 
                                           1 
                                         
                                       
                                        
                                       
                                         J 
                                         2 
                                         T 
                                       
                                        
                                       
                                         f 
                                         2 
                                         * 
                                       
                                     
                                     - 
                                     
                                       
                                         j 
                                         2 
                                       
                                        
                                       
                                         q 
                                         . 
                                       
                                     
                                   
                                 
                               
                             
                              
                           
                         
                       
                     
                   
                   
                     
                       
                         = 
                         
                           
                             
                               ( 
                               AN 
                               ) 
                             
                             + 
                           
                            
                           
                             ( 
                             
                               
                                 τ 
                                 * 
                               
                               - 
                               
                                 
                                   J 
                                   T 
                                 
                                  
                                 
                                   f 
                                   * 
                                 
                               
                             
                             ) 
                           
                         
                       
                     
                   
                 
               
               
                 
                   ( 
                   24 
                   ) 
                 
               
             
           
         
       
     
       Where Λ 2 =(J 2 A −1 J 2   T ) −1 . 
       [0039]    Substituting γ* min  into the control law of equation (5) gives: 
         [0000]      τ′=( I −( J   2   N ) +   J   2   A   −1 ) J   T   f *+( J   2   N ) + (Λ 2   −1   f*   2   +{dot over (J)}   2   {dot over (q)} )+ A{dot over (J)}   2   {dot over (q)}   (25) 
         [0040]    It is also possible to realize second-priority second order joint and Cartesian space impedances. This requires an additional measurement of externally applied forces or joint accelerations. In these embodiments, the first-priority Cartesian space impedance is: 
         [0000]    
       
         
           
             
               
                 
                   
                     
                       M 
                        
                       
                         
                           x 
                           ~ 
                         
                         ¨ 
                       
                     
                     + 
                     
                       f 
                       * 
                     
                   
                   = 
                   f 
                 
               
               
                 
                   ( 
                   26 
                   ) 
                 
               
             
           
         
       
     
         [0000]    Where f* is defined in equation (6) and 
         [0000]    
       
         
           
             
               
                 x 
                 ~ 
               
               ¨ 
             
             = 
             
               
                 x 
                 ¨ 
               
               - 
               
                 
                   x 
                   ¨ 
                 
                 * 
               
             
           
         
       
     
         [0000]    is POR acceleration error with respect to an acceleration reference {umlaut over (x)}*. 
         [0041]    The prioritization of the impedance laws is realized by selecting the joint space acceleration closest to that required by the secondary impedance law while still realizing the primary impedance. The space of joint accelerations that realize the first-priority Cartesian impedance is: 
         [0000]        {umlaut over (q)}=J   +   {umlaut over (x)}*+J   +   M   −1 ( f−f* )+ Nλ−J   +   {dot over (J)}{dot over (q)}   (27) 
         [0042]    For a second-priority impedance expressed in the joint space, the impedance law is: 
         [0000]    
       
         
           
             
               
                 
                   
                     
                       Γ 
                        
                       
                         
                           q 
                           ~ 
                         
                         ¨ 
                       
                     
                     + 
                     
                       τ 
                       * 
                     
                   
                   = 
                   τ 
                 
               
               
                 
                   ( 
                   28 
                   ) 
                 
               
             
           
         
       
     
         [0043]    The corresponding joint acceleration is: 
         [0000]        {umlaut over (q)}′={umlaut over (q)}*+Γ   −1 (τ−τ*)  (29) 
         [0044]    The value of γ is solved for that minimizes ∥{umlaut over (q)}−{umlaut over (q)}′∥. Expanding this expression, to find: 
         [0000]    
       
         
           
             
               
                 
                   
                     λ 
                     min 
                   
                   = 
                   
                     
                       
                         argmin 
                         λ 
                       
                        
                       
                          
                          
                       
                        
                       
                         J 
                         + 
                       
                        
                       
                         
                           x 
                           ¨ 
                         
                         * 
                       
                     
                     + 
                     
                       
                         J 
                         + 
                       
                        
                       
                         
                           M 
                           
                             - 
                             1 
                           
                         
                          
                         
                           ( 
                           
                             f 
                             - 
                             
                               f 
                               * 
                             
                           
                           ) 
                         
                       
                     
                     + 
                     
                       N 
                        
                       
                           
                       
                        
                       λ 
                     
                     - 
                     
                       
                         J 
                         + 
                       
                        
                       
                         J 
                         . 
                       
                        
                       
                         q 
                         . 
                       
                     
                     - 
                     
                       
                         q 
                         ¨ 
                       
                       * 
                     
                     - 
                     
                       
                         Γ 
                         
                           - 
                           1 
                         
                       
                        
                       
                         ( 
                         
                           τ 
                           - 
                           
                             
                               τ 
                               . 
                             
                             * 
                           
                         
                         ) 
                       
                     
                   
                 
               
               
                 
                   ( 
                   30 
                   ) 
                 
               
             
           
         
       
     
         [0045]    The solution can be found by taking the pseudoinverse of N and using the fact that since N is a projection the matrix N=N + : 
         [0000]      γ min   =N ( {umlaut over (q)}*+Γ   −1 (τ−τ*))  (31) 
         [0046]    Substituting into equation (1) and using the fact that since N is idempotent, N=NN, the resulting control law is: 
         [0000]    
       
         
           
             
               
                 
                   
                     
                       
                         
                           τ 
                           a 
                         
                         = 
                           
                          
                         
                           
                             A 
                              
                             
                               q 
                               ¨ 
                             
                           
                           - 
                           τ 
                           + 
                           η 
                         
                       
                     
                   
                   
                     
                       
                         = 
                           
                          
                         
                           
                             
                               AJ 
                               + 
                             
                              
                             
                               ( 
                               
                                 
                                   
                                     x 
                                     ¨ 
                                   
                                   * 
                                 
                                 + 
                                 
                                   
                                     M 
                                     
                                       - 
                                       1 
                                     
                                   
                                    
                                   
                                     ( 
                                     
                                       f 
                                       - 
                                       
                                         f 
                                         * 
                                       
                                     
                                     ) 
                                   
                                 
                                 - 
                                 
                                   
                                     J 
                                     . 
                                   
                                    
                                   
                                     q 
                                     . 
                                   
                                 
                               
                               ) 
                             
                           
                           + 
                           
                             AN 
                              
                             
                                 
                             
                              
                             
                               λ 
                               min 
                             
                           
                           - 
                           τ 
                           + 
                           η 
                         
                       
                     
                   
                   
                     
                       
                         = 
                           
                          
                         
                           
                             
                               AJ 
                               + 
                             
                              
                             
                               ( 
                               
                                 
                                   
                                     x 
                                     ¨ 
                                   
                                   * 
                                 
                                 + 
                                 
                                   
                                     M 
                                     
                                       - 
                                       1 
                                     
                                   
                                    
                                   
                                     ( 
                                     
                                       f 
                                       - 
                                       
                                         f 
                                         * 
                                       
                                     
                                     ) 
                                   
                                 
                                 - 
                                 
                                   
                                     J 
                                     . 
                                   
                                    
                                   
                                     q 
                                     . 
                                   
                                 
                               
                               ) 
                             
                           
                           + 
                           
                             AN 
                             ( 
                             
                               
                                 
                                   q 
                                   ¨ 
                                 
                                 * 
                               
                               + 
                               
                                 Γ 
                                 
                                   - 
                                   1 
                                 
                               
                             
                           
                         
                       
                     
                   
                   
                     
                       
                           
                          
                         
                           
                             ( 
                             
                               τ 
                               - 
                               
                                 τ 
                                 * 
                               
                             
                             ) 
                           
                           - 
                           τ 
                           + 
                           η 
                         
                       
                     
                   
                 
               
               
                 
                   ( 
                   32 
                   ) 
                 
               
             
           
         
       
     
         [0047]    Note again that this control law requires measurements of the joint torques τ and POR forces f resulting from externally applied forces. 
         [0048]    Rather than realizing a second-priority joint space impedance, it is possible to realize a second priority Cartesian space impedance instead. The first-priority objective is still the second order impedance of equation (26). The second-priority impedance is defined in the Cartesian space: 
         [0000]    
       
         
           
             
               
                 
                   
                     
                       
                         M 
                         2 
                       
                        
                       
                         
                           
                             x 
                             ~ 
                           
                           ¨ 
                         
                         2 
                       
                     
                     + 
                     
                       f 
                       2 
                       * 
                     
                   
                   = 
                   
                     f 
                     2 
                   
                 
               
               
                 
                   ( 
                   33 
                   ) 
                 
               
             
           
         
       
     
         [0000]    Where f* 2  is: 
         [0000]    
       
         
           
             
               
                 
                   
                     f 
                     2 
                     * 
                   
                   = 
                   
                     
                       
                         B 
                         2 
                       
                        
                       
                         
                           
                             x 
                             ~ 
                           
                           . 
                         
                         2 
                       
                     
                     + 
                     
                       
                         K 
                         2 
                       
                        
                       
                         x 
                         ~ 
                       
                     
                   
                 
               
               
                 
                   ( 
                   34 
                   ) 
                 
               
             
           
         
       
     
         [0049]    As before, the space of joint space accelerations that realize equation (26) is given by equation (27). The corresponding space of Cartesian accelerations at the second POR is: 
         [0000]        {umlaut over (x)}   2   =J   2   J   +   [{umlaut over (x)}*+M   −1 ( f−f* )]+ J   2   Nλ−J   2   J   +   {dot over (J)}{dot over (q)}+{dot over (J)}   2   {dot over (q)}   (35) 
         [0000]    Where J 2  is the manipulator Jacobian matrix associated with x 2 . 
         [0050]    Re-arranging equation (33), the desired Cartesian acceleration at the second POR is: 
         [0000]        {umlaut over (x)}′   2   ={umlaut over (x)}*   2   +M   2   −1 ( f   2   −f*   2 )  (36) 
         [0051]    The approach to realizing equation (33) as a second priority is to find the value for λ that makes equation (35) as close as possible to {umlaut over (x)}′ 2 . Equivalently: 
         [0000]    
       
         
           
             
               
                 
                   
                     
                       
                         
                           γ 
                           min 
                         
                         = 
                         
                           
                             argmin 
                             γ 
                           
                            
                           
                              
                             
                               
                                 
                                   x 
                                   ¨ 
                                 
                                 2 
                               
                               - 
                               
                                 
                                   x 
                                   ¨ 
                                 
                                 2 
                                 ′ 
                               
                             
                              
                           
                         
                       
                     
                   
                   
                     
                       
                         = 
                         
                           
                             argmin 
                             γ 
                           
                            
                           
                              
                             
                               
                                 
                                   
                                     
                                       
                                         J 
                                         2 
                                       
                                        
                                       
                                         
                                           J 
                                           + 
                                         
                                          
                                         
                                           ( 
                                           
                                             
                                               
                                                 x 
                                                 ¨ 
                                               
                                               * 
                                             
                                             + 
                                             
                                               
                                                 M 
                                                 
                                                   - 
                                                   1 
                                                 
                                               
                                                
                                               
                                                 ( 
                                                 
                                                   f 
                                                   - 
                                                   
                                                     f 
                                                     * 
                                                   
                                                 
                                                 ) 
                                               
                                             
                                           
                                           ) 
                                         
                                       
                                     
                                     + 
                                     
                                       
                                         J 
                                         2 
                                       
                                        
                                       N 
                                        
                                       
                                           
                                       
                                        
                                       λ 
                                     
                                     - 
                                   
                                 
                               
                               
                                 
                                   
                                     
                                       
                                         J 
                                         2 
                                       
                                        
                                       
                                         J 
                                         + 
                                       
                                        
                                       
                                         J 
                                         . 
                                       
                                        
                                       
                                         q 
                                         . 
                                       
                                     
                                     + 
                                     
                                       
                                         
                                           J 
                                           . 
                                         
                                         2 
                                       
                                        
                                       
                                         q 
                                         . 
                                       
                                     
                                     - 
                                     
                                       
                                         x 
                                         ¨ 
                                       
                                       2 
                                       * 
                                     
                                     - 
                                     
                                       
                                         
                                           M 
                                           . 
                                         
                                         2 
                                         
                                           - 
                                           1 
                                         
                                       
                                        
                                       
                                         ( 
                                         
                                           
                                             f 
                                             2 
                                           
                                           - 
                                           
                                             f 
                                             2 
                                             * 
                                           
                                         
                                         ) 
                                       
                                     
                                   
                                 
                               
                             
                              
                           
                         
                       
                     
                   
                   
                     
                       
                         = 
                         
                           
                             
                               J 
                               ^ 
                             
                             2 
                             + 
                           
                            
                           
                             [ 
                             
                               
                                 
                                   
                                     
                                       
                                         x 
                                         ¨ 
                                       
                                       2 
                                       * 
                                     
                                     + 
                                     
                                       
                                         M 
                                         2 
                                         
                                           - 
                                           1 
                                         
                                       
                                        
                                       
                                         ( 
                                         
                                           
                                             f 
                                             2 
                                           
                                           - 
                                           
                                             f 
                                             2 
                                             * 
                                           
                                         
                                         ) 
                                       
                                     
                                     - 
                                     
                                       
                                         J 
                                         2 
                                       
                                        
                                       
                                         J 
                                         + 
                                       
                                     
                                   
                                 
                               
                               
                                 
                                   
                                     
                                       ( 
                                       
                                         
                                           
                                             x 
                                             ¨ 
                                           
                                           * 
                                         
                                         + 
                                         
                                           
                                             M 
                                             
                                               - 
                                               1 
                                             
                                           
                                            
                                           
                                             ( 
                                             
                                               f 
                                               - 
                                               
                                                 f 
                                                 * 
                                               
                                             
                                             ) 
                                           
                                         
                                       
                                       ) 
                                     
                                     - 
                                     
                                       
                                         
                                           J 
                                           . 
                                         
                                         2 
                                       
                                        
                                       
                                         q 
                                         . 
                                       
                                     
                                     + 
                                     
                                       
                                         J 
                                         2 
                                       
                                        
                                       
                                         J 
                                         + 
                                       
                                        
                                       
                                         J 
                                         . 
                                       
                                        
                                       
                                         q 
                                         . 
                                       
                                     
                                   
                                 
                               
                             
                             ] 
                           
                         
                       
                     
                   
                 
               
               
                 
                   ( 
                   37 
                   ) 
                 
               
             
           
         
       
     
         [0000]    Where Ĵ 2 =J 2 N. 
         [0052]    Substituting into equation (1), the resulting control law is: 
         [0000]      τ a   =A ( I−Ĵ   2   +   J   2 ) Ĵ   1   +   [{umlaut over (x)}*+M   −1 ( f−f* )− {dot over (J)}{dot over (q)}+AĴ   2   +   [{umlaut over (x)}*   2   +M   2   −1 ( f   2   −f*   2 )− {dot over (J)}   2   {dot over (q)}]+η−τ   (38) 
         [0053]    This control law realizes the first-priority impedance law for the first POR in equation (26) while also realizing the second-priority impedance at the second POR in equation (33) to the extent possible without disrupting the first impedance objective. 
         [0054]    Another embodiment of the invention realizes a dual-priority Cartesian stiffness at two PORs. The stiffness objective at the first POR is: 
         [0000]      f=f*  (39) 
         [0000]    Where f* is defined in equation (6). 
         [0055]    The stiffness objective at the second POR is: 
         [0000]      f 2 =f* 2   (40) 
         [0000]    Where f* 2  is defined in equation (34). 
         [0056]    The first objective is realized by applying a torque from the space: 
         [0000]      τ= J   T   f*   1   +N   1 λ  (41) 
         [0000]    Where λ is arbitrary. 
         [0057]    J is the Jacobian for the first POR and N=I−J + J is the associated null space projection matrix. This space of joint torques correspond to the following set of wrenches at the second POR: 
         [0000]      f 2 J 2   +     T   J 1   T f* 1 +J 2   +     T   N 1 λ  (42) 
         [0000]    Where J 2  is the Jacobian for the second POR. 
         [0058]    Finding the value for λ that makes f 2  as close as possible to f* 2  gives: 
         [0000]    
       
         
           
             
               
                 
                   
                     
                       
                         
                           γ 
                           min 
                         
                         = 
                         
                           arg 
                            
                           
                             
                               min 
                               λ 
                             
                              
                             
                                
                               
                                 
                                   f 
                                   2 
                                 
                                 - 
                                 
                                   f 
                                   2 
                                   * 
                                 
                               
                                
                             
                           
                         
                       
                     
                   
                   
                     
                       
                         = 
                         
                           arg 
                            
                           
                             
                               min 
                               λ 
                             
                              
                             
                                
                               
                                 
                                   
                                     J 
                                     2 
                                     
                                       + 
                                       T 
                                     
                                   
                                    
                                   
                                     J 
                                     1 
                                     T 
                                   
                                    
                                   
                                     f 
                                     1 
                                     * 
                                   
                                 
                                 + 
                                 
                                   
                                     J 
                                     2 
                                     
                                       + 
                                       T 
                                     
                                   
                                    
                                   
                                     N 
                                     1 
                                   
                                    
                                   λ 
                                 
                                 - 
                                 
                                   f 
                                   2 
                                   * 
                                 
                               
                                
                             
                           
                         
                       
                     
                   
                   
                     
                       
                         = 
                         
                           
                             
                               J 
                               ^ 
                             
                             2 
                             + 
                           
                            
                           
                             [ 
                             
                               
                                 f 
                                 2 
                                 * 
                               
                               - 
                               
                                 
                                   J 
                                   2 
                                   
                                     + 
                                     T 
                                   
                                 
                                  
                                 
                                   J 
                                   1 
                                   T 
                                 
                                  
                                 
                                   f 
                                   1 
                                   * 
                                 
                               
                             
                             ] 
                           
                         
                       
                     
                   
                 
               
               
                 
                   ( 
                   43 
                   ) 
                 
               
             
           
         
       
     
         [0000]    Where Ĵ 2 =J 2   +     T   N. 
         [0059]    Integrating this result with equation (41), the joint torque that optimally achieves both prioritized objectives is: 
         [0000]      τ=( I−Ĵ   2   +   J   2   +     T   ) J   T   f*+Ĵ   2   +   f*   2   (44) 
         [0060]    Substituting into equation (1) and setting accelerations to zero (because of the steady-state assumption) gives the following control law: 
         [0000]      τ a =−( I−Ĵ   2   +   J   2   +     T   ) J   T   f*+Ĵ   2   +   f*   2 +η  (45) 
         [0061]      FIG. 3  is a flow chart diagram  80  showing a depiction of how a system and method of the invention combines force control, position control and impedance control in task coordinates for multi-priority impedance control. The diagram  80  includes a hierarchical task controller  82  that identifies a number of tasks at boxes  84  for a particular robotic arm. Each task has a certain mass M, damping B and stiffness K for the impedance control portion of the task and a particular force F and task position X for the position control portion of the task. Because the operation is performing impedance control, each of the mass M, damping B and stiffness K are defined. Particularly, for example, a robotic finger moves to the particular position X with the particular force F defined by that task. Once all of the tasks at the boxes  84  have been identified in a hierarchical manner, the process determines the equivalent values for the mass, damping, stiffness, force and position in the joint space at box  86 . Once each of the equivalent values for the mass, damping, stiffness, force and position is provided in the joint space for the joints of the robotic arm, then the tasks are performed at box  88  using a series-elastic joint control scheme at a high-rate operation. The tasks are executed based on the hierarchy control defined by the multiple priority impedance control. 
         [0062]    The foregoing discussion discloses and describes merely exemplary embodiments of the present invention. One skilled in the art will readily recognize from such discussion and from the accompanying drawings and claims that various changes, modifications and variations can be made therein without departing from the spirit and scope of the invention as defined in the following claims.