# Parametric trajectories and measurement error in inverse optimal control

Ahmed-Manaf Dahmani, David Daney, François Charpillet

► To cite this version:

Ahmed-Manaf Dahmani, David Daney, François Charpillet. Parametric trajectories and measurement error in inverse optimal control. MMT Symposium 2024 - Mechanism and Machine Theory Symposium, Jun 2024, Guimaraes, Portugal. hal-04718505

HAL Id: hal-04718505

https://hal.science/hal-04718505v1

Submitted on 10 Oct 2024

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
# PARAMETRIC TRAJECTORIES AND MEASUREMENT ERROR IN INVERSE OPTIMAL CONTROL

Ahmed-Manaf Dahmani<fnref n="1" /> <fnref n="2" />, David Daney<fnref n="1" />, François Charpillet<fnref n="2" />

## 1 INTRODUCTION

Inverse optimal control (IOC) is a growing field of research that has gained significant traction in human motion prediction and learning. However, current methods for solving IOC have several problems ranging from computation time to handling noisy data.

This work builds upon the foundation laid by Colombel et al. [1] in their attempt to explain the IOC through the lens of Karush-Kuhn-Tucker (KKT) optimality conditions and the concept of singularity curves. This abstract tackles the issue of finding metrics to relate parametric representations of trajectories to discrete measurement error.

## 2 GENERAL TOPIC

The use of discrete trajectories is common in solving IOC problems. However, the high dimensionality of the data has a big impact on the computation time. An alternative is to use parametric representations where a trajectory $s(\alpha, t)$ is a function of a number of parameters $\alpha = (\alpha_0, \dots, \alpha_m)$. An example of a parametric representations are polynomials $s(\alpha, t) = \sum_{k=0}^d \alpha_k t^k$. Collocation allows for further reduction of the number of free parameters by including linear constraints like initial and end conditions in coefficient calculations.

A trajectory $s(\alpha^*)$ is the solution to a Direct Optimal Control (DOC) problem for a cost function $C(s)$ and equality and inequality constraints $g(s)$ and $f(s)$ if it verifies the optimization problem 1.

$$ \alpha^* = \underset{\alpha}{\operatorname{argmin}} C(s(\alpha)) \quad \text{s.t. } f(s(\alpha)) = 0, \quad g(s(\alpha)) \ge 0 \qquad (1) $$

The IOC is the inverse problem. From a trajectory $s(\alpha^*)$ assumed to be optimal, a basis of cost functions $B = (C_1(s), \dots, C_n(s))$, it aims to find a weight vector $\omega = (\omega_1, \omega_2, \dots, \omega_n)$ such that the trajectory $s(\alpha^*)$ is optimal for the cost function $C(s) = \sum_{k=1}^n \omega_k C_k(s)$.

The stationarity condition for the IOC problem in KKT can be written as in equation 2. This equation is at the origin of the concept of a singularity curve which is the set of trajectories $s(\alpha)$ such that $J(s(\alpha))$ is singular, in that case, an SVD decomposition is sufficient to find $\omega$.

$$ \exists \lambda \in \mathbb{R}^{n_f}, \exists \mu \in \mathbb{R}^{n_g}, \underbrace{\left( \frac{\partial C}{\partial \alpha}^T(s(\alpha^*)) \quad \frac{\partial f}{\partial \alpha}^T(s(\alpha^*)) \quad \frac{\partial g}{\partial \alpha}^T(s(\alpha^*)) \right)}_{J(s(\alpha^*))} \begin{pmatrix} \omega \\ \lambda \\ \mu \end{pmatrix} = 0 \qquad (2) $$

A common solving algorithm for the IOC [1,2] is the orthogonal projection on the singularity curve defined in equation 3 where $\alpha_M$ is the parametric approximation of the noisy observation and $\alpha^*$ is the optimal orthogonal projection.

$$ \alpha^* = \underset{\alpha}{\operatorname{argmin}} \|s(\alpha_M) - s(\alpha)\|_2^2 \quad \text{s.t. } \det(J(s(\alpha))^T J(s(\alpha))) = 0 \qquad (3) $$

## 3 CONTRIBUTION

Performing an orthogonal projection assumes that the distance used is directly related to the measurement error. However, this is no longer true in a parametric representation of the trajectory. This can be solved by a simple rescaling as this section shows.

Let $\alpha = (\alpha_1 \ \alpha_2 \ \dots \ \alpha_m)^T$ be a parametric trajectory. The equation linking $\alpha$ to its corresponding discrete trajectory is of the form $s = A\alpha + v$ with A a tall matrix. This means that $\|\Delta s\|_2^2 = \Delta s^T \Delta s = \Delta \alpha^T A^T A \Delta \alpha$. Then by a truncated SVD where $A = USV^T$ such that S is square, the equation becomes $\|\Delta s\|_2^2 = \Delta \alpha^T VSSV^T \Delta \alpha$. By

<footnote n="1">¹ Inria, Talence, France, {ahmed-manaf.dahmani@inria.fr; david.daney@inria.fr}</footnote>

<footnote n="2">² Inria, Villers-lès-Nancy, France, {francois.charpillet@inria.fr}</footnote>
rotating the parametric space with the orthogonal matrix $V^T$, and rescaling it with the diagonal matrix $S$, a new set of coordinates is made such that $\alpha' = SV^T\alpha$. The previous equation then becomes $\|\Delta s\|_2^2 = \Delta\alpha'^T\Delta\alpha' = \|\Delta\alpha'\|_2^2$. Thus the distance in parametric space after rescaling is equal to the induced measurement error and orthogonal projections can be safely done.

## 4 RESULTS

In order to illustrate the importance of rescaling the coordinates let us consider the example of a two-joint rigid arm. A trajectory has been selected at random from the data used by Berret et al in [3]. The IOC problem constraints are the initial and end positions as shown in Figure 1 with zero velocity and acceleration at both ends. A polynomial approximation of degree 7 was used for both joints meaning that $\theta_1(t) = \sum_{n=0}^7 \alpha_n t^n$ and $\theta_2(t) = \sum_{n=0}^7 \beta_n t^n$. Collo- cation is used in order to include start and end constraints in the polynomial coefficient calculations, further reducing the number of free parameters to only $(\alpha_6, \alpha_7, \beta_6, \beta_7)$.

<figure><img src="image_2.png" /><figcaption>Figure 1: IOC Problem definition.</figcaption></figure>

The Basis of cost functions $B = (C_1, C_2)$ used is Angle acceleration $C_1 = \sum_{t=0}^{T} (\ddot{\theta}_1(t)^2 + \ddot{\theta}_2(t)^2)$ and Torque $C_2 = \sum_{t=0}^{T} (\tau_1(t)^2 + \tau_2(t)^2)$. The polynomial trajectory is then projected orthogonally on the singularity curve generated by this basis in the parametric space $(\alpha_6, \alpha_7, \beta_6, \beta_7)$ with and without rescaling. The comparison between the resulting orthogonal projections and the original trajectory can be seen in Figure 2 showing that orthogonal projection with rescaling resulted in a closer trajectory to the original than without rescaling. Objectively, the orthogonal projection without rescaling resulted in an RMSE of 1.47743 compared to the polynomial approximation of the original trajectory, while the trajectory with rescaling resulted in an RMSE of 0.16341.

<figure><img src="image_3.png" /><figcaption>Figure 2: Orthogonal projection with and without homogenization.</figcaption></figure>

## REFERENCES

[1] J. Colombel, D. Daney, and F. Charpillet, “Holistic view of Inverse Optimal Control by introducing projections on singularity curves,” in *2023 IEEE International Conference on Robotics and Automation (ICRA)*, May 2023, pp. 12240–12246. [Online]. Available: https://ieeexplore.ieee.org/abstract/document/10161163

[2] F. Bečanović, V. Bonnet, R. Dumas, K. Jovanović, and S. Mohammed, “Force Sharing Problem During Gait Using Inverse Optimal Control,” *IEEE Robotics and Automation Letters*, vol. 8, no. 2, pp. 872–879, Feb. 2023, conference Name: IEEE Robotics and Automation Letters. [Online]. Available: https://ieeexplore.ieee.org/document/9930338

[3] 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, publisher: Public Library of Science. [Online]. Available: https://journals.plos.org/ploscompbiol/article?id=10.1371/journal.pcbi.1002183