Add the integral term in the angular momentum task in tsid

[GiulioRomualdi]

Following @gabrielenava's suggestions in Stability Analysis and Design of Momentum-based Controllers for Humanoid Robots I noticed that adding an integral term in the angular momentum tasks allows improving the convergence performances of angular momentum to the desired value.

The integral will be computed numerically on the error by the update function however it is important that update is called only once by TSID otherwise the outcome of the integrator will be wrong.

What do you think @S-Dafarra @isorrentino @traversaro?

[S-Dafarra]

What did you have in mind?

[traversaro]

If the fix is not difficult, probably it is easier to discuss with the code?

[GiulioRomualdi]

There are two possible options:

1. Add a method computeIntegral in the AngularMomentumTask. In this case, the user is in charge to call it. I'm not a big fan of this
2. compute the integral in the update of the task. The update must be called only by the TSID.
   In this case the code will become (modulo hardcoded values)

diff --git a/src/TSID/src/AngularMomentumTask.cpp b/src/TSID/src/AngularMomentumTask.cpp
index dd459bc6..4ed96c21 100644
--- a/src/TSID/src/AngularMomentumTask.cpp
+++ b/src/TSID/src/AngularMomentumTask.cpp
@@ -70,6 +70,8 @@ bool AngularMomentumTask::setVariablesHandler(const System::VariablesHandler& va
         iDynTree::toEigen(this->subA(contactWrench.variable)).rightCols<3>().setIdentity();
     }
 
+    angularMomentumIntegral.setZero();
+
     return true;
 }
 
@@ -162,10 +164,17 @@ bool AngularMomentumTask::update()
 
     m_isValid = false;
 
+    double ki = 100;
+    double dt = 0.001;
     // update the control law
+    angularMomentumIntegral
+        += dt * iDyn::toEigen(m_kinDyn->getCentroidalTotalMomentum().getAngularVec3());
+
     m_R3Controller.setState(iDyn::toEigen(m_kinDyn->getCentroidalTotalMomentum().getAngularVec3()));
     m_R3Controller.computeControlLaw();
-    m_b = m_R3Controller.getControl().coeffs();
+    m_b = m_R3Controller.getControl().coeffs() - ki * angularMomentumIntegral;
     const Eigen::Vector3d comPosition = iDyn::toEigen(m_kinDyn->getCenterOfMassPosition());

[GiulioRomualdi]

Furthermore, regarding the sampling period. Should we pass it through the parametersHandler?

[traversaro]

I am probably missing/not remembering something. Where are we running the PID of the star trick? Can't we have something similar for this?

[DanielePucci]

Concerning the addition of the integral term, I endorse its use in the on-line control loop. I just want to highlight that if we use pure integrations of the angular momentum instead of state-dependent approximations - see Remark 2 of Stability Analysis and Design of Momentum-based Controllers for Humanoid Robots - we may encounter the following issues:

• Non reproducibility of the experiments: the integral term tends to add a degree of non-stationarity into the system especially when the robot is interacting with a human being/make-break contacts. So, it may be possible to obtain different experimental outcomes for similar experiments
• Unsteady robot behaviours at the equilibrium: the pure time-integration based integral term makes the control action to compensate for constant errors. So, in practice, what often happens is that the robot converges to a (steady) configuration with a constant tracking error, which is integrated in time. As soon as the integral action overcomes the stiction, then the robot moves and converges to another position with another constant error. Thus, the process repeats over and over with the overall result that the robot keeps moving at the equilibrium. In this case, bounding the integral action may help, but we thus reduce its effectiveness

[GiulioRomualdi]

Hi @DanielePucci, in the paper you linked the angular term called angular momentum integral is not equal to the integral of the angular momentum right? I was wandering on how to extend that work in a TSID framework instead of applying it in a pure momentum based controller. But I think we can discuss this as soon as I come back

[DanielePucci]

Sure Giulio, we can discuss F2F whenever you like, anyways I did not want to block the introduction of the angular momentum integral, but just raise some points that may occur, so feel free to proceed with the classical time-based integrations!

Other forms of non-linear integral I used back in time (see simulation results) can be found in Time sub-optimal nonlinear PI and PID controllers applied to longitudinal headway car control