├── .gitignore
├── LICENSE
├── README.md
├── lib
    ├── README.md
    ├── disturbance_kalman_filter.h
    ├── disturbance_kalman_filter_exp.h
    ├── disturbance_kalman_filter_rnea.h
    ├── disturbance_observer.h
    ├── disturbance_observer_rnea.h
    ├── external_observer.h
    ├── filtered_dyn_observer.h
    ├── filtered_dyn_observer_rnea.h
    ├── filtered_range_observer.h
    ├── iir_filter.h
    ├── kalman_filter.h
    ├── kalman_filter_continous.cpp
    ├── kalman_filter_continous.h
    ├── momentum_observer.h
    ├── momentum_observer_rnea.h
    ├── robot_dynamics_rnea.cpp
    ├── sliding_mode_observer.h
    └── sliding_mode_observer_rnea.h
├── matlab
    ├── Makefile
    ├── README.md
    ├── bin
    │   └── empty.txt
    ├── input.csv
    ├── observers.cpp
    ├── observers.h
    ├── test.m
    └── test_lib.cpp
└── tests
    ├── Makefile
    ├── README.md
    ├── double_link.h
    ├── double_link_rnea.h
    ├── force.gnuplot
    ├── kalman.gnuplot
    ├── observers.cpp
    ├── observers.h
    ├── test.cpp
    ├── test_kalman.cpp
    └── test_rnea.cpp


/.gitignore:
--------------------------------------------------------------------------------
 1 | # Prerequisites
 2 | *.d
 3 | 
 4 | # Compiled Object files
 5 | *.slo
 6 | *.lo
 7 | *.o
 8 | *.obj
 9 | 
10 | # Precompiled Headers
11 | *.gch
12 | *.pch
13 | 
14 | # Compiled Dynamic libraries
15 | *.so
16 | *.dylib
17 | *.dll
18 | 
19 | # Fortran module files
20 | *.mod
21 | *.smod
22 | 
23 | # Compiled Static libraries
24 | *.lai
25 | *.la
26 | *.a
27 | *.lib
28 | 
29 | # Executables
30 | *.exe
31 | *.out
32 | *.app
33 | 


--------------------------------------------------------------------------------
/LICENSE:
--------------------------------------------------------------------------------
 1 | MIT License
 2 | 
 3 | Copyright (c) 2020 Stanislav Mikhel
 4 | 
 5 | Permission is hereby granted, free of charge, to any person obtaining a copy
 6 | of this software and associated documentation files (the "Software"), to deal
 7 | in the Software without restriction, including without limitation the rights
 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 9 | copies of the Software, and to permit persons to whom the Software is
10 | furnished to do so, subject to the following conditions:
11 | 
12 | The above copyright notice and this permission notice shall be included in all
13 | copies or substantial portions of the Software.
14 | 
15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
21 | SOFTWARE.
22 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | # ext_observer
 2 | 
 3 | External torque observers for a robot manipulator. 
 4 | 
 5 | **lib** contains C++ implementations for such observers as:
 6 | - momentum observer
 7 | - disturbance observer
 8 | - sliding mode observer 
 9 | - Kalman filter based observer
10 | - dynamic filter based observer
11 | 
12 | **matlab** is the Matlab wrapper code for the observers.
13 | 
14 | **tests** contains some tests.
15 | 
16 | Integration with **ROS** can be found in the "ros_examples" branch.
17 | 
18 | The library was developed for the [research](https://www.frontiersin.org/articles/10.3389/frobt.2020.571574/full?utm_campaign=Artificial%2BIntelligence%2BWeekly&utm_medium=web&utm_source=Artificial_Intelligence_Weekly_189) 
19 | ```
20 | @article{mamedov2020practical,
21 |   title={Practical aspects of model-based collision detection},
22 |   author={Mamedov, Shamil and Mikhel, Stanislav},
23 |   journal={Frontiers in Robotics and AI},
24 |   volume={7},
25 |   pages={571574},
26 |   year={2020},
27 |   publisher={Frontiers Media SA}
28 | }
29 | ```
30 | 


--------------------------------------------------------------------------------
/lib/README.md:
--------------------------------------------------------------------------------
 1 | # Implementation of the torque observers 
 2 | 
 3 | The main method for each observer is 
 4 | ```cpp
 5 | Vector getExternalTorque(Vector& q, Vector& dq, Vector& tau, double dt)
 6 | ```
 7 | where _q_ is the vector of joint angles, _dq_ is the vector of joint velocities, _tau_ is the vector of measured joint torques, _dt_ is the time step. 
 8 | 
 9 | The robot dynamics can be defined in 2 ways. If all the matrices (_M_, _C_ and _G_) are known exactly, use default implementation. When dynamics is defined in form of RNEA algorithm (i.e. as a function _f(q,dq,ddq)_) use implementations with "Rnea" in the end.
10 | 


--------------------------------------------------------------------------------
/lib/disturbance_kalman_filter.h:
--------------------------------------------------------------------------------
  1 | // Copyright 2020-2024 Stanislav Mikhel
  2 | 
  3 | /**
  4 |  * @file disturbance_kalman_filter.h
  5 |  *
  6 |  * @brief Disturbance kalman filter observer.
  7 |  *
  8 |  * Expected explicit robot dynamics matrices.
  9 |  */
 10 | #ifndef DISTURBANCE_KALMAN_FILTER_H
 11 | #define DISTURBANCE_KALMAN_FILTER_H
 12 | 
 13 | #include "external_observer.h"
 14 | #include "kalman_filter.h"
 15 | 
 16 | #define ID_DKalmanObserver 1
 17 | 
 18 | /**
 19 |  * @brief Disturbance Kalman filter from Hu et. al.
 20 |  */
 21 | class DKalmanObserver final : public ExternalObserver {
 22 | public:
 23 |   /**
 24 |    * @brief Object constructor
 25 |    * @param rd pointer to the robot object.
 26 |    * @param s disturbance dynamics matrix.
 27 |    * @param h disturbance observation matrix.
 28 |    * @param q covariance of the process noise.
 29 |    * @param r covariance of the observation noise.
 30 |    */
 31 |   DKalmanObserver(RobotDynamics* rd, MatrixJ& s, MatrixJ& h, MatrixJ& q, MatrixJ& r);
 32 | 
 33 |   /**
 34 |    * @brief Object destructor.
 35 |    */
 36 |   virtual ~DKalmanObserver();
 37 | 
 38 |   /**
 39 |    * @brief External torque estimation.
 40 |    * @param q joint angle vector.
 41 |    * @param qd joint velocity vector.
 42 |    * @param tau joint torque vector.
 43 |    * @param dt time step.
 44 |    * @return external torque vector.
 45 |    */
 46 |   VectorJ getExternalTorque(VectorJ& q, VectorJ& qd, VectorJ& tau, double dt) override;
 47 | 
 48 |   /**
 49 |    * @brief Observer settings.
 50 |    * @param q covariance of the process noise.
 51 |    * @param r covariance of the observation noise.
 52 |    */
 53 |   void settings(MatrixJ& q, MatrixJ& r);
 54 | 
 55 | private:
 56 |   // disturbance observation
 57 |   MatrixJ H;
 58 |   // state, input and momentum
 59 |   VectorJ X, u, p;
 60 |   // Kalman filter object
 61 |   KalmanFilter *filter;
 62 | };  // DKalmanObserver
 63 | 
 64 | 
 65 | // Initialization
 66 | DKalmanObserver::DKalmanObserver(RobotDynamics* rd, MatrixJ& s, MatrixJ& h, MatrixJ& q, MatrixJ& r)
 67 |   : ExternalObserver(rd, ID_DKalmanObserver)
 68 |   , H(h)
 69 |   , X(VectorJ(2*jointNo))
 70 |   , u(VectorJ(jointNo))
 71 |   , p(VectorJ(jointNo))
 72 |   , filter(0)
 73 | {
 74 |   // prepare matrices
 75 |   MatrixJ A(2*jointNo, 2*jointNo), B(2*jointNo, jointNo), C(jointNo, 2*jointNo);
 76 |   B.setZero();
 77 |   C.setZero();
 78 |   // B & C matrices
 79 |   for(int i = 0; i < jointNo; i++) {
 80 |     B(i, i) = 1;
 81 |     C(i, i) = 1;
 82 |   }
 83 |   // A matrix
 84 |   A.setZero();
 85 |   A.block(      0, jointNo, jointNo, jointNo) = h;
 86 |   A.block(jointNo, jointNo, jointNo, jointNo) = s;
 87 |   // make filter
 88 |   filter = new KalmanFilter(A, B, C);
 89 |   filter->setCovariance(q, r);
 90 | }
 91 | 
 92 | // Clear dynamically allocated memory
 93 | DKalmanObserver::~DKalmanObserver()
 94 | {
 95 |   delete filter;
 96 | }
 97 | 
 98 | // Torque estimation
 99 | VectorJ DKalmanObserver::getExternalTorque(VectorJ& q, VectorJ& qd, VectorJ& tau, double dt)
100 | {
101 |   p = dyn->getM(q) * qd;
102 |   u = tau - dyn->getG(q) - dyn->getFriction(qd);
103 |   u += dyn->getC(q, qd).transpose() * qd;
104 | 
105 |   if(isRun) {
106 |     X = filter->step(u, p, dt);
107 |   } else {
108 |     // prepare X0
109 |     X.setZero();
110 |     for(int i = 0; i < jointNo; i++) X(i) = p(i);
111 |     // reset
112 |     filter->reset(X);
113 |     isRun = true;
114 |   }
115 |   // disturbance = H * omega   (reuse variable)
116 |   p = H * X.block(jointNo, 0, jointNo, 1);
117 | 
118 |   return p;
119 | }
120 | 
121 | // settings
122 | void DKalmanObserver::settings(MatrixJ& Q, MatrixJ& R)
123 | {
124 |   filter->setCovariance(Q, R);
125 | }
126 | 
127 | #endif  // DISTURBANCE_KALMAN_FILTER_H
128 | 


--------------------------------------------------------------------------------
/lib/disturbance_kalman_filter_exp.h:
--------------------------------------------------------------------------------
  1 | // Copyright 2020-2024 Stanislav Mikhel
  2 | 
  3 | /**
  4 |  * @file disturbance_kalman_filter.h
  5 |  *
  6 |  * @brief Disturbance kalman filter observer.
  7 |  *
  8 |  * Expected explicit robot dynamics matrices.
  9 |  */
 10 | #ifndef DISTURBANCE_KALMAN_FILTER_EXP_H
 11 | #define DISTURBANCE_KALMAN_FILTER_EXP_H
 12 | 
 13 | #include "external_observer.h"
 14 | #include "kalman_filter_continous.h"
 15 | 
 16 | #define ID_DKalmanObserverExp 7
 17 | 
 18 | /**
 19 |  * @brief Disturbance Kalman filter from Hu et. al.
 20 |  *
 21 |  * Use matrix exponent for discretizaion.
 22 |  */
 23 | class DKalmanObserverExp final : public ExternalObserver {
 24 | public:
 25 |   /**
 26 |    * @brief Object constructor
 27 |    * @param rd pointer to the robot object.
 28 |    * @param s disturbance dynamics matrix.
 29 |    * @param h disturbance observation matrix.
 30 |    * @param q covariance of the process noise.
 31 |    * @param r covariance of the observation noise.
 32 |    */
 33 |   DKalmanObserverExp(RobotDynamics* rd, MatrixJ& s, MatrixJ& h, MatrixJ& q, MatrixJ& r);
 34 | 
 35 |   /**
 36 |    * @brief Object destructor.
 37 |    */
 38 |   virtual ~DKalmanObserverExp();
 39 | 
 40 |   /**
 41 |    * @brief External torque estimation.
 42 |    * @param q joint angle vector.
 43 |    * @param qd joint velocity vector.
 44 |    * @param tau joint torque vector.
 45 |    * @param dt time step.
 46 |    * @return external torque vector.
 47 |    */
 48 |   VectorJ getExternalTorque(VectorJ& q, VectorJ& qd, VectorJ& tau, double dt) override;
 49 | 
 50 |   /**
 51 |    * @brief Observer settings.
 52 |    * @param q covariance of the process noise.
 53 |    * @param r covariance of the observation noise.
 54 |    */
 55 |   void settings(MatrixJ& q, MatrixJ& r);
 56 | 
 57 | private:
 58 |   // disturbance observation
 59 |   MatrixJ H, M;
 60 |   // state, input and momentum
 61 |   VectorJ X, u, p;
 62 |   // Kalman filter object
 63 |   KalmanFilterContinous *filter;
 64 | };  // DKalmanObserverExp
 65 | 
 66 | 
 67 | // Initialization
 68 | DKalmanObserverExp::DKalmanObserverExp(
 69 |   RobotDynamics* rd, MatrixJ& s, MatrixJ& h, MatrixJ& q, MatrixJ& r)
 70 |   : ExternalObserver(rd, ID_DKalmanObserverExp)
 71 |   , H(h)
 72 |   , M(MatrixJ(jointNo, jointNo))
 73 |   , X(VectorJ(2*jointNo))
 74 |   , u(VectorJ(jointNo))
 75 |   , p(VectorJ(jointNo))
 76 |   , filter(0)
 77 | {
 78 |   // prepare matrices
 79 |   MatrixJ A(2*jointNo, 2*jointNo), B(2*jointNo, jointNo), C(jointNo, 2*jointNo);
 80 |   B.setZero();
 81 |   C.setZero();
 82 |   // B & C matrices
 83 |   for(int i = 0; i < jointNo; i++) {
 84 |     B(i, i) = 1;
 85 |     C(i, i) = 1;
 86 |   }
 87 |   // A matrix
 88 |   A.setZero();
 89 |   A.block(      0, jointNo, jointNo, jointNo) = h;
 90 |   A.block(jointNo, jointNo, jointNo, jointNo) = s;
 91 |   // make filter
 92 |   filter = new KalmanFilterContinous(A, B, C);
 93 |   filter->setCovariance(q, r);
 94 | }
 95 | 
 96 | // Clear dynamically allocated memory
 97 | DKalmanObserverExp::~DKalmanObserverExp()
 98 | {
 99 |   delete filter;
100 | }
101 | 
102 | // Torque estimation
103 | VectorJ DKalmanObserverExp::getExternalTorque(VectorJ& q, VectorJ& qd, VectorJ& tau, double dt)
104 | {
105 |   M = dyn->getM(q);
106 |   p = M * qd;
107 |   u = tau - dyn->getG(q) - dyn->getFriction(qd);
108 |   u += dyn->getC(q, qd).transpose() * qd;
109 | 
110 |   filter->updateR(M);
111 | 
112 |   if(isRun) {
113 |     X = filter->step(u, p, dt);
114 |   } else {
115 |     // prepare X0
116 |     X.setZero();
117 |     for(int i = 0; i < jointNo; i++) X(i) = p(i);
118 |     // reset
119 |     filter->reset(X);
120 |     isRun = true;
121 |   }
122 |   // disturbance = H * omega   (reuse variable)
123 |   p = H * X.block(jointNo, 0, jointNo, 1);
124 | 
125 |   return p;
126 | }
127 | 
128 | // settings
129 | void DKalmanObserverExp::settings(MatrixJ& Q, MatrixJ& R)
130 | {
131 |   filter->setCovariance(Q, R);
132 | }
133 | 
134 | #endif  // DISTURBANCE_KALMAN_FILTER_EXP_H
135 | 


--------------------------------------------------------------------------------
/lib/disturbance_kalman_filter_rnea.h:
--------------------------------------------------------------------------------
  1 | // Copyright 2020-2024 Stanislav Mikhel
  2 | 
  3 | /**
  4 |  * @file disturbance_kalman_filter.h
  5 |  *
  6 |  * @brief Disturbance kalman filter observer.
  7 |  *
  8 |  * Expected explicit robot dynamics matrices.
  9 |  */
 10 | #ifndef DISTURBANCE_KALMAN_FILTER_RNEA_H
 11 | #define DISTURBANCE_KALMAN_FILTER_RNEA_H
 12 | 
 13 | #include "external_observer.h"
 14 | #include "kalman_filter.h"
 15 | 
 16 | #define ID_DKalmanObserverRnea 21
 17 | 
 18 | /**
 19 |  * @brief Disturbance Kalman filter from Hu et. al.
 20 |  *
 21 |  * Use RNEA technique for dynamics.
 22 |  */
 23 | class DKalmanObserverRnea final : public ExternalObserverRnea {
 24 | public:
 25 |   /**
 26 |    * @brief Object constructor
 27 |    * @param rd pointer to the robot object.
 28 |    * @param s disturbance dynamics matrix.
 29 |    * @param h disturbance observation matrix.
 30 |    * @param q covariance of the process noise.
 31 |    * @param r covariance of the observation noise.
 32 |    */
 33 |   DKalmanObserverRnea(RobotDynamicsRnea* rd, MatrixJ& s, MatrixJ& h, MatrixJ& q, MatrixJ& r);
 34 | 
 35 |   /**
 36 |    * @brief Object destructor.
 37 |    */
 38 |   virtual ~DKalmanObserverRnea();
 39 | 
 40 |   /**
 41 |    * @brief External torque estimation.
 42 |    * @param q joint angle vector.
 43 |    * @param qd joint velocity vector.
 44 |    * @param tau joint torque vector.
 45 |    * @param dt time step.
 46 |    * @return external torque vector.
 47 |    */
 48 |   VectorJ getExternalTorque(VectorJ& q, VectorJ& qd, VectorJ& tau, double dt) override;
 49 | 
 50 |   /**
 51 |    * @brief Observer settings.
 52 |    * @param q covariance of the process noise.
 53 |    * @param r covariance of the observation noise.
 54 |    */
 55 |   void settings(MatrixJ& q, MatrixJ& r);
 56 | 
 57 | private:
 58 |   // disturbance observation
 59 |   MatrixJ H;
 60 |   // state, input and momentum
 61 |   VectorJ X, u, p, zero;
 62 |   // Kalman filter object
 63 |   KalmanFilter *filter;
 64 | };  // DKalmanObserverRnea
 65 | 
 66 | // Initialization
 67 | DKalmanObserverRnea::DKalmanObserverRnea(
 68 |   RobotDynamicsRnea* rd, MatrixJ& s, MatrixJ& h, MatrixJ& q, MatrixJ& r)
 69 |   : ExternalObserverRnea(rd, ID_DKalmanObserverRnea)
 70 |   , H(h)
 71 |   , X(VectorJ(2*jointNo))
 72 |   , u(VectorJ(jointNo))
 73 |   , p(VectorJ(jointNo))
 74 |   , zero(VectorJ::Zero(jointNo))
 75 |   , filter(0)
 76 | {
 77 |   // prepare matrices
 78 |   MatrixJ A(2*jointNo, 2*jointNo), B(2*jointNo, jointNo), C(jointNo, 2*jointNo);
 79 |   B.setZero();
 80 |   C.setZero();
 81 |   // B & C matrices
 82 |   for(int i = 0; i < jointNo; i++) {
 83 |     B(i, i) = 1;
 84 |     C(i, i) = 1;
 85 |   }
 86 |   // A matrix
 87 |   A.setZero();
 88 |   A.block(      0, jointNo, jointNo, jointNo) = h;
 89 |   A.block(jointNo, jointNo, jointNo, jointNo) = s;
 90 |   // make filter
 91 |   filter = new KalmanFilter(A, B, C);
 92 |   filter->setCovariance(q, r);
 93 | }
 94 | 
 95 | // Clear dynamically allocated memory
 96 | DKalmanObserverRnea::~DKalmanObserverRnea()
 97 | {
 98 |   delete filter;
 99 | }
100 | 
101 | // Torque estimation
102 | VectorJ DKalmanObserverRnea::getExternalTorque(VectorJ& q, VectorJ& qd, VectorJ& tau, double dt)
103 | {
104 |   p = dyn->rnea(q, zero, qd);     // M * qd
105 |   u = tau - dyn->rnea(q, zero, zero, GRAVITY) - dyn->getFriction(qd);
106 |   u += dyn->tranCqd(q, qd);      // C' * qd
107 | 
108 |   if(isRun) {
109 |     X = filter->step(u, p, dt);
110 |   } else {
111 |     // prepare X0
112 |     X.setZero();
113 |     for(int i = 0; i < jointNo; i++) X(i) = p(i);
114 |     // reset
115 |     filter->reset(X);
116 |     isRun = true;
117 |   }
118 |   // disturbance = H * omega   (reuse variable)
119 |   p = H * X.block(jointNo, 0, jointNo, 1);
120 | 
121 |   return p;
122 | }
123 | 
124 | // settings
125 | void DKalmanObserverRnea::settings(MatrixJ& Q, MatrixJ& R)
126 | {
127 |   filter->setCovariance(Q, R);
128 | }
129 | 
130 | #endif  // DISTURBANCE_KALMAN_FILTER_RNEA_H
131 | 


--------------------------------------------------------------------------------
/lib/disturbance_observer.h:
--------------------------------------------------------------------------------
  1 | // Copyright 2020-2024 Stanislav Mikhel
  2 | 
  3 | /**
  4 |  * @file disturbance_observer.h
  5 |  *
  6 |  * @brief Disturbance observer class.
  7 |  *
  8 |  * Expected explicit robot dynamics matrices.
  9 |  */
 10 | #ifndef DISTURBANCE_OBSERVER_H
 11 | #define DISTURBANCE_OBSERVER_H
 12 | 
 13 | #include "external_observer.h"
 14 | 
 15 | #define ID_DisturbanceObserver 4
 16 | 
 17 | /**
 18 |  * @brief Disturbance observer from Mohammadi et. al.
 19 |  */
 20 | class DisturbanceObserver final : public ExternalObserver {
 21 | public:
 22 |   /**
 23 |    * @brief Object constructor.
 24 |    * @param rd pointer to the robot object.
 25 |    * @param sigma ...
 26 |    * @param xeta ...
 27 |    * @param beta ...
 28 |    */
 29 |   DisturbanceObserver(RobotDynamics *rd, double sigma, double xeta, double beta);
 30 | 
 31 |   /**
 32 |    * @brief External torque estimation.
 33 |    * @param q joint angle vector.
 34 |    * @param qd joint velocity vector.
 35 |    * @param tau joint torque vector.
 36 |    * @param dt time step.
 37 |    * @return external torque vector.
 38 |    */
 39 |   VectorJ getExternalTorque(VectorJ& q, VectorJ& qd, VectorJ& tau, double dt) override;
 40 | 
 41 |   /**
 42 |    * @brief Observer settings.
 43 |    * @param sigma ...
 44 |    * @param xeta ...
 45 |    * @param beta ...
 46 |    */
 47 |   void settings(double sigma, double xeta, double beta);
 48 | 
 49 | private:
 50 |   // temporary objects
 51 |   MatrixJ Y, L, I;
 52 |   MatrixJ lft, rht;
 53 |   VectorJ p, z, torque;
 54 | };  // DisturbanceObserver
 55 | 
 56 | 
 57 | // Initialization
 58 | DisturbanceObserver::DisturbanceObserver(RobotDynamics *rd, double sigma, double xeta, double beta)
 59 |   : ExternalObserver(rd, ID_DisturbanceObserver)
 60 |   , Y(MatrixJ(jointNo, jointNo))
 61 |   , L(MatrixJ(jointNo, jointNo))
 62 |   , I(MatrixJ::Identity(jointNo, jointNo))
 63 |   , lft(MatrixJ(jointNo, jointNo))
 64 |   , rht(MatrixJ(jointNo, jointNo))
 65 |   , p(VectorJ(jointNo))
 66 |   , z(VectorJ(jointNo))
 67 |   , torque(VectorJ(jointNo))
 68 | {
 69 |   settings(sigma, xeta, beta);
 70 | }
 71 | 
 72 | // Get torque
 73 | VectorJ DisturbanceObserver::getExternalTorque(VectorJ& q, VectorJ& qd, VectorJ& tau, double dt)
 74 | {
 75 |   L = Y * dyn->getM(q).inverse();
 76 |   L *= dt;
 77 |   p = Y * qd;
 78 | 
 79 |   if(isRun) {
 80 |     torque = tau - dyn->getFriction(qd);
 81 |     lft = I + L;
 82 |     rht = z + L*(dyn->getC(q, qd)*qd + dyn->getG(q) - torque - p);
 83 |     z = lft.inverse() * rht;
 84 |   } else {
 85 |     z = -p;
 86 |     isRun = true;
 87 |   }
 88 | 
 89 |   p += z;
 90 | 
 91 |   return p;
 92 | }
 93 | 
 94 | // Update parameters
 95 | void DisturbanceObserver::settings(double sigma, double xeta, double beta)
 96 | {
 97 |   double k = 0.5*(xeta + 2*beta*sigma);
 98 |   Y = k * MatrixJ::Identity(jointNo, jointNo);
 99 | }
100 | 
101 | #endif  // DISTURBANCE_OBSERVER_H
102 | 


--------------------------------------------------------------------------------
/lib/disturbance_observer_rnea.h:
--------------------------------------------------------------------------------
  1 | // Copyright 2020-2024 Stanislav Mikhel
  2 | 
  3 | /**
  4 |  * @file disturbance_observer_rnea.h
  5 |  *
  6 |  * @brief Disturbance observer class.
  7 |  *
  8 |  * Expected robot dynamics in form of RNEA.
  9 |  */
 10 | #ifndef DISTURBANCE_OBSERVER_RNEA_H
 11 | #define DISTURBANCE_OBSERVER_RNEA_H
 12 | 
 13 | #include "external_observer.h"
 14 | 
 15 | #define ID_DisturbanceObserverRnea 24
 16 | 
 17 | /**
 18 |  * @brief Disturbance observer from Mohammadi et. al.
 19 |  *
 20 |  * Use RNEA technique for dynamics.
 21 |  */
 22 | class DisturbanceObserverRnea final : public ExternalObserverRnea {
 23 | public:
 24 |   /**
 25 |    * @brief Object constructor.
 26 |    * @param rd pointer to the robot object.
 27 |    * @param sigma ...
 28 |    * @param xeta ...
 29 |    * @param beta ...
 30 |    */
 31 |   DisturbanceObserverRnea(RobotDynamicsRnea *rd, double sigma, double xeta, double beta);
 32 | 
 33 |   /**
 34 |    * @brief External torque estimation.
 35 |    * @param q joint angle vector.
 36 |    * @param qd joint velocity vector.
 37 |    * @param tau joint torque vector.
 38 |    * @param dt time step.
 39 |    * @return external torque vector.
 40 |    */
 41 |   VectorJ getExternalTorque(VectorJ& q, VectorJ& qd, VectorJ& tau, double dt) override;
 42 | 
 43 |   /**
 44 |    * @brief Observer settings.
 45 |    * @param sigma ...
 46 |    * @param xeta ...
 47 |    * @param beta ...
 48 |    */
 49 |   void settings(double sigma, double xeta, double beta);
 50 | 
 51 | private:
 52 |   // temporary objects
 53 |   MatrixJ Y, L, I;
 54 |   MatrixJ lft, rht;
 55 |   VectorJ p, z, torque, zeros;
 56 | };  // DisturbanceObserverRnea
 57 | 
 58 | 
 59 | // Initialization
 60 | DisturbanceObserverRnea::DisturbanceObserverRnea(
 61 |   RobotDynamicsRnea *rd, double sigma, double xeta, double beta)
 62 |   : ExternalObserverRnea(rd, ID_DisturbanceObserverRnea)
 63 |   , Y(MatrixJ(jointNo, jointNo))
 64 |   , L(MatrixJ(jointNo, jointNo))
 65 |   , I(MatrixJ::Identity(jointNo, jointNo))
 66 |   , lft(MatrixJ(jointNo, jointNo))
 67 |   , rht(MatrixJ(jointNo, jointNo))
 68 |   , p(VectorJ(jointNo))
 69 |   , z(VectorJ(jointNo))
 70 |   , torque(VectorJ(jointNo))
 71 |   , zeros(VectorJ::Zero(jointNo))
 72 | {
 73 |   settings(sigma, xeta, beta);
 74 | }
 75 | 
 76 | // Get torque
 77 | VectorJ DisturbanceObserverRnea::getExternalTorque(VectorJ& q, VectorJ& qd, VectorJ& tau, double dt)
 78 | {
 79 |   L = Y * dyn->getM(q).inverse();
 80 |   L *= dt;
 81 |   p = Y * qd;
 82 | 
 83 |   if(isRun) {
 84 |     torque = tau - dyn->getFriction(qd);
 85 |     lft = I + L;
 86 |     rht = z + L*(dyn->rnea(q, qd, zeros, GRAVITY) - torque - p);
 87 |     z = lft.inverse() * rht;
 88 |   } else {
 89 |     z = -p;
 90 |     isRun = true;
 91 |   }
 92 | 
 93 |   p += z;
 94 | 
 95 |   return p;
 96 | }
 97 | 
 98 | // Update parameters
 99 | void DisturbanceObserverRnea::settings(double sigma, double xeta, double beta)
100 | {
101 |   double k = 0.5*(xeta + 2*beta*sigma);
102 |   Y = k * MatrixJ::Identity(jointNo, jointNo);
103 | }
104 | 
105 | #endif  // DISTURBANCE_OBSERVER_RNEA_H
106 | 


--------------------------------------------------------------------------------
/lib/external_observer.h:
--------------------------------------------------------------------------------
  1 | // Copyright 2020-2024 Stanislav Mikhel
  2 | 
  3 | /**
  4 |  * @file external_observer.h
  5 |  *
  6 |  * @brief Abstract classes for a robot and external torque observer.
  7 |  *
  8 |  * Define robot interfaces for working with dynamic matrices and recursice Newton-Euler algorithm
  9 |  * and corresponding observers.
 10 |  */
 11 | #ifndef EXTERNAL_OBSERVER_H
 12 | #define EXTERNAL_OBSERVER_H
 13 | 
 14 | #include <eigen3/Eigen/Geometry>
 15 | 
 16 | /** @brief Gravity constant. */
 17 | const double GRAVITY = 9.81;
 18 | 
 19 | /**
 20 |  * @brief Simplified object call.
 21 |  */
 22 | typedef Eigen::MatrixXd MatrixJ;
 23 | typedef Eigen::VectorXd VectorJ;
 24 | 
 25 | /**
 26 |  * @brief Common elements of a robot interface.
 27 |  */
 28 | class RobotDynamicsBase {
 29 | public:
 30 |   virtual ~RobotDynamicsBase() = default;
 31 | 
 32 |   /**
 33 |    * @brief Friction model.
 34 |    *
 35 |    * Find expected friction using velocity.
 36 |    * @param qd vector of joint velocities
 37 |    * @return torque loss due to friction.
 38 |    */
 39 |   virtual VectorJ getFriction(VectorJ& qd) = 0;
 40 | 
 41 |   /**
 42 |    * @brief Number of movable joints in robot.
 43 |    * @return Joint number.
 44 |    */
 45 |   virtual int jointNo() = 0;
 46 | };  // RobotDynamicsBase
 47 | 
 48 | 
 49 | /**
 50 |  * @brief Base class when M, C and G are known.
 51 |  *
 52 |  * Base class for a robot if all dynamical matrices could be found explicitly.
 53 |  */
 54 | class RobotDynamics : public RobotDynamicsBase {
 55 | public:
 56 |   virtual ~RobotDynamics() = default;
 57 | 
 58 |   /**
 59 |    * @brief Get inertia matrix.
 60 |    * @param q joint angle vector.
 61 |    * @return matrix M.
 62 |    */
 63 |   virtual MatrixJ getM(VectorJ& q) = 0;
 64 | 
 65 |   /**
 66 |    * @brief Get Coriolis/centrifugal matrix.
 67 |    * @param q joint angle vector.
 68 |    * @param qd joint angle velocity vector.
 69 |    * @return matrix C.
 70 |    */
 71 | 
 72 |   virtual MatrixJ getC(VectorJ& q, VectorJ& qd) = 0;
 73 |   /**
 74 |    * @brief Get gravity terms.
 75 |    * @param q joint angle vector.
 76 |    * @return vector G.
 77 |    */
 78 |   virtual VectorJ getG(VectorJ& q) = 0;
 79 | };  // RobotDynamics
 80 | 
 81 | 
 82 | /**
 83 |  * @brief Base class when RNEA is defined.
 84 |  *
 85 |  * Base class for a robot if only RNEA could be applied.
 86 |  */
 87 | class RobotDynamicsRnea : public RobotDynamicsBase {
 88 | public:
 89 |   /**
 90 |    * @brief Object constructor.
 91 |    */
 92 |   RobotDynamicsRnea();
 93 | 
 94 |   virtual ~RobotDynamicsRnea() = default;
 95 | 
 96 |   /**
 97 |    * @brief Call recursibe Newton-Euler algorithm.
 98 |    * @param q joint angle vector.
 99 |    * @param qd joint velocity vector.
100 |    * @param q2d joint acceleraiton vector.
101 |    * @param g gravity constant.
102 |    * @return torque value.
103 |    */
104 |   virtual VectorJ rnea(VectorJ& q, VectorJ& qd, VectorJ& q2d, double g = 0) = 0;
105 | 
106 |   /**
107 |    * @brief Angle step for derivative estimation.
108 |    *
109 |    * Joint angle step for numerical evaluation of dM/dt.
110 |    * @param d desired value.
111 |    */
112 |   inline void setDelta(double d) noexcept { delta = d; }
113 | 
114 |   /**
115 |    * @brief Estimation of product C^T * qd.
116 |    *
117 |    * Use numerical differentiation technique.
118 |    * @param q joint angle vector.
119 |    * @param qd joint velocity vector.
120 |    * @return product value estiamtion.
121 |    */
122 |   VectorJ tranCqd(VectorJ& q, VectorJ& qd);
123 | 
124 |   /**
125 |    * @brief Build matrix of inertia.
126 |    * @param q joint angle vector.
127 |    * @return matrix M.
128 |    */
129 |   MatrixJ getM(VectorJ& q);
130 | 
131 | protected:
132 |   /** @brief Temporary variables, avoid memory reallocation. */
133 |   VectorJ _qext, _p0, _zero, _sum;
134 |   MatrixJ _M;
135 |   /** @bried Differentiation step. */
136 |   double delta = 1E-7;
137 | };  // RobotDynamicsRnea
138 | 
139 | 
140 | class ExternalObserverBase {
141 | public:
142 |   /**
143 |    * @brief Object constructor.
144 |    */
145 |   ExternalObserverBase()
146 |   : jointNo(0)
147 |   , objType(-1)
148 |   , isRun(false) {}
149 | 
150 |   virtual ~ExternalObserverBase() = default;
151 | 
152 |   /**
153 |    * @brief Estimate external torques.
154 |    *
155 |    * The main method to use.
156 |    * @param q joint angle vector.
157 |    * @param qd joint velocity vector.
158 |    * @param tau joint torque vector.
159 |    * @param dt time step from the previous call.
160 |    * @return external torque vector.
161 |    */
162 |   virtual VectorJ getExternalTorque(VectorJ& q, VectorJ& qd, VectorJ& tau, double dt) = 0;
163 | 
164 |   /**
165 |    * @brief Reset observer state.
166 |    */
167 |   inline void reset() noexcept { isRun = false; }
168 | 
169 |   /**
170 |    * @brief Get observer type.
171 |    */
172 |   inline int type() const noexcept { return objType; }
173 | 
174 | protected:
175 |   int jointNo;           /**< Number of joints. */
176 |   int objType;           /**< Observer type. */
177 |   bool isRun;            /**< First call check. */
178 | };  // ExternalObserverBase
179 | 
180 | 
181 | /**
182 |  * @brief External torque observer.
183 |  *
184 |  * Work with matrices M, C ang G.
185 |  */
186 | class ExternalObserver : public ExternalObserverBase {
187 | public:
188 |   /**
189 |    * @brief Object constructor.
190 |    * @param rd pointer to the robot interface.
191 |    */
192 |   ExternalObserver(RobotDynamics *rd, int type)
193 |   : ExternalObserverBase()
194 |   , dyn(rd)
195 |   {
196 |     jointNo = rd->jointNo();
197 |     objType = type;
198 |   }
199 | 
200 |   virtual ~ExternalObserver() = default;
201 | 
202 | protected:
203 |   RobotDynamics *dyn;    /**< Pointer to the robot interface. */
204 | };  // ExternalObserver
205 | 
206 | 
207 | /**
208 |  * @brief External torque observer.
209 |  *
210 |  * Work with RNEA algorithm.
211 |  */
212 | class ExternalObserverRnea : public ExternalObserverBase {
213 | public:
214 |   /**
215 |    * @brief Object constructor.
216 |    * @param rd pointer to the robot interface.
217 |    */
218 |   ExternalObserverRnea(RobotDynamicsRnea *rd, int type)
219 |   : ExternalObserverBase()
220 |   , dyn(rd)
221 |   {
222 |     jointNo = rd->jointNo();
223 |     objType = type;
224 |   }
225 | 
226 |   virtual ~ExternalObserverRnea() = default;
227 | 
228 | protected:
229 |   RobotDynamicsRnea *dyn; /**< Pointer to the robot interface. */
230 | };  // ExternalObserverRnea
231 | 
232 | #endif  // EXTERNAL_OBSERVER_H
233 | 
234 | 


--------------------------------------------------------------------------------
/lib/filtered_dyn_observer.h:
--------------------------------------------------------------------------------
 1 | // Copyright 2020-2024 Stanislav Mikhel
 2 | 
 3 | #ifndef FILTERED_DYNAMIC_OBSERVER_H
 4 | #define FILTERED_DYNAMIC_OBSERVER_H
 5 | 
 6 | #include "external_observer.h"
 7 | #include "iir_filter.h"
 8 | 
 9 | #define ID_FDynObserver 2
10 | 
11 | class FDynObserver final : public ExternalObserver {
12 | public:
13 |   FDynObserver(RobotDynamics *rd, double cutOffHz, double sampHz);
14 | 
15 |   VectorJ getExternalTorque(VectorJ& q, VectorJ& qd, VectorJ& tau, double dt) override;
16 | 
17 |   void settings(double cutOffHz, double sampHz);
18 | 
19 | private:
20 |   FilterF1 f1;
21 |   FilterF2 f2;
22 |   VectorJ p, res;
23 | };  // FDynObserver
24 | 
25 | 
26 | FDynObserver::FDynObserver(RobotDynamics *rd, double cutOffHz, double sampHz)
27 |   : ExternalObserver(rd, ID_FDynObserver)
28 |   , f1(FilterF1(cutOffHz, sampHz, jointNo))
29 |   , f2(FilterF2(cutOffHz, sampHz, jointNo))
30 |   , p(VectorJ(jointNo))
31 |   , res(VectorJ(jointNo))
32 | {
33 | }
34 | 
35 | void FDynObserver::settings(double cutOffHz, double sampHz)
36 | {
37 |   f1.update(cutOffHz, sampHz);
38 |   f2.update(cutOffHz, sampHz);
39 | }
40 | 
41 | VectorJ FDynObserver::getExternalTorque(VectorJ& q, VectorJ& qd, VectorJ& tau, double dt)
42 | {
43 |   p = dyn->getM(q) * qd;
44 | 
45 |   if(isRun) {
46 |     res = f2.filt(p, dt) + f2.getOmega() * p ;
47 |     p = dyn->getFriction(qd) + dyn->getG(q) - dyn->getC(q, qd).transpose() * qd;  // reuse
48 |     p -= tau;
49 |     res += f1.filt(p, dt);
50 |   } else {
51 |     f2.set(p);
52 |     p = dyn->getFriction(qd) + dyn->getG(q) - dyn->getC(q, qd).transpose() * qd;  // reuse
53 |     p -= tau;
54 |     f1.set(p);
55 |     res.setZero();
56 |     isRun = true;
57 |   }
58 | 
59 |   return res;
60 | }
61 | 
62 | #endif  // FILTERED_DYNAMIC_OBSERVER_H
63 | 


--------------------------------------------------------------------------------
/lib/filtered_dyn_observer_rnea.h:
--------------------------------------------------------------------------------
 1 | // Copyright 2020-2024 Stanislav Mikhel
 2 | 
 3 | #ifndef FILTERED_DYNAMIC_OBSERVER_RNEA_H
 4 | #define FILTERED_DYNAMIC_OBSERVER_RNEA_H
 5 | 
 6 | #include "external_observer.h"
 7 | #include "iir_filter.h"
 8 | 
 9 | #define ID_FDynObserverRnea 22
10 | 
11 | class FDynObserverRnea final : public ExternalObserverRnea {
12 | public:
13 |   FDynObserverRnea(RobotDynamicsRnea *rd, double cutOffHz, double sampHz);
14 | 
15 |   VectorJ getExternalTorque(VectorJ& q, VectorJ& qd, VectorJ& tau, double dt) override;
16 | 
17 |   void settings(double cutOffHz, double sampHz);
18 | 
19 | private:
20 |   FilterF1 f1;
21 |   FilterF2 f2;
22 |   VectorJ p, res, zero;
23 | };  // FDynObserver
24 | 
25 | 
26 | FDynObserverRnea::FDynObserverRnea(RobotDynamicsRnea *rd, double cutOffHz, double sampHz)
27 |   : ExternalObserverRnea(rd, ID_FDynObserverRnea)
28 |   , f1(FilterF1(cutOffHz, sampHz, jointNo))
29 |   , f2(FilterF2(cutOffHz, sampHz, jointNo))
30 |   , p(VectorJ(jointNo))
31 |   , res(VectorJ(jointNo))
32 |   , zero(VectorJ::Zero(jointNo))
33 | {
34 | }
35 | 
36 | void FDynObserverRnea::settings(double cutOffHz, double sampHz)
37 | {
38 |   f1.update(cutOffHz, sampHz);
39 |   f2.update(cutOffHz, sampHz);
40 | }
41 | 
42 | VectorJ FDynObserverRnea::getExternalTorque(VectorJ& q, VectorJ& qd, VectorJ& tau, double dt)
43 | {
44 |   p = dyn->rnea(q, zero, qd);     // M * qd
45 | 
46 |   if(isRun) {
47 |     res = f2.filt(p, dt) + f2.getOmega() * p ;
48 |     p = dyn->getFriction(qd) + dyn->rnea(q, zero, zero, GRAVITY) - dyn->tranCqd(q, qd);  // reuse
49 |     p -= tau;
50 |     res += f1.filt(p, dt);
51 |   } else {
52 |     f2.set(p);
53 |     p = dyn->getFriction(qd) + dyn->rnea(q, zero, zero, GRAVITY) - dyn->tranCqd(q, qd);  // reuse
54 |     p -= tau;
55 |     f1.set(p);
56 |     res.setZero();
57 |     isRun = true;
58 |   }
59 | 
60 |   return res;
61 | }
62 | 
63 | #endif  // FILTERED_DYNAMIC_OBSERVER_RNEA_H
64 | 


--------------------------------------------------------------------------------
/lib/filtered_range_observer.h:
--------------------------------------------------------------------------------
 1 | // Copyright 2020-2024 Stanislav Mikhel
 2 | 
 3 | #ifndef FILTERED_RANGE_OBSERVER_H
 4 | #define FILTERED_RANGE_OBSERVER_H
 5 | 
 6 | #include "external_observer.h"
 7 | #include "iir_filter.h"
 8 | 
 9 | #define ID_FRangeObserver 6
10 | 
11 | class FRangeObserver final : public ExternalObserver {
12 | public:
13 |   FRangeObserver(RobotDynamics *rd, double cutOffHz, double sampHz, double k);
14 | 
15 |   VectorJ getExternalTorque(VectorJ& q, VectorJ& qd, VectorJ& tau, double dt) override;
16 | 
17 |   void settings(double cutOffHz, double sampHz, double k);
18 | 
19 | private:
20 |   FilterF1 f1;
21 |   FilterF2 f2;
22 |   VectorJ p, res;
23 |   double shift;
24 | };  // FRangeObserver
25 | 
26 | 
27 | FRangeObserver::FRangeObserver(RobotDynamics *rd, double cutOffHz, double sampHz, double k)
28 |   : ExternalObserver(rd, ID_FRangeObserver)
29 |   , f1(FilterF1(cutOffHz, sampHz, jointNo))
30 |   , f2(FilterF2(cutOffHz, sampHz, jointNo))
31 |   , p(VectorJ(jointNo))
32 |   , res(VectorJ(jointNo))
33 |   , shift(k)
34 | {
35 | }
36 | 
37 | void FRangeObserver::settings(double cutOffHz, double sampHz, double k)
38 | {
39 |   f1.update(cutOffHz, sampHz);
40 |   f2.update(cutOffHz, sampHz);
41 |   shift = k;
42 | }
43 | 
44 | VectorJ FRangeObserver::getExternalTorque(VectorJ& q, VectorJ& qd, VectorJ& tau, double dt)
45 | {
46 |   p = dyn->getM(q) * qd;
47 |   p *= shift;
48 | 
49 |   if(isRun) {
50 |     res = f2.filt(p, dt) + f2.getOmega() * p ;
51 |     p = dyn->getFriction(qd) + dyn->getG(q) - dyn->getC(q, qd).transpose() * qd;  // reuse
52 |     p *= shift;
53 |     res += f1.filt(p, dt);
54 |   } else {
55 |     f2.set(p);
56 |     p = dyn->getFriction(qd) + dyn->getG(q) - dyn->getC(q, qd).transpose() * qd;  // reuse
57 |     p *= shift;
58 |     f1.set(p);
59 |     res.setZero();
60 |     isRun = true;
61 |   }
62 | 
63 |   return res;
64 | }
65 | 
66 | #endif  // FILTERED_RANGE_OBSERVER_H
67 | 


--------------------------------------------------------------------------------
/lib/iir_filter.h:
--------------------------------------------------------------------------------
  1 | // Copyright 2020-2024 Stanislav Mikhel
  2 | 
  3 | #ifndef IIR_FILTER_H
  4 | #define IIR_FILTER_H
  5 | 
  6 | #include <eigen3/Eigen/Dense>
  7 | #include <cmath>
  8 | 
  9 | class FilterIIR {
 10 | public:
 11 |   virtual Eigen::VectorXd filt(Eigen::VectorXd& x) = 0;
 12 | 
 13 |   virtual void update(double cutOff, double sampTime) = 0;
 14 | };  // FilterIIR
 15 | 
 16 | 
 17 | // H(s) = w / (s + w)
 18 | class FilterF1 final : public FilterIIR {
 19 | public:
 20 |   FilterF1(double cutOff, double sampTime, int N)
 21 |   : FilterIIR()
 22 |   , x1(Eigen::VectorXd(N))
 23 |   , y1(Eigen::VectorXd(N))
 24 |   { update(cutOff, sampTime); }
 25 | 
 26 |   Eigen::VectorXd filt(Eigen::VectorXd& x) override;
 27 | 
 28 |   Eigen::VectorXd filt(Eigen::VectorXd& x, double dt);
 29 | 
 30 |   inline void set(Eigen::VectorXd& x0) { x1 = x0; y1 = x0; }
 31 | 
 32 |   void update(double cutOff, double sampTime) override;
 33 | 
 34 | private:
 35 |   Eigen::VectorXd x1, y1;
 36 |   double k1, k2, cut;
 37 | };  // FilterF1
 38 | 
 39 | 
 40 | // H(s) = -w^2 / (s + w)
 41 | class FilterF2  final: public FilterIIR {
 42 | public:
 43 |   FilterF2(double cutOff, double sampTime, int N)
 44 |   : FilterIIR()
 45 |   , x1(Eigen::VectorXd(N))
 46 |   , y1(Eigen::VectorXd(N))
 47 |   { update(cutOff, sampTime); }
 48 | 
 49 |   Eigen::VectorXd filt(Eigen::VectorXd& x) override;
 50 | 
 51 |   Eigen::VectorXd filt(Eigen::VectorXd& x, double dt);
 52 | 
 53 |   inline void set(Eigen::VectorXd& x0) { x1 = x0; y1 = -f2*omega*x0; }
 54 | 
 55 |   inline double getOmega() const noexcept { return cut; }
 56 | 
 57 |   void update(double cutOff, double sampTime) override;
 58 | 
 59 | private:
 60 |   Eigen::VectorXd x1, y1;
 61 |   double f2, omega, k1, k2, cut;
 62 | };  // FilterF2
 63 | 
 64 | 
 65 | class FilterButterworth final : public FilterIIR {
 66 | public:
 67 |   FilterButterworth(double cutOff, double sampTime, int N)
 68 |   : FilterIIR()
 69 |   , x1(Eigen::VectorXd(N))
 70 |   , x2(Eigen::VectorXd(N))
 71 |   , y1(Eigen::VectorXd(N))
 72 |   , y2(Eigen::VectorXd(N))
 73 |   , res(Eigen::VectorXd(N))
 74 |   { update(cutOff, sampTime); }
 75 | 
 76 |   Eigen::VectorXd filt(Eigen::VectorXd& x) override;
 77 | 
 78 |   void update(double cutOff, double sampTime) override;
 79 | 
 80 | private:
 81 |   Eigen::VectorXd x1, x2, y1, y2, res;
 82 |   double kx0, kx1, ky1, ky2;
 83 | };  // FilterButterworth
 84 | 
 85 | 
 86 | class FilterLowPass final : public FilterIIR {
 87 | public:
 88 |   FilterLowPass(double cutOff, double sampTime, int N)
 89 |   : FilterIIR()
 90 |   , x1(Eigen::VectorXd(N))
 91 |   , y1(Eigen::VectorXd(N))
 92 |   { update(cutOff, sampTime); }
 93 | 
 94 |   void update(double cutOff, double sampTime) override;
 95 | 
 96 |   Eigen::VectorXd filt(Eigen::VectorXd& x) override;
 97 | 
 98 | private:
 99 |   Eigen::VectorXd x1, y1;
100 |   double k1, k2;
101 | };  // FilterLowPass
102 | 
103 | 
104 | class FilterHighPass final : public FilterIIR {
105 | public:
106 |   FilterHighPass(double cutOff, double sampTime, int N)
107 |   : FilterIIR()
108 |   , x1(Eigen::VectorXd(N))
109 |   , y1(Eigen::VectorXd(N))
110 |   { update(cutOff, sampTime); }
111 | 
112 |   void update(double cutOff, double sampTime) override;
113 | 
114 |   Eigen::VectorXd filt(Eigen::VectorXd& x) override;
115 | 
116 | private:
117 |   Eigen::VectorXd x1, y1;
118 |   double k1, k2;
119 | };  // FilterHighPass
120 | 
121 | 
122 | Eigen::VectorXd FilterF1::filt(Eigen::VectorXd& x)
123 | {
124 |   y1 = k1*y1 + k2*(x + x1);
125 |   x1 = x;
126 | 
127 |   return y1;
128 | }
129 | 
130 | void FilterF1::update(double cutOff, double sampTime)
131 | {
132 |   double omega = tan(cutOff*sampTime*0.5);
133 |   k1 = (1-omega) / (1+omega);
134 |   k2 = omega / (1 + omega);
135 |   cut = cutOff;
136 | }
137 | 
138 | Eigen::VectorXd FilterF1::filt(Eigen::VectorXd& x, double dt)
139 | {
140 |   double omega = tan(cut*dt*0.5);
141 |   k1 = (1-omega) / (1+omega);
142 |   k2 = omega / (1 + omega);
143 | 
144 |   return filt(x);
145 | }
146 | 
147 | Eigen::VectorXd FilterF2::filt(Eigen::VectorXd& x)
148 | {
149 |   y1 = k1*y1 + k2*(x + x1);
150 |   x1 = x;
151 | 
152 |   return y1;
153 | }
154 | 
155 | 
156 | void FilterF2::update(double cutOff, double sampTime)
157 | {
158 |   cut = cutOff;
159 |   f2 = 2/sampTime;
160 |   omega = tan(cutOff*sampTime*0.5);
161 |   k1 = (1-omega)/(1+omega);
162 |   k2 = -f2*omega*omega/(1+omega);
163 | }
164 | 
165 | Eigen::VectorXd FilterF2::filt(Eigen::VectorXd& x, double dt)
166 | {
167 |   f2 = 2/dt;
168 |   omega = tan(cut*dt*0.5);
169 |   k1 = (1-omega)/(1+omega);
170 |   k2 = -f2*omega*omega/(1+omega);
171 | 
172 |   return filt(x);
173 | }
174 | 
175 | void FilterButterworth::update(double cutOff, double sampTime)
176 | {
177 |   double omega = tan(cutOff * sampTime * 0.5);
178 |   double omega2 = omega * omega;
179 |   double denom = 1 + M_SQRT2*omega + omega2;
180 | 
181 |   ky1 = 2*(1-omega2)/denom;
182 |   ky2 = (M_SQRT2*omega-1-omega2)/denom;
183 |   kx0 = omega2 / denom;            // kx2 == kx0
184 |   kx1 = 2*kx0;
185 | }
186 | 
187 | Eigen::VectorXd FilterButterworth::filt(Eigen::VectorXd& x)
188 | {
189 |   res = ky1*y1 + ky2*y2 + kx0*(x + x2) + kx1*x1;
190 |   x2 = x1; x1 = x;
191 |   y2 = y1; y1 = res;
192 | 
193 |   return res;
194 | }
195 | 
196 | void FilterLowPass::update(double cutOff, double sampTime)
197 | {
198 |   double omega = tan(cutOff*sampTime*0.5);
199 |   k1 = (1-omega)/(1+omega);
200 |   k2 = omega/(1+omega);
201 | }
202 | 
203 | Eigen::VectorXd FilterLowPass::filt(Eigen::VectorXd& x)
204 | {
205 |   y1 *= k1;
206 |   y1 += k2*(x + x1);
207 |   x1 = x;
208 |   return y1;
209 | }
210 | 
211 | void FilterHighPass::update(double cutOff, double sampTime)
212 | {
213 |   double omega = tan(cutOff*sampTime*0.5);
214 |   k1 = (1-omega) / (1+omega);
215 |   k2 = 1 / (1+omega);
216 | }
217 | 
218 | Eigen::VectorXd FilterHighPass::filt(Eigen::VectorXd& x)
219 | {
220 |   y1 *= k1;
221 |   y1 += k2*(x - x1);
222 |   x1 = x;
223 |   return y1;
224 | }
225 | 
226 | #endif  // IIR_FILTER_H
227 | 


--------------------------------------------------------------------------------
/lib/kalman_filter.h:
--------------------------------------------------------------------------------
  1 | // Copyright 2020-2024 Stanislav Mikhel
  2 | 
  3 | /**
  4 |  * @file kalman_filter.h
  5 |  *
  6 |  * @brief Discrete Kalman filter implementation.
  7 |  */
  8 | 
  9 | #ifndef KALMAN_FILTER_H
 10 | #define KALMAN_FILTER_H
 11 | 
 12 | #include <eigen3/Eigen/Geometry>
 13 | 
 14 | /**
 15 |  * @brief Discrete Kalman filter implementation.
 16 |  */
 17 | class KalmanFilter {
 18 | public:
 19 |   /**
 20 |    * @brief Object constructor.
 21 |    * @param a state transition matrix.
 22 |    * @param b control input matrix.
 23 |    * @param c observation matrix.
 24 |    */
 25 |   KalmanFilter(Eigen::MatrixXd& a, Eigen::MatrixXd& b, Eigen::MatrixXd& c);
 26 | 
 27 |   /**
 28 |    * @brief Update covariance matrices.
 29 |    *
 30 |    * Defauls are unit matrices.
 31 |    * @param q covariance of the process noise.
 32 |    * @param r covariance of the observation noise.
 33 |    */
 34 |   void setCovariance(Eigen::MatrixXd& q, Eigen::MatrixXd& r);
 35 | 
 36 |   /**
 37 |    * @brief Reset filter state.
 38 |    *
 39 |    * Define initial system state.
 40 |    * @param x0 initial system state vector.
 41 |    */
 42 |   void reset(Eigen::VectorXd& x0);
 43 | 
 44 |   /**
 45 |    * @brief Estimate current system state.
 46 |    *
 47 |    * Time step is constant.
 48 |    * @param u control input vector.
 49 |    * @param y measured output.
 50 |    * @return expected system state.
 51 |    */
 52 |   Eigen::VectorXd step(Eigen::VectorXd& u, Eigen::VectorXd& y);
 53 | 
 54 |   /**
 55 |    * @brief Estimate current system state.
 56 |    *
 57 |    * Time step is variable.
 58 |    * @param u control input vector.
 59 |    * @param y measured output.
 60 |    * @param dt current time step.
 61 |    * @return expected system state.
 62 |    */
 63 |   Eigen::VectorXd step(Eigen::VectorXd& u, Eigen::VectorXd& y, double dt);
 64 | 
 65 | private:
 66 |   // System description
 67 |   Eigen::MatrixXd A, B, C;
 68 |   // Covariance
 69 |   Eigen::MatrixXd Q, R;
 70 |   // Intermediate matrices
 71 |   Eigen::MatrixXd P, K, Y, I, At;
 72 |   // System state
 73 |   Eigen::VectorXd X;
 74 |   // matrix size
 75 |   int nx, ny;
 76 | };  // KalmanFilter
 77 | 
 78 | 
 79 | // Initialization
 80 | KalmanFilter::KalmanFilter(Eigen::MatrixXd& a, Eigen::MatrixXd& b, Eigen::MatrixXd& c)
 81 |   : A(a)
 82 |   , B(b)
 83 |   , C(c)
 84 |   , Q(Eigen::MatrixXd(1, 1))
 85 |   , R(Eigen::MatrixXd(1, 1))
 86 |   , P(Eigen::MatrixXd(1, 1))
 87 |   , K(Eigen::MatrixXd(1, 1))
 88 |   , Y(Eigen::MatrixXd(1, 1))
 89 |   , I(Eigen::MatrixXd(1, 1))
 90 |   , At(a)
 91 |   , X(Eigen::VectorXd(1))
 92 | {
 93 |   nx = A.rows();
 94 |   ny = C.rows();
 95 |   P.resize(nx, nx);
 96 |   X.resize(nx);
 97 |   Q = Eigen::MatrixXd::Identity(nx, nx);
 98 |   R = Eigen::MatrixXd::Identity(ny, ny);
 99 |   K.resize(nx, ny);
100 |   Y.resize(ny, ny);
101 |   I = Eigen::MatrixXd::Identity(nx, nx);
102 | }
103 | 
104 | // Filter settings
105 | void KalmanFilter::setCovariance(Eigen::MatrixXd& q, Eigen::MatrixXd& r)
106 | {
107 |   Q = q;
108 |   R = r;
109 | }
110 | 
111 | // Define new initial state
112 | void KalmanFilter::reset(Eigen::VectorXd& x0)
113 | {
114 |   X = x0;
115 |   P = Eigen::MatrixXd::Zero(nx, nx);
116 | }
117 | 
118 | // State estimation for constant time step
119 | Eigen::VectorXd KalmanFilter::step(Eigen::VectorXd& u, Eigen::VectorXd& y)
120 | {
121 |   // predict
122 |   X = A * X + B * u;                       // i | i-1
123 |   P = A * P * A.transpose() + Q;           // i | i-1
124 |   // update
125 |   Y = C * (P * C.transpose()) + R;         // i
126 |   K = P * (C.transpose() * Y.inverse());   // i
127 | 
128 |   X += K * (y - C * X);                    // i | i
129 |   P = (I - K * C) * P;                     // i | i
130 | 
131 |   return X;
132 | }
133 | 
134 | // State estimation for variable time step
135 | Eigen::VectorXd KalmanFilter::step(Eigen::VectorXd& u, Eigen::VectorXd& y, double dt)
136 | {
137 |   // find current A matrix
138 |   At = I + dt*A;
139 | 
140 |   // predict
141 |   X = At * X + dt * (B * u);               // i | i-1
142 |   P = At * P * At.transpose() + Q;         // i | i-1
143 |   // update
144 |   Y = C * (P * C.transpose()) + R;         // i
145 |   K = P * (C.transpose() * Y.inverse());   // i
146 | 
147 |   X += K * (y - C * X);                    // i | i
148 |   P = (I - K * C) * P;                     // i | i
149 | 
150 |   return X;
151 | }
152 | 
153 | #endif  // KALMAN_FILTER_H
154 | 


--------------------------------------------------------------------------------
/lib/kalman_filter_continous.cpp:
--------------------------------------------------------------------------------
  1 | // Copyright 2020-2024 Stanislav Mikhel
  2 | 
  3 | #include "kalman_filter_continous.h"
  4 | 
  5 | #define EXP_TERMS 5
  6 | 
  7 | // Initialization
  8 | KalmanFilterContinous::KalmanFilterContinous(
  9 |   Eigen::MatrixXd& a, Eigen::MatrixXd& b, Eigen::MatrixXd& c)
 10 |   : Ad(a)
 11 |   , Bd(b)
 12 |   , Cd(c)
 13 |   , R(Eigen::MatrixXd(1, 1))
 14 |   , Rd(Eigen::MatrixXd(1, 1))
 15 |   , Qd(Eigen::MatrixXd(1, 1))
 16 |   , Rupd(Eigen::MatrixXd(1, 1))
 17 |   , AB(Eigen::MatrixXd(1, 1))
 18 |   , AQ(Eigen::MatrixXd(1, 1))
 19 |   , ABd(Eigen::MatrixXd(1, 1))
 20 |   , AQd(Eigen::MatrixXd(1, 1))
 21 |   , P(Eigen::MatrixXd(1, 1))
 22 |   , K(Eigen::MatrixXd(1, 1))
 23 |   , Y(Eigen::MatrixXd(1, 1))
 24 |   , I(Eigen::MatrixXd(1, 1))
 25 |   , X(Eigen::VectorXd(1))
 26 | {
 27 |   na = a.rows();
 28 |   nc = c.rows();
 29 |   nb = b.cols();
 30 |   P.resize(na, na);
 31 |   X.resize(na);
 32 |   Qd = Eigen::MatrixXd::Identity(na, na);
 33 |   R  = Eigen::MatrixXd::Identity(nc, nc);
 34 |   Rd = R;
 35 |   Rupd = R;
 36 |   K.resize(na, nc);
 37 |   Y.resize(nc, nc);
 38 |   I = Eigen::MatrixXd::Identity(na, na);
 39 |   // prepare matrix combinations
 40 |   // [A B;0 0]
 41 |   AB.resize(na+nb, na+nb);
 42 |   AB.setZero();
 43 |   AB.block(0, 0, na, na) = a;
 44 |   AB.block(0, na, na, nb) = b;
 45 |   ABd.resize(na+nb, na+nb);
 46 |   // [-A Q; 0 A']
 47 |   AQ.resize(na+na, na+na);
 48 |   AQ.block(0, 0, na, na) = -a;
 49 |   AQ.block(0, na, na, na) = Qd;
 50 |   AQ.block(na, na, na, na) = a.transpose();
 51 |   AQd.resize(na+na, na+na);
 52 | }
 53 | 
 54 | // Filter settings
 55 | void KalmanFilterContinous::setCovariance(Eigen::MatrixXd& q, Eigen::MatrixXd& r)
 56 | {
 57 |   AQ.block(0, na, na, na) = q;
 58 |   R = r;
 59 | }
 60 | 
 61 | // Define new initial state
 62 | void KalmanFilterContinous::reset(Eigen::VectorXd& x0)
 63 | {
 64 |   X = x0;
 65 |   P = Eigen::MatrixXd::Zero(na, na);
 66 | }
 67 | 
 68 | // State estimation for constant time step
 69 | Eigen::VectorXd KalmanFilterContinous::step(Eigen::VectorXd& u, Eigen::VectorXd& y, double dt)
 70 | {
 71 |   makeDiscrete(dt);
 72 | 
 73 |   // predict
 74 |   X = Ad * X + Bd * u;                       // i | i-1
 75 |   P = Ad * P * Ad.transpose() + Qd;           // i | i-1
 76 |   // update
 77 |   Y = Cd * (P * Cd.transpose()) + Rd;         // i
 78 |   K = P * (Cd.transpose() * Y.inverse());   // i
 79 | 
 80 |   X += K * (y - Cd * X);                    // i | i
 81 |   P = (I - K * Cd) * P;                     // i | i
 82 | 
 83 |   return X;
 84 | }
 85 | 
 86 | void KalmanFilterContinous::makeDiscrete(double dt)
 87 | {
 88 |   // find At and Bt
 89 |   // ABd = (AB*dt).exp();
 90 |   ABd = exponential(AB, dt);
 91 |   Ad = ABd.block(0, 0, na, na);
 92 |   Bd = ABd.block(0, na, na, nb);
 93 |   // find Qt and Rt
 94 |   Rd = Rupd / dt;
 95 |   // AQd = (AQ*dt).exp();
 96 |   AQd = exponential(AQ, dt);
 97 |   Qd = AQd.block(na, na, na, na).transpose() * AQd.block(0, na, na, na);
 98 | }
 99 | 
100 | void KalmanFilterContinous::updateR(Eigen::MatrixXd& m)
101 | {
102 |   Rupd = m * R * m.transpose();
103 | }
104 | 
105 | Eigen::MatrixXd KalmanFilterContinous::exponential(Eigen::MatrixXd& m, double dt)
106 | {
107 |   Eigen::MatrixXd res = Eigen::MatrixXd::Identity(m.rows(), m.cols());
108 |   Eigen::MatrixXd acc = res;
109 | 
110 |   for(int i = 1; i <= EXP_TERMS; i++) {
111 |     acc *= m * (dt/i);
112 |     res += acc;
113 |   }
114 | 
115 |   return res;
116 | }
117 | 


--------------------------------------------------------------------------------
/lib/kalman_filter_continous.h:
--------------------------------------------------------------------------------
 1 | // Copyright 2020-2024 Stanislav Mikhel
 2 | 
 3 | /**
 4 |  * @file kalman_filter_continous.h
 5 |  *
 6 |  * @brief Discrete Kalman filter implementation based on continous system description.
 7 |  */
 8 | 
 9 | #ifndef KALMAN_FILTER_CONTINOUS_H
10 | #define KALMAN_FILTER_CONTINOUS_H
11 | 
12 | #include <eigen3/Eigen/Geometry>
13 | 
14 | /**
15 |  * @brief Discrete Kalman filter implementation.
16 |  *
17 |  * Use matrices for continous system.
18 |  */
19 | class KalmanFilterContinous {
20 | public:
21 |   /**
22 |    * @brief Object constructor.
23 |    * @param a state transition matrix.
24 |    * @param b control input matrix.
25 |    * @param c observation matrix.
26 |    */
27 |   KalmanFilterContinous(Eigen::MatrixXd& a, Eigen::MatrixXd& b, Eigen::MatrixXd& c);
28 | 
29 |   /**
30 |    * @brief Update covariance matrices.
31 |    *
32 |    * Defauls are unit matrices.
33 |    * @param q covariance of the process noise.
34 |    * @param r covariance of the observation noise.
35 |    */
36 |   void setCovariance(Eigen::MatrixXd& q, Eigen::MatrixXd& r);
37 | 
38 |   /**
39 |    * @brief Reset filter state.
40 |    *
41 |    * Define initial system state.
42 |    * @param x0 initial system state vector.
43 |    */
44 |   void reset(Eigen::VectorXd& x0);
45 | 
46 |   /**
47 |    * @brief Estimate current system state.
48 |    *
49 |    * Time step is variable.
50 |    * @param u control input vector.
51 |    * @param y measured output.
52 |    * @param dt current time step.
53 |    * @return expected system state.
54 |    */
55 |   Eigen::VectorXd step(Eigen::VectorXd& u, Eigen::VectorXd& y, double dt);
56 | 
57 |   void updateR(Eigen::MatrixXd& m);
58 | 
59 |   Eigen::MatrixXd exponential(Eigen::MatrixXd& m, double dt);
60 | 
61 | private:
62 |   /**
63 |    * @brief Find discrete-time versions of matrices.
64 |    *
65 |    * @param dt current time step.
66 |    */
67 |   void makeDiscrete(double dt);
68 | 
69 |   // System description
70 |   Eigen::MatrixXd Ad, Bd, Cd;
71 |   // Covariance
72 |   Eigen::MatrixXd R, Rd, Qd, Rupd;
73 |   // Groups
74 |   Eigen::MatrixXd AB, AQ, ABd, AQd;
75 |   // Intermediate matrices
76 |   Eigen::MatrixXd P, K, Y, I;
77 |   // System state
78 |   Eigen::VectorXd X;
79 |   // matrix size
80 |   int na, nc, nb;
81 | };  // KalmanFilterContinous
82 | 
83 | #endif  // KALMAN_FILTER_CONTINOUS_H
84 | 


--------------------------------------------------------------------------------
/lib/momentum_observer.h:
--------------------------------------------------------------------------------
 1 | // Copyright 2020-2024 Stanislav Mikhel
 2 | 
 3 | /**
 4 |  * @file momentum_observer.h
 5 |  *
 6 |  * @brief Momentum observer class.
 7 |  *
 8 |  * Expected explicit robot dynamics matrices.
 9 |  */
10 | #ifndef MOMENTUM_OBSERVER_H
11 | #define MOMENTUM_OBSERVER_H
12 | 
13 | #include "external_observer.h"
14 | 
15 | #define ID_MomentumObserver 5
16 | 
17 | /**
18 |  * @brief Momentum observer from De Luca et al.
19 |  */
20 | class MomentumObserver final : public ExternalObserver {
21 | public:
22 |   /**
23 |    * @brief Object constructor.
24 |    * @param rd pointer to robot object.
25 |    * @param k vector of joint gains.
26 |    */
27 |   MomentumObserver(RobotDynamics *rd, VectorJ& k);
28 | 
29 |   /**
30 |    * @brief External torque estimation.
31 |    * @param q joint angle vector.
32 |    * @param qd joint velocity vector.
33 |    * @param tau joint torque vector.
34 |    * @param dt time step.
35 |    * @return external torque vector.
36 |    */
37 |   VectorJ getExternalTorque(VectorJ& q, VectorJ& qd, VectorJ& tau, double dt) override;
38 | 
39 |   /**
40 |    * @brief Observer settings.
41 |    * @param k vector of the joint gains.
42 |    */
43 |   inline void settings(VectorJ& k) { ko = k; }
44 | 
45 | private:
46 |   // accumulators
47 |   VectorJ sum, r;
48 |   // intermediate variables
49 |   VectorJ p, beta, torque, tprev;
50 |   // coefficients
51 |   VectorJ ko;
52 | };  // MomentumObserver
53 | 
54 | 
55 | // Initialization
56 | MomentumObserver::MomentumObserver(RobotDynamics *rd, VectorJ& k)
57 |   : ExternalObserver(rd, ID_MomentumObserver)
58 |   , sum(VectorJ(jointNo))
59 |   , r(VectorJ(jointNo))
60 |   , p(VectorJ(jointNo))
61 |   , beta(VectorJ(jointNo))
62 |   , torque(VectorJ(jointNo))
63 |   , tprev(VectorJ(jointNo))
64 |   , ko(k)
65 | {
66 |   settings(k);
67 | }
68 | 
69 | // Torque estimation
70 | VectorJ MomentumObserver::getExternalTorque(VectorJ& q, VectorJ& qd, VectorJ& tau, double dt)
71 | {
72 |   p = dyn->getM(q) * qd;     // M * qd
73 |   beta = dyn->getG(q) - dyn->getC(q, qd).transpose() * qd;  // G - C' * qd
74 | 
75 |   torque = tau - dyn->getFriction(qd);  // exclude friction
76 |   if(isRun) {
77 |     torque += r - beta;      // tau + r - beta
78 |     sum += 0.5 * dt * (torque + tprev);
79 |   } else {
80 |     torque -= beta;
81 |     r.setZero();
82 |     sum = p;
83 |     isRun = true;
84 |   }
85 |   tprev = torque;
86 | 
87 |   p -= sum;                 // p - integral - p0
88 | 
89 |   // elementwise product
90 |   for(int i = 0; i < jointNo; i++) {
91 |     r(i) = ko(i) * p(i);
92 |     torque(i) = r(i);
93 |   }
94 | 
95 |   return torque;
96 | }
97 | 
98 | #endif  // MOMENTUM_OBSERVER_H
99 | 


--------------------------------------------------------------------------------
/lib/momentum_observer_rnea.h:
--------------------------------------------------------------------------------
  1 | // Copyright 2020-2024 Stanislav Mikhel
  2 | 
  3 | /**
  4 |  * @file momentum_observer_rnea.h
  5 |  *
  6 |  * @brief Momentum observer class.
  7 |  *
  8 |  * Expected robot dynamics in form of RNEA.
  9 |  */
 10 | #ifndef MOMENTUM_OBSERVER_RNEA_H
 11 | #define MOMENTUM_OBSERVER_RNEA_H
 12 | 
 13 | #include "external_observer.h"
 14 | 
 15 | #define ID_MomentumObserverRnea 25
 16 | 
 17 | /**
 18 |  * @brief Momentum observer from De Luca et al.
 19 |  *
 20 |  * Find dynamics using RNEA technique.
 21 |  */
 22 | class MomentumObserverRnea final : public ExternalObserverRnea {
 23 | public:
 24 |   /**
 25 |    * @brief Object constructor.
 26 |    * @param rd pointer to robot object.
 27 |    * @param k vector of joint gains.
 28 |    */
 29 |   MomentumObserverRnea(RobotDynamicsRnea *rd, VectorJ& k);
 30 | 
 31 |   /**
 32 |    * @brief External torque estimation.
 33 |    * @param q joint angle vector.
 34 |    * @param qd joint velocity vector.
 35 |    * @param tau joint torque vector.
 36 |    * @param dt time step.
 37 |    * @return external torque vector.
 38 |    */
 39 |   VectorJ getExternalTorque(VectorJ& q, VectorJ& qd, VectorJ& tau, double dt) override;
 40 | 
 41 |   /**
 42 |    * @brief Observer settings.
 43 |    * @param k vector of the joint gains.
 44 |    */
 45 |   inline void settings(VectorJ& k) { ko = k; }
 46 | 
 47 | private:
 48 |   // accumulators
 49 |   VectorJ sum, r, zero;
 50 |   // intermediate variables
 51 |   VectorJ p, beta, torque, tprev;
 52 |   // coefficients
 53 |   VectorJ ko;
 54 | };  // MomentumObserverRnea
 55 | 
 56 | 
 57 | // Initialization
 58 | MomentumObserverRnea::MomentumObserverRnea(RobotDynamicsRnea *rd, VectorJ& k)
 59 |   : ExternalObserverRnea(rd, ID_MomentumObserverRnea)
 60 |   , sum(VectorJ(jointNo))
 61 |   , r(VectorJ(jointNo))
 62 |   , zero(VectorJ::Zero(jointNo))
 63 |   , p(VectorJ(jointNo))
 64 |   , beta(VectorJ(jointNo))
 65 |   , torque(VectorJ(jointNo))
 66 |   , tprev(VectorJ(jointNo))
 67 |   , ko(k)
 68 | {
 69 |   settings(k);
 70 | }
 71 | 
 72 | // Torque estimation
 73 | VectorJ MomentumObserverRnea::getExternalTorque(VectorJ& q, VectorJ& qd, VectorJ& tau, double dt)
 74 | {
 75 |   p = dyn->rnea(q, zero, qd);     // M * qd
 76 |   beta = dyn->rnea(q, zero, zero, GRAVITY)- dyn->tranCqd(q, qd);  // G - C' * qd
 77 | 
 78 |   torque = tau - dyn->getFriction(qd);  // exclude friction
 79 |   if(isRun) {
 80 |     torque += r - beta;      // tau + r - beta
 81 |     sum += 0.5 * dt * (torque + tprev);
 82 |   } else {
 83 |     torque -= beta;
 84 |     r.setZero();
 85 |     sum = p;
 86 |     isRun = true;
 87 |   }
 88 |   tprev = torque;
 89 | 
 90 |   p -= sum;                 // p - integral - p0
 91 | 
 92 |   // elementwise product
 93 |   for(int i = 0; i < jointNo; i++) {
 94 |     r(i) = ko(i) * p(i);
 95 |     torque(i) = r(i);   // reuse variable
 96 |   }
 97 | 
 98 |   return torque;
 99 | }
100 | 
101 | #endif  // MOMENTUM_OBSERVER_RNEA_H
102 | 


--------------------------------------------------------------------------------
/lib/robot_dynamics_rnea.cpp:
--------------------------------------------------------------------------------
 1 | // Copyright 2020-2024 Stanislav Mikhel
 2 | 
 3 | #include "external_observer.h"
 4 | 
 5 | RobotDynamicsRnea::RobotDynamicsRnea()
 6 |   : RobotDynamicsBase()
 7 |   , _qext(VectorJ(1))
 8 |   , _p0(VectorJ(1))
 9 |   , _zero(VectorJ(1))
10 |   , _sum(VectorJ(1))
11 |   , _M(MatrixJ(1, 1))
12 | {
13 | }
14 | 
15 | // Find C^T * qd
16 | VectorJ RobotDynamicsRnea::tranCqd(VectorJ& q, VectorJ& qd)
17 | {
18 |   // TODO: call it once
19 |   int N = jointNo();
20 |   _zero.resize(N);
21 |   _zero.setZero();
22 |   _p0 = rnea(q, _zero, qd);  // M * qd
23 |   _sum.resize(N);
24 |   _sum.setZero();
25 | 
26 |   for(int i = 0; i < N; i++) {
27 |     _qext = q;
28 |     _qext(i) += delta;
29 |     _sum += (rnea(_qext, _zero, qd) - _p0) * (qd(i) / delta);
30 |   }
31 |   _sum -= rnea(q, qd, _zero);  // M'*qd - C*qd
32 | 
33 |   return _sum;
34 | }
35 | 
36 | // Create M matrix from sequence of RNEA calls
37 | MatrixJ RobotDynamicsRnea::getM(VectorJ& q)
38 | {
39 |   // TODO: call it once
40 |   int N = jointNo();
41 |   _zero.resize(N);
42 |   _zero.setZero();
43 |   _M.resize(N, N);
44 |   _qext.resize(N);
45 | 
46 |   for(int i = 0; i < N; i++) {
47 |     _qext.setZero();  // use _qext to choose column
48 |     _qext(i) = 1;
49 |     _p0 = rnea(q, _zero, _qext);  // use _p0 to save matrix column
50 |     for(int j = 0; j < N; j++)
51 |       _M(j, i) = _p0(j);
52 |   }
53 | 
54 |   return _M;
55 | }
56 | 


--------------------------------------------------------------------------------
/lib/sliding_mode_observer.h:
--------------------------------------------------------------------------------
  1 | // Copyright 2020-2024 Stanislav Mikhel
  2 | 
  3 | /**
  4 |  * @file sliding_mode_observer.h
  5 |  *
  6 |  * @brief Sliding mode observer class.
  7 |  *
  8 |  * Expected explicit robot dynamics matrices.
  9 |  */
 10 | #ifndef SLIDING_MODE_OBSERVER_H
 11 | #define SLIDING_MODE_OBSERVER_H
 12 | 
 13 | #include "external_observer.h"
 14 | 
 15 | #define ID_SlidingModeObserver 3
 16 | 
 17 | /**
 18 |  * @brief Sliding mode observer from Garofalo et. al.
 19 |  *
 20 |  * "Sliding mode momentum observers for estimation of external torques
 21 |  *  and joint acceleration", 2019
 22 |  */
 23 | class SlidingModeObserver final : public ExternalObserver {
 24 | public:
 25 |   /**
 26 |    * @brief Object constructor.
 27 |    * @param rd pointer to the robot object.
 28 |    * @param t1 ...
 29 |    * @param s1 ...
 30 |    * @param t2 ...
 31 |    * @param s2 ...
 32 |    */
 33 |   SlidingModeObserver(RobotDynamics* rd, VectorJ& t1, VectorJ& s1, VectorJ& t2, VectorJ& s2);
 34 | 
 35 |   /**
 36 |    * @brief External torque estimation.
 37 |    * @param q joint angle vector.
 38 |    * @param qd joint velocity vector.
 39 |    * @param tau joint torque vector.
 40 |    * @param dt time step.
 41 |    * @return external torque vector.
 42 |    */
 43 |   VectorJ getExternalTorque(VectorJ& q, VectorJ& qd, VectorJ& tau, double dt) override;
 44 | 
 45 |   /**
 46 |    * @brief Observer settings.
 47 |    * @param t1 ...
 48 |    * @param s1 ...
 49 |    * @param t2 ...
 50 |    * @param s2 ...
 51 |    */
 52 |   void settings(VectorJ& t1, VectorJ& s1, VectorJ& t2, VectorJ& s2);
 53 | 
 54 | private:
 55 |   const double BIG = 50.0;  /**< Map tanh to sign. */
 56 | 
 57 |   // Temporary objects
 58 |   VectorJ sigma, p_hat;
 59 |   VectorJ p, spp, dp_hat, torque, dsigma;
 60 |   VectorJ T1, S1, T2, S2;
 61 | };  // SlidingModeObserver
 62 | 
 63 | 
 64 | // Initialization
 65 | SlidingModeObserver::SlidingModeObserver(RobotDynamics* rd,
 66 |                                 VectorJ& t1, VectorJ& s1, VectorJ& t2, VectorJ& s2)
 67 |   : ExternalObserver(rd, ID_SlidingModeObserver)
 68 |   , sigma(VectorJ(jointNo))
 69 |   , p_hat(VectorJ(jointNo))
 70 |   , p(VectorJ(jointNo))
 71 |   , spp(VectorJ(jointNo))
 72 |   , dp_hat(VectorJ(jointNo))
 73 |   , torque(VectorJ(jointNo))
 74 |   , dsigma(VectorJ(jointNo))
 75 |   , T1(VectorJ(jointNo)), S1(VectorJ(jointNo))
 76 |   , T2(VectorJ(jointNo)), S2(VectorJ(jointNo))
 77 | {
 78 |   settings(t1, s1, t2, s2);
 79 | }
 80 | 
 81 | // Update parameters
 82 | void SlidingModeObserver::settings(VectorJ& t1, VectorJ& s1, VectorJ& t2, VectorJ& s2)
 83 | {
 84 |   T1 = t1;
 85 |   S1 = s1;
 86 |   T2 = t2;
 87 |   S2 = s2;
 88 | }
 89 | 
 90 | // External torque
 91 | VectorJ SlidingModeObserver::getExternalTorque(VectorJ& q, VectorJ& qd, VectorJ& tau, double dt)
 92 | {
 93 |   p = dyn->getM(q) * qd;
 94 |   torque = tau - dyn->getFriction(qd);
 95 | 
 96 |   if(!isRun) {
 97 |     sigma.setZero();
 98 |     p_hat = p;
 99 |     isRun = true;
100 |   }
101 | 
102 |   p = p_hat - p;  // reuse p
103 |   for(int i = 0; i < jointNo; i++) {
104 |     spp(i) = tanh(p(i)*BIG);
105 |   }
106 |   // for(int i = 0; i < jointNo; i++) spp(i) = (p(i) > 0 ? 1 : (p(i) < 0 ? -1 : 0));
107 | 
108 |   dp_hat = torque + dyn->getC(q, qd).transpose()*qd - dyn->getG(q) + sigma;
109 |   for(int i = 0; i < jointNo; i++) {
110 |     // - T2*p
111 |     dp_hat(i) -= T2(i)*p(i);
112 |     // - sqrt(abs(p)).*T1*spp
113 |     dp_hat(i) -= sqrt( fabs(p(i)) ) * T1(i) * spp(i);
114 |   }
115 |   // dsigma
116 |   for(int i = 0; i < jointNo; i++) {
117 |     dsigma(i) = -S1(i)*spp(i) - S2(i)*p(i);
118 |   }
119 | 
120 |   p = sigma;     // reuse to save result
121 |   p_hat += dt*dp_hat;
122 |   sigma += dt*dsigma;
123 | 
124 |   return p;
125 | }
126 | 
127 | #endif  // SLIDING_MODE_OBSERVER_H
128 | 


--------------------------------------------------------------------------------
/lib/sliding_mode_observer_rnea.h:
--------------------------------------------------------------------------------
  1 | // Copyright 2020-2024 Stanislav Mikhel
  2 | 
  3 | /**
  4 |  * @file sliding_mode_observer_rnea.h
  5 |  *
  6 |  * @brief Sliding mode observer class.
  7 |  *
  8 |  * Expected robot dynamics in form of RNEA.
  9 |  */
 10 | #ifndef SLIDING_MODE_OBSERVER_RNEA_H
 11 | #define SLIDING_MODE_OBSERVER_RNEA_H
 12 | 
 13 | #include "external_observer.h"
 14 | 
 15 | #define ID_SlidingModeObserverRnea 23
 16 | 
 17 | /**
 18 |  * @brief Sliding mode observer from ...
 19 |  *
 20 |  * Use RNEA for dynamics.
 21 |  */
 22 | class SlidingModeObserverRnea final : public ExternalObserverRnea {
 23 | public:
 24 |   /**
 25 |    * @brief Object constructor.
 26 |    * @param rd pointer to the robot object.
 27 |    * @param t1 ...
 28 |    * @param s1 ...
 29 |    * @param t2 ...
 30 |    * @param s2 ...
 31 |    */
 32 |   SlidingModeObserverRnea(
 33 |     RobotDynamicsRnea* rd, VectorJ& t1, VectorJ& s1, VectorJ& t2, VectorJ& s2);
 34 | 
 35 |   /**
 36 |    * @brief External torque estimation.
 37 |    * @param q joint angle vector.
 38 |    * @param qd joint velocity vector.
 39 |    * @param tau joint torque vector.
 40 |    * @param dt time step.
 41 |    * @return external torque vector.
 42 |    */
 43 |   VectorJ getExternalTorque(VectorJ& q, VectorJ& qd, VectorJ& tau, double dt) override;
 44 | 
 45 |   /**
 46 |    * @brief Observer settings.
 47 |    * @param t1 ...
 48 |    * @param s1 ...
 49 |    * @param t2 ...
 50 |    * @param s2 ...
 51 |    */
 52 |   void settings(VectorJ& t1, VectorJ& s1, VectorJ& t2, VectorJ& s2);
 53 | 
 54 | private:
 55 |   const double BIG = 50.0;  /**< Map tanh to sign. */
 56 | 
 57 |   // Temporary objects
 58 |   VectorJ sigma, p_hat;
 59 |   VectorJ p, spp, dp_hat, torque, dsigma, zeros;
 60 |   VectorJ T1, S1, T2, S2;
 61 | };  // SlidingModeObserverRnea
 62 | 
 63 | 
 64 | // Initialization
 65 | SlidingModeObserverRnea::SlidingModeObserverRnea(
 66 |   RobotDynamicsRnea* rd, VectorJ& t1, VectorJ& s1, VectorJ& t2, VectorJ& s2)
 67 |   : ExternalObserverRnea(rd, ID_SlidingModeObserverRnea)
 68 |   , sigma(VectorJ(jointNo))
 69 |   , p_hat(VectorJ(jointNo))
 70 |   , p(VectorJ(jointNo))
 71 |   , spp(VectorJ(jointNo))
 72 |   , dp_hat(VectorJ(jointNo))
 73 |   , torque(VectorJ(jointNo))
 74 |   , dsigma(VectorJ(jointNo))
 75 |   , zeros(VectorJ::Zero(jointNo))
 76 |   , T1(VectorJ(jointNo)), S1(VectorJ(jointNo))
 77 |   , T2(VectorJ(jointNo)), S2(VectorJ(jointNo))
 78 | {
 79 |   settings(t1, s1, t2, s2);
 80 | }
 81 | 
 82 | // Update settings
 83 | void SlidingModeObserverRnea::settings(VectorJ& t1, VectorJ& s1, VectorJ& t2, VectorJ& s2)
 84 | {
 85 |   T1 = t1;
 86 |   S1 = s1;
 87 |   T2 = t2;
 88 |   S2 = s2;
 89 | }
 90 | 
 91 | // External torque
 92 | VectorJ SlidingModeObserverRnea::getExternalTorque(VectorJ& q, VectorJ& qd, VectorJ& tau, double dt)
 93 | {
 94 |   p = dyn->rnea(q, zeros, qd);
 95 |   torque = tau - dyn->getFriction(qd);
 96 | 
 97 |   if(!isRun) {
 98 |     sigma.setZero();
 99 |     p_hat = p;
100 |     isRun = true;
101 |   }
102 | 
103 |   p = p_hat - p;  // reuse p
104 |   for(int i = 0; i < jointNo; i++) {
105 |     spp(i) = tanh(p(i)*BIG);
106 |   }
107 | 
108 |   dp_hat = torque + dyn->tranCqd(q, qd) - dyn->rnea(q, zeros, zeros, GRAVITY) + sigma;
109 |   for(int i = 0; i < jointNo; i++) {
110 |     // - T2*p
111 |     dp_hat(i) -= T2(i)*p(i);
112 |     // - sqrt(abs(p)).*T1*spp
113 |     dp_hat(i) -= sqrt( fabs(p(i)) ) * T1(i) * spp(i);
114 |   }
115 |   // dsigma
116 |   for(int i = 0; i < jointNo; i++) {
117 |     dsigma(i) = -S1(i)*spp(i) - S2(i)*p(i);
118 |   }
119 | 
120 |   p = sigma;     // reuse to save result
121 |   p_hat += dt*dp_hat;
122 |   sigma += dt*dsigma;
123 | 
124 |   return p;
125 | }
126 | 
127 | #endif  // SLIDING_MODE_OBSERVER_RNEA_H
128 | 


--------------------------------------------------------------------------------
/matlab/Makefile:
--------------------------------------------------------------------------------
 1 | CC = g++
 2 | CFLAGS = -Wall
 3 | INCDIR = -I /usr/include/eigen3
 4 |       
 5 | SRC = bin/observers.o bin/fcont.o 
 6 |       
 7 | .PHONY: all clear test
 8 | 
 9 | # get information from csv file
10 | all: ${SRC}
11 | 	${CC} ${SRC} -shared -o observers.so 
12 | 
13 | # remove object files
14 | clear:
15 | 	rm -f bin/*.o
16 | 
17 | # make object files
18 | 
19 | # dynamic library
20 | bin/observers.o: observers.cpp
21 | 	${CC} ${CFLAGS} observers.cpp -c -fPIC -o $@
22 | 	
23 | bin/fcont.o: ../lib/kalman_filter_continous.cpp
24 | 	${CC} ${CFLAGS} ../lib/kalman_filter_continous.cpp -c -fPIC -o $@	
25 | 	
26 | # call the dynamic library from cpp 
27 | test: test_lib.cpp
28 | 	${CC} ${CFLAGS} -Wl,-rpath=$(shell pwd) test_lib.cpp -o test_lib  -L. -l:observers.so
29 | 	./test_lib
30 | 
31 | 


--------------------------------------------------------------------------------
/matlab/README.md:
--------------------------------------------------------------------------------
1 | # Matlab wrapper
2 | 
3 | Contains an example of the Matlab wrapper generation for a simple 2 link manipulator. Call _make_ to build the dynamic library and use _test.m_ to check the result. 
4 | 
5 | Build code with _make test_ to call the dynamic library from C++ file.
6 | 
7 | 


--------------------------------------------------------------------------------
/matlab/bin/empty.txt:
--------------------------------------------------------------------------------
1 | Save objects here


--------------------------------------------------------------------------------
/matlab/input.csv:
--------------------------------------------------------------------------------
  1 | 0,0,0,1.3,0.8,9.81,2.4525
  2 | 0.01,0.0129996,0.00799991,1.29989,0.799974,9.77942,2.44379
  3 | 0.02,0.0259971,0.0159993,1.29956,0.799898,9.74652,2.434
  4 | 0.03,0.0389901,0.0239977,1.29901,0.79977,9.71132,2.42313
  5 | 0.04,0.0519766,0.0319945,1.29824,0.79959,9.67382,2.41119
  6 | 0.05,0.0649542,0.0399893,1.29725,0.79936,9.63405,2.39817
  7 | 0.06,0.0779209,0.0479816,1.29605,0.799079,9.59201,2.38409
  8 | 0.07,0.0908745,0.0559707,1.29462,0.798746,9.54773,2.36894
  9 | 0.08,0.103813,0.0639563,1.29298,0.798362,9.50124,2.35274
 10 | 0.09,0.116733,0.0719378,1.29111,0.797927,9.45254,2.33549
 11 | 0.1,0.129634,0.0799147,1.28903,0.797441,9.40167,2.3172
 12 | 0.11,0.142513,0.0878865,1.28673,0.796904,9.34865,2.29788
 13 | 0.12,0.155368,0.0958526,1.28421,0.796316,9.29351,2.27755
 14 | 0.13,0.168197,0.103813,1.28148,0.795677,9.23628,2.2562
 15 | 0.14,0.180997,0.111766,1.27853,0.794988,9.177,2.23387
 16 | 0.15,0.193767,0.119712,1.27536,0.794247,9.1157,2.21055
 17 | 0.16,0.206503,0.127651,1.27198,0.793455,9.05241,2.18627
 18 | 0.17,0.219205,0.135581,1.26838,0.792613,8.98717,2.16104
 19 | 0.18,0.23187,0.143503,1.26457,0.79172,8.92003,2.13487
 20 | 0.19,0.244496,0.151415,1.26055,0.790776,8.85101,2.10779
 21 | 0.2,0.257081,0.159318,1.25631,0.789782,8.78017,2.0798
 22 | 0.21,0.269622,0.167211,1.25186,0.788737,8.70755,2.05094
 23 | 0.22,0.282117,0.175093,1.24719,0.787642,8.63319,2.02121
 24 | 0.23,0.294565,0.182964,1.24232,0.786496,8.55714,1.99065
 25 | 0.24,0.306963,0.190823,1.23724,0.7853,8.47945,1.95926
 26 | 0.25,0.319309,0.198669,1.23195,0.784053,8.40017,1.92707
 27 | 0.26,0.331601,0.206503,1.22645,0.782757,8.31934,1.89411
 28 | 0.27,0.343837,0.214324,1.22074,0.78141,8.23703,1.86039
 29 | 0.28,0.356015,0.222131,1.21482,0.780013,8.15327,1.82594
 30 | 0.29,0.368133,0.229924,1.20871,0.778567,8.06813,1.79078
 31 | 0.3,0.380188,0.237703,1.20238,0.77707,7.98167,1.75494
 32 | 0.31,0.39218,0.245466,1.19586,0.775524,7.89392,1.71844
 33 | 0.32,0.404105,0.253213,1.18913,0.773928,7.80497,1.68131
 34 | 0.33,0.415962,0.260944,1.1822,0.772283,7.71485,1.64356
 35 | 0.34,0.427748,0.268658,1.17507,0.770588,7.62363,1.60524
 36 | 0.35,0.439462,0.276356,1.16774,0.768844,7.53136,1.56636
 37 | 0.36,0.451102,0.284035,1.16021,0.767051,7.43811,1.52695
 38 | 0.37,0.462666,0.291697,1.15249,0.765209,7.34394,1.48704
 39 | 0.38,0.474151,0.299339,1.14458,0.763317,7.2489,1.44665
 40 | 0.39,0.485557,0.306963,1.13647,0.761377,7.15306,1.40581
 41 | 0.4,0.49688,0.314567,1.12816,0.759388,7.05647,1.36456
 42 | 0.41,0.508119,0.32215,1.11967,0.757351,6.9592,1.32291
 43 | 0.42,0.519273,0.329713,1.11099,0.755265,6.86131,1.28089
 44 | 0.43,0.530339,0.337255,1.10212,0.753131,6.76286,1.23853
 45 | 0.44,0.541315,0.344776,1.09307,0.750948,6.6639,1.19586
 46 | 0.45,0.552199,0.352274,1.08383,0.748717,6.56452,1.15291
 47 | 0.46,0.562991,0.35975,1.0744,0.746439,6.46475,1.1097
 48 | 0.47,0.573687,0.367203,1.0648,0.744113,6.36467,1.06627
 49 | 0.48,0.584286,0.374632,1.05501,0.741739,6.26433,1.02263
 50 | 0.49,0.594786,0.382037,1.04505,0.739317,6.1638,0.97882
 51 | 0.5,0.605186,0.389418,1.03491,0.736849,6.06313,0.934862
 52 | 0.51,0.615484,0.396774,1.02459,0.734333,5.96239,0.890784
 53 | 0.52,0.625678,0.404105,1.01411,0.73177,5.86164,0.846612
 54 | 0.53,0.635766,0.41141,1.00345,0.72916,5.76092,0.802373
 55 | 0.54,0.645746,0.418688,0.992618,0.726504,5.66031,0.758094
 56 | 0.55,0.655617,0.425939,0.981621,0.723801,5.55987,0.713799
 57 | 0.56,0.665378,0.433164,0.970459,0.721052,5.45964,0.669516
 58 | 0.57,0.675026,0.44036,0.959132,0.718257,5.35968,0.625267
 59 | 0.58,0.68456,0.447529,0.947643,0.715416,5.26006,0.58108
 60 | 0.59,0.693978,0.454669,0.935995,0.712529,5.16082,0.536977
 61 | 0.6,0.703279,0.461779,0.924188,0.709596,5.06202,0.492984
 62 | 0.61,0.712462,0.46886,0.912224,0.706618,4.96371,0.449123
 63 | 0.62,0.721523,0.475911,0.900107,0.703595,4.86595,0.405419
 64 | 0.63,0.730463,0.482932,0.887838,0.700526,4.76879,0.361893
 65 | 0.64,0.73928,0.489922,0.875418,0.697413,4.67228,0.318569
 66 | 0.65,0.747971,0.49688,0.862851,0.694255,4.57647,0.275467
 67 | 0.66,0.756536,0.503807,0.850137,0.691053,4.48141,0.23261
 68 | 0.67,0.764973,0.510701,0.83728,0.687807,4.38715,0.190018
 69 | 0.68,0.773281,0.517563,0.824282,0.684516,4.29373,0.147712
 70 | 0.69,0.781459,0.524391,0.811144,0.681182,4.2012,0.105712
 71 | 0.7,0.789504,0.531186,0.797869,0.677804,4.10961,0.0640359
 72 | 0.71,0.797415,0.537947,0.78446,0.674383,4.019,0.0227041
 73 | 0.72,0.805192,0.544674,0.770918,0.670918,3.92941,-0.0182658
 74 | 0.73,0.812833,0.551365,0.757245,0.667411,3.84088,-0.0588558
 75 | 0.74,0.820337,0.558022,0.743445,0.663861,3.75346,-0.0990488
 76 | 0.75,0.827702,0.564642,0.729518,0.660268,3.66719,-0.138828
 77 | 0.76,0.834927,0.571227,0.715469,0.656634,3.5821,-0.178178
 78 | 0.77,0.842011,0.577775,0.701299,0.652957,3.49823,-0.217082
 79 | 0.78,0.848953,0.584286,0.68701,0.649238,3.41562,-0.255525
 80 | 0.79,0.855751,0.59076,0.672605,0.645478,3.33431,-0.293494
 81 | 0.8,0.862404,0.597195,0.658086,0.641677,3.25432,-0.330973
 82 | 0.81,0.868912,0.603593,0.643457,0.637834,3.17569,-0.367951
 83 | 0.82,0.875273,0.609952,0.628718,0.633951,3.09845,-0.404413
 84 | 0.83,0.881486,0.616272,0.613873,0.630027,3.02263,-0.440347
 85 | 0.84,0.88755,0.622552,0.598925,0.626063,2.94827,-0.475742
 86 | 0.85,0.893464,0.628793,0.583875,0.622058,2.87539,-0.510585
 87 | 0.86,0.899227,0.634993,0.568726,0.618014,2.80401,-0.544868
 88 | 0.87,0.904838,0.641153,0.553482,0.61393,2.73417,-0.578578
 89 | 0.88,0.910297,0.647272,0.538144,0.609807,2.66589,-0.611707
 90 | 0.89,0.915601,0.653349,0.522715,0.605645,2.59919,-0.644244
 91 | 0.9,0.920751,0.659385,0.507197,0.601445,2.5341,-0.676182
 92 | 0.91,0.925745,0.665378,0.491594,0.597205,2.47064,-0.707511
 93 | 0.92,0.930582,0.671329,0.475908,0.592928,2.40883,-0.738224
 94 | 0.93,0.935263,0.677236,0.460141,0.588612,2.34869,-0.768314
 95 | 0.94,0.939785,0.683101,0.444297,0.584259,2.29024,-0.797772
 96 | 0.95,0.944148,0.688921,0.428377,0.579869,2.23349,-0.826594
 97 | 0.96,0.948352,0.694698,0.412386,0.575441,2.17847,-0.854772
 98 | 0.97,0.952396,0.70043,0.396324,0.570977,2.12518,-0.882301
 99 | 0.98,0.956278,0.706117,0.380196,0.566476,2.07365,-0.909176
100 | 0.99,0.959999,0.71176,0.364003,0.561939,2.02389,-0.935391
101 | 1,0.963558,0.717356,0.347748,0.557365,1.47591,-1.46094
102 | 1.01,0.966954,0.722907,0.331435,0.552757,1.42972,-1.48582
103 | 1.02,0.970187,0.728411,0.315066,0.548112,1.38533,-1.51003
104 | 1.03,0.973255,0.733869,0.298644,0.543433,1.34276,-1.53357
105 | 1.04,0.976159,0.73928,0.282171,0.538719,1.30202,-1.55643
106 | 1.05,0.978899,0.744643,0.265651,0.53397,1.26311,-1.5786
107 | 1.06,0.981472,0.749959,0.249085,0.529188,1.22604,-1.60009
108 | 1.07,0.98388,0.755227,0.232478,0.524371,1.19082,-1.62089
109 | 1.08,0.986122,0.760446,0.215831,0.519521,1.15745,-1.64101
110 | 1.09,0.988197,0.765617,0.199148,0.514637,1.12594,-1.66043
111 | 1.1,0.990105,0.770739,0.182431,0.509721,1.0963,-1.67916
112 | 1.11,0.991845,0.775811,0.165683,0.504772,1.06852,-1.6972
113 | 1.12,0.993418,0.780834,0.148908,0.499791,1.04262,-1.71454
114 | 1.13,0.994823,0.785807,0.132107,0.494777,1.01859,-1.73119
115 | 1.14,0.99606,0.79073,0.115284,0.489732,0.996432,-1.74715
116 | 1.15,0.997129,0.795602,0.0984409,0.484656,0.976156,-1.7624
117 | 1.16,0.998029,0.800423,0.0815816,0.479549,0.957756,-1.77696
118 | 1.17,0.99876,0.805192,0.0647085,0.474411,0.941234,-1.79083
119 | 1.18,0.999323,0.809911,0.0478244,0.469242,0.926589,-1.804
120 | 1.19,0.999717,0.814577,0.0309323,0.464044,0.913818,-1.81647
121 | 1.2,0.999942,0.819192,0.014035,0.458816,0.902921,-1.82825
122 | 1.21,0.999998,0.823753,-0.00286477,0.453559,0.893894,-1.83933
123 | 1.22,0.999884,0.828263,-0.019764,0.448272,0.886733,-1.84972
124 | 1.23,0.999602,0.832719,-0.0366599,0.442957,0.881436,-1.85941
125 | 1.24,0.999151,0.837122,-0.0535496,0.437613,0.877997,-1.86841
126 | 1.25,0.998531,0.841471,-0.0704303,0.432242,0.876412,-1.87672
127 | 1.26,0.997743,0.845766,-0.087299,0.426843,0.876675,-1.88434
128 | 1.27,0.996785,0.850008,-0.104153,0.421416,0.87878,-1.89127
129 | 1.28,0.99566,0.854195,-0.120989,0.415963,0.88272,-1.89752
130 | 1.29,0.994366,0.858327,-0.137805,0.410483,0.888488,-1.90308
131 | 1.3,0.992904,0.862404,-0.154598,0.404976,0.896076,-1.90795
132 | 1.31,0.991274,0.866426,-0.171365,0.399444,0.905476,-1.91214
133 | 1.32,0.989476,0.870393,-0.188102,0.393886,0.91668,-1.91564
134 | 1.33,0.987512,0.874304,-0.204808,0.388303,0.929678,-1.91847
135 | 1.34,0.98538,0.878159,-0.221479,0.382695,0.94446,-1.92062
136 | 1.35,0.983082,0.881958,-0.238113,0.377063,0.961017,-1.92209
137 | 1.36,0.980618,0.8857,-0.254706,0.371406,0.979336,-1.92289
138 | 1.37,0.977988,0.889386,-0.271257,0.365726,0.999408,-1.92301
139 | 1.38,0.975193,0.893015,-0.287761,0.360022,1.02122,-1.92246
140 | 1.39,0.972233,0.896586,-0.304217,0.354295,1.04476,-1.92124
141 | 1.4,0.969109,0.9001,-0.320622,0.348546,1.07001,-1.91936
142 | 1.41,0.965821,0.903557,-0.336972,0.342774,1.09697,-1.91681
143 | 1.42,0.96237,0.906956,-0.353266,0.336981,1.12562,-1.91359
144 | 1.43,0.958756,0.910297,-0.3695,0.331165,1.15594,-1.90972
145 | 1.44,0.95498,0.913579,-0.385671,0.325329,1.18792,-1.90518
146 | 1.45,0.951043,0.916803,-0.401777,0.319472,1.22154,-1.89999
147 | 1.46,0.946945,0.919968,-0.417815,0.313594,1.25679,-1.89414
148 | 1.47,0.942687,0.923075,-0.433783,0.307696,1.29366,-1.88763
149 | 1.48,0.938269,0.926122,-0.449677,0.301779,1.33212,-1.88048
150 | 1.49,0.933693,0.92911,-0.465495,0.295842,1.37215,-1.87268
151 | 1.5,0.92896,0.932039,-0.481235,0.289886,1.41375,-1.86422
152 | 1.51,0.924069,0.934908,-0.496893,0.283912,1.4569,-1.85513
153 | 1.52,0.919022,0.937717,-0.512468,0.27792,1.50156,-1.84539
154 | 1.53,0.91382,0.940466,-0.527955,0.271909,1.54773,-1.83501
155 | 1.54,0.908463,0.943155,-0.543354,0.265882,1.59539,-1.824
156 | 1.55,0.902953,0.945784,-0.558661,0.259837,1.64451,-1.81234
157 | 1.56,0.89729,0.948352,-0.573873,0.253776,1.69508,-1.80006
158 | 1.57,0.891476,0.950859,-0.588988,0.247698,1.74707,-1.78714
159 | 1.58,0.885511,0.953306,-0.604004,0.241605,1.80046,-1.7736
160 | 1.59,0.879396,0.955692,-0.618918,0.235496,1.85523,-1.75943
161 | 1.6,0.873133,0.958016,-0.633727,0.229372,1.91135,-1.74463
162 | 1.61,0.866722,0.960279,-0.648429,0.223234,1.96881,-1.72922
163 | 1.62,0.860165,0.96248,-0.663021,0.217081,2.02757,-1.71319
164 | 1.63,0.853462,0.96462,-0.677501,0.210914,2.08763,-1.69655
165 | 1.64,0.846615,0.966699,-0.691867,0.204734,2.14894,-1.67929
166 | 1.65,0.839625,0.968715,-0.706116,0.19854,2.21148,-1.66143
167 | 1.66,0.832493,0.970669,-0.720246,0.192334,2.27523,-1.64296
168 | 1.67,0.825221,0.972562,-0.734254,0.186116,2.34017,-1.62389
169 | 1.68,0.817809,0.974392,-0.748138,0.179886,2.40625,-1.60422
170 | 1.69,0.810258,0.976159,-0.761895,0.173644,2.47347,-1.58395
171 | 1.7,0.802571,0.977865,-0.775523,0.167391,2.54178,-1.5631
172 | 1.71,0.794748,0.979507,-0.789021,0.161127,2.61117,-1.54166
173 | 1.72,0.786791,0.981087,-0.802385,0.154853,2.68159,-1.51963
174 | 1.73,0.778701,0.982604,-0.815614,0.14857,2.75302,-1.49703
175 | 1.74,0.770479,0.984058,-0.828705,0.142276,2.82544,-1.47385
176 | 1.75,0.762127,0.98545,-0.841655,0.135974,2.8988,-1.4501
177 | 1.76,0.753647,0.986778,-0.854464,0.129663,2.97308,-1.42578
178 | 1.77,0.745039,0.988043,-0.867128,0.123343,3.04825,-1.40091
179 | 1.78,0.736305,0.989245,-0.879645,0.117016,3.12427,-1.37548
180 | 1.79,0.727446,0.990383,-0.892014,0.110681,3.20111,-1.34949
181 | 1.8,0.718465,0.991458,-0.904232,0.104339,3.27874,-1.32297
182 | 1.81,0.709362,0.99247,-0.916298,0.0979904,3.35712,-1.2959
183 | 1.82,0.700139,0.993418,-0.928208,0.0916355,3.43622,-1.2683
184 | 1.83,0.690798,0.994303,-0.939962,0.0852747,3.51601,-1.24017
185 | 1.84,0.681341,0.995124,-0.951556,0.0789085,3.59644,-1.21153
186 | 1.85,0.671768,0.995881,-0.96299,0.0725373,3.67749,-1.18236
187 | 1.86,0.662081,0.996574,-0.974262,0.0661614,3.75911,-1.15269
188 | 1.87,0.652283,0.997204,-0.985368,0.0597813,3.84128,-1.12252
189 | 1.88,0.642375,0.99777,-0.996308,0.0533973,3.92394,-1.09185
190 | 1.89,0.632358,0.998272,-1.00708,0.04701,4.00707,-1.0607
191 | 1.9,0.622234,0.99871,-1.01768,0.0406196,4.09063,-1.02907
192 | 1.91,0.612004,0.999084,-1.02811,0.0342266,4.17458,-0.996975
193 | 1.92,0.601672,0.999395,-1.03837,0.0278314,4.25888,-0.964415
194 | 1.93,0.591238,0.999641,-1.04845,0.0214345,4.3435,-0.931402
195 | 1.94,0.580704,0.999823,-1.05835,0.0150362,4.42838,-0.897944
196 | 1.95,0.570071,0.999942,-1.06807,0.00863689,4.5135,-0.864053
197 | 1.96,0.559343,0.999996,-1.07762,0.00223706,4.59881,-0.829736
198 | 1.97,0.54852,0.999986,-1.08698,-0.00416292,4.68428,-0.795005
199 | 1.98,0.537604,0.999913,-1.09616,-0.0105626,4.76986,-0.759868
200 | 1.99,0.526597,0.999775,-1.10515,-0.0169617,4.85551,-0.724338
201 | 2,0.515501,0.999574,-1.11396,-0.0233596,5.4412,-0.188423
202 | 2.01,0.504319,0.999308,-1.12257,-0.0297561,5.52688,-0.152136
203 | 2.02,0.493051,0.998978,-1.131,-0.0361506,5.61251,-0.115488
204 | 2.03,0.481699,0.998585,-1.13924,-0.0425429,5.69805,-0.0784898
205 | 2.04,0.470266,0.998128,-1.14728,-0.0489324,5.78346,-0.0411545
206 | 2.05,0.458754,0.997606,-1.15513,-0.0553188,5.86869,-0.003494
207 | 2.06,0.447164,0.997021,-1.16279,-0.0617016,5.95372,0.0344791
208 | 2.07,0.435499,0.996372,-1.17025,-0.0680805,6.03849,0.0727518
209 | 2.08,0.42376,0.99566,-1.17751,-0.074455,6.12296,0.111311
210 | 2.09,0.41195,0.994883,-1.18457,-0.0808248,6.2071,0.150143
211 | 2.1,0.400069,0.994043,-1.19143,-0.0871894,6.29087,0.189235
212 | 2.11,0.388122,0.99314,-1.19809,-0.0935484,6.37422,0.228571
213 | 2.12,0.376108,0.992172,-1.20455,-0.0999015,6.45711,0.268139
214 | 2.13,0.364031,0.991142,-1.2108,-0.106248,6.5395,0.307922
215 | 2.14,0.351893,0.990047,-1.21685,-0.112588,6.62135,0.347907
216 | 2.15,0.339695,0.98889,-1.2227,-0.118921,6.70263,0.388078
217 | 2.16,0.32744,0.987669,-1.22833,-0.125246,6.7833,0.42842
218 | 2.17,0.315129,0.986385,-1.23376,-0.131563,6.8633,0.468916
219 | 2.18,0.302765,0.985038,-1.23898,-0.137871,6.94262,0.509552
220 | 2.19,0.29035,0.983627,-1.244,-0.144171,7.0212,0.550311
221 | 2.2,0.277886,0.982154,-1.2488,-0.150461,7.09901,0.591177
222 | 2.21,0.265375,0.980618,-1.25339,-0.156742,7.17601,0.632133
223 | 2.22,0.252819,0.97902,-1.25777,-0.163013,7.25217,0.673163
224 | 2.23,0.24022,0.977358,-1.26193,-0.169274,7.32746,0.714249
225 | 2.24,0.227581,0.975634,-1.26589,-0.175523,7.40182,0.755375
226 | 2.25,0.214903,0.973848,-1.26963,-0.181762,7.47524,0.796524
227 | 2.26,0.202189,0.971999,-1.27315,-0.187988,7.54767,0.837677
228 | 2.27,0.189441,0.970088,-1.27646,-0.194203,7.61908,0.878818
229 | 2.28,0.176661,0.968115,-1.27955,-0.200405,7.68944,0.919928
230 | 2.29,0.163851,0.96608,-1.28243,-0.206595,7.75872,0.960991
231 | 2.3,0.151013,0.963983,-1.28509,-0.212771,7.82689,1.00199
232 | 2.31,0.138149,0.961824,-1.28753,-0.218934,7.89391,1.0429
233 | 2.32,0.125263,0.959604,-1.28976,-0.225082,7.95975,1.08372
234 | 2.33,0.112355,0.957323,-1.29177,-0.231217,8.02439,1.12441
235 | 2.34,0.0994281,0.95498,-1.29356,-0.237336,8.0878,1.16496
236 | 2.35,0.0864845,0.952576,-1.29513,-0.24344,8.14996,1.20537
237 | 2.36,0.0735262,0.950111,-1.29648,-0.249529,8.21082,1.24559
238 | 2.37,0.0605556,0.947586,-1.29761,-0.255601,8.27038,1.28563
239 | 2.38,0.0475747,0.944999,-1.29853,-0.261658,8.32861,1.32546
240 | 2.39,0.0345858,0.942353,-1.29922,-0.267697,8.38547,1.36506
241 | 2.4,0.021591,0.939645,-1.2997,-0.27372,8.44096,1.40442
242 | 2.41,0.00859255,0.936878,-1.29995,-0.279725,8.49506,1.44351
243 | 2.42,-0.00440733,0.934051,-1.29999,-0.285712,8.54773,1.48233
244 | 2.43,-0.0174065,0.931164,-1.2998,-0.29168,8.59896,1.52084
245 | 2.44,-0.0304027,0.928217,-1.2994,-0.29763,8.64874,1.55905
246 | 2.45,-0.0433937,0.925212,-1.29878,-0.303561,8.69705,1.59692
247 | 2.46,-0.0563774,0.922146,-1.29793,-0.309473,8.74387,1.63445
248 | 2.47,-0.0693516,0.919022,-1.29687,-0.315365,8.78919,1.67161
249 | 2.48,-0.0823141,0.915839,-1.29559,-0.321236,8.833,1.70839
250 | 2.49,-0.0952627,0.912597,-1.29409,-0.327087,8.87528,1.74478
251 | 2.5,-0.108195,0.909297,-1.29237,-0.332917,8.91603,1.78075
252 | 2.51,-0.121109,0.905939,-1.29043,-0.338726,8.95523,1.81629
253 | 2.52,-0.134003,0.902523,-1.28828,-0.344513,8.99288,1.8514
254 | 2.53,-0.146874,0.899049,-1.2859,-0.350278,9.02897,1.88604
255 | 2.54,-0.15972,0.895517,-1.28331,-0.356021,9.0635,1.92021
256 | 2.55,-0.17254,0.891929,-1.2805,-0.361741,9.09646,1.95389
257 | 2.56,-0.18533,0.888283,-1.27748,-0.367438,9.12784,1.98708
258 | 2.57,-0.198088,0.88458,-1.27424,-0.373111,9.15765,2.01975
259 | 2.58,-0.210814,0.880821,-1.27078,-0.37876,9.18589,2.05189
260 | 2.59,-0.223503,0.877005,-1.26711,-0.384385,9.21256,2.0835
261 | 2.6,-0.236155,0.873133,-1.26323,-0.389986,9.23765,2.11455
262 | 2.61,-0.248767,0.869205,-1.25913,-0.395561,9.26117,2.14504
263 | 2.62,-0.261337,0.865222,-1.25482,-0.401111,9.28313,2.17495
264 | 2.63,-0.273863,0.861183,-1.2503,-0.406636,9.30354,2.20428
265 | 2.64,-0.286343,0.857089,-1.24557,-0.412134,9.32239,2.23301
266 | 2.65,-0.298774,0.85294,-1.24062,-0.417607,9.33969,2.26114
267 | 2.66,-0.311154,0.848737,-1.23547,-0.423052,9.35547,2.28865
268 | 2.67,-0.323482,0.84448,-1.2301,-0.42847,9.36972,2.31553
269 | 2.68,-0.335756,0.840168,-1.22453,-0.433861,9.38245,2.34179
270 | 2.69,-0.347972,0.835802,-1.21876,-0.439224,9.39369,2.36741
271 | 2.7,-0.36013,0.831383,-1.21277,-0.444559,9.40344,2.39237
272 | 2.71,-0.372227,0.826911,-1.20658,-0.449866,9.41172,2.41669
273 | 2.72,-0.384261,0.822386,-1.20019,-0.455144,9.41854,2.44034
274 | 2.73,-0.39623,0.817809,-1.1936,-0.460392,9.42393,2.46333
275 | 2.74,-0.408132,0.813178,-1.1868,-0.465612,9.42789,2.48565
276 | 2.75,-0.419966,0.808496,-1.1798,-0.470801,9.43045,2.50729
277 | 2.76,-0.431728,0.803763,-1.17261,-0.47596,9.43164,2.52826
278 | 2.77,-0.443417,0.798977,-1.16521,-0.481089,9.43145,2.54854
279 | 2.78,-0.455031,0.794141,-1.15762,-0.486187,9.42993,2.56813
280 | 2.79,-0.466569,0.789254,-1.14983,-0.491254,9.4271,2.58704
281 | 2.8,-0.478027,0.784316,-1.14185,-0.496289,9.42297,2.60526
282 | 2.81,-0.489405,0.779328,-1.13367,-0.501293,9.41757,2.62279
283 | 2.82,-0.5007,0.77429,-1.12531,-0.506265,9.41092,2.63962
284 | 2.83,-0.511911,0.769203,-1.11675,-0.511204,9.40306,2.65577
285 | 2.84,-0.523034,0.764066,-1.108,-0.51611,9.39401,2.67122
286 | 2.85,-0.53407,0.758881,-1.09907,-0.520984,9.38379,2.68599
287 | 2.86,-0.545015,0.753647,-1.08995,-0.525824,9.37244,2.70006
288 | 2.87,-0.555868,0.748364,-1.08065,-0.53063,9.35998,2.71345
289 | 2.88,-0.566628,0.743034,-1.07117,-0.535403,9.34644,2.72616
290 | 2.89,-0.577291,0.737656,-1.0615,-0.540141,9.33185,2.73819
291 | 2.9,-0.587857,0.732231,-1.05165,-0.544845,9.31624,2.74953
292 | 2.91,-0.598324,0.72676,-1.04163,-0.549514,9.29964,2.76021
293 | 2.92,-0.608689,0.721241,-1.03143,-0.554147,9.28209,2.77022
294 | 2.93,-0.618952,0.715677,-1.02106,-0.558745,9.26362,2.77956
295 | 2.94,-0.62911,0.710067,-1.01051,-0.563308,9.24425,2.78825
296 | 2.95,-0.639161,0.704411,-0.999794,-0.567834,9.22402,2.79629
297 | 2.96,-0.649105,0.69871,-0.988908,-0.572324,9.20297,2.80368
298 | 2.97,-0.658939,0.692964,-0.977855,-0.576777,9.18112,2.81043
299 | 2.98,-0.668662,0.687175,-0.966637,-0.581194,9.15851,2.81656
300 | 2.99,-0.678271,0.681341,-0.955255,-0.585573,9.13517,2.82206
301 | 3,-0.687766,0.675463,-0.943712,-0.589915,9.11114,2.82694
302 | 


--------------------------------------------------------------------------------
/matlab/observers.cpp:
--------------------------------------------------------------------------------
  1 | #include "../tests/double_link.h"
  2 | #include "../lib/momentum_observer.h"
  3 | #include "../lib/disturbance_observer.h"
  4 | #include "../lib/sliding_mode_observer.h"
  5 | #include "../lib/disturbance_kalman_filter.h"
  6 | #include "../lib/filtered_dyn_observer.h"
  7 | #include "../lib/filtered_range_observer.h"
  8 | #include "../lib/disturbance_kalman_filter_exp.h"
  9 | 
 10 | #include "observers.h"
 11 | 
 12 | #define ARRAY_LEN 30  // max number of observers
 13 | 
 14 | // robot dynamics
 15 | static DoubleLink robot;
 16 | // observers
 17 | static ExternalObserver *observer[ARRAY_LEN] = {0};
 18 | // position to add new element
 19 | static int _nextIndex = 0;
 20 | 
 21 | void reset(int ind)
 22 | {
 23 |   if(ind >= 0 && ind < _nextIndex)
 24 |     observer[ind]->reset();
 25 | }
 26 | 
 27 | void freeAll()
 28 | {
 29 |   MomentumObserver *mo;
 30 |   DisturbanceObserver *dis;
 31 |   SlidingModeObserver *sm;
 32 |   DKalmanObserver *dk;
 33 |   FDynObserver *fd;
 34 |   FRangeObserver *fr;
 35 |   DKalmanObserverExp *dke;
 36 |   for(int i = 0; i < _nextIndex; i++) {
 37 |     switch (observer[i]->type() ) {
 38 |     case ID_MomentumObserver:
 39 |       mo = (MomentumObserver*) observer[i];
 40 |       delete mo;
 41 |       break;
 42 |     case ID_DisturbanceObserver:
 43 |       dis = (DisturbanceObserver*) observer[i];
 44 |       delete dis;
 45 |       break;
 46 |     case ID_SlidingModeObserver:
 47 |       sm = (SlidingModeObserver*) observer[i];
 48 |       delete sm;
 49 |       break;
 50 |     case ID_DKalmanObserver:
 51 |       dk = (DKalmanObserver*) observer[i];
 52 |       delete dk;
 53 |       break;
 54 |     case ID_FDynObserver:
 55 |       fd = (FDynObserver*) observer[i];
 56 |       delete fd;
 57 |       break;
 58 |     case ID_FRangeObserver:
 59 |       fr = (FRangeObserver*) observer[i];
 60 |       delete fr;
 61 |       break;
 62 |     case ID_DKalmanObserverExp:
 63 |       dke = (DKalmanObserverExp*) observer[i];
 64 |       delete dke;
 65 |       break;
 66 |     }
 67 |     observer[i] = 0;
 68 |   }
 69 |   _nextIndex = 0;
 70 | }
 71 | 
 72 | int getExternalTorque(int ind, double* ext, double *q, double *qd, double *tau, double dt)
 73 | {
 74 |   if(ind < 0 || ind >= _nextIndex) return ERR_WRONG_INDEX;
 75 |   ExternalObserver* ob = observer[ind];
 76 | 
 77 |   VectorJ vext(JOINT_NO), vq(JOINT_NO), vqd(JOINT_NO), vtau(JOINT_NO);
 78 | 
 79 |   for(int i = 0; i < JOINT_NO; i++) {
 80 |     vq(i) = q[i];
 81 |     vqd(i) = qd[i];
 82 |     vtau(i) = tau[i];
 83 |   }
 84 | 
 85 |   vext = ob->getExternalTorque(vq,vqd,vtau,dt);
 86 | 
 87 |   for(int i = 0; i < JOINT_NO; i++) {
 88 |     ext[i] = vext(i);
 89 |   }
 90 | 
 91 |   return 0;
 92 | }
 93 | 
 94 | void getRobotTorque(double *tau, double *q, double *qd, double *q2d)
 95 | {
 96 |   VectorJ vq(JOINT_NO), vqd(JOINT_NO), vq2d(JOINT_NO), res(JOINT_NO);
 97 |   for(int i = 0; i < JOINT_NO; i++) {
 98 |     vq(i) = q[i];
 99 |     vqd(i) = qd[i];
100 |     vq2d(i) = q2d[i];
101 |   }
102 | 
103 |   res = robot.getM(vq) * vq2d + robot.getC(vq,vqd) * vqd + robot.getG(vq) + robot.getFriction(vqd);
104 | 
105 |   for(int i = 0; i < JOINT_NO; i++)
106 |     tau[i] = res(i);
107 | }
108 | 
109 | int configMomentumObserver(int ind, double *k)
110 | {
111 |   // prepare
112 |   MomentumObserver* ptr;
113 |   VectorJ vk(JOINT_NO);
114 |   for(int i = 0; i < JOINT_NO; i++)
115 |     vk(i) = k[i];
116 | 
117 |   if(ind == ADD_NEW) {
118 |     if(_nextIndex == ARRAY_LEN) return ERR_NO_SLOTS;  // can't add new
119 |     // create new
120 |     ptr = new MomentumObserver(&robot, vk);
121 |     observer[_nextIndex] = ptr;
122 |     return _nextIndex++;    // new element _nextIndex
123 |   } else if(ind < ADD_NEW || ind >= ARRAY_LEN) {
124 |     return ERR_WRONG_INDEX;
125 |   }
126 | 
127 |   // update state
128 |   ptr = (MomentumObserver*) observer[ind];
129 |   if(ptr->type() != ID_MomentumObserver)
130 |     return ERR_WRONG_TYPE;
131 |   ptr->settings(vk);
132 | 
133 |   return ind; // ok
134 | }
135 | 
136 | int configDisturbanceObserver(int ind, double sigma, double xeta, double beta)
137 | {
138 |   // prepare
139 |   DisturbanceObserver* ptr;
140 | 
141 |   if(ind == ADD_NEW) {
142 |     if(_nextIndex == ARRAY_LEN) return ERR_NO_SLOTS;  // can't add new
143 |     // create new
144 |     ptr = new DisturbanceObserver(&robot,sigma,xeta,beta);
145 |     observer[_nextIndex] = ptr;
146 |     return _nextIndex++;    // new element _nextIndex
147 |   } else if(ind < ADD_NEW || ind >= ARRAY_LEN) {
148 |     return ERR_WRONG_INDEX;
149 |   }
150 |   // update state
151 |   ptr = (DisturbanceObserver*) observer[ind];
152 |   if(ptr->type() != ID_DisturbanceObserver)
153 |     return ERR_WRONG_TYPE;
154 |   ptr->settings(sigma,xeta,beta);
155 | 
156 |   return ind; // ok
157 | }
158 | 
159 | int configSlidingModeObserver(int ind, double *T1, double *S1, double *T2, double *S2)
160 | {
161 |   // prepare
162 |   SlidingModeObserver* ptr;
163 |   VectorJ vT1(JOINT_NO), vS1(JOINT_NO), vT2(JOINT_NO), vS2(JOINT_NO);
164 |   for(int i = 0; i < JOINT_NO; i++) {
165 |     vT1(i) = T1[i];
166 |     vS1(i) = S1[i];
167 |     vT2(i) = T2[i];
168 |     vS2(i) = S2[i];
169 |   }
170 | 
171 |   if(ind == ADD_NEW) {
172 |     if(_nextIndex == ARRAY_LEN) return ERR_NO_SLOTS;  // can't add new
173 |     // create new
174 |     ptr = new SlidingModeObserver(&robot,vT1,vS1,vT2,vS2);
175 |     observer[_nextIndex] = ptr;
176 |     return _nextIndex++;    // new element _nextIndex
177 |   } else if(ind < ADD_NEW || ind >= ARRAY_LEN) {
178 |     return ERR_WRONG_INDEX;
179 |   }
180 | 
181 |   ptr = (SlidingModeObserver*) observer[ind];
182 |   if(ptr->type() != ID_SlidingModeObserver)
183 |     return ERR_WRONG_TYPE;
184 |   ptr->settings(vT1,vS1,vT2,vS2);
185 | 
186 |   return ind;
187 | }
188 | 
189 | int configDistKalmanObserver(int ind, double *S, double *H, double *Q, double *R)
190 | {
191 |   DKalmanObserver* ptr;
192 |   MatrixJ mS(JOINT_NO,JOINT_NO), mH(JOINT_NO,JOINT_NO), mQ(2*JOINT_NO,2*JOINT_NO), mR(JOINT_NO,JOINT_NO);
193 |   int k = 0;
194 |   for(int c = 0; c < JOINT_NO; c++) {
195 |     for(int r = 0; r < JOINT_NO; r++,k++) {
196 |       mS(r,c) = S[k];
197 |       mH(r,c) = H[k];
198 |       mR(r,c) = R[k];
199 |     }
200 |   }
201 |   k = 0;
202 |   for(int c = 0; c < 2*JOINT_NO; c++) {
203 |     for(int r = 0; r < 2*JOINT_NO; r++,k++)
204 |       mQ(r,c) = Q[k];
205 |   }
206 | 
207 |   if(ind == ADD_NEW) {
208 |     if(_nextIndex == ARRAY_LEN) return ERR_NO_SLOTS;  // can't add new
209 |     // create new
210 |     ptr = new DKalmanObserver(&robot,mS,mH,mQ,mR);
211 |     observer[_nextIndex] = ptr;
212 |     return _nextIndex++;    // new element _nextIndex
213 |   } else if(ind < ADD_NEW || ind >= ARRAY_LEN) {
214 |     return ERR_WRONG_INDEX;
215 |   }
216 | 
217 |   ptr = (DKalmanObserver*) observer[ind];
218 |   if(ptr->type() != ID_DKalmanObserver)
219 |     return ERR_WRONG_TYPE;
220 |   ptr->settings(mQ,mR);
221 | 
222 |   return ind;
223 | }
224 | 
225 | int configDistKalmanObserverExp(int ind, double *S, double *H, double *Q, double *R)
226 | {
227 |   DKalmanObserverExp* ptr;
228 |   MatrixJ mS(JOINT_NO,JOINT_NO), mH(JOINT_NO,JOINT_NO), mQ(2*JOINT_NO,2*JOINT_NO), mR(JOINT_NO,JOINT_NO);
229 |   int k = 0;
230 |   for(int c = 0; c < JOINT_NO; c++) {
231 |     for(int r = 0; r < JOINT_NO; r++,k++) {
232 |       mS(r,c) = S[k];
233 |       mH(r,c) = H[k];
234 |       mR(r,c) = R[k];
235 |     }
236 |   }
237 |   k = 0;
238 |   for(int c = 0; c < 2*JOINT_NO; c++) {
239 |     for(int r = 0; r < 2*JOINT_NO; r++,k++)
240 |       mQ(r,c) = Q[k];
241 |   }
242 | 
243 |   if(ind == ADD_NEW) {
244 |     if(_nextIndex == ARRAY_LEN) return ERR_NO_SLOTS;  // can't add new
245 |     // create new
246 |     ptr = new DKalmanObserverExp(&robot,mS,mH,mQ,mR);
247 |     observer[_nextIndex] = ptr;
248 |     return _nextIndex++;    // new element _nextIndex
249 |   } else if(ind < ADD_NEW || ind >= ARRAY_LEN) {
250 |     return ERR_WRONG_INDEX;
251 |   }
252 | 
253 |   ptr = (DKalmanObserverExp*) observer[ind];
254 |   if(ptr->type() != ID_DKalmanObserverExp)
255 |     return ERR_WRONG_TYPE;
256 |   ptr->settings(mQ,mR);
257 | 
258 |   return ind;
259 | }
260 | 
261 | 
262 | int configFilterDynObserver(int ind, double cutOff, double dt)
263 | {
264 |   FDynObserver* ptr;
265 | 
266 |   if(ind == ADD_NEW) {
267 |     if(_nextIndex == ARRAY_LEN) return ERR_NO_SLOTS;  // can't add new
268 |     // create new
269 |     ptr = new FDynObserver(&robot,cutOff,dt);
270 |     observer[_nextIndex] = ptr;
271 |     return _nextIndex++;    // new element _nextIndex
272 |   } else if(ind < ADD_NEW || ind >= ARRAY_LEN) {
273 |     return ERR_WRONG_INDEX;
274 |   }
275 | 
276 |   ptr = (FDynObserver*) observer[ind];
277 |   if(ptr->type() != ID_FDynObserver)
278 |     return ERR_WRONG_TYPE;
279 |   ptr->settings(cutOff,dt);
280 | 
281 |   return ind;
282 | }
283 | 
284 | int configFilterRangeObserver(int ind, double cutOff, double dt, double k)
285 | {
286 |   FRangeObserver* ptr;
287 | 
288 |   if(ind == ADD_NEW) {
289 |     if(_nextIndex == ARRAY_LEN) return ERR_NO_SLOTS;
290 |     ptr = new FRangeObserver(&robot,cutOff,dt,k);
291 |     observer[_nextIndex] = ptr;
292 |     return _nextIndex++;
293 |   } else if(ind < ADD_NEW || ind >= ARRAY_LEN) {
294 |     return ERR_WRONG_INDEX;
295 |   }
296 | 
297 |   ptr = (FRangeObserver*) observer[ind];
298 |   if(ptr->type() != ID_FRangeObserver)
299 |     return ERR_WRONG_TYPE;
300 |   ptr->settings(cutOff,dt,k);
301 | 
302 |   return ind;
303 | }
304 | 


--------------------------------------------------------------------------------
/matlab/observers.h:
--------------------------------------------------------------------------------
 1 | #ifndef OBSERVERS_DYN_H
 2 | #define OBSERVERS_DYN_H
 3 | 
 4 | #define JOINT_NO 2
 5 | 
 6 | #define ERR_WRONG_INDEX -1
 7 | #define ERR_NO_SLOTS    -2
 8 | #define ERR_WRONG_TYPE  -3
 9 | 
10 | #define ADD_NEW -1
11 | // If index -1, return id of the new observer, else update existant
12 | 
13 | #ifdef __cplusplus
14 | extern "C" {
15 | #endif
16 | 
17 | 
18 | int configMomentumObserver(int index, double k[JOINT_NO]);
19 | 
20 | int configDisturbanceObserver(int index, double sigma, double xeta, double beta);
21 | 
22 | int configSlidingModeObserver(int index, double T1[JOINT_NO], double S1[JOINT_NO], double T2[JOINT_NO], double S2[JOINT_NO]);
23 | 
24 | int configDistKalmanObserver(int index, double S[JOINT_NO*JOINT_NO], double H[JOINT_NO*JOINT_NO], double Q[4*JOINT_NO*JOINT_NO], double R[JOINT_NO*JOINT_NO]);
25 | 
26 | int configDistKalmanObserverExp(int index, double S[JOINT_NO*JOINT_NO], double H[JOINT_NO*JOINT_NO], double Q[4*JOINT_NO*JOINT_NO], double R[JOINT_NO*JOINT_NO]);
27 | 
28 | int configFilterDynObserver(int index, double cutOff, double dt);
29 | 
30 | int configFilterRangeObserver(int ind, double cutOff, double dt, double k);
31 | 
32 | // Get external torque estimation
33 | int getExternalTorque(int index, double ext[JOINT_NO], double q[JOINT_NO], double qd[JOINT_NO], double tau[JOINT_NO], double dt);
34 | 
35 | // reset observer state
36 | void reset(int index);
37 | 
38 | // clear memory
39 | void freeAll();
40 | 
41 | // get expected joint torques
42 | void getRobotTorque(double tau[JOINT_NO], double q[JOINT_NO], double qd[JOINT_NO], double q2d[JOINT_NO]);
43 | 
44 | #ifdef __cplusplus
45 | }
46 | #endif
47 | 
48 | 
49 | #endif // OBSERVERS_DYN_H
50 | 


--------------------------------------------------------------------------------
/matlab/test.m:
--------------------------------------------------------------------------------
  1 | % External torque observers - comparison
  2 | % 2020, Stanislav Mikhel
  3 | 
  4 | clear all; close all
  5 | % get library
  6 | if not(libisloaded('observers'))
  7 |     loadlibrary('observers.so','observers.h')
  8 | end
  9 | % methods 
 10 | %libfunctions observers -full
 11 | 
 12 | % read file
 13 | data = csvread('input.csv');
 14 | tm   = data(:,1);      % time
 15 | q    = data(:,2:3);    % angles
 16 | qd   = data(:,4:5);    % velocities
 17 | cur  = data(:,6:7);    % torques
 18 | 
 19 | JNT = 2;
 20 | K = [1,1];    % current to torque, N/A
 21 | res = libpointer('doublePtr',zeros(1,JNT));
 22 | 
 23 | % use 
 24 | % calllib('observers','reset',id)
 25 | % to reset observer state
 26 | 
 27 | % ========== Momentum Observer ================
 28 | 
 29 | % configuration
 30 | Kmo = [50,50];  
 31 | 
 32 | % get observer ID
 33 | id_mo = calllib('observers','configMomentumObserver',-1,Kmo);  %    -1 means create and configure
 34 | 
 35 | ext_mo = zeros(size(cur));
 36 | for i = 2:size(cur,1)
 37 |     calllib('observers','getExternalTorque',id_mo,res,q(i,:),qd(i,:),K.*cur(i,:),tm(i)-tm(i-1));
 38 |     ext_mo(i,:) = res.Value(:);
 39 | end
 40 | 
 41 | plot(tm,ext_mo);
 42 | figure;
 43 | 
 44 | % ======= Disturbance Observer ===============
 45 | 
 46 | % configuration
 47 | sigma = 21;
 48 | xeta = 18;
 49 | beta = 50;
 50 | 
 51 | % get ID
 52 | id_dis = calllib('observers','configDisturbanceObserver',-1,sigma,xeta,beta);
 53 | 
 54 | ext_dis = zeros(size(cur));
 55 | for i = 2:size(cur,1)
 56 |     calllib('observers','getExternalTorque',id_dis,res,q(i,:),qd(i,:),K.*cur(i,:),tm(i)-tm(i-1));
 57 |     ext_dis(i,:) = res.Value(:);
 58 | end
 59 | 
 60 | plot(tm,ext_dis);
 61 | title("Disturbance observer");
 62 | figure;
 63 | 
 64 | % ======= Sliding Mode Observer ============
 65 | 
 66 | % configuration
 67 | S1 = [20,30];
 68 | T1 = 2*sqrt(S1);
 69 | S2 = [10,10];
 70 | T2 = 2*sqrt(S2);
 71 | 
 72 | % get ID
 73 | id_sm = calllib('observers','configSlidingModeObserver',-1,T1,S1,T2,S2);
 74 | 
 75 | ext_sm = zeros(size(cur));
 76 | for i = 2:size(cur,1)
 77 |     calllib('observers','getExternalTorque',id_sm,res,q(i,:),qd(i,:),K.*cur(i,:),tm(i)-tm(i-1));
 78 |     ext_sm(i,:) = res.Value(:);
 79 | end
 80 | 
 81 | plot(tm,ext_sm);
 82 | title("Sliding Mode Observer");
 83 | figure;
 84 | 
 85 | % ========= Kalman Filter Observer =======
 86 | 
 87 | % configuration 
 88 | S = zeros(JNT,JNT);
 89 | H = eye(JNT,JNT);
 90 | Q = blkdiag(0.002*eye(JNT,JNT), 0.3*eye(JNT,JNT));
 91 | R = 0.05*eye(JNT,JNT);
 92 | 
 93 | % get ID
 94 | id_kf = calllib('observers','configDistKalmanObserver',-1,S,H,Q,R);
 95 | 
 96 | ext_kf = zeros(size(cur));
 97 | for i = 2:size(cur,1)
 98 |     calllib('observers','getExternalTorque',id_kf,res,q(i,:),qd(i,:),K.*cur(i,:),tm(i)-tm(i-1));
 99 |     ext_kf(i,:) = res.Value(:);
100 | end
101 | 
102 | plot(tm,ext_kf);
103 | title("Kalman Filter Observer");
104 | figure;
105 | 
106 | % ========= Kalman Filter Continous System Observer ===== 
107 | 
108 | % configuration 
109 | %S = zeros(JOINT_NO,JOINT_NO);
110 | %H = eye(JOINT_NO,JOINT_NO);
111 | %Q = blkdiag(0.2*eye(JOINT_NO,JOINT_NO), 30*eye(JOINT_NO,JOINT_NO));
112 | %R = 0.0005*eye(JOINT_NO,JOINT_NO);
113 | 
114 | % get ID
115 | %id_kfe = calllib('observers','configDistKalmanObserverExp',-1,S,H,Q,R);
116 | 
117 | %ext_kfe = zeros(size(cur));
118 | 
119 | %for i = 2:size(cur,1)
120 | %    calllib('observers','getExternalTorque',id_kfe,res,q(i,:),qd(i,:),K.*cur(i,:),tm(i)-tm(i-1));
121 | %    ext_kfe(i,:) = res.Value(:); 
122 | %end
123 | 
124 | %plot(tm,ext_kfe);
125 | %title("Continous Kalman Filter Observer");
126 | %figure;
127 | 
128 | % ====== Filtered Dynamics ==============
129 | 
130 | % cinfiguration 
131 | cutOff = 8;  % rad/s
132 | timeStep = 0.01;  % s
133 | 
134 | % get ID
135 | id_df = calllib('observers','configFilterDynObserver',-1,cutOff,timeStep);
136 | 
137 | ext_df = zeros(size(cur));
138 | for i = 2:size(cur,1)
139 |     calllib('observers','getExternalTorque',id_df,res,q(i,:),qd(i,:),K.*cur(i,:),tm(i)-tm(i-1));
140 |     ext_df(i,:) = res.Value(:);
141 | end
142 | 
143 | plot(tm,ext_df);
144 | title("Filtered dynamics");
145 | figure;
146 | 
147 | % ===== Filtered Range ===================
148 | 
149 | % cinfiguration 
150 | %cutOff = 8;  % rad/s
151 | %timeStep = 0.01;  % s
152 | delta = 0.1;
153 | 
154 | % get ID
155 | id_dr1 = calllib('observers','configFilterRangeObserver',-1,cutOff,timeStep, delta);
156 | id_dr2 = calllib('observers','configFilterRangeObserver',-1,cutOff,timeStep,-delta);
157 | 
158 | ext_up = zeros(size(cur));
159 | ext_low = zeros(size(cur));
160 | for i = 2:size(cur,1)
161 |     calllib('observers','getExternalTorque',id_dr1,res,q(i,:),qd(i,:),K.*cur(i,:),tm(i)-tm(i-1));
162 |     ext_up(i,:) = res.Value(:);
163 |     calllib('observers','getExternalTorque',id_dr2,res,q(i,:),qd(i,:),K.*cur(i,:),tm(i)-tm(i-1));
164 |     ext_low(i,:) = res.Value(:);
165 | end
166 | 
167 | n = 1;  % joint index
168 | plot(tm,[ext_up(:,n),ext_low(:,n),ext_df(:,n)]); 
169 | title("Range");
170 | 
171 | % =============== exit ===================
172 | 
173 | calllib('observers','freeAll'); % clear memory
174 | 
175 | unloadlibrary('observers');


--------------------------------------------------------------------------------
/matlab/test_lib.cpp:
--------------------------------------------------------------------------------
  1 | // Test observers with expreimental results
  2 | 
  3 | #include <iostream> 
  4 | #include <fstream>
  5 | #include <sstream>
  6 | #include <vector>
  7 | #include <string> 
  8 | #include <cstdlib>
  9 | #include <iomanip>
 10 | 
 11 | #include "observers.h"
 12 | 
 13 | // settings
 14 | #define IN_NAME  "input.csv"       // CSV: time, positions, velocities, torques
 15 | #define OUT_NAME "external.csv"    // CSV: time, external torques 
 16 | 
 17 | #define BEG_Q    1
 18 | #define BEG_QD   (BEG_Q+JOINT_NO)
 19 | #define BEG_TAU  (BEG_QD+JOINT_NO)
 20 | 
 21 | // Reag csv line of numbers
 22 | bool csvRead(std::istream& str, std::vector<double>& v);
 23 | 
 24 | 
 25 | int main(int argc, char** argv)
 26 | {   
 27 |   double q[JOINT_NO], qd[JOINT_NO], tau[JOINT_NO], ext[JOINT_NO];
 28 | 
 29 |   // torque coefficients
 30 |   double K[] = {1,1};      // current to force, N/A
 31 | 
 32 |   // test with Kalman disturbance observer
 33 |   double S[JOINT_NO*JOINT_NO] = {0};
 34 |   double H[] = {1,0,
 35 |                 0,1};   
 36 |   double Q[(2*JOINT_NO)*(2*JOINT_NO)] = {0};
 37 |   for(int i = 0; i < (2*JOINT_NO)*(2*JOINT_NO); i += 2*JOINT_NO+1) {
 38 |     Q[i] = (i < 2*JOINT_NO*JOINT_NO) ? 0.2 : 30;    
 39 |   }
 40 |   double R[] = {5E-4,0,
 41 |                    0,5E-4};
 42 |   int id_kfe = configDistKalmanObserverExp(-1,S,H,Q,R);
 43 |   
 44 |   
 45 |   // save to file 
 46 |   std::ofstream oFile(OUT_NAME);
 47 |   oFile.setf(std::ios::fixed);
 48 |   oFile.precision(4);
 49 | 
 50 |   // read from file
 51 |   std::ifstream iFile(IN_NAME);
 52 | 
 53 |   std::vector<double> val;
 54 |   val.reserve(3*JOINT_NO+1);
 55 | 
 56 |   double prev = -1, zero = 0, dt = 0;
 57 |     
 58 |   while(csvRead(iFile,val)) {
 59 | 
 60 |     double curr = val[0];
 61 |     if(prev >= 0) {
 62 |       dt = curr - prev;
 63 |     } else {
 64 |       dt = 0;
 65 |       zero = curr;
 66 |     }
 67 |     prev = curr;
 68 |     curr -= zero;  // start from 0
 69 | 
 70 |     // read values
 71 |     for(int i = 0; i < JOINT_NO; i++) {
 72 |       q[i]   = val[BEG_Q + i];
 73 |       qd[i]  = val[BEG_QD + i];
 74 |       tau[i] = val[BEG_TAU + i] * K[i];      
 75 |     }
 76 |     
 77 |     getExternalTorque(id_kfe,ext,q,qd,tau,dt);
 78 | 
 79 |     // save result 
 80 |     oFile <<  curr;
 81 |     for(int i = 0; i < JOINT_NO; i++) {
 82 |       oFile << "," << ext[i];
 83 |     }
 84 |     oFile << std::endl;
 85 |   }
 86 |       
 87 |   oFile.close();
 88 |   iFile.close();
 89 |   
 90 |   freeAll();
 91 |   
 92 |   return 0;
 93 | }
 94 | 
 95 | bool csvRead(std::istream& str, std::vector<double>& result)
 96 | {
 97 |   std::string                line;
 98 |   std::getline(str,line);
 99 |   std::stringstream          lineStream(line);
100 |   std::string                cell;
101 | 
102 |   result.clear();
103 | 
104 |   while(std::getline(lineStream,cell, ','))  {
105 |     result.push_back((double) atof(cell.c_str()) );
106 |   }
107 | 
108 |   return !result.empty();
109 | }
110 | 


--------------------------------------------------------------------------------
/tests/Makefile:
--------------------------------------------------------------------------------
 1 | INCDIR = -I /usr/include/eigen3
 2 | 
 3 | .PHONY: test kalman rnea dyn
 4 | 
 5 | # check observers with M, C, G matrices
 6 | TEST_SRC = test.cpp  ../lib/kalman_filter_continous.cpp
 7 | test: ${TEST_SRC}
 8 | 	g++ -Wall ${INCDIR} ${TEST_SRC} -o test -lm
 9 | 	./test
10 | 	gnuplot -p force.gnuplot
11 | 
12 | # check observers with RNEA
13 | RNEA_SRC = test_rnea.cpp  ../lib/robot_dynamics_rnea.cpp
14 | rnea: $(RNEA_SRC)
15 | 	g++ -Wall $(RNEA_SRC) -o rnea -lm
16 | 	./rnea
17 | 	gnuplot -p force.gnuplot	
18 | 
19 | # check Kalman filter
20 | kalman: test_kalman.cpp
21 | 	g++ -Wall test_kalman.cpp -o kalman -lm
22 | 	./kalman
23 | 	gnuplot -p kalman.gnuplot	
24 | 
25 | # build dynamic library
26 | dyn: observers.cpp
27 | 	g++ -shared -o libobservers.so -fPIC observers.cpp
28 | 


--------------------------------------------------------------------------------
/tests/README.md:
--------------------------------------------------------------------------------
 1 | # Tests / examples 
 2 | 
 3 | Check observers with _M_, _C_, _G_ matrices:
 4 | 
 5 |     make test 
 6 | 
 7 | Check observers with _RNEA_:
 8 | 
 9 |     make rnea 
10 | 
11 | Generate dynamic library:
12 | 
13 |     make dyn 
14 | 
15 | Check Kalman filter:
16 | 
17 |     make kalman
18 | 


--------------------------------------------------------------------------------
/tests/double_link.h:
--------------------------------------------------------------------------------
 1 | #ifndef DOUBLE_LINK_H
 2 | #define DOUBLE_LINK_H
 3 | 
 4 | #include "../lib/external_observer.h"
 5 | #include <cmath>
 6 | 
 7 | class DoubleLink : public RobotDynamics {
 8 | public:
 9 |   DoubleLink();
10 | 
11 |   // get inertia
12 |   MatrixJ getM(VectorJ& q);
13 |   // get Coriolis/centrifugal matrix
14 |   MatrixJ getC(VectorJ& q, VectorJ& qd);
15 |   // get GRAVITYity
16 |   VectorJ getG(VectorJ& q);
17 |   // friction model
18 |   VectorJ getFriction(VectorJ& qd) { return fric; }
19 |   // number of joints
20 |   int jointNo() { return 2; }
21 | 
22 | private:
23 |   MatrixJ M, C, J;
24 |   VectorJ G, fric;
25 |   double m1,m2;
26 |   double l1, l2;
27 |   double lc1, lc2;
28 |   double I1, I2;
29 | };
30 | 
31 | DoubleLink::DoubleLink()
32 |            : RobotDynamics()
33 |            , M(MatrixJ(2,2))
34 |            , C(MatrixJ(2,2))
35 |            , J(MatrixJ(6,2))
36 |            , G(VectorJ(2))
37 |            , fric(VectorJ(2))
38 | {
39 |   m1 = 1; m2 = 1;
40 |   l1 = 0.5; l2 = 0.5;
41 |   lc1 = 0.25; lc2 = 0.25;
42 |   I1 = 0.3; I2 = 0.2;
43 |   M.setZero();
44 |   C.setZero();
45 |   J.setZero();
46 |   G.setZero();
47 |   fric.setZero();
48 | }
49 | 
50 | MatrixJ DoubleLink::getM(VectorJ& q)
51 | {
52 |   M(0,0) = m1*lc1*lc1 + m2*(l1*l1 + lc2*lc2 + 2*l1*lc2*cos(q(1))) + I1 + I2;
53 |   M(0,1) = m2*(lc2*lc2 + l1*lc2*cos(q(1))) + I2;
54 |   M(1,0) = M(0,1);
55 |   M(1,1) = m2*lc2*lc2 + I2;
56 |   return M;
57 | }
58 | 
59 | MatrixJ DoubleLink::getC(VectorJ& q, VectorJ& qd)
60 | {
61 |   double h = -m2*l1*lc2*sin(q(1));
62 |   C(0,0) = h*qd(1);
63 |   C(0,1) = h*(qd(0)+qd(1));
64 |   C(1,0) = -h*(qd(0));
65 |   return C;
66 | }
67 | 
68 | VectorJ DoubleLink::getG(VectorJ& q)
69 | {
70 |   double c12 = cos(q(0)+q(1));
71 |   G(0) = (m1*lc1 + m2*l1)*GRAVITY*cos(q(0)) + m2*lc2*GRAVITY*c12;
72 |   G(1) = m2*lc2*GRAVITY*c12;
73 |   return G;
74 | }
75 | 
76 | /*
77 | Matrix DoubleLink::getJacobian(Vector& q)
78 | {
79 |   double s12 = sin(q(0)+q(1)), c12 = cos(q(0)+q(1));
80 |   J(0,0) = -l1*sin(q(0))-l2*s12;
81 |   J(1,0) = l1*cos(q(0)) + l2*c12;
82 |   J(5,0) = 1;
83 |   J(0,1) = -l2*s12;
84 |   J(1,1) = l2*c12;
85 |   J(5,1) = 1;
86 |   return J;
87 | }
88 | */
89 | 
90 | #endif // DOUBLE_LINK_H
91 | 


--------------------------------------------------------------------------------
/tests/double_link_rnea.h:
--------------------------------------------------------------------------------
 1 | #ifndef DOUBLE_LINK_RNEA_H
 2 | #define DOUBLE_LINK_RNEA_H
 3 | 
 4 | #include "../lib/external_observer.h"
 5 | #include <cmath>
 6 | 
 7 | class DoubleLinkRnea : public RobotDynamicsRnea {
 8 | public:
 9 |   DoubleLinkRnea();
10 | 
11 |   VectorJ rnea(VectorJ& q, VectorJ& qd, VectorJ& q2d, double g = 0);
12 |   // friction model
13 |   VectorJ getFriction(VectorJ& qd) { return fric; }
14 |   // number of joints
15 |   int jointNo() { return 2; }
16 | 
17 | private:
18 |   VectorJ tau, fric;
19 |   double m1,m2;
20 |   double l1, l2;
21 |   double lc1, lc2;
22 |   double I1, I2;
23 | }; // DoubleLinkRnea
24 | 
25 | DoubleLinkRnea::DoubleLinkRnea()
26 |            : RobotDynamicsRnea()
27 |            , tau(VectorJ(2))
28 |            , fric(VectorJ(2))
29 | {
30 |   m1 = 1; m2 = 1;
31 |   l1 = 0.5; l2 = 0.5;
32 |   lc1 = 0.25; lc2 = 0.25;
33 |   I1 = 0.3; I2 = 0.2;
34 |   fric.setZero();
35 | }
36 | 
37 | VectorJ DoubleLinkRnea::rnea(VectorJ& q, VectorJ& qd, VectorJ& q2d, double g)
38 | {
39 |   double M00 = m1*lc1*lc1 + m2*(l1*l1 + lc2*lc2 + 2*l1*lc2*cos(q(1))) + I1 + I2;
40 |   double M01 = m2*(lc2*lc2 + l1*lc2*cos(q(1))) + I2;
41 |   double M10 = M01;
42 |   double M11 = m2*lc2*lc2 + I2;
43 |   double h = -m2*l1*lc2*sin(q(1));
44 |   double C00 = h*qd(1);
45 |   double C01 = h*(qd(0)+qd(1));
46 |   double C10 = -h*(qd(0));
47 |   double c12 = cos(q(0)+q(1));
48 |   double G0 = (m1*lc1 + m2*l1)*g*cos(q(0)) + m2*lc2*g*c12;
49 |   double G1 = m2*lc2*g*c12;
50 | 
51 |   tau(0) = M00*q2d(0) + M01*q2d(1) + C00*qd(0) + C01*qd(1) + G0;
52 |   tau(1) = M10*q2d(0) + M11*q2d(1) + C10*qd(0)             + G1;
53 | 
54 |   return tau;
55 | }
56 | 
57 | /*
58 | Matrix DoubleLink::getJacobian(Vector& q)
59 | {
60 |   double s12 = sin(q(0)+q(1)), c12 = cos(q(0)+q(1));
61 |   J(0,0) = -l1*sin(q(0))-l2*s12;
62 |   J(1,0) = l1*cos(q(0)) + l2*c12;
63 |   J(5,0) = 1;
64 |   J(0,1) = -l2*s12;
65 |   J(1,1) = l2*c12;
66 |   J(5,1) = 1;
67 |   return J;
68 | }
69 | */
70 | 
71 | #endif // DOUBLE_LINK_RNEA_H
72 | 


--------------------------------------------------------------------------------
/tests/force.gnuplot:
--------------------------------------------------------------------------------
1 | # use gnuplot -p force.gnuplot
2 | set datafile separator ','
3 | plot 'force.csv' using 1:2 with lines, '' using 1:3 with lines
4 | 


--------------------------------------------------------------------------------
/tests/kalman.gnuplot:
--------------------------------------------------------------------------------
1 | # use gnuplot -p kalman.gnuplot
2 | set datafile separator ','
3 | plot 'estimation.csv' using 1:2 with lines, '' using 1:3 with lines
4 | 


--------------------------------------------------------------------------------
/tests/observers.cpp:
--------------------------------------------------------------------------------
  1 | #include "double_link.h"
  2 | #include "../lib/momentum_observer.h"
  3 | #include "../lib/disturbance_observer.h"
  4 | #include "../lib/sliding_mode_observer.h"
  5 | #include "../lib/disturbance_kalman_filter.h"
  6 | #include "../lib/filtered_dyn_observer.h"
  7 | 
  8 | #include "observers.h"
  9 | 
 10 | #define ARRAY_LEN 20
 11 | #define JOINT_NO 2
 12 | 
 13 | // robot dynamics
 14 | static DoubleLink robot;
 15 | // observers
 16 | static ExternalObserver *observer[ARRAY_LEN] = {0};
 17 | // position to add new element
 18 | static int _nextIndex = 0;
 19 | 
 20 | void reset(int ind)
 21 | {
 22 |   if(ind >= 0 && ind < _nextIndex)
 23 |     observer[ind]->reset();
 24 | }
 25 | 
 26 | void freeAll()
 27 | {
 28 |   MomentumObserver *mo;
 29 |   DisturbanceObserver *dis;
 30 |   SlidingModeObserver *sm;
 31 |   DKalmanObserver *dk;
 32 |   FDynObserver *fd;
 33 |   for(int i = 0; i < _nextIndex; i++) {
 34 |     switch (observer[i]->type() ) {
 35 |     case ID_MomentumObserver:
 36 |       mo = (MomentumObserver*) observer[i];
 37 |       delete mo;
 38 |       break;
 39 |     case ID_DisturbanceObserver:
 40 |       dis = (DisturbanceObserver*) observer[i];
 41 |       delete dis;
 42 |       break;
 43 |     case ID_SlidingModeObserver:
 44 |       sm = (SlidingModeObserver*) observer[i];
 45 |       delete sm;
 46 |       break;
 47 |     case ID_DKalmanObserver:
 48 |       dk = (DKalmanObserver*) observer[i];
 49 |       delete dk;
 50 |       break;
 51 |     case ID_FDynObserver:
 52 |       fd = (FDynObserver*) observer[i];
 53 |       delete fd;
 54 |       break;
 55 |     }
 56 |   }
 57 | }
 58 | 
 59 | int getExternalTorque(int ind, double* ext, double *q, double *qd, double *tau, double dt)
 60 | {
 61 |   if(ind < 0 || ind >= _nextIndex) return ERR_WRONG_INDEX;
 62 |   ExternalObserver* ob = observer[ind];
 63 | 
 64 |   VectorJ vext(JOINT_NO), vq(JOINT_NO), vqd(JOINT_NO), vtau(JOINT_NO);
 65 | 
 66 |   for(int i = 0; i < JOINT_NO; i++) {
 67 |     vq(i) = q[i];
 68 |     vqd(i) = qd[i];
 69 |     vtau(i) = tau[i];
 70 |   }
 71 | 
 72 |   vext = ob->getExternalTorque(vq,vqd,vtau,dt);
 73 | 
 74 |   for(int i = 0; i < JOINT_NO; i++) {
 75 |     ext[i] = vext(i);
 76 |   }
 77 | 
 78 |   return 0;
 79 | }
 80 | 
 81 | int configMomentumObserver(int ind, double *k)
 82 | {
 83 |   // prepare
 84 |   MomentumObserver* ptr;
 85 |   VectorJ vk(JOINT_NO);
 86 |   for(int i = 0; i < JOINT_NO; i++)
 87 |     vk(i) = k[i];
 88 | 
 89 |   if(ind == ADD_NEW) {
 90 |     if(_nextIndex == ARRAY_LEN) return ERR_NO_SLOTS;  // can't add new
 91 |     // create new
 92 |     ptr = new MomentumObserver(&robot, vk);
 93 |     observer[_nextIndex] = ptr;
 94 |     return _nextIndex++;    // new element _nextIndex
 95 |   } else if(ind < ADD_NEW || ind >= ARRAY_LEN) {
 96 |     return ERR_WRONG_INDEX;
 97 |   }
 98 |   // update state
 99 |   ptr = (MomentumObserver*) observer[ind];
100 |   if(ptr->type() != ID_MomentumObserver)
101 |     return ERR_WRONG_TYPE;
102 |   ptr->settings(vk);
103 | 
104 |   return ind; // ok
105 | }
106 | 
107 | int configDisturbanceObserver(int ind, double sigma, double xeta, double beta)
108 | {
109 |   // prepare
110 |   DisturbanceObserver* ptr;
111 | 
112 |   if(ind == ADD_NEW) {
113 |     if(_nextIndex == ARRAY_LEN) return ERR_NO_SLOTS;  // can't add new
114 |     // create new
115 |     ptr = new DisturbanceObserver(&robot,sigma,xeta,beta);
116 |     observer[_nextIndex] = ptr;
117 |     return _nextIndex++;    // new element _nextIndex
118 |   } else if(ind < ADD_NEW || ind >= ARRAY_LEN) {
119 |     return ERR_WRONG_INDEX;
120 |   }
121 |   // update state
122 |   ptr = (DisturbanceObserver*) observer[ind];
123 |   if(ptr->type() != ID_DisturbanceObserver)
124 |     return ERR_WRONG_TYPE;
125 |   ptr->settings(sigma,xeta,beta);
126 | 
127 |   return ind; // ok
128 | }
129 | 
130 | int configSlidingModeObserver(int ind, double *T1, double *S1, double *T2, double *S2)
131 | {
132 |   // prepare
133 |   SlidingModeObserver* ptr;
134 |   VectorJ vT1(JOINT_NO), vS1(JOINT_NO), vT2(JOINT_NO), vS2(JOINT_NO);
135 |   for(int i = 0; i < JOINT_NO; i++) {
136 |     vT1(i) = T1[i];
137 |     vS1(i) = S1[i];
138 |     vT2(i) = T2[i];
139 |     vS2(i) = S2[i];
140 |   }
141 | 
142 |   if(ind == ADD_NEW) {
143 |     if(_nextIndex == ARRAY_LEN) return ERR_NO_SLOTS;  // can't add new
144 |     // create new
145 |     ptr = new SlidingModeObserver(&robot,vT1,vS1,vT2,vS2);
146 |     observer[_nextIndex] = ptr;
147 |     return _nextIndex++;    // new element _nextIndex
148 |   } else if(ind < ADD_NEW || ind >= ARRAY_LEN) {
149 |     return ERR_WRONG_INDEX;
150 |   }
151 | 
152 |   ptr = (SlidingModeObserver*) observer[ind];
153 |   if(ptr->type() != ID_SlidingModeObserver)
154 |     return ERR_WRONG_TYPE;
155 |   ptr->settings(vT1,vS1,vT2,vS2);
156 | 
157 |   return ind;
158 | }
159 | 
160 | int configDistKalmanObserver(int ind, double *S, double *H, double *Q, double *R)
161 | {
162 |   DKalmanObserver* ptr;
163 |   MatrixJ mS(JOINT_NO,JOINT_NO), mH(JOINT_NO,JOINT_NO), mQ(2*JOINT_NO,2*JOINT_NO), mR(JOINT_NO,JOINT_NO);
164 |   int k = 0;
165 |   for(int r = 0; r < JOINT_NO; r++) {
166 |     for(int c = 0; c < JOINT_NO; c++,k++) {
167 |       mS(r,c) = S[k];
168 |       mH(r,c) = H[k];
169 |       mR(r,c) = R[k];
170 |     }
171 |   }
172 |   k = 0;
173 |   for(int r = 0; r < 2*JOINT_NO; r++) {
174 |     for(int c = 0; c < 2*JOINT_NO; c++,k++)
175 |       mQ(r,c) = Q[k];
176 |   }
177 | 
178 |   if(ind == ADD_NEW) {
179 |     if(_nextIndex == ARRAY_LEN) return ERR_NO_SLOTS;  // can't add new
180 |     // create new
181 |     ptr = new DKalmanObserver(&robot,mS,mH,mQ,mR);
182 |     observer[_nextIndex] = ptr;
183 |     return _nextIndex++;    // new element _nextIndex
184 |   } else if(ind < ADD_NEW || ind >= ARRAY_LEN) {
185 |     return ERR_WRONG_INDEX;
186 |   }
187 | 
188 |   ptr = (DKalmanObserver*) observer[ind];
189 |   if(ptr->type() != ID_DKalmanObserver)
190 |     return ERR_WRONG_TYPE;
191 |   ptr->settings(mQ,mR);
192 | 
193 |   return ind;
194 | }
195 | 
196 | int configFilterDynObserver(int ind, double cutOff, double dt)
197 | {
198 |   FDynObserver* ptr;
199 | 
200 |   if(ind == ADD_NEW) {
201 |     if(_nextIndex == ARRAY_LEN) return ERR_NO_SLOTS;  // can't add new
202 |     // create new
203 |     ptr = new FDynObserver(&robot,cutOff,dt);
204 |     observer[_nextIndex] = ptr;
205 |     return _nextIndex++;    // new element _nextIndex
206 |   } else if(ind < ADD_NEW || ind >= ARRAY_LEN) {
207 |     return ERR_WRONG_INDEX;
208 |   }
209 | 
210 |   ptr = (FDynObserver*) observer[ind];
211 |   if(ptr->type() != ID_FDynObserver)
212 |     return ERR_WRONG_TYPE;
213 |   ptr->settings(cutOff,dt);
214 | 
215 |   return ind;
216 | }
217 | 


--------------------------------------------------------------------------------
/tests/observers.h:
--------------------------------------------------------------------------------
 1 | #ifndef OBSERVERS_DYN_H
 2 | #define OBSERVERS_DYN_H 
 3 | 
 4 | #define ERR_WRONG_INDEX -1
 5 | #define ERR_NO_SLOTS    -2
 6 | #define ERR_WRONG_TYPE  -3
 7 | 
 8 | #define ADD_NEW -1
 9 | // If index -1, return id of the new observer, else update existant
10 | 
11 | #ifdef __cplusplus
12 | extern "C" {
13 | #endif
14 | 
15 | int configMomentumObserver(int index, double k[2]);
16 | 
17 | int configDisturbanceObserver(int index, double sigma, double xeta, double beta);
18 | 
19 | int configSlidingModeObserver(int index, double T1[2], double S1[2], double T2[2], double S2[2]);
20 | 
21 | int configDistKalmanObserver(int index, double S[2*2], double H[2*2], double Q[4*4], double R[2*2]);
22 | 
23 | int configFilterDynObserver(int index, double cutOff, double dt);
24 | 
25 | // Get external torque estimation
26 | int getExternalTorque(int index, double ext[2], double q[2], double qd[2], double tau[2], double dt); 
27 | 
28 | // reset observer state
29 | void reset(int index);
30 | 
31 | // clear memory
32 | void freeAll();
33 | 
34 | #ifdef __cplusplus
35 | }
36 | #endif
37 | 
38 | 
39 | #endif // OBSERVERS_DYN_H
40 | 


--------------------------------------------------------------------------------
/tests/test.cpp:
--------------------------------------------------------------------------------
  1 | 
  2 | #include <iostream>
  3 | #include <fstream>
  4 | #include "double_link.h"
  5 | //
  6 | // Uncemment the desirable observer
  7 | //
  8 | //#include "../lib/momentum_observer.h"
  9 | //#include "../lib/disturbance_observer.h"
 10 | #include "../lib/sliding_mode_observer.h"
 11 | //#include "../lib/disturbance_kalman_filter.h"
 12 | //#include "../lib/filtered_dyn_observer.h"
 13 | //#include "../lib/filtered_range_observer.h"
 14 | //#include "../lib/disturbance_kalman_filter_exp.h"
 15 | 
 16 | #define OMEGA1 1.3
 17 | #define OMEGA2 0.8
 18 | #define FNAME "force.csv"
 19 | #define TSTEP 0.01
 20 | 
 21 | // use it to emulate external torque
 22 | #define SET_TORQUE
 23 | 
 24 | int main(int argc, char** argv)
 25 | {
 26 |   DoubleLink robot;
 27 |   VectorJ q(2), qd(2), q2d(2), tau(2), ext(2);
 28 | 
 29 | #ifdef MOMENTUM_OBSERVER_H
 30 |   Vector k(2);
 31 |   k << 30,30;
 32 |   MomentumObserver m_observer(&robot,k);
 33 | #endif
 34 | #ifdef DISTURBANCE_OBSERVER_H
 35 |   double sigma = 21, xeta = 18, beta = 50;
 36 |   DisturbanceObserver d_observer(&robot,sigma,xeta,beta);
 37 | #endif
 38 | #ifdef SLIDING_MODE_OBSERVER_H
 39 |   VectorJ T1(2), S1(2), T2(2), S2(2);
 40 |   S1 << 15,20;
 41 |   T1(0) = 2*sqrt(S1(0)); T1(1) = 2*sqrt(S1(1));
 42 |   S2 << 10,10; // set 0 to exclude linear part
 43 |   T2(0) = 2*sqrt(S2(0)); T2(1) = 2*sqrt(S2(1));
 44 |   SlidingModeObserver sm_observer(&robot,T1,S1,T2,S2);
 45 | #endif
 46 | #ifdef DISTURBANCE_KALMAN_FILTER_H
 47 |   Matrix S = Matrix::Zero(2,2);
 48 |   Matrix H = Matrix::Identity(2,2);
 49 |   Matrix Q = Matrix::Identity(4,4);
 50 |   Matrix R = Matrix::Identity(2,2);
 51 |   Q(0,0) = 0.002; Q(1,1) = 0.002; Q(2,2) = 0.3; Q(3,3) = 0.3;
 52 |   R *= 0.05;
 53 |   DKalmanObserver dkm_observer(&robot,S,H,Q,R);
 54 | #endif
 55 | #ifdef DISTURBANCE_KALMAN_FILTER_EXP_H
 56 |   Matrix S = Matrix::Zero(2,2);
 57 |   Matrix H = Matrix::Identity(2,2);
 58 |   Matrix Q = Matrix::Identity(4,4);
 59 |   Matrix R = Matrix::Identity(2,2);
 60 |   Q(0,0) = 0.2; Q(1,1) = 0.2; Q(2,2) = 30; Q(3,3) = 30;
 61 |   R *= 0.0005;
 62 |   DKalmanObserverExp dkme_observer(&robot,S,H,Q,R);
 63 | #endif
 64 | #ifdef FILTERED_DYNAMIC_OBSERVER_H
 65 |   FDynObserver fd_observer(&robot, 8, TSTEP);
 66 | #endif
 67 | #ifdef FILTERED_RANGE_OBSERVER_H
 68 |   FRangeObserver fr_observer(&robot, 8, TSTEP, -0.1);
 69 | #endif
 70 | 
 71 | 
 72 |   // save to file
 73 |   std::ofstream file;
 74 |   file.open(FNAME);
 75 |   // second file with the robot state
 76 |   std::ofstream file2;
 77 |   file2.open("input.csv");
 78 | 
 79 |   double dt = TSTEP;
 80 |   for(double t = 0; t < 3; t += dt) {
 81 |     double c1 = cos(OMEGA1*t), c2 = cos(OMEGA2*t);
 82 |     double s1 = sin(OMEGA1*t), s2 = sin(OMEGA2*t);
 83 |     // state
 84 |     q(0) = s1; q(1) = s2;
 85 |     qd(0) = OMEGA1*c1; qd(1) = OMEGA2*c2;
 86 |     q2d(0) = -OMEGA1*OMEGA1*s1; q2d(1) = -OMEGA2*OMEGA2*s2;
 87 | 
 88 |     // torque
 89 |     tau = robot.getM(q)*q2d + robot.getC(q,qd)*qd + robot.getG(q);
 90 | #ifdef SET_TORQUE
 91 |     if(t > 1 && t < 2) {
 92 |       tau(0) -= 0.5;
 93 |       tau(1) -= 0.5;
 94 |     }
 95 | #endif // SET_TORQUE
 96 |     // estimate torque and save
 97 | 
 98 | #ifdef MOMENTUM_OBSERVER_H
 99 |     ext = m_observer.getExternalTorque(q,qd,tau,dt);
100 | #endif
101 | #ifdef DISTURBANCE_OBSERVER_H
102 |     ext = d_observer.getExternalTorque(q,qd,tau,dt);
103 | #endif
104 | #ifdef SLIDING_MODE_OBSERVER_H
105 |     ext = sm_observer.getExternalTorque(q,qd,tau,dt);
106 | #endif
107 | #ifdef DISTURBANCE_KALMAN_FILTER_H
108 |     ext = dkm_observer.getExternalTorque(q,qd,tau,dt);
109 | #endif
110 | #ifdef DISTURBANCE_KALMAN_FILTER_EXP_H
111 |     ext = dkme_observer.getExternalTorque(q,qd,tau,dt);
112 | #endif
113 | #ifdef FILTERED_DYNAMIC_OBSERVER_H
114 |     ext = fd_observer.getExternalTorque(q,qd,tau,dt);
115 | #endif
116 | #ifdef FILTERED_RANGE_OBSERVER_H
117 |     ext = fr_observer.getExternalTorque(q,qd,tau,dt);
118 | #endif
119 | 
120 |     file << t << "," << ext(0) << "," << ext(1) << std::endl;
121 |     // robot state
122 |     file2 << t << "," << q(0) << "," << q(1) << ","
123 |                       << qd(0) << "," << qd(1) << ","
124 |                       << tau(0) << "," << tau(1) << std::endl;
125 |   }
126 | 
127 |   file.close();
128 |   file2.close();
129 | 
130 |   return 0;
131 | }
132 | 
133 | 


--------------------------------------------------------------------------------
/tests/test_kalman.cpp:
--------------------------------------------------------------------------------
 1 | 
 2 | #include <iostream>
 3 | #include <fstream>
 4 | #include <cstdlib>
 5 | #include <cmath>
 6 | 
 7 | #include "../lib/kalman_filter.h"
 8 | 
 9 | #define RANDOM ((rand() % 1000)/1000.0)
10 | #define FNAME "estimation.csv"
11 | #define DELTA 0.02
12 | 
13 | using namespace Eigen;
14 | 
15 | int main(int argc, char **argv)
16 | {
17 |   // system 
18 |   MatrixXd A(2,2), B(2,1), C(1,2);
19 |   A << -5, 1, 1, -4;
20 |   B << 0.5, 0.1;
21 |   C << 2, 1;
22 | 
23 |   // to discret system
24 |   A = MatrixXd::Identity(2,2) + DELTA * A;
25 |   B = DELTA * B;
26 | 
27 |   // control impact 
28 |   VectorXd u(1);
29 | 
30 |   // initial state 
31 |   VectorXd x(2,1), y(1), yest(1); 
32 |   x.setZero();
33 | 
34 |   // filter
35 |   KalmanFilter filter(A,B,C);
36 |   filter.reset(x);
37 | 
38 |   MatrixXd Q(2,2), R(1,1);
39 |   Q << 0.003, 0, 0, 0.003;
40 |   R << 1;
41 |   filter.setCovariance(Q,R);
42 | 
43 |   // save to file 
44 |   std::ofstream file;
45 |   file.open(FNAME); 
46 | 
47 |   for(double t = 0; t < 3; t += DELTA) {
48 |     // next state
49 |     u(0) = sin(2*t);
50 |     x = A * x + B * u; 
51 |     y = C * x;
52 |     y(0) += 0.02*(2*RANDOM - 1);
53 |     // estimation
54 |     yest = C * filter.step(u,y);
55 |     // write
56 |     file << t << "," << y(0) << "," << yest(0) << std::endl;
57 |   }
58 | 
59 |   file.close();
60 | 
61 |   return 0;
62 | }
63 | 


--------------------------------------------------------------------------------
/tests/test_rnea.cpp:
--------------------------------------------------------------------------------
  1 | 
  2 | #include <iostream>
  3 | #include <fstream>
  4 | #include "double_link_rnea.h"
  5 | 
  6 | //
  7 | // Uncomment the desirable observer
  8 | //
  9 | #include "../lib/momentum_observer_rnea.h"
 10 | //#include "../lib/disturbance_observer_rnea.h"
 11 | //#include "../lib/sliding_mode_observer_rnea.h"
 12 | //#include "../lib/disturbance_kalman_filter_rnea.h"
 13 | //#include "../lib/filtered_dyn_observer_rnea.h"
 14 | 
 15 | #define OMEGA1 1.3
 16 | #define OMEGA2 0.8
 17 | #define FNAME "force.csv"
 18 | #define TSTEP 0.01
 19 | 
 20 | // use it to emulate external torque
 21 | #define SET_TORQUE
 22 | 
 23 | int main(int argc, char** argv)
 24 | {
 25 | 
 26 |   DoubleLinkRnea robot;
 27 |   VectorJ q(2), qd(2), q2d(2), tau(2), ext(2);
 28 | 
 29 | #ifdef MOMENTUM_OBSERVER_RNEA_H
 30 |   VectorJ k(2);
 31 |   k << 50,50;
 32 |   MomentumObserverRnea m_observer(&robot,k);
 33 | #endif
 34 | #ifdef DISTURBANCE_OBSERVER_RNEA_H
 35 |   double sigma = 21, xeta = 18, beta = 50;
 36 |   DisturbanceObserverRnea d_observer(&robot,sigma,xeta,beta);
 37 | #endif
 38 | #ifdef SLIDING_MODE_OBSERVER_RNEA_H
 39 |   Vector T1(2), S1(2), T2(2), S2(2);
 40 |   S1 << 20,30;
 41 |   T1(0) = 2*sqrt(S1(0)); T1(1) = 2*sqrt(S1(1));
 42 |   S2 << 10,10; // set 0 to exclude linear part
 43 |   T2(0) = 2*sqrt(S2(0)); T2(1) = 2*sqrt(S2(1));
 44 |   SlidingModeObserverRnea sm_observer(&robot,T1,S1,T2,S2);
 45 | #endif
 46 | #ifdef DISTURBANCE_KALMAN_FILTER_RNEA_H
 47 |   Matrix S = Matrix::Zero(2,2);
 48 |   Matrix H = Matrix::Identity(2,2);
 49 |   Matrix Q = Matrix::Identity(4,4);
 50 |   Matrix R = Matrix::Identity(2,2);
 51 |   Q(0,0) = 0.002; Q(1,1) = 0.002; Q(2,2) = 0.3; Q(3,3) = 0.3;
 52 |   R *= 0.05;
 53 |   DKalmanObserverRnea dkm_observer(&robot,S,H,Q,R);
 54 | #endif
 55 | #ifdef FILTERED_DYNAMIC_OBSERVER_RNEA_H
 56 |   FDynObserverRnea fd_observer(&robot, 8, TSTEP);
 57 | #endif
 58 | 
 59 |   // save to file
 60 |   std::ofstream file;
 61 |   file.open(FNAME);
 62 | 
 63 |   double dt = TSTEP;
 64 |   for(double t = 0; t < 3; t += dt) {
 65 |     double c1 = cos(OMEGA1*t), c2 = cos(OMEGA2*t);
 66 |     double s1 = sin(OMEGA1*t), s2 = sin(OMEGA2*t);
 67 |     // state
 68 |     q(0) = s1; q(1) = s2;
 69 |     qd(0) = OMEGA1*c1; qd(1) = OMEGA2*c2;
 70 |     q2d(0) = -OMEGA1*OMEGA1*s1; q2d(1) = -OMEGA2*OMEGA2*s2;
 71 | 
 72 |     // torque
 73 |     tau = robot.rnea(q,qd,q2d,GRAVITY);
 74 | #ifdef SET_TORQUE
 75 |     if(t > 1 && t < 2) {
 76 |       tau(0) -= 0.5;
 77 |       tau(1) -= 0.5;
 78 |     }
 79 | #endif // SET_TORQUE
 80 |     // estimate torque and save
 81 | 
 82 | #ifdef MOMENTUM_OBSERVER_RNEA_H
 83 |     ext = m_observer.getExternalTorque(q,qd,tau,dt);
 84 | #endif
 85 | #ifdef DISTURBANCE_OBSERVER_RNEA_H
 86 |     ext = d_observer.getExternalTorque(q,qd,tau,dt);
 87 | #endif
 88 | #ifdef SLIDING_MODE_OBSERVER_RNEA_H
 89 |     ext = sm_observer.getExternalTorque(q,qd,tau,dt);
 90 | #endif
 91 | #ifdef DISTURBANCE_KALMAN_FILTER_RNEA_H
 92 |     ext = dkm_observer.getExternalTorque(q,qd,tau,dt);
 93 | #endif
 94 | #ifdef FILTERED_DYNAMIC_OBSERVER_RNEA_H
 95 |     ext = fd_observer.getExternalTorque(q,qd,tau,dt);
 96 | #endif
 97 | 
 98 |     file << t << "," << ext(0) << "," << ext(1) << std::endl;
 99 |   }
100 | 
101 |   file.close();
102 | 
103 |   return 0;
104 | }
105 | 
106 | 


--------------------------------------------------------------------------------