From d19b264633426f3dabd79f4ffcba645d3f7398ef Mon Sep 17 00:00:00 2001 From: Walter Fetter Lages Date: Thu, 6 Jul 2023 19:06:30 -0300 Subject: [PATCH] Add Adaptation. --- .../twist_mrac_linearizing_controller.hpp | 18 +++- src/twist_mrac_linearizing_controller.cpp | 117 ++++++++++++++++++++- 2 files changed, 129 insertions(+), 6 deletions(-) diff --git a/include/linearizing_controllers/twist_mrac_linearizing_controller.hpp b/include/linearizing_controllers/twist_mrac_linearizing_controller.hpp index 6ea19e3..7f59f82 100644 --- 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 This program is free software: you can redistribute it and/or modify @@ -97,6 +97,22 @@ namespace effort_controllers Eigen::Vector2d uModel_; Eigen::DiagonalMatrix Alpha_; + + Eigen::Matrix Wf_; + + bool adaptive_; + + Eigen::DiagonalMatrix 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 --- 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 + Twist MR(A)C Linearizing Controller + Copyright (C) 2018, 2022 Walter Fetter Lages 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>("G",std::vector()); auto_declare>("Alpha",std::vector()); + + auto_declare("adaptive",false); + + auto_declare>("Epsilon",std::vector()); auto_declare("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(AlphaVec.data(),2); + + adaptive_=node_->get_parameter("adaptive").as_bool(); + + if(adaptive_) + { + std::vector EpsilonVec; + if(!node_->get_parameter("Epsilon",EpsilonVec)) + throw std::runtime_error("No 'Epsilon' parameter."); + Epsilon_.diagonal()=Eigen::Map(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,¶m) == -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 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(); } -- 2.12.0