# On the Reliability of Inverse Optimal Control

Jessica Colombel, David Daney, François Charpillet

► To cite this version:

Jessica Colombel, David Daney, François Charpillet. On the Reliability of Inverse Optimal Control. ICRA 2022 - IEEE International Conference on Robotics and Automation, May 2022, Philadelphia, United States. pp.8504-8510. hal-03349528v2

HAL Id: hal-03349528

https://inria.hal.science/hal-03349528v2

Submitted on 5 Sep 2022

HAL is a multi-disciplinary open access
archive for the deposit and dissemination of sci-
entific research documents, whether they are pub-
lished or not. The documents may come from
teaching and research institutions in France or
abroad, or from public or private research centers.

L'archive ouverte pluridisciplinaire HAL, est
destinée au dépôt et à la diffusion de documents
scientifiques de niveau recherche, publiés ou non,
émanant des établissements d'enseignement et de
recherche français ou étrangers, des laboratoires
publics ou privés.

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

Distributed under a Creative Commons Attribution 4.0 International License
# On the Reliability of Inverse Optimal Control

Jessica Colombel<fnref n="1" />, David Daney<fnref n="2" />, François Charpillet<fnref n="1" />

**Abstract**—Inverse Optimal Control (IOC) is a popular method for human motion analysis. In the context of these methods it is necessary to pay attention to the reliability of the results. This paper proposes an approach based on the evaluation of Karush-Kuhn-Tucker conditions relying on a complete analysis with Singular Value Decomposition and provides a detailed analysis of reliability. With respect to a ground truth, our simulations illustrate how the proposed method analyzes the reliability of the resolution. After introducing a clear methodology, the properties of the matrices are studied with different noise levels and different experimental models and conditions. We show how to implement the method, step by step, by explaining the numerical difficulties encountered during the resolution and thus how to make the results of the IOC problem reliable.

Human Motion Analysis, Identification, Inverse Optimal Control.

## I. INTRODUCTION

Human motion analysis is relevant to provide a robot response adapted to human behavior. It is important to understand how this biological motion is generated and which elements impact it, whether they are external factors such as the task to be performed or internal factors such as emotions [1]. Many methods have been developed to try to understand these different factors, such as Fourier methods [2], PCA [3], as well as machine learning [4]. One interesting method focuses on the analysis of motion, hypothesizing that its generation can be considered an optimal control problem.

This method, called Inverse Optimal Control (IOC), is used to understand better the control that governs human movement [5] and also in robotics in two main areas. First, reproduction of human-like motion for robotics [6], [7], especially for humanoids. Second, for human-robot interaction as a prediction [8], [9] or as a way of understanding task division [10]. Human locomotion is one of the most studied motions [11]–[14] as well as its style [15]. A recent paper also explores affect in arm motion [16]. These publications underline that behavior change can be studied with IOC.

IOC, similar to Inverse Reinforcement Learning [17], [18], for human motion analysis is based on the assumption that human motion is a trajectory resulting from an optimization process in which a given set of cost functions is minimized (or in case of Reinforcement Learning, a given set of Reward function is maximized). The goal is then to find the weights of the associated cost functions (see II-B for examples).

Two main approaches are used to solve the IOC problem. The first one is called Bi-Level and relies on the convergence of sequential Direct Optimal Control (DOC) towards the reference trajectories [6], [13], [19]. One issue with this approach is

the computational complexity and computing time [20]. The second approach, named Approximate Inverse Optimal Control (AIOC), is based on the necessity of respecting the optimality conditions provided by Karush-Kuhn-Tucker (KKT) [21] which are interpreted as a residual minimization process [20], [22]. This method has decreased computational cost compared to the Bi-level method which enables to process, online, a trajectory through a sliding window. Some studies have explored these methods for incomplete trajectories [23] or trajectories with multiple phases [24]. However, the resulting cost function can be far away from reality if a rigorous mathematical analysis of the problem is not performed. This paper revisits the AIOC problem by analyzing its KKT optimality matrices using Singular Value Decomposition.

The main contributions of the paper are: (i) details of the properties of the methods that provide indices to the feasibility and identifiability of the parameters; (ii) analysis of the reliability of the approach with regard to measurement noise. The results are presented on the simulation of a 2-bar robotic arm. This allows the robustness results to be highlighted with knowledge of the original costs.

First, this paper introduces the IOC method and the proposed resolution, then it details the analysis and interpretation of the problem. The experiments in simulation are provided before presenting the results and the discussion.

## II. INVERSE OPTIMAL CONTROL METHOD

### A. Direct Optimal Control formulation

A strong hypothesis considers that human motion trajectories (noted s) are generated by optimizing a convex criterion $C(s)$, under $n_f$ constraints $f_i(s)$ ($i = 1, \dots, n_f$) that allows to model it as a Direct Optimal Control problem as follows:

$$s = \arg \min_C(s) \text{ s.t. } f_{1, \dots, n_f}(s) = 0 \quad (1)$$

where $s \in \mathbb{R}^{n_s \times n_t}$ is a trajectory composed, by $n_t$ of frames; each are defined by a state $\mathbf{x}_t$ of dimension $n_s$, such that $s = [\mathbf{x}_1, \dots, \mathbf{x}_t, \dots, \mathbf{x}_{n_t}]^T$. In this paper, the state is given by $\mathbf{x}_t = [x_t, \dot{x}_t, \ddot{x}_t]^T$. Note that in the context of human motion analysis, it is not necessary to introduce a control variable. To clarify our statements, we will not use this formalism which can be found in [23], for instance. $f$ is the set of $n_f$ equality constraints. Inequality constraints can also be considered, as in [25], but not presented in this paper for the sake of simplicity. Equality constraints $f$ are defined by $g$ that connect the variables and their derivative to figure the kinematics of the system, in addition to constraints on given values of the initial ($\mathbf{x}_{start}$) and final ($\mathbf{x}_{goal}$) states.

$$f: g(\mathbf{x}_1, \dots, \mathbf{x}_{n_t}) = 0; \mathbf{x}_1 - \mathbf{x}_{start} = 0; \mathbf{x}_{n_t} - \mathbf{x}_{goal} = 0 \quad (2)$$

<footnote n="1">¹Université de Lorraine, CNRS, Inria, LORIA, F-54000 Nancy, France</footnote>

<footnote n="2">²Inria Bordeaux Sud Ouest - IMS (UMR 5218), F-33405 Talence, France</footnote>
Let C be the cost functions to be minimized, their physical meaning will be given in the next section:

$$C(\mathbf{s}) = \sum_{k=1}^{n_c} \omega_k \sum_{t=1}^{n_t} C_k(\mathbf{x}_t) \quad (3)$$

with $\omega_k$ the $k^{th}$ component of the weight normalized vector $\omega$ associated to the cost function $C_k$. Note that in the context of our problem, we consider that the weights do not change with time. To guarantee the convexity of this problem, it can be chosen that $\omega_k \ge 0$ and $C_k$ is quadratic for all $k \in [1, n_c]$.

### B. Cost functions for human motion

One of the issue to identify human motion is to model the motor control that the human will optimize to generate its motion. In the case of IOC, it means to select several cost functions which should be based on this motor behaviour. In the literature, there are two main approaches to define these cost functions. The first one is to consider the kinematics of motion, for example, the jerk to report its smoothness [26]. Then the cost functions are built on kinematic criteria such as: the distance to the goal, the velocity, the acceleration and, obviously the jerk. The second one is to explain the motion through torque-change in joints, then dynamics cost functions should be defined using energy, torque, angular power, e.g. [27].

IOC's literature studies human motion with all cost functions that can explain the motion either they are kinematics or dynamics and depending on the motion, different cost functions are highlighted. Moreover, descriptors for motion can qualify separately the task achived but also the style of the motion and it is possible to implement descriptors as cost functions to describe the motion better [28]. Recent paper shows these other types of cost functions as Laban's effort (time, space, etc.) or quantity of motions to explore the affect part of a motion [16].

### C. Inverse Optimal Control through KKT formulation

The following formulation considers the ideal case in which the measurements are perfects (noised-free). The goal of inverse optimal control is to find the objective function weights $\omega_k$ from a given trajectory $s^*$ supposed to be optimal, knowing the cost functions $C_k$ for all $k = 1, ..., n_c$ and equality constraints $f_i$ for all $i = 1, ..., n_f$. The approach presented here is based on Karush-Kuhn-Tucker optimality conditions. Let $s^*$ be an optimal trajectory associated with $\omega$. Let's define the Lagrangian $L$ associated to the direct optimisation problem of equation 1:

$$L(s^*, \lambda) = \sum_{k=1}^{n_c} \omega_k C_k(s^*) + \sum_{i=1}^{n_f} \lambda_i f_i(s^*) \quad (4)$$

with $\lambda \in \mathbb{R}^{n_f}$ is the vector of $n_f$ Lagrange multiplier $\lambda_i$ of equality constraints functions $f$.

Let the observed state vector $s^*$ be considered as the optimal trajectory associated with $\lambda$. Therefore, to minimize the Lagrangian $L$, its derivative $\frac{\partial L}{\partial s}(s^*)$, evaluated at $s^*$, must be equal to vector zero.

$$\frac{\partial L}{\partial s}(s^*, \lambda) = \sum_{k=1}^{n_c} \omega_k \frac{\partial C_k}{\partial s}(s^*) + \sum_{i=1}^{n_f} \lambda_i \frac{\partial f_i}{\partial s}(s^*) = 0 \quad (5)$$

Moreover, as $s^*$ is computed according to the constraints described in Equation 1, we get:

$$f_i(s^*) = 0, i = 1, \dots, n_f \quad (6)$$

These two last equations 5 (stationary condition) and 6 (primal feasibility condition) define here the KKT conditions. Solving the inverse optimal control problem corresponds to identify the unknown vector weight ($\omega$ as well as the associated Lagrange multiplier $\lambda$) subject to these conditions.

### D. Resolution using KKT conditions

Equation 5 can be rewritten as $\mathbf{J} \cdot \mathbf{z} = 0$ with:

$$\underbrace{[\mathbf{J}\omega, \mathbf{J}\lambda]}_{\mathbf{J}} \begin{pmatrix} \omega \\ \lambda \end{pmatrix} = 0 \quad (7)$$

and

$$\mathbf{J}_{\omega} = \left[ \frac{\partial C_1}{\partial s}(s^*) \dots \frac{\partial C_{n_c}}{\partial s}(s^*) \right], \\ \mathbf{J}_{\lambda} = \left[ \frac{\partial f_1}{\partial s}(s^*) \dots \frac{\partial f_{n_f}}{\partial s}(s^*) \right].$$

Classically, we remark that:

* If $\mathbf{J}$ is not singular, equation 7 only admits as a trivial solution $\mathbf{z} = 0$. This means that trajectory $s^*$ does not correspond to any trajectory minimizing criterion $C(s^*)$ (equation 3) and this whatever the value of $\omega$.

* If $\mathbf{J}$ is singular, this means that $\mathbf{z}$ is a vector of the null-space of $\mathbf{J}$.

In the last case, $\mathbf{z}$ can easily be obtained through the Singular Value Decomposition of $\mathbf{J}$ so that $\mathbf{USV}^T = \mathbf{J}$ with $\sigma_1, \dots, \sigma_L$ the singular values in descending order, given by the diagonal of $\mathbf{S}$, $V_1, \dots, V_L$ the right singular vector associated with $\sigma_1, \dots, \sigma_L$ so that orthogonal matrix $\mathbf{V}^T = [V_1^T, \dots, V_L^T]^T$ and $\mathbf{U}$ also an orthogonal matrix. Then, if rank deficiency of $\mathbf{J}$ is equal to 1 ($\sigma_L = 0$), the solution for $\mathbf{z}$ is $V_L$.

$$\text{If } \mathbf{z} = V_L \text{ then} \\ \underbrace{\mathbf{USV}^T}_{\mathbf{J}} \mathbf{z} = \mathbf{US} \begin{pmatrix} V_1^T & V_L \\ \vdots & \vdots \\ V_L^T & V_L \end{pmatrix} = \mathbf{U} \begin{pmatrix} 0 \\ \vdots \\ 0 \\ \sigma_L \end{pmatrix} \\ \text{and } \|\mathbf{J}\mathbf{z}\| = \sigma_L \quad (8)$$

Note that a re-normalization of $\mathbf{z} = V_L$ is necessary to provide values to $\omega$ so that $\omega = (V_{L1}, \dots, V_{Ln_c})^T / \|(V_{L1}, \dots, V_{Ln_c})^T\|$.

If more than one singular values is equal to zero, each linear combinations of singular vector associated to null singular values can be a solution of $\mathbf{z}$ for equation 7. However, the physical interpretation of this case is problematic and will be discussed in section II-F.

**Problem:** Ideally, the resolution of Equation 7 solves the IOC problem. However, in real applications, trajectories and models suffer from inaccuracies making the IOC problem more complex to solve, e.g $\sigma_L$ should not be equal to zero but near to zero.

This is why, according to equation 8 we suggest using the lower singular value as an indicator of the quality of the
approximation of z by $V_L$. Our challenge is to analyze the reliability of the IOC solutions. Given that the singular values are not zero, to what extent can we trust the recovered vector? This paper shows that the singular values are indices of the reliability of the solution to this problem.

### E. Difference with AIOC approach

Our previous proposal insists on the solving of equation 5 and reformulated in equation 7. However, the previous section shows that uncertainties prevent us from obtaining a clear solution and this will be discussed in the next section.

Classically, this problem is considered by the community by transforming the solving problem, given in equation 5 into a constrained minimization problem by considering residuals $r_s(\omega, \lambda, s) = J(s)[\omega, \lambda]^T$. The stationary condition is then called stationary residuals $r_s$ and the aim of the method is to find the minimum of it square norm in $(\omega, \lambda)$ as :

$$ \begin{array}{ll} \underset{\omega, \lambda}{\text{minimize}} & \|r_s(\omega, \lambda, s)\|^2 \\ \text{s.t.} & \omega_k \ge 0, k = 1, \dots, n_c. \end{array} $$

The additional constraints $(\omega_k \ge 0)$ ensure the convexity of the criterion used in the COD (see end of the section II-A). There are several methods in order to solve this problem. However, this formulation of the problem must admit drawbacks. It always provides a solution whatever the nature on the real optimality of the considered trajectory ($s = s^*$) is regarding the $\omega$ solution. If the resolution is perform by optimization algorithms, it will inform if the result obtained is due to a successful convergence but it will not give any information on the validity of the solution. The other methods of resolution (e.g. SVD, Least Square) do not even give any information on the convergence of the solution. Moreover, the inequality constraints on the positivity of $\omega$ project the values without any control of the approximation made.

Thus, it becomes difficult to rely on the solution found. The following section presents an analysis of the reliability of the IOC problem based on Singular Values study.

### F. Rank analysis and reliability

As mentioned in section II-D, the ideal case is rank-deficient of 1 for $J$, meaning that an optimal trajectory exists in the set defined by constraints $f$ regarding criterion $C(s)$.

In the opposite case, several cases can be explained :

* **Identifiability of $\omega$ or $\lambda$** The identifiability of the parameters is studied on the sub-matrices. Equation 5 can be written as $J_\omega \omega = -J_\lambda \lambda$. Thus it is not possible to identify $\omega$ (resp. $\lambda$) if the rank of $J_\omega$ (resp. $J_\lambda$) is not equal to dim $\omega$ (resp. dim $\lambda$). This case is classic in parameter's identification and needs to be checked. It can be done by a QR decomposition of $J_\omega$ (or $J_\lambda$) to detect and compute the dependencies between parameters $\omega$ (or $\lambda$), see Appendix A of [29].

In the case of rank deficiency of $J_\omega$, this phenomenon can have several origins. First, the cost functions $C_k$ are linearly dependent generating a redundant information on the cost of the trajectories. This dependency can be formal, due to a bad definition or selection of basis $C$

or punctually numerical for a particular trajectory $s$ (e.g. constant trajectory with acceleration equal to zero). Another case can happen if a basis is not dependent to trajectory parameters. The weak identifiability is a complex case which appears when one (or several) of the bases of $C$ is not excited enough by the evaluated trajectory. In these conditions the value of the smallest singular value of $J_\omega$ is low but not null: in this case, a selection criterion, often associated by a normalization of the matrix $J_\omega$, is needed to properly exclude the base(s): some strategies can be implemented as [30] for a calibration problem.

The rank deficiency of $J_\lambda$ is linked to a bad parametrization of the manifold described by $f$, i.e. the number of constraints is not minimal to represent the set of trajectories. This means that the dimension of $\lambda$ is too large and the number of constraints must be reduced to reach the size $rank(J_\lambda) = n_f$.

In both cases ($J_\omega$ and $J_\lambda$), the full identifiability of $\omega$ and $\lambda$ have to be verified before processing the IOC resolution, this to prevent (in part) the rank deficiency of $J$ greater than 1.

* **$J$ is near singular** Because of the uncertainties related to the observation of the trajectory in real conditions, it is difficult to determine if matrix $J$ is singular or not. However, we suggest a decision criterion which is one of the contributions of this article. The quantification of singular values is problematic because numerical zero is dependent on the floating-point relative accuracy and the size of the matrices. Moreover, we need to integrate the tolerance allowed between the observed trajectory and the trajectory reconstructed from the result of IOC $(\omega_k)$. However, this approach is incompatible with the online calculation requirements.

In this paper, the evaluation of the drop in between the singular values of matrix $J$ is used instead of pure rank as a good criteria to interpret the problem and the results. A simple algorithm to detect the drop is used. First the set of singular values are calculated successively $(\sigma_{k-1}/\sigma_k$ for $k=1..L)$. Then the greater gap gives the "drop": if it is the $k$th, the drop is of $L-k$. Additionally, the normalization of the matrices allows to amplify the difference between the singular values clarifying the criterion on the rank. Under these conditions, if a drop in the singular values is detected, we consider the matrix as singular.

* ◦ If this drop is equal to 1, the solution of $z$ is the lower right singular vector $V_L$. Weights $\omega_k$ are then deduced if and only if their values are positive in order to certify that criterion $C(s)$ is convex; otherwise we prefer not to provide a solution.

* ◦ If this drop is greater than 1 and the identifiability of $\omega$ and $\lambda$ have been verified ($J_\omega$ and $J_\lambda$ are full rank), every linear combination of the singular vectors associated to the singular values considered as null is a solution for $z$. Thus the IOC problem would have multiple solutions with a dimension equal to the rank drop. We believe that this case is due to the approximation of the rank under specific conditions and that only the lower null singular value is significant. An example will be given in section IV but the question remains open. However, this example also shows
the danger of using the stationary residue minimization approach.

* **Full rank of J (but not near singular)** In this case, equation 5 does not admit any solution (except the trivial solution $z = 0$). The tested trajectory is not optimal regarding $C(s)$ whatever the choice of $\omega$ may be. In this case, we prefer not to provide a solution to the IOC problem because its reliability is questionable.

### III. EXPERIMENT SIMULATIONS

#### A. Simulation

The first example proposed is a two-link robot arm. It is defined by $\mathbf{s}_t = [\theta_{1,t}, \theta_{2,t}, \dot{\theta}_{1,t}, \dot{\theta}_{2,t}, \dot{\theta}_{1,t}, \dot{\theta}_{2,t}]^T$ the state of the two-link robot arm with $\mathbf{s} = [\mathbf{s}_1, \dots, \mathbf{s}_{n_t}]^T$ the trajectory of the system to go from a start point ($\mathbf{s}_{start}$) to an objective point ($\mathbf{s}_{goal}$). The dynamics of the two-link robot arm is expressed as followed:

$$ \tau_t = \mathbf{M}(\theta_t) \ddot{\theta}_t + \mathbf{R}(\theta_t, \dot{\theta}_t) \dot{\theta}_t + \mathbf{g}(\theta_t) \quad (9) $$

with $\tau_t = [\tau_1, \tau_2]_t^T$ the torques applied to each joint, $\mathbf{M}$ the positive-definite inertia matrix, $\mathbf{R}$ the Coriolis matrix and $\mathbf{g}$ the gravity vector. All the equations detailing this model will be in appendix I.

The cost functions of the 10 trajectories of simulation are presented with their cost weight in Table I with $C_k = \{\tau_1^2, \tau_2^2, \ddot{\theta}_1^2, \ddot{\theta}_2^2, \ddot{\theta}_1^2, \ddot{\theta}_2^2, (\dot{\theta}_1 \tau_1)^2, (\dot{\theta}_2 \tau_2)^2\}$. The equality constraints $f$ are defined by:

$$ \mathbf{f}(\mathbf{s}) = \begin{cases} \mathbf{s}_1 - \mathbf{s}_{start} \\ \mathbf{s}_{n_t} - \mathbf{s}_{goal} \\ \forall i \in [1, n_t - 1] \\ \begin{bmatrix} \dot{\theta}_{i+1} \\ \dot{\theta}_{i+1} \end{bmatrix} - \begin{bmatrix} \dot{\theta}_i \\ \dot{\theta}_i \end{bmatrix} - \Delta t \begin{bmatrix} \dot{\theta}_i \\ \ddot{\theta}_i \end{bmatrix} \end{cases} = \begin{pmatrix} 0 \\ \vdots \\ 0 \end{pmatrix} \quad (2n_s+n_t(n_s-2)) \times 1 $$

with the same values for each trajectories $\mathbf{s}_{start} = [0, 0, 0, 0, 0, 0]$ and $\mathbf{s}_{goal} = [\pi/2, \pi/2, 0, 0, 0, 0]$.

Remark: The simulations are done with Matlab R2019a. The DOC is done with `fmincon`. Matrix **J** is chosen to be normalised with a norm 1 which corresponds to the maximum absolute column sum of the matrix. It was found more robust than other types of norms for the examples proposed (2, infinit and frobenius, as in [24]). The normalisation of $\omega$ is done with norm 2.

The simulation steps are as follows:

1) Generate a reference trajectory with DOC and $\omega = \omega_{ref}$ for each trajectory of the Table I as presented in section II-A;

2) Modification of the basis (III-B) or addition of measurement noise (III-C);

3) Resolution of IOC as presented in section II-D to obtain $\omega_{sol}$;

4) Generate new trajectories with DOC from the found vector weight $\omega_{sol}$;

5) Analysis of the results (III-D), by comparing previous trajectories with reference ones.

#### B. Identification of basis

A preliminary question is to define the cost functions that could have been used to generate the human movement, its state variables and the associated constraints. For human movement it is all the more difficult as we do not really know what cost functions are associated with each type of movement. This section tests different types of trials to simulate a number of problems or errors that can affect the IOC implementation. They correspond to the case described in section II-F:

* Cost function symbolically correlated among all the cost functions proposed (e.g. $(2\tau_1)^2$ correlated to $\tau_1^2$);

* One cost function missing among those used to generate the motion (e.g. $\tau_1^2$);

* Cost functions added with no symbolical links with the initial ones, in the results are called "useless" functions (e.g. $\dot{\theta}_1^2$).

#### C. Noise level

This paper suggests exploring the effects of noise uncertainties on the reliability of the results. Uncertainties are designed as white Gaussian noise with mean $m = 0$ and different values of variance var. Two types of noises will be tested. The first test it what is called "constant noise": the same noise level will be applied to all observed states (i.e. angle, velocity and acceleration). This test will allow us to investigate the violation of the problem of equality constraints. The second test, called "real noise" is only added once on angles. Other state variables are obtained by derivation without any additional noise. This is designed to simulate the kind of data that can be retrieved in real life using simple sensors. In addition, it allows the constraints to be maintained but the cost functions are more impacted. For both tests, the ten trajectories are being assessed for 8 noise level with var from $10^{-7}$ to 1 with 10 repetitions of each with randomized noise.

#### D. Evaluation criteria

To compute the error between $\omega_{ref}$ used to generate the reference trajectory by a DOC and the one identified by IOC $\omega_{sol}$ presented in Section II-D the dot product is assessed. It gives the similarity to $\omega_{ref}$. It is important to notice that all the $\omega$s are normalized and that all the $\omega$s have the same sign (necessary condition for convexity, $\omega \ge 0$ to within one sign). In addition to the comparison of $\omega$, the Root Mean Square Error (RMSE) between the initial trajectory and the trajectory generated by the $\omega_{sol}$ is assessed to evaluate the precision of the algorithm. The trajectory means here the state vector composed of angular position, velocity and acceleration. Finally, the rank drop by singular values is assessed.

### IV. RESULTS

#### A. Identification of basis

The different tests on the identification of basis are computed on all the trajectories and the resulting RMSE are assessed. The results are presented in Table II. The first line of this table shows the results in ideal case. The average and standard deviation of RMSE is $3.3E^{-5} \pm 5.0E^{-5}$. With the adding of 6 "useless" cost functions, the RMSE is almost equal to the one
in ideal case with a mean of $3.6E^{-5}$ and standard deviation of $3.8E^{-5}$. For both ideal case and useless cost functions, the mean over 10 trajectories of the rank drop of $J_\omega$ is of $1.1 \pm 0.32$. It is not of mean 1 because trajectory 5 has with our algorithm a rank of 2 even though $J_\omega$ and $J_\lambda$ are full rank. However the other clue of reliability is $\omega \ge 0$ and in this case $\omega$ does not meet this criterion.

As presented in part II-F, the number of correlated functions is linked with the drop in rank of $J$ but especially of $J_\omega$. The rank drop of $J_\omega$ is 1 for all trajectories, while the mean of $J$ is $1.8 \pm 0.42$. Once the QR algorithm is used and the collinear functions pinpointed, the fall in rank returns to a fall of 1 and the proposed approach allows to determine a right $\omega$. Without the QR algorithm, the average RMSE is of $1.4E^2 \pm 2.2$.

While one cost function is missing, the RMSE increases to $2.8E^{51} \pm 8.9E^{51}$ with complete rank for $J$ (rank drop of $0.1 \pm 0.32$) and $J_\omega$. These huge RMSE are obtained by different signs in $\omega$ resulting in absurd recovered trajectories.

It should be noted that the rank of $J_\lambda$ is always full with the same minimum ratio (0.26). Indeed, this submatrix is constant for the examples which have been provided.

## B. Noise level

a) **Constant Noise:** All trajectories indicate an inflection point at which the noise is so high that the evolution of RMSE is erratic and it exceeds $10^4$. Before this point, the trajectories show a correlation between increased noise and increased RMSE on the reconstructed trajectory. For trajectories 6 and 8 the inflection point is at a noise of $10^{-3}$. Trajectory 1 has its inflection point at a noise of $10^{-1}$. All the other trajectories have an inflection point at a noise of $10^{-2}$. After these points, the rank is systematically considered as full and $\omega$ is irretrievable. For half of the trajectories this inflection point also corresponds to a change in the fall in rank. The trajectories still have a rank drop of 1 at this point, then it becomes full rank beyond. The other half is divided into two categories. The first, which includes trajectories 1 and 3, goes into full rank at the

inflection point, but they exceed the RMSE error of $10^{-1}$. The second category includes trajectories 5, 6 and 7 which also pass through the full rank before their inflection point but this does not correspond to high RMSE errors ($\le 10^{-1}$). Figure 1 shows the example of three of the trajectories (1, 6, 9) to highlight these three types of behaviors and to explore the results in more detail. It is possible to see the evolution of the $\omega_{sol}$ error in relation to the noise level. For trajectory 1, the similarity of $\omega_{sol}$ to $\omega_{ref}$ is of 1 until the noise level $10^{-1}$ where it starts to decrease with 0.995. Trajectory 6 shows a decrease in similarity from 1 to $0.782$ at $10^{-3}$ which corresponds to a RMSE of $6.9E^{-2}$. Finally trajectory 8 has an error in $\omega_{sol}$ that decreases on the rank deficiency at the error level of $10^{-2}$ (0.774385) before it increases again after on the two last noise levels (0.89794, 0.95311). These errors correspond to a RMSE error on trajectory greater than $10^{12}$.

b) **Real Noise:** The results obtained with real noise are clear. Whatever the noise level is, the RMSE quickly exceeds $10^10$. The most robust trajectories to this type of measurement noise are trajectories 1, 2, 9 and 10, the RMSE of which is of the order of $10^{-2}$ at a noise level of $10^{-6}$ before exceeding $10^4$ at a noise level of $10^{-5}$; as well as trajectory 4, which even obtains a RMSE of $4.9E^{-3}$ for a level noise of $10^{-6}$.

## V. DISCUSSION

### A. Identification of Basis

The results show that the ideal case does not generate the same exact trajectory with a RMSE of zero. This can be explained by the computing error and the precision of the arithmetic use. Therefore, all results should refer to this result for the ideal case. Then it is important to notice that the addition of cost functions hardly affects the results, provided that the added functions are not correlated. In contrast, if a function is missing, the trajectories are not recoverable.

Therefore, it is recommended to put as many cost functions as possible, paying attention to symbolic correlations. Then it is essential to look at the ranks of the two sub-matrices to

<table><caption>TABLE I: Table of cost functions weight vector $\omega$ for each trajectory generated in simulation. The cost functions are normalized.</caption><thead><tr><th rowspan="2"></th><th colspan="2">Torque</th><th colspan="2">Jerk</th><th colspan="2">Acceleration</th><th colspan="2">Power</th></tr><tr><th>τ<sub>1</sub><sup>2</sup></th><th>τ<sub>2</sub><sup>2</sup></th><th>θ̂<sub>1</sub><sup>2</sup></th><th>θ̂<sub>2</sub><sup>2</sup></th><th>θ̂<sub>1</sub><sup>2</sup></th><th>θ̂<sub>2</sub><sup>2</sup></th><th>(θ̂<sub>1</sub>τ<sub>1</sub>)<sup>2</sup></th><th>(θ̂<sub>2</sub>τ<sub>2</sub>)<sup>2</sup></th></tr></thead><tbody><tr><td>Traj 1</td><td>0.981</td><td>0.196</td><td>0</td><td>0</td><td>0</td><td>0</td><td>0</td><td>0</td></tr><tr><td>Traj 2</td><td>0.196</td><td>0.981</td><td>0</td><td>0</td><td>0</td><td>0</td><td>0</td><td>0</td></tr><tr><td>Traj 3</td><td>0.196</td><td>0.981</td><td>0.002</td><td>0.010</td><td>0</td><td>0</td><td>0</td><td>0</td></tr><tr><td>Traj 4</td><td>0.117</td><td>0.078</td><td>0</td><td>0</td><td>0.971</td><td>0.194</td><td>0</td><td>0</td></tr><tr><td>Traj 5</td><td>0.0002</td><td>0.001</td><td>0.002</td><td>0.010</td><td>0.981</td><td>0.196</td><td>0</td><td>0</td></tr><tr><td>Traj 6</td><td>0.004</td><td>0.004</td><td>0.007</td><td>0.007</td><td>0.707</td><td>0.707</td><td>0</td><td>0</td></tr><tr><td>Traj 7</td><td>0.019</td><td>0.097</td><td>0.002</td><td>0.010</td><td>0.971</td><td>0.194</td><td>0.019</td><td>0.097</td></tr><tr><td>Traj 8</td><td>0.096</td><td>0.172</td><td>0.010</td><td>0.002</td><td>0.957</td><td>0.191</td><td>0.019</td><td>0.096</td></tr><tr><td>Traj 9</td><td>0.019</td><td>0.097</td><td>0</td><td>0</td><td>0.971</td><td>0.194</td><td>0.097</td><td>0.019</td></tr><tr><td>Traj 10</td><td>0.019</td><td>0.097</td><td>0</td><td>0</td><td>0.971</td><td>0.194</td><td>0.019</td><td>0.097</td></tr></tbody></table>

<table><caption>TABLE II: Table of results for tests on basis functions. The results are expressed as mean ± standard deviation over the ten trajectories tested.</caption><thead><tr><th></th><th>RMSE</th><th>ω error</th><th>J Rank drop</th><th>Jω Rank drop</th></tr></thead><tbody><tr><td>Ideal Case</td><td>3.3E-5 ± 5.0E-5</td><td>1 ± 2.3E-9</td><td>1.1 ± 0.32</td><td>0 ± 0</td></tr><tr><td>Useless Functions</td><td>3.6E-5 ± 3.8E-5</td><td>1 ± 8.6E-6</td><td>1.1 ± 0.32</td><td>0 ± 0</td></tr><tr><td>Correlated function</td><td>1.4E2 ± 2.2</td><td>0.15 ± 0.27</td><td>1.8 ± 0.42</td><td>1 ± 0</td></tr><tr><td>Function missing</td><td>2.8E51 ± 8.9E51</td><td>0.78 ± 0.33</td><td>0.1 ± 0.32</td><td>0 ± 0</td></tr></tbody></table>
<figure><img src="image_2.png" /><figcaption>Fig. 1: This figure shows results on 3 trajectories for constant noise. Figure A illustrates the RMSE median and Figure B the median $\omega$ error, in relation to the noise level. The shadows on both graphs represent percentiles 25 and 75. The vertical coloured lines running through both graphs show the median of changes in rank (before the bar the rank deficiency is of 1, from the bar, the rank is complete). No percentiles are illustrate for the change in rank because they are equal to the median.</figcaption></figure>

check that there is no identifiability problem. If the sub-matrix or sub-matrices are not of full rank, it is then possible to do a QR decomposition which will allow to find the superfluous column(s) of the sub-matrices.

Finally, even if the rank drop is of one, the last check is on $\omega$. If it does not meet the positivity condition, the result is automatically considered false. This may mean that one or more cost functions explaining the movement is missing.

## B. Noise Level

As the results show, the constant noise has much less impact than the real noise. This can be explained by the fact that successive first-order derivatives increase the noise on accelerations. This has a direct impact on the noise level on torques and jerks, for instance. It is therefore very important to ensure measurement quality on the higher derivative variables. The types of sensors used in the experiments may influence the IOC results when analyzing human movement. In addition, constant noise violates the KKT equalities constraints (Equation 6) but still allows information to be recovered up to a certain noise level. It can be deduced that it is more important to guarantee the quality of the cost functions variables than to maintain the constraints. Then, it is possible to use filters and polynomial functions to get as close as possible to the initial trajectories rather than trying to maintain these constraints.

It is interesting to notice that the error on $\omega$ does not indicate precisely the error made on the reconstructed trajectory. This is

because the different cost functions do not impact the generation of the trajectory in the same way. Thus an error on $\omega_k$, does not impact the trajectory the same way if it is on $\omega_{k+1}$.

# VI. CONCLUSION

In this paper, a study of an Inverse Optimal Control approach is proposed and discussed. It is shown that the singular values and the drop in rank can be good features to understand the reliability of the results obtained. The paper presents two different types of problems with the interpretation of the results: the identifiability of the basis and the reliability under uncertainties. It is important to make sure that the model is well defined for cost functions and constraints. This information is studied independently in each sub-matrices ($J_\omega$ and $J_\lambda$) of the KKT stationary condition. It is recommended to use as many cost functions as possible, as long as the correlations between them are taken into account. Beware, depending on the size of the model and the number of variables, the addition of cost functions may be detrimental to the calculation time. The tests carried out on the robustness to noise indicate that the more noisy the cost functions are the more difficult it is to find the desired results. It is therefore very important to try to guarantee the reliability of the state variables used by the cost functions. On the other hand, it is possible to violate the equality constraints of the model and still to recover a reliable result. This reliability can be seen in the rank drop of the main matrix $J$. The closer the last singular value is to the penultimate, the less reliable the results are. Thus, when the rank is full, there is a good chance for the trajectory to be too noisy or too badly modelled to recover the unknowns sought.

# APPENDIX I

## DETAILED MODEL FOR SIMULATION

$$ 
\begin{aligned}
\mathbf{M}(\theta) &= \begin{bmatrix} a_1 + 2a_2\cos(\theta_2) & a_3 + a_2\cos(\theta_2) \\ a_3 + a_2\cos(\theta_2) & a_3 \end{bmatrix} \\
\mathbf{R}(\theta, \dot{\theta}) &= \begin{bmatrix} -a_2\dot{\theta}_2\sin(\theta_2) & -a_2(\dot{\theta}_1 + \dot{\theta}_2)\sin(\theta_2) \\ a_2\dot{\theta}_1\sin(\theta_2) & 0 \end{bmatrix} \\
\mathbf{g}(\theta) &= \begin{bmatrix} b_1b_3\cos(\theta_1) + b_2b_3\cos(\theta_1 + \theta_2) \\ b_2b_3\cos(\theta_1 + \theta_2) \end{bmatrix}
\end{aligned}
$$

$$ a_1 = m_1 r_1^2 + m_2 (l_1^2 + r_2^2) + I_1 + I_2 $$

$$ a_2 = m_2 l_1 r_2 $$

$$ a_3 = m_2 r_2^2 + I_2 $$

$$ b_1 = l_1 m_2 + r_1 m_1 $$

$$ b_2 = r_2 m_2 $$

$$ b_3 = g = 9.81 $$

and for each link: $m_1 = 1$, $m_2 = 1.5kg$ are the mass, $l_1 = 1$, $l_2 = 1.2m$ are the length, $r_1 = 0.5$, $r_2 = 0.6m$ are the distance from the joint center to the center of mass and $I_1 = 0.5$, $I_2 = 0.7kgm^2$
REFERENCES

[1] G. Venture, H. Kadone, T. Zhang, J. Grèzes, A. Berthoz, and H. Hicheur, "Recognizing Emotions Conveyed by Human Gait," *Int J of Soc Robotics*, vol. 6, no. 4, pp. 621-632, Nov. 2014.

[2] C. Westhoff and N. F. Troje, "Kinematic cues for person identification from biological motion," *Perception & Psychophysics*, vol. 69, no. 2, pp. 241-253, Feb. 2007.

[3] N. F. Troje, "Decomposing biological motion: A framework for analysis and synthesis of human gait patterns," *Journal of Vision*, vol. 2, no. 5, pp. 2-2, Sep. 2002.

[4] T. Randhavane, U. Bhattacharya, K. Kapsaskis, K. Gray, A. Bera, and D. Manocha, "Identifying Emotions from Walking using Affective and Deep Features," *arXiv:1906.11884 [cs]*, Jun. 2019, arXiv: 1906.11884.

[5] B. Berret, E. Chiovetto, F. Nori, and T. Pozzo, "Evidence for Composite Cost Functions in Arm Movement Planning: An Inverse Optimal Control Approach," *PLOS Computational Biology*, vol. 7, no. 10, p. e1002183, Oct. 2011.

[6] K. Mombaur, A. Truong, and J.-P. Laumond, "From human to humanoid locomotion—an inverse optimal control approach," *Auton Robot*, vol. 28, no. 3, pp. 369-383, Apr. 2010.

[7] T. Park and S. Levine, "Inverse Optimal Control for Humanoid Locomotion," p. 5.

[8] J. Mainprice, R. Hayne, and D. Berenson, "Predicting human reaching motion in collaborative tasks using Inverse Optimal Control and iterative re-planning," in *2015 IEEE International Conference on Robotics and Automation (ICRA)*, May 2015, pp. 885-892.

[9] S. Gaurav and B. Ziebart, "Discriminatively Learning Inverse Optimal Control Models for Predicting Human Intentions," in *Proceedings of the 18th International Conference on Autonomous Agents and MultiAgent Systems*, ser. AAMAS '19. Montreal QC, Canada: International Foundation for Autonomous Agents and Multiagent Systems, May 2019, pp. 1368-1376.

[10] J. F.-S. Lin, V. Bonnet, A. M. Panchea, N. Ramdani, G. Venture, and D. Kulić, "Human motion segmentation using cost weights recovered from inverse optimal control," in *2016 IEEE-RAS 16th International Conference on Humanoid Robots (Humanoids)*, Nov. 2016, pp. 1107-1113.

[11] D. Clever, K. Hatz, and K. Mombaur, "Studying Dynamical Principles of Human Locomotion using Inverse Optimal Control," *PAMM*, vol. 14, no. 1, pp. 801-802, 2014.

[12] D. Clever, R. Malin Schemschat, M. L. Felis, and K. Mombaur, "Inverse optimal control based identification of optimality criteria in whole-body human walking on level ground," in *2016 6th IEEE International Conference on Biomedical Robotics and Biomechatronics (BioRob)*, Jun. 2016, pp. 1192-1199.

[13] S. Albrecht, P. Basili, S. Glasauer, M. Leibold, and M. Ulbrich, "Modeling and Analysis of Human Navigation with Crossing Interferer Using Inverse Optimal Control," *IFAC Proceedings Volumes*, vol. 45, no. 2, pp. 475-480, Jan. 2012.

[14] A.-S. Puydupin-Jamin, M. Johnson, and T. Bretl, "A convex approach to inverse optimal control and its application to modeling human locomotion," in *2012 IEEE International Conference on Robotics and Automation*, May 2012, pp. 531-536.

[15] S. J. Lee and Z. Popović, "Learning behavior styles with inverse reinforcement learning," *ACM Trans. Graph.*, vol. 29, no. 4, pp. 122:1-122:7, Jul. 2010.

[16] P. Carreno-Medrano, T. Harada, J. F.-S. Lin, D. Kulić, and G. Venture, "Analysis of Affective Human Motion During Functional Task Performance: An Inverse Optimal Control Approach," in *2019 IEEE-RAS 19th International Conference on Humanoid Robots (Humanoids)*, Oct. 2019, pp. 461-468.

[17] S. Albrecht, M. Ulbrich, and M. Leibold, "A bilevel optimization approach to obtain optimal cost functions for human arm movements," *NACO*, vol. 2, no. 1, pp. 105-127, Mar. 2012.

[18] N. Ab Azar, A. Shahmansoorian, and M. Davoudi, "From inverse optimal control to inverse reinforcement learning: A historical review," *Annual Reviews in Control*, vol. 50, pp. 119-138, Jan. 2020.

[19] P. Abbeel and A. Y. Ng, "Apprenticeship learning via inverse reinforcement learning," in *Proceedings of the twenty-first international conference on Machine learning*, ser. ICML '04. Banff, Alberta, Canada: Association for Computing Machinery, Jul. 2004, p. 1.

[20] M. Johnson, N. Aghasadeghi, and T. Bretl, "Inverse optimal control for deterministic continuous-time nonlinear systems," in *52nd IEEE Conference on Decision and Control*, Dec. 2013, pp. 2906-2913.

[21] P. Englert, N. A. Vien, and M. Toussaint, "Inverse KKT: Learning cost functions of manipulation tasks from demonstrations," *The International Journal of Robotics Research*, vol. 36, no. 13-14, pp. 1474-1488, Dec. 2017.

[22] A. Keshavarz, Y. Wang, and S. Boyd, "Imputing a convex objective function," in *2011 IEEE International Symposium on Intelligent Control*, Sep. 2011, pp. 613-619.

[23] W. Jin, D. Kulić, S. Mou, and S. Hirche, "Inverse Optimal Control with Incomplete Observations," *arXiv:1803.07696 [cs]*, May 2019.

[24] W. Jin, D. Kulić, J. F.-S. Lin, S. Mou, and S. Hirche, "Inverse Optimal Control for Multiphase Cost Functions," *IEEE Transactions on Robotics*, vol. 35, no. 6, pp. 1387-1398, Dec. 2019.

[25] A. Panchea, *Inverse optimal control for redundant systems of biological motion*. Orléans, Dec. 2015.

[26] N. Hogan, "An organizing principle for a class of voluntary movements," *J Neurosci*, vol. 4, no. 11, pp. 2745-2754, Nov. 1984.

[27] Y. Uno, M. Kawato, and R. Suzuki, "Formation and control of optimal trajectory in human multijoint arm movement," *Biol. Cybern.*, vol. 61, no. 2, pp. 89-101, Jun. 1989.

[28] C. Larboulette and S. Gibet, "A Review of Computable Expressive Descriptors of Human Motion," in *Proceedings of the 2nd International Workshop on Movement and Computing*, ser. MOCO '15. New York, NY, USA: ACM, 2015, pp. 21-28.

[29] S. Besnard and W. Khalil, "Identifiable parameters for parallel robots kinematic calibration," in *Proceedings 2001 ICRA. IEEE International Conference on Robotics and Automation (Cat. No.01CH37164)*, vol. 3, 2001, pp. 2859-2866 vol.3.

[30] T. Gayral and D. Daney, "A sufficient condition for parameter identifiability in robotic calibration," in *Computational Kinematics*. Springer, 2014, pp. 131-138.