sign in sign up
<<
Home , Position (geometry), Trajectory

2020 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) October 25-29, 2020, Las Vegas, NV, USA (Virtual)

Model Predictive and Trajectory Tracking Control for Robot-Environment Interaction

Tobias Gold, Andreas Volz,¨ and Knut Graichen

Abstract— The development of modern sensitive lightweight compliance control with model predictive path-following robots allows the use of robot arms in numerous new scenarios. control by considering an additional admittance dynamic. Especially in applications where interaction between the robot A similar approach is followed by the of [7], which and an object is desired, e.g. in assembly, conventional purely position-controlled robots fail. Former research has focused, combines model predictive path-following control with force among others, on control methods that center on robot- control. The author of [8] transforms occurring contact environment interaction. However, these methods often consider into an evasive for a model predictive pose trajectory only separate scenarios, as for example a pure force control control. Similar to admittance control, this leads to a new scenario. The present paper aims to address this drawback and trajectory, which differs from the original desired trajectory, proposes a control framework for robot-environment interac- tion that allows a wide range of possible interaction types. At but minimizes contact forces. In [9], the interaction force the same , the approach can be used for setpoint generation is introduced as a separate state of the prediction model to of position-controlled robot arms, where no interaction takes realize a parallel position/force control. The previous work place. Thus, switching between different controller types for of the authors [10] catches up the idea of formulating the specific interaction kinds is not necessary. This versatility is force as a state variable and presents a universal framework achieved by a model predictive control-based framework which allows trajectory following control of joint or end-effector based on MPC for interactions with elastic environments. position as well as of forces for compliant or rigid robot- For the prediction of the interaction forces in the internal environment interactions. For this purpose, the robot optimization problem, a spring model which considers the is predicted by an approximated dynamic model and the force environmental stiffness is often assumed. To this end, the behavior by an interaction model. The characteristics of the stiffness of the environment must be estimated either before approach are discussed on the of two scenarios on a lightweight robot. an interaction in an offline procedure or during the interaction with an online method. Three different methods for stiffness I.INTRODUCTION estimation are discussed, for example, in [11] in the context Industrial robots have been automating processes in in- of classical impedance and force control. dustry for many years. However, the robots mainly act This paper presents an approach for model predictive in a position-controlled manner, so that a physical robot- control of robot-environment interaction, which is able to environment interaction is not the primary focus. This is now handle interaction with both elastic and rigid environmental being changed by the use of modern sensitive lightweight behaviors. If only the environmental stiffness is considered robots. Already the construction of these robots is designed in the prediction model, a limitation of compliant robot- for a possible interaction, e.g. with humans. The lightweight environment interaction is required. This is because in a construction of the robots reduces, for example, the rigid interaction, the stiffness of the environment becomes transferred during collisions. In addition, special control infinite, which leads to numerical instabilities. The presented techniques have been developed to provide compliance in approach addresses this by explicitly considering the stiffness case of interaction [1]. Furthermore, sensitive manipulation of the underlying position controller. is possible by means of force control methods [2]. These The MPC acts as a setpoint generator for the robot internal methods have already been known since the 1980s, see position control. The dynamics of this underlying control e.g. early works on hybrid position/force control [3] and is therefore also considered in the joint in order to impedance control [4]. apply the approach for position-controlled robots. Due to the A more recent field for dealing with this requirement is position control, the robot behavior can be approximated as the use of model predictive control (MPC). This method a linear spring-damper system. Furthermore, an interaction is supposed to explicitly comply with force constraints or dynamic is taken into account in the MPC framework. to enforce a defined behavior in case of contact loss. In Since the behavior of the environment is described by a [5], an approach for model predictive admittance control is spring model, the interaction between robot and environment presented, which allows an explicit consideration of force can be regarded mathematically as a parallel connection of constraints. In this context, the use of MPC for interaction position controller and environmental stiffness [12]. This control is discussed in general. The work of [6] combines has the consequence that the lower stiffness dominates the interaction model. Thus, an ideal rigid contact results in an The authors are with the Chair of Automatic Control, Friedrich- interaction stiffness equal to the controller stiffness of the Alexander-Universitat¨ Erlangen-Nurnberg,¨ 91058 Erlangen, Germany {tobias.gold, andreas.voelz, internal position controller. The advantage of this approach knut.graichen}@fau.de is a universal usability for lightweight robots. At the same

978-1-7281-6211-9/20/$31.00 ©2020 IEEE 7397 time, the MPC can be used to achieve a desired impedance effects such as , Coriolis and centripetal effects can be behavior, even though this may be a compliant or rigid neglected (M(q)q¨ C(q, q˙)q˙ 0). Such an approxima- behavior. Furthermore, the approach can be extended to a tion may also be applied≈ to the inverse≈ dynamics approach position and force trajectory tracking control. This leads to (4), where q¨ 0 is assumed. This simplifies the dynamics a powerful framework for the control of robot-environment of the controlled≈ robot in both cases to a first-order system interactions, since in addition kinematic and force constraints 1 1 T q˙ = D− KP(q q) D− J(q) ext . (7) can be explicitly considered by the MPC. The properties d − − F of the proposed methods are discussed on the basis of two The model describes the controlled robot system during free control scenarios with a 7-DOF lightweight robot. motion ( = 0) as well as under the influence of external F ext forces ( = 0) during an interaction. On this basis, the II.MODELING ext contactF situation6 is modelled subsequently. In this section the model of robot-environment interaction is discussed. First, the controlled robot system is derived and B. Robot-Environment Interaction then the interaction is addressed. In principle, two basic types of contact can be distin- guished in an interaction situation between robot and en- A. Robot System vironment. On the one hand, the interaction with a rigid The dynamic model of a robot arm with n joints body causes a constraint on the permitted robot motion. The case involves dynamic interaction with a body. M(q)q¨+ C(q, q˙)q˙ + g(q) = τ J τ ext, (1) − Fundamental mechanical effects such as inertia, dissipation, describes the relationship between the resulting motion of the or elasticity thereby describe the interaction dynamics. The n q R and their time derivatives presented approach should be able to handle both cases of ∈ n due to the applied generalized forces τ J R . The external interaction. For this purpose, the interaction model in the n ∈ τ ext R are the result of an external force on ∈ case of a purely linear elastic interaction is derived first. It is the robot structure. In the case of an external force at the assumed that no dissipative and inertial effects occur in the T end-effector, the external torques τ ext = J(q) ext are direction of the interaction motion. In addition, a purely rigid T F T T obtained from the (6 1) wrench vector ext = [F , m ] , × F ext ext robot system is supposed by localizing all intrinsic passive composed by the forces and torques at the end-effector, using robot compliance in the environment. Thus the interaction the transposed end-effector Jacobian J(q). can be described according to a mechanical spring [12], [14] An underlying position control realizes the stabilization o o env = Kedp , (8) of a desired position qd for which the corresponding joint F o,e torques τ J = u are calculated. For this purpose, the non- where the penetration dp = p p results from the linear system (1) is first transferred to a linear decoupled o,e − o of the end-effector frame Σe with respect to system using the approach of exact linearization the task frame Σo located on the object surface. Note that o u = M(q)y + n(q, q˙) (2) the force env is referred to the task frame Σo. In addition, the pose vectorF via state feedback [13]. The nonlinear term n(q, q˙) = T T T b C(q, q˙)q˙ + g(q) and the M(q) are feedfor- p(q) = [t(q) , φ(q) ] = T e(q) (9) warded to compensate for them exactly. The resulting new composes the end-effector position t and the orientation, input y can be chosen as a linear and decoupled PD control denoted by the Euler φ, which is calculated by the coordinate transformation of the joint position to the y = q¨ = KP(qd q) Dq˙ (3) − − Cartesian base frame Σb. For the sake of clarity, no super- to shape the dynamics of the controlled system according to script indices are added for quantities defined in the base T and the location and of the frames q¨ = KP(q q) Dq˙ J(q) ext . (4) d − − − F in the base frame is determined by the respective pose. The The (n n) gain matrices are selected as diagonal matrices, elastic wrench depends, as pictured in Figure 1, on the × F env e.g. KP = diag([kP,1, . . . , kP,n]), with the gain factors of penetration of the robot end-effector into the object. Thereby, the individual joint variables. Instead of (2), the PD control the resistance to penetration is defined by the (6 6) sym- o × with gravitational compensation metric positive (semi)-definite stiffness matrix Ke. Under the neglection of the coupling stiffness between translational u = τ J = KP(qd q) Dq˙ + g(q) (5) o − − and rotational stiffness, the environmental stiffness Ke = o o is a popular control method in robotics due to its simplicity, diag( Ktrans, Ktors) is composed by the (3 3) translational o ×o but also due to the fact that it provides asymptotic stability and rotational stiffness matrices Ktrans and Krot [14]. Due [13]. The closed loop of the PD controlled robot to the location of the task frame Σo on the object surface, o T the stiffness matrix Ke can be selected according to the M(q)q¨+ C(q, q˙)q˙ = KP(q q) Dq˙ J(q) ext (6) d − − − F restricted directions. In the example of Figure 2, only the is obtained by substituting (5) in (1). During interaction, normal of the surface is motion constrained. Therefore the typically only low are realized. Therefore, dynamic stiffness matrix has only entries for the respective direction.

7398 described by the translational and the angular velocities of dpo,e the end-effector v(q, q˙) and ω(q, q˙), is determined by the env ctr F product of the Jacobian J(q) and the joint velocity q˙. F Σ Σe o However, the equations (8), (12), and (14) only hold in the B case of a contact. Therefore, it is necessary to distinguish dpe,d between free motion and contact. One possibility is the dpo,d Σd evaluation of the end-effector forces by a threshold value Σb ( o o o = 0 if F ext,i < i or mext,i < i Ke,i(t) (16) Fig. 1. PD-controlled robot in inter- Fig. 2. Frames in robot-environment > 0 else . with an environment. interaction. If this threshold i is exceeded in a certain Cartesian direction In contrast, the PD-controlled robot reacts according to the i, the end-effector is in contact in this direction. Another approximated spring-damper system (7). The stiffness of this possibility is to create a model with and mechanical system is defined by the joint space controller stiffness KP. features of the environment using, for example, camera The force of the controller acting in the static case data or information from previous contact situations. This information is generally used to model the object surface and = K dp (10) o o F ctr P, cart e,d the stiffness Ke. In this case, Ke is selected in dependence results from the weighted error of desired and current end- on the environment model effector position. Note that the (6 6) Cartesian controller ( o × o > 0 if pi stiffness matrix results from the locally valid transformation Ke,i(t) ∈ B (17) = 0 else , T # # KP, cart = (J(q) ) KP(J(q)) (11) whereby it is analyzed whether the end-effector has pene- of the joint space stiffness matrix to the Cartesian space with trated the object . From this on, the respective object the matrix pseudo inverse ( )# [15] [16]. stiffness is assignedB to the prediction model. Due to the · If the equality dp = dp dp and ctr = env model predictive character of the presented approach, this o,e o,d − e,d F F [17] according to Figure 1 and 2 is considered, the resulting can be used to react prior to contact. Thus, the motion can external wrench on the end-effector is the equivalent spring already be slowed down when predicting the contact in order o o 1 o to reduce occurring force peaks. = ( K + K )− K K dp F ext e P, cart e P, cart o,d However, there remains the question of how to handle a o ¯ = Kdpo,d . (12) rigid contact with the approach. It can be seen from (12) that o The overall stiffness K¯ thus results mathematically from due to the parallel connection of the stiffnesses, the total ¯ the parallel connection of the individual stiffnesses [12]. In stiffness K is always smaller than the smallest stiffness. o o ¯ order to express the force in the base frame, the wrench For the case of Ke,i , the overall stiffness Ki = o → ∞ o KP,cart thus results in the Cartesian controller stiffness. ext in the object frame has to be rotated using the rotation ,i F b T Thus, the interaction stiffness is of a convenient order of matrix Ro = Rb Ro. This results in the elastic wrench in the base frame magnitude. The simultaneous consideration of controller and b T  environmental stiffness yields a numerically more stable Ro 0 o ¯ ¯ formulation of the interaction model. ext = b T Kdpo,d = Kdpo,d . (13) F 0 Ro The aim of the presented approach is to handle the force as a III.MODEL PREDICTIVE CONTROL FORMULATION state variable in the MPC. To this end, the dynamic behavior Model predictive control is based on the iterative (subop- of the force has to be considered for the assumption of a timal) solution of a dynamic optimization problem constant task frame on the object surface. Thus, the time Z T derivative of the force is determined according to min J(u; xk) = V (x(T )) + l(x(τ), u(τ)) dτ (18a) u ˙ ¯ 0 ext = KJ(q)q˙d . (14) F s.t. x˙ (τ) = f(x(τ), u(τ)) , x(0) = xk (18b) The authors are aware that (14) only holds locally. This is x(τ) , u(τ) (18c) due to the position dependency of KP, cart and K¯ from (11). ∈ X ∈ U However, this can be neglected in the MPC if the over a moving horizon τ [0,T ] with length T . The ∈ prediction horizon is sufficiently small and the velocity in characteristic of MPC is that the optimization problem is the direction of interaction is assumed to be low. Basically, initialized with the current measured or estimated value xk the model (14) shows how the forces increase or decrease in of the state variables in each sampling step and the initial the individual Cartesian directions during a particular desired solution of the control variable is taken from the optimization of the last time step. Subsequently, the first part of the motion p˙d = J(q)q˙d. Note, that the Cartesian pose motion optimal solution is applied to the controlled system up to T T T p˙(q, q˙) = [v(q, q˙) , ω(q, q˙) ] = J(q)q˙ , (15) the next time step.

7399 Therefore, the cost function (18a) may be based for example q (t) des solely on the integral cost term ˙ pdes(t) ext F ¯ = KJ (q)q˙d l(x, u) = lpos(q) + lpose(p) + lforce( ext) + lctr(u) . (20) des(t) l(x, u) F F The problem of trajectory tracking can be handled by penal- q izing the deviation of a quantity x˜(τ) = x(τ) xdes(τ) , τ d − ∈ Grampc [0,T ] to the desired trajectory xdes(t) by using the weighted 2 1 T norm x˜ A = 2 x˜ Ax˜ with the positive (semi)-definite q, ext weightingk k matrix A. Note that the choice of the individual x˙ = f(x, u) F penalty terms depends on the respective application and can x , u ∈ X ∈ U 1 also be set to zero. Some example applications are presented q˙ = D− KP(qd q) − 1 T D− J(q) ext in Section IV and in the previous work of the authors [10]. − F C. System Constraints Fig. 3. System layout of the model predictive interaction control for generalized robot-environment interaction. In addition, the MPC scheme allows to consider con- straints (18c) systematically. In the case of interaction con- trol, system-related constraints, such as limited joint position The entire system of the model predictive interaction + control is schematically shown in Figure 3. The MPC, which q [q−, q ] , (21a) ∈ is solved with the GRAMPC toolbox [18], considers the as well as task-related constraints, such as a limited dynamics of the controlled robot and the interaction, marked workspace or limited interaction forces with dashed boxes. The degree of freedom in the choice + of cost functions and reference values is problem-specific p [p−, p ] (22a) ∈ and varies according to the application. In the following, the + ext [ − , ] (22b) individual parts of the problem (18) are introduced. F ∈ F ext F ext can be taken into account. The potential constraints are not A. System Dynamics limited to box constraints of the state variables as shown. The robot-environment interaction can be described by the T IV. EXPERIMENTAL VALIDATION set of states x = q q p . The dynamics of d ext The presented approach is validated in two scenarios the robots joint position in freeF motion as well as during with a 7-DOF Franka Emika Panda robot. The external interaction is directly derived from (7). The desired joint wrench at the end-effector is estimated using the build-in position q actuates the robot system dynamics (7). As d joint torques measurements with a generalized pictured in Figure 3, this is forwarded to the robot system observer [19]. Alternatively, the force can also be measured as control variable. However, to avoid steps in the control using a force/ sensor on the end-effector. However, trajectory, the first derivative of the desired position u = q˙ d the approach is not limited to measured states. Estimated is internally handled as MPC control variable. Since the quantities can also be used, e.g. following the approach of external force is considered as a state variable, its dynamics [20] or [21]. must also be considered in the optimization problem. This is In the first scenario, a pure force trajectory is applied to done via equation (14) and the conditions (16) or (17). For materials with different stiffness up to a rigid contact. The reasons of computational efficiency, the end-effector pose is second scenario pairs the force control for a rigid contact also chosen as a state, whereby its dynamics can be described with a motion control in the complementary directions. For using (15). This leads to the overall nonlinear dynamics (18b) this purpose, the robot arm is drawing a on a with during a contact situation according to chalk. The orientation of the object surface is set horizontally    1 1 T  q˙ D− KP(q q) D− J(q) ext so that the orientation of the object frame and base frame is d − − F  q˙d   u  identical. Thus, no rotation of the wrench (12) is necessary.   =   . (19)  ˙ ext  KJ¯ (q)u  The sampling time of the MPC is set to 10 ms and F p˙ J(q)q˙ the underlying PD control with gravitational compensation according to (5) is running with a sampling time of 1 ms. The initial state value x(0) = xk is thereby initialized by the measurements of the respective state in step k. A. Comparing Different Stiffness First, the approach is demonstrated for the interaction with B. Cost Function Design different object stiffness. For this purpose, an interaction The control target of MPC is defined in the cost function setup as schematically shown in Figure 3 is assumed. The (18a). With the presented method, an offline calculated robot shall apply a defined force of 2 N in z-direction to − trajectory xdes(t) of the state variable has to be stabilized op- different objects. On the one hand, a foam with a stiffness o 1 timally. By penalizing the deviation from a desired trajectory, of ke,z = 500 N m− and, on the other hand, the rigid table the optimization problem tries to minimize these deviations. surface is chosen for the interaction. The object stiffness

7400 0 soft contact 4 [N] desired ,z 2 0 measured ext − [N] F 4 4 ,z − − 0 rigid contact ext F [N] 8

,z − 2 ext − 12 F 4 − 0 2 4 6 8 − o 0 rigid contact with wrong Ke [N] time [s]

,z 2 ext −

F Fig. 5. Oscillating force trajectory applied on a compliant surface. 4 − 0 0.5 1 1.5 2 2.5 3 time [s] In addition, the absolute force in z-direction is constrained to a maximum of 8 N. The Figure 5 shows a continuous interac- Fig. 4. Comparison of force trajectories in different interactions. tion with an oscillating force trajectory from the beginning. The constraint at 8 N is continuously satisfied during the subsequent trajectory− course. In case of a setpoint within the was calculated offline before interaction. However, an online permissible range, the trajectory is followed sufficiently well. stiffness estimation as presented in [10] is also possible. Figure 4 shows the force trajectory for three different B. Writing with Chalk cases. The first plot shows the trajectory for the elastic The second application aims to demonstrate the applica- interaction. Due to the low object stiffness, the overall stiff- bility of the methodology as hybrid force/motion control ¯ ness K is significantly reduced compared to the transformed for rigid contact. For this purpose, the robot has to draw controller stiffness KP, cart. This allows the robot to react fast a path on a blackboard with a piece of chalk. The degrees enough to the contact that occurs at approximately 0.5 s. The of freedom of the end-effector are divided for this purpose desired reference value is thus reached after an additional into position and force controlled subspaces. In the following time of 0.6 s without overshoot. example, the x-direction is supposed to be stabilized to a The second plot of Figure 4 shows the rigid interaction constant value of 5 N, while the y- and z-direction shall with the table surface. Due to the object stiffness tending follow a trajectory according to a Lissajous figure towards infinity, the total stiffness K¯ is equal to the trans- formed controller stiffness K . As with the elastic inter- t des(t) = 0.1 sin(0.15 2πt) (24a) P, cart x, · action, contact occurs shortly before 0.5 s and the setpoint is ty,des(t) = 0.1 sin(0.075 2πt) + 0.9 , (24b) reached after another 0.6 s. Theoretically, the object stiffness · has to be defined as infinitely high. In practice, especially whereby the orientation is kept to a constant value. The when using lightweight robots, the stiffness of the joints constraints (21a) are considered in the optimization problem. 4 1 with approximately 10 N rad− will become noticeable. By The path followed by the end-effector is shown in Figure 6 assuming a concentration of robot intrinsic stiffness in the for two Lissajous repetitions, whereas the corresponding environment, the joint stiffness is dominating the object trajectories are given in Figure 7. The motion starts with stiffness. This means that the joint stiffness can be used to an initial offset from the desired trajectory that is corrected specify the object stiffness in the rigid case. immediately. When considering the MPC in The last plot of Figure 4 shows the behavior of the control Table I, it is noticeable that especially the deviation to the o in case of a wrong assumption of the object stiffness Ke. desired trajectory is highly weighted in order to achieve a Again, the robot is interacting with the rigid table surface, but good trajectory tracking. In contrast, the deviation of the o 1 assumes a compliant object stiffness of ke,z = 500 N m− . force to the reference value is less penalized, which results Thus, a significant overshoot can be seen in the transient in a worse force tracking accuracy. It can be seen that the zone, since the controller expects a weaker increase of the maximum values in the periodic force trajectory correlate forces. In the Cartesian case, this would mean that the controller initially places the target position of the underlying TABLE I PD controller further into the object. MPC PARAMETERSFORTHEHYBRIDFORCE/MOTIONCONTROL. However, the previous tests only considered a constant setpoint. As the presented methodology also allows time- prediction horizon T 0.3 s variant setpoints, a force trajectory following control is sampling points Nhor 30 max. gradient iterations i 4 examined in the following for a soft contact interaction. The grad max. multiplier iterations imult 1 4 4 target trajectory is chosen to position Qp diag([0, 10 , 10 , 10, 10, 10]) interaction force weights QF diag([0.5, 0, 0, 0, 0, 0]) Fext, z, des(t) = 5N sin(πt + 0.1) 7N . (23) control weights R diag([0.1,..., 0.1]) −

7401 1 tx 1 t ty

[m] 0.5

0.9 t tz

[m] 0.8 0 z 0.7 10 8 0.6 [N] 6 time [s]

0.2 ,x 0.1 0.4 4 0 0.3 ext 2 0.1 0.1 0.2 F − 0.2 0 0 y [m] − x [m] 0 10 20 30 40 time [s] Fig. 6. Path of the end-effector while writing with chalk.

Fig. 7. End-effector and force trajectory while writing with chalk. with the writing direction. If the chalk is rather pushed, larger forces result. On the other hand, the forces decrease as soon [3] M. Raibert and J. Craig, “Hybrid position/force control of manipu- as the chalk is pulled. However, if Figure 7 is considered with lators,” Journal of Dynamic Systems, Measurement and Control, vol. the background of a moving interaction under the influence 102, no. 3, pp. 126–133, 1981. of friction, the force trajectory still represents a good com- [4] N. Hogan, “Impedance control: An approach to manipulation,” Journal of Dynamic Systems, Measurement and Control, vol. 107, no. 1, pp. promise. The experiments have revealed that with a higher 8–16, 1985. weighting of the force deviation, the force tracking becomes [5] A. Wahrburg and K. Listmann, “MPC-based admittance control for significantly better. However, the quality of the end-effector robotic manipulators,” in Proc. of CDC, 2016, pp. 7548–7554. [6] K. J. Kazim, J. Bethge, J. Matschke, and R. Findeisen, “Combined trajectory deteriorates as a result. In addition, a reduction predictive path following and admittance control,” in Proc. of ACC, of the end-effector trajectory velocity also improves force 2018, pp. 3153–3158. tracking noticeably. [7] J. Matschke, J. Bethge, P. Zometa, , and R. Findeisen, “Force feedback and pathfollowing using predictive control: Concept and application The controllers are computed for both scenarios on a to a lightweight robot,” in Proc. of IFAC World Congress, 2017, pp. Ubuntu 18.04 OS with Intel(R) Core(TM) i5-8250U CPU. 9827–9832. [8] A. Zube, J. Hofmann, and C. Frese, “Model predictive contact control With the MPC parameters from Table I, an average com- for human-robot interaction,” in Proc. of ISR, 2016, pp. 279–285. puting time of 3.0 ms and a worst case computing time of [9] J. de la Casa Cardenas, A. Garca, S. Martnez, J. Garca, and J. Or- 4 ms is achieved. Thus, the computing time is well within tega, “Model predictive position/force control of an anthropomorphic robotic arm,” in Proc. of ICIT, 2015, pp. 326–331. the 10 ms MPC sampling time. [10] T. Gold, A. Volz,¨ and K. Graichen, “Model predictive interaction control for industrial robots,” in IFAC World Congress, 2020, accepted. V. CONCLUSIONS [11] D. Erickson, M. Weber, and I. Sharf, “Contact stiffness and damping estimation for robotic systems,” The International Journal of Robotics This contribution presents an MPC-based approach to con- Research, vol. 22, no. 1, pp. 41–57, 2003. trol the robot-environment interaction. The handling of the [12] S. Jung, T. C. Hsia, and R. G. Bonitz, “Force tracking impedance control for robot manipulators with an unknown environment: The- problem using MPC results in a flexible and comprehensive ory, simulation, and experiment,” International Journal of Robotics framework for various application scenarios where interac- Research, vol. 20, no. 9, pp. 765–774, 2001. tion with an environment is desired including interactions [13] M. Spong, S. Hutchinson, and M. Vidyasagar, Robot Modeling and Control. New York: Wiley, 2005. with a rigid environment. The results of the experimental [14] B. Siciliano and O. Khatib (Eds.), Springer Handbook of Robotics. validation show that even small forces can be realized with Berlin, Heidelberg: Springer, 2016. a good control quality during an end-effector motion. This [15] J. K. Salisbury, “Active stiffness control of a manipulator in Cartesian coordinates,” in Proc. of CDC, 1980, pp. 95–100. allows to use the approach for sequential manipulation tasks [16] S.-F. Chen and I. Kao, “Simulation of conservative congruence trans- such as the classical peg-in-hole problem to avoid switching formation. Conservative properties in the joint and cartesian ,” between different controller types. Further work will focus in Proc. of ICRA, 2000, pp. 1283–1288. [17] B. Siciliano, L. Sciavicco, L. Villani, et al., Robotics - Modelling, on the topic of environment modeling based on camera and Planning and Control. London: Springer, 2009. tactile data. This improves the approach by predicting the [18] T. Englert, A. Volz,¨ F. Mesmer, S. Rhein, and K. Graichen, “A contact before interaction. Further downloadable material software framework for embedded nonlinear model predictive control using a gradient-based augmented Lagrangian approach (GRAMPC),” for this article is available at http://ieeexplore.ieee.org. The Optimization and Engineering, vol. 20, no. 3, pp. 769–809, 2019. material includes a video that illustrates the writing with [19] A. De Luca and R. Mattone, “Actuator fault detection and isolation chalk scenario. using generalized momenta,” in Proc. of ICRA, 2003, pp. 634–639. [20] T. Gold, A. Volz,¨ and K. Graichen, “External torque estimation for an industrial robot arm using joint torsion and motor current REFERENCES measurements,” in Proc. of Joint Conference on MECHATRONICS and NOLCOS, 2019, pp. 879–884. [1] A. Albu-Schaffer and G. Hirzinger, “Cartesian impedance control [21] A. Wahrburg, E. Morara, B. Cesari, G.and Matthias, and H. Ding, techniques for torque controlled light- robots,” in Proc. of ICRA, “Cartesian contact force estimation for robotic manipulators using 2002, pp. 657–663. kalman filters and the generalized momentum,” in Proc. of CASE, [2] G. Zeng and A. Hemami, “An overview of robot force control,” 2015, pp. 1230–1235. Robotica, vol. 15, no. 5, pp. 473–482, 1997.

7402