WO2020001742A1 - Device for controlling a robot manipulator - Google Patents

Abstract

The invention relates to a Device (10) for controlling a robot manipulator (20) with an endeffector (21), comprising: - A first unit (1) being configured to provide a measured or estimated external wrench F̅̅ ext acting on the manipulator and/or on the endeffector, - a controller (3) being connected to the first unit (1), wherein the controller (3) comprises a force controller (5) and an impedance controller (7), wherein the force controller (5) is configured to generate a preliminary force control signal F f based upon a predefined desired force signal F d and upon F̅̅ ext , and configured to modify F f by applying an attenuating function α f derived from a first virtual energy tank T f to obtain a final force control signal F' f , wherein T f is based upon a measured or estimated velocity ẋ of the robot manipulator and/or of the endeffector and upon F f , and is configured to transform F' f to a force control signal u' f with a Jacobian map J, wherein the impedance controller (7) is configured to modify a preliminary desired velocity ẋ d signal by applying an attenuating function α i derived from a second virtual energy tank T i to obtain a final desired velocity signal ẋ' d , wherein T i is based upon ẋ d , and upon F' f and F̅̅ ext , and wherein the impedance controller (7) is configured to generate an impedance control signal u' i based upon ẋ' d and upon ẋ, and - a summation unit (9) configured to generate an actuator command u' by adding u' f and u' i .

Description

Device for controlling a robot manipulator

The invention relates to a device for controlling a robot manipulator with an endeffector, a robot manipulator with an endeffector and such a device, and a method of controlling a robot manipulator with an endeffector.

The sources of prior art mentioned below and additional sources are as follows: [1 ] C. Schindlbeck and S. Haddadin,“Unified passivity-based cartesian force/impedance control for rigid and flexible joint robots via task- energy tanks,” in IEEE International Conference on Robotics and Automation (ICRA2015), Seattle, USA, 2015.

[2] N. Hogan,“Impedance control: An approach to manipulation: Part I - theory, part II - implementation, part III - applications,” ASME Journal of Dynamic Systems,

Measurement, and Control, vol. 107, pp. 1-24, 1985.

[3] A. Albu-Schaffer, M. Fischer, G. Schreiber, F. Schoeppe, and G. Hirzinger,“Soft robotics: what cartesian stiffness can obtain with passively compliant, uncoupled joints?” in Intelligent Robots and Systems, 2004.(IROS 2004). Proceedings. 2004 IEEE/RSJ International Conference on, vol. 4. IEEE, 2004, pp. 3295-3301.

[4] L. Villani and J. D. Schutter,“Force control,” in Springer Handbook of Robotics.

Springer, 2008, pp. 161-185.

[5] E. Lutscher and G. Cheng,“Constrained manipulation in unstructured environment utilizing hierarchical task specification for indirect force controlled robots,” in IEEE International Conference on Robotics and Automation 2014 (ICRA2014), 2014.

[6] D. Lee and K. Huang,“Passive-set-position-modulation framework for interactive robotic systems,” Robotics, IEEE Transactions on, vol. 26, no. 2, pp. 354-369, 2010.

[7] E. Lutscher, E. C. Dean-Leon, and G. Cheng,“Hierarchical force and positioning task specification for indirect force controlled robots,” IEEE Transactions on Robotics, 2017.

[8] G. Borghesan and J. De Schutter,“Constraint-based specification of hybrid position- impedance-force tasks,” in IEEE International Conference on Robotics and Automation

2014 (ICRA2014), 2014.

[9] M. Raibert and J. Craig,“Hybrid position/force control of manipulators,” ASME Journal of Dynamical Systems, Measurement and Control, vol. 105, pp. 126-133, 1981.

[10] G. Zeng and A. Hemami,“An overview of robot force control,” Robotica, vol. 15, no. 05, pp. 473-482, 1997. [11 ] J. Duffy,“The fallacy of modern hybrid control theory that is based on orthogonal complements of twist and wrench spaces,” Journal of Robotic Systems, vol. 7, no. 2, pp. 139-144, 1990.

[12] K. Ohishi, M. Miyazaki, M. Fujita, and Y. Ogino,“H observer based force control without force sensor,” in Industrial Electronics, Control and Instrumentation, 1991.

Proceedings. IECON’91., 1991 International Conference on. IEEE, 1991 , pp. 1049-1054.

[13] T. Murakami, F. Yu, and K. Ohnishi,“Torque sensorless control in multidegree-of- freedom manipulator,” Industrial Electronics, IEEE Transactions on, vol. 40, no. 2, pp. 259-265, Apr 1993.

[14] K. S. Eom, I. H. Suh, W. K. Chung, and S.-R. Oh,“Disturbance observer based force control of robot manipulator without force sensor,” in Robotics and Automation, 1998. Proceedings. 1998 IEEE International Conference on, vol. 4. IEEE, 1998, pp. 3012-3017.

[15] H. Cho, M. Kim, H. Lim, and D. Kim,“Cartesian sensor-less force control for industrial robots,” in IEEE/RSJ Int. Conf. On Intelligent Robots and Systems (IROS2014), Chicago, USA, 2014, pp. 1623-1630.

[16] A. De Luca, A. Albu-Schaffer, S. Haddadin, and G. Hirzinger,“Collision detection and safe reaction with the dlr-iii lightweight manipulator arm,” in International Conference on Intelligent Robots and Systems (IROS), 2006 IEEE/RSJ. IEEE, 2006, pp. 1623-1630.

[17] S. Haddadin, A. Albu-Schaffer, A. De Luca, and G. Hirzinger,“Collision detection and reaction: A contribution to safe physical human-robot interaction,” in Intelligent Robots and Systems, 2008. IROS 2008. IEEE/RSJ International Conference on Intelligent Robots and Systems. IEEE, 2008, pp. 3356-3363.

[18] V. Duindam and S. Stramigioli,“Port-based asymptotic curve tracking for mechanical systems,” European Journal of Control, vol. 10, no. 5, pp. 411^120, 2004.

[19] C. Secchi, S. Stramigioli, and C. Fantuzzi,“Position drift compensation in port- hamiltonian based telemanipulation,” in Intelligent Robots and Systems, 2006 IEEE/RSJ International Conference on. IEEE, 2006, pp. 4211— 4216.

[20] M. Franken, S. Stramigioli, S. Misra, C. Secchi, and A. Macchelli,“Bilateral telemanipulation with time delays: A two-layer approach combining passivity and transparency,” Robotics, IEEE Transactions on, vol. 27, no. 4, pp. 741-756, 2011 .

[21] A. Franchi, C. Secchi, H. I. Son, H. H. Bulthoff, and P. R. Giordano,“Bilateral teleoperation of groups of mobile robots with time-varying topology,” Robotics, IEEE Transactions on, vol. 28, no. 5, pp. 1019- 1033, 2012.

[22] C. Secchi, A. Franchi, H. Bulthoff, and P. R. Giordano,“Bilateral teleoperation of a group of UAVs with communication delays and switching topology,” in Robotics and Automation (ICRA), 2012 IEEE International Conference on. IEEE, 2012, pp. 4307^1314. [23] F. Ferraguti, C. Secchi, and C. Fantuzzi,“A tank-based approach to impedance control with variable stiffness.” in IEEE International Conference on Robotics and

Automation 2013 (ICRA2013), 2013, pp. 4948-4953.

[24] T. S. Tadele, T. J. De Vries, and S. Stramigioli,“Combining energy and power based safety metrics in controller design for domestic robots,” in Robotics and Automation (ICRA), 2014 IEEE International Conference on. IEEE, 2014, pp. 1209-1214.

[25] J. J. Craig,“Introduction to robotics: Manipulation and control,” 1986.

[26] A. Albu-Schaffer, C. Ott, and G. Hirzinger,“A unified passivity-based control framework for position, torque and impedance control of flexible joint robots,” The Int. J.

Of Robotics Research, vol. 26, pp. 23-39, 2007.

[27] B. Paden and R. Panja,“Globally asymptotically stable pd-i-controller for robot manipulators,” International Journal of Control, vol. 47, no. 6, pp. 1697-1712, 1988.

[28]“Franka Emika” https://www.franka.de/, accessed: 2017-01 -08.

[29] M. Spong,“Modeling and control of elastic joint robots,” ASME J. On Dynamic Systems, Measurement, and Control, vol. 109, pp. 310-319, 1987.

[30] C. Ott, A. Albu-Schaffer, A. Kugi, S. Stramigioli, and G. Hirzinger,“A Passivity Based Cartesian impedance controller (7) for Flexible Joint Robots - Part I: Torque Feedback and Gravity Compensation,” in IEEE International Conference on Robotics and

Automation 2004 (ICRA2004), 2004, pp. 2666-2672.

[31 ] J. Vorndamme, M. Schappler, and S. Haddadin,“Collision detection, isolation and identification for humanoids,” 2017.

[32] S. Haddadin, Towards Safe Robots: Approaching Asimov’s 1 st Law.

Springer Publishing Company, Incorporated, 2013.

[33] S. Haddadin, A. De Luca, and A. Albu-Schaffer,“Robot collisions: Detection, isolation, and identification,” Submitted to IEEE Transactions on Robotics, 2017.

[34] O. Khatib,“Inertial properties in robotic manipulation: An object-level framework,” Int. J. Of Robotics Research, vol. 14, no. 1 , pp. 19-36, 1995.

[35] A. Albu-Schaffer, C. Ott, U. Frese, and G. Hirzinger,“Cartesian impedance control of redundant robots: Recent results with the DLR- light-weight-arms,” in Robotics and Automation, 2003. Proceedings. ICRA’03. IEEE International Conference on, vol. 3. IEEE, 2003, pp. 3704-3709.

[36] H. K. Khalil,“Nonlinear systems, 3rd,” New Jewsey, Prentice Hall, vol. 9, 2002.

[37] J. T. Wen and S. Murphy,“Stability analysis of position and force control problems for robot arms,” in Robotics and Automation, 1990. Proceedings., 1990 IEEE International Conference on. IEEE, 1990, pp. 252-257. [38] S. Jung, T. C. Hsia, and R. G. Bonitz,“Force tracking impedance control of robot manipulators under unknown environment,” IEEE Transactions on Control Systems Technology, vol. 12, no. 3, pp. 474^183, 2004.

[39] E. Shahriari, A. Kramberger, A. Gams, A. Ude, and S. Haddadin,“Adapting to contacts: Energy tanks and task energy for passivity-based dynamic movement primitives.”

[40] F. Caccavale, B. Siciliano, and L. Villani,“Quaternion-based impedance with nondiagonal stiffness for robot manipulators,” in American Control Conference, 1998. Proceedings of the 1998, vol. 1. IEEE, 1998, pp. 468^172.

Some of the following information are to be considered pure background knowledge, e.g. publicly observable tendencies in industry and do not necessarily refer to a particular technical solution in prior art.

Nowadays, robotic systems outperform humans in terms of repetitive speed and precision tasks. In terms of sensitive force and compliance control humans still show superior performance. However, at the same time an increasing set of tasks in robotic

manipulation, and in particular also in real- world applications, deals now with sensitive object handling and assembly. This requires an intricate coordination of contact force and motion generation for which sophisticated control algorithms were developed over the last decades. In this context, impedance control [2] has become one of the most popular concepts, which aims for mimicking human behavior by imposing mass-spring-damper- like response via active control on the robot. In general, compliance in robotic systems, either achieved via active control or by deliberately introducing compliant mechanical elements into the drive train has become very popular due to their ability to cope with process uncertainty and exert only well defined force ranges on their environment or the objects they manipulate. However, it is usually not possible to display arbitrary Cartesian compliance with uncoupled elastic joints only [3], making it necessary to combine passive compliance with active control to solve the issue. Active interaction control can be subdivided into direct and indirect force control [4] Recently, indirect force controllers using set-point generation [5]-[7] were introduced. A constraint-based access to control force, position, and impedance in different subspaces was proposed in [8]. Despite the significant progress that was made in the domain of force control, some basic problems remained unsolved. For example, an impedance controller (7) with a closed loop stiffness K executes desired forces f_D (during rigid contact) typically through establishing a virtual displacement x_d from the true robot position x, such that f_d = KAx. Therefore, the energy stored in the virtual spring is E = ^Ax^TKAx. These controllers do not take into account sensed external forces. Hence, in order to accurately apply desired forces, the exact surface geometry and the contact properties (stiffness) need to be known a-priori. This of course contradicts the core idea of an impedance controller to work effectively in unmodeled environments. Furthermore, this paradigm has the drawback that when applying a larger force with such a feed-forward approach and then unexpectedly loosing contact with the environment, this may result in an undesired and possibly very unsafe motion of the robot towards the (distant) set-point. This happens due to an instantaneous energy release of the potential energy stored in the preloaded virtual spring to kinetic energy. The direct force control paradigm provides the basics to accurately exchange contact forces and thus directly manipulate objects or apply forces on surfaces. This capability is also a core necessity in industrial applications, since the rather imprecise impedance control based force regulation, or let alone position control, do not suffice due to modeling and/or planning errors, resulting in possibly large process uncertainties. This problem has led to approaches such as hybrid position/force control [9], which idea is to partition the task space into complementary force and motion subspaces such that force and motion control is only applied in its respective subspace. However, a major drawback of (hybrid) force control methods is that they show very low robustness with respect to contact loss. Furthermore, the environment contact properties need to be modeled very accurately for good performance, which is hardly ever the case. Furthermore, in order to determine the stability properties of force controllers, the environment is typically modeled as a simple spring-damper system. An overview of different classical force control algorithms, including remarks on their respective stability properties can be found in [10]. A very general critique regarding hybrid force/motion control was also formulated in [11] that was based on the problem of coordinate choice and the respective choice of metric. To achieve force control, external forces need to be known or at least estimated. Force sensors located at the robot’s endeffector are not only expensive but also limit the payload of the robot. Therefore, it seems reasonable to find ways to relieve this burden by constructing observers for sensed external forces. In literature, these methods are often named“sensorless" and have received significant attention lately. In [12] a force observer is designed based on an H -controller. In [13], [14] the external torque is estimated by employing a disturbance observer and mapped via the Jacobian in order to obtain external forces for a rigid robot model. Flexible joint robots equipped with torque sensors allow to measure the torque between the motor and the link due to inherent flexibilities caused by e.g. gears. Similar to the aforementioned approach for rigid robot models, in [15] the external force is treated as a disturbance to the flexible joint model and therefore estimated via a disturbance observer. Another approach for flexible joint robots is presented in [16], [17] via a momentum-based observer, which has been shown to yield a first-order filtered version of the real external torques. The concept of energy tanks has been introduced in [18] and then subsequently been applied in the field of haptic interfaces [19], in the field of teleoperation of single and multi-robots [20]-[22], and in the context of impedance control in terms of variable stiffness [23] or safety metrics [24] The first integration of PID-control via energy tanks has been carried out in [1]

It is the objective of the invention to provide a device and a method for improved handling of manipulation skills.

A first aspect of the invention relates to a device for controlling a robot manipulator with an endeffector. The device comprises a first unit being configured to provide a measured or estimated external wrench F_ext acting on the manipulator and/or on the endeffector, and a controller being connected to the first unit, wherein the controller comprises a force controller and an impedance controller. The force controller is configured to generate a preliminary force control signal F_f based upon a predefined desired force signal F_d and upon F_ext, and configured to modify F_f by applying an attenuating function a_f derived from a first virtual energy tank 7) to obtain a final force control signal F'_f , wherein T_f is based upon a measured or estimated velocity x of the robot manipulator and/or of the endeffector and upon F_f , and is configured to transform F'_f to a force control signal u'_f with a Jacobian map ]. Furthermore, the impedance controller is configured to modify a preliminary desired velocity x_d signal by applying an attenuating function a_t derived from a second virtual energy tank T* to obtain a final desired velocity signal x'_d, wherein T* is based upon x_d, and upon F'_f and F_ext. Moreover, the impedance controller is configured to generate an impedance control signal u based upon x'_d and upon x. The device further comprises a summation unit configured to generate an actuator command u' by adding u'_f and u'*.

It is an advantage of the invention that the classical concepts from active compliance and force control are extended and unified into a single framework. To guarantee the stability of the overall system, passivity analysis can be thoroughly applied to it. This invention provides a robust passivity-based approach by combining force tracking with impedance control based on the concept of energy tanks. The demonstrated approach guarantees stability for arbitrary passive environments and has no need to apply set-point variations, which can show rather inaccurate behavior for general environments. The invention allows for robust, compliant, and stable manipulations without the need to choose between a force controller or an impedance controller, but rather unify the best of all. Furthermore, a solution is presented that is able to get rid of the inherent drawback of force and set-point based indirect force control: the low robustness with respect to contact loss and the according possibility of unsafe abrupt robot motions. In summary, the advantages of the invention are a simultaneous passivity-based impedance control and wrench regulation and tracking, stability for arbitrary passive environments instead of model-based environments can be proved, the invention is suitable for rigid and flexible joint robot models, and in certain embodiments a task-based energy tank design and initialization, and a contact/non-contact stabilization is shown. Preferred applications involve

manipulation tasks especially for service robots in which applying certain force is required during the motion around an unmodeled environment. An example can be polishing or grinding car parts, where the robot manipulator should adapt to and move on objects with complex geometries and apply force at the same time. Other examples may include wood and metal working such as filing and carving, or household chores like cutting fruits.

According to an embodiment of the invention for the transposed vector of F_ext, namely F _xt, the following holds: F _xt = [ f^T, m^T ], wherein f^T is the transposed vector of a column vector of forces and m^T is the transposed vector of a column vector of moments.

According to another embodiment of the invention the first unit is an observer configured to provide the estimated external wrench F_ext acting on the manipulator and/or on the endeffector.

According to another embodiment of the invention the force controller comprises a force switching logic unit, wherein the force switching logic unit is configured to modify F_f to obtain F'_f by applying the attenuating function a_f to F_f , only if a condition y_f is not fulfilled.

When the condition

is fulfilled, in particular this means = 1. When the condition y is not fulfilled, in particular this means

= 0.

According to another embodiment of the invention

is fulfilled if x^TF_f < 0, wherein x^T is the transposed vector of the measured or estimated velocity x.

According to another embodiment of the invention 7} comprises a tank overloading term b_b, such that a time derivative of T_f is zero, namely†_f = 0,

= 0 and is fulfilled, wherein for b holds: /3 = 1 if 7 < T^UJ and b/ = 0 otherwise, wherein T^UJ is a predefined upper limit storage.

According to another embodiment of the invention 7} is proportional to ^x _tf and wherein a time derivative of x_tf , namely x_tf is proportional to x^TF_f , wherein x^T is the transposed vector of the measured or estimated velocity x.

According to another embodiment of the invention the impedance controller comprises an impedance switching logic unit, wherein the impedance switching logic unit is configured to modify x_d signal to obtain x'_d by applying the attenuating function a_t to x_d, only if a condition g is not fulfilled.

When the condition g is fulfilled, in particular this means y* = 1. When the condition is K_jnot fulfilled, in particular this means g = 0.

According to another embodiment of the invention g is fulfilled if x_d {F'f + F_ext) > 0, wherein x_d is the transposed vector of the column vector containing the preliminary desired velocity x_d signal.

According to another embodiment of the invention 7^ is proportional to xf_t and wherein a time derivative of x_ti, namely x_ti is proportional to x_d (F'_f + F_ext), wherein x_d is the transposed vector of the column vector containing the preliminary desired velocity x_d signal.

According to another embodiment of the invention the force controller is configured to modify F_f by applying the attenuating function a_f and based upon a function p to obtain the final force control signal F'_f, wherein p is a function-valued vector comprising translational and rotational components with controller shaping functions.

According to another embodiment of the invention the first virtual energy tank 7} is initialized with an initial task energy E_Tf and/or the second virtual energy tank 7^ is initialized with an initial task energy E_{T i2}.

Another aspect of the invention relates to a robot manipulator with an endeffector and a device according to one of the preceding claims.

Another aspect of the invention relates to a method of controlling a robot manipulator with an endeffector, comprising the steps of:

- Providing a measured or estimated external wrench F_ext acting on the manipulator and/or on the endeffector by a first unit,

- Generating a preliminary force control signal F_f based upon a predefined desired force signal F_d and upon F_ext by a force controller,

- Modifying F_f by applying an attenuating function a_f derived from a first virtual energy tank T_f to obtain a final force control signal F'_f by the force controller, wherein T_f is based upon a measured or estimated velocity x of the robot manipulator and/or of the endeffector and upon F_f ,

- Transforming F'_f to a force control signal u'_f with a Jacobian map J by the force controller,

- Modifying a preliminary desired velocity x_d signal by applying an attenuating function a_t derived from a second virtual energy tank T* by an impedance controller to obtain a final desired velocity signal x'_d, wherein 7) is based upon x_d and upon F'_f and F_ext, and

- Generating an impedance control signal u based upon x'_d and upon x by the impedance controller, and

- Generating an actuator command u' by adding u'_f and u by a summation unit.

In the following the main aspects of the invention are derived, further explained, and preferred embodiments are shown how these are preferably being implemented:

The following notation is used in the following:

The robot posex e M⁶ is defined as x: = ( r^t, f^t)^t . It is comprised of a translational part p e ff³ and preferably a suitable rotational representation, e.g. Euler angles f e ff³.

Similarly, wrenches are preferably defined as F _xt = [ f^T, n^T ] consisting preferably of translational forces / e ff³ and rotational moments m e ff³. Moreover, n denotes the degrees of freedom (DOF) of the system.

First case, a rigid robot model:

The well-known dynamics of a second-order rigid body robot model with n DOFs are preferably described by

M(q)q + C(q, q)q + g(q ) = T_m + T_ext (1 ) where q, q, q ^nxn are respectively link position, velocity, and acceleration. The symmetric positive definite mass matrix is denoted by M(q ) e M^nxn, the Coriolis and centrifugal matrix by C(q, q ) e Mⁿ and the gravity vector by g{q ) e Mⁿ. The control input of the system is the motor torquer_m e Mⁿ, while r_ext e Mⁿ comprises all externally applied torques. Friction is for this case (and for the motor-side dynamics) not considered for sake of clarity. However, the derived theory does hold without loss of generalization. External forces are denoted by the Cartesian space wrenches F_ext = [f_ext ^T , m_ext ^T]^T e M⁶. This wrench can be mapped via the contact Jacobian ]^T(q ) to joint space external torques by ^t _Cί— J (J)F_ext·

Second case, flexible joint robot model:

For lightweight or series elastic actuator-type systems, equation (1 ) may not sufficiently accurate to describe the inherent dynamics due to the presence of flexible transmission. The joint elasticity caused by gears (e.g. Harmonic Drive gear) and integrated joint torque sensors may not be negligible and may have to be taken into account. Therefore, the (reduced) flexible joint model from [29] will be preferably considered for such structures. It is preferably defined as

M(q)q + C(q, q)q + g(q ) = t_a + r_ext (2)

BQ + T_a = T_m (3)

ta = k - q) + D0 - q) (4) with Q e Mⁿ being the motor position. Equations (2) and (3) constitute the link- and motor- side dynamics, respectively.

Equation (4) couples (2) and (3) via the elastic joint torque t_a e Mⁿ which is considered to have linear viscoelastic characteristics, i.e. a stiffness and a damping. The matrices K, D, B e ^nxn are constant diagonal positive definite matrices expressing the lumped joint stiffness, damping and motor inertia, respectively. Again, no friction effects are considered, neither on motor nor on link side.

Inertia and Damping Scaling by Joint Torque Feedback:

Robot manipulators equipped with joint torque sensing allow for shaping the motor inertia and joint damping by preferably employing the following control law [30]

such that the motor-side dynamics as well as the joint dynamics from (3) and (4) can be written as

B_bq + t_a = u (6) T_a = K(e - q) + D_e - q) (7) yielding a scaled motor inertia B_q and joint damping D_e. Furthermore, the system is driven by a new auxiliary control input u. Without loss of generality, the subsequent theory still holds if no inertia scaling is done. For a thorough analysis without inertia scaling see [1].

Sensing the external wrench F_{ext \}

1 ) Wrist Wrench Sensing: Preferably, external contact wrenches at the endeffector are measured by a force/torque sensor in the wrist. However, beyond parasitic parts such as noise or sensor drift the wrench caused by the load in order to obtain F_ext from the sensor reading is preferably taken into account. These effects are caused by gravitational and kinetic energy of the sensor body as well as any inertial body attached to it. The formulation of this problem and a systematic approach to accurately estimate this load can be found in [31]. Hence, for sake of simplicity it is assumed that F_ext is the pure external wrench, acting on the endeffector of the robot manipulator.

2) Joint Torque Sensing: Alternatively, observer-based approaches to obtain external wrenches instead of using a designated wrist sensor are preferably applied. Typically, the momentum- based observer proposed and analyzed in [16], [17], [32] is the state of the art to obtain an accurate estimation of r_ext based on joint torque sensing, an accurate dynamics model and without the need for the sensor. The dynamics of the observer are preferably written as

where p = M(q)q is the generalized momentum of the robot manipulator and K₀ the designed positive definite observer gain, such that†_ext ® r_ext for K₀ ® ¥. The

momentum-based observer yields an inherently stable first-order filter of the external torque for a positive definite observer gain which is related to the cut-off frequency. An overview over various methods to observe r_ext as well as their advantages and disadvantages is given in [33]. The relationship

then yields the corresponding estimation of external forces F_ext e M⁶ in Cartesian space via the right-hand Moore-Penrose pseudo inverse ]^#{q) = ]^T (q)(J (q)]^T (q))^-1. In case the left-hand pseudo inverse J^#(q ) = (J^T (q^')Kq^'))^~1J^T (q) is used, (9) becomes

Fe_Xt = J^T q)i_ext where (9) and (10) deliver F_ext as explained above. The stability analysis shown later does not consider any effect whether the contact wrench is obtained from wrist sensing or external joint torque estimation. Thus, herein the sensed external force is denoted as F_ext instead of distinguishing between a perfectly sensed F_ext and an estimated (e.g. by an observer as in (8)) F_ext. Analogue to this, a sensed and an estimated pose signal as well as the actual value are both denoted x, without taking sensor inaccuracies into account. Any sensor noise is small or zero, and in general at least assumed to be bounded. This is particular due to the fact that sensor noise or bias is in general independent of any other state of the entire dynamic system.

CONTROLLER DESIGN

The force controller is preferably a PID force controller, whose feedforward term is preferably designed in Cartesian space and mapped into joint space via the endeffector Jacobian. a) Rigid Robot: For rigid robots the control law of the force controller is preferably chosen as

K_p, K_d, Ki e M^6x6 are positive definite diagonal gain matrices. Fpp is an optional feedforward term to improve the overall force control behavior. The power associated to the force controller is

b) Flexible Joint Robot: For the flexible joint case the control law of the force controller preferably is:

^/,p ^Tf,p

uf,i ^'— ^t/,ί

uf,d ^' = ^Tf,d

uf,FF -· ^{= T}f,FF Rigid Joint Cartesian Impedance Control: a) Tracking: In order to have a closed loop Cartesian impedance behavior with the desired stiffness K_x e M^6x6, damping D_x e M^6x6, with inertia being identical to the real Cartesian robot inertia, the control law of the impedance controller preferably becomes [27]

where r_m = T_j in (1 ). The map f: Mⁿ ® M⁶ denotes the forward kinematics and x_d e M⁶ the differentiable desired Cartesian position. M_c(q ), C_c(q, q) and F_g q ) are respectively the inertia matrix and the Coriolis and centrifugal matrix as well as gravity vector of the robot expressed in Cartesian space. These relate to the equivalent joint quantities via [34]

b) Regulation: In case the impedance desired pose is constant (x_d = 0) the control law reduces to the well-known compliance control law

For both cases the matrices K_x and D_x are positive definite. D_x is preferably chosen manually (constant, often diagonal) or obtained by appropriate damping design [35] (state dependent, non-diagonal) such that e.g. critical damping is achieved even for varying configuration. However, positive definiteness has to be ensured.

Flexible Joint Cartesian Impedance Control: a) Tracking: An impedance controller for the flexible joint case - in contrast to existing works - is preferably introduced as

Here, u = u_t in (6) and B_{e c}(q) is the shaped motor inertia projected into Cartesian space via

B_ec(q) = J^#T (q)B_eJ^#(q ) (24)

Moreover, c_q is defined as c_q : = J(q)6 (25) and X_{ff d} is introduced as a desired motor position, and can be obtained from solving Kc(q)(xed - XD) + Dec{q)(xe,d - Xd) = M_c(q)x_d + C_c(.q, q)x_d (26) where K_c(q ) e M⁶ and D_{e c}(q) e M⁶ are the joint stiffness and damping projected into Cartesian space via

Hence, x is the equivalent of x in quasi-static case. More details can be found in [30]. b) Regulation: For the flexible joint regulation case the compliance control law is preferably chosen to be

Note that (21 ) does not simply reduce to (32) by setting x_d = 0. In fact, this second collapsed regulation law is

Again, c_{q ά} is obtained from solving (26).

Preliminary Controller as a combination of force controller and impedance controller:

For the rigid robot summing

leads to the preferred preliminary native combined force controller - impedance controller

T_m = Ti + Tf (34) while for the flexible joint robot the combined controller is preferably u = Ui + U_p (35)

In the course of the subsequent stability analysis, it will become evident that this native unified controller requires significant modifications to guarantee passivity and stability. Based on this, the proposed concept of unifying force and impedance control is derived.

STABILITY ANALYSIS:

Definitions: Stability in terms of Lyapunov is defined as follows [36].

Theorem 1 : (Lyapunov stability)

Let x = 0 be an equilibrium point for x = /(x) and W e Eⁿ be a domain containing x = 0. Let : D ® l be a continuously differentiable function such that 7(0) = 0 and (x) > 0 in D \ 0, V (x) < OinD. Then, x = 0 is stable Moreover, if V(x) < Oin D \ {0} then x = 0 is asymptotically stable.

Theorem 2: (Passivity)

A state-space model of the standard nonlinear form x = /(x, u) with output y = h(x, u is said to be passive if there exists a continuously differentiable positive semidefinite function S(x) (storage function) such that , it) (36)

A well-known property of passivity-based control lies therein that passivity is preserved if passive blocks are interconnected in parallel or feedback. Subsequently, this property be exploited in the following in order to prove the stability of the proposed closed- loop system.

Preliminary Passivity Analysis:

The first step of the passivity analysis is preferably to perform a port-based decomposition of the closed-loop system, i.e. a division into blocks that only communicate via their respective power variable pairs, namely efforts and flows. The preferred chosen blocks are the environment and the combined rigid body dynamics with the combined force controller -impedance controller. In the following, these blocks are analyzed with regard to their passivity properties.

1 ) Environment: An important advantage of passivity-based control is that there is no need to exactly model the considered environment to prove stability except for assuming it to be passive. Therefore, there is no restriction to specific types of environments, which on the other hand is typically assumed in stability proofs of force controllers [37], [38]. In fact, it is sufficient to assume that the environment is passive with regard to the pair (x, -F_ext). Necessarily, this implies that

~x^TF_ext £ S_env (37) holds, where S_env is a positive definite storage function associated to the passive environment. Obviously, non-passive environments inject energy into the system and may therefore potentially destabilize it. Force controller - impedance controller rigid robot: a) Tracking: The closed loop dynamics of the force controller- impedance controller controlled robot in Cartesian space is preferably (see Appendix, section A)

When defining its storage function to be

it is straight forward to see that (see Appendix, section C)

wherein 5c^TD_x5c ³ 0

Thus, as the sign of x^TF_f and x_d (F_f + F_ext ) are a priori unknown, and defining proper storage functions for the relative ports is not feasible, considering (36), passivity with respect to (x, F_ext) cannot be guaranteed. b) Regulation: For compliance control (20), the closed-loop dynamics (38) becomes

M_c(q)x + C_c(q, q)x + D_xx + K_xx = F_f + F_ext (41 )

The storage function is preferably

and its time-derivative

S_cy. = x

wherein

Due to the existence of x^TF_f, passivity is not guaranteed. Force-Impedance Controlled Flexible-Joint Robot: a) Tracking: Following the same line of analysis as for the rigid case, the closed-loop behavior of the force-impedance controlled flexible joint robot in Cartesian space will be (see Appendix, section B)

where it is assumed that

Fg{q) Fg ) (45)

The proposed storage function is

which despite having a negative quadratic term, is a proper storage function following the same argument as [30]. As shown in Appendix, section D, the time-derivative of the storage function becomes

Here it is assumed that J{q) = const locally such that c_q = J{q)6 (see (95) in Appendix section B). Again, due to the unknown characteristics of the port related to the force controller

the ports associated to the impedance control in contact with environment (i .e.{x_d, -F_ext)) and regarding the interaction with the force controller (x_d, -F_f), passivity can not be guaranteed with regard to the pair {±, F_ext). b) Regulation: For the compliance controller (32) with (45) the closed-loop dynamics is

Its storage function and respective time derivative are

Once more, due to the unknown characteristics of the force controller port (x, F_f), passivity can not be guaranteed with regard to the pair (x, F_ext). It should be noted that for the second compliance control (33), the closed loop dynamics (48) turns to

The proposed storage function is:

Nevertheless, the time-derivative of the storage function will have the same form as (50). Tank-based Augmentation I: First virtual energy tank:

In order to solve the aforementioned problem of passivity violation the concept of virtual energy tanks is applied. The subsequent concept and proof follows similar lines of thought as proposed in [1], [23]. First, a preferred virtual energy tank for the force

controller, i.e. the first virtual energy tank, is introduced (i.e. the port ( x, F_f )). Its energy is preferably defined as ^Tf = \ ^xtf , where x_tf denotes its state with preferred dynamics

( )

Yf = 1 else and b_b is defined as b_b = 1 iVT_f £ T^UJ and

(55)

b = 0 else and its purpose is to avoid tank overloading if a certain upper limit storage T^UJ is reached. Finally while a_f could simply be defined as a binary signal, for sake of smooth attenuating the transition from filled to empty, a_f is preferably chosen as a_f = l\f Tf ³ T_{l f} + 5_Tf and

a/ = - \1— cos( ^_dt^ ^p)] 'f T^' _l + ^T ³ T_f ³ Ti and (56)

<X = 0 else

Herein, a_f is responsible for smoothly detaching the energy tank from the force- impedance controller, after the tank energy crossed the lower limit energy threshold T_lf + S_T . Lowering the gating decays to zero after some threshold S_{T f} ³ 0 and remains at zero after the lower energy bound Ti _f. Practically, this smooth behavior avoids sudden disconnection of the tank, which may lead to unwanted instantaneous change in robot motions. Moreover, when T_f reaches T_lf + S_T , the gating variable bf is set to zero in order to stop further energy filling of the tank, which may otherwise lead to chattering effects. This is preferably realized by monitoring the dynamics of

and setting /3 to zero as soon as d_f < 0. Finally, (11 ) is then multiplied with y_f . It is to be noted that it may also be possible to further sub-define

as a vector y_f [g_r, y*, g_a, g_RR]^t such that each element corresponds to one component of the force controller. However, considering that x is a flow, corresponding to F_f , it is more interpretable to define a single variable

correlated to power x^TF_f . The energy tank is preferably connected to the force controller via the port ( x, F'_f ) and as a result the control law for the force controller preferably becomes

Note: The entire design of (53)-(58) could have been done with q and instead of x and Ff . The subsequent analysis would also be analogous.

Tank-based Augmentation II: Second virtual energy tank: Using the same approach as above the second virtual energy tank is preferably defined for the port (x_d, (F'_f + F_ext)). The according tank energy is preferably defined as T_t = x_t where x_ti is the tank state with following preferred dynamics

St' = x - (yt + <Zi(l - Ui))^c _a (60)

Here, y_t is defined as + ^Fext ) > ⁰ and ₍₆₁ ,

0 else which is equivalent to g = 1 if x_d (F'_f + F_ext) > e and g = 0 else with e > 0 and e being bounded; in particular, e ® 0 for less sensor/estimation error, hence e = 0 holds for a perfect sensor or a perfect estimation.

Moreover, /?* and a_t are preferably defined as

where T^{U i} and T_{l i} are the upper and lower energy limit respectively and 5_{T i} is the designed threshold for the lower limit of the tank energy. The modified control law for the impedance controller, for obtaining the final desired velocity signal, after tank

augmentation is preferably x'_d = (Yi + a (l - Yi))x_d (64) where x'_d is the to final desired velocity signal for the impedance controller. Thus, the impedance control inputs (14) and (21 ) become

where x' = x - x'_d. Moreover, similarly to (26), c'_q:ά can be obtained form solving

K_C(_.q)(_.x'e,_d ^~ x'_d) + Do _C(q)(_.x'e,_d ^~ x'_d) = M_c(q)x'_d + C_c(q, q)x'_d (67)

Finally, note that (59) is written for rigid robots. For the flexible-joint robot the term 5c'^TD_x5c' is preferably replaced

Final passivity-based force controller-impedance controller:

Considering (57) and (65) the passivity based Cartesian force controller - impedance controller for the rigid body robot is ' n = t'ί + t / (68)

For the flexible joint robot the passivity-based force controller - impedance controller according to (57) and (66) is preferably u' = u'i + u'f (69)

Passivity analysis after tank augmentation:

1 ) Force controller - impedance controller controlled rigid robot:

The new storage function for the whole system of the rigid robot manipulator and controller becomes

Considering (53) and (59), the time evolution of T is

Depending on the value of 7 and 7; , there will be four conditions that are shown in Table A. In all cases, the time derivative of S_{tot r} will be less than or equal to the input power x^TF_ext, and as a result the overall system of rigid joint robot with force controller - impedance controller and tank is always passive with regard to (x, F_ext).

2) Force controller -impedance controller controlled flexible-joint robot: Considering (47) and following the same approach as above, the overall storage function as well as semi-negativity of its time-derivative is preferably:

Stotj = ft,/ + T (73)

(Table A)

Stability Analysis via Lyapunov Function: Consider the case of a rigid robot. The Lyapunov function V for the closed-loop dynamics are preferably derived including the environment, the link-side dynamics as well as the energy tank. A candidate Lyapunov function is given by

V(x, x_d, x, x_d, x_tf, x_{ti <} Text )^— Stot,r + Senv (75)

Now, the timely evolution of (75) along the solutions of the dynamical system of the rigid robot (1 ) with control law (57) as well as the energy tanks (53) and (59) are preferably analyzed with regard to Theorem 1. For this, inequalities of Table A as well as (37) are used.

where in (76): S_{tot r} £ x^TF_ext and S_env £ -x^TF_ext. Therefore, V is a Lyapunov function and the equilibrium point of the closed-loop system with energy tank is stable. For the special case bi = bf = 1 inequalities of Table A degrade to

Stot,r ⁼ X^T Text (77)

If the robot is now in contact with a dissipation-free (lossless: S_env = -x^TF_ext)

environment, (76) turns into

meaning that the time-derivative V can become zero even if there is no equilibrium. Thus, the asymptotic stability of this system cannot be proven anymore. However, practically such a scenario is not possible due to various dissipation effects and non-ideal compensations in the real world. Hence, for the non-equilibrium case, it is physically reasonable to conclude

and consequently also asymptotic stability.

TANK-BASED INITIALIZATION:

Although stability is proven this does not necessarily mean that the task can be fulfilled as desired. If any of the tanks (first virtual energy tank, second virtual energy tank) is not loaded with sufficient energy, the force controller will be deactivated or the impedance controller will change to compliance control. If this happens during manipulation, the intended task goal will not be achieved since the force cannot be regulated accordingly, or the desired trajectory will not be followed. For solving this problem, preferably the concept of initial task energy E_T is applied, which is defined as the minimal initial energy to be stored in the tanks for fulfilling all requirements of a manipulation task (in the given context, this is force regulation). This task energy - which was introduced in [1] and extended in [39] - needs to be known or at least estimated prior to execution, if not only stability but also correct task execution and performance are of concern.

First virtual energy tank initialization:

The translational case is considered first. In order to calculate the task energy ideal contact force tracking is assumed: fd ( ~ fext ~ fw— K_W t(Pw— Pw,o) (80) where f_w is the reaction force of the environment, which is modeled (In principle, also nonlinear relations including friction effects may be assumed, usually requiring numerical solutions for the initial tank energy) as a wall with initial position p_{W 0} and stiffness K_{w t}. Solving for p_w(t) yields the work required to move the wall as

For the regulation case f_d = const the task energy becomes preferably

This principle may be extended and adapted to the rotational case. A potential function defined via quaternions [40] is given by

E_r,f (t) = 2Ak_v ^TK_W Ak_v (83) where Ak = (Ak₀ Ak_v): = k^~ _Qk_w defines the quaternion rotation error (with scalar part Ak₀ and vector part Ak_v) from the wall initial orientation denoted by k_wfi =

and the equilibrium orientation given by k_w = (k_{0 w}, k_{v w}). Here, K_{w q} is the wall rotational stiffness matrix, defined in quaternion space. However, it should be noted that on SO(3) multiple equilibrium points exist of which only one is stable, i.e. the potential function needs to be considered only locally.

Second virtual energy tank initialization:

Depending on the relative direction of x_d and surface reactive forces, the estimated energy to perform the tracking task while in contact can be divided into two terms:

-Nonorthogonal reactive forces: When the desired trajectory is not orthogonal to the surface reactive forces, both x_dF_f and x_dF_ext will be non-zero. However, as the force controller is supposed to deal with these reactive forces, and the intention of this work is not indirect force control through desired trajectory, this case is not desirable and one can set arbitrary small value for the initial energy of the tank.

-Orthogonal reactive forces: If the desired trajectory is parallel to the surface, although x_dF_f = 0, passivity can still be violated due to friction. A preferred way to model surface translational friction is ffrc = ~ d_csgn(v ) - d_vv, d_c = m f_N (84) where v is the translational velocity on the wall surface, d_c and d_v are respectively the Coulomb and viscous coefficients, m is frictional factor, and f_N is the magnitude of reactive forces from the surface. Now, assuming an ideal force tracking, one

can estimate f_N(t ) f_d(t) and as a result, knowing surface parameters d_c and d_v, one can estimate the required energy as

where v_d is the magnitude of the translational desired trajectory.

CONTACT/NON-CONTACT STABILIZATION:

Although the controller is proven to be stable, this does not mean that unsafe motions cannot occur when losing contact with the environment. Similar to the impedance controller, which is proven to be asymptotically stable, applying a force via set point generation and losing contact leads to an instantaneous jump with according transient behavior of the robot to the specified set point. Although activating / deactivating the force when contact is detected / not detected appears as the most intuitive strategy, this may lead to unpleasant, possibly dangerous and destabilizing chattering behavior due to sensor noise, force/torque observer and environment uncertainties. Here, a novel and robust position-based strategy relying on the mechanism of controller shaping is proposed. Overall, the solution avoids abrupt motions and gives the user an intuitively interpretable design variable to shape the overall controller behavior to his needs.

Controller Shaping Function: The extended controller design preferably starts with the function-valued vector

comprising translational and rotational components. Its translational parts are preferably defined as

Pt,x^^APx)·— 0 else and the synchronized rotational part p_r(i/ is preferably

The shaping function p_{t X}(Ap_x) for the translational case is preferably constructed such that the force part of the controller is active when the desired force f_d and the vector noting the difference Dr between the current position p and the set-point p_d, namely Dr = p_d - p, point in the same direction. More precisely, if these vectors include an angle larger than (dot product in (87)) the force controller is preferably deactivated. To avoid instantaneous switching/chattering behavior, a robustness region d_max is defined, where smooth transition of the controller shaping function takes place, here preferably chosen to be a trigonometric function. In general, any smooth interpolation would suffice. The user may choose d_max depending on the task or application. Similarly, the rotation controller shaping function p_r(A<p) is constructed. To avoid singularities, r_n{Ay) is preferably based on quaternions, see also Sec. V. The rotation error is preferably defined as Ak: = k^~1k_d and Af: = 2arccos(/c₀). The user-defined rotational robustness region can then be specified as an angle i ½_aL;, which relates to the scalar component of the quaternion Ythac = 2arccos (k_{0 Tnax}). From a stability point of view, since shaping function only scales the force controller part of the combined force-impedance controller, (58) is preferably redefined as

F 7 = 0/ + _f (l - _YfW_f" (89) where

thus again stability is ensured. The multiplication of p is understood component-wise and the passivity analysis for the system with first/second energy tank can be carried out in a similar manner.

Soft-material Treatment:

If during tracking the desired force robot endeffector is in contact with a soft material, it might pass the impedance controller set point, although it is still in contact. As a result, shaping function would decrease the effect of the force controller. Thus, not only the initial energy of the respective tank should be calculated in advance, but also d_max and y _max should be designed according to rigidity of the environment so that the shaping function does not stop the force controller before contact-loss.

APPENDIX

A. Closed-loop dynamics for Force-Impedance Controlled Rigid Robot:

Considering (1 ), (10), (16), (17) and (18) the rigid robot model in Cartesian space will be

Substituting r_m with its equivalent according to (34), and considering (14) and (11 ), (91 ) becomes

B. Closed-loop dynamics for Force-Impedance Controlled Flexible-joint Robot:

Considering equations (2), (6), (10), (16-19), (24) and (25), the dynamics model of the flexible-joint robot in Cartesian space can be written as

Substituting u with its equivalent according to (35), and considering (13) and (21 ), (94) becomes

and consequently

C. Time Evolution of Force- Impedance Controlled Rigid Robot Storage Function:

Considering (38), the time-derivative of the storage function (39) is is

wherein in equation (98) the following terms are zero and cancel each other respectively ic^T(M_c(q, q ) - 2 C_c(q, q))5c = 0, and x^T(...—K_xx)... +5c^TK_xx = 0, and where the skew- symmetry of (M_c(si_> R) ^~ 2C_c(q, q)) was taken into account. D. Time Evolution of Force- Impedance Controlled Flexible Joint Robot Storage Function:

First note that rewriting (7) and (21 ) in Cartesian space for the quasi-static case leads to

On the other hand, (26) in quasi-static case turns to

KcO?)C*¾ - x) = 0 ® x_e,d = Xd (10) Thus, in quasi-static case, one can write

In order to derive the time-derivative of the storage function (46), the power equivalence of the closed loop dynamics (97) is taken into account considering the power flow 5c_e as follows:

wherein (

(M_c q)x_d

were used in Cartesian space and under the assumption that x = J(q)q ® x = ]{q)q for ] q) = const. locally. Consequently, one gets

Equivalently, this can be expanded as

wherein

and (K_c

After re-arrangement and considering beginning and end of (101 ), it can be seen that

wherein +x^rF_g q ) and -x^rF_g q) cancel each other in (108), and where it was assumed that ic^TF_g{q) ~ ^TF_g{q) (109)

Sc^TF_f * x^TF_f (110)

The invention is explained above with reference to the aforementioned embodiments. However, it is clear that the invention is not only restricted to these embodiments, but comprises all possible embodiments within the spirit and scope of the inventive thought and the patent claims.

Other objects and many of the attendant advantages of this invention will be readily appreciated as the same becomes better understood by reference to the following detailed description when considered in connection with the accompanying drawings wherein:

Fig. 1 shows a device for controlling a robot manipulator with an endeffector according to a first embodiment of the invention,

Fig. 2 shows a robot manipulator with an endeffector and with a device for

controlling the robot manipulator according to the first embodiment of the invention, and Fig. 3 shows a method of controlling a robot manipulator with an endeffector according to another embodiment of the invention. Detailed description of the drawings:

Fig. 1 shows a device 10 for controlling a robot manipulator 20 with an endeffector 21 , comprising:

- a first unit 1 being configured to provide a measured or estimated external wrench F_ext acting on the manipulator and/or on the endeffector 21 ,

- a controller 3 being connected to the first unit (1 ),

wherein the controller 3 comprises a force controller 5 and an impedance controller 7, wherein the force controller 5 is configured to generate a preliminary force control signal F_f based upon a predefined desired force signal F_d and upon F_ext, and configured to modify F_f by applying an attenuating function a_f derived from a first virtual energy tank T_f to obtain a final force control signal F'_f, wherein 7) is based upon a measured or estimated velocity of the robot manipulator and/or of the endeffector and upon F_f , and is configured to transform F'_f to a force control signal u'f with a Jacobian map ], wherein the impedance controller (7) is configured to modify a preliminary desired velocity x_d signal by applying an attenuating function a_t derived from a second virtual energy tank T* to obtain a final desired velocity signal x'_d, wherein T* is based upon x_d, and upon F'_f and F_ext, and wherein the impedance controller (7) is configured to generate an impedance control signal u based upon x'_d and upon x, and

- a summation unit (9) configured to generate an actuator command u' by adding u'_f and u'i. wherein for the transposed vector of F_ext, namely F _xt, holds: F _xt = [ f^T, m^T ], wherein f^T is the transposed vector of a column vector of forces and n^T is the transposed vector of a column vector of moments. The force controller 5 comprises a force switching logic unit, wherein the force switching logic unit is configured to modify Ff to obtain F'f by applying the attenuating function a_f to F_f , only if a condition y_f is not fulfilled. Here,

is fulfilled if x^TFf < 0, wherein x^T is the transposed vector of the measured or estimated velocity x. Moreover, T_f comprises a tank overloading term

such that a time derivative of Tf is zero, namely tf = 0, if /3 = 0 and

is fulfilled, wherein for bf holds: bf = 1 if Tf £ T^UJ and b_ί = 0 otherwise, wherein T^UJ is a predefined upper limit storage. The impedance controller (7) comprises an impedance switching logic unit, wherein the impedance switching logic unit is configured to modify x_d signal to obtain x'_d by applying the attenuating function a_t to x_d, only if a condition g is not fulfilled, wherein g is fulfilled if x_d {F'f + F_ext) > 0, wherein x_d is the transposed vector of the column vector containing the preliminary desired velocity x_d signal. Fig. 2 shows a robot manipulator 20 with an endeffector 21 and with a device 10 according to Fig. 1 .

Fig. 3 shows a method of controlling a robot manipulator with an endeffector, comprising the steps of:

- Providing S1 a measured or estimated external wrench F_ext acting on the manipulator and/or on the endeffector by a first unit (1 ),

- Generating S2 a preliminary force control signal F_f based upon a predefined desired force signal F_f and upon F_ext by a force controller 5,

- Modifying S3 F_f by applying an attenuating function a_f derived from a first virtual energy tank T_f to obtain a final force control signal F'_f by the force controller 5, wherein 7) is based upon a measured or estimated velocity x of the robot manipulator and/or of the endeffector and upon F_f ,

- Transforming S4 F'_f to a force control signal u'_f with a Jacobian map ] by the force controller 5,

- Modifying S4 a preliminary desired velocity x_d signal by applying an attenuating function cT_j derived from a second virtual energy tank T* by an impedance controller 7 to obtain a final desired velocity signal x'_d, wherein T* is based upon x_d and upon F'_f and F_ext, and

- Generating S5 an impedance control signal u based upon x'_d and upon x by the impedance controller 7, and

- Generating S6 an actuator command u' by adding u'_f and u by a summation unit 9.

The invention is explained above with reference to the aforementioned embodiments. However, it is clear that the invention is not only restricted to these embodiments, but comprises all possible embodiments within the spirit and scope of the inventive thought and the patent claims.

Reference sign list

1 first unit

3 controller

5 force controller

7 impedance controller 9 summation unit

10 device

20 robot manipulator

21 endeffector S1 Providing

52 Generating

53 Modifying

54 Modifying

55 Generating

S6 Generating

Claims

Claims

1. Device (10) for controlling a robot manipulator (20) with an endeffector (21 ),

comprising:

- a first unit (1 ) being configured to provide a measured or estimated external wrench F_ext acting on the manipulator and/or on the endeffector,

- a controller (3) being connected to the first unit (1 ),

wherein the controller (3) comprises a force controller (5) and an impedance controller (7),

wherein the force controller (5) is configured to generate a preliminary force control signal F_f based upon a predefined desired force signal F_d and upon F_ext, and configured to modify F_f by applying an attenuating function a_f derived from a first virtual energy tank T_f to obtain a final force control signal F'_f , wherein 7) is based upon a measured or estimated velocity x of the robot manipulator and/or of the endeffector and upon F_f , and is configured to transform F'_f to a force control signal u'f with a Jacobian map ],

wherein the impedance controller (7) is configured to modify a preliminary desired velocity x_d signal by applying an attenuating function a_t derived from a second virtual energy tank T* to obtain a final desired velocity signal x'_d, wherein T* is based upon x_d, and upon F'_f and F_ext, and

wherein the impedance controller (7) is configured to generate an impedance control signal u based upon x'_d and upon x, and

- a summation unit (9) configured to generate an actuator command u' by adding u'_f and u'i.

2. Device (10) according to claim 1 ,

wherein for the transposed vector of F_ext, namely F _xt, holds: F _xt = [ f^T, m^T ] , wherein f^T is the transposed vector of a column vector of forces and m^T is the transposed vector of a column vector of moments.

3. Device (10) according to one of the preceding claims,

wherein the first unit is an observer configured to provide the estimated external wrench F_ext acting on the manipulator and/or on the endeffector.

4. Device (10) according to one of the preceding claims, wherein the force controller (5) comprises a force switching logic unit, wherein the force switching logic unit is configured to modify F_f to obtain F'_f by applying the attenuating function a_f to F_f , only if a condition y_f is not fulfilled.

Device (10) according to one of the preceding claims,

wherein y is fulfilled if x^TF_f < 0, wherein x^T is the transposed vector of the measured or estimated velocity x.

Device (10) according to one of the preceding claims,

wherein T_f comprises a tank overloading term /3 , such that a time derivative of T_f is zero, namely

= 0, if /3 = 0 and is fulfilled, wherein for

holds: /^ = 1 if 7} < T^UJ and b_{ = 0 otherwise, wherein T^UJ is a predefined upper limit storage.

Device (10) according to one of the preceding claims,

wherein T_f is proportional to xh and wherein a time derivative of x_tf , namely x_tf , is proportional to x^TF_f , wherein x^T is the transposed vector of the measured or estimated velocity x.

Device (10) according to one of the preceding claims,

wherein the impedance controller (7) comprises an impedance switching logic unit, wherein the impedance switching logic unit is configured to modify x_d signal to obtain x'_d by applying the attenuating function a_t to x_d, only if a condition y* is not fulfilled.

9. Device (10) according to one of the preceding claims,

wherein g is fulfilled if x_d {F'f + F_ext) > 0, wherein x_d is the transposed vector of the column vector containing the preliminary desired velocity x_d signal.

10. Device (10) according to one of the preceding claims,

wherein T_j is proportional to cb and wherein a time derivative of x_ti, namely x_ti is proportional to x_d (F'_f + F_ext), wherein x_d is the transposed vector of the column vector containing the preliminary desired velocity x_d signal.

1 1. Device (10) according to one of the preceding claims,

wherein the force controller (5) is configured to modify F_f by applying the attenuating function a_f and based upon a function p to obtain the final force control signal F'_f , wherein p is a function-valued vector comprising translational and rotational components with controller shaping functions.

12. Device (10) according to one of the preceding claims,

wherein the first virtual energy tank 7} is initialized with an initial task energy E_Tf and/or the second virtual energy tank 7) is initialized with an initial task energy E_{T i2}.

13. Robot manipulator (20) with a device (10) according to one of the preceding claims.

14. Method of controlling a robot manipulator (20) with an endeffector (21 ), comprising the steps of:

- Providing (S1 ) a measured or estimated external wrench F_ext acting on the manipulator and/or on the endeffector by a first unit (1 ),

- Generating (S2) a preliminary force control signal F_f based upon a predefined desired force signal F_d and upon F_ext by a force controller (5),

- Modifying (S3) F_f by applying an attenuating function a_f derived from a first virtual energy tank 7) to obtain a final force control signal F'_f , wherein 7) is based upon a measured or estimated velocity x of the robot manipulator and/or of the endeffector and upon F_f ,

- Transforming (S4) F'_f to a force control signal u'_f with a Jacobian map ],

- Modifying (S4) a preliminary desired velocity x_d signal by applying an attenuating function derived from a second virtual energy tank Ti to obtain a final desired velocity signal x'_d, wherein 7) is based upon x_d and upon F'_f and F_ext, and

- Generating (S5) an impedance control signal '_j based upon x'_d and upon x by an impedance controller (7), and

- Generating (S6) an actuator command u' by adding u'_f and u'_j by a summation unit (9).