<figure><img src="image_30.png" /><figcaption>**Figure 5.5** – Peg-in-Hole simulation: Feature’s depth estimation Vs Ground-Truth. Under sufficiently exciting trajectories, the filter is capable of reconstruct the depth’s dynamics. Once the camera lies on top of the target (around 1 s), the motion is not exciting enough to allow the estimates to further converge towards the true values.</figcaption></figure>

and initializing the filter with another depth could lead to failure execution.

To circumvent the persistency of excitation issue, we assume that the forthcoming IBVS experiments will include a "measured" of the features’ depth (extracted from the Aprittag’s pose). The measurement equations (5.28) are then augmented by the availability of the depth, being now

$$ 
\begin{aligned}
\mathbf{R}_{k+1} &= \begin{bmatrix} \mathbf{R}_s & \mathbf{O}_{8 \times 4} \\ \mathbf{O}_{4 \times 8} & \mathbf{R}_z \end{bmatrix} \\
\mathbf{C}_{k+1} &= \begin{bmatrix} \mathbb{I}_8 & \mathbf{O}_{8 \times 4} & \mathbf{O}_{8 \times 16} \\ \mathbf{O}_{4 \times 8} & \mathbb{I}_4 & \mathbf{O}_{4 \times 16} \end{bmatrix}
\end{aligned}
$$

in which $\mathbf{R}_z$ is the measurement noise covariance of the features depth.

We then repeated the same experiment for the PBVS and IBVS (with depth measurements) cases using the same controller (5.42), gains, and filter parameters of the previous experiment and their results are reported in Figures 5.6 and 5.7.

The choice of a constant acceleration model for the target motion is well evident from the plotted curves in Figure 5.8; note how the angular velocity about the y axis ($^c w_{oy}$) follows the target motion about the same direction ($f(t) = 0.2\sin(2t)$). In Figure 5.7c (bottom) it is possible to appreciate what looks like a small horizontal shift between the Ground-Truth and the estimated target’s pose. This is due to the crossing of the representation boundary at $\phi_i = \pi$ and not because of the time delay, as one could think; the Kalman filter helps mitigate the delay effects.

With the proposed process models, the filters were able to properly reconstruct the disturbance (target velocity), thus succeeding in completing the task. Apart from the very beginning of the simulation, which is affected by the transitory
<figure><img src="image_31.png" /><figcaption>**Figure 5.6** – IBVS Peg-in-Hole simulation: Initial (a) and final (b) camera views and robot configurations. It is possible to appreciate, in green, the features paths in the image plane and camera path in blue. (c) Estimated Vs Ground-Truth feature’s depth. (d) Estimated Vs Ground-Truth feature’s positions (top) and feature’s error (bottom).</figcaption></figure>

<figure><img src="image_32.png" /><figcaption>**Figure 5.7** – PBVS Peg-in-Hole simulation: Initial (left) and final (right) camera views and robot configurations. Features position (top) and orientation (bottom) errors. On the right image it is possible to appreciate, in green, the trajectory of the target corners in the image plane. Convergence is successfully attained.</figcaption></figure>

of the filter convergence and the effects the “smoothing” exponential ($\mu = 8$), the features trajectories shows a nice exponential decrease. Simulations suggest
<figure><img src="image_33.png" /><figcaption>**Figure 5.8** – Peg-in-Hole simulations: Estimated target's Linear (top) and angular (bottom) velocity in camera frame Vs Ground-Truth for both the IBVS (a) and PBVS (b) cases.</figcaption></figure>

that second-order visual-servoing controllers can be suitably used to deal with
interactions tasks. In the next section we deal with the real implementation of
such controllers.

5.7 Real experiments

An experiment for tracking a moving target on a turntable was performed on a Franka Emika Panda robot equipped with a wrist mounted RealSense D435 camera operating at 60 fps. Note that, no Force/Torque sensor is required to implement controller (5.42). In the following we report the results of the second-order visual servoing for the IBVS case, which is more complex than its PBVS counterpart given both the sensitivity with respect to the feature depths, as we shown before, and the difficulties related to the rank deficiency of the task Jacobian (more details below). The simulations and experiments for both the IBVS and PBVS cases presented in subsection 5.6.2 as well as some interaction experiments using the filters and controllers reported in this chapter can be found in the video reachable through this link https://www.youtube.com/watch?v=fOSRnxd1d1E.

5.7.1 Implementation issues

Most works have presented their results on second-order visual-servoing in simulation and tackled just few of the real world issues like measurement noise [46,47], or instead of generating the torque command from the features acceleration, a velocity signal command is obtained from numerical integration and sent to the robot, as in [78] for a MPC controller. In this work we aimed at pushing forward the state of the art by tackling several implementation aspects of a real implementation not covered in previous works.
<figure><img src="image_34.png" /><figcaption>**Figure 5.9** – Setup: Panda robot equipped with a RealSense D435 operating at 60 fps. The center of the target is located at 22 cm from the center of the turntable, which is manually operated.</figcaption></figure>

### 5.7.1.1 Data rate

The main bottleneck of a second-order visual-servoing controller implementation is certainly the low data rate coming from the vision sensor and the latency of the computer vision algorithms for features extraction. It is important to evaluate the performance of the vision system as the whole system controlling the robot. When considering the lowest level servo rate of the robot controller, i.e., the joint controller of the manipulator, it is generally accepted that the servo rate $T_s$ should staisfy the condition:

$$T_s < \frac{T_m}{10} \qquad (5.43)$$

where $1/T_m = f_m$ is the mechanical resonance frequency of the manipulator's structure [4] (see [3] for more details). It is therefore necessary that the low level robot's controller operates with a feedback rate of 200 ~ 1000 Hz, which means a cycle time of 1 ~ 5 ms since the mechanical resonance frequency of a manipulator usually ranges from 20 ~ 50 Hz. It is obvious that higher control frequencies are desirable at any case and that conventional visual systems alone are too slow for controlling a robotic system at torque level.

One of the main contributions of this chapter are the proposed EKFs which virtually increase the data rate coming from the camera and helps alleviate the negative effects of the computer vision latency, allowing us to effectively deal with the issue of the servo rate discussed above. If this problem is not addressed, it is practically impossible to implement such a control scheme without resorting to expensive high-speed cameras.
### 5.7.1.2 Ill-condition of the task Jacobian

The computation of the control command (5.42) requires to pseudo-invert the task Jacobian $J = J_s B^{-1} \in \mathbb{R}^{k \times n}$ which in practice turns out to be strongly ill-conditioned. This problem is due to the double effect of the ill-condition of the inertia matrix [79] which is then combined with the image Jacobian, resulting in a magnified singular values ratio. This issue is particularly pronounced when the disparity between the masses and inertia tensors of the individual links is large. Featherstone [80] has empirically found that the condition number of the inertia matrix can asymptotically grow with $O(n^4)$ of the number of bodies, which applies to our case since we are servoing a 7-joints manipulator.

In the simulations, we computed the control command by direct pseudo-inversion of the task Jacobian, through Singular Value Decomposition without observing any issue on the demanded torques, as one can see from the simulations on the video https://www.youtube.com/watch?v=fOSRnxd1d1E (min [0:06 - 0:55]). The task Jacobian is then

$$J = U \Sigma V^{\top} \qquad (5.44)$$

with $U \in \mathbb{R}^{k \times k}$, $\Sigma \in \mathbb{R}^{k \times n}$ and $V \in \mathbb{R}^{n \times n}$. $\Sigma$ is a rectangular diagonal matrix whose diagonal elements are the singular values. $U$ and $V$ are orthogonal matrices. For the real robot, using the same control gains as in simulation, demanded torques were unrealistic and incompatible with the joint torque limits, leading the robot to abruptly stop due to the high command values requested. Lowering down the control gains to obtain command values compatible with the joint torque limits, we were unable to properly servo the robot. We then realized that the task Jacobian was highly ill-conditioned.

A common approach in robotics to deal with ill-conditioned problems is to perform a regularization, which allows for a numerically stable solution. Damped Least Squares (DLS), also known as Levenberg-Marquardt method, is one of the most common regularization techniques for solving the inverse kinematic problem in the form

$$Ax = b \\ \min_{x} \quad \|Ax-b\|^2 + R\|x\|^2$$

where $R = diag(\lambda, \dots, \lambda)$ is a regularization diagonal matrix. In this case, finding the solution to the problem is a compromise between preserving the control requirements and maintaining the solution limited [77]. With this solution, the singular values of the pseudo-inverse change from $\frac{1}{\epsilon_i}$ to $\frac{\epsilon_i}{\epsilon_i+\lambda}$ (with $\epsilon_i$ the i-th singular value of the non-inverted matrix). However, we found that with a DLS-based
regularization, rotations about x- and y-axis were far less “stiff” than the other directions, leading to poor orientation tracking performance.

In order to adjust separately for each singular value the given damping factor, we used a SVD-based regularization with a Gaussian function of the singular value as regularization term, $g_i(\epsilon_i) = m \exp(-\frac{\epsilon_i^2}{2\sigma^2})$, where m is the height of the curve’s peak and σ controls the width of the “bell”. We set then $P = diag(g_1(\epsilon_i), ..., g_m(\epsilon_m))$, and compute the regularized solution as

$$x = V(\Sigma^\top\Sigma + P)^\dagger\Sigma^\top U^\top b.$$

### 5.7.1.3 Joints friction

In [7], the dynamic friction for our robot has been identified and we used the released library to compensate for it adding this torque contribution in controller (5.42). Unfortunately we do not have a good model for the dry/static friction, and as a result small torques are actuating the robot, limiting the positioning performance close to the goal position; we have found this threshold to be ~ 0.5 Nm for our robot. This phenomenon particularly affects the last joint since the controlled torques are often lower than this value. To alleviate this issue as already mentioned in Section 5.6.2, an integral term on the features error was added, allowing to reduce the remaining steady state error. Further increasing the proportional gain could in fact lead to an unstable behaviour.

### 5.7.1.4 Illumination conditions

Despite evident when working with vision sensors, experiments are often conducted under poor illumination conditions with consequent performance degradation. In our case, it is of paramount importance to guarantee and maintain a continuous and constant image stream to avoid that the data fusion of delayed visual measurements leads to catastrophic results. A good illuminated environment stabilizes the feature extraction with ViSP to ~ 8 ms while in regular conditions it can oscillate from 8 to 20 ms. Spotlights were then placed around the experimental platform also trying to avoid obscuring the target or blinding the camera with reflections.

## 5.7.2 Results

The proposed EKF proves to be effective both in increasing the data rate of the camera, providing an accurate estimate of the features (and depth) at higher
frequency, and in alleviating the effects of delays in the feature extraction process as well as in providing an accurate estimate of the target velocity. In Fig. 5.10 we can see how the feature's errors remain limited despite the fast motion (up to 40 [cm/s] for the linear and almost 2 [rad/s] for the angular velocity of the target) manually applied to the turntable. Once the target is kept at rest (t > 40 s), the feature errors slowly converge to zero. This is due to the high dry friction in the joints that the integral term has to overcome. Dry friction remains the main issue limiting the tracking accuracy of our implementation, while SVD-based regularization managed to cope better with the strong ill-conditioning of the task Jacobian than Damped Least Squares.

<figure><img src="image_35.png" /><figcaption>**Figure 5.10** – Estimated target linear (top) and angular (middle) velocities in camera frame compared to the feature errors (bottom). The circular motion produces a linear velocity component tangential to the circle and from a frame on top of the target it produces a linear and an angular velocity in camera frame along the x-axis and about the z-axis respectively.</figcaption></figure>

In the video https://www.youtube.com/watch?v=fOSRnxd1d1E, some further interaction experiments with different gains are carried out (min [2:12 - 2:23]). It is possible to appreciate (left bottom window) how the robot does some “jerky” motions. This happens when joint torque limits are violated. The ViSP wrapper of the *libfranka* that manages the control loop is implemented in such a way that the execution is recovered (a maximum of 10 times) after a violation occurs (e.g., by crossing joint torque limits, communication error due to packet loss, and so on). A possible solution could be to saturate the commanded joint torques at the expense of the tracking performance when the saturation is crossed.
The last part of the same video (min [2:23 - 3:00]), shows the comparison of two experiments with a slightly different inertial configuration of the robot since a wrist-mounted Force/Torque sensor with its aluminium flanges (~ 0.6 kg) is present in one of the experiments (right window — note that the sensor is not used). The presence of this “payload” redistributes the inertias in the kinematic chain improving the condition number of the inertia matrix of the robot, causing a smoother motion. This is also confirmed by the simulator since for the same configuration of the robot, the one in figure 6.5, the condition number of the inertia matrix of the real robot is ~ 800, while for the simulated robot it is ~ 300. This is due to the identified parameters retrieval process, reported in Chapter 7, that tends to uniformly distribute the solution among the links of the manipulator.

## 5.8 Summary

In this chapter, we presented our results on Second-Order visual servoing for interaction control and moving targets. The proposed EKFs effectively tracks the target motion and provides high rate visual information allowing for the implementation on a real platform with very good performance for the motion tracking. Several real-world implementation issues have been discussed alongside the solutions adopted. We recognize in the dry friction the main cause limiting the tracking performance of our implementation, especially at low speeds.

The derivations and results shown in this chapter are the result of the works issued on the papers:

* **A. A. Oliva**, P. Robuffo Giordano, and F. Chaumette, "A general visual-impedance framework for effectively combining vision and force sensing in feature space," *IEEE Robotics and Automation Letters*, vol. 6, no. 3, pp.4441-4448, 2021.
Video: https://www.youtube.com/watch?v=Qw0Dnhq2uRQ

* **A. A. Oliva**, E. Aertbeliën, J. de Schutter, P. Robuffo Giordano, and F. Chaumette, "Towards dynamic visual servoing for interaction control and moving targets," *ICRA 2022 - IEEE International Conference on Robotics and Automation*, May 2022, Philadelphia, United States. (to appear)
Video: https://www.youtube.com/watch?v=fOSRnxd1d1E
CHAPTER

# 6

# Feature Space Admittance

## Contents

<table><tr><td>6.1</td><td>Introduction</td><td>90</td></tr><tr><td>6.2</td><td>Related works</td><td>90</td></tr><tr><td>6.3</td><td>Feature Space Admittance</td><td>92</td></tr><tr><td>6.4</td><td>The Extended External Hybrid Vision-Force Control Scheme</td><td>94</td></tr><tr><td>6.4.1</td><td>Force regulation</td><td>95</td></tr><tr><td>6.5</td><td>Stability analysis</td><td>96</td></tr><tr><td>6.6</td><td>Experiments</td><td>97</td></tr><tr><td>6.6.1</td><td>Experimental Setup</td><td>97</td></tr><tr><td>6.6.2</td><td>Peg-in-Hole Experiment</td><td>99</td></tr><tr><td>6.6.3</td><td>Extended External Hybrid vs External Hybrid</td><td>101</td></tr><tr><td>6.7</td><td>Fictitious forces</td><td>103</td></tr><tr><td>6.8</td><td>Summary</td><td>104</td></tr></table>
## 6.1 Introduction

As it was shown in the previous chapter, the feature impedance control cannot be implemented as is with classical cameras due to their intrinsic technological limitations, e.g., slow frame rate compared to the faster force/torque sensing, which makes it impossible to control a manipulator at torque level. The low control bandwidth resulting from the use of low-rate cameras is in contrast with the need of high gains in the impedance controller for ensuring good tracking performances and disturbance rejection. It follows that there exists a trade-off between the trajectory tracking accuracy and the compliance given to the robot. We have successfully overcome these limitations with the proposed Extended Kalman Filters and by tackling the further real-world difficulties that have arisen. On the other hand, when an interaction task can/must be performed in static or quasi-static conditions, the dynamics of the manipulator can be neglected and one can use the joint level manipulator's velocity controller; in this way, the manipulator can be seen as an ideal positioning device. The extra complexity needed to let the impedance controller properly operate can be avoided using, for example, an admittance law in feature space coupled with a velocity resolved controller, which is the subject of this chapter. In this case, thanks to the force/torque measurements provided by a 6D Force/Torque sensor, we will be able to deform the visual trajectory reference given to the robot according to the measured forces, being able to provide compliance.

## 6.2 Related works

The hybrid vision/force scheme presented in Section 4.2 aims at controlling force along constrained directions while vision controls the motion of the remaining ones. The task geometry needs to be known *a priori* in order to properly design the controller via a *selection matrix* for ensuring orthogonality between vision and force controlled directions. Despite the use of feedforward terms to share the controlled directions among vision and force, the intrinsic sensory separation enforced by the underlying hybrid scheme, does not fully exploit the complementarity of vision and force sensing.

As we have shown in Section 4.3, a position-based impedance controller was used to achieve compliance in Cartesian space while an external vision control loop is closed around the former [55]. The main disadvantage of this control scheme is that it can get stuck in a local minimum where the simultaneous convergence towards the force and visual reference will not be reached. Recently,
three image-based visual impedance control laws have been proposed [81]. Al-
though the presented first- and second-order controllers rely on the regulation of
the visual error, compliance of the end-effector carrying the object is achieved
along/about its Cartesian directions.

Mezouar et al. [57] developed the External/Hybrid vision/force control scheme detailed in Section 4.4, to overcome the drawbacks of the Hybrid and Impedance-based vision-force schemes, for which they have provided an exhaustive comparative analysis. In their approach, the external *wrench* is transformed into a displacement of the image feature reference. This transformation is equivalent to an undamped spring which, as we show in Section 6.6, can start oscillating without ever reaching the convergence of the task or, in more serious cases, can damage the tool. We then seek for a higher order relation linking forces and features motion. One of the first works aiming at figuring out this relationship for vision driven robotic systems is [70], in which the visual servoing dynamics for a ball target is derived and, by exploiting a general definition and pose invariance of the Lagrangian function in the feature space, authors yields to an *ad hoc* simplified model dynamics for the features and the system considered. Vice versa, we are interested in the full Lagrangian model of the manipulator and in a generalized treatment that is independent of the type of visual feature.

Carelli et al. [82] proposed a Hybrid force- and vision-based impedance con-
troller to perform a *peg-in-hole* task. Pose-based visual servoing was used to guide
the end-effector towards the hole. Interaction forces were fed back to correct small
errors of visual guidance by modifying the visual reference along the horizontal
plane while pure force control was used along the vertical axis. The use of selec-
tion matrices does not allow to use vision along the vertical axis. The interaction
wrench were fed back simply by changing the point of application through a coor-
dinate transformation. The Authors do not provide a mapping to project physical
forces into the feature space, so they could consider fictitious forces synthetically
generated in the image plane or physical forces in operational space, limiting the
range of applicability.

In the work presented in this chapter, we instead leverage on the derivation
of second order visual servoing shown in Section 3.4 that does not depend on the
particular choice of visual features: this allows us to derive a general framework
that can effectively combine vision and force sensing directly in feature space.
This differs from previous works on this topic since the derivation of second-order
models has often been formulated *ad hoc* from the definition of the considered
features. Furthermore, thanks to this formulation, the projection of the *wrench*
applied on the camera into the feature space can also be generalized. This led us
to use a feature admittance law coupled with a force control law to ensure precise force regulation.

## 6.3 Feature Space Admittance

In Chapter 5 it was shown how the external wrench projects into the feature space as virtual per unit of mass/inertia force/torque that acts on the visual features. Exploiting this relationship and the concept of compliant frame, shown in section 2.4.3, we can link the desired reference and the compliant one with a second-order relationship that will be used to feed a vision control loop.

For the PBVS case, the admittance is equivalent to the one defined in the operational space, as reported in [13], and depends on the chosen orientation representation (e.g., Euler angles, axis/angle, etc). In this work we have adopted the axis/angle representation for the 3D features. In our case, on the other side, the task frame is the reference frame attached to the camera rather than to the end-effector's frame (see Figure 6.5).

For the IBVS case, the concept of compliant frame can be extended to the image space taking into account the vector of the current features $\mathbf{s}$, the vector of the desired features $\mathbf{s}^d$ and defining a new vector of compliant features $\mathbf{s}^*$, as shown in Figure 6.1. As for the PBVS case, the compliant feature vector is

<figure><img src="image_36.png" /><figcaption>**Figure 6.1** – The Compliant frame concept extended to the feature space: (a) Desired and Compliant features are coincident, the current features follow the compliant features. (b) The sensor undergoes the action of an external force. (c) The compliant features are linked to the desired ones with a mass-spring-damper relationship.</figcaption></figure>

coincident with the desired one until it moves under the action of an external per unit of mass/inertia force/torque $\mathbf{f}_{s_{ext}}$. A controller will minimize the error between current and compliant feature vectors $\mathbf{e}_s = (\mathbf{s}^* - \mathbf{s})$ while the error $\bar{\mathbf{e}}_s = (\mathbf{s}^d - \mathbf{s}^*)$ between desired and compliant vectors will be linked through the second order relationship:

$$ M_s(\ddot{\mathbf{s}}^d - \ddot{\mathbf{s}}^*) + D_s(\dot{\mathbf{s}}^d - \dot{\mathbf{s}}^*) + K_s(\mathbf{s}^d - \mathbf{s}^*) = \bar{\mathbf{f}}_{s_{ext}} \quad (6.1) $$
with $\bar{f}_{sext}$ the external wrench projected in feature space with the constant inertia as in (5.17). The difference between (5.17) and (6.1) are the features involved. In the former, the current $\mathbf{s}$ and the desired $\mathbf{s}^d$ features are considered while in the latter, are the desired $\mathbf{s}^d$ and the compliant $\mathbf{s}^*$ feature vectors to be related. When no external forces act on the end-effector, the compliant and desired reference frames are coincident ($\Sigma_* \equiv \Sigma_d$) or ($\mathbf{s}^* = \mathbf{s}^d$).

The feature reference position $\mathbf{s}^*$, as well as its velocity $\dot{\mathbf{s}}^*$ are obtained by time integration of the impedance equation (6.1) by isolating $\ddot{\mathbf{s}}^*$ and then integrating it twice:

$$ \ddot{\mathbf{s}}^* = \ddot{\mathbf{s}}^d + \mathbf{M}_{\mathbf{s}}^{-1}(\mathbf{D}_{\mathbf{s}}(\dot{\mathbf{s}}^d - \dot{\mathbf{s}}^*) + \mathbf{K}_{\mathbf{s}}(\mathbf{s}^d - \mathbf{s}^*) - \bar{f}_{sext}) \quad (6.2a) $$

$$ \dot{\mathbf{s}}^* = \int \ddot{\mathbf{s}}^* dt \qquad (6.2b) $$

$$ \mathbf{s}^* = \int \dot{\mathbf{s}}^* dt \qquad (6.2c) $$

It is interesting to notice that depending on both the type of visual feature chosen and the values assigned to the impedance parameters for each feature, we can have a different compliant behavior and this can be exploited to our advantage. If for example we have four feature lines expressed in the polar coordinates, as shown in Section 3.3.1, one can opportunely tune the relative in per unit of mass/inertia stiffness of the impedance law and achieve the desired behaviour. Referring to Figure 6.2, the relative stiffness matrix was chosen in such a way we

<figure><img src="image_37.png" /><figcaption>**Figure 6.2** – The four sides of the AprilTag are used as image line features. The reference lines (thicker lines) experience a spring-like displacement according to the chosen impedance parameters and the sensed forces.</figcaption></figure>

are stiff with respect to linear displacements of the yellow and green lines as well as for the rotations of all four lines while we are more compliant with respect to linear displacements of the blue and red lines, allowing us to have the yellow and
green lines as a rail on which we can slide. The same reasoning applies to any other visual feature defined in the image plane.

## 6.4 The Extended External Hybrid Vision-Force Control Scheme

From what explained in the previous section, we are modifying the visual reference according to the sensed force; therefore, we are not facing a regulation problem anymore but a trajectory tracking one. To this end, an appropriate visual control law is necessary in order to better track the moving visual reference [45, 48], as we show in Section 3.3.5. In this case we have to track the trajectory $\dot{\mathbf{s}}^d(t)$, while $\dot{\mathbf{s}}(t) = \mathbf{L}_s^c \mathbf{v}_c$ because we are considering the target to be static $^c\mathbf{v}_o = 0$. Imposing as usual an exponential decoupled decrease of the task error as control objective $\dot{\mathbf{e}}_s = -\lambda \mathbf{e}_s$, being $\dot{\mathbf{e}}_s = \dot{\mathbf{s}}^d(t) - \dot{\mathbf{s}}(t) = \dot{\mathbf{s}}^d(t) - \mathbf{L}_s^c \mathbf{v}_c$, we obtain

$$ ^c\mathbf{v}_c = \lambda \mathbf{L}_s^\dagger \mathbf{e}_s + \mathbf{L}_s^\dagger \dot{\mathbf{s}}^* \quad (6.3) $$

In the more general case in which also the target moves, an approximation of the error rate of change can be computed as $\hat{\dot{\mathbf{e}}}_s = \frac{d\mathbf{e}_s}{dt}$ which contains ideally $\dot{\mathbf{e}}_s = \dot{\mathbf{s}}^d(t) - \mathbf{L}_s^c \mathbf{v}_c + \dot{\mathbf{s}}_o$, i.e., the contribution to the error variation due to the changes in the trajectory reference and the camera and target motion. Subtracting then the feature velocity due to the camera motion, we can obtain an approximation of both the trajectory changes and the target motion

$$ ^c\mathbf{v}_c = \lambda \mathbf{L}_s^\dagger \mathbf{e}_s + \mathbf{L}_s^\dagger (\hat{\dot{\mathbf{e}}}_s + \mathbf{J}_s \dot{\mathbf{q}}) \quad (6.4) $$

otherwise, we can of course use the results obtained in the previous chapter for the tracking of a moving target and directly compensating for its motion, finally obtaining

$$ ^c\mathbf{v}_c = \lambda \mathbf{L}_s^\dagger \mathbf{e}_s + \mathbf{L}_s^\dagger \dot{\mathbf{s}}^* + \mathbf{L}_s^\dagger \dot{\mathbf{s}}_o \quad (6.5) $$

Combining this classical visual servoing control law with the feature space admittance defined in the previous section we can build an interaction control scheme as follows:

It is worth noticing that, since we are moving the compliant features $\mathbf{s}^*$ around the image plane, we should use the interaction matrix $\mathbf{L}_s(\mathbf{s}^*)$ associated to those features to project the external wrench into the feature space while for the vision control law (VCL) (6.3) we use the interaction matrix associated to the current
<figure><img src="image_38.png" /><figcaption>**Figure 6.3** – Basic control scheme: An outer admittance control loop is closed around an inner vision control loop. The feature admittance enables the classical visual-servoing controller to accommodate to interaction forces with the environment. Although $\ddot{s}^*$ is available, it is not used in the VCL. The external loop is quite general and can be plugged in many vision control schemes.</figcaption></figure>

features $L_s(s)$. When the controller tracks the compliant feature vector closely, we could use the interaction matrix associated to the current features to approximate the projection into feature space of the external wrench.

### 6.4.1 Force regulation

For many robotic applications the regulation of the exchanged forces with the environment is of particular importance for the correct execution of the task itself. It should be stressed that, since our controller achieves force control by regulating the velocity setpoint, the amount of exerted force is a consequence of the positional error and the chosen impedance stiffness. In particular, for the visual-feature admittance law in Figure 6.3, at the equilibrium (i.e., $\ddot{e}_s = \dot{e}_s = 0$) and referring directly to the external wrench in the end-effector frame $^e\mathbf{h}_e$, we have:

$$ K_s \bar{e}_s = L_s(s^*) \bar{B}_c^{-1} {}^c\mathbb{F}_e {}^e\mathbf{h}_e \quad (6.6) $$

Our intention is to regulate the amount of exchanged force along/about a certain direction to a specific value. To this purpose we use, together with the visual-feature admittance, a PI controller based on the force error in operational space (where the wrench is naturally defined) projected into the feature space. We can replace then the second member of (6.1) with the following force control law (FCL):

$$ \begin{aligned} \bar{f}_{sext}^* &= L_s(s^*) \bar{B}_c^{-1} {}^c\mathbb{F}_e {}^e\mathbf{h}_e^* \quad \text{with} \\ {}^e\mathbf{h}_e^* &= K_{fP}({}^e\mathbf{h}_e^d - {}^e\mathbf{h}_e) + K_{fI} \int ({}^e\mathbf{h}_e^d - {}^e\mathbf{h}_e) \cdot dt \end{aligned} \quad (6.7) $$
being $^e\mathbf{h}_e^d$ the desired wrench in the end-effector frame, $\mathbf{K}_{fP}$ and $\mathbf{K}_{fI}$ two 6 × 6 positive gain matrices for the proportional and integral force error terms respectively. The presence of the integral term in the controller creates a dominance of the force loop over the internal positional loop. It is of course clear that such a desired force/torque to be regulated has to be specified according to the task geometry (along/about a constrained direction) otherwise velocities will arise along/about unconstrained directions.

Once all the parts of the framework have been explained, it is possible to put them all together. Figure 6.4 shows the block diagram of the proposed controller: it presents a hierarchical structure in which a FCL, together with a feature space admittance law, is closed around an inner vision control loop implementing the VCL in (6.3). The reference trajectory $\mathbf{s}^d(t)$ is opportunely modified by the

<figure><img src="image_39.png" /><figcaption>**Figure 6.4** – Extended external hybrid vision/force control scheme. A feature space admittance is used as reference modifier to achieve compliance along visual features directions. Force control law ensures force regulation when the integral term is activated.</figcaption></figure>

admittance (6.1) proportionally to the output of the FCL implementing (6.7). We named the resulting controller as the *Extended External Hybrid* vision-force control scheme since it presents the same structure of the External Hybrid (see Section 4.4). The main advantage over the latter is that our framework offers more flexibility in the choice of the compliant behavior we want to implement, thanks to a greater number of parameters in the admittance that can be tuned, besides giving the possibility to achieve force regulation. It will be shown in a comparative experiment in Section 6.6.3 how this framework outperforms the External Hybrid when a soft contact behaviour is required.

## 6.5 Stability analysis

As already pointed out, the proposed control scheme presents a hierarchical struc-
ture where an external force control loop is closed around an internal vision loop. The external force loop is used to modify the reference trajectory from its planned path, according to the sensed forces/torques and with a behaviour determined by the impedance parameters. In order to study its stability, let us start from the simplest case and gradually consider all possible cases.

If we assume that the manipulator moves in free space towards a motionless target regulated by a constant desired task position, we are in the conditions of a classical visual servoing control scheme. In this case, the stability conditions are those given in Subsection 3.3.3.

If we now consider the more general case of trajectory and target tracking, for which controller equation (6.5) is replaced in the time variation of the error $\dot{e}_s = \dot{s}^d - \dot{s} = \dot{s}^d - L_s^c v_c + \dot{s}_o$, we obtain

$$ 
\begin{aligned}
\dot{e}_s &= \dot{s}^d - L_s(\lambda \hat{L}_s^\dagger e_s + \hat{L}_s^\dagger \dot{s}^* + \hat{L}_s^\dagger \hat{\dot{s}}_o) + \dot{s}_o = \\
&= \dot{s}^d - \lambda L_s \hat{L}_s^\dagger e_s - L_s \hat{L}_s^\dagger \dot{s}^* - L_s \hat{L}_s^\dagger \hat{\dot{s}}_o + \dot{s}_o
\end{aligned}
\qquad (6.8) $$

Even if $L_s \hat{L}_s^\dagger > 0$, the error will converge to zero only if the estimation of the features velocities due to the target motion $\hat{s}_o$ is sufficiently accurate so that

$$ L_s \hat{L}_s^\dagger \hat{\dot{s}}_o = \dot{s}_o \qquad (6.9) $$

while for the trajectory tracking term $\dot{s}^*$, the trajectory to follow is usually known ad can be fully compensated.

## 6.6 Experiments

In this section, we show the results of our proposed *Extended External Hybrid* controller executing a *Peg-in-Hole* task. We then compare it to the External Hybrid controller [57] in a critical situation. For a better understanding of this work we suggest to refer to the relative video https://www.youtube.com/watch?v=Qw0Dnhq2uRQ in which, in addition to the experiments here reported, we show a trial using lines as visual features and another trial that simultaneously copes with physical and fictitious forces to maintain the visual references in the field of view.

### 6.6.1 Experimental Setup

The setup consists of a Panda robot with its control software running on a Desktop PC with an Arch-Linux distribution patched with a *Preemptive* RT-Kernel 5.4.52-rt31. A wrist mounted six axis force/torque sensor Alberobotics FT45 with force
<figure><img src="image_40.png" /><figcaption>**Figure 6.5** – System setup and reference frames.</figcaption></figure>

and torque ranges of $F_z \pm 1000N$, $F_x$, $F_y \pm 500N$ and $M_{x,y,y} \pm 20Nm$ respectively is present as well as a RealSense SR300 camera operating at 30 fps, mounted in a *eye-in-hand* configuration (see Figure 6.5).

Force/torque sensors are capable of measuring any kind of force, being it gravitational, inertial or contact forces. We are interested only in the contact forces. Since force measurements modify the visual reference, gravitational and inertial effects should be compensated. If the payload is not well estimated, spurious force/torque readings remain that cannot be compensated for, without resorting to online estimation strategies of the payload parameters. These spurious readings will be interpreted as forces/torques acting on the features reference and will move them according to the admittance stiffness.

To make the system more robust against misestimation of the payload parameters (*i.e.*, mass, inertia, and center of mass) or inertial effects, we can choose higher values of the virtual per unit of mass/inertia stiffness of the admittance at the expense of a more rigid system. In our setup, in addition to the standard gripper, we have the camera, the force/torque sensor and some 3D printed parts whose weight has to be estimated. An efficient way to estimate these parameters is reported in [83] that relies on the identified coefficients of the robot dynamics that can be found in [7] for our robot and will be the subject of Chapter 7. As we use a wrist-mounted force/torque sensor, we need to compensate only for the payload downstream the sensors flange. In all the reported experiments, a 3^rd^-order Butterworth low-pass filters with *cut-off* frequency of 2 Hz is used to smooth the sensor readings of the Force/Torque sensor.
### 6.6.2 Peg-in-Hole Experiment

The task consists in inserting a *peg* into a *hole* present in a wood workpiece, both with a diameter of 1 cm. The depth of the *hole* is 2 cm while the length of the *peg* is 4 cm. An AprilTag of the 36h11 family of size 6.45 cm is applied to the surface of the workpiece. The latter is supported by a wooden structure fixed to the work table, which holds an inclined groove of about 45° with respect to the horizontal plane on one side and about 10° on the other (see Figure 6.6(a)).

<figure><img src="image_41.png" /><figcaption>**Figure 6.6** – Peg-in-hole experiment setup: (a) The robot at the initial configuration. (b) Forces exerted by the end-effector on the environment during phase 2: the task requires to apply $[5, -5, -15]N$ along the end-effector's x-(blue), y-(green) and z-axis (red arrow) respectively.</figcaption></figure>

The experiment consists of two phases. In the former the robot has to insert the *peg* in the *hole* without computing any trajectory while in the latter it pushes the *peg* towards the bottom of the *hole* and simultaneously applies lateral forces such that the workpiece slides into the groove until a desired force is reached along both directions. The task is executed using our proposed Extended External Hybrid controller (see Figure 6.3) for which the VCL gain is set to $\lambda = 1.5 s^{-1}$. The vision system guides the camera towards a desired final position of the features in the image plane. This final position corresponds to the position in which the peg, held by the robot gripper, is inserted in the hole at a depth of about 1.5 cm. Using Visp [10], the AprilTag on the workpiece is tracked and the coordinates of its corners are used as visual features $s \in \mathbb{R}^k$, with $k = 8$. In Subection 3.3.1 we have shown the expression of the interaction matrix $L_s \in \mathbb{R}^{8 \times 6}$ associated to these features [48].
During the first phase of the task, the FCL only projects the external wrench into the feature space allowing the system to accommodate for unmodeled interaction forces. So, according to (6.7), we have: $\bar{f}_{s_{ext}}^* = -L_s \bar{B}_c^{-1} c \mathbf{h}_c$, meaning that $\mathbf{K}_{fP} = \mathbb{I}_8$, the identity matrix of dimension $8 \times 8$, $\mathbf{K}_{fI} = 0 \ s^{-1}$ and $^e\mathbf{h}_e^d = 0$. The admittance parameters have been chosen to have small interaction forces and sufficient damping to attenuate the oscillations that can be triggered while maintaining the overall reactiveness of the system ($\mathbf{M}_s = \mathbb{I}_8 \frac{kg}{kg}$, $\mathbf{K}_s = \text{diag}(300) \frac{N/m}{kg}$, $\mathbf{D}_s = \text{diag}(200) \frac{N/m \cdot s^{-1}}{kg}$). During the second phase, that is the force regulation phase, the FCL fully implements equation (6.7) with $\mathbf{K}_{fP} = \text{diag}(0.2)$, $\mathbf{K}_{fI} = \text{diag}(5) \ s^{-1}$ so as to guarantee fast force convergence and limited overshoot while keeping the system stable. Being now the peg's motion fully constrained inside the workpiece's hole, we make the controller way more stiffer ($\mathbf{K}_s = \text{diag}(20000) \frac{N/m}{kg}$, $\mathbf{D}_s = \text{diag}(400) \frac{N/m \cdot s^{-1}}{kg}$). The desired wrench is set to $^e\mathbf{h}_e^d = [5 \ N \ -5 \ N \ -15 \ N \ 0 \ 0 \ 0]^\top$ and is transformed to the camera frame as $^c\mathbf{h}_c^d = {}^c\mathbb{F}_e {}^e\mathbf{h}_e^d$. For both phases, the desired apparent constant inertia we want the camera to exhibit is $\bar{\mathbf{B}}_c = {}^c\mathbb{T}_e \bar{\mathbf{B}}_e^{-1} {}^c\mathbb{T}_e^\top$ with $\bar{\mathbf{B}}_e = \text{diag}(1 \ \mathbb{I}_3 \ kg, 0.1 \ \mathbb{I}_3 \ kg \cdot m^2)$.

<figure><img src="image_42.png" /><figcaption>**Figure 6.7** – Peg-in-hole experiment: (a): Initial camera view of the experiment. The green crosses are the current features while the red crosses are the references. (b): Final camera view. It is possible to appreciate (in green) the trajectory of the features in the image plane. (c): Feature errors ($\mathbf{s}^* - \mathbf{s}$), (d) End-effector forces and (e) moments.</figcaption></figure>
The experimental setup and the framework's behaviour during the task execution are shown in Figures 6.6 and 6.7 respectively. As we can appreciate from the plots in Fig. 6.7c and the image points trajectory (almost straight) in Fig. 6.7b, the visual task begins to converge exponentially until it hits the structure, not being aware of it since there is neither a previous knowledge of the environment nor a pre-calculated trajectory. The impact force in the approach direction is approximately 35 N and it projects into the feature space as virtual per unit of mass/inertia forces/torques that pull the four point features reference towards the target centroid causing the robot to slow down along this direction while it continues converging along the others. The robot then approaches the *hole* and the *peg* is successfully inserted. These two collisions are perfectly visible both in Figure 6.7b (small saw-teeth close to the desired features) and Figure 6.7d (two peaks along z-axis). Once the *peg* is within the *hole*, we observe that the visual error converges allowing the system to autonomously switch to the second phase in which the integral term in the FCL is activated. The lateral force pushes the workpiece into the groove and against the wall of the structure. The integral term makes the output of the FCL increase until the force error is nullified. The integral term creates a dominance of the force loop over the internal (positional) vision loop.

### 6.6.3 Extended External Hybrid vs External Hybrid

The external hybrid approach [57] has been shown to be successful where impedance- and hybrid-based vision/force schemes have failed. Then follows our interest in comparing this control scheme with the proposed one, as we inherit its structure and extend it, eliminating its drawbacks.

In Section 4.4 we have shown that, for the External Hybrid controller [57], a Cartesian displacement is firstly computed as $d\mathbf{X} = \mathbf{L}_{\mathbf{X}}^{-1} \mathbf{K}^{-1} (e\mathbf{h}_e^d - e\mathbf{h}_e)$ being $\mathbf{K} \in \mathbb{R}^{6 \times 6}$ the contact stiffness. This displacement is then projected in feature space using the interaction matrix $ds = \mathbf{L}_s d\mathbf{X}$. Finally, the compliant reference is obtained by adding the computed feature space displacement to the desired reference $\mathbf{s}^* = \mathbf{s}^d + ds$.

To evaluate their relative performance over the task, we have executed different trials with both methods, each starting and ending from a different position. This time the workpiece is clamped on the work surface, preventing it from moving. The results of one of these experiments is shown in Figure 6.8.

To make the comparison as fair as possible, we choose the same gain $\lambda = 1.5 s^{-1}$ for both controllers and the proportional gain of the external hybrid, i.e., the
<figure><img src="image_43.png" /><figcaption>**Figure 6.8** – Peg-in-hole comparison: (a) the results obtained with our method and, (b) those obtained with the external hybrid [57]. The images show the initial and final camera views with the current and reference feature trajectories in green and red respectively. The plots report, from top to bottom: the visual feature errors ($\mathbf{s}^* - \mathbf{s}$), measured end-effector forces and moments. $e\mathbf{h}_e^d = [5N, 0, -20N, 0, 0, 0]^\top$</figcaption></figure>

contact stiffness, in such a way that both methods achieve the same displacement of the visual reference when the same force is applied leading to choose $\mathbf{K} = (\mathbf{L}_X \mathbf{L}_s^\dagger \mathbf{K}_s^{-1} \mathbf{L}_s \bar{\mathbf{B}}_c^{-1} \mathbb{F}_e)^{-1}$. On the other hand, $\bar{\mathbf{B}}_c$, $\mathbf{K}_{f_P}$, $\mathbf{K}_{f_I}$ and $\mathbf{M}_s$ are as in the previous experiment, for both phases, while for this experiment we impose a less stiff behaviour by choosing $\mathbf{K}_s = \text{diag}(200) \frac{N/m}{kg}$ and $\mathbf{D}_s = \text{diag}(150) \frac{N/m \cdot s^{-1}}{kg}$ for the first phase.

At the beginning, both methods starts converging towards the target with exactly the same behaviour. In fact, as long as there are no interaction forces, the reference is not modified and the task is a pure VS. When the *peg* approaches the *hole*, it hits the border before entering. As shown in Figure 6.8, for both the external hybrid and our method, the impact force along the approach direction stays quite limited even though for our method it is slightly higher. This is due to the presence of the damping term. Both methods succeed in the insertion of the *peg* tip but the increase of lateral forces once the *peg* is inside the workpiece, triggers the oscillation of the non-damped spring of the stiffness control for the external hybrid method. If these oscillations remain limited, they do not allow to reach the convergence of the visual reference while, in the event they explode as it is the case, it can lead to damage the manipulated object or even the robot
tool. This experimental result has shown that endowing the feature's motion
with a dynamic capable of damping their velocity, improves the performance of
the system, managing to dampen the oscillations that can be triggered.

6.7 Fictitious forces

The vector of per unit of mass/inertia force/torques in the feature space is an
artifact created to allow the physical forces to be mapped into the feature space.
However, a vector of artificial forces in the feature space could be generated.

<figure><img src="image_44.png" /><figcaption>**Figure 6.9** – Simultaneous interaction with both fictitious and physical forces. (a) Camera view: the reference features (red +) moves under the action of the physical force (purple arrow). A repulsive relative force field is generated on the left edge of the image plane (green arrows). The current features are the corners of the AprilTag (green +). (b) 3D visualization of the relative force intensity, the closer to the edge the higher. (c) Robotic system equipped with a camera in a *eye-in-hand* configuration. The operator physically pulls the end-effector (purple arrow) pushing the reference features towards the repulsive fictitious force field.</figcaption></figure>

As can be seen from Figure 6.9, it is possible to generate, for example, a
repulsive linear relative in per unit of mass/inertia force field on the left edge of
the image plane, that prevents the reference to leave the *Field-of-View* when we
physically interact with the manipulator's end-effector.
## 6.8 Summary

In this chapter we proposed a framework for dealing with the coupling of vision and force sensing directly in the feature space. The proposed controller presents the same hierarchical architecture of the External Hybrid vision/force control scheme [57] but with a substantial difference in the way we manage the compliant reference. Our controller, uses a second-order relationship between sensed forces and displacement quantities, i.e., it implements an impedance law in feature space. This was possible thanks to the force map from Cartesian to feature space shown in Chapter 5. This framework allows to treat the combination of vision and force sensing in a unified way regardless the chosen visual features. Unlike the Hybrid vision-force scheme, we can control simultaneously all the system's degrees of freedom with both vision and force sensing. The hierarchical juxtaposition of the force control loop over the vision loop, avoids the rise of inconsistencies at actuation level and the convergence towards local minima, as for [57]. Furthermore, we have shown that simultaneous interaction with both physical and fictitious forces (i.e. generated in the image plane) can be handled in this framework to account for e.g., the physical interaction with the environment and/or a visually generated repulsive force field in a collision avoidance tasks, as well as the achievement of force regulation. Finally, we have shown that compliance is achieved in feature space, along the feature that defines the visual tasks. Due to the common architecture and properties shared with [57], we have named our controller the “Extended External Hybrid vision/force” control scheme, which features some improvements. The implementation results confirm the validity of this approach.

The derivations and experiments shown in this chapter are the result of the work published on the paper:

* A. A. Oliva, P. Robuffo Giordano, and F. Chaumette, “A general visual-impedance framework for effectively combining vision and force sensing in feature space,” *IEEE Robotics and Automation Letters*, vol. 6, no. 3, pp.4441–4448, 2021.

Video: https://www.youtube.com/watch?v=Qw0Dnhq2uRQ
# Part III Manipulator dynamic model and simulation
CHAPTER

7

Dynamic model of the
Franka Emika's Panda robot

Contents

<table><tr><td>7.1</td><td>Introduction</td><td>108</td></tr><tr><td>7.2</td><td>The Panda robot</td><td>110</td></tr><tr><td>7.3</td><td>Identification procedure</td><td>111</td></tr><tr><td>7.3.1</td><td>Friction estimation</td><td>113</td></tr><tr><td>7.4</td><td>Retrieval of feasible parameters</td><td>114</td></tr><tr><td>7.5</td><td>Results</td><td>117</td></tr><tr><td>7.5.1</td><td>Validation on a physics simulator</td><td>121</td></tr><tr><td>7.6</td><td>Summary</td><td>125</td></tr></table>
## 7.1 Introduction

The knowledge of accurate dynamic models is of fundamental importance for many robotic applications. It is necessary, in fact, for designing control laws with superior performance in free motion or when interacting with the environment [13], e.g., in strategies for the sensorless detection, isolation and reaction to unexpected collisions [84] or when regulating force or imposing a desired impedance control at the contact [85].

The controllers developed in Chapter 5 are indeed based on the manipulator model, which allows for inverse dynamic control. The cancellation of the manipulator dynamics is more effective the accurate the model is. On the other hand, as we will see in the next chapter, knowledge of the dynamic parameters is required for the dynamic simulation of the manipulator. Moreover, a first implementation step on the simulator was required to study the problem of second-order visual-servoing, followed by a second implementation step on the real platform. It would have been extremely difficult to determine the source of errors or control malfunctions, as well as their causes, without this intermediate step. The necessity of estimating these parameters is evident from the aforementioned considerations.

To obtain an estimation of the dynamic model, regression techniques are widely employed for industrial [86,87] or humanoid robots [88,89]. These techniques are hinged on a fundamental property: the linear dependence of the robot dynamic equations in terms of a set of $\rho$ dynamic coefficients $\pi_R \in \mathbb{R}^\rho$ [20], also known in the literature as base parameters [21], which are linear combinations of the dynamic parameters of each link composing the robot. In particular, each link has 10 parameters, specifying the mass, the position of the center of mass (CoM), and the 6 elements of the symmetric inertia tensor. Then, a robot with $l$ links has a total 10 $l$ of such parameters, denoted as $\mathbf{p} \in \mathbb{R}^{10l}$. In addition, one may also include a number of parameters for modeling joint friction.

The regrouping of dynamic parameters, i.e., the dynamic coefficients, occurs because not all the parameters are excitable during motion (and therefore some of them have no influence on the robot dynamics), and others have an effect on the dynamics only in combination, that is, they are not identifiable separately. Hence, the dynamic parameters in $\mathbf{p}$ will appear as a combination and very rarely as singletons in $\pi_R$.

The identification of the dynamic coefficients $\pi_R$ is often sufficient for a number of robotic applications, such as dynamic motion robot control and motion planning, since knowledge of $\pi_R$ allows for a numerical evaluation of the robot dynamic model in the Euler-Lagrange (E-L) form. However, the retrieval of a set of feasible
numerical values for the dynamic parameters **p** is also relevant. This is the case, for instance, when performing dynamic simulations via a CAD-based robotic simulator - like CoppeliaSim [75] - or when implementing torque-level control laws (such as the feedback linearization) under hard real-time constraints. In this case, a widely adopted solution is to use the recursive numerical Newton-Euler (N-E) algorithm, which is preferred to the evaluation of the symbolic computationally more expensive E-L approach (which relies on dynamic coefficients). However, usual N-E routines require the knowledge of the dynamic parameters **p** of each link in the kinematic chain, and not just of the dynamic coefficients $\pi_R$. In [20], the problem of recovering a complete set of values for the original robot parameters starting from the identified dynamic coefficients was addressed. In general, this is a nonlinear problem admitting an infinite number of solutions. However, not all solutions are physically consistent (as example, negative masses may appear). To discard unfeasible solutions, we considered upper and lower bounds on each component of **p** by solving a constrained nonlinear optimization problem. Because of the ill-conditioned nature of the solution space, we used global optimization methods, such as simulated annealing.

The physical consistency of the identified dynamic parameters, as introduced in [90], is currently attracting more and more attention. Researchers have treated the problem of physical feasibility within the framework of linear matrix inequalities (LMIs), solving the problem of the identification of a physically consistent set of parameters by means of semi-definite programming (SDP) techniques [91]. Recently, this framework has been enriched by the addition of the triangle inequality of the inertia tensors [23, 24], a constraint which was originally mentioned in [22], providing the scientific community with a valuable tool for the dynamic identification problem. The approach presented in this chapter can be considered as an alternative to the LMI-SDP framework.

All these approaches act on the parameter identification phase, obtaining from this dynamic coefficients that are typically different from the classic ordinary least squares (OLS) solution [86, 87].

The approach presented in [24, 91] requires to express constraints as linear matrix inequalities, while the proposed optimization algorithm manages linear, nonlinear and even conditional constraints (e.g., *if-else*), without any mathematical manipulation. This additional flexibility allows to handle directly nonlinear constraints coming from the geometric shape of cylindrical or spherical links (like for Universal Robots manipulators or for the sixth link of the KUKA LWR IV+), or when the use of approximate box constraints may generate solutions which are even unfeasible (e.g., a center of mass outside a convex link of the robot).
On the other hand, a drawback of the proposed algorithm is that convergence to a global optimum in a finite number of steps cannot be guaranteed. It has to be noted that, whatever method is adopted, it is very unlikely that the original dynamic parameters are retrieved; indeed, the more constraints are supplied, the closer the solution will be to the actual one. Thanks to the algorithmic nature of the presented framework, a wide range of constraints can be easily added.

The proposed approach is general and can be applied to a large class of robot manipulators. In this work, as case study, we apply it to the Franka Emika Panda robot (see Figure 7.1), for which we have identified a complete set of feasible dynamic parameters.

## 7.2 The Panda robot

<figure><img src="image_45.png" /><figcaption>**Figure 7.1** – The Panda arm equipped with its gripper and interfaced with its controller. The signal flows from and to the controller are depicted: The user sends a command to the *libfranka* API that communicates with the FCI controller through Ethernet. This input is then converted to a commanded torque $\tau_c$ to the robot that returns the measured joint torque $\tau$, as well as the joint positions $q$ and velocities $\dot{q}$. The FCI controller computes the numerical values for the inertia matrix $\bar{B}(q)$, the gravity vector $\bar{g}(q)$, the Jacobian $\bar{J}_e(q)$, and the Coriolis term $\bar{c}(q, \dot{q})$ as well. These data are sent back to the user through the *libfranka* API. More details can be found at: https://frankaemika.github.io/docs/index.html.</figcaption></figure>

The robot considered in the experiments and simulations all along this thesis work, is the popular Panda robot manufactured by Franka Emika. Panda is a torque-controlled cobot that is attracting a large interest both on research and in small-sized business because of its safety, versatility, reliability and relatively low price (less than 20 k€). Its total weight is about 18 kg and it is capable of handling payloads up to 3 kg. It is a redundant manipulator with $n = 7$ revolute joints, each of which is equipped with a link-side torque sensor. This robot can
be controlled with five different interfaces: at joint level by sending torque $\tau_d$, position $q_d$ or velocity $\dot{q}_d$ commands or by sending Cartesian pose $T_d$ or velocity $v_d$ commands to the Franka Control Interface (FCI) through the *libfranka*, its open-source C++ API. Through this library it is possible to send real-time control values (at 1 kHz) and have full access to the robot state, e.g., the joint positions $q$ and velocities $\dot{q}$, as well as the link side torque vector $\tau$. Moreover, it returns the *numerical* values of the inertia matrix $\bar{B}(q)$, of the gravity vector $\bar{g}(q)$, as well as the Jacobian $\bar{J}_e(q)$ and the Coriolis vector $\bar{c}(q, \dot{q})$ at a given joint configuration $q$ and velocity $\dot{q}$. These data will be of fundamental importance for the identification process of the dynamic model of the robot described in the next section. Robots with the Research Interface can also be controlled using the *franka_ros* metapackage [92], which integrates and exposes the complete *libfranka* API to the ROS ecosystem. This metapackage also provides hardware abstraction and model description in Universal Robotic Description Format (URDF) to simulate the robot in Gazebo [93].

The FCI controller is designed in such a way that the command inputs given by the user are appropriately manipulated so that the motors generate the proper torque $\tau_c$ for the commanded task. Figure 7.1 depicts all the above-mentioned control signals.

## 7.3 Identification procedure

In order to derive the symbolic dynamic model of a robot with elastic joints, such as the Franka Emika Panda, one may follow the procedure presented in [94], separating the motor torques from the link-side torques. Nevertheless, the particular features offered by the robot controller allow us to simplify the modeling: in fact, since the FCI controller is able to return the estimations of the link-side torques (exploiting the motor position measures read from the encoders), we are able to adopt the classical model structure as for a rigid joints robot, neglecting the elasticity [95], and henceforth $\tau \in \mathbb{R}^n$ is the vector of the *link-side torques*.

Exploiting the features offered by the controller of the Panda robot, as we mention in Section 7.2), it is possible to avoid the classical procedure involving exciting trajectories [96], and obtain the estimates of the gravitational and inertial coefficients by collecting a set of static positions only by means of a reverse engineering procedure (See Appendix A for a comparison of the dynamic coefficients estimated through the reverse engineering process with those obtained from the classical approach that exploits exciting trajectories). The same procedure had been used to retrieve the dynamic coefficients of the KUKA LWR robot [95, 97]:
in that case, the gravitational and the inertial coefficients have been estimated separately, while now a slightly different approach has been used, due to the fact that many coefficients can be retrieved both from the inertia matrix and from the gravity vector. As a first operation, one has to rearrange the symbolic inertia matrix $B(q)$ in a vector form (the inertia stack), by exploiting its symmetry. Having for the Panda robot $n = 7$ joints, we obtain a vector $\tilde{b}(q) \in \mathbb{R}^m$, with $m = n(n+1)/2 = 28$ components, containing all the lower triangular elements of $B(q)$. Now, it is possible to obtain – as described in Section 2.3.1 – the symbolic regressor $Y_s(q)$ from the column vector $\zeta(q) \in \mathbb{R}^{m+n}$ in such a way that

$$ \zeta(q) = \begin{bmatrix} \tilde{b}(q) \\ g(q) \end{bmatrix} = Y_s(q)\pi_s, \qquad (7.1) $$

where $\pi_s \in \mathbb{R}^{10n}$ is the vector containing both the gravitational and inertial dynamic parameters, which are the same – excluding joint friction and motor inertias – as in vector $\pi$ of eq. (2.33).

In order to obtain a numerical estimation of the dynamic coefficients vector, a data acquisition procedure should be carried out: in the general case, performing exciting trajectories is required in order to span all the admissible joint positions, velocities and accelerations (see eq. (2.34)). Since $\zeta(q)$ depends only on the joint positions $q$, it is just sufficient to retrieve data by imposing static positions.

The libfranka API exposes the numerical evaluation of the gravity vector and the inertia matrix of the Panda robot at the current link position. Therefore, it is possible to collect a fair amount of data even only in a static way, i.e., bringing the manipulator to a desired configuration and then retrieving and storing the numerical values of the gravity vector and the inertia matrix. This acquisition procedure can be performed during a motion as well. The main advantage of imposing static joint positions is that this procedure avoids any influence of friction and uncertainty (e.g., due to measure noise or to any unmodeled phenomenon).

The data is acquired and collected in a list of $M$ different (special and/or random) configurations, under the weak condition

$$ Mn \gg p. \qquad (7.2) $$

For a generic configuration $q_k$, with $k = 1...M$, we have

$$ \bar{\zeta}_k = \begin{bmatrix} \bar{b}_k \\ \bar{g}_k \end{bmatrix} = \bar{Y}_{sk} \pi_s, \qquad (7.3) $$

where $\bar{b}_k$ and $\bar{g}_k$ represent, respectively, the numerical inertia stack vector and the numerical gravity vector, as they are retrieved from the libfranka interface at a given configuration $q_k$, and $\bar{Y}_{sk} = Y_s(q_k)$ is the evaluated regressor.
When all data are collected, they can be stacked into a vector $\bar{\zeta}$ such as:

$$ \bar{\zeta} = \begin{bmatrix} \bar{\zeta}_1 \\ \bar{\zeta}_2 \\ \vdots \\ \bar{\zeta}_N \end{bmatrix} = \begin{bmatrix} \overline{\mathbf{Y}}_{s1} \\ \overline{\mathbf{Y}}_{s2} \\ \vdots \\ \overline{\mathbf{Y}}_{sN} \end{bmatrix} \quad \pi_s = \overline{\overline{\mathbf{Y}}}_s \pi_s. \qquad (7.4) $$

Nevertheless, the regressor $\overline{\mathbf{Y}}_s$ is typically rank-deficient: this implies that the elements of the vector $\pi_s$ are not fully identifiable. Therefore, it is necessary to drop linear dependent columns of the regressor in order to reach a full (column) rank condition (i.e., by means of the Gauss-Jordan elimination technique). As a consequence, some dynamic parameters will be grouped together accordingly, in the form of dynamic coefficients [21]. Exploiting condition (7.2) on the minimal number $M$ of samples to retrieve, the ill-conditioning of the matrix is avoided. Denoting as $\hat{\pi}_{s,R}$ the vector containing the regrouped parameters (a.k.a. dynamic coefficients), and as $\overline{\mathbf{Y}}_{s,R}$ the full rank numerical regressor, eq. (7.4) is solved using a least squares technique as

$$ \hat{\pi}_{s,R} = (\overline{\mathbf{Y}}_{s,R}^{\top} \overline{\mathbf{Y}}_{s,R})^{-1} \overline{\mathbf{Y}}_{s,R}^{\top} \bar{\zeta} = \overline{\mathbf{Y}}_{s,R}^{\top} \bar{\zeta}, \qquad (7.5) $$

where $\dagger$, as usual, denotes pseudoinversion.

Once one has the dynamic coefficients estimation $\hat{\pi}_s$, from eq. (7.1), it is possible to obtain the estimates $\hat{g}(q)$ and $\hat{B}(q)$ as:

$$ \hat{\zeta}(q) = \begin{bmatrix} \hat{b}(q) \\ \hat{g}(q) \end{bmatrix} = \mathbf{Y}_{s,R}(q) \hat{\pi}_{s,R}, \qquad (7.6) $$

where $\mathbf{Y}_{s,R}(q)$ is the symbolic regressor pruned of the dependent columns (according to the full-rank matrix $\overline{\mathbf{Y}}_{s,R}$) and $\hat{B}(q)$ is built from the estimated inertia stack $\hat{b}(q)$. Finally, the estimation of the Coriolis and centrifugal forces vector $\hat{c}(q, \dot{q})$ is derived from $\hat{B}(q)$ using the Christoffel's symbols according to [13]. The form of the inverse dynamics formula, providing an estimation of the joint torques $\hat{\tau}$ needed to accomplish a given trajectory $(q, \dot{q}, \ddot{q})$, is therefore:

$$ \hat{\tau}(q, \dot{q}, \ddot{q}) = \hat{B}(q)\ddot{q} + \hat{c}(q, \dot{q}) + \hat{g}(q). \qquad (7.7) $$

### 7.3.1 Friction estimation

If a validation trajectory is executed and the measured torques $\tau$ are compared (after a filtering procedure) with the estimated torques $\hat{\tau}$ generated by eq. (7.7),
a difference in the two signals may be observed. This discrepancy is due to estimation errors (e.g., due to the noise affecting the measurements) or to unmodeled effects, such as joint friction. Typically, the latter effect, assumed to act separately on each joint, is expressed as an additional torque $\tau_{f,j}$, $j \in \{1, \dots, n\}$:

$$ \tau_{f,j}(\dot{q}_j) = f_{v,j}\dot{q}_j + f_{c,j}\mathrm{sign}(\dot{q}_j) + f_{o,j}, \qquad (7.8) $$

where $f_{v,j}$ and $f_{c,j}$ represent, respectively, viscous and Coulomb friction, while $f_{o,j}$ is the Coulomb friction offset. Model (7.8), although being simple and effective, has the main drawback of exhibiting sudden discontinuities in the neighborhood of $\dot{q}_j = 0$. In order to attenuate this chattering, a sigmoidal friction model can be used for avoiding discontinuities for low joint velocities. In case the viscous effects are negligible and a symmetric behavior for positive and negative joint velocities is observed (as for the Panda robot), $\tau_{f,j}(\dot{q}_j)$ can be expressed as the following function:

$$ \tau_{f,j}(\dot{q}_j) = \frac{\varphi_{1,j}}{1 + e^{-\varphi_{2,j}(\dot{q}_j + \varphi_{3,j})}} - \frac{\varphi_{1,j}}{1 + e^{-\varphi_{2,j}\varphi_{3,j}}}, \qquad (7.9) $$

which is characterized by 3 parameters for each joint $(\varphi_{1,j}, \varphi_{2,j}, \varphi_{3,j})$. In order to estimate the $3n = 21$ friction parameters for the Panda robot, trajectories spanning all possible joint velocities can be executed for each joint (possibly, keeping the others at rest). Computing the inverse dynamics for the previous trajectories according to (7.7), the difference $\Delta\tau_j = \tau_j - \hat{\tau}_j$ (where $\tau_j$ and $\hat{\tau}_j$ are, respectively, the measured and the estimated joint torques) can be interpreted as a measure of the friction effects. Solving a least squares problem, it is possible to find the parameters that make the curve (7.9) to fit data at best – e.g., by means of a Nelder-Mead routine.

The presented reverse engineering approach has the main advantage of allowing an estimation of the dynamic parameters by exploiting only static measures (which are affected by low noise) without the need of numerical derivations, which can dramatically affect the results of the identification process. Moreover, having a separate step for joint friction identification allows to repeat only this step (instead of the whole identification process) when lubrication changes or when moving to a new instance of the same robot. Conversely, we must rely on the numerical values of $\bar{\zeta}_k$ provided by the manufacturer.

## 7.4 Retrieval of feasible parameters

In [20], authors presented a framework for retrieving a feasible set of dynamic parameters from the previously identified dynamic coefficients, by solving a non-linear global optimization problem.
For some purposes, in fact, the estimated dynamic coefficients (linear combination of dynamic parameters, such as masses, position of the centers of mass and inertia tensors) are not sufficient, and an estimation of the dynamic parameters themselves is needed. This occurs, for instance, if the dynamic parameters values are needed for simulating the robot behavior in a CAD software (like CoppeliaSim as we will see in the next chapter), or if a Newton-Euler routine is used to compute the joint torques estimation during the robot motion, with strict time constraints (in this case, a Lagrangian approach may not fit the hard real-time requirements), or in case one is interested in computing the wrenches acting on the robot joints.

The approach presented in [20] is able to return a possible and feasible set of the dynamic parameters: in general, infinite solutions exist (among which the real one) and the more constraints are given, the closer to the real solution is the returned one.

Starting from the identified vector of dynamic coefficients $\hat{\pi}_R$, the following transformations may be applied to the corresponding symbolic vector $\pi_R(p)$ (parallel axis theorem, see [13]):

$$ 
\begin{aligned}
J_{ixx} & \rightarrow I_{ixx} + m_i c_{iy}^2 + m_i c_{iz}^2; \quad J_{ixy} \rightarrow I_{ixy} - c_{ix} c_{iy} m_i \\
J_{iyy} & \rightarrow I_{iyy} + m_i c_{ix}^2 + m_i c_{iz}^2; \quad J_{ixz} \rightarrow I_{ixz} - c_{ix} c_{iz} m_i \\
J_{izz} & \rightarrow I_{izz} + m_i c_{ix}^2 + m_i c_{iy}^2; \quad J_{iyz} \rightarrow I_{iyz} - c_{iy} c_{iz} m_i
\end{aligned} 
\quad (7.10) $$

for each link $l_i$, $i = 1...n$. We can now rearrange the parameters vector $p = [p_1^\top \ p_2^\top \ p_3^\top]^\top \in \mathbb{R}^{10n}$ as:

$$ 
\begin{aligned}
\boldsymbol{p}_1 &= [m_1 \ \dots \ m_n]^\top \in \mathbb{R}^n, \\
\boldsymbol{p}_2 &= [c_{1x} \ c_{1y} \ c_{1z} \ \dots \ c_{nx} \ c_{ny} \ c_{nz}]^\top \in \mathbb{R}^{3n}, \\
\boldsymbol{p}_3 &= [\boldsymbol{\mathcal{I}}_1^\top \ \dots \ \boldsymbol{\mathcal{I}}_n^\top]^\top \in \mathbb{R}^{6n},
\end{aligned} 
\quad (7.11) $$

where

$$ \boldsymbol{\mathcal{I}}_i = [I_{ixx} \ I_{ixy} \ I_{ixz} \ I_{iyy} \ I_{iyz} \ I_{izz}]^\top. \quad (7.12) $$

It is possible to provide lower bounds (LB) and upper bounds (UB) to $p$ based on a priori information. In particular, condition (2.37) is managed by assigning a lower bound of zero to each link mass. Upper bounds for the masses are assigned exploiting, for instance, data retrieved from the datasheets of the robot. Moreover, for each link, one can easily infer that the center of mass is located inside the smallest parallel box which includes the link geometry, in the most general case. The lower and upper bounds are then set in order to guarantee a physical meaning to the obtained solution. In order to retrieve a possible set of dynamic parameter $\hat{p}$, we propose to solve the nonlinear optimization problem depicted in Algorithm 1.
<caption>**Algorithm 1:** Parameters retrieval</caption>

1 $\mathbf{p}_0 \leftarrow LB + (UB - LB)\mathbf{u}$, with $u \sim U(0, 1)$;
2 $\xi_1 \leftarrow 0$;

3 **for** $k = 1, \dots, \kappa$ **do**

4 // Start the optimization from the previous step solution

5 $p_{k,init} \leftarrow p_{k-1}$;

6 // Solve the optimization problem using SA and Nelder-Mead as IP

$$ 
\left\{ 
\begin{array}{l}
\min_{\mathbf{p}_k} f(\mathbf{p}_k) = \phi(\mathbf{p}_k) + \xi_k \gamma(\mathbf{p}_k) \\
\qquad = \|\boldsymbol{\pi}(\mathbf{p}_k) - \hat{\boldsymbol{\pi}}\|^2 + \xi_k \sum_{i} g(h_i(\mathbf{p}_k)) \\
\text{s.t.} \quad \mathit{LB} \le \mathbf{p}_k \le \mathit{UB} \\
\xi_{k+1} \leftarrow 10k
\end{array}
\right.
\qquad (7.13)
$$

7 **end**

The first two lines of Algorithm 1 are the initialization steps: the starting point is randomly selected between the lower and the upper bounds using a uniform distribution. Moreover, $\xi_1$ is set to zero. Lines 3–6 of Algorithm 1 consist in solving the constrained nonlinear optimization problem $\kappa$ times: at a given step $k = 1 \dots \kappa$, the initial state is the optimal solution found at step $k-1$. The objective function presents also an exterior penalty function, in which $\xi_k$ is the – progressively increasing – penalty coefficient: this function provides a penalty in case one of any additive constraint $h_i(\mathbf{p}_k)$ is violated; function $g(\cdot)$ is chosen to return a measure of the constraint violation. In particular, two kinds of constraints have been included:

* for each link $l_i$, the triangle inequality (2.42) must be satisfied;

* the total sum of the link masses must be in a given range, that is:

$$ m_{\text{rob,min}} \le \sum_{i} m_i \le m_{\text{rob,max}}. \qquad (7.14) $$

Note that the presented framework is extremely flexible, and further external constraints can be easily included. The manifold generated by the cost function $f(\mathbf{p}_k)$ contains multiple local minima, and therefore a global optimization method, like genetic algorithms [98] or simulated annealing (SA) [99] is mandatory to address the problem (7.13). In the present case, we have used SA, applying a more sophisticated interior-point (IP) Nelder-Mead local optimization algorithm
at the end of each SA iteration *k*. Moreover, *Q* runs have been launched having a different random initial point *p*₀, in order to span as much as possible the cost function manifold.

The improvement of the parameters retrieval framework (with respect to the
one presented in [20]) has been proved necessary since the introduction of some
constraints (e.g. instance the triangle inequality of the inertia tensors) eventually
led the algorithm to get stuck in local minima. This was caused by recurring
abrupt changes in the cost function given by the constant penalty function adopted
before. Therefore, a violation-dependent penalty [100] has been implemented for
the present algorithm, solving this problem. Moreover, the sequence of successive
runs of the algorithm could in practice help in improving the solution.

The term $\phi(\mathbf{p}_k)$ of the parameters retrieval algorithm in eq. (7.13) requires to be computed a previous identification step returning the coefficients estimation $\hat{\pi}$.
Another possible $\phi(\mathbf{p}_k)$ function, yielding to a single-step procedure, is described in Appendix A.

7.5 Results

In this section, results from the dynamic coefficients and joint friction estimations
for the Panda robot are reported, in addition to results from the parameters
retrieval procedure.

In order to obtain a numerical estimation of the dynamic coefficients $\pi_{s,R}$ (see eq. (7.5)), the numerical values of the inertia matrix and the gravity vector are retrieved from a set of $M = 1010$ static positions, spanning the whole joint space, according to the robot documentation<fnref n="1" />. The symbolic vector $\varsigma(q)$ (see eq. (7.1)) has been computed according to [21], using the modified Denavit-Hartenberg convention.

Since our robot is mounted on a table parallel to the ground, the first element
of the gravity vector (relative to joint 1) is not informative, and therefore it is
discarded.

Stacking all the numerical quantities of the data acquisition phase and af-
ter evaluating the corresponding regressor (see eq. (7.4)), we obtained that
rank($\overline{\mathbf{Y}}_s$) = 43: using the Matlab function rref, which implements Gauss-Jordan
elimination technique, we finally obtained the dynamic coefficients estimations

<footnote n="1">¹The joint position bounds of the Franka Emika Panda robot can be found at: https://frankaemika.github.io/docs/control_parameters.html</footnote>
$\hat{\pi}_{s,R}$ according to eq. (7.5), from a regressor $\overline{\overline{Y}}_{s,R}$ whose condition number is 49. Moreover, the relative error percentage of predictions (defined in eq. (84) of [91]) for the identification set is 0.031%. Computing their standard deviations (see [87,91]), we found that two coefficients exhibit a standard deviation greater than 30%, and therefore they were discarded (since their estimations are not reliable). All the estimated dynamic coefficients are reported with their standard deviations in the Appendix A, together with a comparison with the corresponding coefficients obtained from the classical identification procedure (see eq. (2.34)).

From the identified dynamic coefficients $\hat{\pi}_{s,R}$, we were able to reconstruct the inertia matrix $\hat{B}(q)$, the gravity vector $\hat{g}(q)$ and the Coriolis and centrifugal force vector $\hat{c}(q, \dot{q})$ following a Lagrangian approach (see Sec. 7.3).

After deriving the inverse dynamic model (7.7), we validate it by comparing the measured joint torques with the estimated ones during several motions. In particular, the robot was commanded in velocity-mode by means of sinusoidal trajectories in the joint space. In other words, each joint is commanded according to the following equation:

$$ \dot{q}_{i,\text{des}}(t) = A_i \sin\left(\frac{2\pi}{T_i}t\right), \quad i \in [1, \dots, n] \qquad (7.15) $$

where $A_i$ is the amplitude of the velocity profile and $T_i$ is the period of the sinusoidal signal for the i-th joint. The numerical values for $A_i$ and $T_i$, $i \in [1, \dots, n]$ for a typical experiment are reported in Table 7.1, where 5458 samples were collected. The joint torque signals were recorded (and filtered through a 4-th order zero-phase digital Butterworth filter with a cutoff frequency of 1 Hz) during this motion, and compared with our Lagrangian inverse dynamics estimations $\hat{\tau}(q, \dot{q}, \ddot{q})$ (from eq. (7.7)), feeding that model with the measured joint positions and velocities, and with the joint accelerations obtained by numerical differentiation of the filtered velocities. The joint torque comparison is reported in Figure 7.2: the torque estimations are almost perfectly superimposing the measured ones for joints 1...4, while the last joints show some discrepancies. One can notice, though, that the difference between the two signals strongly depends on joint velocities (it is more evident, e.g., for joint 7): this behavior is typical of joint friction.

Therefore, we performed the estimation of the joint friction according to the procedure reported in Section 7.3.1: we collected more than 10k samples of joint velocities and torques during sinusoidal motions in (7.15); eventually, we found that the friction model that best fits the data was given by a sigmoidal function (7.9), which is characterized by 3 parameters per joint. These were estimated by solving a nonlinear least squares problem by means of a Nelder-Mead routine
<figure><img src="image_46.png" /><figcaption>**Figure 7.2** – Comparison between the torques of the derived E-L dynamic model. The red lines represent the measured joint torques during the validation experiment reported in Tab 7.1, while the green lines are the torque estimations $\hat{\tau}$ computed according to eq. (7.7)). The blue lines represent the joint torque estimates comprehensive of the joint friction term (7.9). The dashed and the dotted lines are the errors between the torque sensors readings and, respectively, the torques estimates without and with the friction component.</figcaption></figure>

(Matlab function `fminsearch`), using as fitting data the differences $\Delta\tau_i$ between the measured joint torques and the estimated torques $\hat{\tau}_i$ for each joint $i$ separately. The results of the fitting procedures are reported in Fig. 7.3, while Table 7.2 provides the numerical values of the parameters obtained with the fitting procedure described in Section 7.3.1. These parameters refer to the equations (7.9) which models the friction acting at the robot joints.

<table><caption>**Table 7.1** – Amplitudes $A_i$ and periods $T_i$, $i \in [1, \dots, n]$ of the sinusoidal trajectories in eq. (7.15) used for validating the friction acting on the robot joints.</caption><thead><tr><th></th><th>1</th><th>2</th><th>3</th><th>4</th><th>5</th><th>6</th><th>7</th><th>units</th></tr></thead><tbody><tr><th>A<sub>i</sub></th><td>2.21</td><td>-2.21</td><td>1.2</td><td>-2.1</td><td>-2.3</td><td>2.1</td><td>-2.5</td><td>rad/s</td></tr><tr><th>T<sub>i</sub></th><td>3.68</td><td>2.04</td><td>2.98</td><td>1.75</td><td>4.43</td><td>2.749</td><td>1.06</td><td>s</td></tr></tbody></table>
<table><caption>**Table 7.2** – Estimated joint friction parameters.</caption><thead><tr><th></th><th>Joint 1</th><th>Joint 2</th><th>Joint 3</th><th>Joint 4</th><th>Joint 5</th><th>Joint 6</th><th>Joint 7</th><th>units</th></tr></thead><tbody><tr><td>φ<sub>1</sub></td><td>5.4615e-01</td><td>0.87224</td><td>6.4068e-01</td><td>1.2794e+00</td><td>8.3904e-01</td><td>3.0301e-01</td><td>5.6489e-01</td><td>N·m</td></tr><tr><td>φ<sub>2</sub></td><td>5.1181e+00</td><td>9.0657e+00</td><td>1.0136e+01</td><td>5.5903e+00</td><td>8.3469e+00</td><td>1.7133e+01</td><td>1.0336e+01</td><td>s/rad</td></tr><tr><td>φ<sub>3</sub></td><td>3.9533e-02</td><td>2.5882e-02</td><td>-4.6070e-02</td><td>3.6194e-02</td><td>2.6226e-02</td><td>-2.1047e-02</td><td>3.5526e-03</td><td>rad/s</td></tr></tbody></table>

Finally, adding the newly estimated joint friction components $\hat{\tau}_f(\dot{q}) = [\hat{\tau}_{f,1}(\dot{q}_1) \ \hat{\tau}_{f,2}(\dot{q}_2) \ \dots \ \hat{\tau}_{f,7}(\dot{q}_7)]^T$ to the previous estimations $\hat{\tau}$, we obtained a satisfactory compensation, as shown with the blue solid lines of Fig. 7.2.

Despite the estimated inverse dynamic model in Lagrangian form described so far, is very cumbersome and computationally intensive for a 7 DoF robot, we manage to release a C++ library, with some naive code optimizations, that reliably works under hard real-time constraints <fnref n="2" /> (the mean execution time of the model is ~ 0.05 ms on a Dell Latitude 7490 - Intel CORE i7-8650U - 32Gb RAM). For performance purposes, a Newton-Euler (N-E) approach is often more appropriate and effective to quickly return joint torque estimates, due to its recursive form. Nevertheless, a N-E routine requires the dynamic parameters (masses, inertia tensors and centers of mass of each link) of the robot. Exploiting the parameters retrieval algorithm (7.13), though, we are able to extract a *feasible* set of dynamic parameters, which provides the same dynamics (although there is no guarantee that the estimated robot parameters set is coincident with the real one).

In order to implement Algorithm 1, the (bounded) simulated annealing Matlab function `simulannealbnd` has been used, together with the IP hybrid function `fmincon`; the problem parameters were $Q = 100$ (total number of independent runs, providing $Q$ solutions – possibly coincident, since there might exist multiple minima) and $\kappa = 10$ (number of successive runs). The penalty functions $g(h_t(\mathbf{p}_k))$ are chosen as the distance functions of the external constraints, that is:

$$ 
\begin{aligned}
g_{1,i} &= -\min\{0, \text{tr}(\mathbf{I}_{\ell_i})/2 - \lambda_{\max}(\mathbf{I}_{\ell_i})\}, \quad i \in [1, \dots, 7] \\
g_2 &= -\min\left\{0, m_{\text{rob,max}} - \sum_{i=1}^7 m_i, \sum_{i=1}^7 m_i - m_{\text{rob,min}}\right\}
\end{aligned} 
\quad (7.16) 
$$

where $g_{1,i}$ regards the triangle inequality on each inertia tensor (eq. 2.42), while $g_2$ regards the constraint on the total mass of the robot (we chose $m_{\text{rob,min}} = 16$ kg and $m_{\text{rob,max}} = 20$ kg).

<footnote n="2">²The Lagrangian C++ library can be found in the following repository https://github.com/marcocognetti/FrankaEmikaPandaDynModel/tree/master/cpp.</footnote>
<figure><img src="image_47.png" /><figcaption>**Figure 7.3** – Joint friction estimates. The red dots are the differences $\Delta\tau_j = \tau_j - \hat{\tau}_j$ for each joint j, while the blue lines are the sigmoidal fitting functions. The effect of friction is clear for joints 4...7, while its contribution to the total joint torque is slight for the other joints.</figcaption></figure>

A feasible set of dynamic parameters $\hat{p}$ has been then retrieved and its numerical values are reported in Tabs. 7.4 and 7.5.

In order to validate this set, we inserted the retrieved dynamic parameters in a N-E routine in order to compute the joint torques $\hat{\tau}_{ne}$ necessary to perform a given validation trajectory in the joint space. In particular, we commanded a sequence of sinusoidal trajectories (with different amplitudes and periods) to the joints according to eq. (7.15). We then compared the measured joint torques $\tau$ with the estimations $\hat{\tau}_{ne} + \hat{\tau}_f$: the result of this comparison is reported in Fig. 7.4, showing good results.

### 7.5.1 Validation on a physics simulator

In order to validate the set of identified parameters reported in Tables 7.4 and 7.5, we built a robot model in CoppeliaSim [75], as shown in Figure 7.5, from which we subsequently derived the simulator presented in Chapter 8. We adjusted the parameters for each link, in order to be compatible with the CoppeliaSim interface (e.g., for each link, the inertia tensor in CoppeliaSim has to be expressed w.r.t.
<figure><img src="image_48.png" /><figcaption>**Figure 7.4** – Comparison between the torques $\hat{\tau}_{ne}$ (including friction $\hat{\tau}_f$) from the N-E dynamic model and the ones retrieved from the joint torque sensors. The overlapping of two signals shows the quality of the obtained parameters estimation $\hat{p}$.</figcaption></figure>

the given CoM reference frame).

The validation process follows the idea that if the identification of the robot dynamic parameters is satisfactory, the measured torques on the real robot, apart from friction which is not simulated in CoppeliaSim, should match the ones provided by the physics engines of the simulator when performing the same trajectory.

In particular, CoppeliaSim provides four different free licence physics engines. A performance comparison of these engines is reported in Figs. 7.6 and 7.7. Both the real and the simulated robot are commanded through sinusoidal joint velocity inputs (spanning their maximum available range). Figure 7.6 shows, for each joint, a filtered version of the measured torques of the real robot (solid blue lines) vs. those provided by the different physics engines (light blue: Bullet 2.78, orange: Bullet 2.83, yellow: ODE, and purple: Newton). As it can be seen, the simulated torques for all engines have a good overlap with the measured ones. Figure 7.7 shows the mean and the standard deviation of the torque errors for each joint.
<figure><img src="image_49.png" /><figcaption>**Figure 7.5** – The Franka Emika Panda robot in CoppeliaSim. The released model is available by default since the version V4.0.0.</figcaption></figure>

This error is defined as the difference between the measured torque on the real robot and the one generated from the simulation. We can notice that the mean torque errors are close to zero while the peaks of the standard deviations are due to measurement noise on the real robot torque readings. In our experiments, all engines showed a good numerical performance, as confirmed by the values in Table 7.3, and looking at the results in joint space, ODE (although it is the most noisy among the engines) and Bullet 2.83 seem to be the best performing engines in CoppeliaSim.

<table><caption>**Table 7.3** – Mean values of the torque errors at the robot joints for each physics engine available in CoppeliaSim.</caption><thead><tr><th></th><th>Joint 1</th><th>Joint 2</th><th>Joint 3</th><th>Joint 4</th><th>Joint 5</th><th>Joint 6</th><th>Joint 7</th><th>units</th></tr></thead><tbody><tr><th>Bullet 2.78</th><td>0.0554</td><td>1.7616</td><td>-0.7363</td><td>-0.0111</td><td>0.0735</td><td>0.0970</td><td>-0.0372</td><td>N·m</td></tr><tr><th>Bullet 2.83</th><td>0.0508</td><td>0.5735</td><td>-0.2617</td><td>-0.0844</td><td>0.0698</td><td>0.0628</td><td>-0.0386</td><td>N·m</td></tr><tr><th>ODE</th><td>0.0474</td><td>0.1782</td><td>-0.0986</td><td>-0.1493</td><td>0.0795</td><td>0.0397</td><td>-0.0386</td><td>N·m</td></tr><tr><th>Newton</th><td>0.1091</td><td>-0.8096</td><td>-0.1440</td><td>-1.4133</td><td>0.0989</td><td>0.0027</td><td>-0.0255</td><td>N·m</td></tr></tbody></table>

To confirm the quality of the proposed solution, we analyzed the performance also in Cartesian space. To this aim, we compared the 3D pose (as obtained from CoppeliaSim) of the end-effector of the simulated robot with the corresponding pose obtained by reading the joint positions from the real robot and passing them to the direct kinematics module. We defined then a position and an orientation error (following the Roll-Pitch-Yaw convention), whose mean values are reported
<figure><img src="image_50.png" /><figcaption>**Figure 7.6** – Comparison between the torques generated by the real robot during an experiment (blue) and those obtained from the simulated Panda robot in CoppeliaSim using different physics engines (Bullet 2.78, Bullet 2.83, ODE, Newton).</figcaption></figure>

<figure><img src="image_51.png" /><figcaption>**Figure 7.7** – Torque errors (w.r.t. the real robot) of the physics engines Bullet 2.78, Bullet 2.83, ODE, and Newton in CoppeliaSim during an experiment with the Panda robot for each of its 7 joints. The boxes indicate mean values of the errors while the bars denote their standard deviations.</figcaption></figure>

in Fig. 7.8 for all physics engines available in CoppeliaSim. This comparison is shown also in the video accompanying [7] (https://ieeexplore.ieee.org/document/8772145/media#media).

The results support once again the quality of the estimated dynamic parameters, since the mean position error is below 1.5 cm and the mean orientation error is below 3 degrees for all the engines. Particularly good results are obtained with the ODE engine, with mean errors of less than 5 mm in position and below 1.2 deg in orientation. Moreover, the maximum errors for the worst performing
<figure><img src="image_52.png" /><figcaption>**Figure 7.8** – The position (left) and orientation (right) mean error for the different physics engines available in CoppeliaSim. While all the physics engines show a good performance, ODE seems to outperform the others.</figcaption></figure>

physics engine (Bullet 2.78) are 9.13 cm in position and 3.14 deg in orientation. Notice that the velocity command in CoppeliaSim was given in open-loop. Furthermore, the simulation step was set to 20 ms in order to run faster simulations albeit leading to greater integration errors in the physics engines solver. Better performance can be achieved with a smaller simulation step as we will see in the results of the next chapter.

## 7.6 Summary

In this chapter we addressed the problem of extracting a feasible set of parameters that characterizes the dynamics of the robot. We identified the dynamic coefficients through a standard least squares algorithm by means of a reverse engineering approach. Thanks to an improved version of the algorithm proposed in [20], we retrieved a set of feasible parameters by solving a nonlinear optimization problem, taking into account several constraints including the physical bounds on the dynamic parameters (such as the total mass of the robot) and the triangle inequalities of the link inertia tensors. The proposed framework, was validated by deriving the Lagrangian model of the Franka Emika Panda robot and testing it through experiments. We also validated the extracted feasible set of dynamic parameters using a Newton-Euler routine <fnref n="3" />. To the best of the authors' knowledge, this was the first work that retrieves the dynamic coefficients and a feasible parameter set for the Panda robot. A major feature of the proposed framework is that it can be easily modified to include further (possibly) nonlinear

<footnote n="3">³The dynamic identification procedure presented in this chapter has been performed accordingly to a reverse engineering approach. In the Appendix A, however, the reader can find also the results of the classical identification approach.</footnote>
constraints (e.g., based on a *priori* information on the robot). We have released both the dynamic model and the parameters retrieval framework as open-source code, so that they are available to the robotics community at the following website: https://github.com/marcocognetti/FrankaEmikaPandaDynModel.

The results presented in this chapter are issued in the paper:

* C. Gaz, M. Cognetti, **A. Oliva**, P. Robuffo Giordano, and A. De Luca, "Dynamic identification of the Franka Emika Panda robot with retrieval of feasible parameters using penalty-based optimization," *IEEE Robotics and Automation Letters*, 2019

The supplementary material and accompanying video is reachable at https://ieeexplore.ieee.org/document/8772145/media#media.
<table><caption>**Table 7.5** – Lower and upper bounds for the inertia tensor elements of the Panda robot and their corresponding estimated values.</caption><thead><tr><th>parameter</th><th>LB</th><th>UB</th><th>p̂</th><th>units</th></tr></thead><tbody><tr><td>I<sub>1xx</sub></td><td>0</td><td>1</td><td>7.0337e-01</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>1xy</sub></td><td>-1</td><td>1</td><td>-1.3900e-04</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>1xz</sub></td><td>-1</td><td>1</td><td>6.7720e-03</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>1yy</sub></td><td>0</td><td>1</td><td>7.0661e-01</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>1yz</sub></td><td>-1</td><td>1</td><td>1.9169e-02</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>1zz</sub></td><td>0</td><td>1</td><td>9.1170e-03</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>2xx</sub></td><td>0</td><td>1</td><td>7.9620e-03</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>2xy</sub></td><td>-1</td><td>1</td><td>-3.9250e-03</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>2xz</sub></td><td>-1</td><td>1</td><td>1.0254e-02</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>2yy</sub></td><td>0</td><td>1</td><td>2.8110e-02</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>2yz</sub></td><td>-1</td><td>1</td><td>7.0400e-04</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>2zz</sub></td><td>0</td><td>1</td><td>2.5995e-02</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>3xx</sub></td><td>0</td><td>1</td><td>3.7242e-02</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>3xy</sub></td><td>-1</td><td>1</td><td>-4.7610e-03</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>3xz</sub></td><td>-1</td><td>1</td><td>-1.1396e-02</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>3yy</sub></td><td>0</td><td>1</td><td>3.6155e-02</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>3yz</sub></td><td>-1</td><td>1</td><td>-1.2805e-02</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>3zz</sub></td><td>0</td><td>1</td><td>1.0830e-02</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>4xx</sub></td><td>0</td><td>1</td><td>2.5853e-02</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>4xy</sub></td><td>-1</td><td>1</td><td>7.7960e-03</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>4xz</sub></td><td>-1</td><td>1</td><td>-1.3320e-03</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>4yy</sub></td><td>0</td><td>1</td><td>1.9552e-02</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>4yz</sub></td><td>-1</td><td>1</td><td>8.6410e-03</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>4zz</sub></td><td>0</td><td>1</td><td>2.8323e-02</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>5xx</sub></td><td>0</td><td>1</td><td>3.5549e-02</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>5xy</sub></td><td>-1</td><td>1</td><td>-2.1170e-03</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>5xz</sub></td><td>-1</td><td>1</td><td>-4.0370e-03</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>5yy</sub></td><td>0</td><td>1</td><td>2.9474e-02</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>5yz</sub></td><td>-1</td><td>1</td><td>2.2900e-04</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>5zz</sub></td><td>0</td><td>1</td><td>8.6270e-03</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>6xx</sub></td><td>0</td><td>1</td><td>1.9640e-03</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>6xy</sub></td><td>-1</td><td>1</td><td>1.0900e-04</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>6xz</sub></td><td>-1</td><td>1</td><td>-1.1580e-03</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>6yy</sub></td><td>0</td><td>1</td><td>4.3540e-03</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>6yz</sub></td><td>-1</td><td>1</td><td>3.4100e-04</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>6zz</sub></td><td>0</td><td>1</td><td>5.4330e-03</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>7xx</sub></td><td>0</td><td>1</td><td>1.2516e-02</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>7xy</sub></td><td>-1</td><td>1</td><td>-4.2800e-04</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>7xz</sub></td><td>-1</td><td>1</td><td>-1.1960e-03</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>7yy</sub></td><td>0</td><td>1</td><td>1.0027e-02</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>7yz</sub></td><td>-1</td><td>1</td><td>-7.4100e-04</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>7zz</sub></td><td>0</td><td>1</td><td>4.8150e-03</td><td>kg·m<sup>2</sup></td></tr></tbody></table>

<table><caption>**Table 7.4** – Lower and upper bounds for masses and centers of mass of the Panda robot and their corresponding estimated values.</caption><thead><tr><th>parameter</th><th>LB</th><th>UB</th><th>p̂</th><th>units</th></tr></thead><tbody><tr><td>m<sub>1</sub></td><td>0</td><td>10</td><td>4.970684</td><td>kg</td></tr><tr><td>m<sub>2</sub></td><td>0</td><td>10</td><td>0.646926</td><td>kg</td></tr><tr><td>m<sub>3</sub></td><td>0</td><td>10</td><td>3.228604</td><td>kg</td></tr><tr><td>m<sub>4</sub></td><td>0</td><td>10</td><td>3.587895</td><td>kg</td></tr><tr><td>m<sub>5</sub></td><td>0</td><td>10</td><td>1.225946</td><td>kg</td></tr><tr><td>m<sub>6</sub></td><td>0</td><td>10</td><td>1.666555</td><td>kg</td></tr><tr><td>m<sub>7</sub></td><td>0</td><td>10</td><td>7.35522e-01</td><td>kg</td></tr><tr><td>c<sub>1x</sub></td><td>-0.05</td><td>0.05</td><td>3.875e-03</td><td>m</td></tr><tr><td>c<sub>1y</sub></td><td>-0.05</td><td>0.05</td><td>2.081e-03</td><td>m</td></tr><tr><td>c<sub>1z</sub></td><td>-0.4</td><td>0.05</td><td>*</td><td>m</td></tr><tr><td>c<sub>2x</sub></td><td>-0.05</td><td>0.05</td><td>-3.141e-03</td><td>m</td></tr><tr><td>c<sub>2y</sub></td><td>-0.15</td><td>0.05</td><td>-2.872e-02</td><td>m</td></tr><tr><td>c<sub>2z</sub></td><td>-0.05</td><td>0.05</td><td>3.495e-03</td><td>m</td></tr><tr><td>c<sub>3x</sub></td><td>-0.05</td><td>0.15</td><td>2.7518e-02</td><td>m</td></tr><tr><td>c<sub>3y</sub></td><td>-0.05</td><td>0.05</td><td>3.9252e-02</td><td>m</td></tr><tr><td>c<sub>3z</sub></td><td>-0.1</td><td>0.05</td><td>-6.6502e-02</td><td>m</td></tr><tr><td>c<sub>4x</sub></td><td>-0.15</td><td>0.05</td><td>-5.317e-02</td><td>m</td></tr><tr><td>c<sub>4y</sub></td><td>-0.05</td><td>0.15</td><td>1.04419e-01</td><td>m</td></tr><tr><td>c<sub>4z</sub></td><td>-0.05</td><td>0.05</td><td>2.7454e-02</td><td>m</td></tr><tr><td>c<sub>5x</sub></td><td>-0.05</td><td>0.05</td><td>-1.1953e-02</td><td>m</td></tr><tr><td>c<sub>5y</sub></td><td>-0.05</td><td>0.05</td><td>4.1065e-02</td><td>m</td></tr><tr><td>c<sub>5z</sub></td><td>-0.05</td><td>0.05</td><td>-3.8437e-02</td><td>m</td></tr><tr><td>c<sub>6x</sub></td><td>-0.05</td><td>0.15</td><td>6.0149e-02</td><td>m</td></tr><tr><td>c<sub>6y</sub></td><td>-0.05</td><td>0.05</td><td>-1.4117e-02</td><td>m</td></tr><tr><td>c<sub>6z</sub></td><td>-0.05</td><td>0.05</td><td>-1.0517e-02</td><td>m</td></tr><tr><td>c<sub>7x</sub></td><td>-0.05</td><td>0.05</td><td>1.0517e-02</td><td>m</td></tr><tr><td>c<sub>7y</sub></td><td>-0.05</td><td>0.05</td><td>-4.252e-03</td><td>m</td></tr><tr><td>c<sub>7z</sub></td><td>0.04</td><td>0.15</td><td>6.1597e-02</td><td>m</td></tr></tbody></table>
CHAPTER

8

FrankaSim

Contents

<table><tr><td>8.1</td><td>Introduction</td><td>130</td></tr><tr><td>8.2</td><td>Related Works</td><td>130</td></tr><tr><td>8.3</td><td>The simulator: FrankaSim</td><td>132</td></tr><tr><td>8.3.1</td><td>Kinematics</td><td>132</td></tr><tr><td>8.3.2</td><td>Dynamics</td><td>133</td></tr><tr><td>8.3.3</td><td>ViSP</td><td>135</td></tr><tr><td>8.3.4</td><td>VispRos</td><td>136</td></tr><tr><td>8.3.5</td><td>CoppeliaSim</td><td>136</td></tr><tr><td>8.3.6</td><td>Software Architecture</td><td>137</td></tr><tr><td>8.4</td><td>Experiments</td><td>138</td></tr><tr><td>8.4.1</td><td>Single-Arm Experiment: Real vs Simulated</td><td>138</td></tr><tr><td>8.4.2</td><td>The Dual-Arm Experiment</td><td>140</td></tr><tr><td>8.5</td><td>Summary</td><td>141</td></tr></table>
## 8.1 Introduction

Software simulators are valuable proof-of-concept tools for validating methods, theories and ideas in a simple and cost-effective way. They are useful for testing and validating ideas and algorithms that would otherwise be hard to implement due to limited resources or restrictions [101]. For instance, recently robot simulators are being exploited for generating large amounts of training data for training neural networks allowing to reach training levels that would be impossible with data collected on real hardware [102, 103].

Any robotic simulator must be able to simulate different kinds of robots, actuators and sensors with enough accuracy for closing the reality gap. The most common robotic system that is featured by virtually any robotic simulator is a manipulator arm for tasks involving motion control, pick-and-place, interaction with the environment, and so forth. A correct dynamic modeling of a manipulator is of paramount importance in relation to problems of motion simulation, analysis of manipulation structures and control algorithms. Simulating the motion of a manipulator allows indeed to safely test control strategies and trajectory planning techniques in a low-risk environment without the need to refer to a physically available platform. Modern robotics simulators are flexible and dynamic and can handle complex physics simulations as well as disparate sensors that a user may need to evaluate. In this respect, the goal of this work is to propose a *high-fidelity* and *openly available* simulator for a popular manipulator arm, the Panda robot manufactured by Franka Emika, with in addition, as case study, the integration and exploitation of the simulator for achieving visual-servoing tasks [44]. Indeed, visual-servoing is a very popular set of techniques for controlling the motion of a robot from visual input provided by one or more cameras, and in this chapter we show how an accurate dynamical simulation of the Panda robot can be exploited to perform realistic and non-trivial visual servoing schemes.

The robot considered in this work is, as the name of the simulator suggest, the Panda robot by Franka Emika. For more details on the robot refer to Section 7.2.

## 8.2 Related Works

The manufacturer has not released, to the best of our knowledge, the official dynamic parameters of the Panda robot and before the recent identification results reported in [7], no parameters were publicly available. To overcome this drawback, a first attempt to obtain an estimation of the dynamic parameters of the Panda was made in [104] by taking the original URDF description files and the robot
meshes from the official repository and adding the missing gazebo-specific tags and inertial parameters (mass, center of mass and inertia tensor) recovered from a CAD-based software. Once the identified dynamic parameters became available, some researchers started building their own simulators from the same repository and tweaking the URDFs [105, 106] to achieve more realistic simulations both in Gazebo and MuJoCo [107] respectively. In [108], the authors built a Panda simulation platform using Simscape Multibody MATLAB Toolbox based on the kinematic, dynamic and friction models identified in [7]. Simscape is neither open nor free, and the developed code for the simulation cannot be transferred to real implementations.

As reported in the surveyed literature in [101], the most important criterias for choosing a simulator are: **small reality gap** (32%), **to be open-source** (24%), **light & fast** (11%), **simulation-to-real code transfer** (9%), **customization** (6%), **others** (5%), **no inter-penetration between bodies** (3%). Despite Gazebo is the most used simulator in research, CoppeliaSim (formerly V-REP) [75] scores the best in the previous criteria [101] while [109] pointed out that it offers a number of useful features, such as multiple physics engines (Bullet, ODE, Vortex and Newton), a comprehensive model library, the ability of a user to interact with the world during simulation and, most importantly, mesh manipulation and optimisation. Moreover, it automatically spawns new threads on multiple CPU cores and therefore utilises the full amount of CPU power when necessary. It is therefore suitable for high-precision modelling although it is the most demanding in terms of resources among the compared simulators. In the reality gap analysis conducted in [110] on a grasping task, CoppeliaSim with Newton and Vortex performed 1st and 3rd respectively

Most of the reviewed works are based on Gazebo, which has some limitations compared to CoppeliaSim which, despite its higher demand on resources, is more accurate and faithful to reality. To the best of our knowledge, the simulator proposed in this work is the only dynamic simulator supporting CoppeliaSim. Besides offering the characteristics of the other Panda simulators in terms of kinematics, dynamic model parameters and ROS integration, this simulator further offers full integration within the ViSP library [10], enabling it with visual-servoing capabilities. There exist some other libraries allowing for rapid prototyping of visual-servoing applications [111, 112], but probably ViSP is the most complete open-source visual-servoing library.

The proposed simulator features a Lagrangian Dynamic model library that can provide the Coriolis matrix of the manipulator and the estimated joint dynamic friction, unlike dynamic models based on standard Newton-Euler algorithms. Fi-
nally, being the code of the simulator implemented as a replica of the ViSP class
that wraps the *libfranka*, it greatly facilitates the possibility of transfer verified
code to the real robot. This simulator lends itself well to simulate Dynamic Visual-
Servoing [46] or Vision-Force control schemes [5, 55, 57], relieving the user from
the burden of implementing his/her own simulation platform.

8.3 The simulator: FrankaSim

The simulator proposed in this work, is a co-simulator that includes several con-
veniently integrated components allowing for rapid prototyping and testing of
manipulator controllers. It is mainly based on ViSP code that provides a bulk
of utilities such as native support to linear algebra and transformation matrices,
among others, greatly simplifying both the simulator implementation itself as well
as the deployment of user defined controllers. ViSP further extends the simulator
control skills by enabling it with visual servoing control capabilities. A physical
simulation is performed in CoppeliaSim, in which one (or more) instance(s) of a
Panda robot can be used, with its identified dynamic model parameters [7]. The
communication between CoppeliaSim and the C++ instance of a simulated robot
takes place through ROS. All the software presented in this work, the CoppeliaSim
scenes and models as well as different experiments, for both the simulated and real
robot, are available on https://github.com/lagadic/vispRos while step-by-
step tutorials can be found in the ROS wiki https://wiki.ros.org/vispRos.

A high-level view of the architecture and operation of the simulator software
will be provided below, after having detailed the various components.

8.3.1 Kinematics

As we have shown in Chapter 2, a manipulator can be mechanically described
as a kinematic chain consisting of rigid bodies (links) connected by means of
revolute or prismatic joints, which constitute the DoF of the structure. One end
of the chain is bounded to the floor while at the other extreme a tool is usually
attached (see Fig. 8.1). The kinematic description of the Panda according to its
Denavit-Hartenberg parameters was shown in Section 2.2.

In this subsection, under the term *kinematics* we encompass either direct,
inverse and differential kinematics (see Chapter 2 for further details).

In FrankaSim, we delegate the issue of dealing with kinematics to the well-
known kinematics and dynamics library OROCOS_KDL [113]. The Orocos
project aims at providing kinematic and dynamic code usable in real time; it
<figure><img src="image_53.png" /><figcaption>**Figure 8.1** – An instance of the Panda model in CoppeliaSim with its gripper and a wrist mounted camera. The floor $Σ_f$, flange $Σ_{fl}$, end-effector $Σ_e$ and camera $Σ_c$ frames are depicted as well as some transformation matrices between them.</figcaption></figure>

contains code for rigid body kinematic representation and calculations for structures and their direct and inverse kinematic solvers.

### 8.3.2 Dynamics

The dynamic model of a manipulator provides a description of the relationship between the actuation torques at the joints and the motion of the structure. The Lagrange formulation of the equation of motion has been presented in Section 2.3.

In the previous chapter, an accurate dynamic model of the robot has been identified and a set of feasible dynamic parameters retrieved. These parameters have been adopted in the MATLAB Robotics toolbox<fnref n="1" /> [114] alongside many hand-made URDFs of other simulators [105, 106] and, more recently, were also included in the official URDF model provided by the manufacturer<fnref n="2" />, making our identified model, *de facto*, the standard model.

From the dynamic parameters identified in [7], the authors released both

<footnote n="1">¹https://github.com/petercorke/robotics-toolbox-matlab/blob/master/models/mdl_panda.m</footnote>

<footnote n="2">²https://github.com/frankaemika/frankaanie/mik/a/frankaanie/mik/a/franka_ros/blob/develop/franka_description/robots/panda_gazebo.xacro</footnote>
a MATLAB and C++ libraries as open-source software<fnref n="3" /> under GPLv3 license and a CoppeliaSim model of the Panda (arm only, see Figure 8.1). These libraries provide an estimate of the dynamic terms of the Lagrangian model (2.27) ($\hat{B}(q), \hat{C}(q, \dot{q}), \hat{g}(q), \hat{\tau}_f(\dot{q})$) computed from the symbolic expressions of the equation of motion and identified parameters. The main limitation of this library is that the model was identified until the flange of the Panda arm, making it useless if one wants to attach a payload on it, e.g. the Panda Hand gripper, for which we have also provided a model for CoppeliaSim within the simulator package (see Fig 8.2).

In the work presented in this chapter, we have further extended this library, parameterizing it with respect to both the payload parameters, as reported in [83], and the gravitational acceleration vector. This way the library can provide the same features offered by Newton-Euler (N-E) algorithms, i.e. adding payloads to the chain or mounting the robot in a position different from vertical, while providing the full Coriolis matrix and the viscous friction terms. This constitutes an advantage w.r.t. standard N-E algorithms since they are not able to provide neither the Coriolis matrix $\hat{C}(q, \dot{q})$ -or any of its columns- nor the mixed velocity term $\hat{C}(q, \dot{q})\dot{q}_r$, with $\dot{q}_r$ a reference velocity different from the one used to compute $\hat{C}(q, \dot{q})$ [115], but only the resulting vector $\hat{c}(q, \dot{q}) = \hat{C}(q, \dot{q})\dot{q}$. These matrices are needed, for instance, to evaluate the residual vector for collision detection and safe reaction applications [116] or to implement passivity-based trajectory tracking control laws [117]. Although [115] has proposed a modified N-E algorithm to overcome the aforementioned limitation of the standard N-E method, it is not implemented in OROCOS\_KDL nor it is provided by the *libfranka*. This motivates the use of our library both to avoid implementing this method in OROCOS\_KDL from scratch and because this library can be easily integrated within the real robot controller code without having the dependency of the KDL library.

To add a tool to the real robot, one has to pass through the Panda's Desk web interface to specify both the new pose of the end-effector in flange frame ($\Sigma_{fl}$) and the dynamic parameters. Figure 8.2 reports those values for the real gripper. Due to possible simulation malfunctioning with some physics engines, such as Bullet and ODE mainly, when the difference between connected masses/inertias is too large it is convenient to split the total mass of the gripper among the fingers and the body uniformly, in order to avoid “strange” behaviours. By doing so, the total mass will stay the same but the inertia tensor and the location of the center of

<footnote n="3">³https://github.com/marcocognetti/FrankaEmikaPandaDynModel</footnote>
<figure><img src="image_54.png" /><figcaption>**Figure 8.2** – CoppeliaSim model of the Franka Hand gripper. End-effector position $^f\mathbf{r}_e = [0; 0; d_e]^\top [m]$ and orientation $^f\mathbf{R}_e = R_z(\theta_e)$ and CoM position $^f\mathbf{r}_{\text{CoM}} = [-0.01; 0; 0.03]^\top [m]$ in flange frame. Mass $m_L = 0.73 \text{ [kg]}$ and inertia tensor $\mathbf{I}_L = \text{diag}(0.001; 0.0025; 0.0017) \text{ [kg.m}^2\text{]}. \\ \theta_e = -45^\circ, d_e = 0.1034[m]$.</figcaption></figure>

mass of the overall gripper will differ from the real one. This is not an issue since, as we will see in Section 8.3.5, we automatically gather this information from the simulator and send it to the robot through ROS.

### 8.3.3 ViSP

ViSP<fnref n="4" /> or Visual Servoing Platform, is a modular C++ library developed and maintained by the Rainbow team (former Lagadic) at Inria/IRISA Rennes, France, that allows for fast development of visual servoing applications. This library was designed to be hardware independent, simple, portable and easily extendable. Furthermore, it features a large class of elementary tasks with various visual features ranging from image points, to image lines and moments, 3D pose estimation and so forth, as well as providing native support for linear algebra operations, visual trackers, plotters, and of course, visual-servoing controllers.

There is already a class in ViSP that wraps the *libfranka* robot API named *vpRobotFranka*. This is a multi-thread implementation that runs the robot control routine in a separated thread at the control rate (1 kHz) allowing the main program to continue its own execution; this means that from the same *main()*, one can read and process information coming from other sensors, like cameras and force/torque sensors, even at slower sampling rates. This implementation allows to simultaneously control more than one robot at a time.

<footnote n="4">⁴https://visp.inria.fr</footnote>
lation step between the C++ code and the physical simulation. In order to make
the synchronization more transparent to the user, the triggers were embedded in
the *vpROSRobotFrankaCoppeliasim* class, and a flag has to be selected to enable
it.

Within the simulator we deployed the scenes of the experiments, the model of
the Panda Hand gripper (see Figure 8.2) and some AprilTags.

8.3.6 Software Architecture

<figure><img src="image_55.png" /><figcaption>**Figure 8.3** – Block diagram of the main components of the FrankaSim architecture. An UML-like diagram highlights the principal class relationships while a wide range of visualization and debugging possibilities at ViSP (green), ROS (blue) and CoppeliaSim (red) levels is depicted.</figcaption></figure>

A block diagram of the co-simulation environment with some of the C++
classes and dependencies in a UML-like diagram is shown in Figure 8.3. The
diagram is complemented with some data visualization utilities provided by the
various software components that help the user in analyzing the system perfor-
mance and/or debugging.

A simulation is built as a ROS node that benefits from both FrankaSim and ViSP capabilities to implement one or multiple Panda robots controllers. The scene rendering and the physics engine are supported by CoppeliaSim. Communication between FrankaSim and CoppeliaSim is performed using ROS communication level. The ROS node could be seen as a *main()* that firstly initializes
the simulation (e.g. establishing robot and camera connections, initialize position and so on) and then enters an infinite *while* loop. At each iteration of the loop, from the robot state measurements (position, velocity or torque) and, potentially any other sensor measurement like an image acquired by a virtual camera, a new joint/Cartesian velocity/torque command is computed and sent back to CoppeliaSim to update the scene rendering. There exists a mechanisms that allows to perform a synchronous execution between the simulation step in CoppeliaSim and the control program. Typically, for a visual servoing simulation, one iteration of the loop could be synchronized at 20 ms (like for a real camera), while for impedance control it is recommended to reduce this time step between 1 and 3 ms (the simulation will be more accurate with shorter time steps but will last longer). FrankaSim has been designed in such a way as to hide ROS from the user, making its use transparent. This approach makes it very easy to prototype an application in simulation first, and then to deploy it on a real robot by just changing few lines of code, as we will see in Section 8.4.1.

## 8.4 Experiments

In this section we present two experiments to evaluate on the one hand the reality gap between the simulation and the real hardware both from the implementation and the obtained results point of view and, on the other hand, to show the great simulation potential of the proposed package.

### 8.4.1 Single-Arm Experiment: Real vs Simulated

In this experiment we consider the following joint impedance controller

$$ \tau_t = B(q) \left( Ke + D\dot{e} + I \int e dt + \ddot{q}_d \right) + C(q, \dot{q})\dot{q} + \tau_f(\dot{q}) - \tau_0 e^{-\mu t} \quad (8.1) $$

with $\tau_t$ the joint torque command at time $t$, $q_d(t)$ a desired joint trajectory, $e = q_d - q$, and $K, D, I$ are the proportional, derivative and integral diagonal gain matrices respectively. The exponential term allows for a smooth start of the commands making the computed joint torques start from zero having set $\tau_0 = \tau_t(0)$ equal to the computed torque at $t = 0$ while $\mu$ determines the decay rate. A Panda arm equipped with its gripper and a camera attached to the flange is controlled using (8.1). Figures 8.4 and 8.5 compare the joint positions and torques measured on a real Panda arm versus a simulated one using FrankaSim while applying a desired sinusoidal joint trajectory on joints 1, 3 and 4. As
expected, the obtained results on the real robot are noisier, but perfectly follow
those obtained in simulation. In the video related to this work we show also a
PBVS and an IBVS experiment with an AprilTag target where the visual-servo
behaviors are very close between the real and simulated cases. More than 95% of
the C++ code is common between real and simulated experiments. Only about
ten lines of code are simulator-specific to initiate communication with the robot
and the camera if the latter is used.

<figure><img src="image_56.png" /><figcaption>**Figure 8.4** – Real vs Simulated measured joint positions while tracking a desired joint trajectory using controller (8.1). The total joint position Root Mean Square Error (RMSE) along the whole trajectory ([:0.5; 15] [s]) for all the joints is RMSE_q = 0.006 [rad].</figcaption></figure>

<figure><img src="image_57.png" /><figcaption>**Figure 8.5** – Real vs Simulated measured joint torques while tracking a desired joint trajectory using controller (8.1). The total RMS torque error along the whole trajectory ([:0.5; 15] [s]) for all the joints is RMSE_τ = 0.5546 [Nm].</figcaption></figure>
8.4.2 The Dual-Arm Experiment

This example was designed to showcase together most of the salient features of the
simulator, i.e., the possibility to handle multiple robots in the same program, the
ability to control the robot in velocity or in torque, the capacity of the presented
Lagrangian dynamic model library to fully compensate for the gravity and the
payload (gripper) even when not vertically mounted, the synchronous execution
with the physics simulator, as well as simulating a lower camera rate and a Visual-
admittance Pose-Based Visual-Servoing in interaction with the environment.

<figure><img src="image_58.png" /><figcaption>**Figure 8.6** – Dual-Arm Experiment: two Panda robots constitute the arms of “Franko”. The torque controlled left arm holds an AprilTag while following a circular trajectory. The right arm implements an Extended External Hybrid controller to follow the AprilTag while dealing with the collision with the environment. The circular and “D” shaped paths of both arms are visible (blue lines). The camera view and some plotted curbs are shown in the bottom of the figure.</figcaption></figure>

The simulation platform consists of a dual arm manipulator named “Franko”, equipped with two Panda arms with their grippers and a wrist mounted camera on the right arm. The left arm is torque controlled; it implements a computed torque Cartesian controller and holds an AprilTag of the family 36h11. The right arm implements an Extended External Hybrid controller [5] to visually follow the AprilTag. At start, both arms are not servoed and the left one is only compensating for the gravitational effects (both for the arm and the gripper), then it is commanded to reach a desired pose in the workspace while the right arm is commanded to make sure that the AprilTag is centered in the image at a distance of 20 cm. Once the visual-servoing converges, the left arm starts tracking a circular trajectory with the end-effector, causing the right arm to follow. There is an obstacle on the worktable to which the gripper of the right arm collides with while
chasing the AprilTag, resulting in a “D” shaped trajectory (please see Fig. 8.6 and
related video for more in depth and clear understanding of this experiment).

8.5 Summary

In this chapter, we presented our simulator for the Franka Emika Panda robot. The simulator has been designed with most of the criteria of importance to users in mind i.e., small reality gap, open-source, simulation-to-real code transfer, customization, etc. It has a number of features that make it unique compared to other dynamic simulators for this robot; in fact, to date, it is the only one to support CoppeliaSim and to integrate a library providing the Coriolis matrix and the estimated dynamic friction as well as being fully integrated into the ViSP ecosystem that enables it with visual servoing capabilities. The simulator is currently being used by researchers in the Rainbow team for prototyping and testing novel sensor-based control strategies, to study second-order visual servoing, and the combined use of vision and force sensing for precision assembly tasks. We hope that providing the code to the entire community will allow increasing the number of end-users and applications addressed.

The work presented in this chapter was extracted from the paper:

* **A. A. Oliva**, F. Spindler, P. Robuffo Giordano, and F. Chaumette, "FrankaSim: A Franka Emika's Panda robot simulator with visual-servoing enabled capabilities," 2022 *IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)*. (submitted)

Video: https://www.youtube.com/watch?v=Kxn3pXsK9h4

The video related to this work shows some features of the simulator while an IBVS and PBVS experiments are performed. Then, a complex multi-robot simulation is showcased; the left arm is torque controlled while the right one is commanded in velocity, using our Extended External Hybrid controller, which allows the manipulator to deal with the impacts with the environment. Finally, an experimental comparison between simulated and real robot executing the same trajectory is shown followed by some further experiments carried out along the thesis work.
# Part IV Conclusions and future directions
CHAPTER

9

Conclusion

In this concluding chapter, we want to review the results obtained in this thesis
and point out what might be open points that need further attention.

9.1 Summary and contributions

The goal of this thesis was to understand how to effectively combine visual and
force information within the same control algorithm. The system considered is
a classical serial manipulator equipped with a wrist-mounted 6D force/torque
sensor and a monocular camera, regardless of its mounting (i.e., Eye-in-hand or
Eye-to-hand). What we have set out to accomplish was a *shared* integration of
vision and force sensing along all the controlled directions as well as a general
treatment, that was independent of the visual feature. This led us to explore
second-order models that relate the motion of the visual features in the image
plane to the forces acting on the end-effector of the manipulator. This research
allowed us, as shown in Chapter 5, to express the model of the manipulator in the
visual feature space and from there derive an impedance controller in the same
space. This type of controllers had been little studied in the literature with few
recent works which reported their advantages over the classical kinematic visual
servoing controllers. Those studies had been conducted to control the motion of
the manipulator in free space and their implementations were limited to simulated
environments only. Instead, we investigated these controllers and their feasibility
for interactions control both through simulations and by implementing them on
a real platform. We have proposed an estimator based on the kinematic model
of visual servoing to virtually increase the visual information and thus overcome
one of the biggest limitations of visual control, namely the low data rate from
the camera and latency associated with computer vision algorithms for extracting
visual features. This estimator also allows to estimate the velocity of the tracked
moving target. The results obtained are very promising; we have demonstrated the
feasibility of the practical use of these controllers in tasks of interaction with the
environment on the one hand, and on the other we have obtained, with classic low-
cost cameras, tracking performances comparable to those using dedicated high-frequency cameras and specialized hardware.

On the other hand, when one needs to perform manipulation tasks interacting with the environment under static or quasi-static conditions, the dynamics of the manipulator can be neglected, and it might be convenient to rely on a velocity resolved controller, which results much simpler than computed-torque ones. In this regard in Chapter 6 we have proposed what we have called the Extended External Hybrid Vision-Force Control scheme in which, thanks to an admittance law in feature space, the visual reference is modified accordingly with the measured external wrench allowing to achieve compliant motion, including soft behaviours. This controller, due to its hierarchical structure, avoids both inconsistencies at the actuation level and convergence towards local minima. Moreover, given the presence of a force controller on the external loop, it is possible to regulate the exchanged force/torque with the environment to a precise value. We have also shown how this controller can simultaneously deal with both physical and fictitious forces, the latter being generated in the image plane and that can be used, for example, as repulsive force field preventing the visual features to leave the field of view or in collision avoidance tasks to avoid the detected obstacles.

In both proposed controllers, vision and force sensory integration occurs in visual feature space with consequent compliance achieved along the visual features that define the visual task.

The impedance control presented in Chapter 5 imposes a system behavior equivalent to a mass-spring-damper system in the visual feature space due to the fact that, through feedback linearization, it cancels out the manipulator’s dynamics and imposes the desired behavior. To do this, one needs to know the model of the manipulator. In this regard in Chapter 7 we proposed a framework for the retrieval of a feasible set of the identified dynamic model parameters of the manipulator and we provided for the first time a set of these parameters for our manipulator as well as an accurate model of the dynamic friction in its joints; this model has become *de facto* the standard model adopted by the scientific community for this manipulator.

From this identification procedure, a C++ library has been created which provides all the dynamic coefficients in the Lagrangian model. We have then further developed this library and a dynamic simulator was developed, reported in Chapter 8, which allowed us to obtain the results shown in Chapter 5. This simulator is very useful to rapidly prototype visual servoing applications and allows to easily switch, by modifying a few lines of code, to the implementation on the real robot.

This thesis work has covered several aspects both theoretical and practi-
cal/implementation on the problem of the coupling of vision and force control for manipulators by providing both the necessary tools for its study and implementation. It has also provided innovative control schemes capable of addressing the different interaction tasks that can be encountered in more or less structured environments, responding therefore to the needs raised by the smart production.

## 9.2 Open issues and future perspectives

Despite the proposed EKFs track the target's motion very well and provides high-rate visual information, the control bandwidth is still insufficient to impose higher control gains without becoming unstable. These filters allowed torque-level controllers to be implemented on a real platform with low-cost cameras, but the stability margins are limited if compared to those achieved with traditional impedance control. Time domain passivity could be used to monitor and passivize the extra energy that time delays provide to the system as in [118], but directly in the feature space.

Concerning the sensor integration problem, much research has been done in the last three decades, including this thesis, to couple the contact information from a force/torque sensor with the visual information provided by cameras within a control loop. All the approaches surveyed in this thesis, except for some of the constraint-based methods presented in Section 4.5, use a “task frame” concept in one way or another to specify the task, at least the force goals of the task. This is a legacy that carries over from the fact that integrated vision/force control schemes are in essence repurposed motion/force schemes for which the contact modelization have been done based on the assumption that there is a unique contact point between the end-effector and the environment. However, this is not what actually happens in real scenarios. The sensed contact wrench is the aggregate of all the forces/torques acting downstream the wrist-mounted force/torque sensor [119], and the resultant wrench of a contact situation can be indistinguishable from a completely different other that generates the same measured wrench at the sensor frame. A systematic and general contact model which is intuitive, flexible and is able to deal with multiple, complex, and time-varying motion constraints between non-polyhedral objects, was developed in [120]. They showed that it is nevertheless possible to reconstruct a complex, multi-point contact situation from the measurements of a force sensor alone, but full identification of 3D contact situations with general and unknown objects is difficult. Humans, nature's most versatile manipulators possess a distributed sense of touch, which makes them particularly adept at manipulating objects, reacting to, or even taking advan-
tage of the different perceived contacts. Furthermore, in a recent study, it was shown that human visual and tactile sensory systems share common features in object recognition processes [121]. It might be then worth investigating the use of distributed force sensors, as suggested in [119], because they can provide richer information about the contact than traditional wrist-mounted force/torque sensors. In this context, the main question posed in this thesis would hold, but with a slight difference: *how to effectively combine vision and distributed force sensing?*

There are many tactile sensors based on different physical principles that are already advanced at commercial level. As for the human, artificial vision and tactile sensing share common characteristics, that are more evident when using a visuo-tactile sensor like gelsight [122]. The *tactile images* can be exploited in a way similar to vision, unifying the processing of both visual and tactile data taking advantage of the use of existing methods in the fields of computer vision and visual servoing for the aspects of perception and control respectively

Not only distributed contact information, that is spatially localized in a small surface (like the fingers) is of interest, but also sparse multiple contacts that can occur on the mechanical structure of the manipulator may be; for instance in situations where one has to carry/manipulate a large and/or too heavy object to lift with only the terminal link; in these cases it could be advantageous to “embrace” the object by making contact on several points between the manipulator and the object (imagine someone carrying a large box under the arm). Further questions arise: *how to servo the robot in order to achieve a desired multi contact configuration?*, which is an active field of research if we think at the in-hand manipulation problem. On the other hand, *how to describe the combined vision/force manipulation task?* would perhaps require more attention.

In addition to multi-contact modelling, one also needs sensors capable of per-
ceiving these contacts. Using only proprioceptive sensors [123, 124] monitor the
residual and the robot generalized momentum for contact detection, isolation and
identification (a survey [69]); using probabilistic methods, [125] were able to locate
the point of contact on one of the manipulator's links with good precision using
a particle filter, while with a similar approach [126] performs small exploratory
motions once contact was established to locate the contact. In a different way, the
use of artificial skins could be valuable in retrieving those contacts, but *how can
the knowledge of this multiple contacts be coupled with visual cues in a meaningful
manner?* is still an open question.
## A.1 Comparison of dynamic coefficients

The manufacturer does not provide any information on the dynamic parameters of the Panda robot, and we have no ground truth values at our disposal for the dynamic coefficients (a.k.a., base parameters) that have to be estimated.

Nevertheless, we have compared the dynamic coefficients obtained with the reverse engineering approach in [7], that we discussed on Chapter 7, with those of a classical identification method [127] that uses measurements of the joint positions and joint torques only. In this case, static positioning is no longer sufficient and the robot must execute sufficiently exciting trajectories in order to allow a reliable estimation of the dynamic coefficients. In particular, following [91], we designed the reference trajectories for the joints $j \in \{1, 7\}$ as

$$q_j(t) = \sum_{l=1}^{L} \frac{a_{l,j}}{l\omega_f} \sin(l\omega_f t) - \frac{b_{l,j}}{l\omega_f} \cos(l\omega_f t) + q_{0,j},$$

where $L = 5$ is the number of harmonics, $\omega_f = 0.15\pi$, and the coefficients $a_{l,j}$, $b_{l,j}$ and $q_{0,j}$ are reported in Table A.1. These trajectories have been commanded to the robot joints during 21 s, collecting a total of 20544 samples of positions and torques per joint.

Since we noticed already that joint friction is not negligible for the robot under study, we added in the robot dynamic model suitable friction functions to each joint $j \in \{1, 7\}$, which are linear in the three parameters $f_{v,j}$, $f_{c,j}$, and $f_{o,j}$ (see eq. (2.34)):

$$\tau_{f,j}(\dot{q}_j) = f_{v,j}\dot{q}_j + f_{c,j}\text{sign}(\dot{q}_j) + f_{o,j}.$$

These functions may suffer from sudden discontinuities in the neighbourhood of $\dot{q}_j = 0, j = 1, \dots, 7$, but are adopted here because of their linearity in the defining parameters.

The results of the identification process is summarized in Table A.2. The first column provides the symbolic expressions of the dynamic coefficients. The
<table><caption>**Table A.1** – Trajectory parameters for model identification of the Franka Emika Panda robot.</caption><thead><tr><th>Joint j</th><th>1</th><th>2</th><th>3</th><th>4</th><th>5</th><th>6</th><th>7</th></tr></thead><tbody><tr><td>a<sub>1,j</sub></td><td>-0.2031</td><td>-0.0699</td><td>0.3076</td><td>0.1269</td><td>-0.2773</td><td>0.2100</td><td>-0.2273</td></tr><tr><td>a<sub>2,j</sub></td><td>0.1295</td><td>-0.5380</td><td>0.5864</td><td>-0.0253</td><td>-0.0857</td><td>-0.1194</td><td>-0.5636</td></tr><tr><td>a<sub>3,j</sub></td><td>-0.0090</td><td>-0.2015</td><td>-0.4813</td><td>-0.2405</td><td>0.4810</td><td>-0.0950</td><td>-0.2099</td></tr><tr><td>a<sub>4,j</sub></td><td>0.2319</td><td>-0.5535</td><td>-0.1228</td><td>0.2178</td><td>0.5311</td><td>-0.0964</td><td>0.5725</td></tr><tr><td>a<sub>5,j</sub></td><td>-0.7598</td><td>0.2352</td><td>-0.5273</td><td>0.6984</td><td>-0.1639</td><td>-0.0399</td><td>-0.3748</td></tr><tr><td>b<sub>1,j</sub></td><td>-0.1136</td><td>-0.2437</td><td>0.3898</td><td>0.0401</td><td>0.5491</td><td>0.0692</td><td>0.2023</td></tr><tr><td>b<sub>2,j</sub></td><td>0.6600</td><td>0.1820</td><td>-0.0268</td><td>-0.1503</td><td>0.1046</td><td>-0.6946</td><td>-0.2570</td></tr><tr><td>b<sub>3,j</sub></td><td>-0.1858</td><td>-0.2390</td><td>0.0179</td><td>0.2359</td><td>0.0407</td><td>-0.4442</td><td>-0.5010</td></tr><tr><td>b<sub>4,j</sub></td><td>0.1867</td><td>-0.2647</td><td>0.4767</td><td>-0.2022</td><td>-0.0432</td><td>-0.5749</td><td>0.0774</td></tr><tr><td>b<sub>5,j</sub></td><td>0.2357</td><td>0.4252</td><td>0.2726</td><td>0.3693</td><td>0.2448</td><td>-0.0698</td><td>-0.4833</td></tr><tr><td>q<sub>0,j</sub></td><td>-0.5850</td><td>-0.1744</td><td>-0.3373</td><td>-1.8767</td><td>-1.0631</td><td>1.7917</td><td>-0.7284</td></tr></tbody></table>

second and third columns show the values of the coefficients $\hat{\pi}_{R-E}$ identified using the reverse engineering approach and, respectively, their standard deviations $\%\sigma(\hat{\pi}_{R-E})$. The regressor condition number is 48.9, while the relative error percentage is 0.0315%. The fifth and sixth columns show the values of the coefficients $\hat{\pi}_{CLS}$ identified through the classical method with exciting trajectories and, respectively, their standard deviations $\%\sigma(\hat{\pi}_{CLS})$. In this case, the regressor condition number is 121.88, while the relative error percentage is 1.41%. Finally, the fourth column reports the numerical value of the dynamic coefficients $\pi(\hat{p})$ computed by substituting the values of the dynamic parameters retrieved with our framework [7] (see Chap. 7) in their symbolic expressions. It can be noted that these values are slightly different from the original ones, showing that the ordinary least squares solutions $\hat{\pi}_{R-E}$ and $\hat{\pi}_{CLS}$ are usually not physically consistent [91].

On the other hand, to estimate a set of dynamic coefficients which are physically consistent, the following one-step approach can be used. It is possible, indeed, to change the term $\phi(p_k)$ of the cost function in the Parameters Retrieval Algorithm 1 in chapter 7, as:

$$ \phi(p_k) = \| \bar{Y} \pi(p_k) - \bar{\tau} \|^2, $$

where $\bar{Y}$ is the stacked regressor, $\bar{\tau}$ is the stacked measurements vector and $\pi(p_k)$ is the coefficients vector computed from the current parameters vector $p_k$. The drawback of the use of this function $\phi$ (with respect to the one we reported in chapter 7) is that it is computed from the cumbersome stacked regressor, thus affecting the computational time. The identified dynamic coefficients $\hat{\pi}_{OSI}$ obtained from the previously described one-step identification procedure are reported in the seventh column of Tab. A.2.

Note that the fourth column of Table A.2 has two missing entries for the eight and twenty-fourth dynamic coefficients: these small coefficients are in fact
<table><caption>**Table A.2** – Dynamic coefficients identified using our reverse engineering approach ($\hat{\pi}_{R-E}$) and using a classical approach ($\hat{\pi}_{CLS}$), with their standard deviations (in percentage). The coefficients $\pi(\hat{\pi})$ are computed from the retrieved dynamic parameters $\hat{\pi}$. Dynamic coefficients $\hat{\pi}_{OSI}$ are obtained from a one-step identification procedure.</caption><thead><tr><th>Dynamic coefficients</th><th>π<sub>OSI</sub> - E</th><th>%σ(π<sub>OSI</sub> - E)</th><th>π(π̂)</th><th>π<sub>CLS</sub></th><th>%σ(π<sub>CLS</sub>)</th><th>π<sub>OSI</sub></th></tr></thead><tbody><tr><td>J<sub>229y</sub> + J<sub>212z</sub></td><td>0.0292166719319167</td><td>0.373464999260476</td><td>0.0373379631298541</td><td>0.0204361103682226</td><td>2.36090168554492</td><td>0.0132</td></tr><tr><td>J<sub>229x</sub> - J<sub>229y</sub> + J<sub>212y</sub> + 0.0998m<sub>3</sub> + 0.632c<sub>3z</sub>m<sub>4</sub> + 0.1067(m<sub>1</sub> + m<sub>4</sub> + m<sub>5</sub> + m<sub>6</sub> + m<sub>7</sub>)</td><td>0.98181386278147</td><td>0.0176017697538843</td><td>0.989623634551615</td><td>0.981821119067433</td><td>0.12021126487721</td><td>0.9485</td></tr><tr><td>J<sub>229z</sub></td><td>-0.0051912442243026</td><td>1.56250607091839</td><td>-0.00398364337529688</td><td>-0.0032892484222389</td><td>11.0391771409267</td><td>0.0014</td></tr><tr><td>J<sub>212z</sub></td><td>0.0284199502364441</td><td>0.26975339175229</td><td>0.0102607714654092</td><td>0.000145827279030233</td><td>14.3569921920133</td><td>7.4640e-04</td></tr><tr><td>J<sub>212y</sub></td><td>-0.00349341313716902</td><td>2.33296149141574</td><td>0.000768626805457974</td><td>0.0154536414540448</td><td>2.3054111679656</td><td>-0.0113</td></tr><tr><td>J<sub>229y</sub> + J<sub>212z</sub> + 0.09985 m<sub>1</sub> + 0.632 c<sub>3z</sub> m<sub>4</sub> + 0.1067 (m<sub>4</sub> + m<sub>5</sub> + m<sub>6</sub> + m<sub>7</sub>)</td><td>1.04277709872499</td><td>0.00913880051976703</td><td>1.03577961636038</td><td>0.934608341141512</td><td>0.0972895278336854</td><td>0.9491</td></tr><tr><td>J<sub>212x</sub> - J<sub>229y</sub> + J<sub>229y</sub> - 0.0068 m<sub>4</sub></td><td>0.0105900270669364</td><td>1.26639366187113</td><td>0.0115952028662261</td><td>0.0148617879406265</td><td>5.81492624535104</td><td>-0.0164</td></tr><tr><td>J<sub>212y</sub> + 0.0825 c<sub>3z</sub> m<sub>4</sub></td><td>-0.000212959942456617</td><td>30.362154615825</td><td>---</td><td>0.0126539359353843</td><td>3.56681816344772</td><td>-0.002</td></tr><tr><td>J<sub>212z</sub></td><td>-0.0104385481552488</td><td>0.487939874522834</td><td>-0.00548819376576177</td><td>-0.00608661113924325</td><td>5.21289431802778</td><td>0.0021</td></tr><tr><td>J<sub>229y</sub> + J<sub>212z</sub> + 0.0068 m<sub>1</sub> + 0.1036 m<sub>5</sub> + 0.0136 m<sub>6</sub> + 0.0136 m<sub>7</sub></td><td>1.16875696122888</td><td>0.0690642374025275</td><td>-0.00437747027079482</td><td>-0.0137886404379563</td><td>2.39065216162938</td><td>-0.0055</td></tr><tr><td>J<sub>212x</sub> - J<sub>229y</sub> + J<sub>229y</sub> + 0.768 c<sub>3z</sub> m<sub>3</sub> + 0.1406 (m<sub>5</sub> + m<sub>6</sub> + m<sub>7</sub>)</td><td>0.53241322270191</td><td>0.0202220115117817</td><td>0.540830009861762</td><td>0.54615645093602</td><td>0.0951153240309647</td><td>0.5381</td></tr><tr><td>J<sub>212y</sub> + 0.0317 m<sub>5</sub> + 0.0317 m<sub>6</sub> + 0.0317 m<sub>7</sub> + 0.0825 c<sub>3z</sub> m<sub>5</sub></td><td>-0.00605576541609</td><td>0.0274046793405843</td><td>0.138763629896975</td><td>0.12021718586004</td><td>0.166544012462708</td><td>0.1255</td></tr><tr><td>J<sub>212z</sub></td><td>0.0047722336242583</td><td>0.907940594588059</td><td>0.00390545637077114</td><td>0.0039942878536737</td><td>6.52849780592255</td><td>-0.0020</td></tr><tr><td>J<sub>229y</sub> + J<sub>212z</sub> + 0.00273179005775726</td><td>0.63693557916876</td><td>0.0102544668139368</td><td>0.632542072189963</td><td>0.619605971610654</td><td>0.08294257250259799</td><td>0.6089</td></tr><tr><td>J<sub>229y</sub> + J<sub>212z</sub> + 0.1543 m<sub>4</sub> + 0.1543 m<sub>5</sub> + 0.1543 m<sub>6</sub> + 0.1543 m<sub>7</sub> + 0.768 c<sub>3z</sub> m<sub>5</sub></td><td>0.323153688598929</td><td>0.27385880119597</td><td>-0.007491178156382638</td><td>0.011398068548426</td><td>3.4723420529251</td><td>1.8047e-04</td></tr><tr><td>J<sub>212x</sub> - J<sub>229y</sub> + J<sub>229y</sub> + 0.0077 m<sub>7</sub></td><td>0.180693692687778</td><td>0.161771068964401</td><td>-0.00164499853619947</td><td>0.0078404363089336</td><td>3.4723420529251</td><td>1.8047e-04</td></tr><tr><td>J<sub>212y</sub> - J<sub>229y</sub> + J<sub>229y</sub> - 0.0077 m<sub>7</sub></td><td>-0.00653174843772509</td><td>1.0591268329267</td><td>-0.000884713343309503</td><td>-0.015959380396465</td><td>3.8294073634316</td><td>-0.0077</td></tr><tr><td>J<sub>212z</sub> + 0.088 c<sub>3z</sub> m<sub>7</sub></td><td>0.00953625100475549</td><td>5.3311773705522</td><td>0.0055114006361939</td><td>0.0079984960252438</td><td>2.7132870999367</td><td>0.0077</td></tr><tr><td>J<sub>229y</sub> + J<sub>212z</sub> + 0.0077 m<sub>7</sub></td><td>-1.2901022237792E-005</td><td>218.642951947916</td><td>---</td><td>-0.00466040910538727</td><td>3.86493113464633</td><td>0.0039</td></tr><tr><td>J<sub>229y</sub> + J<sub>212z</sub> + 0.0077 m<sub>7</sub></td><td>0.000820441198475142</td><td>3.30831813966642</td><td>-0.00011149560566302</td><td>0.0008631001643519202</td><td>27.128692850487</td><td>-1.0796e-04</td></tr><tr><td>J<sub>212y</sub> + J<sub>229y</sub> + 0.0077 m<sub>7</sub></td><td>0.0250439243623208</td><td>0.177252873282703</td><td>0.030388912660974</td><td>0.0144969724265609</td><td>1.82265361166624</td><td>0.0204</td></tr><tr><td>J<sub>212x</sub> - J<sub>229y</sub> + J<sub>229y</sub> - 0.00176750950330307</td><td>-0.00176750950330307</td><td>2.73778209818965</td><td>0.002421653892493</td><td>0.0023614137960484</td><td>7.3233918251556</td><td>2.6087e-04</td></tr><tr><td>J<sub>212y</sub></td><td>0.001185322055771447</td><td>1.2032031695229</td><td>-0.00039543029416216</td><td>0.0048748355465031</td><td>1.8699808609363</td><td>0.0011</td></tr><tr><td>J<sub>212z</sub></td><td>0.00175781529261036</td><td>1.22993712958718</td><td>-0.00167239278928351</td><td>0.0023354216588897</td><td>4.88885570340326</td><td>0.0013</td></tr><tr><td>J<sub>212y</sub></td><td>-0.0005963963046054158</td><td>3.67539867359549</td><td>-0.00054879944912121</td><td>-0.00269495602553136</td><td>4.71731909809941</td><td>-0.0016</td></tr><tr><td>J<sub>212z</sub></td><td>0.00127244042127159</td><td>2.612913640630981</td><td>0.0049093643159561</td><td>-0.00426963114218529</td><td>4.48067711177889</td><td>0.0022</td></tr><tr><td>c<sub>3z</sub> m<sub>3</sub></td><td>-0.00536870991776331</td><td>1.55126356721021</td><td>-0.00203460783952851</td><td>0.028759108662679</td><td>1.8292590743338</td><td>0.0280</td></tr><tr><td>c<sub>29</sub> m<sub>2</sub> - 0.316 m<sub>4</sub> - 0.316 m<sub>5</sub> - 0.316 m<sub>6</sub> - 0.316 m<sub>7</sub> - 0.316 m<sub>8</sub> - c<sub>3z</sub> m<sub>4</sub></td><td>-3.10257562423396</td><td>0.00030900035457445</td><td>-3.10434336792372</td><td>-3.136256454641</td><td>0.0105154837122877</td><td>-3.1353</td></tr><tr><td>c<sub>3z</sub> m<sub>3</sub> - 0.0825 m<sub>4</sub> + 0.0825 m<sub>5</sub> + 0.0825 m<sub>6</sub> + 0.0825 m<sub>7</sub> + c<sub>3z</sub> m<sub>3</sub></td><td>0.687355500831219</td><td>0.00104345627736313</td><td>0.684154638778066</td><td>0.68027032107252</td><td>0.0487075032145087</td><td>0.6814</td></tr><tr><td>c<sub>3z</sub> m<sub>3</sub> - c<sub>3z</sub> m<sub>4</sub></td><td>0.0233765365133791</td><td>0.0352910904204845</td><td>0.0282266909891094</td><td>-0.0121106270760641</td><td>1.20132179900525</td><td>-0.0103</td></tr><tr><td>c<sub>3z</sub> m<sub>4</sub> - 0.0825 m<sub>6</sub> + 0.0825 m<sub>7</sub> - 0.0825 m<sub>8</sub></td><td>-0.48883350422897</td><td>0.00156814546422313</td><td>-0.490080870249493</td><td>-0.472630922902374</td><td>0.0367798072443234</td><td>-0.4742</td></tr><tr><td>0.384 m<sub>5</sub> + 0.384 m<sub>6</sub> + 0.384 m<sub>7</sub> + c<sub>3z</sub> m<sub>5</sub></td><td>1.71848566994063</td><td>0.0004507008576665</td><td>1.72068233187568</td><td>1.75173249241485</td><td>0.0192516301067527</td><td>1.7525</td></tr><tr><td>c<sub>3z</sub> m<sub>5</sub></td><td>-0.01000419670455902</td><td>0.0643953740324495</td><td>-0.0146541895222708</td><td>-0.000503912493505951</td><td>26.569855964617</td><td>0.0015</td></tr><tr><td>c<sub>3z</sub> m<sub>6</sub></td><td>0.0770531295113475</td><td>0.00379674847168779</td><td>0.08238616909591039</td><td>0.01681837992151493</td><td>0.1362374568342</td><td>0.0815</td></tr><tr><td>0.088 m<sub>7</sub> + c<sub>3z</sub> m<sub>6</sub></td><td>0.165680379211635</td><td>0.0033594455916706</td><td>0.164967969875474</td><td>0.163721445813231</td><td>0.0744111762455879</td><td>0.1639</td></tr><tr><td>c<sub>3z</sub> m<sub>6</sub> - c<sub>3z</sub> m<sub>7</sub></td><td>-0.0677117737044103</td><td>0.00825760446226429</td><td>-0.0688323213466581</td><td>-0.0622560121354882</td><td>0.15272568276745</td><td>-0.0613</td></tr><tr><td>c<sub>3z</sub> m<sub>7</sub></td><td>0.00623108688342054</td><td>0.0743590105306178</td><td>0.00773512225159871</td><td>0.000133680246679693</td><td>6.57812339642614</td><td>6.3142e-04</td></tr><tr><td>c<sub>3z</sub> m<sub>8</sub></td><td>-0.00048808593311965</td><td>0.949033575145546</td><td>-0.0031276886416296</td><td>0.00476676862634269</td><td>1.678251719505328</td><td>0.0033</td></tr><tr><td>f<sub>1,1</sub></td><td></td><td></td><td></td><td>0.0665266343059956</td><td>1.96581451621768</td><td>0.0628</td></tr><tr><td>f<sub>1,2</sub></td><td></td><td></td><td></td><td>0.198746668762208</td><td>0.961542935001025</td><td>0.2088</td></tr><tr><td>f<sub>1,3</sub></td><td></td><td></td><td></td><td>0.039926055264758</td><td>3.49705294041023</td><td>0.0361</td></tr><tr><td>f<sub>1,4</sub></td><td></td><td></td><td></td><td>0.225710406362974</td><td>0.912430413683524</td><td>0.2174</td></tr><tr><td>f<sub>1,5</sub></td><td></td><td></td><td></td><td>0.102266664468244</td><td>1.58679668486263</td><td>0.1021</td></tr><tr><td>f<sub>1,6</sub></td><td></td><td></td><td></td><td>-0.0132100723685054</td><td>12.2606754655529</td><td>1.6128e-04</td></tr><tr><td>f<sub>1,7</sub></td><td></td><td></td><td></td><td>0.0637878416456024</td><td>2.13885157364764</td><td>0.0632</td></tr><tr><td>f<sub>1,8</sub></td><td></td><td></td><td></td><td>0.2450234775950843</td><td>0.588949258058454</td><td>0.2549</td></tr><tr><td>f<sub>1,9</sub></td><td></td><td></td><td></td><td>0.152392042413405</td><td>0.960241278437485</td><td>0.1413</td></tr><tr><td>f<sub>1,10</sub></td><td></td><td></td><td></td><td>0.182625732631744</td><td>0.7532389092308952</td><td>0.1879</td></tr><tr><td>f<sub>1,11</sub></td><td></td><td></td><td></td><td>0.3591080461687031</td><td>0.415181633314249</td><td>0.3625</td></tr><tr><td>f<sub>1,12</sub></td><td></td><td></td><td></td><td>0.266921997663113</td><td>0.466786859291591</td><td>0.2728</td></tr><tr><td>f<sub>1,13</sub></td><td></td><td></td><td></td><td>0.165805633616467</td><td>0.807745692948203</td><td>0.1529</td></tr><tr><td>f<sub>1,14</sub></td><td></td><td></td><td></td><td>0.210904095871819</td><td>0.705536701742261</td><td>0.2097</td></tr><tr><td>f<sub>1,15</sub></td><td></td><td></td><td></td><td>-0.107311659739619</td><td>0.693156685250328</td><td>-0.1069</td></tr><tr><td>f<sub>1,16</sub></td><td></td><td></td><td></td><td>-0.156635309821668</td><td>2.95495313788316</td><td>-0.1601</td></tr><tr><td>f<sub>1,17</sub></td><td></td><td></td><td></td><td>-0.0686347620012536</td><td>1.15656157885046</td><td>-0.0718</td></tr><tr><td>f<sub>1,18</sub></td><td></td><td></td><td></td><td>-0.25220906909088585</td><td>0.9536307919122076</td><td>-0.2562</td></tr><tr><td>f<sub>1,19</sub></td><td></td><td></td><td></td><td>0.00445732602659933</td><td>19.41632616331215</td><td>0.0079</td></tr><tr><td>f<sub>1,20</sub></td><td></td><td></td><td></td><td>0.0910111971131084</td><td>0.839029697813094</td><td>0.0935</td></tr><tr><td>f<sub>1,21</sub></td><td></td><td></td><td></td><td>-0.0126692991218763</td><td>6.26279708120922</td><td>-0.0070</td></tr></tbody></table>

discarded due to their large standard deviations (see the corresponding second
column in the table).

In conclusion, the similarity between the results obtained with the classical method and with our approach indirectly confirms the validity of the set of dynamic coefficients in Table A.2 for the Panda robot.

## A.2 Comparison of dynamic parameters

The dynamic parameters of the Panda robot, i.e., the mass, center of mass, and inertia tensor of each link, have been retrieved by solving the nonlinear optimization problem presented in Section 7.4 as well as by using the LMI-SDP framework presented in [91], based on the Python code available at https://github.com/cdsousa/wam7 Dyn Ident. This code has been slightly modified in order to include the triangular inequalities on the inertia tensors, according to [24]. The results are reported in the two Tables A.3 and A.4.

In Tab. A.3, the value of $c_{1z}$ is reported as “*” (don't care) since it does not have any influence on the dynamics (i.e., it does not appear in the E-L model, and can take any value).

The used lower (LB) and upper (UB) bounds and the final value $\hat{p}$ obtained with our optimization algorithm are shown in the second to fourth columns. The final parameters $\delta$ (including 21 friction parameters) obtained with the LMI-SDP method are reported in the fifth columns. In particular, these latter values were retrieved from the dynamic coefficients $\beta^{*e}$ by means of eq. (49) in [91], using the same bounds (properly manipulated) that we adopted for our algorithm. The resulting $\delta$ set was then slightly manipulated, in order to obtain for each link the center of mass from the its first moment of inertia and the barycentric inertia tensor from the inertia tensors w.r.t. the link frame.

Figure A.1 shows a comparison between the torques measured by the joint torque sensors of the Panda robot during a validation trajectory and the estimated torques generated from a Newton-Euler (N-E) routine using the $\hat{p}$ parameters and the $\delta$ parameters. In order to further validate the parameter sets $\hat{p}$ and $\delta$, we also designed 10 new validation trajectories (each lasting for 10 seconds), performing the same joint torque comparisons and then computing the mean square error (MSE) for each trajectory. The concatenated joint trajectories are reported in Fig. A.3, the concatenated joint torques in Fig. A.2, and the total MSE in Table A.5. These results confirm that both estimates are reliable and consistent.
<table><caption>**Table A.4** – Lower and upper bounds for the inertia tensor elements of the Panda robot and their corresponding estimated values using our approach ($\hat{\mathbf{p}}$) and the one in [91] ($\mathbf{\delta}$).</caption><thead><tr><th>parameter</th><th>LB</th><th>UB</th><th>p̂</th><th>δ (from [91])</th><th>units</th></tr></thead><tbody><tr><td>I<sub>1xx</sub></td><td>0</td><td>1</td><td>7.0337e-01</td><td>0.6</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>1xy</sub></td><td>-1</td><td>1</td><td>-1.3900e-04</td><td>-3.2497e-14</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>1xz</sub></td><td>-1</td><td>1</td><td>6.7720e-03</td><td>-6.4280e-09</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>1yy</sub></td><td>0</td><td>1</td><td>7.0661e-01</td><td>0.6</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>1yz</sub></td><td>-1</td><td>1</td><td>1.9169e-02</td><td>-6.4280e-09</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>1zz</sub></td><td>0</td><td>1</td><td>9.1170e-03</td><td>2.0353e-06</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>2xx</sub></td><td>0</td><td>1</td><td>7.9620e-03</td><td>0.028</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>2xy</sub></td><td>-1</td><td>1</td><td>-3.9250e-03</td><td>5.5650e-05</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>2xz</sub></td><td>-1</td><td>1</td><td>1.0254e-02</td><td>0.0056</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>2yy</sub></td><td>0</td><td>1</td><td>2.8110e-02</td><td>0.0291</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>2yz</sub></td><td>-1</td><td>1</td><td>7.0400e-04</td><td>-2.7636e-04</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>2zz</sub></td><td>0</td><td>1</td><td>2.5995e-02</td><td>0.0011</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>3xx</sub></td><td>0</td><td>1</td><td>3.7242e-02</td><td>2.0154e-06</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>3xy</sub></td><td>-1</td><td>1</td><td>-4.7610e-03</td><td>-5.9144e-09</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>3xz</sub></td><td>-1</td><td>1</td><td>-1.1396e-02</td><td>1.1523e-08</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>3yy</sub></td><td>0</td><td>1</td><td>3.6155e-02</td><td>2.0243e-06</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>3yz</sub></td><td>-1</td><td>1</td><td>-1.2805e-02</td><td>6.0956e-09</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>3zz</sub></td><td>0</td><td>1</td><td>1.0830e-02</td><td>2.0164e-06</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>4xx</sub></td><td>0</td><td>1</td><td>2.5853e-02</td><td>2.0242e-06</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>4xy</sub></td><td>-1</td><td>1</td><td>7.7960e-03</td><td>7.1405e-09</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>4xz</sub></td><td>-1</td><td>1</td><td>-1.3320e-03</td><td>8.8917e-10</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>4yy</sub></td><td>0</td><td>1</td><td>1.9552e-02</td><td>2.0036e-06</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>4yz</sub></td><td>-1</td><td>1</td><td>8.6410e-03</td><td>-2.2039e-09</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>4zz</sub></td><td>0</td><td>1</td><td>2.8323e-02</td><td>2.0260e-06</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>5xx</sub></td><td>0</td><td>1</td><td>3.5549e-02</td><td>2.0057e-06</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>5xy</sub></td><td>-1</td><td>1</td><td>-2.1170e-03</td><td>1.6453e-10</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>5xz</sub></td><td>-1</td><td>1</td><td>-4.0370e-03</td><td>-3.6824e-10</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>5yy</sub></td><td>0</td><td>1</td><td>2.9474e-02</td><td>2.0037e-06</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>5yz</sub></td><td>-1</td><td>1</td><td>2.2900e-04</td><td>2.3080e-09</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>5zz</sub></td><td>0</td><td>1</td><td>8.6270e-03</td><td>2.0028e-06</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>6xx</sub></td><td>0</td><td>1</td><td>1.9640e-03</td><td>2.0026e-06</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>6xy</sub></td><td>-1</td><td>1</td><td>1.0900e-04</td><td>1.5823e-09</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>6xz</sub></td><td>-1</td><td>1</td><td>-1.1580e-03</td><td>2.0530e-09</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>6yy</sub></td><td>0</td><td>1</td><td>4.3540e-03</td><td>2.0070e-06</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>6yz</sub></td><td>-1</td><td>1</td><td>3.4100e-04</td><td>-4.9387e-10</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>6zz</sub></td><td>0</td><td>1</td><td>5.4330e-03</td><td>2.0073e-06</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>7xx</sub></td><td>0</td><td>1</td><td>1.2516e-02</td><td>2.0026e-06</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>7xy</sub></td><td>-1</td><td>1</td><td>-4.2800e-04</td><td>2.6159e-10</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>7xz</sub></td><td>-1</td><td>1</td><td>-1.1960e-03</td><td>1.4055e-09</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>7yy</sub></td><td>0</td><td>1</td><td>1.0027e-02</td><td>2.0037e-06</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>7yz</sub></td><td>-1</td><td>1</td><td>-7.4100e-04</td><td>-4.2047e-10</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>7zz</sub></td><td>0</td><td>1</td><td>4.8150e-03</td><td>2.0020e-06</td><td>kg·m<sup>2</sup></td></tr></tbody></table>

<table><caption>**Table A.3** – Lower and upper bounds for masses and centers of mass of the Panda robot and their corresponding estimated values using our approach ($\hat{\mathbf{p}}$) and the one in [91] ($\mathbf{\delta}$).</caption><thead><tr><th>parameter</th><th>LB</th><th>UB</th><th>p̂</th><th>δ (from [91])</th><th>units</th></tr></thead><tbody><tr><td>m<sub>1</sub></td><td>0</td><td>10</td><td>4.970684</td><td>2.0643e-05</td><td>kg</td></tr><tr><td>m<sub>2</sub></td><td>0</td><td>10</td><td>0.646926</td><td>9.2017</td><td>kg</td></tr><tr><td>m<sub>3</sub></td><td>0</td><td>10</td><td>3.228604</td><td>1.4575</td><td>kg</td></tr><tr><td>m<sub>4</sub></td><td>0</td><td>10</td><td>3.587895</td><td>4.5856</td><td>kg</td></tr><tr><td>m<sub>5</sub></td><td>0</td><td>10</td><td>1.225946</td><td>0.5231</td><td>kg</td></tr><tr><td>m<sub>6</sub></td><td>0</td><td>10</td><td>1.666555</td><td>2.0604</td><td>kg</td></tr><tr><td>m<sub>7</sub></td><td>0</td><td>10</td><td>7.35522e-01</td><td>0.1718</td><td>kg</td></tr><tr><td>c<sub>1x</sub></td><td>-0.05</td><td>0.05</td><td>3.875e-03</td><td>5.202e-08</td><td>m</td></tr><tr><td>c<sub>1y</sub></td><td>-0.05</td><td>0.05</td><td>2.081e-03</td><td>5.202e-08</td><td>m</td></tr><tr><td>c<sub>1z</sub></td><td>-0.4</td><td>0.05</td><td>*</td><td>-0.1750</td><td>m</td></tr><tr><td>c<sub>2x</sub></td><td>-0.05</td><td>0.05</td><td>-3.141e-03</td><td>0.0015</td><td>m</td></tr><tr><td>c<sub>2y</sub></td><td>-0.15</td><td>0.05</td><td>-2.872e-02</td><td>-0.0578</td><td>m</td></tr><tr><td>c<sub>2z</sub></td><td>-0.05</td><td>0.05</td><td>3.495e-03</td><td>-0.0384</td><td>m</td></tr><tr><td>c<sub>3x</sub></td><td>-0.05</td><td>0.15</td><td>2.7518e-02</td><td>0.1096</td><td>m</td></tr><tr><td>c<sub>3y</sub></td><td>-0.05</td><td>0.05</td><td>3.9252e-02</td><td>0.05</td><td>m</td></tr><tr><td>c<sub>3z</sub></td><td>-0.1</td><td>0.05</td><td>-6.6502e-02</td><td>-0.1</td><td>m</td></tr><tr><td>c<sub>4x</sub></td><td>-0.15</td><td>0.05</td><td>-5.317e-02</td><td>-0.0485</td><td>m</td></tr><tr><td>c<sub>4y</sub></td><td>-0.05</td><td>0.15</td><td>1.04419e-01</td><td>0.15</td><td>m</td></tr><tr><td>c<sub>4z</sub></td><td>-0.05</td><td>0.05</td><td>2.7454e-02</td><td>0.0147</td><td>m</td></tr><tr><td>c<sub>5x</sub></td><td>-0.05</td><td>0.05</td><td>-1.1953e-02</td><td>-0.004</td><td>m</td></tr><tr><td>c<sub>5y</sub></td><td>-0.05</td><td>0.05</td><td>4.1065e-02</td><td>0.045</td><td>m</td></tr><tr><td>c<sub>5z</sub></td><td>-0.05</td><td>0.05</td><td>-3.8437e-02</td><td>-0.05</td><td>m</td></tr><tr><td>c<sub>6x</sub></td><td>-0.05</td><td>0.15</td><td>6.0149e-02</td><td>0.0732</td><td>m</td></tr><tr><td>c<sub>6y</sub></td><td>-0.05</td><td>0.05</td><td>-1.4117e-02</td><td>-0.0251</td><td>m</td></tr><tr><td>c<sub>6z</sub></td><td>-0.05</td><td>0.05</td><td>-1.0517e-02</td><td>-0.0276</td><td>m</td></tr><tr><td>c<sub>7x</sub></td><td>-0.05</td><td>0.05</td><td>1.0517e-02</td><td>-0.0326</td><td>m</td></tr><tr><td>c<sub>7y</sub></td><td>-0.05</td><td>0.05</td><td>-4.252e-03</td><td>0.0087</td><td>m</td></tr><tr><td>c<sub>7z</sub></td><td>0.04</td><td>0.15</td><td>6.1597e-02</td><td>0.0396</td><td>m</td></tr></tbody></table>
<figure><img src="image_59.png" /><figcaption>**Figure A.1** – Comparison between measured joint torques during a validation trajectory (blue solid lines) and estimated torques generated from a N-E routine using the $\hat{p}$ parameters from our parameter retrieval method in [7] (see Chap. 7) (solid red lines) and the parameters $\delta$ from the LMI-SDP framework in [91] (solid yellow lines). The torque errors are reported as well.</figcaption></figure>

<table><caption>**Table A.5** – Mean Square Error (MSE) [(Nm)$^2$] of torque predictions using the parameters $\hat{p}$ of our method [7] illustrated in chapter 7 and the parameters $\delta$ of LMI-SDP approach [91] in validation experiments of Fig. A.3.</caption><thead><tr><th></th><th>Joint 1</th><th>Joint 2</th><th>Joint 3</th><th>Joint 4</th><th>Joint 5</th><th>Joint 6</th><th>Joint 7</th></tr></thead><tbody><tr><td>MSE <i>p̂</i></td><td>0.4068</td><td>1.0282</td><td>0.5951</td><td>0.4231</td><td>0.1749</td><td>0.0403</td><td>0.0267</td></tr><tr><td>MSE <i>δ</i></td><td>0.4955</td><td>1.0155</td><td>0.2751</td><td>0.6627</td><td>0.0828</td><td>0.0508</td><td>0.0284</td></tr></tbody></table>
<figure><img src="image_60.png" /><figcaption>Figure A.2 – Joint torques recorded during the 10 validation trajectories reported in Fig. A.3.</figcaption></figure>

<figure><img src="image_61.png" /></figure>

<figure><img src="image_62.png" /><figcaption>Figure A.3 – Joint positions recorded during 10 validation trajectories (10 seconds each). The associated joint torques are reported in Fig. A.2.</figcaption></figure>

<figure><img src="image_63.png" /></figure>
# B Parameters retrieval using nonlinear and conditional constraints

In order to retrieve a feasible set of dynamic parameters $\hat{p}$ for the Panda robot, only linear constraints were used, since the non-convex shapes of the links would allow their centers of mass to be located even outside the link themselves. We present here a simple example of retrieval of dynamic parameters where the use of nonlinear constraints would be preferable, and which fits then in the framework of our method, presented in Chapter 7.

Consider the spatial 2R robot with orthogonal joint axes in Fig. B.1. Its (standard) Denavit-Hartenberg kinematic parameters are reported in Table B.1. The robot body consists of two links: link 1 is a cylinder of height $l_1 = 1.5$ m and radius $r_1 = 0.15$ m; link 2 has a total length of $l_2 = 0.8$ m and is composed by a cylinder of length $l_2 - l'_2$ and radius $r_2 = 0.08$ m, connected at the end with a truncated cone of length $l'_2 = 0.2$ m and with radius $r_3 = 0.02$ m for its smaller base.

<figure><img src="image_64.png" /><figcaption>**Figure B.1** – A spatial 2R robot with joints along axes $z_0$ and $z_1$.</figcaption></figure>

The assumed ground truth values of the dynamic parameters $\mathbf{p}_{\text{real}}$ are given in the fourth column of Table B.2.
<table><caption>**Table B.2** – Lower and upper bounds for the dynamic parameters of the 2R robot in Fig. B.1, with real $p_{\text{real}}$ and estimated $\hat{p}$ values.</caption><thead><tr><th>parameter</th><th>LB</th><th>UB</th><th>p<sub>real</sub></th><th>p&#770;</th><th>units</th></tr></thead><tbody><tr><td>m<sub>1</sub></td><td>0</td><td>10</td><td>8</td><td>4.5788</td><td>kg</td></tr><tr><td>m<sub>2</sub></td><td>0</td><td>5</td><td>3</td><td>3.7828</td><td>kg</td></tr><tr><td>c<sub>1x</sub></td><td>-∞</td><td>∞</td><td>0.01</td><td>-0.016124</td><td>m</td></tr><tr><td>c<sub>1y</sub></td><td>-1.42</td><td>0.08</td><td>-0.75</td><td>-0.74408</td><td>m</td></tr><tr><td>c<sub>1z</sub></td><td>-∞</td><td>∞</td><td>-0.02</td><td>-3.8e-04</td><td>m</td></tr><tr><td>c<sub>2x</sub></td><td>-0.8</td><td>0</td><td>-0.1</td><td>-0.24487</td><td>m</td></tr><tr><td>c<sub>2y</sub></td><td>-∞</td><td>∞</td><td>0</td><td>6.16e-06</td><td>m</td></tr><tr><td>c<sub>2z</sub></td><td>-∞</td><td>∞</td><td>0</td><td>-3.66e-03</td><td>m</td></tr><tr><td>I<sub>1xx</sub></td><td>0</td><td>1</td><td>0.03</td><td>0.61296</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>1xy</sub></td><td>-1</td><td>1</td><td>0</td><td>0.071738</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>1xz</sub></td><td>-1</td><td>1</td><td>0</td><td>-0.0099797</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>1yy</sub></td><td>0</td><td>1</td><td>0.03</td><td>0.022982</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>1yz</sub></td><td>-1</td><td>1</td><td>0</td><td>0.031154</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>1zz</sub></td><td>0</td><td>1</td><td>0.01</td><td>0.6172</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>2xx</sub></td><td>0</td><td>1</td><td>0.01</td><td>0.019819</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>2xy</sub></td><td>-1</td><td>1</td><td>0</td><td>-4.525e-06</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>2xz</sub></td><td>-1</td><td>1</td><td>0</td><td>-0.0077412</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>2yy</sub></td><td>0</td><td>1</td><td>0.04</td><td>0.35386</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>2yz</sub></td><td>-1</td><td>1</td><td>0</td><td>-5.9342e-05</td><td>kg·m<sup>2</sup></td></tr><tr><td>I<sub>2zz</sub></td><td>0</td><td>1</td><td>0.04</td><td>0.34412</td><td>kg·m<sup>2</sup></td></tr></tbody></table>

<table><caption>**Table B.1** – DH table of the spatial 2R robot.</caption><thead><tr><th><i>i</i></th><th><b>a</b><sub><i>i</i></sub></th><th><b>α</b><sub><i>i</i></sub></th><th><b>d</b><sub><i>i</i></sub></th><th><b>θ</b><sub><i>i</i></sub></th></tr></thead><tbody><tr><td>1</td><td>0</td><td>π/2</td><td>l<sub>1</sub></td><td>q<sub>1</sub></td></tr><tr><td>2</td><td>l<sub>2</sub></td><td>0</td><td>0</td><td>q<sub>2</sub></td></tr></tbody></table>

The symbolic form of the dynamic coefficients $\pi_{2R} \in \mathbb{R}^8$ of this robot is:

$$ \pi_{2R} = \begin{pmatrix} J_{1yy} + J_{2yy} - 0.64m_2 \\ J_{2xx} - J_{2yy} + 0.64m_2 \\ J_{2xy} \\ J_{2xz} - 0.8c_{2z}m_2 \\ J_{2yz} \\ J_{2zz} - 0.8m_2 \\ 0.8m_2 + c_{2x}m_2 \\ c_{2y}m_2 \end{pmatrix}. $$

In order to estimate the values of the dynamic coefficients $\pi_{2R}$, a numerical simulation is performed using $p_{\text{real}}$ and the joint torques are recorded during an exciting motion. Then the dynamic coefficients are properly estimated with an ordinary least squares method.

At this stage, the algorithm for the retrieval of the dynamic parameters $\hat{p}$ is
launched, using the bounds on physical feasibility reported in Table B.2, upper and lower bounds on the total mass, $1 \le m_1 + m_2 \le 15$, and linear constraints on the inertia tensors (exploiting the triangular inequality). When using box constraints on the position of the center of mass of each of the two links, the solution would be searched in an extra volume (i.e., a parallelepiped) which is $V_{\text{link1}} = 4/\pi \approx 1.27$ times larger than the volume of the cylindric link 1 and $V_{\text{link2}} = 4/\pi (1 + 2l'_2/l_2) \approx 2$ times larger than the one of link 2. On the other hand, considering that the distance of the center of mass from the major link axis must be less than its radius, the previous (approximate) box constraints on the center of mass of the two links can be replaced by the following (exact) nonlinear and conditional (if-else) constraints $g_{\text{CoM}_1}$ and $g_{\text{CoM}_2}$, to be used within our core optimization Algorithm 1 in chapter 7:

$$ \alpha_1 = r_1 - \sqrt{c_{1y}^2 + c_{1z}^2}, \\ \alpha_2 = \begin{cases} r_2 - \sqrt{c_{2y}^2 + c_{2z}^2}, & \text{if } c_{2x} < -l'_2 \\ (r_2 + \frac{r_3-r_2}{l'_2}(l'_2 + c_{2x})) - \sqrt{c_{2y}^2 + c_{2z}^2}, & \text{otherwise,} \end{cases} \\ g_{\text{CoM}_1} = -\min\{0, \alpha_1\}, \quad g_{\text{CoM}_2} = -\min\{0, \alpha_2\}. $$

Taking advantage of the knowledge of the link shapes, these nonlinear constraints ensure that each center of mass will lie inside the corresponding link shape but that no feasible position is being excluded, thus yielding a complete solution. A validation test was finally performed, comparing the joint torques on sinusoidal trajectories that are computed using the real parameters with the joint torques estimated by means of a N-E routine fed with the parameters $\hat{p}$ reported in the fifth column of Table B.2. The retrieved solution generates just the same dynamics as obtained with the real parameters.

Another interesting quantity that can be estimated only when using the N-E inverse dynamics algorithm is the wrench acting at the robot joint level, i.e., the exchanged forces/moments between two successive links connected by a joint. To this aim, it is very important to retrieve a set of dynamic parameters that is as close as possible to the real one. To show this, we relaxed the constraints in order to obtain a second solution set of dynamic parameters $\hat{p}'$, which, however, contains also non-feasible parameters (i.e., parameters that have no physical meaning).

In Fig. B.2, the yellow lines represent the estimated joint torques during a validation experiment coming from a N-E routine that is being fed with the un-feasible parameters $\hat{p}'$. Despite this, reliable motion torque estimations can still be appreciated. On the contrary, a strong inconsistency may be observed when estimating the internal forces acting on the joints, obtained as a byproduct of
<figure><img src="image_65.png" /><figcaption>**Figure B.2** – Validation of estimated parameters in Table B.2. A N-E routine fed with the feasible parameters $\hat{p}$ and with some unfeasible parameters $\hat{p}'$ returns similar estimated torques (respectively, red and yellow lines) along the motion. When comparing these estimates with the actual torques recorded along a sinusoidal joint trajectory (blue lines, almost overlapped by the red line), we notice that both sets provide a good torque estimation.</figcaption></figure>

the standard N-E algorithm. As shown by the values of these estimated forces in Fig. B.3, it can be seen that estimations obtained from the feasible parameters $\hat{p}$ (red lines) are close to the real forces (blue lines), while estimations retrieved from the unfeasible parameters $\hat{p}'$ (yellow lines) are almost $10^6$ times larger than the real values “felt” at the joints. This confirms that a physically-consistent solution which is close to the real one is mandatory for estimating the joint wrenches.
<figure><img src="image_66.png" /></figure>

**Figure B.3** – Internal forces acting on the joints of the simulated 2R robot during a sinusoidal joint trajectory. Estimates of the internal forces obtained from a N-E routine are significantly closer to the real internal forces (blue lines) if feasible (i.e., physically consistent) dynamic parameters are provided to the routine, while estimates obtained from an unfeasible set $\hat{p}'$ can be even $10^6$ times larger than real internal forces (note that the yellow line was scaled for the ease of reading).
Bibliography

[1] R. Bajcsy, "Integrating vision and touch for robotics applications," in *Artificial Intelligence Applications for Business: Proceedings of the NYU Symposium*, May 1983, pp. 297 – 323.

[2] J. Baeten and J. De Schutter, *Integrated Visual Servoing and Force Control - The Task Frame Approach*. Springer, 01 2003.

[3] R. P. Paul, *Robot manipulators: mathematics, programming, and control: the computer control of robot manipulators*. MIT Press, 1981.

[4] Y. Nakabo and M. Ishikawa, "Visual impedance using 1 ms visual feedback system," in *Proceedings. 1998 IEEE International Conference on Robotics and Automation (Cat. No.98CH36146)*, vol. 3, 1998, pp. 2333–2338 vol.3.

[5] A. A. Oliva, P. Robuffo Giordano, and F. Chaumette, "A general visual-impedance framework for effectively combining vision and force sensing in feature space," *IEEE Robotics and Automation Letters*, vol. 6, no. 3, pp. 4441–4448, 2021.

[6] A. A. Oliva, E. Aertbeliën, J. de Schutter, P. Robuffo Giordano, and F. Chaumette, "Towards dynamic visual servoing for interaction control and moving targets," *2022 IEEE International Conference on Robotics and Automation (to appear)*, May 2022.

[7] C. Gaz, M. Cognetti, A. Oliva, P. Robuffo Giordano, and A. De Luca, "Dynamic identification of the Franka Emika Panda robot with retrieval of feasible parameters using penalty-based optimization," *IEEE Robotics and Automation Lett.*, 2019.

[8] A. A. Oliva, F. Spindler, P. Robuffo Giordano, and F. Chaumette, "FrankaSim: A Franka Emika's Panda sobot simulator with visual-servoing enabled capabilities," *IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) (submitted)*, 2022.
[9] A. Cherubini and D. Navarro-Alarcon, "Sensor-based control for collaborative robots: Fundamentals, challenges, and opportunities." *Front. Neurorobot.*, 2021.

[10] E. Marchand, F. Spindler, and F. Chaumette, "Visp for visual servoing: a generic software platform with a wide class of robot control skills," *IEEE Robotics and Automation Magazine*, vol. 12, no. 4, pp. 40–52, December 2005.

[11] O. Khatib, "A unified approach for motion and force control of robot manipulators: The operational space formulation," *IEEE Journal on Robotics and Automation*, vol. 3, no. 1, pp. 43–53, 1987.

[12] N. Hogan, "Impedance control: An approach to manipulation: Part I—Theory," *Journal of Dynamic Systems, Measurement, and Control*, vol. 107, no. 1, pp. 1–7, 03 1985.

[13] B. Siciliano, L. Sciavicco, L. Villani, and G. Oriolo, *Robotics: Modeling, Planning and Control*, 3rd ed. London: Springer, 2008.

[14] M. H. Raibert and J. J. Craig, "Hybrid position/force control of manipulators," *Journal of Dynamic Systems, Measurement, and Control*, vol. 103, no. 2, pp. 126–133, 06 1981.

[15] M. T. Mason, "Compliance and force control for computer controlled manipulators," *IEEE Transactions on Systems, Man, and Cybernetics*, vol. 11, no. 6, pp. 418–432, 1981.

[16] S. Chiaverini and L. Sciavicco, "The parallel approach to force/position control of robotic manipulators," *IEEE Transactions on Robotics and Automation*, vol. 9, no. 4, pp. 361–373, 1993.

[17] J. De Schutter and H. Van Brussel, "Compliant robot motion ii. a control approach based on external control loops," *The International Journal of Robotics Research*, vol. 7, no. 4, pp. 18–33, 1988.

[18] J. Denavit and R. Hartenberg, "A kinematic notation for lower-pair mechanisms based on matrices," *Trans ASME Journal of Applied Mechanics*, p. 215–221, June 1955.

[19] J. J. Craig, *Introduction to Robotics: Mechanics and Control*, 3rd ed. Pearson Prentice Hall, 2005.
[20] C. Gaz, F. Flacco, and A. De Luca, "Extracting feasible robot parameters from dynamic coefficients using nonlinear optimization methods," in *Proc. IEEE Int. Conf. on Robotics and Automation*, 2016, pp. 2075-2081.

[21] W. Khalil and E. Dombre, *Modeling, Identification and Control of Robots*. Hermes Penton London, 2002.

[22] S. Traversaro, S. Brossette, A. Escande, and F. Nori, "Identification of fully physical consistent inertial parameters using optimization on manifolds," in *2016 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)*, Oct 2016, pp. 5446-5451.

[23] P. Wensing, S. Kim, and J.-J. E. Slotine, "Linear matrix inequalities for physically-consistent inertial parameter identification: A statistical perspective on the mass distribution," *IEEE Robotics and Automation Letters*, vol. PP, pp. 1-1, 07 2017.

[24] C. Sousa and R. Cortesão, "Inertia tensor properties in robot dynamics identification: A linear matrix inequality approach," *IEEE/ASME Transactions on Mechatronics*, vol. PP, pp. 1-1, 01 2019.

[25] A. De Luca, C. Manes, and F. Nicolò, "A task space decoupling approach to hybrid control of manipulators," *IFAC Proceedings Volumes*, vol. 21, no. 16, pp. 157-162, 1988, 2nd IFAC Symposium on Robot Control 1988 (SYROCO '88), Karlsruhe, FRG, 5-7 October. [Online]. Available: https://www.sciencedirect.com/science/article/pii/S1474667017546030

[26] T. Yoshikawa, "Dynamic hybrid position/force control of robot manipulators--description of hand constraints and calculation of joint driving force," *IEEE Journal on Robotics and Automation*, vol. 3, no. 5, pp. 386-392, 1987.

[27] T. Yoshikawa, T. Sugie, and M. Tanaka, "Dynamic hybrid position/force control of robot manipulators-controller design and experiment," *IEEE Journal on Robotics and Automation*, vol. 4, no. 6, pp. 699-705, 1988.

[28] A. De Luca, C. Manes, and G. Ulivi, "Robust hybrid dynamic control of robot arms," in *Proceedings of the 28th IEEE Conference on Decision and Control*, 1989, pp. 2641-2646 vol.3.

[29] A. De Luca and C. Manes, "Hybrid force-position control for robots in contact with dynamic environments," *IFAC Proceedings Volumes*, vol. 24,
no. 9, pp. 177–182, 1991, 3rd IFAC Symposium on Robot Control 1991 (SYROCO’91), Vienna, Austria, 16-18 September 1991. [Online]. Available: https://www.sciencedirect.com/science/article/pii/S1474667017510526

[30] W. D. Fisher and M. S. Mujtaba, “Hybrid position/force control: A correct formulation,” *The International Journal of Robotics Research*, vol. 11, no. 4, pp. 299–311, 1992. [Online]. Available: https://doi.org/10.1177/027836499201100403

[31] L. D. Joly and A. Micaelli, “Hybrid position/force control, velocity projection, and passivity,” *IFAC Proceedings Volumes*, vol. 30, pp. 325–331, 1997.

[32] C. Collewet and E. Marchand, “Photometric visual servoing,” *IEEE Transactions on Robotics*, vol. 27, no. 4, pp. 828–834, 2011.

[33] Q. Bateux and E. Marchand, “Histograms-based visual servoing,” *IEEE Robotics and Automation Letters*, vol. 2, no. 1, pp. 80–87, 2017.

[34] Q. Bateux, E. Marchand, J. Leitner, F. Chaumette, and P. Corke, “Visual Servoing from Deep Neural Networks,” in *RSS 2017 - Robotics: Science and Systems, Workshop New Frontiers for Deep Learning in Robotics*, Boston, United States, Jul. 2017, pp. 1–6. [Online]. Available: https://hal.inria.fr/hal-01589887

[35] J. Hill and W. T. Park, “Real time control of a robot with a mobile camera,” *Proc. 9th ISIR*, pp. 233–246, Mar. 1979.

[36] P. Corke, *Visual Control of Robots: High Performance Visual Servoing*. Research Studies Press Ltd, 1996.

[37] D. Forsyth and J. Ponce, *Computer Vision: A Modern Approach. (Second edition)*. Prentice Hall, Nov. 2011. [Online]. Available: https://hal.inria.fr/hal-01063327

[38] Y. Ma, S. Soatto, J. Kosecka, and S. S. Sastry, *An Invitation to 3-D Vision: From Images to Geometric Models*. SpringerVerlag, 2003.

[39] F. Chaumette, “Visual Servoing,” in *Robot Manipulators: Modeling, Performance Analysis and Control*, Dombre, Etienne, Khalil, and Wissama, Eds. ISTE, 2007, pp. 279–336. [Online]. Available: https://hal.inria.fr/hal-00920418
[40] B. Espiau, F. Chaumette, and P. Rives, "A new approach to visual servoing in robotics," *IEEE Transactions on Robotics and Automation*, vol. 8, no. 3, pp. 313-326, 1992.

[41] F. Chaumette, "Image moments: a general and useful set of features for visual servoing," *IEEE Transactions on Robotics*, vol. 20, no. 4, pp. 713-723, 2004.

[42] F. Chaumette, P. Rives, and B. Espiau, "Classification and realization of the different vision-based tasks," in *Visual Servoing*, ser. World Scientific Series in Robotics and Intelligent Systems, 1993, vol. 7, pp. 199–228. [Online]. Available: https://hal.inria.fr/hal-01548352

[43] E. Malis, "Visual servoing invariant to changes in camera-intrinsic parameters," *Robotics and Automation, IEEE Transactions on*, vol. 20, pp. 72–81, 03 2004.

[44] F. Chaumette and S. Hutchinson, "Visual servo control. i. basic approaches," *IEEE Robotics Automation Magazine*, vol. 13, no. 4, pp. 82-90, 2006.

[45] —, "Visual servo control. ii. advanced approaches [tutorial]," *IEEE Robotics Automation Magazine*, vol. 14, no. 1, pp. 109-118, 2007.

[46] S. Vandernotte, A. Chriette, P. Martinet, and A. S. Roos, "Dynamic sensor-based control," in *2016 14th International Conference on Control, Automation, Robotics and Vision (ICARCV)*, 2016, pp. 1-6.

[47] F. Fusco, O. Kermorgant, and P. Martinet, "A comparison of visual servoing from features velocity and acceleration interaction models," in *2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)*, 2019, pp. 4447-4452.

[48] F. Chaumette, S. Hutchinson, and P. Corke, "Visual servoing," *Handbook of Robotics*, 2nd edition, pp. 841-866, 2016.

[49] V. Lippiello, B. Siciliano, and L. Villani, "A position-based visual impedance control for robot manipulators," in *Proceedings 2007 IEEE International Conference on Robotics and Automation*, 2007, pp. 2068-2073.

[50] K. Nottensteiner, A. Sachtler, and A. Albu-Schäeffer, "Towards autonomous robotic assembly: Using combined visual and tactile sensing for adaptive task execution," *Journal of Intelligent & Robotic Systems*, vol. 101, 03 2021.
[51] K. Yu and A. Rodriguez, "Realtime state estimation with tactile and visual sensing. application to planar manipulation," in *2018 IEEE International Conference on Robotics and Automation (ICRA)*, 2018, pp. 7778-7785.

[52] K.-T. Yu and A. Rodriguez, "Realtime state estimation with tactile and visual sensing for inserting a suction-held object," in *2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)*, 2018, pp. 1628-1635.

[53] B. Nelson, J. Morrow, and P. Khosla, "Robotic manipulation using high bandwidth force and vision feedback," *Mathematical and Computer Modelling*, vol. 24, no. 5, pp. 11-29, 1996. [Online]. Available: https://www.sciencedirect.com/science/article/pii/0895717796001136

[54] D. Lawrence, "Impedance control stability properties in common implementations," in *Proceedings. 1988 IEEE International Conference on Robotics and Automation*, 1988, pp. 1185-1190 vol.2.

[55] G. Morel, E. Malis, and S. Boudet, "Impedance based combination of visual and force control," in *Proceedings. 1998 IEEE International Conference on Robotics and Automation (Cat. No.98CH36146)*, vol. 2, 1998, pp. 1743-1748 vol.2.

[56] D. E. Whitney, "Force Feedback Control of Manipulator Fine Motions," *Journal of Dynamic Systems, Measurement, and Control*, vol. 99, no. 2, pp. 91-97, 06 1977. [Online]. Available: https://doi.org/10.1115/1.3427095

[57] Y. Mezouar, M. Prats, and P. Martinet, "External hybrid vision/force control," *Intl. Conference on Advanced Robotics*, 2007.

[58] V. Perdereau and M. Drouin, "A new scheme for hybrid force-position control," in *RoManSy 9*, A. Morecki, G. Bianchi, and K. Jaworek, Eds. Berlin, Heidelberg: Springer Berlin Heidelberg, 1993, pp. 150-159.

[59] G. Morel and P. Bidaud, "A reactive external force loop approach to control manipulators in the presence of environmental disturbances," vol. 2, 05 1996, pp. 1229-1234 vol.2.

[60] N. Mansard, O. Stasse, P. Evrard, and A. Kheddar, "A versatile generalized inverted kinematics implementation for collaborative working humanoid robots: The stack of tasks," in *2009 International Conference on Advanced Robotics*, 2009, pp. 1-6.
[61] O. Khatib, L. Sentis, J. Park, and J. Warren, "Whole-body dynamic behavior and control of human-like robots," *Int. J. Humanoid Robotics*, vol. 1, pp. 29-43, 2004.

[62] D. J. Agravante, A. Cherubini, A. Bussy, P. Gergondet, and A. Kheddar, "Collaborative human-humanoid carrying using vision and haptic sensing," in *2014 IEEE International Conference on Robotics and Automation (ICRA)*, 2014, pp. 607-612.

[63] C. Cai and N. Somani, "Visual servoing in a prioritized constraint-based torque control framework," in *2018 IEEE/ACIS 17th International Conference on Computer and Information Science (ICIS)*. Los Alamitos, CA, USA: IEEE Computer Society, jun 2018, pp. 309-314.

[64] J. D. Schutter, T. D. Laet, J. Rutgeerts, W. Decré, R. Smits, E. Aertbeliën, K. Claes, and H. Bruyninckx, "Constraint-based task specification and estimation for sensor-based robot systems in the presence of geometric uncertainty," *The International Journal of Robotics Research*, vol. 26, no. 5, pp. 433-455, 2007. [Online]. Available: https://doi.org/10.1177/027836490707809107

[65] E. Aertbeliën and J. De Schutter, "etasl/etc: A constraint-based task specification language and robot controller using expression graphs," in *2014 IEEE/RSJ International Conference on Intelligent Robots and Systems*, 2014, pp. 1540-1546.

[66] C. Samson, M. Le Borgne, and B. Espiau, *Robot Control: The Task Function Approach*. Caledron Press, Oxford, 1991.

[67] H. Bruyninckx and J. De Schutter, "Specification of force-controlled actions in the "task frame formalism"-a synthesis," *IEEE Transactions on Robotics and Automation*, vol. 12, no. 4, pp. 581-589, 1996.

[68] T. de Laet, R. Smits, H. Bruyninckx, and J. de Schutter, "Constraint-based task specification and control for visual servoing application scenarios," vol. 60, no. 5, pp. 260-269, 2012. [Online]. Available: https://doi.org/10.1524/auto.2012.0996

[69] S. Haddadin, A. De Luca, and A. Albu-Schäffer, "Robot collisions: A survey on detection, isolation, and identification," *IEEE Transactions on Robotics*, vol. 33, no. 6, pp. 1292-1312, 2017.
[70] Hong Zhang and J. P. Ostrowski, "Visual servoing with dynamics: control of an unmanned blimp," in *Proceedings 1999 IEEE International Conference on Robotics and Automation (Cat. No.99CH36288C)*, vol. 1, 1999, pp. 618-623 vol.1.

[71] M. Vukobratović and D. Stokić, "Is dynamic control needed in robotic systems, and, if so, to what extent?" *The International Journal of Robotics Research*, vol. 2, no. 2, pp. 18-34, 1983. [Online]. Available: https://doi.org/10.1177/027836498300200202

[72] M. Ishikawa, A. Namiki, T. Senoo, and Y. Yamakawa, "Ultra high-speed robot based on 1 khz vision system," in *2012 IEEE/RSJ International Conference on Intelligent Robots and Systems*, 2012, pp. 5460-5461.

[73] N. Shahriari, S. Fantasia, F. Flacco, and G. Oriolo, "Robotic visual servoing of moving targets," in *2013 IEEE/RSJ International Conference on Intelligent Robots and Systems*, 2013, pp. 77-82.

[74] A. D. Luca, G. Oriolo, and P. Robuffo Giordano, "On-line estimation of feature depth for image-based visual servoing schemes," in *Proceedings 2007 IEEE International Conference on Robotics and Automation*, 2007, pp. 2823-2828.

[75] E. Rohmer, S. P. N. Singh, and M. Freese, "Coppeliasim (formerly v-rep): a versatile and scalable robot simulation framework," in *Proc. of The International Conference on Intelligent Robots and Systems (IROS)*, 2013. [Online]. Available: www.coppeliarobotics.com

[76] T. Larsen, N. Andersen, O. Ravn, and N. Poulsen, "Incorporation of time delayed measurements in a discrete-time kalman filter," in *Proceedings of the 37th IEEE Conference on Decision and Control (Cat. No.98CH36171)*, vol. 4, 1998, pp. 3972-3977 vol.4.

[77] E. Simetti and G. Casalino, "A novel practical technique to integrate inequality control objectives and task transitions in priority based control," *Journal of Intelligent & Robotic Systems*, vol. 84, pp. 877-902, 2016.

[78] F. Fusco, O. Kermorgant, and P. Martinet, "Integrating features acceleration in visual predictive control," *IEEE Robotics and Automation Letters*, vol. 5, no. 4, pp. 5197-5204, 2020.
[79] A. Maciejewski, "Dealing with the ill-conditioned equations of motion for articulated figures," *IEEE Computer Graphics and Applications*, vol. 10, no. 3, pp. 63-71, 1990.

[80] R. Featherstone, "An empirical study of the joint space inertia matrix," *I. J. Robotic Res.*, vol. 23, pp. 859-871, 09 2004.

[81] V. Lippiello, G. A. Fontanelli, and F. Ruggiero, "Image-based visual-impedance control of a dual-arm aerial manipulator," *IEEE Robotics and Automation Letters*, vol. 3, no. 3, pp. 1856-1863, 2018.

[82] R. Carelli, E. Oliva, C. Soria, and O. Nasisi, "Combined force and visual control of an industrial robot," *Robotica*, vol. 22, no. 2, p. 163-171, 2004.

[83] C. Gaz and A. De Luca, "Payload estimation based on identified coefficients of robot dynamics — with an application to collision detection," in *2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)*, 2017, pp. 3033-3040.

[84] S. Haddadin, A. De Luca, and A. Albu-Schäffer, "Robot collisions: A survey on detection, isolation, and identification," *IEEE Transactions on Robotics*, vol. 33, no. 6, pp. 1292-1312, Dec 2017.

[85] E. Magrini and A. De Luca, "Hybrid force/velocity control for physical human-robot collaboration tasks," in *Proc. IEEE/RSJ Int. Conf. on Intelligent Robots and Systems*, Oct. 2016.

[86] J. Hollerbach, W. Khalil, and M. Gautier, "Model identification," in *Handbook of Robotics*, B. Siciliano and O. Khatib, Eds. Springer, 2008, pp. 321-344.

[87] A. Janot, P. Vandanjon, and M. Gautier, "A generic instrumental variable approach for industrial robot identification," *IEEE Transactions on Control Systems Technology*, vol. 22, no. 1, pp. 132-145, Jan 2014.

[88] J. Jovic, A. Escande, K. Ayusawa, E. Yoshida, A. Kheddar, and G. Venture, "Humanoid and human inertia parameter identification using hierarchical optimization," *IEEE Transactions on Robotics*, vol. 32, no. 3, pp. 726-735, June 2016.

[89] V. Bonnet, P. Fraisse, A. Crosnier, M. Gautier, A. González, and G. Venture, "Optimal exciting dance for identifying inertial parameters of an an-
thropomorphic structure,” *IEEE Transactions on Robotics*, vol. 32, no. 4, pp. 823–836, Aug 2016.

[90] V. Mata, F. Benimeli, N. Farhat, and A. Valera, “Dynamic parameter identification in industrial robots considering physical feasibility,” *Advanced Robotics*, vol. 19, no. 1, pp. 101–119, 2005.

[91] C. Sousa and R. Cortesão, “Physical feasibility of robot base inertial parameter identification: A linear matrix inequality approach,” *Int. J. of Robotics Research*, vol. 33, no. 6, pp. 931–944, 2014. [Online]. Available: https://doi.org/10.1177/0278364913514870

[92] Franka Emika, “Ros integration for franka emika research robots,” https://github.com/frankaemika/frankarosse.

[93] N. Koenig and A. Howard, “Design and use paradigms for gazebo, an open-source multi-robot simulator,” in *2004 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)* (IEEE Cat. No.04CH37566), vol. 3, 2004, pp. 2149–2154 vol.3.

[94] M. Spong, “Modeling and control of elastic joint robots,” *ASME J. Dyn. Syst. Meas. Control*, vol. 109, no. 4, pp. 310–319, 1987.

[95] C. Gaz, F. Flacco, and A. De Luca, “Identifying the dynamic model used by the KUKA LWR: A reverse engineering approach,” in *2014 IEEE International Conference on Robotics and Automation (ICRA)*, May 2014, pp. 1386–1392.

[96] J. Swevers, W. Verdonck, and J. De Schutter, “Dynamic model identification for industrial robots,” *IEEE Control Systems Mag.*, vol. 27, no. 5, pp. 58–71, 2007.

[97] A. Jubien, M. Gautier, and A. Janot, “Dynamic identification of the Kuka Lightweight robot: Comparison between actual and confidential Kuka’s parameters,” in *2014 IEEE/ASME International Conference on Advanced Intelligent Mechatronics*, July 2014, pp. 483–488.

[98] T. Mitchell, *Machine Learning*. McGraw-Hill, 1997.

[99] S. Russell and P. Norvig, *Artificial Intelligence: A Modern Approach*, 3rd ed. Prentice Hall, 2009.