*3.2. Impedance Control*

The forward kinematics of a robotic manipulator is written as [23]:

$$\mathbf{x}(t) = f(q) \tag{1}$$

where *<sup>x</sup>*(*t*) ∈ *<sup>n</sup>* and *<sup>q</sup>* ∈ *<sup>n</sup>* are the pose (i.e., position/orientation) of the end-effector in the Cartesian space and the joint angle coordinates in the joint space, respectively. Differential kinematics is obtained by deriving (1) with respect to time:

$$
\dot{x}(t) = f(q)q \tag{2}
$$

where *<sup>J</sup>*(*q*) ∈ *n*×*<sup>n</sup>* is the Jacobian matrix. Differentiating (2) again results in the acceleration of the end-effector:

$$
\ddot{x}(t) = f(q)q + f(q)\ddot{q} \tag{3}
$$

The robot arm dynamics in the joint space is given by:

$$M(q)\ddot{q} + \mathcal{C}(q, \dot{q})\dot{q} + G(q) = \tau - J^T(q)F \tag{4}$$

where *<sup>M</sup>*(*q*) ∈ *n*×*<sup>n</sup>* is the symmetric positive-definite inertia matrix; *<sup>C</sup>*(*q*, *<sup>q</sup>*˙)*q*˙ ∈ *<sup>n</sup>* is the Coriolis and Centrifugal forces; *<sup>G</sup>*(*q*) ∈ *<sup>n</sup>* is the gravitational force; *<sup>τ</sup>* ∈ *<sup>n</sup>* is the vector of control input; *<sup>F</sup>* ∈ *<sup>n</sup>* denotes the net force exerted by the robot on the environment at the end-effector (*<sup>F</sup>* = *Fr* − *Fext*). Otherwise, *Fr* ∈ *<sup>n</sup>* is the force exerted by the robot on the environment at the end-effector while *Fext* ∈ *<sup>n</sup>* is the external force exerted by the environment on the robot at the end-effector. In our pHRI task, the environment is the human, specifically the human hand that is in contact with the robot. The robot dynamics can be written in the Cartesian space as:

$$M\_x(q)\ddot{\mathbf{x}} + \mathbb{C}\_x(q, q)\dot{\mathbf{x}} + \mathbb{G}\_x(q) = \boldsymbol{\mu} - \boldsymbol{F} \tag{5}$$

where

$$\begin{array}{c} \mathcal{M}\_{\mathfrak{x}}(q) = \mathcal{J}^{-T}(q)\mathcal{M}(q)\mathcal{J}^{-1}(q),\\ \mathcal{C}\_{\mathfrak{x}}(q,\dot{q}) = \mathcal{J}^{-T}(q)(\mathcal{C}(q,\dot{q}) - \mathcal{M}(q)\mathcal{J}^{-1}(q)\dot{\mathcal{J}}(q))\mathcal{J}^{-1}(q),\\ \mathcal{G}\_{\mathfrak{x}}(q) = \mathcal{J}^{-T}(q)\mathcal{G}(q), \boldsymbol{u} = \mathcal{J}^{-T}(q)\boldsymbol{\pi}, \boldsymbol{F} = \boldsymbol{F}\_{\mathfrak{r}} - \boldsymbol{F}\_{\mathfrak{ext}} \end{array}$$

As explained in Section 3.1, the most common interaction controllers for pHRI are admittance and impedance control. Both controllers are based on a target impedance model for the robot and they only differ in terms of input and output (see Figures 3 and 4). Therefore, the target robot impedance model can be represented as a mass-damper-spring system:

$$M\_d(\ddot{\mathbf{x}}\_d - \ddot{\mathbf{x}}) + B\_d(\dot{\mathbf{x}}\_d - \dot{\mathbf{x}}) + \mathbb{K}\_d(\mathbf{x}\_d - \mathbf{x}) = F\_d = F\_r \tag{6}$$

where *Md*, *Bd*, *Kd* are the virtual inertia, damping and stiffness of the robot, respectively. *Fd* is the desired force and *xd* can be interpreted as the rest position of this virtual massdamper-spring system. For pHRI tasks in which the human touches the robot, the human limb (arm+hand) can also be modeled as a mass-damper-spring system:

$$(M\_h \ddot{\mathbf{x}}\_h + B\_h \dot{\mathbf{x}}\_h + K\_h(\mathbf{x}\_h - \mathbf{x}\_{hd}) = F\_h \tag{7}$$

where *Mh*, *Bh*, *Kh* are the limb inertia, damping and stiffness respectively and *Fh* is the force applied by the human to the robot. The limb impedance values are not fixed and depend from person to person, as well as on the task being carried out. *xh* is the position of the human wrist in the robot frame and *xhd* can be interpreted as the desired target position. Discussion on the limb impedance parameters or their calculation are not in the purview of this paper. In fact, the investigation of the limb impedance parameters would have been necessitated if an admittance controller was used, to tackle the potential controller stability issues. Since we have used a torque-based impedance controller for our experiments, it does not face such issues. While real time measurements of limb impedance values could be used for interpreting the intent of the human operator during the operation, this would have required attaching EMG sensors on the arms of the human operator, such as in [24], thereby reducing the practicality of this research for an industrial meat cutting scenario. In our experiments the human intent is interpreted via the force measurements read by the FT sensor, while staying comfortable and intuitive for the human operator.

When no assistance strategy is required and we want the robot to freely follow the motion of the human, we can set *Kd* = 0 or *x* = *xd*. This "direct teaching mode" is essentially based on the elimination of the spring component of the robot impedance model ("minimum impedance" strategy in Section 5.4) and avoids any restoring forces. If the human holds the tool rigidly then we can assume that the forces are transmitted completely to the robot. If the hand is close enough to the end-effector, we can assume that their positions, velocities and forces are equivalent: *xd* = *xhd*, *x*˙*<sup>h</sup>* = *x*˙ and *x*¨*<sup>h</sup>* = *x*¨.

In admittance control (popularly called position-based impedance control) the input is the force applied by the environment and the output is displacement/velocity as shown in Figure 3. In pHRI tasks, the force is applied by the human on the tool at the end-effector (*Fext* = *Fh*) and it is measured by the FT sensor. The difference between the desired force for the robot (*Fd* = *Fr*) and this external force is applied as input to the impedance model in (6) in order to obtain the desired position for the end-effector *xd* as output:

$$M\_d(\ddot{\mathbf{x}}\_d - \ddot{\mathbf{x}}) + B\_d(\dot{\mathbf{x}}\_d - \dot{\mathbf{x}}) + K\_d(\mathbf{x}\_d - \mathbf{x}) = F\_d - F\_h \tag{8}$$

On the contrary, the impedance control (torque-based impedance control) has as input the end-effector displacement/velocity and the desired force as output, as shown in Figure 4. In this case, the desired robot impedance model is the same as in (6) but the difference between the desired robot force and the external/human force is transformed into joint torques for the robot control.

**Figure 3.** Admittance control (position-based impedance control).

**Figure 4.** Torque-based impedance control.

#### *3.3. Long Short-Term Memory Model*

For the intent prediction module, we use RNN-LSTM units [25]. RNNs (Recurrent Neural Networks) are based on processing sequential data, especially temporal data as they have an internal memory. In fact, they are useful for making predictions using timeseries data [23]. RNNs can be improved by using what are called LSTM units in order to solve the vanishing gradient problem (i.e., gradients tend to disappear in RNNs when backpropagating errors in too long sequences). In fact, each LSTM unit has a special structure composed by 3 gates to control what information to keep and what to forget so that more stable errors are backpropagated (see Figure 5):


As a result, RNNs with LSTM units are able to learn long-term dependencies within data sequences that were not possible only with RNNs. Given an input sequence *x*¯1, *x*¯2, ..., *x*¯*<sup>t</sup>* the LSTM unit maps the input sequence to a sequence of hidden states *h*1, *h*2, ..., *ht* (which are also the outputs) by passing information through a combination of gates (see Figure 6):

The Input gate (for updating the cell) is:

$$\dot{a}\_t = \sigma\_\mathcal{\mathbb{S}} \left( \mathsf{W}\_i \mathfrak{x}\_t + \mathsf{R}\_i \mathfrak{h}\_{t-1} + \mathfrak{b}\_i \right) \tag{9}$$

The Forget gate (for reseting the cell/forgetting) is:

$$f\_t = \sigma\_\% \left( \mathcal{W}\_f \mathfrak{x}\_t + \mathcal{R}\_f h\_{t-1} + b\_f \right) \tag{10}$$

The Cell candidate (for adding information to the cell) is:

$$\mathbf{g}\_t = \sigma\_\varepsilon (\mathsf{W}\_\mathcal{S} \bar{\mathbf{x}}\_t + \mathsf{R}\_\mathcal{S} h\_{t-1} + b\_\mathcal{S}) \tag{11}$$

where *σ<sup>c</sup>* is the state activation function (here it is the hyperbolic tangent function: *σ<sup>c</sup>* = *tanh*(*x*¯)). The Output gate is:

$$
\sigma\_t = \sigma\_\circ (\mathsf{W}\_o \vec{\pi}\_t + \mathsf{R}\_o h\_{t-1} + b\_o) \tag{12}
$$

where *σ<sup>g</sup>* is the gate activation (here it is the sigmoid function: *σ*(*x*)=(1 + *e*−*x*¯ )−1) The Memory Cell state at timestep *t* is:

$$\mathfrak{c}\_{t} = f\_{t} \odot \mathfrak{c}\_{t-1} + i\_{t} \odot \mathfrak{g}\_{t} \tag{13}$$

Here is the Hadamard product (element-wise multiplication of vectors). The memory cell selectively retains information from previous timesteps by controlling what to remember via the forget gate *ft*.

**Figure 5.** A single LSTM unit.

**Figure 6.** An unrolled Recurrent Neural Network with LSTM units.

The Hidden state (also called Output state) at time step *t* is :

$$h\_t = o\_t \odot \sigma\_\mathfrak{c}(\mathfrak{c}\_t) \tag{14}$$

The hidden state is passed as input to the next timestep and thus, it is possible to stack numerous LSTM units (see Figure 6). *Wi*, *Wf* , *Wg* are the learnable input weights; *Ri*, *Rf* , *Rg* are the learnable recurrent weights and *bi*, *bf* , *bg* are the learnable bias. By using memory cells and hidden states, LSTM units are able to retain information. The sigmoid function is a good activation function for the 3 gates (In, Out and Forget) since it outputs a value between 0 and 1. However for the memory cell, the values should be able to increase or decrease (which is not possible with the sigmoid activation function whose output is

always non-negative). Therefore, the hyperbolic tangent function (*tanh*) is used as the activation function for the memory cell.

#### **4. Experimental Setup**

#### *4.1. KUKA LWR Robot*

The KUKA LWR is a Torque-controlled Flexible Robot with 7 degrees of freedom. The design and control concepts of the robot have been discussed in [26,27]. There are two control modes available. The joint position control is implemented at a frequency of 3 kHz (decentralized control) [28]. Inverse kinematics and the cartesian impedance control mode run at a frequency of 1 kHz. The KUKA LWR has a torque sensor at each joint that enables torque control and impedance control. It also has motor side position sensors, as well as link side position sensors. Due to friction it is difficult for robots to implement torque control only with motor current commands [28].

The default coordinate system of the KUKA LWR is Right Handed System for which det(R) = 1. The dimensions of the robot can be retrieved from the official manufacturer documentation. Using these dimensions we can obtain the DH parameters (refer Table 1). To simulate the robot, an alternate form of representation called URDF is shown in Table 2 and its corresponding 3D model is shown in Figure 7. The Unified Robotic Description Format (URDF) is an XML file format used in ROS to describe all elements of a robot. The robot interacts with the external PC through the Fast Research Interface [29] through three modes—(a) Joint position control (b) Joint impedance control mode and (c) Cartesian impedance control mode.

**Figure 7.** Exoscarne 3D simulated model with the KUKA LWR arm in Home configuration.


**Table 1.** DH parameters of KUKA LWR 4+ robot.


**Table 2.** URDF description of KUKA LWR 4+ robot.

To fulfill our objectives we used the cartesian impedance control mode available with the KUKA LWR robot. The KUKA manual [30] states that the control law for the cartesian impedance controller is

$$\boldsymbol{\pi}\_{\rm cmd} = \boldsymbol{J}^{\rm T} (\boldsymbol{K}\_{\rm c} (\mathbf{x}\_{\rm desired} - \mathbf{x}\_{\rm ccurrent}) + \boldsymbol{F}\_{\rm cmd}) + \boldsymbol{D}(\boldsymbol{d}\_{\rm c}) + \boldsymbol{f}\_{\rm dynamics} (\boldsymbol{q}, \boldsymbol{q}, \vec{\boldsymbol{q}}) \tag{15}$$

where *<sup>q</sup>* ∈ *<sup>n</sup>* is the joint position vector, *Kd* is the stiffness matrix in the end-effector frame, *Dd* is the normalized damping parameter in the end-effector frame, *x* and *xd* are the current and the desired pose of the end-effector respectively in the global frame. The translational stiffness *Kx*, *Ky*, *Kz* ∈ [0.01, 5000] N/m and rotational stiffness *KAz* , *KBy* , *KCx* ∈ [0.01, 300] N/m-rad.

The KUKA LWR has an inbuilt external tool calibration functionality. Using this feature the robot can account for external tool dynamics as well, thereby enabling gravity compensation for the tool.

In this work we used the KUKA Fast Research Interface (FRI [29]), ROS [31], Kinematics and Dynamics library (KDL [32]) , the MATLAB toolbox by Peter Corke (RCV [33]), and the MATLAB Robotics Toolbox (RTB [34]). For the intent prediction strategy, the high level program was written in MATLAB and connected to the network via ROS (Robot Operating System), by using the MATLAB Robotics Toolbox.

#### *4.2. ATI FT Sensors*

In order for the robot to provide assistive forces as per user comfort we had to measure how much forces are being applied by the user. For this, we used two 6-axis ATI Gamma force-torque sensors (see Figure 8): one mounted below the joystick (sensor B) and another mounted on the end-effector of the arm (sensor A, see Figure 9). Both FT (Force-Torque) sensors provide a 6-dimensional wrench in the sensor frame at 1000 Hz.

(**a**) FT ATI acquisition system (**b**) FT sensor (**c**) Internal view (**d**) Internal view 2 **Figure 8.** Photos of the force-torque (FT) sensor.

**Figure 9.** Attaching the FT sensor A to the KUKA LWR 4+ robot end-effector.

The relation between the different frames associated to these sensors (A and B), to the tool (i.e., tool center point—TCP—or tip of the knife) and the robot world frame O are shown in Table 3 and visualized in Figure 10. To use the impedance controller all the forces must be in the same frame. This requires transformation of the sensed forces in the sensor frame to the end-effector frame of the robot. The equation for transformation of forces from one frame to another is:

$$
\begin{bmatrix} \,^A F\_A \\ \,^A M\_A \end{bmatrix} = \begin{bmatrix} \,^B\_A R & 0 \\ \left[ \,^A t\_x \right]\_A^B R & \,^B\_A R \end{bmatrix} \begin{bmatrix} \,^B F\_B \\ \,^B M\_B \end{bmatrix} \tag{16}
$$

$$\begin{aligned} \text{where } \begin{bmatrix} t\_x \end{bmatrix} = \begin{bmatrix} 0 & -t\_z & t\_y \\ t\_z & 0 & -t\_x \\ -t\_y & t\_x & 0 \end{bmatrix} \text{ or } \\\\ ^A F\_A &= \, ^B\_A T\_f \, ^B F\_B \end{aligned} \tag{17}$$

**Figure 10.** Visualization of all frames relating the tool -knife- with the FT sensors A/B.


**Table 3.** Joystick frames when the robot is in Home configuration. Sensor A is always aligned with the robot end-effector frame.

### *4.3. Allen-Bradley Joystick*

The joystick (i.e., the knife handle) is an Allen-Bradley 440J-N enabling switch. It has 8 electrical connections and when the joystick switch is pushed or released, combinations of these 8 connections are activated as shown in Figure 11.

(**a**) Joystick pressed

**Figure 11.** Allen-Bradley joystick used as the handle of the knife.

The 8 electrical wires from the joystick are connected to an Arduino Uno circuit board which was then connected to a laptop (see Figure 12 for the complete connection diagram). The rosserial ROS package and the ros\_lib library is used to integrate the Arduino Uno with the robot network via ROS.

**Figure 12.** Arduino circuit diagram for connecting the joystick to a laptop.

By using this joystick, the user will be able to communicate his intention clearly with regards to the meat cutting operation. See Section 5.2 for a more detailed description of how the user will use this joystick for a safe and unobtrusive physical human-robot interaction.
