# An Optimization-Based Impedance Approach for Robot Force Regulation with Prescribed Force Limits

Authors: R. de J. Portillo-Vélez; A. Rodriguez-Angeles; C. A. Cruz-Villar
Journal: Mathematical Problems in Engineering
 (2015)

---

## Abstract

An optimization based approach for the regulation of excessive or insufficient forces at the end-effector level is introduced. The objective is to minimize the interaction force error at the robot end effector, while constraining undesired interaction forces. To that end, a dynamic optimization problem (DOP) is formulated considering a dynamic robot impedance model. Penalty functions are considered in the DOP to handle the constraints on the interaction force. The optimization problem is online solved through the gradient flow approach. Convergence properties are presented and the stability is drawn when the force limits are considered in the analysis. The effectiveness of our proposal is validated via experimental results for a robotic grasping task.


---

## Body

## 1. Introduction

The shifting paradigm of robotics from industrial applications to field oriented and human-robot interaction tasks triggered the use of force regulation strategies as a fundamental tool to successfully perform tasks with physical interaction between the robot and the environment, [1, 2]. Nowadays, several applications of robot force control strategies are running in many fields of the human productive and everyday activities [3, 4].Traditionally, to successfully perform physical robotic interaction tasks with the environment, three stages are considered. In the first stage, using only motion control, the robot moves freely towards the surface of interaction (e.g., a human tissue or an iron surface of an object). In the second stage, using a transition controller, contact between the robot and the surface is established with an impact caused by the transition from free motion to constrained motion. Finally, in the constrained motion stage, the goal for the robot is to keep contact with the environment by using a robot force regulation strategy. Robot force control has thoroughly studied the three stages of robot force interaction, focusing mainly in the third one [5, 6].Two major approaches for robot force regulation are distinguished in the literature. Indirect force control (or impedance control) and direct (or explicit) force control [7]. Impedance control implicitly considers the three stages of interaction with the environment without a switching strategy from free motion to constrained motion as opposed to direct motion control approaches.Despite the amount of work regarding robot force regulation, new problems arise in practical applications such as human-robot interaction, robotic surgery and industrial tasks for precision finishing of materials. For example, the notion of safety in human-robot interaction as well as kinematic and dynamic uncertainty in grasping tasks, lead to the definition of new constraints on the interaction force, which along with other constraints are recently focus of interest of control in robotics, [8]. The main restriction is that such problems must be online solved.On limiting the interaction force in robotics, few approaches are considered in the literature. One approach is to design and implement mechanical devices which passively limit excessive forces when the robot interacts with the environment [9–11]. Another approach is to fully design variable stiffness compliant actuators which, together with special software development, which might render the required force safety levels for a given force controlled task [12, 13]. Both approaches, require a complete mechanical design of the force limiter device which demands a lot of design time and might result in a very expensive tool.Alternatively, novel control schemes together with useful force sensors or parameter estimation techniques render an alternative solution to satisfy the required thresholds of undesired forces. In [14], an iterative feedback tuning algorithm is used to comply with the force safety requirements. This is achieved by formulating a constrained optimization problem and solving it using sequential quadratic programming. In [15], a neural network controller ensures that the control force remains within prescribed limits imposed by the user. This is achieved by properly adjusting the feedback gains of the controller. In [16], a force limiting adaptive controller is proposed to bound large interaction forces during a noncontact to contact transition with a mass-spring system. The semiglobal stability of the controller is given and experimental results without force sensors are presented. More recently, in [17], a force threshold-based position algorithm for legged locomotion is introduced. The algorithm uses preplanned motion and force measures to either elevate or depress the foot.Our approach considers that, using the motion control loop of the robot, one can compute online modifications to the commanded robot trajectory using force sensor measurements, in order to satisfy prescribed force limits. To compute such trajectory modifications, a dynamic optimization problem for the force tracking error, considering prescribed interaction force limits, is formulated and solved.The objective is to minimize the error for the interaction force. The optimization problem considers, as a performance index, the square of the weighted sum of the interaction force error as well as its time derivative [18]. In this work, it is assumed that the position of the end-effector of the robot is controlled by a classical position controller as a first control loop, see Figure 1. Moreover, a dynamic impedance model of the interaction between the robot and the environment is implemented via a classical position-based impedance approach as a second control loop [19, 20], see Figure 1. Such impedance model is an ordinary differential equation representing a dynamic equality constraint for the optimization problem, thus leading to a constrained DOP. As an inequality constraint for the DOP, interaction force limits are included, which are handled by penalty functions, [21]. The resulting constrained DOP is online solved through the gradient flow approach [22], which allows easily handling of the differential equation associated to the equality constraint (dynamic impedance model) for a robot. This is the main reason for using the gradient flow approach. The independent variable of the DOP is a computed reference trajectory arising from the impedance model for a given robot. As depicted in Figure 1, such reference trajectory (X
r) is online fed to the robot position controller (plus the desired impedance trajectory, X
I) in order to achieve the desired interaction force while accounting for undesired excessive or even insufficient forces. This represents the main difference with previous works and this feature is of great importance in some tasks such as robotic deburring in manufacturing processes and robotic grasping, where insufficient or excessive interaction forces might lead to break contact with the object or damage it, therefore failing the task.Figure 1
Block representation for the proposed force regulation scheme.The rest of the paper is organized as follows. Section2 provides the mathematical models used in this work. Section 3 presents the proposed optimization approach, the development of the optimization problem (where the interaction force limits are handled by penalty constraint functions) and its solution through the gradient flow method. The convergence and stability properties of the proposed approach are also addressed. Section 4 describes the experiment for validating our approach and presents its analysis and discussion. Section 5 gives the analysis and discussion of the experimental results and finally Section 6 closes the paper with conclusions and future work.


## 2. Mathematical Models

This section presents the mathematical model of a robot, used at the formulation of the optimization problem. It is important to note that the force regulation approach is developed in the Cartesian space. Nevertheless, the inverse kinematic model of the robot is required.


### 2.1. Impedance Model

Consider a rigid robot withn joint degree of freedom. Moreover, its end-effector can move in a m-dimensional Cartesian space. The direct kinematic model for a robot is a nonlinear mapping F
D
K
:
R
n
→
R
m given by(1)
X
=
F
D
K
q
,where the vector X
∈
R
m represents the Cartesian position of the robot end-effector and the vector q
∈
R
n contains the joint positions of the robot.Similarly, the inverse kinematic model of a robot is defined as the nonlinear mappingF
I
K
:
R
m
→
R
n given by(2)
q
=
F
I
K
X
.In general, this model might have infinite solutions and singularities. Thus, in the rest of the work it is assumed that the robot moves outside any singular configuration and the solution of the inverse kinematic model is given such that a unique solution is obtained. Moreover, the position-based impedance approach [19] for the interaction between a robot end-effector and the environment is considered, see Figure 1. It is important to mention that, within this approach to impedance control, two control loops are used to render the desired impedance and a force sensor is required. It is assumed that (at joint level) the motion of a robot is controlled by any position controller which can render trajectory tracking, for example, a PID controller.The first control loop (shaded region in Figure1) guarantees trajectory tracking of the commanded reference trajectory X
c
∈
R
m. In the second control loop, a desired impedance trajectory X
I
∈
R
m is used. Such trajectory is the solution to(3)
D
X
I
¨
+
B
X
I
˙
+
K
X
I
=
F
t
,where D, B
, and K are m
×
m positive definite diagonal mass, damping, and stiffness matrices, respectively, and F represents an external force [20].In Figure1, X
o
∈
R
m represents the desired interaction point located on the environment surface, X
r
∈
R
m is a Cartesian reference trajectory (to be computed as the independent variable), which is used to render the desired interaction force F
r
∈
R
m; q
c
∈
R
n is the joint position commanded to the motion controller, and e
∈
R
n is the tracking joint error used in the first control loop. Note that, on one hand, when X
o
=
X
r
=
0 the classical position-based impedance control approach is obtained. On the other hand, when X
r
=
X
I
=
0 only motion control is rendered.Assume that the end-effector of the robotX matches the commanded trajectory X
c
=
X
o
+
X
I. Under this assumption [20], a linear model for the interaction of the robot and the environment is given by(4)
D
X
¨
+
B
X
˙
+
K
X
-
X
r
=
E
t
.The interaction force error for the robot is denoted by E
(
t
)
=
F
r
-
F
(
t
)
∈
R
m, where F
∈
R
m is the measured interaction force between the robot and the environment. The vector F
r
∈
R
m represents the reference or desired force, which is assumed to be known, and the vector X
r
∈
R
m is an independent reference Cartesian trajectory for the robot end-effector. In this work, the reference trajectory X
r is designed to render the desired interaction force.For the analysis of the force error dynamics, it is required to consider a model for the robot-environment interaction force. A simple but widely adopted model for this interaction is a spring-like one, given by(5)
F
=
K
o
X
-
X
o
,where F
∈
R
m is the robot-object interaction force, K
o
∈
R
m
×
m is the diagonal stiffness matrix of the environment and force sensor at the contact point, and X
o
∈
R
m is the deformed Cartesian position of the contact point on the environment and can be seen as the desired position, see Figure 1. Notice that the deformation (X
-
X
o) represents only a model for recreating the interaction force between the robot and, while in practice, this force corresponds to that measured by the force sensor, mounted at the end-effector of the robot.The Cartesian positionX of the robot end-effector can be obtained from (5) and then substituted in (4). The desired interaction force F
r is set by the user. Nonetheless, the deformed environment position X
o might have inaccuracies due to measurement errors and unexpected motions of the environment. Now, assuming that both, the desired interaction force and the environment position are exactly known and constant [23], the dynamics of the force error is given by(6)
D
E
¨
+
B
E
˙
+
K
+
K
o
E
=
K
F
r
-
K
K
o
X
r
-
X
o
.From (6), it can be observed that the stationary force error is(7)
E
∞
=
K
e
q
K
o
-
1
F
r
+
X
o
-
X
r
,where the matrix K
e
q
=
(
K
+
K
o
)
-
1
K
K
o represents the equivalent stiffness matrix defined by the impedance stiffness matrix and the stiffness matrix of the environment at the contact point.An analysis of (7) reveals that, in order to achieve force tracking (i.e., E
∞
=
0), the reference trajectory must be computed as follows:(8)
X
r
=
X
o
+
K
o
-
1
F
r
.In practice, nevertheless, it is not possible to exactly compute the reference trajectory using (8). This is because neither the real Cartesian position of the deformed contact point on the environment X
o nor the stiffness matrix at the contact point K
o can be exactly known. In the case that estimates of X
o and K
o are given, excessive or insufficient force error might arise, as analyzed in [20]. Therefore, it is necessary to devise a scheme which allows to compute a proper reference trajectory X
r for the robot, that is, such that E
∞
=
0. Within the force regulation schemes, two approaches to compute the reference trajectory has been proposed. In the first one, also known as direct approach, the force is directly used to compute the reference trajectory, for example [20, 24]. In the second approach, known as indirect or implicit approach, online estimation techniques are used to compute the reference trajectory, see [25, 26]. Previously proposed approaches are effective, but they do not consider an aspect of paramount importance in force interaction, that is, the constraints arising from the maximum and minimum allowable forces which render a successful task execution.In the following, an optimization based approach for regulation of interaction forces while considering prescribed force limits is formulated and solved. Theoretical and experimental arguments demonstrate the necessity of bounding the interaction forces for the success of a given task.


## 2.1. Impedance Model

Consider a rigid robot withn joint degree of freedom. Moreover, its end-effector can move in a m-dimensional Cartesian space. The direct kinematic model for a robot is a nonlinear mapping F
D
K
:
R
n
→
R
m given by(1)
X
=
F
D
K
q
,where the vector X
∈
R
m represents the Cartesian position of the robot end-effector and the vector q
∈
R
n contains the joint positions of the robot.Similarly, the inverse kinematic model of a robot is defined as the nonlinear mappingF
I
K
:
R
m
→
R
n given by(2)
q
=
F
I
K
X
.In general, this model might have infinite solutions and singularities. Thus, in the rest of the work it is assumed that the robot moves outside any singular configuration and the solution of the inverse kinematic model is given such that a unique solution is obtained. Moreover, the position-based impedance approach [19] for the interaction between a robot end-effector and the environment is considered, see Figure 1. It is important to mention that, within this approach to impedance control, two control loops are used to render the desired impedance and a force sensor is required. It is assumed that (at joint level) the motion of a robot is controlled by any position controller which can render trajectory tracking, for example, a PID controller.The first control loop (shaded region in Figure1) guarantees trajectory tracking of the commanded reference trajectory X
c
∈
R
m. In the second control loop, a desired impedance trajectory X
I
∈
R
m is used. Such trajectory is the solution to(3)
D
X
I
¨
+
B
X
I
˙
+
K
X
I
=
F
t
,where D, B
, and K are m
×
m positive definite diagonal mass, damping, and stiffness matrices, respectively, and F represents an external force [20].In Figure1, X
o
∈
R
m represents the desired interaction point located on the environment surface, X
r
∈
R
m is a Cartesian reference trajectory (to be computed as the independent variable), which is used to render the desired interaction force F
r
∈
R
m; q
c
∈
R
n is the joint position commanded to the motion controller, and e
∈
R
n is the tracking joint error used in the first control loop. Note that, on one hand, when X
o
=
X
r
=
0 the classical position-based impedance control approach is obtained. On the other hand, when X
r
=
X
I
=
0 only motion control is rendered.Assume that the end-effector of the robotX matches the commanded trajectory X
c
=
X
o
+
X
I. Under this assumption [20], a linear model for the interaction of the robot and the environment is given by(4)
D
X
¨
+
B
X
˙
+
K
X
-
X
r
=
E
t
.The interaction force error for the robot is denoted by E
(
t
)
=
F
r
-
F
(
t
)
∈
R
m, where F
∈
R
m is the measured interaction force between the robot and the environment. The vector F
r
∈
R
m represents the reference or desired force, which is assumed to be known, and the vector X
r
∈
R
m is an independent reference Cartesian trajectory for the robot end-effector. In this work, the reference trajectory X
r is designed to render the desired interaction force.For the analysis of the force error dynamics, it is required to consider a model for the robot-environment interaction force. A simple but widely adopted model for this interaction is a spring-like one, given by(5)
F
=
K
o
X
-
X
o
,where F
∈
R
m is the robot-object interaction force, K
o
∈
R
m
×
m is the diagonal stiffness matrix of the environment and force sensor at the contact point, and X
o
∈
R
m is the deformed Cartesian position of the contact point on the environment and can be seen as the desired position, see Figure 1. Notice that the deformation (X
-
X
o) represents only a model for recreating the interaction force between the robot and, while in practice, this force corresponds to that measured by the force sensor, mounted at the end-effector of the robot.The Cartesian positionX of the robot end-effector can be obtained from (5) and then substituted in (4). The desired interaction force F
r is set by the user. Nonetheless, the deformed environment position X
o might have inaccuracies due to measurement errors and unexpected motions of the environment. Now, assuming that both, the desired interaction force and the environment position are exactly known and constant [23], the dynamics of the force error is given by(6)
D
E
¨
+
B
E
˙
+
K
+
K
o
E
=
K
F
r
-
K
K
o
X
r
-
X
o
.From (6), it can be observed that the stationary force error is(7)
E
∞
=
K
e
q
K
o
-
1
F
r
+
X
o
-
X
r
,where the matrix K
e
q
=
(
K
+
K
o
)
-
1
K
K
o represents the equivalent stiffness matrix defined by the impedance stiffness matrix and the stiffness matrix of the environment at the contact point.An analysis of (7) reveals that, in order to achieve force tracking (i.e., E
∞
=
0), the reference trajectory must be computed as follows:(8)
X
r
=
X
o
+
K
o
-
1
F
r
.In practice, nevertheless, it is not possible to exactly compute the reference trajectory using (8). This is because neither the real Cartesian position of the deformed contact point on the environment X
o nor the stiffness matrix at the contact point K
o can be exactly known. In the case that estimates of X
o and K
o are given, excessive or insufficient force error might arise, as analyzed in [20]. Therefore, it is necessary to devise a scheme which allows to compute a proper reference trajectory X
r for the robot, that is, such that E
∞
=
0. Within the force regulation schemes, two approaches to compute the reference trajectory has been proposed. In the first one, also known as direct approach, the force is directly used to compute the reference trajectory, for example [20, 24]. In the second approach, known as indirect or implicit approach, online estimation techniques are used to compute the reference trajectory, see [25, 26]. Previously proposed approaches are effective, but they do not consider an aspect of paramount importance in force interaction, that is, the constraints arising from the maximum and minimum allowable forces which render a successful task execution.In the following, an optimization based approach for regulation of interaction forces while considering prescribed force limits is formulated and solved. Theoretical and experimental arguments demonstrate the necessity of bounding the interaction forces for the success of a given task.


## 3. Optimization Framework

In this section, the constrained force regulation problem is formulated as a dynamic optimization problem. A gradient-based solution is presented and its stability and convergence properties are drawn.


### 3.1. Optimization Problem Statement

To formulate the dynamic optimization problem (DOP), consider a performance index,I
∈
R. A weighted sum of the interaction force error E
(
t
) and its time derivative E
˙
(
t
) is proposed as the performance index of the DOP. When computing an online reference trajectory X
r, it is important to ensure that such reference does not grow up unbounded [25]. Thus, an intuitive constraint on the computation of such trajectory is its minimization. This fact is considered by the term containing the gain Θ in (9). On one hand, due to physical constraints, such as safety in human-robot interaction or object fragility in robotic grasping, the interaction force F exerted by each robot must be constrained by a maximum force F
m
a
x. On the other hand, a minimum force F
m
i
n must be considered, for example, to avoid breaking contact with a grasped object. Mathematically, this is a constrained DOP and it is written as in problem (9), where α
∈
R
m
×
m and Θ
∈
R
m
×
m are positive definite diagonal matrices of gains, weighting the time derivative of the force error and the reference trajectory, respectively:(9)
m
i
n
X
r
∈
R
m
I
=
1
2
E
t
+
α
E
˙
t
T
E
t
+
α
E
˙
t
k
k
k
k
k
k
k
k
k
k
k
k
k
+
1
2
X
r
T
Θ
X
r
s
u
b
j
e
c
t
t
o
:
F
≤
F
m
a
x
s
u
b
j
e
c
t
t
o
:
F
≥
F
m
i
n
s
u
b
j
e
c
t
t
o
:
D
E
¨
+
B
E
˙
+
K
+
K
o
E
=
K
F
r
-
K
K
o
X
r
-
X
o
.The intuitive meaning of the performance index given in (9) might be divided in two. First, by considering the minimization of the force error, force tracking is pursued. Second, by considering the minimization of the time derivative of the force error, abrupt changes in the force error signal can be effectively damped. Then, by considering a squared function of their weighted sum we ensure the unique minimum is zero. Moreover, note that the term E
(
t
)
+
α
E
˙
(
t
) represents a manifold defined by a first order differential equation for E
(
t
), whose convergence rate to its equilibrium is set by α. Also note that the independent or free variable for the DOP (9) is the reference trajectory X
r.Solving constrained DOP might be computationally expensive, particularly when online solutions are required. Nevertheless, it is possible to transform the constrained DOP (9) into an equivalent one without inequality constraints, so that simpler optimization methods can be used. In this paper, the penalty function approach is employed to establish an equivalent optimization problem, only with equality constraints, see [21]. Therefore, by defining the upper interaction force threshold as F
U
=
F
r
-
F
m
a
x and the lower interaction force threshold as F
L
=
F
m
i
n
-
F
r, the equivalent DOP to (9) can be stated as shown in (10), where I
e denotes the extended performance index (which includes the interaction force constraints), 0
∈
R
m
×
1 is the zero vector, m
a
x
(
·
,
·
) denotes the element-wise maximum value function, and Λ
∈
R
m
×
m and Ψ
∈
R
m
×
m are diagonal gain matrices to penalize excessive or insufficient interaction forces, respectively:(10)
m
i
n
X
r
∈
R
m
I
e
=
1
2
E
t
+
α
E
˙
t
T
E
t
+
α
E
˙
t
m
i
n
X
r
∈
R
m
I
e
=
+
1
2
m
a
x
0
,
F
U
-
E
t
T
Λ
m
i
n
X
r
∈
R
m
I
e
=
·
m
a
x
0
,
F
U
-
E
t
m
i
n
X
r
∈
R
m
I
e
=
+
1
2
m
a
x
0
,
F
L
+
E
t
T
Ψ
m
i
n
X
r
∈
R
m
I
e
=
·
m
a
x
0
,
F
L
+
E
t
m
i
n
X
r
∈
R
m
I
e
=
+
1
2
X
r
T
Θ
X
r
s
u
b
j
e
c
t
t
o
:
D
E
¨
+
B
E
˙
+
K
+
K
o
E
s
u
b
j
e
c
t
t
o
:
=
K
F
r
-
K
K
o
X
r
-
X
o
.The differentiability of the extended performance index I
e in (10) can be analyzed as in [27]. To this end, note that the extended performance index I
e in (10) is continuous at E
(
t
)
=
F
U and E
(
t
)
=
-
F
L when E
˙
(
t
)
=
0.


### 3.2. Gradient-Based Solution

Considering the DOP (10), the gradient flow approach [22] states that the optimal solution X
r
∗ for this problem can be obtained by solving (11), where Γ
∈
R
m
×
m, is a positive definite gain matrix related to convergence properties of the method. Moreover, the gradient ∂
I
e
/
∂
X
r
∈
R
m
×
1 is given in (12):(11)
X
˙
r
=
-
Γ
∂
I
e
∂
X
r
T
,
(12)
∂
I
e
∂
X
r
=
E
t
+
α
E
˙
t
T
S
E
+
α
S
˙
E
-
m
a
x
0
,
F
U
-
E
t
T
·
Λ
S
E
+
m
a
x
0
,
F
L
+
E
t
T
Ψ
S
E
+
X
r
T
Θ
.The matrices S
E
=
∂
E
/
∂
X
r
∈
R
m
×
m and S
˙
E
=
(
d
/
d
t
)
(
∂
E
/
∂
X
r
)
∈
R
m
×
m are the sensitivities of the force error and the time derivative of the force error with respect to the independent reference trajectory X
r, respectively, which can be computed by taking the partial derivative of the interaction force error dynamics (6) with respect to the reference trajectory. By performing straightforward computations, the dynamics of the sensitivities are given by(13)
D
S
¨
E
+
B
S
˙
E
+
K
+
K
o
S
E
=
-
K
K
o
.Note that (13) represents a second order differential equation for the sensitivity matrix S
E, excited by the term -
K
K
o. Moreover, because the matrices K and K
o are constant, no matter the initial conditions of the sensitivity matrix, in steady state, the solution of (13) is given in (14). Thus, the terms [
S
E
+
α
S
˙
E
] and S
E in (12) can be simplified:(14)
S
E
∞
=
-
K
+
K
o
-
1
K
K
o
=
-
K
e
q
⟹
S
E
˙
∞
=
0
.This suggests that the gradient in (12) can be expressed, in a simpler way, as shown in (15), where Ω
(
t
)
=
[
E
(
t
)
+
α
E
˙
(
t
)
]
-
Λ
m
a
x
0
,
F
U
-
E
(
t
)
+
Ψ
m
a
x
0
,
F
L
+
E
(
t
):(15)
∂
I
e
∂
X
r
T
=
-
K
e
q
Ω
t
+
Θ
X
r
.Finally, from (11) and (15), the reference trajectory for the robot end-effector is computed as(16)
X
˙
r
=
Γ
K
e
q
Ω
t
-
Γ
Θ
X
r
.This approach generalizes previous results related to classical force control where the reference trajectory is computed as a PI-like controller, for example [28–30]. This is the optimization-based interaction force approach represented by (16), which can be seen as an extension of the integral force controller or a standard PI force controller when Λ
=
Ψ
=
Θ
=
0. Moreover, the proposed approach renders a framework where it is possible to prove stability of the interaction force when force limits are considered in the analysis, as shown in the following section.


### 3.3. Convergence and Stability Analysis

Theoretical results on the convergence are first given for cases when the constraints are and are not considered in the analysis. It is shown that, whenΘ
=
0, the proposed approach converges to the ideal reference trajectory (8).


#### 3.3.1. Convergence Analysis

The dynamics of the reference trajectory, given by (16), represents a linear time-invariant system for X
r whose convergence is governed by Θ and which is excited by the term Γ
K
e
q
[
Ω
(
t
)
].Consider the extended state vectorξ
-
=
E
E
˙
X
r
T
∈
R
3
m. By writing the state space model of (6) using the vector ξ
-, in closed loop with the reference trajectory (16) results in a third order linear time-invariant system, where three cases are considered: F
m
i
n
<
F
<
F
m
a
x, F
≥
F
m
a
x, and F
≤
F
m
i
n. A theoretical analysis of the steady state response for the interaction force error and the reference trajectory is presented in Table 1 for these cases.Table 1
Convergence analysis.
Case
Reference trajectory in steady state
Interaction force error in steady state
F
m
i
n
<
F
<
F
m
a
x
X
r
∞
=
(
[
K
e
q
]
2
+
Θ
)
-
1
[
K
e
q
]
2
(
K
o
-
1
F
r
+
X
o
)
E
∞
=
(
[
K
e
q
]
2
+
Θ
)
-
1
K
e
q
Θ
(
K
o
-
1
F
r
+
X
o
)
F
≥
F
m
a
x
X
r
∞
=
(
[
K
e
q
]
2
+
Θ
)
-
1
[
K
e
q
]
2
(
I
+
Λ
)
(
K
o
-
1
F
r
+
X
o
)
E
∞
=
(
[
K
e
q
]
2
(
I
+
Λ
)
+
Θ
)
-
1
K
e
q
Θ
(
K
o
-
1
F
r
+
X
o
)
F
≤
F
m
i
n
X
r
∞
=
(
[
K
e
q
]
2
+
Θ
)
-
1
[
K
e
q
]
2
(
I
+
Ψ
)
(
K
o
-
1
F
r
+
X
o
)
E
∞
=
(
[
K
e
q
]
2
(
I
+
Ψ
)
+
Θ
)
-
1
K
e
q
Θ
(
K
o
-
1
F
r
+
X
o
)From Table1, one concludes that in steady state E
∞
≠
0, unless Θ
=
0. In case Θ
≠
0, a trade off between E
∞ and X
r
∞ arises, which corresponds to a classical trade off between position and force tracking in impedance control.By choosingΘ
=
0
, the dynamics of the reference trajectory entirely depends on the interaction force error dynamics. In this case, the reference trajectory remains bounded, provided that the force error converges to zero, as shown in the stability analysis presented in the next section. Moreover, the steady state reference trajectory converges to the ideal reference trajectory (8) for the first case, as presented in Table 1 when Θ
=
0. For the second and third cases the reference trajectory converges to a scaled (by a factor of (
I
+
Λ
) and (
I
+
Ψ
), resp.) ideal reference trajectory. Nevertheless, note that the penalization is only active when excessive or insufficient interaction forces arise. Therefore, force regulation is always achieved.


#### 3.3.2. Stability

Consider the model (6) in closed loop with the reference trajectory (16) with Θ
=
0. Let us introduce the state vector ξ
=
[
E
(
t
)
E
˙
(
t
)
]
T
∈
R
2
m for a robot. Let the performance index I
e in problem (10) be a candidate constrained Lyapunov function [27] of the state vector ξ; that is, (17)
V
ξ
=
1
2
E
t
+
α
E
˙
t
T
E
t
+
α
E
˙
t
+
1
2
m
a
x
0
,
F
U
-
E
t
T
Λ
m
a
x
0
,
F
U
-
E
t
+
1
2
m
a
x
0
,
F
L
+
E
t
T
Ψ
m
a
x
0
,
F
L
+
E
t
.Notice that V
(
ξ
)
=
0 when ξ
=
0 as F
m
i
n
≤
F
r
≤
F
m
a
x. Differentiating (17) with respect to time yields(18)
V
˙
ξ
=
∂
V
ξ
∂
ξ
∂
ξ
∂
X
r
d
X
r
d
t
=
Ω
T
1
α
︷
m
×
2
m
-
K
e
q
0
m
×
m
︸
2
m
×
m
X
˙
r
=
-
Ω
T
K
e
q
X
˙
r
.Substituting (11) (with Θ
=
0) and (12) in (18) gives as result(19)
V
˙
ξ
=
-
Ω
T
K
e
q
Γ
K
e
q
Ω
.Thus, due to the positive definiteness properties of matrices Γ and K
e
q, it is fulfilled that V
˙
(
ξ
)
≤
0. To conclude asymptotic stability of ξ, it is necessary that V
˙
(
0
)
=
0, which is straightforward from the definition of Ω.


### 3.4. Tuning Procedure

Consider (6). In order to study its stability, let the environment position X
o and the reference force F
r be constant. Thus, taking the time derivative of (6) we get(20)
D
E
⃛
+
B
E
¨
+
K
+
K
o
E
˙
=
-
K
K
o
X
˙
r
.Then, by substituting (16) (considering Θ
=
Λ
=
Ψ
=
0) in (20), a homogeneous third order differential equation is obtained as follows(21)
E
⃛
+
D
-
1
B
E
¨
+
D
-
1
K
K
e
K
e
q
Γ
α
+
K
i
+
K
o
E
˙
+
D
-
1
K
K
e
K
e
q
Γ
E
t
=
0
.Our proposal to provide a tuning procedure is to set an equivalent desired dynamic behavior by properly choosing the poles of (22). This is, to get a stable solution to the differential equation (22), where ζ
∈
R
m
×
m, ω
n
∈
R
m
×
m and  p
∈
R
m
×
m are positive diagonal matrices representing damping, natural frequency, and a desired pole, respectively:(22)
E
⃛
+
2
I
ζ
ω
n
+
p
E
¨
+
2
I
ζ
ω
n
p
+
ω
n
2
E
˙
+
ω
n
2
p
E
t
=
0
.By comparing (22) and (21), we obtain the tuning procedure given by (23) and (24), respectively:(23)
Γ
=
K
K
o
K
e
q
-
1
D
ω
n
2
p
,
(24)
α
=
D
ω
n
2
p
-
1
2
I
ζ
ω
n
p
+
ω
n
2
D
-
K
+
K
o
.This analysis only considers the case when F
m
i
n
<
F
<
F
m
a
x, that is, the unconstrained case. The penalization gains Λ and Ψ have not been considered because this will lead to a nonhomogeneous differential equation. Note that the weighted term E
(
t
)
+
α
E
˙
(
t
) in the DOP (10) represents a stable differential equation when it is equated to zero and the matrix α is positive definite. For this, in (24), it must be satisfied that (
2
I
ζ
ω
n
p
+
(
ω
n
)
2
)
D
≥
(
K
+
K
o
). In this context, it is important to highlight that robustness with respect to uncertain stiffness K
o (which is traditionally poorly known) can be achieved by properly choosing ζ
∈
R
m
×
m, ω
n
∈
R
m
×
m, and p
∈
R
m
×
m. Nevertheless, uncertainty at environment position X
o cannot be compensated only with the tuning of the gains Γ and α. In this sense, the introduction of the interaction force penalization approach is justified. Recent studies have attracted interest on this topic from a grasp planning point of view, for example [30].


## 3.1. Optimization Problem Statement

To formulate the dynamic optimization problem (DOP), consider a performance index,I
∈
R. A weighted sum of the interaction force error E
(
t
) and its time derivative E
˙
(
t
) is proposed as the performance index of the DOP. When computing an online reference trajectory X
r, it is important to ensure that such reference does not grow up unbounded [25]. Thus, an intuitive constraint on the computation of such trajectory is its minimization. This fact is considered by the term containing the gain Θ in (9). On one hand, due to physical constraints, such as safety in human-robot interaction or object fragility in robotic grasping, the interaction force F exerted by each robot must be constrained by a maximum force F
m
a
x. On the other hand, a minimum force F
m
i
n must be considered, for example, to avoid breaking contact with a grasped object. Mathematically, this is a constrained DOP and it is written as in problem (9), where α
∈
R
m
×
m and Θ
∈
R
m
×
m are positive definite diagonal matrices of gains, weighting the time derivative of the force error and the reference trajectory, respectively:(9)
m
i
n
X
r
∈
R
m
I
=
1
2
E
t
+
α
E
˙
t
T
E
t
+
α
E
˙
t
k
k
k
k
k
k
k
k
k
k
k
k
k
+
1
2
X
r
T
Θ
X
r
s
u
b
j
e
c
t
t
o
:
F
≤
F
m
a
x
s
u
b
j
e
c
t
t
o
:
F
≥
F
m
i
n
s
u
b
j
e
c
t
t
o
:
D
E
¨
+
B
E
˙
+
K
+
K
o
E
=
K
F
r
-
K
K
o
X
r
-
X
o
.The intuitive meaning of the performance index given in (9) might be divided in two. First, by considering the minimization of the force error, force tracking is pursued. Second, by considering the minimization of the time derivative of the force error, abrupt changes in the force error signal can be effectively damped. Then, by considering a squared function of their weighted sum we ensure the unique minimum is zero. Moreover, note that the term E
(
t
)
+
α
E
˙
(
t
) represents a manifold defined by a first order differential equation for E
(
t
), whose convergence rate to its equilibrium is set by α. Also note that the independent or free variable for the DOP (9) is the reference trajectory X
r.Solving constrained DOP might be computationally expensive, particularly when online solutions are required. Nevertheless, it is possible to transform the constrained DOP (9) into an equivalent one without inequality constraints, so that simpler optimization methods can be used. In this paper, the penalty function approach is employed to establish an equivalent optimization problem, only with equality constraints, see [21]. Therefore, by defining the upper interaction force threshold as F
U
=
F
r
-
F
m
a
x and the lower interaction force threshold as F
L
=
F
m
i
n
-
F
r, the equivalent DOP to (9) can be stated as shown in (10), where I
e denotes the extended performance index (which includes the interaction force constraints), 0
∈
R
m
×
1 is the zero vector, m
a
x
(
·
,
·
) denotes the element-wise maximum value function, and Λ
∈
R
m
×
m and Ψ
∈
R
m
×
m are diagonal gain matrices to penalize excessive or insufficient interaction forces, respectively:(10)
m
i
n
X
r
∈
R
m
I
e
=
1
2
E
t
+
α
E
˙
t
T
E
t
+
α
E
˙
t
m
i
n
X
r
∈
R
m
I
e
=
+
1
2
m
a
x
0
,
F
U
-
E
t
T
Λ
m
i
n
X
r
∈
R
m
I
e
=
·
m
a
x
0
,
F
U
-
E
t
m
i
n
X
r
∈
R
m
I
e
=
+
1
2
m
a
x
0
,
F
L
+
E
t
T
Ψ
m
i
n
X
r
∈
R
m
I
e
=
·
m
a
x
0
,
F
L
+
E
t
m
i
n
X
r
∈
R
m
I
e
=
+
1
2
X
r
T
Θ
X
r
s
u
b
j
e
c
t
t
o
:
D
E
¨
+
B
E
˙
+
K
+
K
o
E
s
u
b
j
e
c
t
t
o
:
=
K
F
r
-
K
K
o
X
r
-
X
o
.The differentiability of the extended performance index I
e in (10) can be analyzed as in [27]. To this end, note that the extended performance index I
e in (10) is continuous at E
(
t
)
=
F
U and E
(
t
)
=
-
F
L when E
˙
(
t
)
=
0.


## 3.2. Gradient-Based Solution

Considering the DOP (10), the gradient flow approach [22] states that the optimal solution X
r
∗ for this problem can be obtained by solving (11), where Γ
∈
R
m
×
m, is a positive definite gain matrix related to convergence properties of the method. Moreover, the gradient ∂
I
e
/
∂
X
r
∈
R
m
×
1 is given in (12):(11)
X
˙
r
=
-
Γ
∂
I
e
∂
X
r
T
,
(12)
∂
I
e
∂
X
r
=
E
t
+
α
E
˙
t
T
S
E
+
α
S
˙
E
-
m
a
x
0
,
F
U
-
E
t
T
·
Λ
S
E
+
m
a
x
0
,
F
L
+
E
t
T
Ψ
S
E
+
X
r
T
Θ
.The matrices S
E
=
∂
E
/
∂
X
r
∈
R
m
×
m and S
˙
E
=
(
d
/
d
t
)
(
∂
E
/
∂
X
r
)
∈
R
m
×
m are the sensitivities of the force error and the time derivative of the force error with respect to the independent reference trajectory X
r, respectively, which can be computed by taking the partial derivative of the interaction force error dynamics (6) with respect to the reference trajectory. By performing straightforward computations, the dynamics of the sensitivities are given by(13)
D
S
¨
E
+
B
S
˙
E
+
K
+
K
o
S
E
=
-
K
K
o
.Note that (13) represents a second order differential equation for the sensitivity matrix S
E, excited by the term -
K
K
o. Moreover, because the matrices K and K
o are constant, no matter the initial conditions of the sensitivity matrix, in steady state, the solution of (13) is given in (14). Thus, the terms [
S
E
+
α
S
˙
E
] and S
E in (12) can be simplified:(14)
S
E
∞
=
-
K
+
K
o
-
1
K
K
o
=
-
K
e
q
⟹
S
E
˙
∞
=
0
.This suggests that the gradient in (12) can be expressed, in a simpler way, as shown in (15), where Ω
(
t
)
=
[
E
(
t
)
+
α
E
˙
(
t
)
]
-
Λ
m
a
x
0
,
F
U
-
E
(
t
)
+
Ψ
m
a
x
0
,
F
L
+
E
(
t
):(15)
∂
I
e
∂
X
r
T
=
-
K
e
q
Ω
t
+
Θ
X
r
.Finally, from (11) and (15), the reference trajectory for the robot end-effector is computed as(16)
X
˙
r
=
Γ
K
e
q
Ω
t
-
Γ
Θ
X
r
.This approach generalizes previous results related to classical force control where the reference trajectory is computed as a PI-like controller, for example [28–30]. This is the optimization-based interaction force approach represented by (16), which can be seen as an extension of the integral force controller or a standard PI force controller when Λ
=
Ψ
=
Θ
=
0. Moreover, the proposed approach renders a framework where it is possible to prove stability of the interaction force when force limits are considered in the analysis, as shown in the following section.


## 3.3. Convergence and Stability Analysis

Theoretical results on the convergence are first given for cases when the constraints are and are not considered in the analysis. It is shown that, whenΘ
=
0, the proposed approach converges to the ideal reference trajectory (8).


### 3.3.1. Convergence Analysis

The dynamics of the reference trajectory, given by (16), represents a linear time-invariant system for X
r whose convergence is governed by Θ and which is excited by the term Γ
K
e
q
[
Ω
(
t
)
].Consider the extended state vectorξ
-
=
E
E
˙
X
r
T
∈
R
3
m. By writing the state space model of (6) using the vector ξ
-, in closed loop with the reference trajectory (16) results in a third order linear time-invariant system, where three cases are considered: F
m
i
n
<
F
<
F
m
a
x, F
≥
F
m
a
x, and F
≤
F
m
i
n. A theoretical analysis of the steady state response for the interaction force error and the reference trajectory is presented in Table 1 for these cases.Table 1
Convergence analysis.
Case
Reference trajectory in steady state
Interaction force error in steady state
F
m
i
n
<
F
<
F
m
a
x
X
r
∞
=
(
[
K
e
q
]
2
+
Θ
)
-
1
[
K
e
q
]
2
(
K
o
-
1
F
r
+
X
o
)
E
∞
=
(
[
K
e
q
]
2
+
Θ
)
-
1
K
e
q
Θ
(
K
o
-
1
F
r
+
X
o
)
F
≥
F
m
a
x
X
r
∞
=
(
[
K
e
q
]
2
+
Θ
)
-
1
[
K
e
q
]
2
(
I
+
Λ
)
(
K
o
-
1
F
r
+
X
o
)
E
∞
=
(
[
K
e
q
]
2
(
I
+
Λ
)
+
Θ
)
-
1
K
e
q
Θ
(
K
o
-
1
F
r
+
X
o
)
F
≤
F
m
i
n
X
r
∞
=
(
[
K
e
q
]
2
+
Θ
)
-
1
[
K
e
q
]
2
(
I
+
Ψ
)
(
K
o
-
1
F
r
+
X
o
)
E
∞
=
(
[
K
e
q
]
2
(
I
+
Ψ
)
+
Θ
)
-
1
K
e
q
Θ
(
K
o
-
1
F
r
+
X
o
)From Table1, one concludes that in steady state E
∞
≠
0, unless Θ
=
0. In case Θ
≠
0, a trade off between E
∞ and X
r
∞ arises, which corresponds to a classical trade off between position and force tracking in impedance control.By choosingΘ
=
0
, the dynamics of the reference trajectory entirely depends on the interaction force error dynamics. In this case, the reference trajectory remains bounded, provided that the force error converges to zero, as shown in the stability analysis presented in the next section. Moreover, the steady state reference trajectory converges to the ideal reference trajectory (8) for the first case, as presented in Table 1 when Θ
=
0. For the second and third cases the reference trajectory converges to a scaled (by a factor of (
I
+
Λ
) and (
I
+
Ψ
), resp.) ideal reference trajectory. Nevertheless, note that the penalization is only active when excessive or insufficient interaction forces arise. Therefore, force regulation is always achieved.


### 3.3.2. Stability

Consider the model (6) in closed loop with the reference trajectory (16) with Θ
=
0. Let us introduce the state vector ξ
=
[
E
(
t
)
E
˙
(
t
)
]
T
∈
R
2
m for a robot. Let the performance index I
e in problem (10) be a candidate constrained Lyapunov function [27] of the state vector ξ; that is, (17)
V
ξ
=
1
2
E
t
+
α
E
˙
t
T
E
t
+
α
E
˙
t
+
1
2
m
a
x
0
,
F
U
-
E
t
T
Λ
m
a
x
0
,
F
U
-
E
t
+
1
2
m
a
x
0
,
F
L
+
E
t
T
Ψ
m
a
x
0
,
F
L
+
E
t
.Notice that V
(
ξ
)
=
0 when ξ
=
0 as F
m
i
n
≤
F
r
≤
F
m
a
x. Differentiating (17) with respect to time yields(18)
V
˙
ξ
=
∂
V
ξ
∂
ξ
∂
ξ
∂
X
r
d
X
r
d
t
=
Ω
T
1
α
︷
m
×
2
m
-
K
e
q
0
m
×
m
︸
2
m
×
m
X
˙
r
=
-
Ω
T
K
e
q
X
˙
r
.Substituting (11) (with Θ
=
0) and (12) in (18) gives as result(19)
V
˙
ξ
=
-
Ω
T
K
e
q
Γ
K
e
q
Ω
.Thus, due to the positive definiteness properties of matrices Γ and K
e
q, it is fulfilled that V
˙
(
ξ
)
≤
0. To conclude asymptotic stability of ξ, it is necessary that V
˙
(
0
)
=
0, which is straightforward from the definition of Ω.


## 3.3.1. Convergence Analysis

The dynamics of the reference trajectory, given by (16), represents a linear time-invariant system for X
r whose convergence is governed by Θ and which is excited by the term Γ
K
e
q
[
Ω
(
t
)
].Consider the extended state vectorξ
-
=
E
E
˙
X
r
T
∈
R
3
m. By writing the state space model of (6) using the vector ξ
-, in closed loop with the reference trajectory (16) results in a third order linear time-invariant system, where three cases are considered: F
m
i
n
<
F
<
F
m
a
x, F
≥
F
m
a
x, and F
≤
F
m
i
n. A theoretical analysis of the steady state response for the interaction force error and the reference trajectory is presented in Table 1 for these cases.Table 1
Convergence analysis.
Case
Reference trajectory in steady state
Interaction force error in steady state
F
m
i
n
<
F
<
F
m
a
x
X
r
∞
=
(
[
K
e
q
]
2
+
Θ
)
-
1
[
K
e
q
]
2
(
K
o
-
1
F
r
+
X
o
)
E
∞
=
(
[
K
e
q
]
2
+
Θ
)
-
1
K
e
q
Θ
(
K
o
-
1
F
r
+
X
o
)
F
≥
F
m
a
x
X
r
∞
=
(
[
K
e
q
]
2
+
Θ
)
-
1
[
K
e
q
]
2
(
I
+
Λ
)
(
K
o
-
1
F
r
+
X
o
)
E
∞
=
(
[
K
e
q
]
2
(
I
+
Λ
)
+
Θ
)
-
1
K
e
q
Θ
(
K
o
-
1
F
r
+
X
o
)
F
≤
F
m
i
n
X
r
∞
=
(
[
K
e
q
]
2
+
Θ
)
-
1
[
K
e
q
]
2
(
I
+
Ψ
)
(
K
o
-
1
F
r
+
X
o
)
E
∞
=
(
[
K
e
q
]
2
(
I
+
Ψ
)
+
Θ
)
-
1
K
e
q
Θ
(
K
o
-
1
F
r
+
X
o
)From Table1, one concludes that in steady state E
∞
≠
0, unless Θ
=
0. In case Θ
≠
0, a trade off between E
∞ and X
r
∞ arises, which corresponds to a classical trade off between position and force tracking in impedance control.By choosingΘ
=
0
, the dynamics of the reference trajectory entirely depends on the interaction force error dynamics. In this case, the reference trajectory remains bounded, provided that the force error converges to zero, as shown in the stability analysis presented in the next section. Moreover, the steady state reference trajectory converges to the ideal reference trajectory (8) for the first case, as presented in Table 1 when Θ
=
0. For the second and third cases the reference trajectory converges to a scaled (by a factor of (
I
+
Λ
) and (
I
+
Ψ
), resp.) ideal reference trajectory. Nevertheless, note that the penalization is only active when excessive or insufficient interaction forces arise. Therefore, force regulation is always achieved.


## 3.3.2. Stability

Consider the model (6) in closed loop with the reference trajectory (16) with Θ
=
0. Let us introduce the state vector ξ
=
[
E
(
t
)
E
˙
(
t
)
]
T
∈
R
2
m for a robot. Let the performance index I
e in problem (10) be a candidate constrained Lyapunov function [27] of the state vector ξ; that is, (17)
V
ξ
=
1
2
E
t
+
α
E
˙
t
T
E
t
+
α
E
˙
t
+
1
2
m
a
x
0
,
F
U
-
E
t
T
Λ
m
a
x
0
,
F
U
-
E
t
+
1
2
m
a
x
0
,
F
L
+
E
t
T
Ψ
m
a
x
0
,
F
L
+
E
t
.Notice that V
(
ξ
)
=
0 when ξ
=
0 as F
m
i
n
≤
F
r
≤
F
m
a
x. Differentiating (17) with respect to time yields(18)
V
˙
ξ
=
∂
V
ξ
∂
ξ
∂
ξ
∂
X
r
d
X
r
d
t
=
Ω
T
1
α
︷
m
×
2
m
-
K
e
q
0
m
×
m
︸
2
m
×
m
X
˙
r
=
-
Ω
T
K
e
q
X
˙
r
.Substituting (11) (with Θ
=
0) and (12) in (18) gives as result(19)
V
˙
ξ
=
-
Ω
T
K
e
q
Γ
K
e
q
Ω
.Thus, due to the positive definiteness properties of matrices Γ and K
e
q, it is fulfilled that V
˙
(
ξ
)
≤
0. To conclude asymptotic stability of ξ, it is necessary that V
˙
(
0
)
=
0, which is straightforward from the definition of Ω.


## 3.4. Tuning Procedure

Consider (6). In order to study its stability, let the environment position X
o and the reference force F
r be constant. Thus, taking the time derivative of (6) we get(20)
D
E
⃛
+
B
E
¨
+
K
+
K
o
E
˙
=
-
K
K
o
X
˙
r
.Then, by substituting (16) (considering Θ
=
Λ
=
Ψ
=
0) in (20), a homogeneous third order differential equation is obtained as follows(21)
E
⃛
+
D
-
1
B
E
¨
+
D
-
1
K
K
e
K
e
q
Γ
α
+
K
i
+
K
o
E
˙
+
D
-
1
K
K
e
K
e
q
Γ
E
t
=
0
.Our proposal to provide a tuning procedure is to set an equivalent desired dynamic behavior by properly choosing the poles of (22). This is, to get a stable solution to the differential equation (22), where ζ
∈
R
m
×
m, ω
n
∈
R
m
×
m and  p
∈
R
m
×
m are positive diagonal matrices representing damping, natural frequency, and a desired pole, respectively:(22)
E
⃛
+
2
I
ζ
ω
n
+
p
E
¨
+
2
I
ζ
ω
n
p
+
ω
n
2
E
˙
+
ω
n
2
p
E
t
=
0
.By comparing (22) and (21), we obtain the tuning procedure given by (23) and (24), respectively:(23)
Γ
=
K
K
o
K
e
q
-
1
D
ω
n
2
p
,
(24)
α
=
D
ω
n
2
p
-
1
2
I
ζ
ω
n
p
+
ω
n
2
D
-
K
+
K
o
.This analysis only considers the case when F
m
i
n
<
F
<
F
m
a
x, that is, the unconstrained case. The penalization gains Λ and Ψ have not been considered because this will lead to a nonhomogeneous differential equation. Note that the weighted term E
(
t
)
+
α
E
˙
(
t
) in the DOP (10) represents a stable differential equation when it is equated to zero and the matrix α is positive definite. For this, in (24), it must be satisfied that (
2
I
ζ
ω
n
p
+
(
ω
n
)
2
)
D
≥
(
K
+
K
o
). In this context, it is important to highlight that robustness with respect to uncertain stiffness K
o (which is traditionally poorly known) can be achieved by properly choosing ζ
∈
R
m
×
m, ω
n
∈
R
m
×
m, and p
∈
R
m
×
m. Nevertheless, uncertainty at environment position X
o cannot be compensated only with the tuning of the gains Γ and α. In this sense, the introduction of the interaction force penalization approach is justified. Recent studies have attracted interest on this topic from a grasp planning point of view, for example [30].


## 4. Experimental Results

In this section, the testbed used to perform the experiments is detailed. Experimental results for a robotic grasping task showing the effectiveness of our proposal are also presented. The grasping task using two robots was selected to test the validity of our approach because of the wide range of robotic applications and because it represents a challenging cooperative robotic task where unexpected forces might arise due to unmodelled object or robot dynamics.


### 4.1. Testbed

The testbed used to perform the experiments consists of two planar robot manipulators. Both robots are built of aluminum (alloy 6063 T-5) of 9.525 mm thickness with three rotational joints, each actuated by a DC brush-less servomotor of theM
i
c
r
o
m
o
© brand. A six axis ATI nano-40 and a JR3 force sensors are suitably mounted at their end effectors. To avoid undesired noisy force signals, a low pass filter with a cut-off frequency of 100 Hz is implemented via software for each sensor. All the time derivatives of the signals required for the implementation of the proposed schema were computed using approximated derivatives (first order filters) using the M
a
t
l
a
b
-
S
i
m
u
l
i
n
k
© Toolbox. The robotic system is depicted in Figure 2.Figure 2
Robotic system used for the experiments.Hereafter, subindex1 refers to the black serial robot (robot 1) and subindex 2 refers to the gray serial robot (robot 2). All Cartesian variables, unless other specification, are measured with respect to an inertial frame {
R
}, aligned horizontally and located at the origin of the first joint of the robot, see Figure 2.Due to its trajectory tracking performance and simplicity of implementation [31], a classical PID joint controller is programmed for each robot as the motion controller for the first control loop, see Figure 1. It must be highlighted that the performance of the proposed force regulation approach depends on the performance of the selected motion controller. Note that if the robot end-effector trajectory X does not match the commanded trajectory X
c, model 4 is no longer valid.To implement the proposed force regulation scheme,M
a
t
l
a
b
S
i
m
u
l
i
n
k
© installed on a personal computer is used. The computer is equipped with two S
e
n
s
o
r
a
y
© data acquisition boards model S-626 to process the control signals of both robots. The implementation of the force regulation strategy is performed via an inner-outer loop (first and second control loop) impedance control approach [7, 19], as shown in Figure 1. The stability issues of such implementation can be consulted in [19]. Note that a PID joint controller is used for each robot; thus, the inverse kinematic model of both robots is required. Nevertheless, Cartesian type controllers might be used.


### 4.2. Grasping in Robotics

One of the most important tasks for robotic devices, such as robotic hands and industrial robots, is objects grasping. To safely grasp an object, it is required that the robotic devices apply proper grasping forces on adequate points on the object surface. The importance of the grasping task is that once it is successfully achieved, it allows robots to perform more complex tasks such as objects transportation and manipulation [32]. Traditionally, to perform any grasping task, two problems should be sequentially solved: grasp synthesis and grasp control.First, given the geometry and physical properties of the object as well as those of the robotic devices, the aim of the grasp synthesis problem is to determine the contact points (or regions) on the object and the grasping forces that must be applied by the robots on the object. The set of such contact points and the corresponding grasping forces defines a grasp. For more details on the grasp synthesis problem, the reader is referred to a recent survey [33] and references therein. One of the main challenges of the grasp synthesis problem is that, traditionally, it has been stated as a constrained optimization problem [34], thus making it difficult to obtain its solution. Constraints related to grasp synthesis are unilateral, and they arise from the fact that robots or fingers can only apply pushing forces on the object surface [35]. This issue is also present in grasp control and has been ignored, nevertheless, it represents a challenging problem with constraints. Second, given the grasping contact points and the required grasping forces, grasp control aims to achieve the essential properties of grasp equilibrium and grasp stability. Other properties such as desired dynamic behavior for a given grasp are also the target of grasp control. From the control point of view, grasping forces can be controlled at the object level, at the end-effector level or at the joint level [36].In general, grasp control has been less explored than grasp synthesis. The reason is that grasp control has been regarded as an extension of classical force control approaches. Notwithstanding, there exist features of paramount importance regarding grasping control, mainly due to grasping force constraints, as in the case of the grasp synthesis stage. This is a nontrivial issue because any mismatch between the actual execution of the grasping task and the planned execution at the grasp synthesis stage might lead to undesired grasping forces. Such forces can be either insufficient [37] or excessive [38], thus losing the contact with the object or damaging it, respectively. In the literature, only few works consider constraints on the grasping forces. In the work of Takahashi et al. [39], excessive forces are considered giving priority to force control over position control. On the other hand, in [40–42] it is proposed that force constraints might be handled by a force threshold, where excessive interaction forces are complied when the threshold is violated. These approaches lack a stability analysis when grasping force constraints are considered. In the following, a grasping experiment considering excessive and insufficient forces is described.


### 4.3. Grasping Experiment

The task for the robots is to grasp a prismatic object made of pine wood ([7.5 × 7.5 × 9.5] [cm]), which is affected by gravity forces, as shown in Figure3 and in the Supplementary Video available online at http://dx.doi.org/10.1155/2015/918301. Note that a DOP is formulated and online solved for each robot; thus, subindex 1 refers to the first robot and subindex 2 refers to the second robot. In order to compute the grasping forces that should be applied on the object, we consider a typical static friction coefficient for metal on wood; that is, μ
=
0.45. The weight of the prismatic object is w
=
0.3 [Kg]. With these data and considering a safety factor of two, it is easy to compute an estimate of the the proper forces to grasp the prismatic object under the action of the gravity force [43]. Therefore, we get that the reference grasping forces must be chosen such that F
r
1
x
=
-
5 [N] for robot 1, and F
r
2
x
=
5 [N] for robot 2. Note that the sign of the reference grasping forces is set according to the sense of the action force defined by a frame attached to the object. Accordingly, the synthesized grasping points are the geometric centers of the opposite rectangular faces of the prismatic object, which are separated by a distance W
o
=
7.5 [cm], see Figure 3(b). Note that the experiments are performed in one dimension (
m
=
1
), that is, along the x-axis of the inertial frame {
R
} in Figure 2. Therefore, all gain matrices reduce to scalars.Figure 3
Pinch command.
(a)
(b)
(c)To perform the grasping task, the following is considered. First, the desired position is such that a perpendicular contact between the object and the end-effectors of the robots is ensured, see Figure3(a). This guarantees that interaction forces are aligned to the force sensor axis along the direction of the applied force, that is, x direction of frame {
R
}. Second, two commands in Cartesian coordinates are sent to the robot motion controllers. The first one is a preshaping command, which is sent to the manipulators in order to reach the first configuration, which aligns the force sensor axis and set a distance W
h between the robot end-effectors, see Figure 3(b). In that moment, the object must be placed between the robot end-effectors. The second is a motion command for each robot to reduce the distance between the end-effectors from W
h to W
p, while keeping the axis alignment. Hereafter, this command will be known as the “pinch” command. Note that distance W
p is chosen smaller than the object width W
o; therefore, the robots make contact with the object performing the grasping task, see Figure 3(c). However, in this situation high grasping forces arise due to a grasping point mismatch. This situation is common in real applications, for instance, when the shape of the object is approximated with vision tools in order to perform grasping tasks, for example [32, 44]. Moreover, traditional approaches of force regulation fail to avoid unexpected interaction forces because this position uncertainty is hardly compensated by the controller unless online adaptation laws are considered [45]. This fact has been highlighted in the tuning procedure presented in Section 3.4.


### 4.4. Experimental Results

In this section, experimental results to show the effectiveness of the proposed approach are presented. In the ideal case, the grasping positions are exactly known. Therefore, the grasping force constraints are not violated and the grasping task is successfully executed. In this work, the main contribution is about handling force interaction limits, nonetheless experimental results for the ideal case are presented in experiment 1 to demonstrate the tradeoff betweenE
i
∞ and X
r
i
∞ when Θ
i
x
≠
0. In experiment 2, it is assumed that there exists a mismatch between the actual robot position X and the synthesized position X
o of the grasping points, that is, W
p
<
W
o. In this situation, excessive interaction forces arise and the effects of the penalization gain Λ on the grasping performance are depicted. Experiment 3 presents a case where both excessive and insufficient grasping forces arise due to a transport task and then the proposed penalization scheme and appropriately tuned gains allows to successfully execute the task.


#### 4.4.1. Experiment 1: Tradeoff betweenE
i
∞ and X
r
i
∞

This experiment is shown to depict the tradeoff betweenE
i
∞ and X
r
i
∞ when Θ
i
x
≠
0. The experiment consisted in commanding desired trajectories to the robots in order to reduce the distance W
h to W
o considering that the grasping points X
o
i are perfectly known, see Figure 3(b) (this was implemented by trajectory planning, using a sigmoid function (
t
a
n
h
(
·
)
) which produces W
h
→
W
o). In order to make more evident the trade off the reference force for each robot was set to F
r
i
x
=
15
[
N
], that is, high squeezing grasping forces arise.The tuning procedure of gainsΓ
i
x and α
i
x is performed as presented in Section 3.4. The gains used for the experiments are shown in Table 2.Table 2
Gains used for the experiments.
Gain (i
=
1,2)
Experiment 1
Experiment 2
Experiment 3
D
i
diag{
1,1
}
diag{
1,1
}
diag{
1,1
}
B
i
diag{
200,200
}
diag{
200,200
}
diag{
200,200
}
K
i
diag{
10000,10000
}
diag{
10000,10000
}
diag{
10000,10000
}
p
i
10000
10000
10000
ζ
i
x
0.7071
1
1
ω
n
i
x
400
400
400
Γ
i
x
0.0016
0.0016
0.0016
α
i
x
0.003623
0.003623
0.003623
Λ
i
x
0
{
0,10
}
{
0,10
}
Ψ
i
x
0
0
{
0,10
}
Θ
i
x
200
0
0Considering the gains in Table2, Table 1, and the estimated stiffness K
o
=
100000 [N/m] of the object, one gets E
i
x
∞
=
4.091 [N] and  X
r
i
x
∞
=
6.82
×
1
0
-
4 [m]. The experimental result is shown in Figure 4, where it is observed that E
i
x
∞
≈
4.15 [N] and X
r
i
x
∞
≈
7.5
×
1
0
-
4 [m], which represent an error of 9.97
% in the reference trajectory and 1.85
% in the force error, with respect to the computed theoretical values.Figure 4
Grasping forcesF
r
x and reference trajectories X
r
x for experiment 1.


#### 4.4.2. Experiment 2: Excessive Grasping Force

The experiments presented in this section were carried out to show the grasping force interaction performance with an induced grasp position uncertainty of0.005 [m] for each robot. The grasping task was performed considering the gains shown in Table 2 for the experiment 2. In this experiment, the “pinch” command is executed such that the distance between the end-effectors reduces from W
h to W
p, see Figure 3(c).The experiments were performed for the unconstrained and the constrained case, that is, withΛ
1
=
Λ
2
=
0 and with Λ
1
=
Λ
2
=
10. Therefore, only the upper grasping force constraints are considered. The maximum force was set to F
m
a
x
1
x
=
8 [N] and F
m
a
x
2
x
=
-
8 [N] and are depicted in Figures 5 and 6 as dotted lines.Figure 5
Grasping forcesF
r
x and reference trajectories X
r
x for experiment 2, unconstrained case (Λ
i
=
0).Figure 6
Grasping forcesF
r
x and reference trajectories X
r
x for experiment 2, constrained case (Λ
i
=
10).In Figure5, note that when Λ
1
=
Λ
2
=
0, excessive interaction forces arise, which excessively violate the grasping force limits. Meanwhile, when Λ
1
=
10 and Λ
2
=
10 the excessive grasping force is not present any more. To that end, the reference trajectories are computed in different fashion, they are modified at the transitory in order to achieve the reference grasping force F
r
i. This experiment demonstrates the fact that position uncertainty cannot be compensated by the proper tuning of the gains. Therefore, consideration of the excessive grasping forces is justified. Moreover, this allows the reduction of the grasping forces, as depicted in Figure 6.


#### 4.4.3. Experiment 3: Insufficient and Excessive Grasping Forces

The main consequence of insufficient grasping forces is losing contact with the object. Thas is, when insufficient grasping forces arise, the object cannot be securely grasped and the grasping task fails. To demonstrate this fact, a grasping transport task is presented. An oscillating motion along the horizontal plane was externally produced by an user and was commanded as the desired trajectory for both robots. This was achieved by setting a third robot as a master robot to send the desired positionX
o, see Figure 1 and the supplementary video. It is shown that the perturbation generated by the oscillating motion leads to both, excessive and insufficient grasping forces, see Figure 7.Figure 7
Grasping forceF
r
x and reference trajectory X
r
x for experiment 3 (Λ
i
=
Ψ
i
=
0), unconstrained case.The first part of the experiment does not consider constraints and in the second constraints are considered. The gains used for this experiment are shown in Table2 for experiment 3. The experimental results are depicted in Figures 7 and 8. The dotted lines represent the maximum and minimum grasping force constraints, the reference grasping force is represented by a dashed line, while the dash-dot and the continuous lines represent the grasping forces for the first and the second robot, respectively.Figure 8
Grasping forceF
r
x and reference trajectory X
r
x for experiment 3 (Λ
i
=
Ψ
i
=
10), constrained case.For the first experiment it can be observed that excessive grasping forces arise. Moreover, in Figure7, when no grasping force constraints are considered, the contact with the object is lost when the external force appears, around t
=
13 [s]. Therefore the grasping task is not accomplished. On the other hand, when the grasping force constraints are considered, the proposed approach compensates excessive and insufficient grasping forces, thus keeping the object securely grasped, during the whole experiment, see Figure 8.


#### 4.4.4. Experiment 4: Unsymmetrical Object

In this experiment an unsymmetrical object was grasped by the robot system, see Figure9. For this object, due to nonsymmetrical grasping, an additional torque must be compensated by the planned grasping forces. In this scenario, if excessive grasping forces arise, such torque might cause to lose contact with the object. Therefore, the online computed reference trajectory must compensate for the undesired forces of the complete robotic task. The gains were the same as for the experiment 3 and the reference grasping forces were set to 5 N.Figure 9
Robotic grasping of a unsymmetrical object.As depicted in Figure10, the grasp forces are applied by the robots and excessive forces are again handled by the grasping force regulation strategy. Thus, the grasping task is again successfully performed.Figure 10
Grasping forceF
r
x and reference trajectory X
r
x for experiment 4 (Λ
i
=
Ψ
i
=
10), constrained case.


## 4.1. Testbed

The testbed used to perform the experiments consists of two planar robot manipulators. Both robots are built of aluminum (alloy 6063 T-5) of 9.525 mm thickness with three rotational joints, each actuated by a DC brush-less servomotor of theM
i
c
r
o
m
o
© brand. A six axis ATI nano-40 and a JR3 force sensors are suitably mounted at their end effectors. To avoid undesired noisy force signals, a low pass filter with a cut-off frequency of 100 Hz is implemented via software for each sensor. All the time derivatives of the signals required for the implementation of the proposed schema were computed using approximated derivatives (first order filters) using the M
a
t
l
a
b
-
S
i
m
u
l
i
n
k
© Toolbox. The robotic system is depicted in Figure 2.Figure 2
Robotic system used for the experiments.Hereafter, subindex1 refers to the black serial robot (robot 1) and subindex 2 refers to the gray serial robot (robot 2). All Cartesian variables, unless other specification, are measured with respect to an inertial frame {
R
}, aligned horizontally and located at the origin of the first joint of the robot, see Figure 2.Due to its trajectory tracking performance and simplicity of implementation [31], a classical PID joint controller is programmed for each robot as the motion controller for the first control loop, see Figure 1. It must be highlighted that the performance of the proposed force regulation approach depends on the performance of the selected motion controller. Note that if the robot end-effector trajectory X does not match the commanded trajectory X
c, model 4 is no longer valid.To implement the proposed force regulation scheme,M
a
t
l
a
b
S
i
m
u
l
i
n
k
© installed on a personal computer is used. The computer is equipped with two S
e
n
s
o
r
a
y
© data acquisition boards model S-626 to process the control signals of both robots. The implementation of the force regulation strategy is performed via an inner-outer loop (first and second control loop) impedance control approach [7, 19], as shown in Figure 1. The stability issues of such implementation can be consulted in [19]. Note that a PID joint controller is used for each robot; thus, the inverse kinematic model of both robots is required. Nevertheless, Cartesian type controllers might be used.


## 4.2. Grasping in Robotics

One of the most important tasks for robotic devices, such as robotic hands and industrial robots, is objects grasping. To safely grasp an object, it is required that the robotic devices apply proper grasping forces on adequate points on the object surface. The importance of the grasping task is that once it is successfully achieved, it allows robots to perform more complex tasks such as objects transportation and manipulation [32]. Traditionally, to perform any grasping task, two problems should be sequentially solved: grasp synthesis and grasp control.First, given the geometry and physical properties of the object as well as those of the robotic devices, the aim of the grasp synthesis problem is to determine the contact points (or regions) on the object and the grasping forces that must be applied by the robots on the object. The set of such contact points and the corresponding grasping forces defines a grasp. For more details on the grasp synthesis problem, the reader is referred to a recent survey [33] and references therein. One of the main challenges of the grasp synthesis problem is that, traditionally, it has been stated as a constrained optimization problem [34], thus making it difficult to obtain its solution. Constraints related to grasp synthesis are unilateral, and they arise from the fact that robots or fingers can only apply pushing forces on the object surface [35]. This issue is also present in grasp control and has been ignored, nevertheless, it represents a challenging problem with constraints. Second, given the grasping contact points and the required grasping forces, grasp control aims to achieve the essential properties of grasp equilibrium and grasp stability. Other properties such as desired dynamic behavior for a given grasp are also the target of grasp control. From the control point of view, grasping forces can be controlled at the object level, at the end-effector level or at the joint level [36].In general, grasp control has been less explored than grasp synthesis. The reason is that grasp control has been regarded as an extension of classical force control approaches. Notwithstanding, there exist features of paramount importance regarding grasping control, mainly due to grasping force constraints, as in the case of the grasp synthesis stage. This is a nontrivial issue because any mismatch between the actual execution of the grasping task and the planned execution at the grasp synthesis stage might lead to undesired grasping forces. Such forces can be either insufficient [37] or excessive [38], thus losing the contact with the object or damaging it, respectively. In the literature, only few works consider constraints on the grasping forces. In the work of Takahashi et al. [39], excessive forces are considered giving priority to force control over position control. On the other hand, in [40–42] it is proposed that force constraints might be handled by a force threshold, where excessive interaction forces are complied when the threshold is violated. These approaches lack a stability analysis when grasping force constraints are considered. In the following, a grasping experiment considering excessive and insufficient forces is described.


## 4.3. Grasping Experiment

The task for the robots is to grasp a prismatic object made of pine wood ([7.5 × 7.5 × 9.5] [cm]), which is affected by gravity forces, as shown in Figure3 and in the Supplementary Video available online at http://dx.doi.org/10.1155/2015/918301. Note that a DOP is formulated and online solved for each robot; thus, subindex 1 refers to the first robot and subindex 2 refers to the second robot. In order to compute the grasping forces that should be applied on the object, we consider a typical static friction coefficient for metal on wood; that is, μ
=
0.45. The weight of the prismatic object is w
=
0.3 [Kg]. With these data and considering a safety factor of two, it is easy to compute an estimate of the the proper forces to grasp the prismatic object under the action of the gravity force [43]. Therefore, we get that the reference grasping forces must be chosen such that F
r
1
x
=
-
5 [N] for robot 1, and F
r
2
x
=
5 [N] for robot 2. Note that the sign of the reference grasping forces is set according to the sense of the action force defined by a frame attached to the object. Accordingly, the synthesized grasping points are the geometric centers of the opposite rectangular faces of the prismatic object, which are separated by a distance W
o
=
7.5 [cm], see Figure 3(b). Note that the experiments are performed in one dimension (
m
=
1
), that is, along the x-axis of the inertial frame {
R
} in Figure 2. Therefore, all gain matrices reduce to scalars.Figure 3
Pinch command.
(a)
(b)
(c)To perform the grasping task, the following is considered. First, the desired position is such that a perpendicular contact between the object and the end-effectors of the robots is ensured, see Figure3(a). This guarantees that interaction forces are aligned to the force sensor axis along the direction of the applied force, that is, x direction of frame {
R
}. Second, two commands in Cartesian coordinates are sent to the robot motion controllers. The first one is a preshaping command, which is sent to the manipulators in order to reach the first configuration, which aligns the force sensor axis and set a distance W
h between the robot end-effectors, see Figure 3(b). In that moment, the object must be placed between the robot end-effectors. The second is a motion command for each robot to reduce the distance between the end-effectors from W
h to W
p, while keeping the axis alignment. Hereafter, this command will be known as the “pinch” command. Note that distance W
p is chosen smaller than the object width W
o; therefore, the robots make contact with the object performing the grasping task, see Figure 3(c). However, in this situation high grasping forces arise due to a grasping point mismatch. This situation is common in real applications, for instance, when the shape of the object is approximated with vision tools in order to perform grasping tasks, for example [32, 44]. Moreover, traditional approaches of force regulation fail to avoid unexpected interaction forces because this position uncertainty is hardly compensated by the controller unless online adaptation laws are considered [45]. This fact has been highlighted in the tuning procedure presented in Section 3.4.


## 4.4. Experimental Results

In this section, experimental results to show the effectiveness of the proposed approach are presented. In the ideal case, the grasping positions are exactly known. Therefore, the grasping force constraints are not violated and the grasping task is successfully executed. In this work, the main contribution is about handling force interaction limits, nonetheless experimental results for the ideal case are presented in experiment 1 to demonstrate the tradeoff betweenE
i
∞ and X
r
i
∞ when Θ
i
x
≠
0. In experiment 2, it is assumed that there exists a mismatch between the actual robot position X and the synthesized position X
o of the grasping points, that is, W
p
<
W
o. In this situation, excessive interaction forces arise and the effects of the penalization gain Λ on the grasping performance are depicted. Experiment 3 presents a case where both excessive and insufficient grasping forces arise due to a transport task and then the proposed penalization scheme and appropriately tuned gains allows to successfully execute the task.


### 4.4.1. Experiment 1: Tradeoff betweenE
i
∞ and X
r
i
∞

This experiment is shown to depict the tradeoff betweenE
i
∞ and X
r
i
∞ when Θ
i
x
≠
0. The experiment consisted in commanding desired trajectories to the robots in order to reduce the distance W
h to W
o considering that the grasping points X
o
i are perfectly known, see Figure 3(b) (this was implemented by trajectory planning, using a sigmoid function (
t
a
n
h
(
·
)
) which produces W
h
→
W
o). In order to make more evident the trade off the reference force for each robot was set to F
r
i
x
=
15
[
N
], that is, high squeezing grasping forces arise.The tuning procedure of gainsΓ
i
x and α
i
x is performed as presented in Section 3.4. The gains used for the experiments are shown in Table 2.Table 2
Gains used for the experiments.
Gain (i
=
1,2)
Experiment 1
Experiment 2
Experiment 3
D
i
diag{
1,1
}
diag{
1,1
}
diag{
1,1
}
B
i
diag{
200,200
}
diag{
200,200
}
diag{
200,200
}
K
i
diag{
10000,10000
}
diag{
10000,10000
}
diag{
10000,10000
}
p
i
10000
10000
10000
ζ
i
x
0.7071
1
1
ω
n
i
x
400
400
400
Γ
i
x
0.0016
0.0016
0.0016
α
i
x
0.003623
0.003623
0.003623
Λ
i
x
0
{
0,10
}
{
0,10
}
Ψ
i
x
0
0
{
0,10
}
Θ
i
x
200
0
0Considering the gains in Table2, Table 1, and the estimated stiffness K
o
=
100000 [N/m] of the object, one gets E
i
x
∞
=
4.091 [N] and  X
r
i
x
∞
=
6.82
×
1
0
-
4 [m]. The experimental result is shown in Figure 4, where it is observed that E
i
x
∞
≈
4.15 [N] and X
r
i
x
∞
≈
7.5
×
1
0
-
4 [m], which represent an error of 9.97
% in the reference trajectory and 1.85
% in the force error, with respect to the computed theoretical values.Figure 4
Grasping forcesF
r
x and reference trajectories X
r
x for experiment 1.


### 4.4.2. Experiment 2: Excessive Grasping Force

The experiments presented in this section were carried out to show the grasping force interaction performance with an induced grasp position uncertainty of0.005 [m] for each robot. The grasping task was performed considering the gains shown in Table 2 for the experiment 2. In this experiment, the “pinch” command is executed such that the distance between the end-effectors reduces from W
h to W
p, see Figure 3(c).The experiments were performed for the unconstrained and the constrained case, that is, withΛ
1
=
Λ
2
=
0 and with Λ
1
=
Λ
2
=
10. Therefore, only the upper grasping force constraints are considered. The maximum force was set to F
m
a
x
1
x
=
8 [N] and F
m
a
x
2
x
=
-
8 [N] and are depicted in Figures 5 and 6 as dotted lines.Figure 5
Grasping forcesF
r
x and reference trajectories X
r
x for experiment 2, unconstrained case (Λ
i
=
0).Figure 6
Grasping forcesF
r
x and reference trajectories X
r
x for experiment 2, constrained case (Λ
i
=
10).In Figure5, note that when Λ
1
=
Λ
2
=
0, excessive interaction forces arise, which excessively violate the grasping force limits. Meanwhile, when Λ
1
=
10 and Λ
2
=
10 the excessive grasping force is not present any more. To that end, the reference trajectories are computed in different fashion, they are modified at the transitory in order to achieve the reference grasping force F
r
i. This experiment demonstrates the fact that position uncertainty cannot be compensated by the proper tuning of the gains. Therefore, consideration of the excessive grasping forces is justified. Moreover, this allows the reduction of the grasping forces, as depicted in Figure 6.


### 4.4.3. Experiment 3: Insufficient and Excessive Grasping Forces

The main consequence of insufficient grasping forces is losing contact with the object. Thas is, when insufficient grasping forces arise, the object cannot be securely grasped and the grasping task fails. To demonstrate this fact, a grasping transport task is presented. An oscillating motion along the horizontal plane was externally produced by an user and was commanded as the desired trajectory for both robots. This was achieved by setting a third robot as a master robot to send the desired positionX
o, see Figure 1 and the supplementary video. It is shown that the perturbation generated by the oscillating motion leads to both, excessive and insufficient grasping forces, see Figure 7.Figure 7
Grasping forceF
r
x and reference trajectory X
r
x for experiment 3 (Λ
i
=
Ψ
i
=
0), unconstrained case.The first part of the experiment does not consider constraints and in the second constraints are considered. The gains used for this experiment are shown in Table2 for experiment 3. The experimental results are depicted in Figures 7 and 8. The dotted lines represent the maximum and minimum grasping force constraints, the reference grasping force is represented by a dashed line, while the dash-dot and the continuous lines represent the grasping forces for the first and the second robot, respectively.Figure 8
Grasping forceF
r
x and reference trajectory X
r
x for experiment 3 (Λ
i
=
Ψ
i
=
10), constrained case.For the first experiment it can be observed that excessive grasping forces arise. Moreover, in Figure7, when no grasping force constraints are considered, the contact with the object is lost when the external force appears, around t
=
13 [s]. Therefore the grasping task is not accomplished. On the other hand, when the grasping force constraints are considered, the proposed approach compensates excessive and insufficient grasping forces, thus keeping the object securely grasped, during the whole experiment, see Figure 8.


### 4.4.4. Experiment 4: Unsymmetrical Object

In this experiment an unsymmetrical object was grasped by the robot system, see Figure9. For this object, due to nonsymmetrical grasping, an additional torque must be compensated by the planned grasping forces. In this scenario, if excessive grasping forces arise, such torque might cause to lose contact with the object. Therefore, the online computed reference trajectory must compensate for the undesired forces of the complete robotic task. The gains were the same as for the experiment 3 and the reference grasping forces were set to 5 N.Figure 9
Robotic grasping of a unsymmetrical object.As depicted in Figure10, the grasp forces are applied by the robots and excessive forces are again handled by the grasping force regulation strategy. Thus, the grasping task is again successfully performed.Figure 10
Grasping forceF
r
x and reference trajectory X
r
x for experiment 4 (Λ
i
=
Ψ
i
=
10), constrained case.


## 4.4.1. Experiment 1: Tradeoff betweenE
i
∞ and X
r
i
∞

This experiment is shown to depict the tradeoff betweenE
i
∞ and X
r
i
∞ when Θ
i
x
≠
0. The experiment consisted in commanding desired trajectories to the robots in order to reduce the distance W
h to W
o considering that the grasping points X
o
i are perfectly known, see Figure 3(b) (this was implemented by trajectory planning, using a sigmoid function (
t
a
n
h
(
·
)
) which produces W
h
→
W
o). In order to make more evident the trade off the reference force for each robot was set to F
r
i
x
=
15
[
N
], that is, high squeezing grasping forces arise.The tuning procedure of gainsΓ
i
x and α
i
x is performed as presented in Section 3.4. The gains used for the experiments are shown in Table 2.Table 2
Gains used for the experiments.
Gain (i
=
1,2)
Experiment 1
Experiment 2
Experiment 3
D
i
diag{
1,1
}
diag{
1,1
}
diag{
1,1
}
B
i
diag{
200,200
}
diag{
200,200
}
diag{
200,200
}
K
i
diag{
10000,10000
}
diag{
10000,10000
}
diag{
10000,10000
}
p
i
10000
10000
10000
ζ
i
x
0.7071
1
1
ω
n
i
x
400
400
400
Γ
i
x
0.0016
0.0016
0.0016
α
i
x
0.003623
0.003623
0.003623
Λ
i
x
0
{
0,10
}
{
0,10
}
Ψ
i
x
0
0
{
0,10
}
Θ
i
x
200
0
0Considering the gains in Table2, Table 1, and the estimated stiffness K
o
=
100000 [N/m] of the object, one gets E
i
x
∞
=
4.091 [N] and  X
r
i
x
∞
=
6.82
×
1
0
-
4 [m]. The experimental result is shown in Figure 4, where it is observed that E
i
x
∞
≈
4.15 [N] and X
r
i
x
∞
≈
7.5
×
1
0
-
4 [m], which represent an error of 9.97
% in the reference trajectory and 1.85
% in the force error, with respect to the computed theoretical values.Figure 4
Grasping forcesF
r
x and reference trajectories X
r
x for experiment 1.


## 4.4.2. Experiment 2: Excessive Grasping Force

The experiments presented in this section were carried out to show the grasping force interaction performance with an induced grasp position uncertainty of0.005 [m] for each robot. The grasping task was performed considering the gains shown in Table 2 for the experiment 2. In this experiment, the “pinch” command is executed such that the distance between the end-effectors reduces from W
h to W
p, see Figure 3(c).The experiments were performed for the unconstrained and the constrained case, that is, withΛ
1
=
Λ
2
=
0 and with Λ
1
=
Λ
2
=
10. Therefore, only the upper grasping force constraints are considered. The maximum force was set to F
m
a
x
1
x
=
8 [N] and F
m
a
x
2
x
=
-
8 [N] and are depicted in Figures 5 and 6 as dotted lines.Figure 5
Grasping forcesF
r
x and reference trajectories X
r
x for experiment 2, unconstrained case (Λ
i
=
0).Figure 6
Grasping forcesF
r
x and reference trajectories X
r
x for experiment 2, constrained case (Λ
i
=
10).In Figure5, note that when Λ
1
=
Λ
2
=
0, excessive interaction forces arise, which excessively violate the grasping force limits. Meanwhile, when Λ
1
=
10 and Λ
2
=
10 the excessive grasping force is not present any more. To that end, the reference trajectories are computed in different fashion, they are modified at the transitory in order to achieve the reference grasping force F
r
i. This experiment demonstrates the fact that position uncertainty cannot be compensated by the proper tuning of the gains. Therefore, consideration of the excessive grasping forces is justified. Moreover, this allows the reduction of the grasping forces, as depicted in Figure 6.


## 4.4.3. Experiment 3: Insufficient and Excessive Grasping Forces

The main consequence of insufficient grasping forces is losing contact with the object. Thas is, when insufficient grasping forces arise, the object cannot be securely grasped and the grasping task fails. To demonstrate this fact, a grasping transport task is presented. An oscillating motion along the horizontal plane was externally produced by an user and was commanded as the desired trajectory for both robots. This was achieved by setting a third robot as a master robot to send the desired positionX
o, see Figure 1 and the supplementary video. It is shown that the perturbation generated by the oscillating motion leads to both, excessive and insufficient grasping forces, see Figure 7.Figure 7
Grasping forceF
r
x and reference trajectory X
r
x for experiment 3 (Λ
i
=
Ψ
i
=
0), unconstrained case.The first part of the experiment does not consider constraints and in the second constraints are considered. The gains used for this experiment are shown in Table2 for experiment 3. The experimental results are depicted in Figures 7 and 8. The dotted lines represent the maximum and minimum grasping force constraints, the reference grasping force is represented by a dashed line, while the dash-dot and the continuous lines represent the grasping forces for the first and the second robot, respectively.Figure 8
Grasping forceF
r
x and reference trajectory X
r
x for experiment 3 (Λ
i
=
Ψ
i
=
10), constrained case.For the first experiment it can be observed that excessive grasping forces arise. Moreover, in Figure7, when no grasping force constraints are considered, the contact with the object is lost when the external force appears, around t
=
13 [s]. Therefore the grasping task is not accomplished. On the other hand, when the grasping force constraints are considered, the proposed approach compensates excessive and insufficient grasping forces, thus keeping the object securely grasped, during the whole experiment, see Figure 8.


## 4.4.4. Experiment 4: Unsymmetrical Object

In this experiment an unsymmetrical object was grasped by the robot system, see Figure9. For this object, due to nonsymmetrical grasping, an additional torque must be compensated by the planned grasping forces. In this scenario, if excessive grasping forces arise, such torque might cause to lose contact with the object. Therefore, the online computed reference trajectory must compensate for the undesired forces of the complete robotic task. The gains were the same as for the experiment 3 and the reference grasping forces were set to 5 N.Figure 9
Robotic grasping of a unsymmetrical object.As depicted in Figure10, the grasp forces are applied by the robots and excessive forces are again handled by the grasping force regulation strategy. Thus, the grasping task is again successfully performed.Figure 10
Grasping forceF
r
x and reference trajectory X
r
x for experiment 4 (Λ
i
=
Ψ
i
=
10), constrained case.


## 5. Analysis and Discussion

In experiments presented in Section4.4.1, it is shown the performance of the proposed scheme when there is no mismatch between the actual and the synthesized contact positions, which were perfectly known. In this case excessive grasping forces do not arise, but the compromise between the reference trajectory and the grasping force error was shown.In experiments presented in Section4.4.2, it is shown the performance of the proposed approach when a mismatch between the actual and the synthesized contact positions is considered. This yields excessive grasping forces which must be avoided. For this, the penalization approach is demonstrated.The effect of increasing the gainΛ on each robot is that the excessive grasping force is dramatically reduced. This is of paramount importance for some applications, for example, when object fragility is considered. The performance our approach relies on the online updating of the reference trajectory, which is the independent variable in the optimization problem. Note that, when the grasping force is achieved (i.e., E
i
∞
=
0), then the reference trajectories X
r
1 and X
r
2 achieve a steady state. In case of experiment 4, by comparing Figures 6 and 10 it can be observed that for the unsymmetrical case, the computed reference trajectories present a drift-like term. This can be due to the torque generated by the nonsymmetrical part of the object and is topic for further research.In experiments in Section4.4.3, excessive and insufficient grasping forces arise due to force variations induced by the oscillating motion of the robots. Therefore, the contact between the object and the end-effectors of the robots is broken when the effects of external force, originated by the robots motion, arise. Accordingly, notice that proper reference trajectories become constant when the contact with the object has been lost. On the other hand, when force limits are considered for both robots, then the interaction grasping forces are maintained and the grasping task is successfully executed. Moreover, it is important to note that communication between grasping robots is not required.An interesting force regulation case might be considered for a given application if the manipulated object does not return to its original dimension. It represents a permanent deformation, which indicates that the applied force is out of linear region of the object model. In principle, this must be not present in any implementation of our approach because high interaction forces are avoided by properly setting the gain matricesΛ and Ψ. Nevertheless, if this is considered as a special case, observe (16). It represents a first order linear equation for the reference trajectory X
r. On the one hand, if the gain Θ is greater than zero, then the reference trajectory will converge to zero. If there is any permanent deformation in the object the contact will be lost. On the other hand, if the gain Θ is zero, then the dynamics of the reference trajectory is driven by the weighted sum of the force error and its time derivative. As our objective is the minimization of the performance index (9); then, force regulation is achieved. In this case, if there is any permanent deformation in the object the contact is maintained because a constant reference trajectory is rendered, as presented at the experiment section of this work paper, see Figures 5 and 6.


## 6. Conclusion

Experimental results as well as the theoretical analysis demonstrate that our approach is effective for force regulation with prescribed limits. Excessive or insufficient forces can be effectively handled by our proposal and its stability is guaranteed. Force limits are included in the problem formulation, thus allowing to be handled in the stability and convergence analysis. Also note that, due to the position-based impedance implementation, neither the dynamic model of the robots nor the object dynamics are required. Experiments demonstrate that our approach is computationally feasible for real-time implementation of force regulation tasks. Moreover, the proposed force regulation scheme generalizes the classic integral force control method, which can be directly obtained from our approach by setting to zero the penalty gains.



---

*Source: 918301-2015-05-19.xml*