Grasped Object Weight Compensation in Reference to Impedance Controlled Robots

Abstract

This paper addresses the problem of grasped object weight compensation in the one-handed manipulation of impedance controlled robots. In an exemplary identification procedure, the weight of an object and its centre of mass together with gripper kinematic configuration are identified. The procedure is based on the measurements from a 6-axis force/torque sensor mounted near the gripper. The proposed method reduces trajectory tracking errors coming from the model imprecision without compromising the main advantages of impedance control. The whole approach is applied according to the embodied agent paradigm and verified on the two-arm service robot both in simulation and on hardware. Due to the general description that follows system engineering standards, the method can be easily modified or applied to similar systems.

1. Introduction

Service robots have become more advanced, but there is still much to accomplish if one compares their abilities with those of humans. An example of a very significant service robot application is the manipulation of a grasped object, which is a typical task for everyday human life. Object manipulation is well known in robotics, even for robots equipped with many effectors [1]. In general, this assignment is preceded by grasping an object. It is worth noticing that a good deal of recent research works concerns this issue, in particular, in grasp planning [2] and learning [3]. The kinematic and dynamic properties of a manipulator’s kinematic chain change when an object is grasped. This should be taken into account during the manipulator’s controller development. Typically, for stiff position control in the manipulator’s joints, the integral part of the position controller deals with the change in plant properties caused by object grasping. The modern approaches to service robots’ control lead to changes in that paradigm.

Although it was previously introduced, position-force control is still being improved [4,5]; most promising seems to be VSA (variable stiffness actuators), e.g., [6], or impedance control, especially in the case of a robot’s end-effector contact with the environment and transition from contact-free motion to contact phase [7,8]. Such types of control are biologically inspired [9]. The compliance and damping are substantial to keep the system stable while the manipulator is in contact with the environment and transitions from contact-free motion to contact with obstacles. The main advantage of impedance control that addresses the current economic and ecological demands is its susceptibility to energy consumption optimisation [10], as, in its nature, the constraining of torques exerted in the joints prevails over the exact trajectory tracking. Impedance control algorithms are based on the computed torque method and are sensitive to the inaccuracy of a robot’s model. If a robot control law ignores the inertial properties of a grasped object, then presumably, undesirable position errors of the robot’s joints or end-effectors may be large. These errors should not be eliminated by applying integral action. Impedance control has no integrals by definition since, with an integral, a contact force would increase, and this goes against the essence of impedance control. Instead, the accuracy of the model parameters should be improved and the control should rely on the compliance [11], which is inherent in impedance control.

Our goal was to propose a model and its parameters identification procedure for a gripper and grasped object that can be effectively analysed and integrated into the control system of impedance controlled robots. To achieve this, the problem analysis and system description fulfil the system engineering standards and are based on SysML [12,13] and its application to cyber–physical systems modelling, i.e., EARL [14] version 1.2 [15]. EARL was recently used to develop systems described in, for example, [16,17]. It is based on the embodied agent theory [18] applied for different systems, especially robotics systems with varying structure [19], or with harmonised tasks [20] as well as to describe and analyse general cyber–physical structures [21] or motion control systems based on artificial intelligence methods [22]. The exemplary identification procedure takes into account the kinematic configuration of the gripper without the need for the gripper model. It addresses this model’s lack because it can be unavailable for the grippers with, for example, specific and customised structures.

The remaining part of the article is organised as follows. Section 2 starts from control system use-case analyses. Then, control system structure and behaviour descriptions are presented, emphasising impedance control laws in operational and joint space and their extension that includes object weight compensation and gripper kinematic configuration. The convenient procedure for the grasping model parameter identification is presented in Section 3. Then, Section 4 describes the experiments performed with Velma https://www.robotyka.ia.pw.edu.pl/robots/velma accessed on 10 October 2021, a custom-built two-arm service robot. Finally, Section 5 summarises the work.

2. Control System

A robot with a grasped object is modelled as an open kinematic-chain mechanism with n rigid joints. This work presents the method of adjusting the EARL based robot model so that the object’s parameters are included in the robot control law. Additionally, the experiment results connected with gravity compensation are preferred to be similar to the control without any object grasped.

The control system development and analysis starts with general use cases (Figure 1).

The variety of applications demands two modes of impedance control [23] (UC01, UC02), gripper fingers control (UC03) and tool parameters setting (UC04), and finally, the identification of the grasped object parameters (UC05), which is the main contribution of this work.

The above requirements can be satisfied with the control system of the general structure depicted in Figure 2.

The system consists of two main parts:

• task/ga: Group of Agents with the specific task implemented.
• core/a: Agent that realizes the core behaviours that are demanded from the point of view of the wide range of applications formulated in task/ga.

Agent core/a provides communications channels for the Group of Agents task/ga. Robot arms (UC01, 02, 04, 05) and grippers (UC03, 05) are controlled through separate channels to ease controller reconfiguration with the changes of hardware (e.g., change of gripper type). In the opposite direction, core/a informs about the arms’ state, gripper state and control subsystem core/a.cs state, which includes wrist wrench measurements. The other part of core/a is the group of subsystems armsGrippers/igs responsible for low-level control and sensor data acquisition from all effectors and receptors.

The use cases depicted in Figure 1 are allocated by primitive transition functions of Agent core/a (Figure 3).

Those primitive transition functions should be associated with basic behaviours that will use them. It is enough to distinguish two basic behaviours as it is presented in Figure 4.

In EARL, the basic behaviours are associated with the finite state machine, which states the general behaviour of the subsystem (Figure 5).

In the following sections, the primitive transition functions core/a.cs.pf are presented, which relate to an impedance control. These functions are part of complex functions formulated to realize the joint space move Figure 6a (UC01) and operational space move Figure 6b (UC02).

2.1. Primitive Transition Function core/a.cs.jointMove/pf

Impedance control law, both in joint and operational spaces, may be described in a hierarchical manner [24,25,26]. In joint space, it can be formulated according to [24] as Equation (1):

$${\mathbf{\tau }}_{imp}={\mathbf{\tau }}_{j\left(1\right)}+{\mathit{N}}_{j\left(1\right)}^{\mathrm{T}}{\mathbf{\tau }}_{j\left(2\right)}.$$

(1)

Herein ${\mathbf{\tau }}_{j\left(1\right)}$ and ${\mathbf{\tau }}_{j\left(2\right)}$ are the $n\times 1$ vectors of torques related to tasks $j\left(1\right)$ and $j\left(2\right)$, respectively, where numbers in parentheses indicate the priority of tasks. Matrix ${\mathit{N}}_{j\left(1\right)}$ denotes an $n\times n$ null space projection as Equation (2):

$${\mathit{N}}_{j\left(1\right)}={\left(\prod _{i=1}^{n_p}{\mathit{N}}_t\left(i\right)\right)}^{\mathrm{T}},$$

(2)

where the $n\times n$ matrices ${\mathit{N}}_t\left(i\right)$, $i=1,\dots ,n_p$ are constructed, using the continuous null space projection method [25]. The Jacobian inverses used in the calculations of ${\mathit{N}}_t\left(i\right)$ can be dynamically consistent [27] or not. As stated in [28], $n_p$ is the number of contact point pairs, as the task with higher priority determines self-collision avoidance in Equation (3):

$${\mathbf{\tau }}_{j\left(1\right)}=-{\left(\frac{\partial V_{coll}\left(\mathit{q}\right)}{\partial \mathit{q}}\right)}^{\mathrm{T}}-{\mathit{D}}_{diag}\dot{\mathit{q}},$$

(3)

where $V_{coll}\left(\mathit{q}\right)$ represents a repulsive potential field according to [29], and it is in terms of the potential energy [28]. Matrix ${\mathit{D}}_{diag}$ denotes a $n\times n$ diagonal damping, and $\mathit{q}$ and $\dot{\mathit{q}}$ denote the $n\times 1$ vectors of measured joint positions and velocities, respectively.

The task with lower priority concerns joint impedance control [23] and can be expressed as Equation (4):

$${\mathbf{\tau }}_{j\left(2\right)}={\mathit{K}}_j\Delta \mathit{q}-{\mathit{D}}_j\dot{\mathit{q}},$$

(4)

where ${\mathit{K}}_j$ is a $n\times n$ diagonal joint stiffness matrix, $\Delta \mathit{q}={\mathit{q}}_d-\mathit{q}$ is a $n\times 1$ difference vector of the desired and measured joint positions, and ${\mathit{D}}_j$ is a $n\times n$ joint damping matrix calculated using the double diagonalisation method [30].

2.2. Primitive Transition Function core/a.cs.operationalMove/pf

In operational space, the impedance control law can also be formulated hierarchically [24] as Equation (5):

$$\begin{array}{c}\hfill \begin{array}{c}\hfill {\mathbf{\tau }}_{imp}={\mathbf{\tau }}_{c\left(1\right)}+{\mathit{N}}_{c\left(1\right)}^{\mathrm{T}}\left({\mathbf{\tau }}_{c\left(2\right)}+{\mathit{N}}_{c\left(2\right)}^{\mathrm{T}}\left({\mathbf{\tau }}_{c\left(3\right)}+{\mathit{N}}_{c\left(3\right)}^{\mathrm{T}}{\mathbf{\tau }}_{c\left(4\right)}\right)\right),\end{array}\end{array}$$

(5)

where ${\mathbf{\tau }}_{c\left(1\right)}$, ${\mathbf{\tau }}_{c\left(2\right)}$, ${\mathbf{\tau }}_{c\left(3\right)}$, and ${\mathbf{\tau }}_{c\left(4\right)}$ are the $n\times 1$ vectors of torques related to tasks $c\left(i\right)$, $i=1,2,3,4$, where i stands for the priority of the task. Matrices ${\mathit{N}}_{c\left(1\right)}$, ${\mathit{N}}_{c\left(2\right)}$, and ${\mathit{N}}_{c\left(3\right)}$ denote the $n\times n$ null space projection.

Let $n_e$ be the number of robot’s arms, each of which has one effector. The task with the lowest priority is the Cartesian impedance control [23] shown as Equation (6):

$${\mathbf{\tau }}_{c\left(4\right)}={\mathit{J}}^{\mathrm{T}}\left({\mathit{K}}_c\Delta \mathbf{r}-{\mathbf{D}}_c(\mathit{q},\mathbf{\xi })\dot{\mathit{r}}\right),$$

(6)

where $\mathit{J}$ is a $6n_e\times n$ Jacobi matrix, ${\mathit{K}}_c$ is a $6n_e\times 6n_e$ diagonal Cartesian stiffness matrix, $\Delta \mathit{r}={\mathit{r}}_d-\mathit{r}$ is a $6n_e\times 1$ vector of virtual spring displacement between desired ${\mathit{r}}_d$ and actual $\mathit{r}$ positions, ${\mathit{D}}_c$ is a $6n_e\times 6n_e$ Cartesian damping matrix obtained from the double diagonalisation method [30], $\mathbf{\xi }$ is a $6n_e\times 1$ vector representing normalized damping along the main directions of the task frames, and $\dot{\mathit{r}}=\mathit{J}\dot{\mathit{q}}$ is a $6n_e\times 1$ vector of velocity. Each of the vectors $\Delta \mathit{r}$, ${\mathit{r}}_d$, $\mathit{r}$, and $\dot{\mathit{r}}$ has a block structure so that the first 6 coordinates concern the first effector, the next 6, the second effector, etc. Moreover, in each $6\times 1$ block of $\mathit{r}$ (analogously for $\Delta \mathit{r}$, ${\mathit{r}}_d$, and $\dot{\mathit{r}}$) the first $3\times 1$ vector denotes positions that are supplemented by a $3\times 1$ vector part of a unit quaternion. Thus, the matrices $\mathit{J}$, ${\mathit{K}}_c$, and ${\mathit{D}}_c$ must also have the appropriate block structure.

The task with higher priority than the previous one is related to vector ${\mathbf{\tau }}_{c\left(3\right)}$. This task determines joint limits avoidance [31] with additional damping applied according to the double diagonalisation method [30] with constant coefficients’ values in an auxiliary diagonal damping matrix ${\mathit{D}}_{\xi }$. The null space projection matrix ${\mathit{N}}_{c\left(3\right)}$, which relates featured tasks, $c\left(3\right)$ and $c\left(4\right)$, can be set as the identity matrix. Consequently, these tasks are treated with the same priority.

The task with the second-highest priority involves self-collision avoidance. Therefore, ${\mathbf{\tau }}_{c\left(2\right)}$ and ${\mathbf{N}}_{c\left(2\right)}$ may be calculated analogously to ${\mathbf{\tau }}_{j\left(1\right)}$ and ${\mathbf{N}}_{j\left(1\right)}$, respectively. Finally, the task with the highest priority determines the wrist collision constraint [26]. This task is similar to the former one, except that it affects only the two joints on each effector that are most distant from the robot’s base. Indeed, the main distinction between ${\mathbf{\tau }}_{c\left(1\right)}$ or ${\mathit{N}}_{c\left(1\right)}$ and ${\mathbf{\tau }}_{c\left(2\right)}$ or ${\mathit{N}}_{c\left(2\right)}$, respectively, is the lower number of contact point pairs [28] in the first task.

2.3. Primitive Transition Function core/a.cs.compensateObjectWeight/pf

The grasped object and gripper configuration gravitational term can be incorporated as a sum component into the equation for the robot with the grasped object. Therefore, the robot control law can be settled as Equation (7):

$${\mathbf{\tau }}_d={\mathbf{\tau }}_{imp}+{\mathbf{\tau }}_{ogc},$$

(7)

where a $n\times 1$ vector of desired torques ${\mathbf{\tau }}_d$ is the sum of a $n\times 1$ vector of joint torques ${\mathbf{\tau }}_{imp}$, calculated according to the joint or Cartesian impedance control law, and a $n\times 1$ vector of object gravity compensation torques ${\mathbf{\tau }}_{ogc}$. The origin of the approach from [32] is stated in [1], where robot systems are considered in terms of kinetic and potential energy.

Let i ($i\in \mathbb{N}$) be the index of the chosen robot’s joint. Then, the object weight compensation torque ${\mathbf{\tau }}_{ogc\left[i\right]}$ acting in joint i can be formulated as Equation (8):

$${\mathbf{\tau }}_{ogc\left[i\right]}=-{\left({\tilde{\mathit{r}}}_i{\mathit{f}}_g\right)}^{\mathrm{T}}{\widehat{\mathit{e}}}_i.$$

(8)

In this equation, the skew-symmetric matrix ${\tilde{\mathit{r}}}_i$ (Equation (9))

$${\tilde{\mathit{r}}}_i=\left[\begin{array}{ccc}0& -r_{i\left[z\right]}& r_{i\left[y\right]}\\ r_{i\left[z\right]}& 0& -r_{i\left[x\right]}\\ -r_{i\left[y\right]}& r_{i\left[x\right]}& 0\end{array}\right]$$

(9)

is associated with the vector ${\mathit{r}}_i={\left[\begin{array}{ccc}{r}_{i\left[x\right]}& r_{i\left[y\right]}& r_{i\left[z\right]}\end{array}\right]}^{\mathrm{T}}$ that denotes the object’s centre of mass position relative to joint i. The $3\times 1$ vector ${\mathit{f}}_g$ represents the object’s weight, and the $3\times 1$ unit vector ${\widehat{\mathit{e}}}_i$ defines the joint’s axis of rotation. The expression ${\mathbf{\tau }}_{g_i}={\tilde{\mathit{r}}}_i{\mathit{f}}_g$ is a cross product of ${\mathit{r}}_i$ and ${\mathit{f}}_g$, and it determines the $3\times 1$ vector of torque. Hence, the torque ${\mathbf{\tau }}_{ogc\left[i\right]}$ is a scalar product of $-{\mathbf{\tau }}_{g_i}$ and ${\widehat{\mathit{e}}}_i$.

Equation (8) assumes that ${\tilde{\mathit{r}}}_i$, ${\mathit{f}}_g$, and ${\widehat{\mathit{e}}}_i$ are expressed in the same coordinate system. However, it is more convenient to express quantities ${\tilde{\mathit{r}}}_i$ and ${\widehat{\mathit{e}}}_i$ in a local Cartesian coordinate system ${\pi }_i$, whereas ${\mathit{f}}_g$ should be rather expressed in the robot’s base Cartesian coordinate system ${\pi }_0$ ($O{x}_0{y}_0{z}_0$). The frame ${\pi }_i$ can be bound up with the link driven by the motor in joint i. Then, Equation (8) may be rewritten as Equation (10):

$${\mathbf{\tau }}_{ogc\left[i\right]}=-(({}_i^0\mathit{R}{}^{(i)}{\tilde{\mathit{r}}}_i{}_i^0{\mathit{R}}^{\mathrm{T}}){}^{(0)}{\mathit{f}}_g{)}^{\mathrm{T}}{}_i^0\mathit{R}{}^{\left(i\right)}{\widehat{\mathit{e}}}_i,$$

(10)

where ${}_i^0\mathit{R}{}^{\left(i\right)}{\tilde{\mathit{r}}}_i{}_i^0{\mathit{R}}^{\mathrm{T}}={}^{\left(0\right)}{\tilde{\mathit{r}}}_i$ and ${}_i^0\mathit{R}{}^{\left(i\right)}{\widehat{\mathit{e}}}_i={}^{\left(0\right)}{\widehat{\mathit{e}}}_i$. Herein, ${}_i^0\mathit{R}$ is the rotation matrix used to transform quantities from ${\pi }_i$ to ${\pi }_0$. The upper left index in the bracket, e.g., j of ${}^{\left(j\right)}\mathit{w}$, indicates that the quantity ${}^{\left(j\right)}\mathit{w}$ is expressed in the coordinate system ${\pi }_j$. Let m be the object’s mass, and g be the gravitational acceleration. Then, the force ${}^{\left(0\right)}{\mathit{f}}_g$ can be stated as Equation (11):

$${}^{\left(0\right)}{\mathit{f}}_g={\left[\begin{array}{ccc}0& 0& -mg\end{array}\right]}^{\mathrm{T}}.$$

(11)

The minus sign in $-mg$ is the result of the assumption that the $z_0$ axis is directed upwards and therefore against the force of gravity.

3. The Identification Procedure of the Object’s Parameters

To apply weight compensation of the object, group of agents task/ga needs to estimate the grasped object’s weight and centre of mass position and command it to agent core/a. Each robot’s manipulator (effector) is equipped with a force/torque sensor mounted between the wrist and the gripper. It is also assumed that the grasped object is rigid, and it cannot move relative to the gripper. Furthermore, the $6\times 1$ wrench measured by the force/torque sensor is established as Equation (12):

$$\mathit{F}={\left[\begin{array}{cccccc}{F}_{\left[x\right]}& F_{\left[y\right]}& F_{\left[z\right]}& F_{\left[Ox\right]}& F_{\left[Oy\right]}& F_{\left[Oz\right]}\end{array}\right]}^{\mathrm{T}},$$

(12)

where $F_{\left[x\right]}$, $F_{\left[y\right]}$, and $F_{\left[z\right]}$ denote components of the $3\times 1$ force in operational space, and $F_{\left[Ox\right]}$, $F_{\left[Oy\right]}$, and $F_{\left[Oz\right]}$ are components of the $3\times 1$ torque.

The procedure consists of the following phases (Figure 7 and Figure 8):

0.

The selected manipulator changes the position of its gripper in order to check the grasp configuration of the gripper’s fingers (Figure 9b). When the gripper tightens on the object, the measurement of the gripper’s joint positions is taken. The grip is released.

1.

The manipulator reaches the reference position for the first wrench measurement (Figure 9c). The gripper’s joint positions are set according to phase 0. The measurement is taken.

2.

The gripper rotates around the axis x of the frame ${\pi }_k$ by $-{90}^{°}$, and the second reference measurement is taken (Figure 9d).

3.

The manipulator grasps the object with its gripper and reaches the same position as in phase 1. The third measurement is taken (Figure 9e).

4.

The arm reaches the same position as in phase 2, and the fourth measurement is taken (Figure 9f). Identification parameters can be computed and transmitted into agent core/a.

The local coordinate frame ${\pi }_k$ (Figure 9a) may be bound up with the manipulator’s wrist.

The object’s weight w can be obtained using the formula of Equation (13):

$$w=mg=-({}^{\left(k\right)}{F}_{\left[z\right]}^3-{}^{\left(k\right)}{F}_{\left[z\right]}^1),$$

(13)

where the upper right index denotes the phase of the identification procedure when the measurement was made, and $\left(k\right)$ in the upper left indices is related to the coordinate frame ${\pi }_k$. It should be noted that due to numerical conditions for the very small values of w, the procedure should stop here, and the compensation factors remain unchanged. Otherwise, the position of the object’s mass centre relative to the force/torque sensor can be computed with Equations (14)–(16):

$$\begin{array}{cc}\hfill {}^{\left(k\right)}{r}_{\left[x\right]}& =({}^{\left(k\right)}{F}_{\left[Oy\right]}^3-{}^{\left(k\right)}{F}_{\left[Oy\right]}^1)/w,\hfill \end{array}$$

(14)

$$\begin{array}{cc}\hfill {}^{\left(k\right)}{r}_{\left[y\right]}& =-({}^{\left(k\right)}{F}_{\left[Ox\right]}^3-{}^{\left(k\right)}{F}_{\left[Ox\right]}^1)/w,\hfill \end{array}$$

(15)

$$\begin{array}{cc}\hfill {}^{\left(k\right)}{r}_{\left[z\right]}& =-({}^{\left(k\right)}{F}_{\left[Ox\right]}^4-{}^{\left(k\right)}{F}_{\left[Ox\right]}^2)/w,\hfill \end{array}$$

(16)

where ${}^{\left(k\right)}\mathit{r}={\left[\begin{array}{ccc}{}^{\left(k\right)}{r}_{\left[x\right]}& {}^{\left(k\right)}{r}_{\left[y\right]}& {}^{\left(k\right)}{r}_{\left[z\right]}\end{array}\right]}^{\mathrm{T}}$ is the position vector that has the direction from the force/torque sensor to the object’s mass centre. For each joint i to obtain the quantity ${}^{\left(0\right)}{\mathit{r}}_i$ from Equation (10), the vector ${}^{\left(k\right)}\mathit{r}$ has to be transformed with the robot kinematic model.

4. The Verification of the Grasped Object Weight Compensation Algorithm

The approach was verified using the Velma service robot simulator [33], and then on the real robot, in both cases with a universal controller implemented within the FABRIC [34] software framework. As the use of domain-specific frameworks is a convenient approach [35], the FABRIC bases on ROS [36] and OROCOS [37]. The Velma robot has already been used in many research works described, e.g., in [2,23,26].

4.1. The Specific Realisation of Impedance Control Law for the Velma Service Robot

In the Velma robot impedance control law, only its two arms and torso are included, because other robot parts are controlled separately. Thus, the vector of desired torques ${\mathbf{\tau }}_d$ from Equation (7) is represented as Equation (17):

$${\mathbf{\tau }}_d={\left[\begin{array}{c}{\mathbf{\tau }}_{torso}\\ {\mathbf{\tau }}_{rightLWR}\\ {\mathbf{\tau }}_{leftLWR}\end{array}\right]}_{15\times 1}.$$

(17)

In Equation (17), ${\mathbf{\tau }}_{torso}$ is the torso torque proportional to the current $i_{\mathbf{\tau }}$ flowing through the motor Equation (18):

$${\mathbf{\tau }}_{torso}=k_{\mathbf{\tau }}{i}_{\mathbf{\tau }},$$

(18)

where $k_{\mathbf{\tau }}$ is the proportionality factor. The torso controller is created using the technology presented in [38].

The vector ${\mathbf{\tau }}_d$ from Equation (17) also consists of the $7\times 1$ right arm torque vector ${\mathbf{\tau }}_{rightLWR}$ and the $7\times 1$ left arm torque vector ${\mathbf{\tau }}_{leftLWR}$. The arms of the Velma robot are 7-DOF redundant KUKA LWR4+ manipulators. Dynamic properties of these arms are supposed to be highly repeatable, but not identical [39]. However, the compensation algorithm described in Section 2.3 does not depend on the particular manipulator, so this issue does not affect the verification of the algorithm.

The Velma robot’s arms are internally torque-controlled [40] (Equation (19)):

$${\mathbf{\tau }}_{cmd}={\mathbf{\tau }}_{FRI}+{\mathit{f}}_{dynamics}(\mathit{q},\dot{\mathit{q}},\ddot{\mathit{q}}).$$

(19)

Impedance control is included in the calculation of the vector ${\mathbf{\tau }}_{FRI}$, which is specified by the KUKA Fast Research Interface (FRI) remote side. The vector ${\mathit{f}}_{dynamics}(\mathit{q},\dot{\mathit{q}},\ddot{\mathit{q}})$ denotes the manipulator’s dynamic model and is computed internally by the KUKA controller. The commanded torque vector is denoted as ${\mathbf{\tau }}_{cmd}$. For the Velma robot, the vector ${\mathbf{\tau }}_{FRI}$ is specified by the robot’s research controller and is identical with ${\mathbf{\tau }}_{rightLWR}$ or ${\mathbf{\tau }}_{leftLWR}$, depending on which manipulator is considered.

It should be noted that there are $n=15$ joints and $n_e=2$ arms (effectors) in the Velma robot control; hence, in Equation (5), the task with the highest priority is treated as two separate tasks, for the right and left arms.

4.2. Exemplary Results of Experiments

A test task concerned object manipulation. In simulation, the main aim was to move the object from one table to another with one of the Velma robot’s arms. For the purpose of the presentation, the right manipulator was chosen, but a similar outcome would be obtained with the left arm. The task was divided into three subsequent stages:

• The robot performed the identification procedure (Section 3, Figure 10a–e, starting from the convenient configuration (Figure 10a). According to phase 0 of the procedure, the robot checked the grasp configuration of the fingers of its right gripper (Figure 10b). Then, the robot took two reference wrench measurements (Figure 10c), and after that, it grasped the object (Figure 10d). Ultimately, the robot took two last wrench measurements (Figure 10e), and thanks to this, the desired parameters could be computed.
• The robot executed a sequence of the movements in Cartesian impedance control mode with linear interpolation (analogous to that described in [23]) between points (vertices) of a square (Figure 10f,g and Figure 11) lying in the plane perpendicular to the x-axis of the global coordinate system (Figure 10a).
• The robot put away the object on the other table (Figure 10h).

The test task was repeated two times for a 2 kg object. The first time, the object’s weight was not compensated, so the vector ${\mathbf{\tau }}_{ogc}$ from Equation (7) was arbitrarily set to zero, and there was no necessity to perform stage 1 of the task. The second time (https://vimeo.com/567801853, accessed on 10 October 2021), the vector ${\mathbf{\tau }}_{ogc}$ was computed in every control cycle, according to the compensation algorithm from Section 2.3, including the identified object’s parameters (Section 3). The computed weight Equation (13) was $w=19.6024$ N, and the mass centre position vector Equations (14)–(16) was ${}^{\left(k\right)}\mathit{r}={\left[\begin{array}{ccc}0.13065& -0.01121& -0.00837\end{array}\right]}^{\mathrm{T}}$ m. Hence, for $g=9.80665\frac{\mathrm{m}}{{\mathrm{s}}^2}$, the computed object’s mass was $m=w/g=1.9989\mathrm{kg}$.

Let $k_{rf}$ and $k_{rt}$ be the values of stiffness from the diagonal Cartesian stiffness matrix Equation (6), specified as Equation (20):

$${\mathit{K}}_c=diag(k_{rf},k_{rf},k_{rf},k_{rt},k_{rt},k_{rt},k_{lf},k_{lf},k_{lf},k_{lt},k_{lt},k_{lt}),$$

(20)

which are related to the right arm and concern components of the $3\times 1$ force and torque, respectively (analogously, $k_{lf}$ and $k_{lt}$ are related to the left arm). During the experiments, these values were set as $k_{rf}=500$ and $k_{rt}=100$, except the identification procedure (Section 3), when they were stated to be higher, i.e., $k_{rf}=1000$ and $k_{rt}=400$ for phases 1 and 2, and $k_{rf}=1500$ and $k_{rt}=1200$ for phases 3 and 4. Each element of the normalized damping vector $\mathbf{\xi }$ Equation (6) was set to $0.7$.

The selected results from stage 2 (Figure 10f,g) of the test task are presented in Figure 11. Only a part of the sequence of the movements is considered in reference to the z-axis and y-axis Cartesian desired positions (Figure 11a,d, respectively) of the right end-effector in the global coordinate frame (Figure 10a). During these movements, the x-axis Cartesian desired position was constant and equal to $0.7$ m.

In the first case (no compensation), the z-axis Cartesian position error (Figure 11b) between the desired and actual position was quite considerable, and it oscillated around a non-zero value. However, in the second case (with weight compensation), this error (Figure 11c) was smaller than previously, as it oscillated around zero and was comparable with the analogous one from experiments with a much lighter object (e.g., 100 times), where there was no need to utilize the mentioned compensation algorithm. In contrast to the z-axis error, the y-axis Cartesian position errors (Figure 11e,f) were almost identical in both cases because the described algorithm refers mainly to the z-axis static effects of the object’s dynamics.

After successful experiments in simulation, the experiments on the real robot (Figure 12) were performed.

The general test scenario was the same as in the simulation. The results were achieved for the number of test repetitions. The general nature of signals overlaps the simulation results and does not change through the subsequent test iterations (Figure 13).

The differences between the results in the simulation and those of reality result primarily from force/torque measurement inaccuracies. The experimental hardware tests confirm the operation of the control system as it was intended (https://vimeo.com/618941465, accessed on 10 October 2021).

5. Conclusions

In an extensive survey [41], authors distinguish three categories of estimation methods of inertial parameters of objects: purely visual, exploratory and fixed object. As the example, authors of [42] investigate the usability of simulation data to learn manipulation tasks in an end-to-end manner with a model-free RL algorithm and train the robot controller, especially in the occurrence of an unknown object being manipulated. The works [43,44,45] consider a similar robotic platform as in our tests, but the artificial neural networks together with fuzzy logic are employed to control the robot hardware. Our exemplary approach can be assigned to fixed methods, such as that in [46,47] and evolved from [48]. It was chosen due to its accuracy, needed by impedance control and correspondence to applications, where the grasp is the indispensable part. Here, we concentrate on static error elimination, as it is one of the main problems of impedance control, where the dynamic errors in contact or free-motion are irrevocable. The verification experiments confirm that the proposed object weight compensation algorithm is correct and fulfils the expectations, as it enables to reduce the z-axis position error. Such errors can cause unexpected collisions and lead to task execution repetition or failure, even for impedance controlled manipulators. It could be especially undesirable in systems, such as autonomous robots [49], where both the energy and time to execute a task are constrained. It should be noted that the method relies on precise measurements of a wrench that, in general, is not trivial to achieve, due to the influence of end-effector inertial parameters both in static [48] and dynamic cases [50,51,52]. A survey of inertia parameter identification methods is presented in [53].

In the article, the compensation method was incorporated into the unique and versatile system with two different impedance control laws. Additionally, the exemplary mass centre parameters identification procedure addresses the problem of an unknown gripper’s model. It is convenient, due to the task-specific gripper customisations. The resultant integrated controller is the base for recent work with the Velma robot. We believe that the system description that fulfils the current system engineering standards and utilises contemporary domain-specific graphical languages will make it easier to apply the results in similar works performed by other researchers.