Physically Consistent Energy Regressor-based Adaptive Laws in Robotic Systems

Lev Kozlov
l.kozlov@innopolis.university
Supervisor: Simeon Nedelchev
s.nedelchev@innopolis.university

Jun 21, 2024

Motivation

Adaptive control laws capable of uncertain model, external load, noisy measurements

Open Problem - Research Question

The primary research question addressed is:

How can we improve parameter identification using acceleration-free energy regressors with physical constraints?

Dynamics modeling

Equation of motion is given by:

\[ M(\mathbf{q}, \Theta) \mathbf{\dot{v}} + h(\mathbf{q}, \mathbf{v}, \Theta) = S(\mathbf{q}) \mathbf{u}\label{eq:manipulator-equation} \] Where \(\Theta\) is a stacked vector of dynamical parameters:

A possible way to obtain through \(L = K - U\): \[ \frac{d}{dt} \frac{\partial L}{\partial \dot{\mathbf{q}}} - \frac{\partial L}{\partial \mathbf{q}} = Q \]

Dynamical parameters

\[ \begin{aligned} &\scriptsize \mathbf{\theta} = \begin{bmatrix} m & mx_c & my_c & mz_c & I_{xx} & I_{xy} & I_{yy} & I_{xz} & I_{yz} & I_{zz} \\ \end{bmatrix}^T \\ & \\ &\scriptsize K = \phi_K(\mathbf{q}, \mathbf{v}) \theta \hspace{1cm} U = \phi_U(\mathbf{q}) \theta \end{aligned} \]

Schematic representation of a rigid body

Parametrized dynamics models

  • Inverse dynamics model: \[\scriptsize \mathbf{Y}_d(\mathbf{q}, \mathbf{v}, \mathbf{\dot{v}}) \Theta = S(\mathbf{q}) \mathbf{u}\]
  • Momentum model: \[\scriptsize \mathbf{Y}_{P}(\mathbf{q}_2, \mathbf{v}_2)^T \Theta - \mathbf{Y}_{P}(\mathbf{q}_1, \mathbf{v}_1)^T \Theta = \int_{t_1}^{t_2} \big ( \frac{\partial L}{\partial q} \Theta + S(\mathbf{q}) \mathbf{u} \, \big) dt\]
  • Energy model: \[\scriptsize \mathbf{Y}_E(\mathbf{q}_2, \mathbf{v}_2)^T \Theta - \mathbf{Y}_E(\mathbf{q}_1, \mathbf{v}_1)^T \Theta = \int_{t_1}^{t_2} \mathbf{v}^T S(\mathbf{q}) \mathbf{u} \, dt\]

Parametrized dynamics models - pendulum

  • Inverse dynamics model \(\mathbf{Y}_d\): \[\scriptsize \begin{bmatrix}\ddot{q} & g\sin{q}\end{bmatrix} \theta = \tau\]
  • Momentum model \(\mathbf{Y}_p\): \[\scriptsize \begin{bmatrix}\dot{q}_2 - \dot{q}_1 & \int_{t_1}^{t_2} g\sin{(q)}dt\end{bmatrix} \theta = \int_{t_1}^{t_2} \tau dt\]
  • Energy model \(\mathbf{Y}_e\): \[\scriptsize \begin{bmatrix}\frac{1}{2}\dot{q}^2_2 - \frac{1}{2}\dot{q}^2_1 & g(\cos{(q_1)} - \cos{(q_2)})\end{bmatrix} \theta = \int_{t_1}^{t_2} \dot{\theta} \tau dt\]

Regressors shape: \(A = Y^T Y\)

Dynamical parameters representations

Pseudo

\[\begin{equation} \scriptsize \begin{aligned} m &= \int_{\mathbb{R}^3} \rho(\mathbf{x}) \, \mathrm{d}V \\ \mathbf{h} &= \int_{\mathbb{R}^3} \mathbf{x} \rho(\mathbf{x}) \, \mathrm{d}V \\ \mathbf{I} &= \iiint_{\mathbb{R}^3} \begin{bmatrix} y^2 + z^2 & -xy & -xz \\ -xy & x^2 + z^2 & -yz \\ -xz & -yz & x^2 + y^2 \end{bmatrix} \rho(\mathbf{x}) \, \mathrm{d}V \end{aligned} \end{equation}\]

\[\begin{equation}\scriptsize \Sigma_c = \frac{1}{2} \mathrm{Tr}(\mathbf{I}) \mathbf{I}_3 - \mathbf{I} \end{equation}\]

\[\begin{equation}\scriptsize \bar{\mathbf{I}}^{\ast} = \begin{bmatrix} \Sigma_c + m \mathbf{r}_c \mathbf{r}_c^T & \mathbf{h} \\ \mathbf{h}^T & m \\ \end{bmatrix}\succ 0\label{eq:wensing-pseudo-inertia} \end{equation}\]

Log-Cholesky

\[ \scriptsize \boldsymbol{\eta}=\left[\alpha, d_1, d_2, d_3, s_{12}, s_{23}, s_{13}, t_1, t_2, t_3\right]^{\top} \in \mathbb{R}^{10} \]

\[ \scriptsize \mathbf{U} = e^{\alpha} \left[\begin{array}{cccc} e^{d_1} & s_{12} & s_{13} & t_1 \\ 0 & e^{d_2} & s_{23} & t_2 \\ 0 & 0 & e^{d_3} & t_3 \\ 0 & 0 & 0 & 1 \end{array}\right] \]

\[ \scriptsize \mathbf{\bar{I}}^{\ast} = \mathbf{U} \mathbf{U}^T \]

Problem Formulation

Optimization formulation in context of system identification problem:

\[ \begin{aligned} \min_{\theta, \eta} \quad & \sum_{j=k}^{N} \left\| \mathbf{Y}_i \theta - \mathbf{y}_j \right\|_2 + d(\theta_0, \theta) \\ \text{where} \quad & \mathbf{Y}_i \in \{ \mathbf{Y}_d(t_k), \mathbf{Y}_e(t_k, H), \mathbf{Y}_p(t_k, H) \} \\ \quad & \theta \in \left\{ \mathbb{R}^{10}, \bar{\boldsymbol{I}}^{\ast}(\theta) \succ 0, \theta(\boldsymbol{\eta}) \right\} \\ \quad & d(\theta_0, \theta) \text{ is a regularization term} \end{aligned} \]

Implementation

Tools used:

  • CVXPY - convex optimization toolbox
  • CasADi - symbolic framework for optimization

Tools developed:

  • DARLi - modeling toolbox for both numerical and symbolic computations
  • Paramid - framework to construct identification problem with interface to CVXPY and CasADi.

Case study 1 - Quadrotor identification

Skydio X2 quadrotor

Tracking trajectory

LQR LTV trajectory

Case study 1 - Results

Theta
Regressor Euclidean Cholesky
Inv. Dyn. 0.1840 -
Momentum 0.0210 -
Energy 0.0350 0.8760

Pseudo inertia
Regressor Euclidean Cholesky
Inv. Dyn. 0.1820 8.5770
Momentum 0.0220 3.9810
Energy 0.0350 0.8760

Log-Cholesky
Regressor Euclidean Cholesky
Inv. Dyn. 0.1800 1.3470
Momentum 0.0070 0.0150
Energy 0.0240 0.0170

Euclidean distance
\(\scriptsize d_1 = ||\theta_1 - \theta_0||_2\)

Log-Cholesky distance
\(\scriptsize d_2 = ||\eta(\theta_1) - \eta(\theta_0)||_2\)

Case study 2 - Panda external load identification

Franka Emika Panda manipulator

System identification results
Distance Metric Inv.Dyn. Momentum Energy
Euclidean 2.020 0.444 0.049
Cholesky 0.980 0.931 0.400


Parameter Ground Truth Inv.Dyn. Momentum Energy
\(m\) 2.7355 0.7285 2.2917 2.6903
\(m x_c\) 0.2077 0.0064 0.2158 0.2116
\(m y_c\) 0.0969 -0.0035 0.0958 0.1080
\(m z_c\) 0.0593 0.0456 0.0483 0.0687
\(I_{xx}\) 0.0204 0.0147 0.0231 0.0273
\(I_{xy}\) -0.0104 -0.0007 -0.0131 -0.0080
\(I_{yy}\) 0.0330 0.0130 0.0330 0.0405
\(I_{xz}\) -0.0031 -0.0010 -0.0082 -0.0059
\(I_{yz}\) -0.0012 -0.0008 -0.0069 -0.0008
\(I_{zz}\) 0.0299 0.0049 0.0360 0.0281

Parameters adaptation

Update estimation of parameters on a horizon:

\[ \begin{aligned} \min_{\theta, \eta} \quad & \sum_{j=k}^{N} \left\| \mathbf{Y}_i \theta - \mathbf{y}_j \right\|_2 + d(\theta_0, \theta) \\ \text{where} \quad & \mathbf{Y}_i \in \{ \mathbf{Y}_d(t_k), \mathbf{Y}_e(t_k, H), \mathbf{Y}_p(t_k, H) \} \\ \quad & \theta \in \left\{ \bar{\boldsymbol{I}}^{\ast}(\theta) \succ 0, \theta(\boldsymbol{\eta}) \right\} \\ \quad & d(\theta_0, \theta) \text{ is a regularization term} \end{aligned} \]

Adaptive system

Case study 2 - Controller tuning

  • Inverse dynamics controller: \(u = \mathbf{Y}_d(\mathbf{q}, \mathbf{v}, \mathbf{\dot{v}}^{\ast}) \hat{\theta}\)
  • On each iteration update \(\hat{\theta}\) using Energy Regressor with LogCholesky parametrization

Parameters estimation error

Controller performance

Analysis of computational complexity

  • Solving LMI is intractable problem
  • Energy regressor is fast to compute due smaller dimension

Speed benchmark

Conclusion

  • ParamId - framework for system identification and adaptation problems
  • Log-Cholesky is a simple parametrization yet ensures fully physically consistency
  • Acceleration-free regressors are more robust to noise

Further work:

  • Adaptation techniques relying on sampling
  • Adaptation based on the combination of regressors depending on the excitation

Demo

Preview

Appendix - \(\eta\) parametrization

\[\scriptsize \boldsymbol{\eta}=\left[\alpha, d_1, d_2, d_3, s_{12}, s_{23}, s_{13}, t_1, t_2, t_3\right]^{\top} \in \mathbb{R}^{10} \]

\[ \scriptsize \boldsymbol{\theta}(\boldsymbol{\eta}) = \begin{bmatrix} e^{2\alpha} \\ t_1 e^{2\alpha} \\ t_2 e^{2\alpha} \\ t_3 e^{2\alpha} \\ s_{23}^2 e^{2\alpha} + t_2^2 e^{2\alpha} + t_3^2 e^{2\alpha} + e^{2\alpha + 2d_2} + e^{2\alpha + 2d_3} \\ -s_{12} e^{2\alpha + d_2} - s_{13} s_{23} e^{2\alpha} - t_1 t_2 e^{2\alpha} \\ s_{12}^2 e^{2\alpha} + s_{13}^2 e^{2\alpha} + t_1^2 e^{2\alpha} + t_3^2 e^{2\alpha} + e^{2\alpha + 2d_1} + e^{2\alpha + 2d_3} \\ -s_{13} e^{2\alpha + d_3} - t_1 t_3 e^{2\alpha} \\ -s_{23} e^{2\alpha + d_3} - t_2 t_3 e^{2\alpha} \\ s_{12}^2 e^{2\alpha} + s_{13}^2 e^{2\alpha} + s_{23}^2 e^{2\alpha} + t_1^2 e^{2\alpha} + t_2^2 e^{2\alpha} + e^{2\alpha + 2d_1} + e^{2\alpha + 2d_2} \end{bmatrix} \]

Appendix - Ellipsoid representation

\[ \bar{\mathbf{I}}^{\ast} = \begin{bmatrix} \Sigma_c + m \mathbf{r}_c \mathbf{r}_c^T & \mathbf{h} \\ \mathbf{h}^T & m \\ \end{bmatrix} \]

eigvals, R = np.linalg.eig(SigmaC)
radii = np.sqrt(5 * eigvals / m)
p_b = h / m

# Ellipsoid shape
# R is the rotation and scale
# p_b is the shift