Update embObjMotionControl and eomcParser to manage the new Kalman Fi…

…lter params from xml
robotology · Jun 17, 2022 · 1c2eb3a · 1c2eb3a
1 parent 40286db
commit 1c2eb3a
Show file tree

Hide file tree

Showing 4 changed files with 120 additions and 1 deletion.
diff --git a/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.cpp b/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.cpp
@@ -134,6 +134,7 @@ bool embObjMotionControl::alloc(int nj)
     _axesInfo.resize(nj);
     _jointEncs.resize(nj);
     _motorEncs.resize(nj);
+    _kalman_params.resize(nj);
 
     //debug purpose
 
@@ -213,7 +214,8 @@ embObjMotionControl::embObjMotionControl() :
     _impedance_params(0),
     _axesInfo(0),
     _jointEncs(0),
-    _motorEncs(0)
+    _motorEncs(0),
+    _kalman_params(0)
 {
     _gearbox_M2J  = 0;
     _gearbox_E2J  = 0;
@@ -844,6 +846,20 @@ bool embObjMotionControl::fromConfig_Step2(yarp::os::Searchable &config)
         {
             updateDeadZoneWithDefaultValues();
         }
+
+        if(!_mcparser->parseKalmanFilterParams(config, _kalman_params))
+        {
+            // if you don't specify the Kalman Filter parameters for all the joints,
+            // then use the default values for all of them.
+            for(int i=0; i<_njoints; i++)
+            {
+                _kalman_params[i].enabled = false;
+                _kalman_params[i].x0.fill(0.0);
+                _kalman_params[i].Q.fill(0.0);
+                _kalman_params[i].R = 0.0;
+                _kalman_params[i].P0 = 0.0;
+            }
+        }
     }
 
 
@@ -1340,6 +1356,11 @@ bool embObjMotionControl::init()
 
         jconfig.tcfiltertype=_trq_pids[logico].filterType;
 
+        jconfig.kalman_params.enabled = _kalman_params[logico].enabled;
+        for(int i=0; i<_kalman_params[logico].x0.size(); i++) jconfig.kalman_params.x0[i] = _kalman_params[logico].x0.at(i);
+        for(int i=0; i<_kalman_params[logico].Q.size(); i++) jconfig.kalman_params.Q[i] = _kalman_params[logico].Q.at(i);
+        jconfig.kalman_params.R = _kalman_params[logico].R;
+        jconfig.kalman_params.P0 = _kalman_params[logico].P0;
 
         if(false == res->setcheckRemoteValue(protid, &jconfig, 10, 0.010, 0.050))
         {

diff --git a/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.h b/src/libraries/icubmod/embObjMotionControl/embObjMotionControl.h
@@ -205,6 +205,7 @@ class yarp::dev::embObjMotionControl:   public DeviceDriver,
     double *                                _gearbox_M2J;   /** the gearbox ratio motor to joint */
     double *                                _gearbox_E2J;   /** the gearbox ratio encoder to joint */
     double *                                _deadzone;
+    std::vector<eomc::kalmanFilterParams_t> _kalman_params;  /** Kalman filter parameters */
 
     eomc::twofocSpecificInfo_t *            _twofocinfo;
 

diff --git a/src/libraries/icubmod/embObjMotionControl/eomcParser.cpp b/src/libraries/icubmod/embObjMotionControl/eomcParser.cpp
@@ -2151,6 +2151,92 @@ bool Parser::parseDeadzoneValue(yarp::os::Searchable &config, double deadzone[],
     return true;
 }
 
+bool Parser::parseKalmanFilterParams(yarp::os::Searchable &config, std::vector<kalmanFilterParams_t> &kalmanFilterParams)
+{
+    Bottle general = config.findGroup("OTHER_CONTROL_PARAMETERS");
+    if (general.isNull())
+    {
+        yWarning() << "embObjMC BOARD " << _boardname << "Missing OTHER_CONTROL_PARAMETERS group. Kalman Filter will be disabled by default.";
+        return false;
+    }    
+    Bottle xtmp;
+
+    // kalmanFilterEnabled
+    if (!extractGroup(general, xtmp, "kalmanFilterEnabled", "kalman filter of joint: ", _njoints, false))
+    {
+        yWarning() << "embObjMC BOARD " << _boardname << "Missing kalmanFilterEnabled parameter. It will be disabled by default.";
+        return false;
+    }
+    for (int j = 0; j<_njoints; j++) kalmanFilterParams[j].enabled = xtmp.get(j + 1).asBool();
+
+    // x0_0
+    if (!extractGroup(general, xtmp, "x0", "Initial state x0 of kalman filter of joint: ", _njoints, false))
+    {
+        yWarning() << "embObjMC BOARD " << _boardname << "Missing initial state x0 parameter. Kalman Filter will be disabled by default.";
+        return false;
+    }
+    for (int j = 0; j<_njoints; j++) kalmanFilterParams[j].x0.at(0) = xtmp.get(j + 1).asFloat32();
+
+    // x0_1
+    if (!extractGroup(general, xtmp, "x1", "Initial state x1 of kalman filter of joint: ", _njoints, false))
+    {
+        yWarning() << "embObjMC BOARD " << _boardname << "Missing initial state x1 parameter. Kalman Filter will be disabled by default.";
+        return false;
+    }
+    for (int j = 0; j<_njoints; j++) kalmanFilterParams[j].x0.at(1) = xtmp.get(j + 1).asFloat32();
+
+    // x0_2
+    if (!extractGroup(general, xtmp, "x2", "Initial state x2 of kalman filter of joint: ", _njoints, false))
+    {
+        yWarning() << "embObjMC BOARD " << _boardname << "Missing initial state x2 parameter. Kalman Filter will be disabled by default.";
+        return false;
+    }
+    for (int j = 0; j<_njoints; j++) kalmanFilterParams[j].x0.at(2) = xtmp.get(j + 1).asFloat32();
+
+    // Q0
+    if (!extractGroup(general, xtmp, "Q0", "Process Q0 noise covariance matrix of kalman filter of joint: ", _njoints, false))
+    {
+        yWarning() << "embObjMC BOARD " << _boardname << "Missing Q0 parameter. Kalman Filter will be disabled by default.";
+        return false;
+    }
+    for (int j = 0; j<_njoints; j++) kalmanFilterParams[j].Q.at(0) = xtmp.get(j + 1).asFloat32();
+
+    // Q1
+    if (!extractGroup(general, xtmp, "Q1", "Process Q1 noise covariance matrix of kalman filter of joint: ", _njoints, false))
+    {
+        yWarning() << "embObjMC BOARD " << _boardname << "Missing Q1 parameter. Kalman Filter will be disabled by default.";
+        return false;
+    }
+    for (int j = 0; j<_njoints; j++) kalmanFilterParams[j].Q.at(1) = xtmp.get(j + 1).asFloat32();
+
+    // Q2
+    if (!extractGroup(general, xtmp, "Q2", "Process Q2 noise covariance matrix of kalman filter of joint: ", _njoints, false))
+    {
+        yWarning() << "embObjMC BOARD " << _boardname << "Missing Q2 parameter. Kalman Filter will be disabled by default.";
+        return false;
+    }
+    for (int j = 0; j<_njoints; j++) kalmanFilterParams[j].Q.at(2) = xtmp.get(j + 1).asFloat32();
+
+    // R
+    if (!extractGroup(general, xtmp, "R", "Measurement noise covariance matrix of kalman filter for joint: ", _njoints, false))
+    {
+        yWarning() << "embObjMC BOARD " << _boardname << "Missing R scalar parameter. Kalman Filter will be disabled by default.";
+        return false;
+    }
+    for (int j = 0; j<_njoints; j++) kalmanFilterParams[j].R = xtmp.get(j + 1).asFloat32();
+
+    // P0
+    if (!extractGroup(general, xtmp, "P0", "Initial state estimation error covariance matrix of kalman filter of joint: ", _njoints, false))
+    {
+        yWarning() << "embObjMC BOARD " << _boardname << "Missing P0 scalar parameter. Kalman Filter will be disabled by default.";
+        return false;
+    }
+    for (int j = 0; j<_njoints; j++) kalmanFilterParams[j].P0 = xtmp.get(j + 1).asFloat32();
+
+    return true;
+}
+
+
 bool Parser::parseMechanicalsFlags(yarp::os::Searchable &config, int useMotorSpeedFbk[])
 {
     Bottle general = config.findGroup("GENERAL");

diff --git a/src/libraries/icubmod/embObjMotionControl/eomcParser.h b/src/libraries/icubmod/embObjMotionControl/eomcParser.h
@@ -24,6 +24,7 @@
 
 #include <string>
 #include <vector>
+#include <array>
 #include <map>
 
 //  Yarp stuff
@@ -306,6 +307,15 @@ typedef struct
     impedanceLimits_t limits;
 } impedanceParameters_t;
 
+typedef struct
+{
+    bool enabled;
+    std::array<float32_t, 3> x0;
+    std::array<float32_t, 3> Q;
+    float32_t R;
+    float32_t P0;
+} kalmanFilterParams_t;
+
 //template <class T>
 
 class Parser
@@ -427,6 +437,7 @@ class Parser
     bool parseMechanicalsFlags(yarp::os::Searchable &config, int useMotorSpeedFbk[]);
     bool parseImpedanceGroup(yarp::os::Searchable &config,std::vector<impedanceParameters_t> &impedance);
     bool parseDeadzoneValue(yarp::os::Searchable &config, double deadzone[], bool *found);
+    bool parseKalmanFilterParams(yarp::os::Searchable &config, std::vector<kalmanFilterParams_t> &kalmanFilterParams);
 };
 
 }}}; //close namespaces