Unified force-impedance control

Abstract

Unified force-impedance control (UFIC) aims at integrating the advantages of impedance control and force control. Compliance and exact force regulation are equally important abilities in modern robot manipulation. The developed passivity-based framework builds on the energy tank concept and is suitable for serial rigid and flexible-joint robots. Furthermore, it is able to deal either with direct force measurements or model-based contact force estimation. Thus, in this theoretical framework, the most relevant practical systems are covered and shown to be stable for arbitrary passive environments. Particular focus is also laid on a robust impedance-based contact/non-contact stabilization methodology that prevents abrupt, unwanted, and potentially dangerous movements of the manipulator in case of contact loss, a well-known problem of both impedance and force control. The validity of the approach is shown in simulation and through various experiments. Our work roots in Haddadin (2015); Schindlbeck and Haddadin (2015), where the basic UFIC regulation controller was proposed. In the present paper, we significantly advance this idea into a complete theoretical UFIC tracking framework, including rigorous stability analysis and extensive experimental evidence.

1. Introduction

The ability to physically interact with the environment is fundamental in today’s robot design. Position-controlled robots were not capable of doing so because they could not provide the necessary compliance for physical contacts, let alone the ability to track a desired wrench profile. As a result, over the past decades, continuous progress has been made in both hardware and software design to enable reliable force and motion control suitable for contact/non-contact scenarios. One of the most significant advancements came with the introduction of impedance control in Hogan (1985), enabling torque-controlled robots not only to follow desired trajectories in free space but also to establish and shape desired interaction behaviors when in physical contact. Additionally, if the task is to regulate the interaction wrench, one approach is to shape the impedance or the desired trajectory based on the sensed wrench, known as Indirect Force Control (Salisbury, 1980; Whitney, 1977; Lutscher et al., 2017). Yet, to avoid energy over-accumulation in the motion controller’s compliance, the Direct Force Control approach, where the robot’s input torque is directly linked to the wrench error, can be used (Eppinger and Seering, 1987; Zeng and Hemami, 1997; Villani and Schutter, 2016). The similarity and distinction between these two approaches are highlighted in Volpe and Khosla (1995) and Won et al. (1997), respectively.

As the diversity of tasks that a robot should perform autonomously increases, having a single interaction controller capable of controlling both motion and force can eliminate the need for constant controller switching and the associated instability issues (Whitney, 1987). Moreover, there are numerous manipulation tasks where force and motion must be controlled simultaneously, see Figure 1. Therefore, the development of simultaneous force-motion controllers has drawn considerable attention for a long time.

Figure 1.

Controlling the contact wrench at high performance over complex, discontinuous, and possibly compliant surfaces requires exact and robust force and compliant motion control.

One of the most well-known approaches is the Hybrid Force-Motion control, initially introduced in Raibert and Craig (1981), where the output of a direct force control is superimposed onto the motion controller’s output. The approach was further improved in subsequent works. In Khatib and Burdick (1986) and Khatib (1987), the dynamics effects of the manipulator and the force sensor were incorporated into the motion control loop. The singularity-related instabilities of the motion control were tackled by incorporating the robot dynamics in Yoshikawa (1987), adjusting motion controller gains in An and Hollerbach (1987), and implementing nullspace controllers in Fisher and Mujtaba (1992). In Anderson and Spong (1988), an impedance controller was employed in the motion control loop. This was the case also for more recent works (Bodie et al., 2020; Brunner et al., 2022) for fully actuated aerial vehicles. Moreover, in Su et al. (1992), the robot’s desired motion was defined in the joint space. Finally, another common approach is based on the use of indirect force control, known as the External Force Feedback Loop approach (Hirzinger, 1983; De Schutter and Van Brussel, 1988; Almeida et al., 1999; Lopes and Almeida, 2008). In this method, the force is regulated in an outer loop, and its output is added to the input of the inner loop motion controller.

Irrespective of the specific controller used, a fundamental restriction exists in simultaneously regulating force and motion: it is generally not possible to control both in the same direction (Hogan, 1985). This limitation led to the incorporation of the Selection matrices ¹ right from the earliest attempts in simultaneous force-motion control designs. The selection matrix ensures that the force and motion control tasks are performed in orthogonal (reciprocal) spaces. Typically, force control is applied in the direction of the environment constraints, while motion is controlled in unconstrained directions (Mason, 1981).

Using a selection matrix introduces some inherent issues. Firstly, as elaborated in Duffy (1990), concerns arise during investigations of orthogonality, such as the choice of units and reference frames. Moreover, the selection matrix must be compatible with the constraints imposed by the environment, necessitating a precise model of the environment for its derivation. However, as stated in Paul (1987), one of the biggest challenges in force-motion control is the lack of a perfect environment constraint model. That is why another class of controllers, known as Parallel force-motion controllers, was introduced. These controllers relax the strict requirement for orthogonality by imposing higher priority on either the force or motion control task. Examples include Chiaverini et al. (1992); Chiaverini and Sciavicco (1993); Chiaverini et al. (1998); Siciliano (1996); De Schutter et al. (1998), where the force controller takes higher priority by incorporating an integrator part in the feedback loop. Such a policy is not always practical. Specifically, if the force controller takes over in an unconstrained direction (e.g., during a contact loss), the robot can accelerate, resulting in a hazardous situation. Similarly, if the motion controller takes higher priority in a constrained direction, significant penetration of the setpoint into the surface can lead to extreme contact wrenches. Hence, to avoid such unstable scenarios, it is crucial to incorporate a good enough environment model even in the parallel control policies.

There have been adaptive force-impedance control approaches aimed at addressing uncertainties in the environment model. One of the pioneering examples is West and Asada (1985), where the selection matrix continuously changes according to the newly imposed constraints. In Namvar and Aghili (2005), an adaptive hybrid controller is proposed, and the environment geometry is continuously estimated under certain assumptions, such as the maximum slope on the surface. In Doulgeri and Karayiannidis (2007); Karayiannidis and Doulgeri (2009), a soft hemispherical tip is assumed for the robot end-effector, and an adaptive term is introduced in the control input to handle uncertainties in the estimated surface normal direction. Other recent work on handling uncertainties in the environment model when using impedance control include Lin et al. (2021); Hathaway et al. (2023); Dyck et al. (2022).

Estimating the environmental constraints solely based on force and motion information is not always straightforward. A typical example involves confusing the surface friction force with a motion constraint. In such cases, not only should we refrain from stopping the motion control in the presence of friction, but we might also want to add a feed-forward force to the motion control output to compensate for the friction (Bona and Indri, 2005). A comprehensive study on the relationship between surface sliding motion and reactive frictional force is presented in Howe and Cutkosky (1996). Another challenge in such model-based approaches is that the environment model might be either complicated or dynamically changing. Therefore, estimating the environmental constraints might not be practical. As a result, rather than heavily relying on the estimated environment model to avoid the aforementioned instabilities, the force-motion control policy should be designed to be robust to possible model dependencies. This robustness ensures that the control approach remains effective even in the presence of uncertainties or variations in the environment.

1.1. Problem statement

In this paper, our aim is to develop a robust force-motion controller that avoids the instability issues associated with force control in unconstrained directions and motion control in constrained directions without requiring knowledge or estimation of a perfect environment model.

As motivated earlier, we will employ the direct force control method to gain explicit control over the interaction wrench. Additionally, to ensure effective interaction with unmodelled environments, we will leverage impedance control to regulate motion. The cornerstone of our approach to guarantee stability in simultaneous force and motion control is the concept of passivity. Passivity analysis offers a solution to tackle the aforementioned instabilities linked to force control during motion and motion control during contact.

The passivity-based control approach was initially introduced in Ortega and Spong (1989) within the context of adaptive control for robot manipulators. However, the concept of passivity dates back to the 1950s in the field of network analysis (Youla et al., 1959). Passivity-based control has found widespread application in robotics to ensure the stability of controllers. In Kanaoka and Yoshikawa (2003) and Hatanaka et al. (2015), systematic approaches for investigating the passivity of controlled systems are presented. One of the common applications of passivity-based approaches is teleoperation. For instance, in Secchi et al. (2006), passivity is employed to address position drift during remote motion control of a robot. Particularly when dealing with time delays, passivity analysis becomes a powerful tool to ensure stability (Yokokohji et al., 2000; Hannaford and Ryu, 2002; Franken et al., 2011; Franchi et al., 2012). Furthermore, passivity-based control is utilized in Chopra and Spong (2006); Secchi et al. (2012); Tognon et al. (2018) for collaborative multi-robot scenarios and in Ott et al. (2004); Albu-Schäffer et al. (2004); Ott et al. (2008) the stability analysis of the flexible-joint robots. Finally, in a scenario more closely aligned with our objective, in Siciliano and Villani (1996), the concept of passivity is employed to ensure force and motion regulation when interacting with elastically compliant surfaces, yet without addressing the contact-loss instability problem.

Passivity allows us to model the energy flow throughout the system and identify stability-violating control actions that could potentially cause unbounded energy generation. In our scenario, force control in an unconstrained direction can be a source of unbounded energy, resulting in the undesired motion of the robot. Similarly, when using the impedance controller, penetrating the setpoint inside the surface can also be associated with the generation of energy, potentially resulting in large interaction wrenches. In a perfect scenario, when the force and motion control actions are in reciprocal subspaces and perfectly aligned with the environment constraints, and moreover, in the absence of surface compliance and friction, there would not be any undesired energy generation, and the overall system remains passive. However, this is not the case if even only one of the above conditions is not met. Although theoretically, it is possible to model the environment and associate limited energy with actions like force control causing motion inside an elastic surface and thus ensuring the system’s passivity, we are interested in a robust solution that is not dependent on the perfect environment model. Our approach will be based on virtual energy tanks. This strategy aims to regulate energy exchange and maintain the system’s passivity even in the presence of uncertainties or imperfect environmental knowledge. Table 1 shows the passivity properties of the state-of-the-art impedance controllers together with the different controllers we introduce in this work.

Table 1.

Passivity properties for state-of-the-art interaction controllers and our work. Here, RJ and FJ stand for the rigid-body and flexible-joint robots.

Task			Joint	Imp. Controller			Force controller			Joint		Robot		Environment		Closed-loop dyn.		Ref.
				Tank	Eq.	Ports	Tank	Eq.	Ports	Eq.	Ports	Eq.	Ports	Eq.	Ports	Eq.	Passive
Impedance control	Regul.	${\mathit{F}}_{\text{ext}}=\mathbf{0}$	RJ	✗	(12)	$〈\stackrel{.}{\mathit{q}},-{\mathit{\tau }}_c〉$	-			-		(1)†	$〈\stackrel{.}{\mathit{q}},{\mathit{\tau }}_m〉$	-		(20)†	✓	Introduction to Robotics, 2005
			FJ	✗	(78)	$〈\stackrel{.}{\mathit{\theta }},-{\mathit{u}}_c〉$	-			(63-64)	$〈\stackrel{.}{\mathit{\theta }},{\mathit{u}}_c〉,〈\stackrel{.}{\mathit{q}},-{\mathit{\tau }}_a〉$	(58)†	$〈\stackrel{.}{\mathit{q}},{\mathit{\tau }}_a〉$	-		(76)†	✓	Albu-Schäffer et al. (2007)
	Track.		RJ	✗	(6)	✗	-			-		(1)†	$〈\stackrel{.}{\mathit{q}},{\mathit{\tau }}_m〉$	-		(17)†	✓	Paden and Panja (1988)
			FJ	✗	(78)	✗	-			(63-64)	$〈\stackrel{.}{\mathit{\theta }},{\mathit{u}}_c〉,〈\stackrel{.}{\mathit{q}},-{\mathit{\tau }}_a〉$	(58)†	$〈\stackrel{.}{\mathit{q}},{\mathit{\tau }}_a〉$	-		(76)†	✓	Here
	Regul.	${\mathit{F}}_{\text{ext}}=\mathbf{0}$	RJ	✗	(12)	$〈\stackrel{.}{\mathit{q}},-{\mathit{\tau }}_c〉$	-			-		(1)	$〈\stackrel{.}{\mathit{q}},{\mathit{\tau }}_m〉,〈\stackrel{.}{\mathit{x}},-{\mathit{F}}_{\text{ext}}〉$	(14)	$〈\stackrel{.}{\mathit{x}},-{\mathit{F}}_{\text{ext}}〉$	(20)‡	✓	Introduction to Robotics, 2005
			FJ	✗	(78)	$〈\stackrel{.}{\mathit{\theta }},-{\mathit{u}}_c〉$	-			(63-64)	$〈\stackrel{.}{\mathit{\theta }},{\mathit{u}}_c〉,〈\stackrel{.}{\mathit{q}},-{\mathit{\tau }}_a〉$	(58)	$〈\stackrel{.}{\mathit{q}},{\mathit{\tau }}_a〉,〈\stackrel{.}{\mathit{x}},-{\mathit{F}}_{\text{ext}}〉$	(14)	$〈\stackrel{.}{\mathit{x}},-{\mathit{F}}_{\text{ext}}〉$	(76)‡	✓	Albu-Schäffer et al. (2007)
	Track.		RJ	✗	(6)	✗	-			-		(1)	$〈\stackrel{.}{\mathit{q}},{\mathit{\tau }}_m〉,〈\stackrel{.}{\mathit{x}},-{\mathit{F}}_{\text{ext}}〉$	(14)	$〈\stackrel{.}{\mathit{x}},-{\mathit{F}}_{\text{ext}}〉$	(17)‡	✗	Paden and Panja (1988)
			FJ	✗	(78)	✗	-			(63-64)	$〈\stackrel{.}{\mathit{\theta }},{\mathit{u}}_c〉,〈\stackrel{.}{\mathit{q}},-{\mathit{\tau }}_a〉$	(58)	$〈\stackrel{.}{\mathit{q}},{\mathit{\tau }}_a〉,〈\stackrel{.}{\mathit{x}},-{\mathit{F}}_{\text{ext}}〉$	(14)	$〈\stackrel{.}{\mathit{x}},-{\mathit{F}}_{\text{ext}}〉$	(76)‡	✗	Here
	Track.		RJ	✓	(6)	✗	-			-		(1)	$〈\stackrel{.}{\mathit{q}},{\mathit{\tau }}_m〉,〈\stackrel{.}{\mathit{x}},-{\mathit{F}}_{\text{ext}}〉$	(14)	$〈\stackrel{.}{\mathit{x}},-{\mathit{F}}_{\text{ext}}〉$	(17)‡	✓	Here
			FJ	✓	(78)	✗	-			(63-64)	$〈\stackrel{.}{\mathit{\theta }},{\mathit{u}}_c〉,〈\stackrel{.}{\mathit{q}},-{\mathit{\tau }}_a〉$	(58)	$〈\stackrel{.}{\mathit{q}},{\mathit{\tau }}_a〉,〈\stackrel{.}{\mathit{x}},-{\mathit{F}}_{\text{ext}}〉$	(14)	$〈\stackrel{.}{\mathit{x}},-{\mathit{F}}_{\text{ext}}〉$	(76)‡	✓	Here
Unified control	Regul.	${\mathit{F}}_{\text{ext}}=\mathbf{0}$	RJ	✗	(12)	$〈\stackrel{.}{\mathit{q}},{\mathit{\tau }}_c〉$	✗	(3)	✗	-		(1)	$〈\stackrel{.}{\mathit{q}},{\mathit{\tau }}_m〉,〈\stackrel{.}{\mathit{x}},-{\mathit{F}}_{\text{ext}}〉$	(14)	$〈\stackrel{.}{\mathit{x}},-{\mathit{F}}_{\text{ext}}〉$	(20)	✗	Schindlbeck and Haddadin (2015)
			FJ	✗	(78)	$〈\stackrel{.}{\mathit{\theta }},{\mathit{u}}_c〉$	✗	(65)	✗	(63-64)	$〈\stackrel{.}{\mathit{\theta }},{\mathit{u}}_c〉,〈\stackrel{.}{\mathit{q}},-{\mathit{\tau }}_a〉$	(58)	$〈\stackrel{.}{\mathit{q}},{\mathit{\tau }}_a〉,〈\stackrel{.}{\mathit{x}},-{\mathit{F}}_{\text{ext}}〉$	(14)	$〈\stackrel{.}{\mathit{x}},-{\mathit{F}}_{\text{ext}}〉$	(76)	✗	Schindlbeck and Haddadin (2015)
	Regul.		RJ	✗	(12)	$〈\stackrel{.}{\mathit{q}},{\mathit{\tau }}_c〉$	✓	(27)	$〈\stackrel{.}{\mathit{q}},{\mathit{\tau }}_f^{\prime }〉$	-		(1)	$〈\stackrel{.}{\mathit{q}},{\mathit{\tau }}_m〉,〈\stackrel{.}{\mathit{x}},-{\mathit{F}}_{\text{ext}}〉$	(14)	$〈\stackrel{.}{\mathit{x}},-{\mathit{F}}_{\text{ext}}〉$	(20)	✓	Schindlbeck and Haddadin (2015)
			FJ	✗	(78)	$〈\stackrel{.}{\mathit{\theta }},{\mathit{u}}_c〉$	✓	(27)	$〈\stackrel{.}{\mathit{\theta }},{\mathit{u}}_f^{\prime }〉$	(63-64)	$〈\stackrel{.}{\mathit{\theta }},{\mathit{u}}_c〉,〈\stackrel{.}{\mathit{q}},-{\mathit{\tau }}_a〉$	(58)	$〈\stackrel{.}{\mathit{q}},{\mathit{\tau }}_a〉,〈\stackrel{.}{\mathit{x}},-{\mathit{F}}_{\text{ext}}〉$	(14)	$〈\stackrel{.}{\mathit{x}},-{\mathit{F}}_{\text{ext}}〉$	(76)	✓	Schindlbeck and Haddadin (2015)
	Track.		RJ	✗	(6)	✗	✓	(27)	$〈\stackrel{.}{\mathit{q}},{\mathit{\tau }}_f^{\prime }〉$	-		(1)	$〈\stackrel{.}{\mathit{q}},{\mathit{\tau }}_m〉,〈\stackrel{.}{\mathit{x}},-{\mathit{F}}_{\text{ext}}〉$	(14)	$〈\stackrel{.}{\mathit{x}},-{\mathit{F}}_{\text{ext}}〉$	(17)	✗	Here
			FJ	✗	(78)	✗	✓	(27)	$〈\stackrel{.}{\mathit{\theta }},{\mathit{u}}_f^{\prime }〉$	(63-64)	$〈\stackrel{.}{\mathit{\theta }},{\mathit{u}}_c〉,〈\stackrel{.}{\mathit{q}},-{\mathit{\tau }}_a〉$	(58)	$〈\stackrel{.}{\mathit{q}},{\mathit{\tau }}_a〉,〈\stackrel{.}{\mathit{x}},-{\mathit{F}}_{\text{ext}}〉$	(14)	$〈\stackrel{.}{\mathit{x}},-{\mathit{F}}_{\text{ext}}〉$	(76)	✗	Here
	Track.		RJ	✓	(35)	✗	✓	(27)	$〈\stackrel{.}{\mathit{q}},{\mathit{\tau }}_f^{\prime }〉$	-		(1)	$〈\stackrel{.}{\mathit{q}},{\mathit{\tau }}_m〉,〈\stackrel{.}{\mathit{x}},-{\mathit{F}}_{\text{ext}}〉$	(14)	$〈\stackrel{.}{\mathit{x}},-{\mathit{F}}_{\text{ext}}〉$	(17)	✓	Here
			FJ	✓	(78)	✗	✓	(27)	$〈\stackrel{.}{\mathit{\theta }},{\mathit{u}}_f^{\prime }〉$	(63-64)	$〈\stackrel{.}{\mathit{\theta }},{\mathit{u}}_c〉,〈\stackrel{.}{\mathit{q}},-{\mathit{\tau }}_a〉$	(58)	$〈\stackrel{.}{\mathit{q}},{\mathit{\tau }}_a〉,〈\stackrel{.}{\mathit{x}},-{\mathit{F}}_{\text{ext}}〉$	(14)	$〈\stackrel{.}{\mathit{x}},-{\mathit{F}}_{\text{ext}}〉$	(76)	✓	Here

^† By setting − F _ext = − F _f = 0 and equivalently τ _ext = τ _f = u _f = 0.

^‡ By setting − F _f = 0 and equivalently τ _f = u _f = 0.

Note that ${\stackrel{.}{\mathit{x}}}^T{\mathit{F}}_{\text{ext}}={\stackrel{.}{\mathit{q}}}^T{\mathit{\tau }}_{\text{ext}}$ .

One of the first explicit use cases of virtual energy tanks was Secchi et al. (2006), where they preserved the passivity of scattering-based communication channels. The approach was also implicitly applied earlier in Secchi et al. (2006), using the so-called power continuous control terms to ensure stability. Over time, energy tanks gained popularity and found applications in different control design policies. In Franken et al. (2009), tanks were employed to stabilize teleoperation setups with discrete time-varying delays, and in Franken et al. (2011), an approach was presented to feed back the dissipated energy into the tank for a two-layered control architecture. Energy tanks have been widely utilized in various contexts, including the stabilization of cooperative multi-robot systems (Franchi et al., 2012; Secchi et al., 2012; Shahriari et al., 2022), time-varying impedance control (Ferraguti et al., 2013; Tadele et al., 2014; Kronander and Billard, 2016; Michel et al., 2020), trajectory modification in dynamic movement primitives (Shahriari et al., 2017), control of aerial vehicles (Rashad et al., 2019; Brunner et al., 2022), pneumatically actuated antagonistic joints (Toedtheide et al., 2017), controller switching policies (Ferraguti et al., 2015), and safety (Tadele et al., 2014; Lachner et al., 2021).

The energy tank augmentation approach serves as a method to control and limit energy generation from identified sources, which might otherwise result in unbounded energy production. Specifically, in our case, these sources are the force control action in the presence of non-orthogonal motion and the motion control action in the presence of non-orthogonal wrenches. By attaching tanks to each of these actions, we ensure that the generated energy remains bounded. A significant challenge in tank-based approaches lies in determining the initial energy of the tanks. Although, in our scenario, the environment model can assist in identifying the tank’s initial energy, we can still estimate the required initial energy even without this information. This estimation can be achieved by prioritizing control objectives based on their potential instabilities. In other words, we assess to what extent we allow potentially passivity-violating actions to perform their tasks. Consequently, many conventional limitations associated with simultaneous force-motion control designs can be mitigated. For instance, we can select arbitrary coordinates for each task and rely on the tank to ensure stability through its coordinate-free metric – energy. While our earlier work (Schindlbeck and Haddadin, 2015) introduced the basic concept, it focused on regulation tasks. The present study strives to offer a universal, all-encompassing solution to the challenge of simultaneous force and motion control, including the tracking case. To the best of the authors’ knowledge, this work represents the first systematic effort to unify impedance with force control under a single paradigm, overcoming the conventional limitations typically associated with such integration. We expanded our previous work to also encompass tracking tasks, which required the development of a dedicated energy tank for the impedance controller. Moreover, we devised a new control law and passivity analysis, taking into account the impact of joint flexibility when employing flexible-joint robots. Additionally, we made a concerted effort to provide a thorough elucidation of various aspects, with special emphasis on energy tank design and initialization strategies. As demonstrated in the experimental section, this approach significantly enhances practicality by minimizing the number of parameters that need tuning.

1.2. Contributions

The main contribution of this paper can be summarized as follows:

1. A stable controller capable of tracking wrench and/or motion without conventional requirements, such as space reciprocity or the need for an environment model.

2. Control design and stability proof for both rigid-body and flexible-joint robots.

3. Base-case virtual tank energy initialization strategy.

4. Systematic experimental analysis of performance, robustness, and accuracy using a seven degrees-of-freedom (DoFs) joint torque-controlled lightweight robot.

1.3. Structure

The remainder of the paper is organized as follows. Section 2 provides the fundamentals of rigid-body robot modeling and wrench sensing as a basis for the following theoretical concepts. Subsequently, Sec. 3 presents the Unified Force-Impedance Controller for rigid-body robots. Section 4 details a passivity analysis of the considered systems with virtual energy tank synthesis to guarantee the overall passivity. In Section 5, an approach for the initialization of the proposed energy tanks is outlined, followed by the contact-loss stabilization method based on coupling the impedance controller to the force controller in Sec. 6. Note that despite the focus of the paper is to a large extent on rigid robots and lightweight robots, ² we also provide the theoretical framework for the flexible-joint case in the Appendix. This would also allow to apply UFIC to intrinsically elastic systems with constant joint viscoelasticity. For validating the theoretical framework, several challenging manipulation experiments are performed with the Franka Emika Robot (Haddadin et al., 2022) in Sec. 7. Finally, the paper concludes in Sec. 8.

2. Preliminaries

2.1. Notation

In this paper, we adopt the subsequent notation. The robot end-effector pose $\mathit{x}\in {\mathbb{R}}^6$ is defined as $\mathit{x}:={\left[{\mathit{p}}^T,{\mathit{\phi }}^T\right]}^T$ . It is comprised of a translational part $\mathit{p}\in {\mathbb{R}}^3$ and a suitable rotational representation, for example, Euler angles $\mathit{\phi }\in {\mathbb{R}}^3$ . Similarly, wrenches are defined as $\mathit{F}:={\left[{\mathit{f}}^T,{\mathit{m}}^T\right]}^T$ consisting of translational forces $\mathit{f}\in {\mathbb{R}}^3$ and rotational moments $\mathit{m}\in {\mathbb{R}}^3$ .

2.2. Rigid-body robot model

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

$\mathit{M}\left(\mathit{q}\right)\ddot{\mathit{q}}+\mathit{C}\left(\mathit{q},\dot{\mathit{q}}\right)\dot{\mathit{q}}+\mathit{g}\left(\mathit{q}\right)={\mathit{\tau }}_m+{\mathit{\tau }}_{\text{ext}},$

(1)

where $\mathit{q},\stackrel{.}{\mathit{q}},\ddot{\mathit{q}}\in {\mathbb{R}}^n$ denote the link position, velocity, and acceleration, respectively. The symmetric positive definite mass matrix is $\mathit{M}\left(\mathit{q}\right)\in {\mathbb{R}}^{n\times n}$ , the Coriolis and centrifugal matrix $\mathit{C}\left(\mathit{q},\stackrel{.}{\mathit{q}}\right)\in {\mathbb{R}}^n$ , and the gravity vector $\mathit{g}\left(\mathit{q}\right)\in {\mathbb{R}}^n$ . The control input of the system is the motor torque ${\mathit{\tau }}_m\in {\mathbb{R}}^n$ , while ${\mathit{\tau }}_{\text{ext}}\in {\mathbb{R}}^n$ comprises all externally applied torques. External forces are denoted by the Cartesian space wrenches ${\mathit{F}}_{\text{ext}}:={\left[{\mathit{f}}_{\text{ext}}^T,{\mathit{m}}_{\text{ext}}^T\right]}^T\in {\mathbb{R}}^6$ . This wrench can be mapped via the contact Jacobian J ^T ( q ) to joint space external torques by

${\mathit{\tau }}_{\text{ext}}={\mathit{J}}^T\left(\mathit{q}\right){\mathit{F}}_{\text{ext}}.$

(2)

For the sake of clarity, friction is explicitly not considered. Yet, the derived theory holds without loss of generalization.

3. Controller design

The control framework is developed in two steps. First, the primary force and impedance controllers are introduced in this section. Thereafter, in Sec. 4, the two energy tanks for the respective force and impedance controllers are deduced based on passivity analysis. Note that in the coming sections, we will focus on the rigid-body robots while the derivations for the flexible-joint robots can be found in the Appendix.

3.1. Cartesian force controller

A direct PID force controller with a feedforward term can be designed in Cartesian space and mapped into joint space via the end-effector Jacobian matrix. For rigid robots the control law becomes

${\mathit{\tau }}_f={\mathit{J}}^T\left(\mathit{q}\right){\mathit{F}}_f,$

(3)

where

$\begin{array}{l}{\mathit{F}}_f={\mathit{K}}_p\left[{\mathit{F}}_d\left(t\right)+{\overline{\mathit{F}}}_{\text{ext}}\left(t\right)\right]+{\mathit{K}}_d\left[{\stackrel{.}{\mathit{F}}}_d\left(t\right)+{\stackrel{.}{\overline{\mathit{F}}}}_{\text{ext}}\left(t\right)\right]\\ +{\mathit{K}}_i\left[{\int }_0^t\left({\mathit{F}}_d\left(\sigma \right)+{\overline{\mathit{F}}}_{\text{ext}}\left(\sigma \right)\right)\hspace{0.17em}d\sigma \right]+{\mathit{F}}_d+{\mathit{F}}_{FF}.\end{array}$

(4)

Here, ${\overline{\mathit{F}}}_{\text{ext}}\in {\mathbb{R}}^6$ denotes the sensed external wrench applied by the environment to the robot and ${\mathit{F}}_d\in {\mathbb{R}}^6$ is the desired wrench applied by the robot to the environment, ³ ${\mathit{K}}_p,{\mathit{K}}_d,{\mathit{K}}_i\in {\mathbb{R}}^{6\times 6}$ are positive definite diagonal gain matrices, and F _FF is the feedforward term which could be chosen arbitrarily depending on the task. For example, as will be seen in Sec. 5, it may be used to represent a feedforward friction or process force compensation wrench. Finally, the power associated with the force controller is

$P_f:={\dot{\mathit{x}}}^T{\mathit{F}}_f={\left(\mathit{J}\left(\mathit{q}\right)\dot{\mathit{q}}\right)}^T{\mathit{F}}_f={\dot{\mathit{q}}}^T{\mathit{\tau }}_f.$

(5)

3.2. Rigid-body Cartesian impedance control

3.2.1. Tracking

It is well-known that in order to have a closed-loop Cartesian impedance behavior with the desired stiffness ${\mathit{K}}_C\in {\mathbb{R}}^{6\times 6}$ , damping ${\mathit{D}}_C\in {\mathbb{R}}^{6\times 6}$ , with inertia being identical to the real Cartesian robot inertia (no inertia shaping), the control law becomes (Paden and Panja 1988)

$\begin{array}{l}{\mathit{\tau }}_i={\mathit{J}}^T\left(\mathit{q}\right)[{\mathit{K}}_C\tilde{\mathit{x}}+{\mathit{D}}_C\dot{\tilde{\mathit{x}}}+{\mathit{M}}_C\left(\mathit{q}\right){\ddot{\mathit{x}}}_d\\ +{\mathit{C}}_C\left(\mathit{q},\dot{\mathit{q}}\right){\dot{\mathit{x}}}_d+{\mathit{F}}_g\left(\mathit{q}\right)]\end{array}$

(6)

$\tilde{\mathit{x}}={\mathit{x}}_d-\mathit{f}\left(\mathit{q}\right)={\mathit{x}}_d-\mathit{x}$

(7)

$\dot{\mathit{x}}=\mathit{J}\left(\mathit{q}\right)\dot{\mathit{q}}\mathbf{.}$

(8)

The mapping $\mathit{f}:{\mathbb{R}}^n\to {\mathbb{R}}^6$ denotes the forward kinematics and ${\mathit{x}}_d\in {\mathbb{R}}^6$ the differentiable desired Cartesian pose. M _C ( q ), ${\mathit{C}}_C\left(\mathit{q},\stackrel{.}{\mathit{q}}\right)$ 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 (Khatib, 1995)

$\begin{array}{l}{\mathit{M}}_C\left(\mathit{q}\right)={\mathit{J}}^{{\#}^T}\left(\mathit{q}\right)\mathit{M}\left(\mathit{q}\right){\mathit{J}}^{\#}\left(\mathit{q}\right)\end{array}$

(9)

${\mathit{C}}_C\left(\mathit{q},\dot{\mathit{q}}\right)=\left({\mathit{J}}^{{\#}^T}\mathit{C}\left(\mathit{q},\dot{\mathit{q}}\right)-{\mathit{M}}_C\left(\mathit{q}\right)\dot{\mathit{J}}\left(\mathit{q}\right)\right){\mathit{J}}^{\#}\left(\mathit{q}\right)$

(10)

$\begin{array}{l}{\mathit{F}}_g\left(\mathit{q}\right)={\mathit{J}}^{{\#}^T}\left(\mathit{q}\right)\mathit{g}\left(\mathit{q}\right),\end{array}$

(11)

in which ${\mathit{J}}^{\#}\left(\mathit{q}\right)={\mathit{J}}^T\left(\mathit{q}\right){\left(\mathit{J}\left(\mathit{q}\right){\mathit{J}}^T\left(\mathit{q}\right)\right)}^{-1}$ is the right-hand Moore-Penrose pseudoinverse.

3.2.2. Regulation

In case the impedance desired pose is stationary $\left({\stackrel{.}{\mathit{x}}}_d=\mathbf{0}\right)$ , the control law reduces to the well-known compliance control law

${\mathit{\tau }}_c={\mathit{J}}^T\left(\mathit{q}\right)\left({\mathit{K}}_C\stackrel{\sim }{\mathit{x}}-{\mathit{D}}_C\dot{\mathit{x}}+{\mathit{F}}_g\left(\mathit{q}\right)\right).$

(12)

For both cases, K _C and D _C , are positive definite. The selection of D _C can be done manually, with options including constant values (often diagonal), or it can be obtained through suitable damping design (Albu-Schäffer et al., 2003), where it becomes state-dependent and non-diagonal. This allows for the achievement of critical damping, even when the configuration varies. Nevertheless, it is essential to guarantee positive definiteness in either case.

3.3. Naive force-impedance controller

Summing (3) and (6) leads to a naive combined force-impedance controller

${\mathit{\tau }}_m:={\mathit{\tau }}_i+{\mathit{\tau }}_f.$

(13)

In the course of the subsequent stability analysis, it becomes evident that (13) requires significant modifications to guarantee passivity and stability. Based on this, the concept of unifying force and impedance control building in energy tanks is derived.

4. Stability analysis

4.1. Preliminary passivity analysis

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

4.1.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. In other words, there is no restriction to specific types of environments, which on the other hand is typically assumed in stability proofs of force controllers (Wen and Murphy, 1990; Jung et al., 2004). In fact, it is sufficient to assume that the environment is passive w.r.t. the pair $\left(\stackrel{.}{\mathit{x}},-{\mathit{F}}_{\text{ext}}\right)$ , see similar arguments in Ott (2007). Non-passive environments inject energy into the system and may therefore potentially destabilize it. Necessarily, this implies that

${\dot{S}}_{\text{env}}\le -{\dot{\mathit{x}}}^T{\mathit{F}}_{\text{ext}}$

(14)

holds, where S_env is a positive definite storage function associated to the passive environment.

4.1.2. Force-impedance controlled rigid robot

4.1.2.1. Tracking

Considering (1), (8), (9), and (10), the dynamics model of the robot in the Cartesian becomes

${\mathit{M}}_C\left(\mathit{q}\right)\ddot{\mathit{x}}+{\mathit{C}}_C\left(\mathit{q},\dot{\mathit{q}}\right)\dot{\mathit{x}}+{\mathit{F}}_g\left(\mathit{q}\right)={\mathit{J}}^{{\#}^T}\left(\mathit{q}\right){\mathit{\tau }}_m+{\mathit{F}}_{\text{ext}}.$

(15)

Substituting τ _m with (13), the closed-loop dynamics equation becomes

$\begin{array}{l}{\mathit{M}}_C\left(\mathit{q}\right)\ddot{\mathit{x}}+{\mathit{C}}_C\left(\mathit{q},\dot{\mathit{q}}\right)\dot{\mathit{x}}+{\mathit{F}}_g\left(\mathit{q}\right)={\mathit{K}}_C\stackrel{\sim }{\mathit{x}}+{\mathit{D}}_C\dot{\stackrel{\sim }{\mathit{x}}}\\ +{\mathit{M}}_C\left(\mathit{q}\right){\ddot{\mathit{x}}}_d+{\mathit{C}}_C\left(\mathit{q},\dot{\mathit{q}}\right){\dot{\mathit{x}}}_d+{\mathit{F}}_g\left(\mathit{q}\right)+{\mathit{F}}_f+{\mathit{F}}_{\text{ext}},\end{array}$

(16)

and consequently

$\begin{array}{l}{\mathit{M}}_C\left(\mathit{q}\right)\ddot{\stackrel{\sim }{\mathit{x}}}+{\mathit{C}}_C\left(\mathit{q},\dot{\mathit{q}}\right)\dot{\stackrel{\sim }{\mathit{x}}}+{\mathit{D}}_C\dot{\stackrel{\sim }{\mathit{x}}}+{\mathit{K}}_C\stackrel{\sim }{\mathit{x}}\\ +{\mathit{F}}_f+{\mathit{F}}_{\text{ext}}=\mathbf{0}.\end{array}$

(17)

Let the storage function of (17) be defined as

$S_{i,r}:=\frac{1}{2}{\stackrel{.}{\stackrel{\sim }{\mathit{x}}}}^T{\mathit{M}}_C\left(\mathit{q}\right)\stackrel{.}{\stackrel{\sim }{\mathit{x}}}+\frac{1}{2}{\stackrel{\sim }{\mathit{x}}}^T{\mathit{K}}_C\stackrel{\sim }{\mathit{x}}.$

(18)

Then, considering (17), it is straightforward to see that the time-derivative of the storage function (18) is

where the skew-symmetry of $\left({\stackrel{.}{\mathit{M}}}_C\left(\mathit{q},\stackrel{.}{\mathit{q}}\right)-2{\mathit{C}}_C\left(\mathit{q},\stackrel{.}{\mathit{q}}\right)\right)$ was taken into account. The sign of ${\stackrel{.}{\mathit{x}}}^T{\mathit{F}}_f$ and ${\stackrel{.}{\mathit{x}}}_d^T\left({\mathit{F}}_f+{\mathit{F}}_{\text{ext}}\right)$ are a priori unknown and defining proper storage functions for the relative ports is not feasible. Thus passivity w.r.t $\left(\stackrel{.}{\mathit{x}},{\mathit{F}}_{\text{ext}}\right)$ cannot be guaranteed.

4.1.2.2. Regulation

For compliance control (12), the closed-loop dynamics (17) becomes

${\mathit{M}}_C\left(\mathit{q}\right)\ddot{\mathit{x}}+{\mathit{C}}_C\left(\mathit{q},\stackrel{.}{\mathit{q}}\right)\stackrel{.}{\mathit{x}}+{\mathit{D}}_C\stackrel{.}{\mathit{x}}={\mathit{K}}_C\stackrel{\sim }{\mathit{x}}+{\mathit{F}}_f+{\mathit{F}}_{\text{ext}}.$

(20)

The storage function is

$S_{c,r}:=\frac{1}{2}{\stackrel{.}{\mathit{x}}}^T{\mathit{M}}_C\left(\mathit{q}\right)\stackrel{.}{\mathit{x}}+\frac{1}{2}{\stackrel{\sim }{\mathit{x}}}^T{\mathit{K}}_C\stackrel{\sim }{\mathit{x}}$

(21)

and its time-derivative

$\begin{array}{l}{\dot{S}}_{c,r}={\stackrel{.}{\mathit{x}}}^T{\mathit{M}}_C\left(\mathit{q}\right)\ddot{\mathit{x}}+\frac{1}{2}{\stackrel{.}{\mathit{x}}}^T{\stackrel{.}{\mathit{M}}}_C\left(\mathit{q},\stackrel{.}{\mathit{q}}\right)\ddot{\mathit{x}}+{\stackrel{.}{\stackrel{\sim }{\mathit{x}}}}^T{\mathit{K}}_C\stackrel{\sim }{\mathit{x}}\\ & ={\stackrel{.}{\mathit{x}}}^T{\mathit{F}}_{\text{ext}}-\underset{\ge 0}{\underbrace{{\stackrel{.}{\mathit{x}}}^T{\mathit{D}}_C\stackrel{.}{\mathit{x}}}}+{\stackrel{.}{\mathit{x}}}^T{\mathit{F}}_f\end{array}.$

(22)

Due to the term ${\stackrel{.}{\mathit{x}}}^T{\mathit{F}}_f$ in (22), overall passivity cannot be guaranteed.

4.2. Tank augmentation: Force controller

In order to solve the problem of passivity violation, the concept of virtual energy tanks is applied. The subsequent concept and proof follow similar lines of thought as we proposed in Schindlbeck and Haddadin (2015) and Shahriari et al. (2018). First, we introduce the virtual energy tank for the force controller, that is, for the port $\left(\stackrel{.}{\mathit{x}},{\mathit{F}}_f\right)$ . Its energy is defined as $T_f=1/2x_{tf}^2$ , with x _tf ≠ 0 denoting its state with dynamics

${\dot{x}}_{tf}=-\frac{{\beta }_f}{x_{tf}}{\gamma }_f{\stackrel{.}{\mathit{x}}}^T{\mathit{F}}_f+\frac{{\alpha }_f}{x_{tf}}\left({\gamma }_f-1\right){\stackrel{.}{\mathit{x}}}^T{\mathit{F}}_f,$

(23)

where γ _f is defined as

${\gamma }_f=\{\begin{array}{ll}1& \text{if}\hspace{0.17em}{\stackrel{.}{\mathit{x}}}^T{\mathit{F}}_f<0\\ 0& \text{else}.,\end{array}$

(24)

indicating whether the force controller is passive or not. Please note that it may also be possible to further sub-define γ _f as a vector ${\mathit{\gamma }}_f={\left[{\gamma }_p,{\gamma }_i,{\gamma }_d,{\gamma }_{FF}\right]}^T$ such that each element corresponds to one particular component of the force controller. However, considering that $\stackrel{.}{\mathit{x}}$ is a flow, corresponding to F _f , it is more interpretable to define a single variable γ _f correlated to power ${\stackrel{.}{\mathit{x}}}^T{\mathit{F}}_f$ . Tank overload prevention, if a certain upper limit storage T^u,f is reached, is ensured by

${\beta }_f=\{\begin{array}{ll}1& \text{if}\hspace{0.17em}{T}_f\le T^{u,f}\\ 0& \text{else}.\end{array}$

(25)

Finally, while α _f could simply be defined as a binary signal, for the sake of smoothening the transition from filled to empty, α _f is chosen to be

${\alpha }_f=\{\begin{array}{ll}1& \text{if} T_f\ge T_{l,f}+{\delta }_{T,f}\\ \frac{1}{2}\left[1-\hspace{0.17em}\cos \left(\frac{T_f-T_{l,f}}{{\delta }_{T,f}}\pi \right)\right]& \text{if} T_{l,f}+{\delta }_{T,f}\ge T_f\ge T_{l,f}\\ 0& \text{else}.\end{array}$

(26)

where T_l,f > 0 is the tank’s lower limit. ⁴ Accordingly, α _f is responsible for smoothly detaching the energy tank after its energy crosses T_l,f + δ_T,f. It decays to zero after some threshold δ_T,f ≥ 0 and remains at zero when T _f reaches T_l,f. Therefore, the tank’s energy level always remains above the tank’s lower limit. Practically, this smooth behavior avoids sudden disconnection of the tank, which may lead to an unwanted instantaneous change in robot motions. The final control law for the force controller of both—rigid-body and flexible-joint robots—becomes

$\begin{array}{ll}{\mathit{\tau }}_f^{\prime }& ={\mathit{u}}_f^{\prime }={\mathit{J}}^T\left(\mathit{q}\right){\mathit{F}}_f^{\prime },\end{array}$

(27)

$\begin{array}{ll}{\mathit{F}}_f^{\prime }& =\left({\gamma }_f+{\alpha }_f\left(1-{\gamma }_f\right)\right){\mathit{F}}_f.\end{array}$

(28)

This controller intervenes only at minimal level, that is, the task degrades only if (i.) the tank is empty and (ii.) the task behaves active in the passivity sense.

4.2.1. Note

The entire design of (23)-(28) could have been done also with $\stackrel{.}{\mathit{q}}$ and τ _f instead of $\stackrel{.}{\mathit{x}}$ and F _f . The subsequent analysis would also be analogous.

4.3. Tank augmentation: Impedance controller

Following the same approach as for the force controller, a second tank is defined for the port $\left({\stackrel{.}{\mathit{x}}}_d,-\left({\mathit{F}}_f^{\prime }+{\mathit{F}}_{\text{ext}}\right)\right)$ . The tank energy is defined as $T_i=1/2x_{ti}^2$ with x _ti being the tank state with the dynamics

$\begin{array}{l}{\dot{x}}_{ti}=\frac{{\beta }_i}{x_{ti}}\left({\gamma }_i{\stackrel{.}{\mathit{x}}}_d^T\left({\mathit{F}}_f^{\prime }+{\overline{\mathit{F}}}_{\text{ext}}\right)+{\stackrel{.}{\tilde{\mathit{x}}}}^{\prime T}{\mathit{D}}_C{\stackrel{.}{\tilde{\mathit{x}}}}^{\prime }\right)\\ +\frac{{\alpha }_i}{x_{ti}}\left(1-{\gamma }_i\right){\stackrel{.}{\mathit{x}}}_d^T\left({\mathit{F}}_f^{\prime }+{\overline{\mathit{F}}}_{\text{ext}}\right)\end{array}$

(29)

$\begin{array}{ll}{\stackrel{.}{\tilde{\mathit{x}}}}^{\prime }& =\left({\gamma }_i+{\alpha }_i\left(1-{\gamma }_i\right)\right){\stackrel{.}{\mathit{x}}}_d-\stackrel{.}{\mathit{x}}.\end{array}$

(30)

Here, γ _i is defined as

${\gamma }_i=\{\begin{array}{ll}1& \text{if} {\stackrel{.}{\mathit{x}}}_d^T\left({\mathit{F}}_f^{\prime }+{\overline{\mathit{F}}}_{\text{ext}}\right)>0\\ 0& \text{else}.\end{array}$

(31)

Note the switched sign in contrast to the force tank. Moreover, β _i and α _i are defined as

${\beta }_i=\{\begin{array}{ll}1& \text{if} T_i\le T^{u,i}\\ 0& \text{else}\end{array}$

(32)

and

${\alpha }_i=\{\begin{array}{ll}1& \text{if}\hspace{0.17em}{T}_i\ge T_{l,i}+{\delta }_{T,i}\\ \frac{1}{2}\left[1-\hspace{0.17em}\cos \left(\frac{T_i-T_{l,i}}{{\delta }_{T,i}}\pi \right)\right]& \text{if}\hspace{0.17em}{T}_{l,i}+{\delta }_{T,i}\ge T_i\ge T_{l,i}\\ 0& \text{else},\end{array}$

(33)

where T^u,i ≥ 0 and T_l,i > 0 are the upper and lower energy limit, respectively, and δ_T,i is the designed threshold for the lower limit of the tank energy. By augmenting the virtual tank, the desired velocity is modified according to

${\stackrel{.}{\mathit{x}}}_d^{\prime }=\left({\gamma }_i+{\alpha }_i\left(1-{\gamma }_i\right)\right){\stackrel{.}{\mathit{x}}}_d.$

(34)

While the desired acceleration ${\ddot{\mathit{x}}}_d^{\prime }$ can be obtained by straightforwardly differentiating the velocity ${\stackrel{.}{\mathit{x}}}_d^{\prime }$ , it is crucial to properly identify the controller setpoint ${\mathit{x}}_d^{\prime }$ . The translational component of the setpoint can be determined by simply integrating the velocity. However, when it comes to the rotational part, it is essential to take into account the difference between the angular velocity and the time derivative of the orientation (Gallego and Yezzi, 2015). The impedance controller (6) is then adjusted to

$\begin{array}{ll}{\mathit{\tau }}_i^{\prime }& ={\mathit{J}}^T\left(\mathit{q}\right)[{\mathit{K}}_C{\tilde{\mathit{x}}}^{\prime }+{\mathit{D}}_C{\stackrel{.}{\tilde{\mathit{x}}}}^{\prime }+{\mathit{M}}_C\left(\mathit{q}\right){\ddot{\mathit{x}}}_d^{\prime }\\ & +{\mathit{C}}_C\left(\mathit{q},\stackrel{.}{\mathit{q}}\right){\stackrel{.}{\mathit{x}}}_d^{\prime }]+\mathit{g}\left(\mathit{q}\right)\mathbf{,}\end{array}$

(35)

where ${\stackrel{\sim }{\mathit{x}}}^{\prime }={\mathit{x}}_d^{\prime }-\mathit{x}$ .

4.4. Final passivity-based force-impedance controller

Considering (27) and (35) the passivity-based Cartesian force-impedance controller for the rigid-body robot is

${\mathit{\tau }}_m^{\prime }={\mathit{\tau }}_i^{\prime }+{\mathit{\tau }}_f^{\prime }.$

(36)

An overview of the fully elaborated unified controller with tanks is depicted in Figure 2.

Figure 2.

Force and impedance tank architecture for UFIC.

4.5. Passivity analysis after tank augmentation

With the new controller design, the storage function for the whole system becomes

$S_{\text{tot},r}:=S_{i,r}+\underset{:=T}{\underbrace{T_f+T_i}}=S_{i,r}+\frac{1}{2}{x}_{tf}^2+\frac{1}{2}{x}_{ti}^2.$

(37)

Considering (23) and (29), the time evolution of the total tank energy T is

$\begin{array}{ll}\dot{T}& =x_{tf}{\dot{x}}_{tf}+x_{ti}{\dot{x}}_{ti}\\ & =-{\beta }_f{\gamma }_f\hspace{0.17em}{\stackrel{.}{\mathit{x}}}^T{\mathit{F}}_f+{\alpha }_f\left({\gamma }_f-1\right)\hspace{0.17em}{\stackrel{.}{\mathit{x}}}^T{\mathit{F}}_f\\ & +{\beta }_i\left({\gamma }_i{\stackrel{.}{\mathit{x}}}_d^T\left({\mathit{F}}_f^{\prime }+{\overline{\mathit{F}}}_{\text{ext}}\right)+{\stackrel{.}{\tilde{\mathit{x}}}}^{\prime T}{\mathit{D}}_C{\stackrel{.}{\tilde{\mathit{x}}}}^{\prime }\right)\\ & +{\alpha }_i\left(1-{\gamma }_i\right){\stackrel{.}{\mathit{x}}}_d^T\left({\mathit{F}}_f^{\prime }+{\overline{\mathit{F}}}_{\text{ext}}\right).\end{array}$

(38)

Hence, considering (19) we get

$\begin{array}{ll}{\dot{S}}_{\text{tot},r}& ={\stackrel{.}{\mathit{x}}}^T{\mathit{F}}_{\text{ext}}-{\stackrel{.}{\tilde{\mathit{x}}}}^{\prime T}{\mathit{D}}_C{\stackrel{.}{\tilde{\mathit{x}}}}^{\prime }+{\stackrel{.}{\mathit{x}}}^T{\mathit{F}}_f^{\prime }-{\stackrel{.}{\mathit{x}}}_d^{\prime T}\left({\mathit{F}}_f^{\prime }+{\overline{\mathit{F}}}_{\text{ext}}\right)\\ & -{\beta }_f{\gamma }_f\hspace{0.17em}{\stackrel{.}{\mathit{x}}}^T{\mathit{F}}_f+{\alpha }_f\left({\gamma }_f-1\right)\hspace{0.17em}{\stackrel{.}{\mathit{x}}}^T{\mathit{F}}_f\\ & +{\beta }_i\left({\gamma }_i{\stackrel{.}{\mathit{x}}}_d^T\left({\mathit{F}}_f^{\prime }+{\overline{\mathit{F}}}_{\text{ext}}\right)+{\stackrel{.}{\tilde{\mathit{x}}}}^{\prime T}{\mathit{D}}_C{\stackrel{.}{\tilde{\mathit{x}}}}^{\prime }\right)\\ & +{\alpha }_i\left(1-{\gamma }_i\right){\stackrel{.}{\mathit{x}}}_d^T\left({\mathit{F}}_f^{\prime }+{\overline{\mathit{F}}}_{\text{ext}}\right)\\ & ={\stackrel{.}{\mathit{x}}}^T{\mathit{F}}_{\text{ext}}-{\stackrel{.}{\tilde{\mathit{x}}}}^{\prime T}{\mathit{D}}_C{\stackrel{.}{\tilde{\mathit{x}}}}^{\prime }+\left({\gamma }_f+{\alpha }_f\left(1-{\gamma }_f\right)\right){\stackrel{.}{\mathit{x}}}^T{\mathit{F}}_f\\ & -\left({\gamma }_i+{\alpha }_i\left(1-{\gamma }_i\right)\right){\stackrel{.}{\mathit{x}}}_d^T\left({\mathit{F}}_f^{\prime }+{\overline{\mathit{F}}}_{\text{ext}}\right)-{\beta }_f{\gamma }_f\hspace{0.17em}{\stackrel{.}{\mathit{x}}}^T{\mathit{F}}_f\\ & +{\alpha }_f\left({\gamma }_f-1\right)\hspace{0.17em}{\stackrel{.}{\mathit{x}}}^T{\mathit{F}}_f+{\beta }_i{\gamma }_i{\stackrel{.}{\mathit{x}}}_d^T\left({\mathit{F}}_f^{\prime }+{\overline{\mathit{F}}}_{\text{ext}}\right)\\ & +{\beta }_i{\stackrel{.}{\tilde{\mathit{x}}}}^{\prime T}{\mathit{D}}_C{\stackrel{.}{\tilde{\mathit{x}}}}^{\prime }+{\alpha }_i\left(1-{\gamma }_i\right){\stackrel{.}{\mathit{x}}}_d^T\left({\mathit{F}}_f^{\prime }+{\overline{\mathit{F}}}_{\text{ext}}\right)\\ & ={\stackrel{.}{\mathit{x}}}^T{\mathit{F}}_{\text{ext}}+\left({\beta }_i-1\right){\stackrel{.}{\tilde{\mathit{x}}}}^{\prime T}{\mathit{D}}_C{\stackrel{.}{\tilde{\mathit{x}}}}^{\prime }+{\gamma }_f\left(1-{\beta }_f\right){\stackrel{.}{\mathit{x}}}^T{\mathit{F}}_f\\ & +{\gamma }_i\left({\beta }_i-1\right){\stackrel{.}{\mathit{x}}}_d^T\left({\mathit{F}}_f^{\prime }+{\overline{\mathit{F}}}_{\text{ext}}\right).\end{array}$

(39)

Depending on the value of γ _f and γ _i , there will be four cases for ${\dot{S}}_{\text{tot},r}$ , see Table 2. For all of them ${\dot{S}}_{\text{tot},r}\le {\stackrel{.}{\mathit{x}}}^T{\mathit{F}}_{\text{ext}}$ holds. As a result, the overall system is always passive w.r.t. $\left(\stackrel{.}{\mathit{x}},{\mathit{F}}_{\text{ext}}\right)$ . Therefore, considering (14), the combination of the overall system and the passive environment results in a passive system with the storage function (S_tot,r + S_env), such that ${\dot{S}}_{\text{tot},r}+{\dot{S}}_{\text{env}}\le 0$ .

Table 2.

Storage function time-derivative for the rigid robot case with different tank values considering (24), (31), and (39).

γ f	γ i	${\dot{S}}_{tot,r}$
1	1	$\stackrel{\le 0}{\overbrace{\underset{\in \left\{-1,0\right\}}{\underbrace{\left({\beta }_i-1\right)}}\underset{\ge 0}{\underbrace{{\stackrel{.}{\stackrel{\sim }{\mathit{x}}}}^{\prime T}{\mathit{D}}_C{\stackrel{.}{\stackrel{\sim }{\mathit{x}}}}^{\prime }}}}}+\stackrel{\le 0}{\overbrace{\underset{\in \left\{0,1\right\}}{\underbrace{\left(1-{\beta }_f\right)}}\underset{\le 0}{\underbrace{{\stackrel{.}{\mathit{x}}}^T{\mathit{F}}_f}}}}+\stackrel{\le 0}{\overbrace{\underset{\in \left\{-1,0\right\}}{\underbrace{\left({\beta }_i-1\right)}}\underset{\ge 0}{\underbrace{{\stackrel{.}{\mathit{x}}}_d^T\left({\mathit{F}}_f^{\prime }+{\overline{\mathit{F}}}_{\text{ext}}\right)}}}}+{\stackrel{.}{\mathit{x}}}^T{\mathit{F}}_{\text{ext}}\le {\stackrel{.}{\mathit{x}}}^T{\mathit{F}}_{\text{ext}}$
1	0	$\stackrel{\le 0}{\overbrace{\underset{\in \left\{-1,0\right\}}{\underbrace{\left({\beta }_i-1\right)}}\underset{\ge 0}{\underbrace{{\stackrel{.}{\stackrel{\sim }{\mathit{x}}}}^{\prime T}{\mathit{D}}_C{\stackrel{.}{\stackrel{\sim }{\mathit{x}}}}^{\prime }}}}}+\stackrel{\le 0}{\overbrace{\underset{\in \left\{0,1\right\}}{\underbrace{\left(1-{\beta }_f\right)}}\underset{\le 0}{\underbrace{{\stackrel{.}{\mathit{x}}}^T{\mathit{F}}_f}}}}+{\stackrel{.}{\mathit{x}}}^T{\mathit{F}}_{\text{ext}}\le {\stackrel{.}{\mathit{x}}}^T{\mathit{F}}_{\text{ext}}$
0	1	$\stackrel{\le 0}{\overbrace{\underset{\in \left\{-1,0\right\}}{\underbrace{\left({\beta }_i-1\right)}}\underset{\ge 0}{\underbrace{{\stackrel{.}{\stackrel{\sim }{\mathit{x}}}}^{\prime T}{\mathit{D}}_C{\stackrel{.}{\stackrel{\sim }{\mathit{x}}}}^{\prime }}}}}+\stackrel{\le 0}{\overbrace{\underset{\in \left\{-1,0\right\}}{\underbrace{\left({\beta }_i-1\right)}}\underset{\ge 0}{\underbrace{{\stackrel{.}{\mathit{x}}}_d^T\left({\mathit{F}}_f^{\prime }+{\overline{\mathit{F}}}_{\text{ext}}\right)}}}}+{\stackrel{.}{\mathit{x}}}^T{\mathit{F}}_{\text{ext}}\le {\stackrel{.}{\mathit{x}}}^T{\mathit{F}}_{\text{ext}}$
0	0	$\stackrel{\le 0}{\overbrace{\underset{\in \left\{-1,0\right\}}{\underbrace{\left({\beta }_i-1\right)}}\underset{\ge 0}{\underbrace{{\stackrel{.}{\stackrel{\sim }{\mathit{x}}}}^{\prime T}{\mathit{D}}_C{\stackrel{.}{\stackrel{\sim }{\mathit{x}}}}^{\prime }}}}}+{\stackrel{.}{\mathit{x}}}^T{\mathit{F}}_{\text{ext}}\le {\stackrel{.}{\mathit{x}}}^T{\mathit{F}}_{\text{ext}}$

4.5.1. Note

Depending on the task, the desired wrench F _d (t) and motion x _d (t) could be defined in different Cartesian coordinate frames. In that case, not only should the signals used to derive the control errors in (4) and (7) be defined in the same coordinate frames as the desired values, but also proper Jacobian matrices should be employed in (3), (6), and (8) to construct the control inputs. Consequently, all the above derivations and, thus, the passivity proof would still hold as power and energy are coordinate-independent terms.

4.6. Stability analysis via Lyapunov function

Let us define the overall system’s state variable to be: $\mathit{z}={\left[{\stackrel{\sim }{\mathit{x}}}^{\prime T},{\stackrel{.}{\stackrel{\sim }{\mathit{x}}}}^{\prime T},x_{tf},x_{ti},{\mathit{x}}_{\text{env}}^T\right]}^T$ , where ${\stackrel{\sim }{\mathit{x}}}^{\prime }={\mathit{x}}_d^{\prime }-\mathit{x}$ and ${\mathit{x}}_{\text{env}}\in {\mathbb{R}}^6$ is the environment state with which the environment’s energy $S_{\text{env}}\in {\mathbb{R}}_{\ge 0}$ is determined. Moreover, consider $x_{tf}*,x_{ti}*\in {\mathbb{R}}_{\ge 0}$ and ${\mathit{x}}_{\text{env}}*\in {\mathbb{R}}^6$ such that

$\begin{array}{ll}\forall \mathit{z}& \in \mathcal{X}\mathcal{\backslash }\left\{{\left[{\mathbf{0}}^T,{\mathbf{0}}^T,x_{tf}*,x_{ti}*,{\mathit{x}}_{\text{env}}^{*T}\right]}^T\right\},\\ & \frac{1}{2}{\stackrel{\sim }{\mathit{x}}}^{\prime T}{\mathit{K}}_C{\stackrel{\sim }{\mathit{x}}}^{\prime }+\frac{1}{2}{\stackrel{.}{\stackrel{\sim }{\mathit{x}}}}^{\prime T}{\mathit{M}}_C\left(\mathit{q}\right){\stackrel{.}{\stackrel{\sim }{\mathit{x}}}}^{\prime }+T_f\left(x_{tf}\right)+T_f\left(x_{ti}\right)\\ & +S_{\text{env}}\left({\mathit{x}}_{\text{env}}\right)>T_f(x_{tf}*)+T_f(x_{ti}*)+S_{\text{env}}({\mathit{x}}_{\text{env}}*),\end{array}$

(40)

where $\mathcal{X}$ is the space of all possible values for z during the control task. In the following, we show that the overall system is stable at ${\mathit{z}}^{*}={\left[{\mathbf{0}}^T,{\mathbf{0}}^T,x_{tf}*,x_{ti}*,{\mathit{x}}_{\text{env}}^{*T}\right]}^T$ . The candidate Lyapunov function is

$\begin{array}{ll}V\left(\mathit{z}\right)& =\frac{1}{2}{\tilde{\mathit{x}}}^{\prime T}{\mathit{K}}_C{\tilde{\mathit{x}}}^{\prime }+\frac{1}{2}{\stackrel{.}{\tilde{\mathit{x}}}}^{\prime T}{\mathit{M}}_C\left(\mathit{q}\right){\stackrel{.}{\tilde{\mathit{x}}}}^{\prime }+\frac{1}{2}{x}_{tf}^2+\frac{1}{2}{x}_{ti}^2\\ & +S_{\text{env}}\left({\mathit{x}}_{\text{env}}\right)-\frac{1}{2}{x}_{tf}^{*2}-\frac{1}{2}{x}_{ti}^{*2}-S_{\text{env}}\left({\mathit{x}}_{\text{env}}*\right).\end{array}$

(41)

It can be seen that V ( z *) = 0, and considering (40):

$\forall \mathit{z}\backslash \mathit{z}*,V\left(\mathit{z}\right)>0.$

(42)

Thus, considering the continuous differentiability of (41), V( z ) is a valid Lyapunov function. Moreover, considering (14) and Table 2:

$\dot{V}\left(\mathit{z}\right)\le 0.$

(43)

Therefore, the overall system is Lyapunov stable at ${\left[{\stackrel{\sim }{\mathit{x}}}^{\prime T},{\stackrel{.}{\stackrel{\sim }{\mathit{x}}}}^{\prime T},x_{tf},x_{ti},{\mathit{x}}_{\text{env}}^T\right]}^T=\left[{\mathbf{0}}^T,{\mathbf{0}}^T,x_{tf}*,x_{ti}*,{\mathit{x}}_{\text{env}}^{*T}\right]$ . Please note that as dissipation can always be presumed in the environment, one can say ${\dot{S}}_{\text{env}}<0$ , resulting in

$\dot{V}\left(\mathit{z}\right)<0.$

(44)

Hence, under such an assumption, the overall system becomes asymptotically stable.

The control objective achievement at z = z * depends on the values of $x_{tf}*$ and $x_{ti}*$ . Considering (33), if $x_{ti}*\ge \sqrt{2\left(T_{l,i}+{\delta }_{T,i}\right)}$ , the value of α _i would be 1 at z = z *, implying the convergence of x , $\stackrel{.}{\mathit{x}}$ to x _d , ${\stackrel{.}{\mathit{x}}}_d$ , respectively. In contrast, for the case α _i < 1, the desired trajectory is modified to ${\mathit{x}}_d^{\prime }\left(t\right)$ and thus x , $\stackrel{.}{\mathit{x}}$ would converge to the shaped versions ${\mathit{x}}_d^{\prime }$ , ${\stackrel{.}{\mathit{x}}}_d^{\prime }$ , respectively. Considering z = z * as the system’s stable equilibrium, the closed-loop dynamics equation (17) becomes

${\mathit{F}}_f^{\prime }+{\mathit{F}}_{\text{ext}}=\mathbf{0}.$

(45)

Please note that as at z = z * there will be no more change in the environment state x _env, that is, ${\stackrel{.}{\mathit{x}}}_{\text{env}}=\mathbf{0}$ resulting in ${\dot{S}}_{\text{env}}=0$ . Thus, the external wrench F _ext can be assumed to be constant at z *. That means that considering (45), ${\mathit{F}}_f^{\prime }$ will also converge to a constant value at z = z *. Now, considering no feedforward wrench in (4) (i.e., F _FF = 0), if at z *, the value of α _f is 1 (i.e., $x_{tf}*\ge \sqrt{2\left(T_{l,f}+{\delta }_{T,f}\right)}$ ), ${\mathit{F}}_f^{\prime }$ would be equal to F _f , and (45) becomes

${\mathit{K}}_p\stackrel{\sim }{\mathit{F}}+{\mathit{K}}_d\stackrel{.}{\stackrel{\sim }{\mathit{F}}}+{\mathit{K}}_i{\int }_{\sigma =0}^{t*}\stackrel{\sim }{\mathit{F}}\hspace{0.17em}d\sigma +\stackrel{\sim }{\mathit{F}}=\mathbf{0}.$

(46)

where $\stackrel{\sim }{\mathit{F}}={\mathit{F}}_d+{\mathit{F}}_{\text{ext}}$ and t* is the time when z has converged to z *.

4.6.1. Passivity cushion

As long as the controller and tank as a combined system behave passive, the force controller (46) converges with its own dynamics to F _d = − F _ext (obviously assuming that this or any other suitable force controller is indeed able to converge by itself). In essence, the passivity cushion means that (i.) a possibly non-passive controller is enhanced by a tank safety buffer and (ii.) together they give the total system more space to converge.

4.6.2. Passivity violation

If the system of the controller within the passivity cushion behaves non-passive, it will be smoothly detached from the overall system not to violate the overall stability. In that case, ${\mathit{F}}_f^{\prime }$ would gradually diverge from F _f towards 0, and the force control objective is sacrificed for the sake of stability.

5. Controller initialization

Note that for the sake of clarity, the time dependency of the desired twists and wrenches are dropped. Furthermore, to maintain brevity, we have presented only one exemplar case for classes 1, 2A, and 2B among different coordinates.

By augmenting virtual energy tanks, the conventional rule that force and motion control need to belong exclusively to reciprocal spaces is overcome. However, two main problems remain when wrenches and velocities (twists) share non-orthogonal components.

First, achieving an arbitrary value for the interaction wrench and the motion in the same direction is generally not feasible (Hogan, 1984). Therefore, one of these objectives needs to be compromised to attain the other one or generally be chosen such that it is inherently consistent with the primary one due. This process can, for example, be accomplished by suitable controller parameterization to prioritize one control objective over the other.

Second, even after attaching the tanks, potentially passivity-violating actions, that is, force control in motion and motion control in contact may still occur. The extent of these actions depends on the amount of energy that can be provided by the tanks. Consequently, a design question arises: How much energy should the tanks reserve and provide?

Before addressing these two challenges, let us first examine the most relevant scenarios that may indeed lead to non-orthogonal forces and motions.

As explained earlier, a common approach for simultaneous control of force and motion is to control force in constrained directions while controlling motion in unconstrained directions. However, this necessitates online an exact geometrical model of the environment. As no model is free from errors and environmental changes, for example, due to the continuous interactions with the robot may lead to model discrepancies, the actual geometrical model is unlikely to match the coordinates in which the force and motion controllers are defined. Consequently, even though tasks are defined in reciprocal spaces, motions in the force control direction and wrenches in the motion control direction occur. Furthermore, even with a perfect geometric model of the environment, the dynamics of the environment may lead to non-orthogonal force and motion components. A classic example is surface friction. Even if motion control is specified in unconstrained directions, friction inherently induces process wrenches along the motion direction. Similarly, compliant surfaces lead to the generation of motion in the direction of the applied wrench. In the following, practical suggestions are presented to address these scenarios.

5.1. Task parametrization

To ensure that force control and impedance control function in reciprocal spaces, it is essential not only to define orthogonal desired motions and wrenches but also to properly set the associated controller parameters. Specifically, referring to (6), the elements of the stiffness and damping matrices K _C , D _C related to the constrained directions should be set 0. Additionally, the elements of the feedback and feedforward terms K _p , K _d , K _i , F _FF in the force controller law (4) associated with the unconstrained directions should also be set to zero.

Subsequently, the force-impedance control tasks are categorized based on the kinematic constraints imposed by different surface classes as follows:

• Class 1: Unconstrained point contact (e.g., filing, indentation, button pushing)

• Class 2A: Unconstrained surface contact (e.g., screwing, polishing, or grinding a 2D surface)

• Class 2B: Semi-constrained surface contact (e.g., polishing or grinding a 3D surface)

• Class 2C: Constrained surface contact (e.g., docking maneuver)

Valid task variable combinations of the UFIC control inputs F _d (t) and ${\stackrel{.}{\mathit{x}}}_d\left(t\right)$ are listed in Table 3 for these four surface classes. Here, ${\mathit{F}}_p\in {\mathbb{R}}^6$ represents the environment process wrenches resulting from the motion control action on the surface, as further elaborated in the following.

Table 3.

UFIC task variable configurations and contact reaction models.

	Desired twist	Desired wrench	Process reaction wrench model
1	$\left[\begin{array}{l}{\left[\begin{array}{lll}{v}_{d,x}& v_{d,y}& 0\end{array}\right]}^T\\ {\left[\begin{array}{lll}{\omega }_{d,x}& {\omega }_{d,y}& {\omega }_{d,z}\end{array}\right]}^T\end{array}\right]$	$\left[\begin{array}{l}{\left[\begin{array}{lll}0& 0& f_{d,z}\end{array}\right]}^T\\ {\left[\begin{array}{lll}0& 0& 0\end{array}\right]}^T\end{array}\right]$	$\left[\begin{array}{l}{\left[\begin{array}{lll}{f}_{p,x}& f_{p,y}& -f_{d,z}\end{array}\right]}^T\\ {\left[\begin{array}{lll}0& 0& 0\end{array}\right]}^T\end{array}\right]$
2A	$\left[\begin{array}{l}{\left[\begin{array}{lll}{v}_{d,x}& v_{d,y}& 0\end{array}\right]}^T\\ {\left[\begin{array}{lll}0& 0& {\omega }_{d,z}\end{array}\right]}^T\end{array}\right]$	$\left[\begin{array}{l}{\left[\begin{array}{lll}0& 0& f_{d,z}\end{array}\right]}^T\\ {\left[\begin{array}{lll}{m}_{d,x}& m_{d,y}& 0\end{array}\right]}^T\end{array}\right]$	$\left[\begin{array}{l}{\left[\begin{array}{lll}{f}_{p,x}& f_{p,y}& -f_{d,z}\end{array}\right]}^T\\ {\left[\begin{array}{lll}-m_{d,x}& -m_{d,y}& m_{p,z}\end{array}\right]}^T\end{array}\right]$
2B	$\left[\begin{array}{l}{\left[\begin{array}{lll}{v}_{d,x}& 0& 0\end{array}\right]}^T\\ {\left[\begin{array}{lll}0& 0& 0\end{array}\right]}^T\end{array}\right]$	$\left[\begin{array}{l}{\left[\begin{array}{lll}0& f_{d,y}& f_{d,z}\end{array}\right]}^T\\ {\left[\begin{array}{lll}{m}_{d,x}& m_{d,y}& m_{d,z}\end{array}\right]}^T\end{array}\right]$	$\left[\begin{array}{l}{\left[\begin{array}{lll}{f}_{p,x}& -f_{d,y}& -f_{d,z}\end{array}\right]}^T\\ {\left[\begin{array}{lll}-m_{d,x}& -m_{d,y}& -m_{d,z}\end{array}\right]}^T\end{array}\right]$
2C	$\left[\begin{array}{l}{\left[\begin{array}{lll}0& 0& 0\end{array}\right]}^T\\ {\left[\begin{array}{lll}0& 0& 0\end{array}\right]}^T\end{array}\right]$	$\left[\begin{array}{l}{\left[\begin{array}{lll}{f}_{d,x}& f_{d,y}& f_{d,z}\end{array}\right]}^T\\ {\left[\begin{array}{lll}{m}_{d,x}& m_{d,y}& m_{d,z}\end{array}\right]}^T\end{array}\right]$	$\left[\begin{array}{l}{\left[\begin{array}{lll}-f_{d,x}& -f_{d,y}& -f_{d,z}\end{array}\right]}^T\\ {\left[\begin{array}{lll}-m_{d,x}& -m_{d,y}& -m_{d,z}\end{array}\right]}^T\end{array}\right]$

As explained earlier, a model discrepancy may lead to passivity violation even if the force and motion control tasks are designed to be reciprocal. Such discrepancy is not always unintentional. In certain cases, when the environment has a complex geometric shape, it may be more practical to employ a simplified model to construct the task coordinates. For instance, assuming a straight flat surface can be an appropriate approach when the surface curve is continuously changing. In this scenario, a practical approach is to increase the compliance of the impedance control, that is, decreasing the stiffness and damping values in directions where neither controlling force nor controlling motion is critical. By doing so, we allow the robot’s end-effector to adapt to the surface (see Sec. 7.4 for an example). In this case, it may be more sensible to define the force control task in the end-effector coordinates to ensure passive behavior. However, this approach can result in non-orthogonal force and motion control tasks if, due to the high compliance, the end-effector pose changes. To address such a situation, a suitable gain structure is to be implemented encoding whether force control or motion control should take precedence in a given direction. A common method involves prioritizing the force controller by using relatively higher values of the integrator feedback gain (Villani and Schutter, 2016).

In addition to the environment kinematics model, the process dynamics characterized by friction, viscoelasticity, wearing behavior or even switching interaction dynamics have significant impacts. Such effects may be addressed through explicit consideration of the expected process wrenches, denoted as F _p , which include, for example, wrenches associated with surface friction or the dynamic effects of an object that the robot is pushing. One approach to compensate for such effects is to utilize the feedforward term F _FF in the force control (4). As will be seen in the following, this can directly influence the behavior of the virtual tanks.

5.2. Tank initialization

Even though stability is proven, this does not necessarily mean that the task can be fulfilled as desired. If any of the tanks is not loaded with a sufficient amount of energy, the force controller will be deactivated, or the impedance control will transition to compliance control. If this happens during manipulation, the intended task goal will not be achieved since the wrench cannot be regulated accordingly or the desired trajectory is not followed properly. To solve this problem, the concept of initial task energy E _T was introduced in Haddadin (2015); Schindlbeck and Haddadin (2015). E _T is defined as the minimal energy to be initially stored in the tanks for fulfilling all requirements of a manipulation task. This task energy or at least an estimated lower bound needs to be known prior to execution if not only stability but also correct task execution and performance are to be achieved. Obviously, the simplest strategy would be to use a practically high enough energy amount. However, since the provided energy is utilized for potentially passivity-violating actions, such as force control in motion and motion control in contact, prioritizing safety suggests avoiding overloading the tanks. Therefore, an approach should be pursued to estimate the minimum required initial energy for the virtual tanks. This estimation can be based on the task or the environment models. Alternatively, with the recent advances in machine learning, data-driven or model-informed hybrid approaches could be employed. For example, in Shahriari et al. (2019) and Benzi et al. (2022), two approaches are introduced for adapting the tank configurations, which prove especially useful in environments undergoing changes. In this study, we present a general base-case strategy for systematically initializing the energy tanks of the UFIC, laying the foundation for future learning-based initialization methods. In the following, we propose a few model-based approaches for the tank initialization process.

5.2.1. Force control tank

To determine how much energy is initially required in the force control tank to fulfill the task, let us first consider the scenarios where tank energy would be drained. As explained in Sec. 4.2, those scenarios occur as soon as the power ${\stackrel{.}{\mathit{x}}}^T{\mathit{F}}_f$ is greater than zero, that is, when the force control and the robot motion are in the same direction. One scenario for this condition being true is the activation of force control while the robot is not in contact yet, resulting in robot acceleration in the direction of the force control. Similarly, another scenario for this case is when the contact is lost while the force control is active. Another scenario occurs when the contact surface is elastic, and consequently, the robot moves through the surface to establish the desired wrench. As the first two scenarios are rather failure cases, we focus on the third case and assume that the first two scenarios are dealt with by appropriate measures such as contact-loss stabilization in Sec. 6. Let us first consider the translational case under the following assumption.

Assumption 1

The robot accurately performs the desired linear force control task such that it is fully compensated by the surface reaction force and moment, that is, f _w = − f _d (t) and m _w = − m _d (t).

For simplicity, the environment is assumed to behave linearly elastic, ⁵ leading for this case with assumption 1 to

${\mathit{f}}_d\left(t\right)\approx -{\mathit{f}}_w={\mathit{K}}_{w,t}\left({\mathit{p}}_w\left(t\right)-{\mathit{p}}_{w,0}\right).$

(47)

Here, f _w is modeled as a linear stiffness wall K _w,t with initial position p _w,0. Solving for ${\mathit{p}}_w\left(t\right)={\mathit{K}}_{w,t}^{-1}{\mathit{f}}_d\left(t\right)+{\mathit{p}}_{w,0}$ yields the work required to move the wall to be

$E_{T,f}\left(t\right)={\int }_0^t\frac{1}{2}{\left({\mathit{p}}_w\left(\sigma \right)-{\mathit{p}}_{w,0}\right)}^T{\mathit{K}}_{w,t}\left({\mathit{p}}_w\left(\sigma \right)-{\mathit{p}}_{w,0}\right)d\sigma .$

(48)

For the regulation case equation (48) reduces to

$E_{T,fl}=\frac{1}{2}{\left({\mathit{p}}_w-{\mathit{p}}_{w,0}\right)}^T{\mathit{K}}_{w,t}({\mathit{p}}_w-{\mathit{p}}_{w,0}).$

(49)

With the help of a quaternion-based potential function (Caccavale et al., 1998)

$E_{T,fq}=2△{\mathit{k}}_v^T{\mathit{K}}_{w,q}△{\mathit{k}}_v$

(50)

the principle for translation can be extended to the rotational case. $△\mathit{k}=\left(△k_0,△{\mathit{k}}_v\right):={\mathit{k}}_{w,0}^{-1}{\mathit{k}}_w$ defines the quaternion rotation error—with scalar part △k₀ and vector part △ k _v —from the wall initial orientation denoted by k _w,0 = (k_0,(w,0), k _v,(w,0)) and the equilibrium orientation k _w = (k_0,w, k _v,w). Here, K _w,q is the wall rotational stiffness matrix defined in quaternion space. Note that on SO(3), multiple equilibrium points exist, of which only one is stable, that is, the potential function needs to be considered only locally.

Finally, another scenario where the force control tank can be drained is due to the feedforward term F _FF when it is employed to address environmental dynamics effects, such as feedforward friction compensation. In this case, the feedforward term will be collinear with the motion control direction. To estimate the associated energy, one approach would be to model the expected friction. As a similar analysis will be used for the impedance control tank initialization process, we address this in the next section. It is worth mentioning that to avoid using the force control tank energy for such objectives, a separate tank could be dedicated solely for the feedforward term. The passivity analysis and tank augmentation procedure would be identical to the ones used for the force controller. However, for the sake of clarity, this is not followed in the present work.

5.2.2. Impedance control tank

As discussed in Sec. 4.3, the power associated with the impedance control tank is the summation of powers $-{\stackrel{.}{\mathit{x}}}_d^T{\mathit{F}}_f$ and $-{\stackrel{.}{\mathit{x}}}_d^T{\mathit{F}}_{\text{ext}}$ , respectively. Let us first discuss when these powers become problematic from the passivity point of view. The condition $-{\stackrel{.}{\mathit{x}}}_d^T{\mathit{F}}_f>0$ holds if the desired motion acts against the force control output, that is, when the impedance control task intends to counteract the force controller. As explained earlier, a common scenario in which this situation might occur is when the two controllers are defined in two independent coordinates, such as the end-effector frame for the force controller and the world frame for the impedance controller. Developing a model-based approach for such scenarios necessitates the consideration of several aspects, including an approximate geometrical model of the environment and the characteristics of the desired motion. However, as this information is not always available, while we stress the significance of having the tanks to address these scenarios, we refrain from devising a model-based approach for their energy initialization process.

On the other hand, $-{\stackrel{.}{\mathit{x}}}_d^T{\mathit{F}}_{\text{ext}}>0$ holds if the desired motion acts against the environment reactive wrenches. This may have the following two disturbance causes.

• disturbance 1: clamping due to unforeseen obstacles or faulty material

• disturbance 2: jamming due to the surface friction or the dynamics effect of an object being carried

Disturbance 1 is clearly an unwanted fault and needs to be detected rather than coped with. Since it is directly related to the contact force, it is reasonable to assume that $r:=-{\stackrel{.}{\mathit{x}}}_d^T{\mathit{F}}_f$ is increasing far beyond expected process forces. Therefore, r is a valid clamping fault detection signal. On the other hand, in the case of knowing or learning the maximum jamming forces from design or experiment, disturbance 2 can be taken into account by allocating a reasonable amount of initial tank energy. If friction is the main cause of the disturbance, simple model ⁶ of it can be used for the initial energy estimation:

${\mathit{F}}_{frc}=-{\mathit{d}}_v°{\stackrel{.}{\mathit{x}}}_w-\underset{{\mathit{d}}_c}{\underbrace{{\left[d_{cf}{\mathbf{1}}_{1\times 3},d_{cm}{\mathbf{1}}_{1\times 3}\right]}^T}}°\text{sgn}\left({\stackrel{.}{\mathit{x}}}_w\right),$

(51)

where ° denotes the element-wise product operation, ${\stackrel{.}{\mathit{x}}}_w\in {\mathbb{R}}^6$ is the contact surface velocity, ${\mathit{d}}_v\in {\mathbb{R}}_{\ge 0}^6$ is the vector of the viscous friction coefficients, and $d_{cf},d_{cm}\in {\mathbb{R}}_{\ge 0}$ are the linear and angular Coulomb friction coefficients, respectively. They can be expressed as

$\left[\begin{array}{l}{d}_{cf}\\ d_{cm}\end{array}\right]=\underset{\mathit{\mu }}{\underbrace{\left[\begin{array}{ll}{\mu }_{ff}& {\mu }_{fm}\\ {\mu }_{mf}& {\mu }_{mm}\end{array}\right]}}\hspace{0.17em}\left[\begin{array}{l}\Vert {\mathit{f}}_N\Vert \\ \Vert {\mathit{m}}_N\Vert \end{array}\right],$

(52)

where ${\mathit{f}}_N,{\mathit{m}}_N\in {\mathbb{R}}^3$ are the surface normal force and moment, respectively. Finally, for estimating the initial impedance control tank energy, we make the following assumptions.

Assumption 2

Because of assumption 1 it follows that ${\mathit{F}}_N\left(t\right)={\left[{\mathit{f}}_N,{\mathit{m}}_N\right]}^T\approx -{\mathit{F}}_d\left(t\right)$ .

Assumption 3

Because the robot accurately performs the motion tracking task for stationary contacts, we assume ${\stackrel{.}{\mathit{x}}}_w\approx {\stackrel{.}{\mathit{x}}}_d\left(t\right)$ in (51).

With these simple assumptions—essentially, they correspond to assuming the UFIC is correctly working—the estimated initial impedance tank energy becomes

$E_{T,i}={\int }_0^t\left({\left|{\stackrel{.}{\mathit{x}}}_d\left(\sigma \right)\right|}^T{\mathit{d}}_c\left({\mathit{F}}_d\left(\sigma \right)\right)+{\stackrel{.}{\mathit{x}}}_d^T\left(\sigma \right)\left({\mathit{d}}_v°{\stackrel{.}{\mathit{x}}}_d\left(\sigma \right)\right)\right)d\sigma ,$

(53)

requiring only knowing the surface frictional characteristics d _v and μ , respectively.

Obviously, the initial energy estimations (48) and (53) have to be tailored to the specific UFIC task at hand, that is, the surface class the robot is dealing with and the desired motion. Table 3 (last column) enumerates the process reaction wrenches according to the described estimation assumptions for all surface classes expressed in the local contact coordinates. The elements of the process wrench F _p may be categorized into two groups as follows. The first group consists of the ones which are generated as the consequence of the force control. They are typically reciprocal to the impedance control desired velocity. Their magnitudes are equal to the desired wrench (assumption 1). The second group involves the elements generated due to the impedance controller desired velocity. They represent the non-zero elements of the surface process wrench F _p in the direction of the contact surface velocity ${\stackrel{.}{\mathit{x}}}_w$ equal to the desired twist (assumption 3). They are functions of the surface normal wrenches which are assumed to be equal to the desired wrench of the force controller (assumption 2). As a result, considering Table 3, the friction wrench F _frc in (51) has three dimensions for surface class 2A, two dimensions for classes 1 and 2B, and no dimension for class 2C.

5.3. Effect of sensory errors on tank performance

So far, we have assumed access to accurate sensed or observed signals like F _ext(t) and x (t) for our tank designs. However, if these signals are inaccurate, this stability may not hold. Consider a scenario where ${\overline{\mathit{F}}}_{\text{ext}}$ differs from F _ext. One immediate consequence would be an ineffective force control output, F _f . This alone would not impact the tank’s performance. Regardless of the accuracy of ${\overline{\mathit{F}}}_{\text{ext}}$ , the force control tank can step in when F _f (an accessible value) initiates motion, and it potentially deactivates force control to maintain stability. This, however, does not apply to the impedance control tank, which relies on the accuracy of ${\overline{\mathit{F}}}_{\text{ext}}$ . If ${\overline{\mathit{F}}}_{\text{ext}}$ is inaccurate, the system may fail to ensure stability. For example, considering the tank dynamics (29), if there is no F _f (i.e., in the absence of force control in the direction of the desired motion) and despite a non-zero external wrench F _ext, the sensed value ${\overline{\mathit{F}}}_{\text{ext}}$ is zero, the impedance control tank would not activate as the associated power ${\stackrel{.}{\mathit{x}}}_d^T{\overline{\mathit{F}}}_{\text{ext}}$ is zero. However, in reality, ${\stackrel{.}{\mathit{x}}}_d^T{\mathit{F}}_{\text{ext}}$ is non-zero, which could lead to instability in the overall system. One way to ensure that stability holds even in the presence of sensory inaccuracy is to ensure that the energy associated with such errors is limited. In other words, one should ensure that a passive system can be associated with sensory inaccuracies.

6. Contact-loss stabilization

A typical necessary, though not sufficient condition for contact loss is unwanted motions in force control directions. ⁷ Dealing with such behavior in the UFIC framework could in principle, be done with the energy tank design if, at the moment of contact loss, the remaining force control tank energy would be sufficiently low to cause a halt before a desired threshold travel distance. However, in reality, this is complex and impractical, given the many objectives the tank has to fulfill simultaneously and the fact everything needs to be designed in the energy domain.

Therefore, we employ a Supplemental state-based control policy alongside the derived energy tanks. This ensures that the energy in the force control tank is exclusively utilized according to the tank initialization procedure and handles abrupt contact-loss scenarios extremely well.

6.1. Controller shaping function

Our solution to the contact-loss problem centers around the basic principle of UFIC, namely, the full integration of impedance control and force control into a unified framework. Specifically, the underlying idea is as simple as it turns out to be effective: Valid contact corresponds to the Cartesian impedance deflection vector $\stackrel{\sim }{\mathit{x}}$ and the desired wrench F _d of the force controller being collinear. The extended controller design starts with the function-valued shaping vector

$\mathit{\rho }:={\left[{\rho }_{t,x}\left(△p_x\right)\hspace{0.17em}{\rho }_{t,y}\left(△p_y\right)\hspace{0.17em}{\rho }_{t,z}\left(△p_z\right)\hspace{0.17em}{\rho }_r\left(△\phi \right){\mathbf{1}}_{1\times 3}\right]}^T,$

(54)

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

${\rho }_{t,x}\left(△p_x\right):=\{\begin{array}{ll}1& \text{if}\hspace{0.17em}{\mathit{f}}_d^T△\mathit{p}\ge 0\\ \frac{1}{2}\left[1+\cos \left(\frac{△p_x}{d_{\max }}\pi \right)\right]& \text{if}\hspace{0.17em}{\mathit{f}}_d^T△\mathit{p}<0\\ & \wedge \Vert △p_x\Vert \in [0,\\ & \hspace{1em}\hspace{1em}\hspace{1em}{d}_{\max }]\\ 0& \text{else}\end{array}$

(55)

and the synchronized rotational part ρ _r (ψ) is

${\rho }_r\left(△\phi \right):=\{\begin{array}{ll}1& \text{if}\hspace{0.17em}{\mathit{m}}_d^T△k_0△{\mathit{k}}_v\ge 0\\ \frac{1}{2}\left[1+\cos \left(\frac{△\phi }{{\phi }_{\max }}\pi \right)\right]& \text{if}\hspace{0.17em}{\mathit{m}}_d^T△k_0△{\mathit{k}}_v<0\\ & \wedge △\phi \in \left[0,{\psi }_{\max }\right]\\ 0& \text{else}.\end{array}$

(56)

The shaping function ρ_t,x (△p _x ) for the translational case is constructed such that the force part of the controller is active when the desired force f _d and the vector of the current position to the setpoint △ p = p _d − p are collinear up to a robustness region d_max, see Figure 3 (upper row). The user may choose d_max depending on the task or application.

Figure 3.

Left column: Impedance controlled robot with robustness region d_max for translation (upper) or ψ_max for rotation (lower). Right column: Controller shaping function for the translational (upper) case and rotational case (lower).

Similarly, the rotation controller shaping function ρ _r (△φ) is constructed. To avoid singularities ρ _r (△φ) is based on quaternions, see also Sec. 5. The rotation error is defined as △ k ≔ k ⁻¹ k _d and $△\phi :=2\hspace{0.17em}\arccos \left(△k_0\right)$ . The user-defined rotational robustness region can then be specified as an angle ψ_max, which relates to the scalar component of the quaternion by ${\psi }_{\max }=2\hspace{0.17em}\arccos \left(k_{0,\max }\right)$ , see Figure 3 (lower row).

From a stability point of view, since the shaping function only scales the force controller part of the combined force-impedance controller, (28) can simply be redefined as

$\begin{array}{ll}{\mathit{F}}_f^{\prime }& =\left({\gamma }_f+{\alpha }_f\left(1-{\gamma }_f\right)\right)\hspace{0.17em}{{\mathit{F}}_f}^{″}\\ & =\left({\gamma }_f+{\alpha }_f\left(1-{\gamma }_f\right)\right)\hspace{0.17em}\mathit{\rho }°{\mathit{F}}_f,\end{array}$

(57)

thus, again, stability is ensured. The multiplication of ρ is understood component-wise, and the passivity analysis for the system with energy tank as outlined in Sec. 4 can be carried out in a similar manner. The block diagram of the overall UFIC-controlled system is depicted in Figure 4 for the rigid and flexible joint case.

Figure 4.

Block diagram of the UFIC, robot system (rigid-body and flexible-joint), and passive environment.

6.2. Discussion on soft-material

Overall, the proposed contact-loss shaping avoids abrupt and unsafe behavior by systematically coupling the advantages of force and impedance control. The robustness regions give the user also an intuitively interpretable design variable to shape the overall behavior of the task at hand.

Note that during tracking the desired force in contact with soft material, the robot might pass the impedance controller set point, although it is still in contact. As a result, the shaping function would decrease the effect of the force controller. This shows that not only the energy of the tank should be initialized and adapted, but also d_max and ψ_max. The design and learning process obviously would need to properly reflect the rigidity and damping of the environment so that the shaping function does not unnecessarily stop the force controller before contact loss. This aspect, however, is beyond the current work and needs future analysis.

7. Experiments

For experimental validation, we conduct six experiments A-F with the Franka Emika robot (Haddadin et al., 2022). The built-in external force F _ext measurement of the system’s 1 kHz Fast Control Interface (FCI) is used in our experimental implementation. Experiments 1-5 and 7 serve to analyze the force control performance, while experiment 6 investigates the moment control performance. For the former, a wool-covered 3D-printed plate is used to perform polishing, and for the latter, a metal file is mounted to the end-effector, see Figure 5. The used controller parameters can be found in Table 4. It is important to highlight that, for the force control law (4), we did not use the feedforward term (i.e., F _FF = 0). Additionally, the elements of F _f corresponding to the directions in which we do not intend to control the wrench are multiplied by zero. Cartesian damping D _C is designed such that critical damping is achieved (Albu-Schäffer et al., 2003).

Figure 5.

Experimental setups with Franka Emika Robot.

Table 4.

Experimental gains and parameters.

	f d,z	m d,y	K C	K p	K d	K i	T l,f	T u,f	T l,i	T u,i
	(N)	(Nm)	[N/m, Nm/rad]		(s)	[1/s]	(J)	(J)	(J)	(J)
Exp. 1	25	-	diag{3000,3000,100,300,300,300}	1I6×6	0I6×6	2.5I6×6	0.01	20	0.01	20
Exp. 2	30	-	diag{3000,3000,100,300,300,300}	1I6×6	0I6×6	2.5I6×6	0.01	20	0.01	20
Exp. 3	var	-	diag{3000,3000,100,300,300,300}	1I6×6	0I6×6	2.5I6×6	0.01	20	0.01	20
Exp. 4	15	-	diag{3000, 3000,10,0,50,300}	1I6×6	0I6×6	4I6×6	0.01	20	0.01	20
Exp. 5	25	-	diag{3000,3000,10,0,50,300}	1I6×6	0I6×6	4I6×6	5	20	0.01	20
Exp. 6	-	−1	diag{3000,3000,3000,300,0,300}	1I6×6	0I6×6	2.5I6×6	0.01	20	0.01	20
Exp. 7	12	-	diag{500,500,0,80,80,80}	1I6×6	0I6×6	0I6×6	0.01	20	0.01	20

7.1. Experiment 1: Controlling contact and motion—polishing

In the first experiment, the task is to control a normal force and execute orthogonal motion, exemplified by polishing a table. In Phase 1, contact is established via plain impedance control. Then, in Phase 2, UFIC control is activated to apply a specific desired force, and in Phase 3, circular motions on the table plane are commanded. Figure 6 depicts the force error in z-direction, the respective energies of the impedance control tank T _i and force control tank T _f , as well as x- and y-position of end-effector. Obviously, due to the low effective velocity in z-direction, the change in force control tank energy is negligible. Taking into account the tank dynamics (29), since the desired wrench and motion spaces are reciprocal, the force control output does not have an impact on the energy level of the impedance control tank. The remaining power values influencing the tank are $P_1:={\mathit{F}}_{\text{ext}}^T{\stackrel{.}{\mathit{x}}}_d$ and $P_2:={\stackrel{.}{\stackrel{\sim }{\mathit{x}}}}^T{\mathit{D}}_C\stackrel{.}{\stackrel{\sim }{\mathit{x}}}$ . Due to surface friction causing an opposing wrench to the desired velocity, P₁ leads to a decrease in the tank’s energy. Conversely, the quadratic nature of P₂ contributes to a continuous increase in the tank’s energy. Given that the influence of the controller’s critical damping is more substantial compared to the surface friction, ultimately, P₂ will exert a more dominant effect on changes in the tank’s energy level. Therefore, the power related to the damping of impedance control, in turn, fills the impedance control tank during the polishing motion in Phase 3a. When the tank energy reaches the upper limit, the limit key β _i is set to zero. This, however, does not change the behavior of the controller, as can be seen in Phase 3b. Overall, the task is executed accurately and robustly. Also, during the dynamic polishing motion, the force control error remains $\approx 1$ N. It is worth noting that if the power P₂ were absent, successful task performance would have required ensuring that the tank’s energy exceeded the energy consumed by power P₁. Given $P_1={\mathit{F}}_{\text{ext}}^T{\stackrel{.}{\mathit{x}}}_d$ and our prior knowledge of ${\stackrel{.}{\mathit{x}}}_d$ before the experiment, we could estimate F _ext for such energy calculation. As extensively discussed in Sec. 5, this estimation could be achieved by modeling the surface friction and coefficients. The accuracy of our model would directly impact the precision of our energy drainage estimate. However, due to inherent modeling errors, a practical approach would be to ensure an energy surplus beyond the predicted amount. In the subsequent sections, we will demonstrate an experiment illustrating the consequences of insufficient energy in the tank.

Figure 6.

Experiment 1: Force error, force, and impedance control tank energy and end-effector position for polishing task with different phases: Establishing contact (Phase 1), applying force (Phase 2), simultaneous polishing (Phase 3a), impedance control tank fill up (Phase 3b). Tank limits are indicated by green and red dotted lines.

7.2. Experiment 2: Robustness—losing contact

To show how unexpected contact loss is handled as described in Sec. 6, the end-effector is placed on the table and in Phase A is controlled to apply the same desired force as in the previous experiment. Simultaneously, a constant velocity is commanded in x-direction, inducing a movement towards the edge of the table until contact is lost. Figure 7 depicts different phases of the experiment, and Figure 8 shows the force error, the energy level of the force control tank T _f , shaping function ρ_t,z according to (55), and the end-effector position z.

Figure 7.

Experiment 2: Phases of contact-loss experiment.

Figure 8.

Experiment 2: Movement in x-direction (Phase A), movement beyond table edge (Phase B).

The experiment is done for two parameterizations. In the first case, the shaping function is activated with d_max = 0.05 m. As can be seen in Phase B immediately after the contact is lost, ρ_t,z smoothly decreases from 1 to 0. In turn, despite the force error ramps up to $\approx 25$ N, the system stabilizes, and the end-effector position z comes to a halt. This small movement in the direction of the desired force leads to a small energy drop for the force controller tank. In the second case, no shaping function is present, and in Phase B due to the high acceleration of the end-effector in the direction of commanded force, the tank energy depletes to the lower level while the motion is monotonically decreasing, that is, the behavior is undesired and dangerous. Clearly, the shaping function is able to mitigate one of the most challenging problems in force control with a programmable safety barrier encoding.

7.3. Experiment 3: Force tracking—bandwidth

To experimentally evaluate the force tracking bandwidth, a sinusoidal wave with increasing frequency is used as a desired force while keeping the impedance controller’s setpoint constant. Figure 9 (upper) shows the tracking behavior for half a period of a sine wave at different frequencies. This time-domain plot is constructed from the concatenation of results from different experiments after some periods for each experiment. Figure 9 (lower) shows the mean absolute error measured for different frequencies with and without considering the phase lag. Even high tracking accuracy can be achieved up to 3.2 Hz. For higher frequencies beyond the mechanical system bandwidth, the phase lag starts causing significant tracking errors.

Figure 9.

Experiment 3: Force tracking performance at various frequencies, considering the time lag between the actual and desired values.

7.4. Experiment 4: Controlling contact and motion under heavy disturbance—aluminum seesaw

In order to show how a robot under UFIC control can robustly adapt to an unknown environment even under unexpected heavy disturbances and contact loss, a seesaw setup is used, see Figure 10. The environment is neither modeled nor do we use a vision system or other external sensors to monitor the seesaw state. As in experiment A, the task goal is to polish a surface at a constant normal force; however, in this experiment, the surface is not always horizontal. To tackle such disturbance, while the impedance control task is defined in the world frame (e.g., attached to the table), the force control task is defined in a coordinate frame aligned with the robot’s end-effector. Additionally, the rotational stiffness of the impedance control is set to zero (see Table 4) to let the end-effector—and thus the polishing plate—passively align with the surface. As a result, while the impedance controller moves the robot along the table, the force controller applies a perpendicular wrench to the seesaw surface. Please note that as explained in Sec. 4.5, such coordinates misalignment between the force and motion control does not violate the stability proof.

Figure 10.

Experiment 4: Phases of the seesaw experiment. Positive and negative signs indicate velocity signs in y-direction. The seesaw can freely rotate at low friction around its lateral axis.

The experiment proceeds as follows, see Figure 10. The robot starts from an initial position ① and initiates sinusoidal polishing in x-direction and linear motion in y-direction. The desired setpoint depth is initiated slightly below the table surface. During the motion process ②, the end-effector pose adapts quickly to the inclination present in the geometry of the seesaw by regulating the desired moment to m _d = 0 Nm. After crossing the unknown natural equilibrium point of the seesaw and respective force threshold, the seesaw tilts abruptly ③ and the robot needs to regulate the force and moment again. Due to a missing counter force as well as shock impact, this results in a momentary force deviation, which is eliminated quickly after the contact is established and the shock impact dynamics are rejected. As already observed during phase ②, the robot end-effector adapts to the inclination of the seesaw ④. The according measurements are shown in Figure 11. Analog to experiment A, small motion artifacts in the force regulation occur while polishing. Since the sinusoidal trajectory is only commanded in x-direction, the magnitude of these errors is smaller as compared to experiment A.

Figure 11.

Experiment 4: Force error, impedance and force control tank energy, and end-effector position.

7.5. Experiment 5: The influence of energy tank limits

To show the effect of the tank with not enough energy, the previous experiment is performed multiple times in a row in a more dynamic setting. First, the lower tank limit is increased from 0 J to 5 J. Second, the desired velocity in y-direction is higher than before, and third, a higher force is commanded such that more energy is drained during the experiment. Looking at Figure 12 it can be seen that during the third iteration (i.e., ③) at t ≈ 23 s, the force controller tank energy is drained. This leads α _f to switch from 1 to 0. As a result, the force controller is deactivated, and the force error reaches 25 N. In practice, when force control is applied without a solid contact, unintended robot motion in the direction of the desired wrench can occur. Through tank augmentation, we constrain the associated energy and thus this potentially hazardous motion. This is particularly crucial in scenarios involving contact loss, where failing to restrict the resulting robot’s acceleration could lead to an unsafe impact. The same rationale applies to the impedance control tank, as penetration of the desired trajectory into a contact may generate a significant interaction wrench, posing a potential hazard to both the robot and its environment. As demonstrated in this experiment, the tank effectively prevents such occurrences.

Figure 12.

Experiment 5: Force error, force controller tank energy, and lower limit key as well as end-effector position in z-direction.

7.6. Experiment 6: Metal filing

The final experiment focuses on the regulation of rotational moments. For this, a metal filing application serves as a real-world evaluation task with the desired moment of m _d = 1 Nm, see Figure 13(a). Simultaneously, a linear motion in y-direction and orthogonal sinusoidal motion along the z-direction are commanded, see Figure 13(b). Thus the robot—while regulating the desired moment—moves toward the edge of the metal piece. Again, the two cases with (a threshold of 2°) and without an activated shaping function are considered. After losing contact in the first case the shaping function smoothly deactivates the force control part. For the second case, the robot accelerates further around the y-axis and again causes an undesired and dangerous situation, see Figure 13(c) and (d). For both experiments, the moment is almost perfectly regulated during phase (i), see Figure 14. During the quite rapid filing process phase (ii) the moment error increases due to dynamic process forces. As the motion in the direction of desired moment is negligible, the tank energy does not change significantly. When contact is lost in phase (iii) the moment error reaches −1 Nm for both cases. While the energy level with the shaping function remains almost constant, energy is constantly drained without the shaping function. The effect of the shaping function can also be observed from the orientation plot as the observed oscillations are caused by impacts from the motion in z-direction.

Figure 13.

Experiment 6: Phases of filing experiment.

Figure 14.

Experiment 6: Force error, force controller tank energy, shaping function, and end-effector orientation around y-direction.

To sum up, the UFIC control framework allows for robust, safe, and accurate simultaneous control of forces, moments, and motion, allowing even extremely challenging contact manipulations such as filing or polishing under heavy process disturbances. Noticeably, the UFIC framework is fully integrated into the Franka Emika robots.

7.7. Experiment 7: Key insertion

In this experiment, the UFIC framework is utilized for a peg-in-hole task, specifically focusing on inserting a key into a keyhole (refer to Figure 15). The experimental setup begins with the robot positioned such that the key tip is in contact with the keyhole, yet the orientation of the end-effector remains heavily misaligned (Figure 15(a)). Consequently, the desired motion is configured to maintain a stationary position while continuously adjusting its orientation (Figure 15(b)). To ascertain the appropriate orientation, the output of the shaping function is consistently monitored. When this output begins to decrease, indicating alignment, the setpoint orientation is fixed. Throughout the experiment, force control is activated with a target force of 12 N in the end-effector’s z-direction. The stiffness of the impedance control in this direction is set to 0 N/m. Upon achieving alignment of the end-effector orientation with the keyhole, the force control is engaged to guide the key into the hole (Figure 15(c)). Figure 16 illustrates the variations in each variable throughout the experiment. Note that this is just one of many solutions one could generate with UFIC. Other strategies could involve active force patterns or combinations of both, leading to poking type behaviors.

Figure 15.

Experiment 7: Desired orientation is continuously adjusted until the key successfully enters the keyhole.

Figure 16.

Experiment 7: Interaction force, end-effector orientation around x-direction, shaping function, and end-effector position.

8. Conclusion

Due to limitations in performance, safety, and robustness, existing force control solutions still have not become a commodity control methodology in the real world. In this paper, we introduced a complete passivity-based framework for Cartesian force-impedance control with integrated contact-loss stabilization. To the best of the authors’ knowledge, this is the first industrially available framework under widespread use.

Specifically, we allow both rigid-body and flexible-joint robots to (i.) accurately and robustly regulate and track interaction forces, while (ii.) safely cope with unforeseen contact discontinuities.

The former is achieved by integrating enhanced force and impedance control algorithms with two distinct energy tanks. As the practical performance of the controller significantly depends on the specific energy level of the virtual tanks, energy initialization approaches for both tanks are introduced. These model-based approaches could be integrated with our previously proposed tank-based energy learning methods Shahriari et al. (2019) in order to address potential model inaccuracies. In our forthcoming work, we will concentrate on the practical implementation and evaluation of these aspects.

The latter ability is achieved by a position-based control shaping function that is parameterized with task-specific knowledge such as expected surface curvature or allowed tolerances. This way, no unwanted rapid motions due to contact loss may occur without deteriorating the control performance unnecessarily.

In summary, our solution is able to overcome many of the limitations and relevant problems that were still predominant in classical force and impedance control. For validation purposes, a series of specific experiments were carried out with a state-of-the-art joint torque-controlled tactile robot. Most importantly, even for time-varying and unforeseen environmental changes, our solution could achieve high levels of performance and robustness for complex force regulation and tracking problems under heavy disturbances. Noticeably, already numerous real-world industrial manufacturing and logistics cases were successfully solved with our framework. We also believe that space, health, and domestic applications will largely benefit from this work. An important step to be taken soon is the integration with suitable domain-driven (force-motion)-policy algorithms. This would ultimately bring tactile robotics and policy learning closer together, forming a powerful solution to learning and executing complex manipulation and interaction tasks. Extensions to the UFIC framework may also involve collaborative tasks, as demonstrated in our first works on multi-manual object lifting/carrying (Shahriari et al., 2022). Finally, note that the developed unification does not require a robot dynamics model. These are needed only for the impedance controller. As there exists a significant body of work on passivity-based impedance control, we refer the reader to the well-known works (Albu-Schäffer et al., 2007; Ott et al., 2008), emphasizing also the necessity of accurate dynamics models for high performance. In the UFIC framework, the unknown parts of the robot dynamics model may be assigned to a finite—yet undisclosed—amount of energy. Consequently, while such uncertainty definitely affects the achievable performance in an undesirable way, the overall system may still be proven to be passive, given the presented methodology.

Supplemental Material