Merge pull request CAST-Robotics#15 from pezhman-abdolahnezhad/main

controller class was added
amirhosein-vedadi · Sep 4, 2021 · 4c0de46 · 4c0de46
2 parents 4cb066e + 3da93ae
commit 4c0de46
Show file tree

Hide file tree

Showing 2 changed files with 107 additions and 0 deletions.
diff --git a/trajectory_planner/include/trajectory_planner/Controller.h b/trajectory_planner/include/trajectory_planner/Controller.h
@@ -0,0 +1,26 @@
+#include <iostream>
+#include <eigen3/Eigen/Eigen>
+#include <math.h>
+
+
+using namespace std;
+using namespace Eigen;
+
+class Controller {
+    public:
+        Controller(Matrix3d K_p_, Matrix3d K_i_, Matrix3d K_zmp_, Matrix3d K_com_);
+        Vector3d dcmController(Vector3d xiRef, Vector3d xiDotRef, Vector3d xiReal, double deltaZVRP);
+        Vector3d comController(Vector3d xCOMRef, Vector3d xDotCOMRef, Vector3d xCOMReal, Vector3d rZMPRef, Vector3d rZMPReal);
+        void setK_p_(Matrix3d K_p);
+        void setK_i_(Matrix3d K_i);
+        void setK_zmp_(Matrix3d K_zmp);
+        void setK_com_(Matrix3d K_com);
+
+
+    private:
+        Matrix3d K_p_;
+        Matrix3d K_i_;
+        Matrix3d K_zmp_;
+        Matrix3d K_com_;
+        Vector3d xiErrorInt;
+};
diff --git a/trajectory_planner/src/Controller.cpp b/trajectory_planner/src/Controller.cpp
@@ -0,0 +1,81 @@
+#include "Controller.h"
+
+
+Controller::Controller(Matrix3d K_p, Matrix3d K_i, Matrix3d K_zmp, Matrix3d K_com){
+    setK_p_(K_p);
+    setK_i_(K_i);
+    setK_zmp_(K_zmp);
+    setK_com_(K_com);
+    //this->K_p_ = K_p;
+    //this->K_i_ = K_i;
+    //this->K_zmp_ = K_zmp;
+    //this->K_com_ = K_com;
+    xiErrorInt << 0.0, 0.0, 0.0;
+}
+
+Vector3d Controller::dcmController(Vector3d xiRef, Vector3d xiDotRef, Vector3d xiReal, double deltaZVRP){
+    xiErrorInt += xiReal - xiRef;
+    Vector3d xiError = xiReal - xiRef;
+    Vector3d rRefZMP;
+    rRefZMP = xiRef - xiDotRef/sqrt(9.81/deltaZVRP) + (K_p_*K_p_) * xiError + (K_i_*K_i_) * xiErrorInt;
+    return rRefZMP;
+}
+
+Vector3d Controller::comController(Vector3d xCOMRef, Vector3d xDotCOMRef, Vector3d xCOMReal, Vector3d rZMPRef, Vector3d rZMPReal){
+    Vector3d xDotStar;
+    xDotStar = xDotCOMRef - K_zmp_*(rZMPRef - rZMPReal) + K_com_*(xCOMRef - xCOMReal);
+    return xDotStar;
+}
+void Controller::setK_p_(Matrix3d K_p){
+    this->K_p_ = K_p;
+}
+void Controller::setK_i_(Matrix3d K_i){
+    this->K_i_ = K_i;
+}
+void Controller::setK_zmp_(Matrix3d K_zmp){
+    this->K_zmp_ = K_zmp;
+}
+void Controller::setK_com_(Matrix3d K_com){
+    this->K_com_ = K_com;
+}
+
+int main(){
+    Matrix3d kp_;
+    kp_<<1.0, 1.0, 1.0,
+        1.0, 1.0, 1.0,
+        1.0, 1.0, 1.0;
+    Matrix3d ki_;
+    ki_<<1.0, 1.0, 1.0,
+        1.0, 1.0, 1.0,
+        1.0, 1.0, 1.0;
+    Matrix3d kzmp_;
+    kzmp_<<1.0, 1.0, 1.0,
+           1.0, 1.0, 1.0,
+           1.0, 1.0, 1.0;
+    Matrix3d kcom_;
+    kcom_<<1.0, 1.0, 1.0,
+           1.0, 1.0, 1.0,
+           1.0, 1.0, 1.0;
+    Vector3d xiRef;
+    xiRef<<1.0, 1.0, 1.0;
+    Vector3d xiDotRef;
+    xiDotRef<<1.0, 1.0, 1.0;
+    Vector3d xiReal;
+    xiReal<<1.0, 1.0, 1.0;
+    double deltaZVRP = 1.0;
+    Vector3d result ;
+
+
+
+
+
+
+    Controller kosammat(kp_, ki_ , kzmp_, kcom_);
+    result = kosammat.dcmController(xiRef, xiDotRef, xiReal, deltaZVRP);
+    cout << result <<endl;
+
+
+}
+
+
+