[calibration] Test a high frequency controller

[jmirabel]

The high frequency controller intends to reduce the effect of bad dynamical robot model.

It takes as input desired torque, supposed to be constant for a time T, and a target velocity and position trajectory (in the form of a list of values for control time).

Then at each iteration, the control law is:
$$\tau = \tau_{des} + M(q) * K * (x_{des}[i] - x)$$
where $K$ is a diagonal matrix, $x$ is the current state, $x_{des}$ is a list of desired state at each time step.

In Python, this is roughly:

    M = pinocchio.crba(rmodel, rdata, q)
    dv = (vdes[i] - v)
    dq = (qdes[i] - q)
    da = Kv * dv + Kq * dq
    return u + M @ da

[jmirabel]

@florent-lamiraux after looking a bit closer at the implementation of the linear feedback controller, I see two ways of testing this.

Mass matrix approximation

We approximate $M(q) \approx M(q_{des}[i])$.
So instead of $\tau = \tau_{des} + M(q) K (x_{des}[i] - x)$, we use
$$\tau = \tau_{des} + \bar{K}[i] (x_{des}[i] - x)$$
where
$$\bar{K}[i] = M(q_{des}[i]) K$$.

This solution has the advantage of requiring no C++ code development.

Modify LFC source code

Update the source code of linear-feedback-controller package to add the possibility to use the above formula. The controller would have a parameter to enable the use of the mass matrix.

• When disabled, the control law is $\tau = \tau_{des} + K (x_{des}[i] - x)$.
• When enabled, the control law is $\tau = \tau_{des} + M(q) K (x_{des}[i] - x)$.

EDIT: I was too optimistic and misread the code. The code of the LFC does not use a reference trajectory but only a reference state so some C++ development is required.

[florent-lamiraux]

With the help of @Kotochleb I was able to install and run the Agimus Coding Week docker image.
In this image, there is a working demo of the franka robot in Gazebo performing a sinusoidal motion of the end-effector using the linear_feedback_controller. This is a good beginning, however, I will need help to get into the code of the controller.

Moreover, displaying the topic /control, only the field position contains non zero values.