Add Adaptation.

diff --git a/include/linearizing_controllers/twist_mrac_linearizing_controller.hpp b/include/linearizing_controllers/twist_mrac_linearizing_controller.hpp
index 6ea19e3..7f59f82 100644 (file)
--- a/include/linearizing_controllers/twist_mrac_linearizing_controller.hpp
+++ b/include/linearizing_controllers/twist_mrac_linearizing_controller.hpp
@@ -1,6 +1,6 @@
 /******************************************************************************
                    ROS 2 linearizing_controllers Package
-                     Twist MRAC Linearizing Controller
+                    Twist MR(A)C Linearizing Controller
           Copyright (C) 2018..2022 Walter Fetter Lages <w.fetter@ieee.org>
 
         This program is free software: you can redistribute it and/or modify
@@ -97,6 +97,22 @@ namespace effort_controllers
                Eigen::Vector2d uModel_;
 
                Eigen::DiagonalMatrix<double,2> Alpha_;
+               
+               Eigen::Matrix<double,2,4> Wf_;
+               
+               bool adaptive_;
+               
+               Eigen::DiagonalMatrix<double,2> Epsilon_;
+               
+               Eigen::Vector4d thetaEst_;
+               
+               Eigen::Vector2d FWthetaEst_;
+               
+               Eigen::Vector2d ef_;
+               
+               Eigen::Vector2d e1_;
+               
+               Eigen::Matrix4d P_;
 
                int priority_;
 

diff --git a/src/twist_mrac_linearizing_controller.cpp b/src/twist_mrac_linearizing_controller.cpp
index 93ce659..0187ca7 100644 (file)
--- a/src/twist_mrac_linearizing_controller.cpp
+++ b/src/twist_mrac_linearizing_controller.cpp
@@ -1,7 +1,7 @@
 /******************************************************************************
                        ROS linearing_controllers Package
-                       Twist MRAC Linearizing Controller
-          Copyright (C) 2018 Walter Fetter Lages <w.fetter@ieee.org>
+                       Twist MR(A)C Linearizing Controller
+          Copyright (C) 2018, 2022 Walter Fetter Lages <w.fetter@ieee.org>
 
         This program is free software: you can redistribute it and/or modify
         it under the terms of the GNU General Public License as published by
@@ -51,6 +51,10 @@ namespace effort_controllers
                        auto_declare<std::vector<double>>("G",std::vector<double>());
 
                        auto_declare<std::vector<double>>("Alpha",std::vector<double>());
+                       
+                       auto_declare<bool>("adaptive",false);
+                       
+                       auto_declare<std::vector<double>>("Epsilon",std::vector<double>());
 
                        auto_declare<double>("time_step",0.01);
 
@@ -160,6 +164,16 @@ namespace effort_controllers
                        if(!node_->get_parameter("Alpha",AlphaVec))
                                throw std::runtime_error("No 'Alpha' parameter.");
                        Alpha_.diagonal()=Eigen::Map<Eigen::Vector2d>(AlphaVec.data(),2);
+                       
+                       adaptive_=node_->get_parameter("adaptive").as_bool();
+                       
+                       if(adaptive_)
+                       {
+                               std::vector<double> EpsilonVec;
+                               if(!node_->get_parameter("Epsilon",EpsilonVec))
+                                       throw std::runtime_error("No 'Epsilon' parameter.");
+                               Epsilon_.diagonal()=Eigen::Map<Eigen::Vector2d>(EpsilonVec.data(),2);
+                       }
 
                        time_step_=node_->get_parameter("time_step").as_double();
                }
@@ -208,6 +222,26 @@ namespace effort_controllers
                for(unsigned int i=0;i < joints_.size();i++)
                        phi_[i]=state_interfaces_[i].get_value();
 
+               if(adaptive_)
+               {                       
+                       Wf_.setZero();
+
+                       // thetaEst=[f12 f21 g11 g21]^T
+                       // F=   [0 f12
+                       //      f21 0]
+                       // Ginv=[0.5/g11 0.5/g21
+                       //      0.5/g11 -0.5/g21]
+                       thetaEst_ << F_(0,1), F_(1,0), 0.5/Ginv_(0,0), 0.5/Ginv_(0,1);
+                       
+                       thetaEst_*=1.5;
+                       
+                       FWthetaEst_.setZero();
+                       ef_.setZero();
+                       e1_.setZero();
+                       P_.setIdentity();
+//                     P_*=1e6;
+               }
+
                 struct sched_param param;
                 param.sched_priority=priority_;
                 if(sched_setscheduler(0,SCHED_FIFO,&param) == -1)
@@ -253,6 +287,7 @@ namespace effort_controllers
                {
                        uRef_[0]=(*command)->linear.x;
                        uRef_[1]=(*command)->angular.z;
+                       P_.setIdentity();
                }
 
                auto param=rt_param_.readFromRT();
@@ -277,15 +312,27 @@ namespace effort_controllers
                                Ginv_(1,1)=-0.5/(*param)->data[3];
                        }
                }
+       
+               if(adaptive_)
+               {               
+                       // thetaEst=[f12 f21 g11 g21]^T
+                       // F=   [0 f12
+                       //      f21 0]
+                       // Ginv=[0.5/g11 0.5/g21
+                       //      0.5/g11 -0.5/g21]
+                       F_(0,1)=thetaEst_[0];
+                       F_(1.0)=thetaEst_[1];
+                       Ginv_(0,0)=0.5/thetaEst_[2];
+                       Ginv_(0,1)=0.5/thetaEst_[3];
+                       Ginv_(1.0)=0.5/thetaEst_[2];
+                       Ginv_(1.1)=-0.5/thetaEst_[3];
+               }
 
                // Compute reference model
                auto deltaUModel=Alpha_*(uRef_-uModel_);
 
                auto v=Alpha_*(uModel_-u)+deltaUModel;
 
-               //update reference model
-               uModel_+=deltaUModel*dt.seconds();
-
                 // Linearization
                Eigen::Vector2d uf(u[0]*u[1],sqr(u[1]));
                auto torque=Ginv_*(v-F_*uf);
@@ -294,6 +341,46 @@ namespace effort_controllers
                for(unsigned int i=0;i < joints_.size();i++)
                        command_interfaces_[i].set_value(torque[i]);
 
+               Eigen::Matrix<double,2,4> W;
+               if(adaptive_)
+               {
+                       // Parameter estimation
+                       W <<    sqr(u[1]), 0, torque[0]+torque[1], 0,
+                               0, u[0]*u[1], 0, torque[1]-torque[0];
+               
+                       auto dWf=Epsilon_.inverse()*(W-Wf_);
+               
+                       auto dFWthetaEst=Epsilon_.inverse()*(W*thetaEst_-FWthetaEst_);
+               
+                       auto e=u-uModel_;
+               
+                       auto def=Epsilon_.inverse()*(e-ef_);
+               
+                       auto WfPhi=def+Alpha_*ef_+FWthetaEst_-Wf_*thetaEst_;
+
+                       auto de1=WfPhi-Alpha_*e1_;
+               
+                       auto dthetaEst=0.5*P_*Wf_.transpose()*WfPhi+0.5*P_*Wf_.transpose()*Alpha_*e1_;
+               
+                       auto dP=-P_*Wf_.transpose()*Wf_*P_;
+               
+                       // Update filters               
+                       Wf_+=dWf*dt.seconds();
+       
+                       FWthetaEst_+=dFWthetaEst*dt.seconds();
+
+                       ef_+=def*dt.seconds();
+               
+                       e1_+=de1*dt.seconds();
+               
+                       thetaEst_+=dthetaEst*dt.seconds();
+               
+                       P_=dP*dt.seconds();
+               }
+
+               //update reference model
+               uModel_+=deltaUModel*dt.seconds();
+
                 if(rt_status_publisher_ && rt_status_publisher_->trylock())
                 {
                        rt_status_publisher_->msg_.header.stamp=time;
@@ -319,6 +406,26 @@ namespace effort_controllers
                                rt_status_publisher_->msg_.linear_dynamics_command[i]=v[i];
                        }
 
+                       rt_status_publisher_->msg_.adaptive=adaptive_;
+                       if(adaptive_)
+                       {
+                               for(int i=0;i < ef_.size();i++)
+                               {
+                                       rt_status_publisher_->msg_.epsilon[i]=Epsilon_.diagonal()(i);
+                                       rt_status_publisher_->msg_.filtered_error[i]=ef_[i];
+                                       rt_status_publisher_->msg_.augmented_error[i]=e1_[i];
+                               }
+                               for(int i=0;i < W.rows();i++) for(int j=0;j < W.cols();j++)
+                               {
+                                       rt_status_publisher_->msg_.regressor[i*W.cols()+j]=W(i,j);
+                                       rt_status_publisher_->msg_.filtered_regressor[i*Wf_.cols()+j]=Wf_(i,j);
+                               }
+                               for(int i=0;i < thetaEst_.size();i++)
+                                       rt_status_publisher_->msg_.estimated_parameters[i]=thetaEst_(i);
+                               for(int i=0;i < P_.size();i++)
+                                       rt_status_publisher_->msg_.covariance[i]=P_(i);
+                       }
+
                        rt_status_publisher_->unlockAndPublish();
                 }