fixed the frequency problem

commands/keyboard_input/src/KeyboardInput.cpp

@@ -27,10 +27,8 @@ void KeyboardInput::timer_callback() {
             inputs_.rx = 0;
             inputs_.ry = 0;
         }
-    } else {
+        publisher_->publish(inputs_);
-        inputs_.command = 0;
     }
-    publisher_->publish(inputs_);
 }
 
 void KeyboardInput::check_command(const char key) {

controllers/unitree_guide_controller/CMakeLists.txt

@@ -24,14 +24,23 @@ foreach (Dependency IN ITEMS ${CONTROLLER_INCLUDE_DEPENDS})
 endforeach ()
 
 add_library(${PROJECT_NAME} SHARED
+
         src/UnitreeGuideController.cpp
+
         src/FSM/StatePassive.cpp
         src/FSM/StateFixedDown.cpp
         src/FSM/StateFixedStand.cpp
         src/FSM/StateSwingTest.cpp
         src/FSM/StateFreeStand.cpp
-        src/robotics/QuadrupedRobot.cpp
+
-        src/robotics/RobotLeg.cpp
+        src/robot/QuadrupedRobot.cpp
+        src/robot/RobotLeg.cpp
+
+        src/control/Estimator.cpp
+
+        src/quadProgpp/Array.cc
+        src/quadProgpp/QuadProg++.cc
+
 )
 target_include_directories(${PROJECT_NAME}
         PUBLIC

controllers/unitree_guide_controller/config/controller.yaml

@@ -1,7 +1,6 @@
 unitree_guide_controller:
   ros__parameters:
     type: unitree_guide_controller/UnitreeGuideController
-    update_rate: 200  # Hz
     joints:
       - FR_hip_joint
       - FR_thigh_joint

controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/FSMState.h

@@ -9,7 +9,7 @@
 #include <utility>
 #include <rclcpp/time.hpp>
 #include <unitree_guide_controller/common/enumClass.h>
-#include <unitree_guide_controller/common/interface.h>
+#include <unitree_guide_controller/control/CtrlComponent.h>
 
 class FSMState {
 public:

controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateFixedDown.h

@@ -28,7 +28,7 @@ private:
     double start_pos_[12] = {};
     rclcpp::Time start_time_;
 
-    double duration_ = 6000; // steps
+    double duration_ = 600; // steps
     double percent_ = 0; //%
     double phase = 0.0;
 };

controllers/unitree_guide_controller/include/unitree_guide_controller/FSM/StateFixedStand.h

@@ -29,7 +29,7 @@ private:
     double start_pos_[12] = {};
     rclcpp::Time start_time_;
 
-    double duration_ = 6000; // steps
+    double duration_ = 600; // steps
     double percent_ = 0; //%
     double phase = 0.0;
 };

controllers/unitree_guide_controller/include/unitree_guide_controller/UnitreeGuideController.h

@@ -96,6 +96,9 @@ namespace unitree_guide_controller {
         std::shared_ptr<FSMState> current_state_;
         std::shared_ptr<FSMState> next_state_;
 
+        std::chrono::time_point<std::chrono::steady_clock> last_update_time_;
+        double update_frequency_;
+
         std::shared_ptr<FSMState> getNextState(FSMStateName stateName) const;
 
         std::unordered_map<

controllers/unitree_guide_controller/include/unitree_guide_controller/control/CtrlComponent.h

@@ -9,7 +9,7 @@
 #include <hardware_interface/loaned_command_interface.hpp>
 #include <hardware_interface/loaned_state_interface.hpp>
 #include <control_input_msgs/msg/inputs.hpp>
-#include <unitree_guide_controller/robotics/QuadrupedRobot.h>
+#include <unitree_guide_controller/robot/QuadrupedRobot.h>
 
 struct CtrlComponent {
     std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> >
@@ -38,7 +38,7 @@ struct CtrlComponent {
     QuadrupedRobot default_robot_model_;
     std::reference_wrapper<QuadrupedRobot> robot_model_;
 
-    CtrlComponent() : control_inputs_(default_inputs_), default_robot_model_(), robot_model_(default_robot_model_) {
+    CtrlComponent() : control_inputs_(default_inputs_), robot_model_(default_robot_model_) {
     }
 
     void clear() {

controllers/unitree_guide_controller/include/unitree_guide_controller/control/Estimator.h

@@ -0,0 +1,16 @@
+//
+// Created by biao on 24-9-14.
+//
+
+#ifndef ESTIMATOR_H
+#define ESTIMATOR_H
+
+
+
+class Estimator {
+
+};
+
+
+
+#endif //ESTIMATOR_H

controllers/unitree_guide_controller/include/unitree_guide_controller/quadProgpp/QuadProg++.hh

@@ -0,0 +1,78 @@
+/*
+ File $Id: QuadProg++.hh 232 2007-06-21 12:29:00Z digasper $
+
+ The quadprog_solve() function implements the algorithm of Goldfarb and Idnani
+ for the solution of a (convex) Quadratic Programming problem
+ by means of an active-set dual method.
+
+The problem is in the form:
+
+min 0.5 * x G x + g0 x
+s.t.
+    CE^T x + ce0 = 0
+    CI^T x + ci0 >= 0
+
+ The matrix and vectors dimensions are as follows:
+     G: n * n
+                g0: n
+
+                CE: n * p
+         ce0: p
+
+          CI: n * m
+   ci0: m
+
+     x: n
+
+ The function will return the cost of the solution written in the x vector or
+ std::numeric_limits::infinity() if the problem is infeasible. In the latter
+case the value of the x vector is not correct.
+
+ References: D. Goldfarb, A. Idnani. A numerically stable dual method for
+solving strictly convex quadratic programs. Mathematical Programming 27 (1983)
+pp. 1-33.
+
+ Notes:
+  1. pay attention in setting up the vectors ce0 and ci0.
+           If the constraints of your problem are specified in the form
+           A^T x = b and C^T x >= d, then you should set ce0 = -b and ci0 = -d.
+  2. The matrices have column dimension equal to MATRIX_DIM,
+     a constant set to 20 in this file (by means of a #define macro).
+     If the matrices are bigger than 20 x 20 the limit could be
+                 increased by means of a -DMATRIX_DIM=n on the compiler command
+line.
+  3. The matrix G is modified within the function since it is used to compute
+     the G = L^T L cholesky factorization for further computations inside the
+function. If you need the original matrix G you should make a copy of it and
+pass the copy to the function.
+
+ Author: Luca Di Gaspero
+                         DIEGM - University of Udine, Italy
+                                 luca.digaspero@uniud.it
+                                 http://www.diegm.uniud.it/digaspero/
+
+ The author will be grateful if the researchers using this software will
+ acknowledge the contribution of this function in their research papers.
+
+ Copyright (c) 2007-2016 Luca Di Gaspero
+
+ This software may be modified and distributed under the terms
+ of the MIT license.  See the LICENSE file for details.
+*/
+
+#ifndef _QUADPROGPP
+#define _QUADPROGPP
+
+#include "Array.hh"
+#include <eigen3/Eigen/Dense>
+
+namespace quadprogpp {
+
+double solve_quadprog(Matrix<double>& G, Vector<double>& g0,
+                      const Matrix<double>& CE, const Vector<double>& ce0,
+                      const Matrix<double>& CI, const Vector<double>& ci0,
+                      Vector<double>& x);
+
+}  // namespace quadprogpp
+
+#endif  // #define _QUADPROGPP

controllers/unitree_guide_controller/src/FSM/StateFixedDown.cpp

@@ -2,12 +2,13 @@
 // Created by tlab-uav on 24-9-11.
 //
 
+#include "unitree_guide_controller/FSM/StateFixedDown.h"
+
 #include <cmath>
-#include <unitree_guide_controller/FSM/StateFixedDown.h>
 
 StateFixedDown::StateFixedDown(CtrlComponent ctrlComp): FSMState(
     FSMStateName::FIXEDDOWN, "fixed down", std::move(ctrlComp)) {
-    // duration_ = ctrlComp_.frequency_ * 15;
+    duration_ = ctrlComp_.frequency_ * 1.2;
 }
 
 void StateFixedDown::enter() {
@@ -34,7 +35,7 @@ void StateFixedDown::exit() {
 }
 
 FSMStateName StateFixedDown::checkChange() {
-    if (percent_ < 2) {
+    if (percent_ < 1.5) {
         return FSMStateName::FIXEDDOWN;
     }
     switch (ctrlComp_.control_inputs_.get().command) {

controllers/unitree_guide_controller/src/FSM/StateFixedStand.cpp

@@ -2,12 +2,13 @@
 // Created by biao on 24-9-10.
 //
 
+#include "unitree_guide_controller/FSM/StateFixedStand.h"
+
 #include <cmath>
-#include <iostream>
-#include <unitree_guide_controller/FSM/StateFixedStand.h>
 
 StateFixedStand::StateFixedStand(CtrlComponent ctrlComp): FSMState(
     FSMStateName::FIXEDSTAND, "fixed stand", std::move(ctrlComp)) {
+    duration_ = ctrlComp_.frequency_ * 1.2;
 }
 
 void StateFixedStand::enter() {
@@ -35,7 +36,7 @@ void StateFixedStand::exit() {
 }
 
 FSMStateName StateFixedStand::checkChange() {
-    if (percent_ < 2) {
+    if (percent_ < 1.5) {
         return FSMStateName::FIXEDSTAND;
     }
     switch (ctrlComp_.control_inputs_.get().command) {

controllers/unitree_guide_controller/src/FSM/StateFreeStand.cpp

@@ -2,8 +2,8 @@
 // Created by tlab-uav on 24-9-13.
 //
 
-#include <unitree_guide_controller/common/mathTools.h>
+#include "unitree_guide_controller/FSM/StateFreeStand.h"
-#include <unitree_guide_controller/FSM/StateFreeStand.h>
+#include "unitree_guide_controller/common/mathTools.h"
 
 StateFreeStand::StateFreeStand(CtrlComponent ctrl_component) : FSMState(FSMStateName::FREESTAND, "free stand",
                                                                         std::move(ctrl_component)) {

controllers/unitree_guide_controller/src/FSM/StatePassive.cpp

@@ -2,9 +2,9 @@
 // Created by tlab-uav on 24-9-6.
 //
 
-#include <iostream>
+#include "unitree_guide_controller/FSM/StatePassive.h"
-#include <unitree_guide_controller/FSM/StatePassive.h>
 
+#include <iostream>
 #include <utility>
 
 StatePassive::StatePassive(CtrlComponent ctrlComp) : FSMState(

controllers/unitree_guide_controller/src/FSM/StateSwingTest.cpp

@@ -3,8 +3,8 @@
 //
 
 #include <iostream>
-#include <unitree_guide_controller/FSM/StateSwingTest.h>
+#include "unitree_guide_controller/FSM/StateSwingTest.h"
-#include <unitree_guide_controller/common/mathTools.h>
+#include "unitree_guide_controller/common/mathTools.h"
 
 StateSwingTest::StateSwingTest(CtrlComponent ctrlComp): FSMState(
     FSMStateName::SWINGTEST, "swing test", std::move(ctrlComp)) {

controllers/unitree_guide_controller/src/UnitreeGuideController.cpp

@@ -2,9 +2,9 @@
 // Created by tlab-uav on 24-9-6.
 //
 
-#include <unitree_guide_controller/UnitreeGuideController.h>
+#include "unitree_guide_controller/UnitreeGuideController.h"
-#include <unitree_guide_controller/FSM/StatePassive.h>
+#include "unitree_guide_controller/FSM/StatePassive.h"
-#include <unitree_guide_controller/robotics/QuadrupedRobot.h>
+#include "unitree_guide_controller/robot/QuadrupedRobot.h"
 
 namespace unitree_guide_controller {
     using config_type = controller_interface::interface_configuration_type;

controllers/unitree_guide_controller/src/control/Estimator.cpp

@@ -0,0 +1,5 @@
+//
+// Created by biao on 24-9-14.
+//
+
+#include "unitree_guide_controller/control/Estimator.h"

controllers/unitree_guide_controller/src/quadProgpp/Array.cc

@@ -0,0 +1,28 @@
+// $Id: Array.cc 201 2008-05-18 19:47:38Z digasper $
+// This file is part of QuadProg++:
+// Copyright (C) 2006--2009 Luca Di Gaspero.
+//
+// This software may be modified and distributed under the terms
+// of the MIT license.  See the LICENSE file for details.
+
+#include "unitree_guide_controller/quadProgpp/Array.hh"
+
+/**
+  Index utilities
+ */
+
+namespace quadprogpp {
+    std::set<unsigned int> seq(unsigned int s, unsigned int e) {
+        std::set<unsigned int> tmp;
+        for (unsigned int i = s; i <= e; i++) tmp.insert(i);
+
+        return tmp;
+    }
+
+    std::set<unsigned int> singleton(unsigned int i) {
+        std::set<unsigned int> tmp;
+        tmp.insert(i);
+
+        return tmp;
+    }
+} // namespace quadprogpp

controllers/unitree_guide_controller/src/quadProgpp/QuadProg++.cc

@@ -0,0 +1,729 @@
+/*
+File $Id: QuadProg++.cc 232 2007-06-21 12:29:00Z digasper $
+
+ Author: Luca Di Gaspero
+ DIEGM - University of Udine, Italy
+ luca.digaspero@uniud.it
+ http://www.diegm.uniud.it/digaspero/
+
+ This software may be modified and distributed under the terms
+ of the MIT license.  See the LICENSE file for details.
+
+ */
+
+#include <iostream>
+#include <algorithm>
+#include <cmath>
+#include <limits>
+#include <sstream>
+#include <stdexcept>
+#include "unitree_guide_controller/quadProgpp/QuadProg++.hh"
+// #define TRACE_SOLVER
+
+namespace quadprogpp {
+    // Utility functions for updating some data needed by the solution method
+    void compute_d(Vector<double> &d, const Matrix<double> &J,
+                   const Vector<double> &np);
+
+    void update_z(Vector<double> &z, const Matrix<double> &J,
+                  const Vector<double> &d, int iq);
+
+    void update_r(const Matrix<double> &R, Vector<double> &r,
+                  const Vector<double> &d, int iq);
+
+    bool add_constraint(Matrix<double> &R, Matrix<double> &J, Vector<double> &d,
+                        unsigned int &iq, double &rnorm);
+
+    void delete_constraint(Matrix<double> &R, Matrix<double> &J, Vector<int> &A,
+                           Vector<double> &u, unsigned int n, int p,
+                           unsigned int &iq, int l);
+
+    // Utility functions for computing the Cholesky decomposition and solving
+    // linear systems
+    void cholesky_decomposition(Matrix<double> &A);
+
+    void cholesky_solve(const Matrix<double> &L, Vector<double> &x,
+                        const Vector<double> &b);
+
+    void forward_elimination(const Matrix<double> &L, Vector<double> &y,
+                             const Vector<double> &b);
+
+    void backward_elimination(const Matrix<double> &U, Vector<double> &x,
+                              const Vector<double> &y);
+
+    // Utility functions for computing the scalar product and the euclidean
+    // distance between two numbers
+    double scalar_product(const Vector<double> &x, const Vector<double> &y);
+
+    double distance(double a, double b);
+
+    // Utility functions for printing vectors and matrices
+    void print_matrix(const char *name, const Matrix<double> &A, int n = -1,
+                      int m = -1);
+
+    template<typename T>
+    void print_vector(const char *name, const Vector<T> &v, int n = -1);
+
+    // The Solving function, implementing the Goldfarb-Idnani method
+    double solve_quadprog(Matrix<double> &G, Vector<double> &g0,
+                          const Matrix<double> &CE, const Vector<double> &ce0,
+                          const Matrix<double> &CI, const Vector<double> &ci0,
+                          Vector<double> &x) {
+        std::ostringstream msg;
+        unsigned int n = G.ncols(), p = CE.ncols(), m = CI.ncols();
+        if (G.nrows() != n) {
+            msg << "The matrix G is not a squared matrix (" << G.nrows() << " x "
+                    << G.ncols() << ")";
+            throw std::logic_error(msg.str());
+        }
+        if (CE.nrows() != n) {
+            msg << "The matrix CE is incompatible (incorrect number of rows "
+                    << CE.nrows() << " , expecting " << n << ")";
+            throw std::logic_error(msg.str());
+        }
+        if (ce0.size() != p) {
+            msg << "The vector ce0 is incompatible (incorrect dimension " << ce0.size()
+                    << ", expecting " << p << ")";
+            throw std::logic_error(msg.str());
+        }
+        if (CI.nrows() != n) {
+            msg << "The matrix CI is incompatible (incorrect number of rows "
+                    << CI.nrows() << " , expecting " << n << ")";
+            throw std::logic_error(msg.str());
+        }
+        if (ci0.size() != m) {
+            msg << "The vector ci0 is incompatible (incorrect dimension " << ci0.size()
+                    << ", expecting " << m << ")";
+            throw std::logic_error(msg.str());
+        }
+        x.resize(n);
+        unsigned int i, j, k, l; /* indices */
+        int ip; // this is the index of the constraint to be added to the active set
+        Matrix<double> R(n, n), J(n, n);
+        Vector<double> s(m + p), z(n), r(m + p), d(n), np(n), u(m + p), x_old(n),
+                u_old(m + p);
+        double f_value, psi, c1, c2, sum, ss, R_norm;
+        double inf;
+        if (std::numeric_limits<double>::has_infinity)
+            inf = std::numeric_limits<double>::infinity();
+        else
+            inf = 1.0E300;
+        double t, t1, t2; /* t is the step lenght, which is the minimum of the partial
+                     * step length t1 and the full step length t2 */
+        Vector<int> A(m + p), A_old(m + p), iai(m + p);
+        unsigned int iq, iter = 0;
+        Vector<bool> iaexcl(m + p);
+
+        /* p is the number of equality constraints */
+        /* m is the number of inequality constraints */
+#ifdef TRACE_SOLVER
+  std::cout << std::endl << "Starting solve_quadprog" << std::endl;
+  print_matrix("G", G);
+  print_vector("g0", g0);
+  print_matrix("CE", CE);
+  print_vector("ce0", ce0);
+  print_matrix("CI", CI);
+  print_vector("ci0", ci0);
+#endif
+
+        /*
+         * Preprocessing phase
+         */
+
+        /* compute the trace of the original matrix G */
+        c1 = 0.0;
+        for (i = 0; i < n; i++) {
+            c1 += G[i][i];
+        }
+        /* decompose the matrix G in the form L^T L */
+        cholesky_decomposition(G);
+#ifdef TRACE_SOLVER
+  print_matrix("G", G);
+#endif
+        /* initialize the matrix R */
+        for (i = 0; i < n; i++) {
+            d[i] = 0.0;
+            for (j = 0; j < n; j++) R[i][j] = 0.0;
+        }
+        R_norm = 1.0; /* this variable will hold the norm of the matrix R */
+
+        /* compute the inverse of the factorized matrix G^-1, this is the initial
+         * value for H */
+        c2 = 0.0;
+        for (i = 0; i < n; i++) {
+            d[i] = 1.0;
+            forward_elimination(G, z, d);
+            for (j = 0; j < n; j++) J[i][j] = z[j];
+            c2 += z[i];
+            d[i] = 0.0;
+        }
+#ifdef TRACE_SOLVER
+  print_matrix("J", J);
+#endif
+
+        /* c1 * c2 is an estimate for cond(G) */
+
+        /*
+         * Find the unconstrained minimizer of the quadratic form 0.5 * x G x + g0 x
+         * this is a feasible point in the dual space
+         * x = G^-1 * g0
+         */
+        cholesky_solve(G, x, g0);
+        for (i = 0; i < n; i++) x[i] = -x[i];
+        /* and compute the current solution value */
+        f_value = 0.5 * scalar_product(g0, x);
+#ifdef TRACE_SOLVER
+  std::cout << "Unconstrained solution: " << f_value << std::endl;
+  print_vector("x", x);
+#endif
+
+        /* Add equality constraints to the working set A */
+        iq = 0;
+        for (i = 0; i < p; i++) {
+            for (j = 0; j < n; j++) np[j] = CE[j][i];
+            compute_d(d, J, np);
+            update_z(z, J, d, iq);
+            update_r(R, r, d, iq);
+#ifdef TRACE_SOLVER
+    print_matrix("R", R, n, iq);
+    print_vector("z", z);
+    print_vector("r", r, iq);
+    print_vector("d", d);
+#endif
+
+            /* compute full step length t2: i.e., the minimum step in primal space s.t.
+              the contraint becomes feasible */
+            t2 = 0.0;
+            if (fabs(scalar_product(z, z)) >
+                std::numeric_limits<double>::epsilon()) // i.e. z != 0
+                t2 = (-scalar_product(np, x) - ce0[i]) / scalar_product(z, np);
+
+            /* set x = x + t2 * z */
+            for (k = 0; k < n; k++) x[k] += t2 * z[k];
+
+            /* set u = u+ */
+            u[iq] = t2;
+            for (k = 0; k < iq; k++) u[k] -= t2 * r[k];
+
+            /* compute the new solution value */
+            f_value += 0.5 * (t2 * t2) * scalar_product(z, np);
+            A[i] = -i - 1;
+
+            if (!add_constraint(R, J, d, iq, R_norm)) {
+                // Equality constraints are linearly dependent
+                throw std::runtime_error("Constraints are linearly dependent");
+                return f_value;
+            }
+        }
+
+        /* set iai = K \ A */
+        for (i = 0; i < m; i++) iai[i] = i;
+
+    l1:
+        iter++;
+#ifdef TRACE_SOLVER
+  print_vector("x", x);
+#endif
+        /* step 1: choose a violated constraint */
+        for (i = p; i < iq; i++) {
+            ip = A[i];
+            iai[ip] = -1;
+        }
+
+        /* compute s[x] = ci^T * x + ci0 for all elements of K \ A */
+        ss = 0.0;
+        psi = 0.0; /* this value will contain the sum of all infeasibilities */
+        ip = 0; /* ip will be the index of the chosen violated constraint */
+        for (i = 0; i < m; i++) {
+            iaexcl[i] = true;
+            sum = 0.0;
+            for (j = 0; j < n; j++) sum += CI[j][i] * x[j];
+            sum += ci0[i];
+            s[i] = sum;
+            psi += std::min(0.0, sum);
+        }
+#ifdef TRACE_SOLVER
+  print_vector("s", s, m);
+#endif
+
+        if (fabs(psi) <=
+            m * std::numeric_limits<double>::epsilon() * c1 * c2 * 100.0) {
+            /* numerically there are not infeasibilities anymore */
+            return f_value;
+        }
+
+        /* save old values for u and A */
+        for (i = 0; i < iq; i++) {
+            u_old[i] = u[i];
+            A_old[i] = A[i];
+        }
+        /* and for x */
+        for (i = 0; i < n; i++) x_old[i] = x[i];
+
+    l2: /* Step 2: check for feasibility and determine a new S-pair */
+        for (i = 0; i < m; i++) {
+            if (s[i] < ss && iai[i] != -1 && iaexcl[i]) {
+                ss = s[i];
+                ip = i;
+            }
+        }
+        if (ss >= 0.0) {
+            return f_value;
+        }
+
+        /* set np = n[ip] */
+        for (i = 0; i < n; i++) np[i] = CI[i][ip];
+        /* set u = [u 0]^T */
+        u[iq] = 0.0;
+        /* add ip to the active set A */
+        A[iq] = ip;
+
+#ifdef TRACE_SOLVER
+  std::cout << "Trying with constraint " << ip << std::endl;
+  print_vector("np", np);
+#endif
+
+    l2a: /* Step 2a: determine step direction */
+        /* compute z = H np: the step direction in the primal space (through J, see
+         * the paper) */
+        compute_d(d, J, np);
+        update_z(z, J, d, iq);
+        /* compute N* np (if q > 0): the negative of the step direction in the dual
+         * space */
+        update_r(R, r, d, iq);
+#ifdef TRACE_SOLVER
+  std::cout << "Step direction z" << std::endl;
+  print_vector("z", z);
+  print_vector("r", r, iq + 1);
+  print_vector("u", u, iq + 1);
+  print_vector("d", d);
+  print_vector("A", A, iq + 1);
+#endif
+
+        /* Step 2b: compute step length */
+        l = 0;
+        /* Compute t1: partial step length (maximum step in dual space without
+         * violating dual feasibility */
+        t1 = inf; /* +inf */
+        /* find the index l s.t. it reaches the minimum of u+[x] / r */
+        for (k = p; k < iq; k++) {
+            if (r[k] > 0.0) {
+                if (u[k] / r[k] < t1) {
+                    t1 = u[k] / r[k];
+                    l = A[k];
+                }
+            }
+        }
+        /* Compute t2: full step length (minimum step in primal space such that the
+         * constraint ip becomes feasible */
+        if (fabs(scalar_product(z, z)) >
+            std::numeric_limits<double>::epsilon()) // i.e. z != 0
+        {
+            t2 = -s[ip] / scalar_product(z, np);
+            if (t2 < 0) // patch suggested by Takano Akio for handling numerical
+                // inconsistencies
+                t2 = inf;
+        } else
+            t2 = inf; /* +inf */
+
+        /* the step is chosen as the minimum of t1 and t2 */
+        t = std::min(t1, t2);
+#ifdef TRACE_SOLVER
+  std::cout << "Step sizes: " << t << " (t1 = " << t1 << ", t2 = " << t2
+            << ") ";
+#endif
+
+        /* Step 2c: determine new S-pair and take step: */
+
+        /* case (i): no step in primal or dual space */
+        if (t >= inf) {
+            /* QPP is infeasible */
+            // FIXME: unbounded to raise
+            return inf;
+        }
+        /* case (ii): step in dual space */
+        if (t2 >= inf) {
+            /* set u = u +  t * [-r 1] and drop constraint l from the active set A */
+            for (k = 0; k < iq; k++) u[k] -= t * r[k];
+            u[iq] += t;
+            iai[l] = l;
+            delete_constraint(R, J, A, u, n, p, iq, l);
+#ifdef TRACE_SOLVER
+    std::cout << " in dual space: " << f_value << std::endl;
+    print_vector("x", x);
+    print_vector("z", z);
+    print_vector("A", A, iq + 1);
+#endif
+            goto l2a;
+        }
+
+        /* case (iii): step in primal and dual space */
+
+        /* set x = x + t * z */
+        for (k = 0; k < n; k++) x[k] += t * z[k];
+        /* update the solution value */
+        f_value += t * scalar_product(z, np) * (0.5 * t + u[iq]);
+        /* u = u + t * [-r 1] */
+        for (k = 0; k < iq; k++) u[k] -= t * r[k];
+        u[iq] += t;
+#ifdef TRACE_SOLVER
+  std::cout << " in both spaces: " << f_value << std::endl;
+  print_vector("x", x);
+  print_vector("u", u, iq + 1);
+  print_vector("r", r, iq + 1);
+  print_vector("A", A, iq + 1);
+#endif
+
+        if (fabs(t - t2) < std::numeric_limits<double>::epsilon()) {
+#ifdef TRACE_SOLVER
+    std::cout << "Full step has taken " << t << std::endl;
+    print_vector("x", x);
+#endif
+            /* full step has taken */
+            /* add constraint ip to the active set*/
+            if (!add_constraint(R, J, d, iq, R_norm)) {
+                iaexcl[ip] = false;
+                delete_constraint(R, J, A, u, n, p, iq, ip);
+#ifdef TRACE_SOLVER
+      print_matrix("R", R);
+      print_vector("A", A, iq);
+      print_vector("iai", iai);
+#endif
+                for (i = 0; i < m; i++) iai[i] = i;
+                for (i = p; i < iq; i++) {
+                    A[i] = A_old[i];
+                    u[i] = u_old[i];
+                    iai[A[i]] = -1;
+                }
+                for (i = 0; i < n; i++) x[i] = x_old[i];
+                goto l2; /* go to step 2 */
+            } else
+                iai[ip] = -1;
+#ifdef TRACE_SOLVER
+    print_matrix("R", R);
+    print_vector("A", A, iq);
+    print_vector("iai", iai);
+#endif
+            goto l1;
+        }
+
+        /* a patial step has taken */
+#ifdef TRACE_SOLVER
+  std::cout << "Partial step has taken " << t << std::endl;
+  print_vector("x", x);
+#endif
+        /* drop constraint l */
+        iai[l] = l;
+        delete_constraint(R, J, A, u, n, p, iq, l);
+#ifdef TRACE_SOLVER
+  print_matrix("R", R);
+  print_vector("A", A, iq);
+#endif
+
+        /* update s[ip] = CI * x + ci0 */
+        sum = 0.0;
+        for (k = 0; k < n; k++) sum += CI[k][ip] * x[k];
+        s[ip] = sum + ci0[ip];
+
+#ifdef TRACE_SOLVER
+  print_vector("s", s, m);
+#endif
+        goto l2a;
+    }
+
+    inline void compute_d(Vector<double> &d, const Matrix<double> &J,
+                          const Vector<double> &np) {
+        int i, j, n = d.size();
+        double sum;
+
+        /* compute d = H^T * np */
+        for (i = 0; i < n; i++) {
+            sum = 0.0;
+            for (j = 0; j < n; j++) sum += J[j][i] * np[j];
+            d[i] = sum;
+        }
+    }
+
+    inline void update_z(Vector<double> &z, const Matrix<double> &J,
+                         const Vector<double> &d, int iq) {
+        int i, j, n = z.size();
+
+        /* setting of z = H * d */
+        for (i = 0; i < n; i++) {
+            z[i] = 0.0;
+            for (j = iq; j < n; j++) z[i] += J[i][j] * d[j];
+        }
+    }
+
+    inline void update_r(const Matrix<double> &R, Vector<double> &r,
+                         const Vector<double> &d, int iq) {
+        int i, j;
+        double sum;
+
+        /* setting of r = R^-1 d */
+        for (i = iq - 1; i >= 0; i--) {
+            sum = 0.0;
+            for (j = i + 1; j < iq; j++) sum += R[i][j] * r[j];
+            r[i] = (d[i] - sum) / R[i][i];
+        }
+    }
+
+    bool add_constraint(Matrix<double> &R, Matrix<double> &J, Vector<double> &d,
+                        unsigned int &iq, double &R_norm) {
+        unsigned int n = d.size();
+#ifdef TRACE_SOLVER
+  std::cout << "Add constraint " << iq << '/';
+#endif
+        unsigned int i, j, k;
+        double cc, ss, h, t1, t2, xny;
+
+        /* we have to find the Givens rotation which will reduce the element
+          d[j] to zero.
+          if it is already zero we don't have to do anything, except of
+          decreasing j */
+        for (j = n - 1; j >= iq + 1; j--) {
+            /* The Givens rotation is done with the matrix (cc cs, cs -cc).
+            If cc is one, then element (j) of d is zero compared with element
+            (j - 1). Hence we don't have to do anything.
+            If cc is zero, then we just have to switch column (j) and column (j - 1)
+            of J. Since we only switch columns in J, we have to be careful how we
+            update d depending on the sign of gs.
+            Otherwise we have to apply the Givens rotation to these columns.
+            The i - 1 element of d has to be updated to h. */
+            cc = d[j - 1];
+            ss = d[j];
+            h = distance(cc, ss);
+            if (fabs(h) < std::numeric_limits<double>::epsilon()) // h == 0
+                continue;
+            d[j] = 0.0;
+            ss = ss / h;
+            cc = cc / h;
+            if (cc < 0.0) {
+                cc = -cc;
+                ss = -ss;
+                d[j - 1] = -h;
+            } else
+                d[j - 1] = h;
+            xny = ss / (1.0 + cc);
+            for (k = 0; k < n; k++) {
+                t1 = J[k][j - 1];
+                t2 = J[k][j];
+                J[k][j - 1] = t1 * cc + t2 * ss;
+                J[k][j] = xny * (t1 + J[k][j - 1]) - t2;
+            }
+        }
+        /* update the number of constraints added*/
+        iq++;
+        /* To update R we have to put the iq components of the d vector
+          into column iq - 1 of R
+          */
+        for (i = 0; i < iq; i++) R[i][iq - 1] = d[i];
+#ifdef TRACE_SOLVER
+  std::cout << iq << std::endl;
+  print_matrix("R", R, iq, iq);
+  print_matrix("J", J);
+  print_vector("d", d, iq);
+#endif
+
+        if (fabs(d[iq - 1]) <= std::numeric_limits<double>::epsilon() * R_norm) {
+            // problem degenerate
+            return false;
+        }
+        R_norm = std::max<double>(R_norm, fabs(d[iq - 1]));
+        return true;
+    }
+
+    void delete_constraint(Matrix<double> &R, Matrix<double> &J, Vector<int> &A,
+                           Vector<double> &u, unsigned int n, int p,
+                           unsigned int &iq, int l) {
+#ifdef TRACE_SOLVER
+  std::cout << "Delete constraint " << l << ' ' << iq;
+#endif
+        unsigned int i, j, k,
+                qq = 0; // just to prevent warnings from smart compilers
+        double cc, ss, h, xny, t1, t2;
+
+        bool found = false;
+        /* Find the index qq for active constraint l to be removed */
+        for (i = p; i < iq; i++)
+            if (A[i] == l) {
+                qq = i;
+                found = true;
+                break;
+            }
+
+        if (!found) {
+            std::ostringstream os;
+            os << "Attempt to delete non existing constraint, constraint: " << l;
+            throw std::invalid_argument(os.str());
+        }
+        /* remove the constraint from the active set and the duals */
+        for (i = qq; i < iq - 1; i++) {
+            A[i] = A[i + 1];
+            u[i] = u[i + 1];
+            for (j = 0; j < n; j++) R[j][i] = R[j][i + 1];
+        }
+
+        A[iq - 1] = A[iq];
+        u[iq - 1] = u[iq];
+        A[iq] = 0;
+        u[iq] = 0.0;
+        for (j = 0; j < iq; j++) R[j][iq - 1] = 0.0;
+        /* constraint has been fully removed */
+        iq--;
+#ifdef TRACE_SOLVER
+  std::cout << '/' << iq << std::endl;
+#endif
+
+        if (iq == 0) return;
+
+        for (j = qq; j < iq; j++) {
+            cc = R[j][j];
+            ss = R[j + 1][j];
+            h = distance(cc, ss);
+            if (fabs(h) < std::numeric_limits<double>::epsilon()) // h == 0
+                continue;
+            cc = cc / h;
+            ss = ss / h;
+            R[j + 1][j] = 0.0;
+            if (cc < 0.0) {
+                R[j][j] = -h;
+                cc = -cc;
+                ss = -ss;
+            } else
+                R[j][j] = h;
+
+            xny = ss / (1.0 + cc);
+            for (k = j + 1; k < iq; k++) {
+                t1 = R[j][k];
+                t2 = R[j + 1][k];
+                R[j][k] = t1 * cc + t2 * ss;
+                R[j + 1][k] = xny * (t1 + R[j][k]) - t2;
+            }
+            for (k = 0; k < n; k++) {
+                t1 = J[k][j];
+                t2 = J[k][j + 1];
+                J[k][j] = t1 * cc + t2 * ss;
+                J[k][j + 1] = xny * (J[k][j] + t1) - t2;
+            }
+        }
+    }
+
+    inline double distance(double a, double b) {
+        double a1, b1, t;
+        a1 = fabs(a);
+        b1 = fabs(b);
+        if (a1 > b1) {
+            t = (b1 / a1);
+            return a1 * sqrt(1.0 + t * t);
+        } else if (b1 > a1) {
+            t = (a1 / b1);
+            return b1 * sqrt(1.0 + t * t);
+        }
+        return a1 * sqrt(2.0);
+    }
+
+    inline double scalar_product(const Vector<double> &x, const Vector<double> &y) {
+        int i, n = x.size();
+        double sum;
+
+        sum = 0.0;
+        for (i = 0; i < n; i++) sum += x[i] * y[i];
+        return sum;
+    }
+
+    void cholesky_decomposition(Matrix<double> &A) {
+        int i, j, k, n = A.nrows();
+        double sum;
+
+        for (i = 0; i < n; i++) {
+            for (j = i; j < n; j++) {
+                sum = A[i][j];
+                for (k = i - 1; k >= 0; k--) sum -= A[i][k] * A[j][k];
+                if (i == j) {
+                    if (sum <= 0.0) {
+                        std::ostringstream os;
+                        // raise error
+                        print_matrix("A", A);
+                        os << "Error in cholesky decomposition, sum: " << sum;
+                        throw std::logic_error(os.str());
+                        exit(-1);
+                    }
+                    A[i][i] = sqrt(sum);
+                } else
+                    A[j][i] = sum / A[i][i];
+            }
+            for (k = i + 1; k < n; k++) A[i][k] = A[k][i];
+        }
+    }
+
+    void cholesky_solve(const Matrix<double> &L, Vector<double> &x,
+                        const Vector<double> &b) {
+        int n = L.nrows();
+        Vector<double> y(n);
+
+        /* Solve L * y = b */
+        forward_elimination(L, y, b);
+        /* Solve L^T * x = y */
+        backward_elimination(L, x, y);
+    }
+
+    inline void forward_elimination(const Matrix<double> &L, Vector<double> &y,
+                                    const Vector<double> &b) {
+        int i, j, n = L.nrows();
+
+        y[0] = b[0] / L[0][0];
+        for (i = 1; i < n; i++) {
+            y[i] = b[i];
+            for (j = 0; j < i; j++) y[i] -= L[i][j] * y[j];
+            y[i] = y[i] / L[i][i];
+        }
+    }
+
+    inline void backward_elimination(const Matrix<double> &U, Vector<double> &x,
+                                     const Vector<double> &y) {
+        int i, j, n = U.nrows();
+
+        x[n - 1] = y[n - 1] / U[n - 1][n - 1];
+        for (i = n - 2; i >= 0; i--) {
+            x[i] = y[i];
+            for (j = i + 1; j < n; j++) x[i] -= U[i][j] * x[j];
+            x[i] = x[i] / U[i][i];
+        }
+    }
+
+    void print_matrix(const char *name, const Matrix<double> &A, int n, int m) {
+        std::ostringstream s;
+        std::string t;
+        if (n == -1) n = A.nrows();
+        if (m == -1) m = A.ncols();
+
+        s << name << ": " << std::endl;
+        for (int i = 0; i < n; i++) {
+            s << " ";
+            for (int j = 0; j < m; j++) s << A[i][j] << ", ";
+            s << std::endl;
+        }
+        t = s.str();
+        t = t.substr(
+            0, t.size() - 3); // To remove the trailing space, comma and newline
+
+        std::cout << t << std::endl;
+    }
+
+    template<typename T>
+    void print_vector(const char *name, const Vector<T> &v, int n) {
+        std::ostringstream s;
+        std::string t;
+        if (n == -1) n = v.size();
+
+        s << name << ": " << std::endl << " ";
+        for (int i = 0; i < n; i++) {
+            s << v[i] << ", ";
+        }
+        t = s.str();
+        t = t.substr(0, t.size() - 2); // To remove the trailing space and comma
+
+        std::cout << t << std::endl;
+    }
+} // namespace quadprogpp

controllers/unitree_guide_controller/src/robot/QuadrupedRobot.cpp

@@ -3,8 +3,8 @@
 //
 
 #include <iostream>
-#include <unitree_guide_controller/common/interface.h>
+#include "unitree_guide_controller/control/CtrlComponent.h"
-#include <unitree_guide_controller/robotics/QuadrupedRobot.h>
+#include "unitree_guide_controller/robot/QuadrupedRobot.h"
 
 void QuadrupedRobot::init(const std::string &robot_description) {
     KDL::Tree robot_tree;

controllers/unitree_guide_controller/src/robot/RobotLeg.cpp

@@ -3,7 +3,7 @@
 //
 
 #include <memory>
-#include <unitree_guide_controller/robotics/RobotLeg.h>
+#include "unitree_guide_controller/robot/RobotLeg.h"
 
 RobotLeg::RobotLeg(const KDL::Chain &chain) {
     chain_ = chain;
@@ -36,6 +36,6 @@ KDL::JntArray RobotLeg::calcTorque(const KDL::JntArray &joint_positions, const K
                                    const KDL::Wrenches &force) const {
     KDL::JntArray torque(chain_.getNrOfJoints());
     id_solver_->CartToJnt(joint_positions, joint_velocities, KDL::JntArray(chain_.getNrOfJoints()), force,
-                              torque);
+                          torque);
     return torque;
 }

descriptions/go2_description/config/robot_control.yaml

@@ -1,8 +1,7 @@
 # Controller Manager configuration
 controller_manager:
   ros__parameters:
-    update_rate: 200  # Hz
+    update_rate: 500  # Hz
-    use_sim_time: true  # If running in simulation
 
     # Define the available controllers
     joint_state_broadcaster:
@@ -11,7 +10,38 @@ controller_manager:
     imu_sensor_broadcaster:
       type: imu_sensor_broadcaster/IMUSensorBroadcaster
 
+    unitree_guide_controller:
+      type: unitree_guide_controller/UnitreeGuideController
+
 imu_sensor_broadcaster:
   ros__parameters:
     sensor_name: "imu_sensor"
-    frame_id: "imu_link"
+    frame_id: "imu_link"
+
+unitree_guide_controller:
+  ros__parameters:
+    joints:
+      - FR_hip_joint
+      - FR_thigh_joint
+      - FR_calf_joint
+      - FL_hip_joint
+      - FL_thigh_joint
+      - FL_calf_joint
+      - RR_hip_joint
+      - RR_thigh_joint
+      - RR_calf_joint
+      - RL_hip_joint
+      - RL_thigh_joint
+      - RL_calf_joint
+
+    command_interfaces:
+      - effort
+      - position
+      - velocity
+      - kp
+      - kd
+
+    state_interfaces:
+      - effort
+      - position
+      - velocity

descriptions/go2_description/launch/hardware.launch.py

@@ -3,8 +3,8 @@ import os
 import xacro
 from ament_index_python.packages import get_package_share_directory
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, OpaqueFunction, IncludeLaunchDescription
+from launch.actions import DeclareLaunchArgument, OpaqueFunction, IncludeLaunchDescription, RegisterEventHandler
-from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.event_handlers import OnProcessExit
 from launch.substitutions import PathJoinSubstitution
 from launch_ros.actions import Node
 from launch_ros.substitutions import FindPackageShare
@@ -29,40 +29,66 @@ def launch_setup(context, *args, **kwargs):
             "robot_control.yaml",
         ]
     )
+
+    robot_state_publisher = Node(
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        name='robot_state_publisher',
+        parameters=[
+            {
+                'publish_frequency': 20.0,
+                'use_tf_static': True,
+                'robot_description': robot_description,
+                'ignore_timestamp': True
+            }
+        ],
+    )
+
+    controller_manager = Node(
+        package="controller_manager",
+        executable="ros2_control_node",
+        parameters=[robot_controllers],
+        remappings=[
+            ("~/robot_description", "/robot_description"),
+        ],
+        output="both",
+    )
+
+    joint_state_publisher = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["joint_state_broadcaster",
+                   "--controller-manager", "/controller_manager"],
+    )
+
+    imu_sensor_broadcaster = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["imu_sensor_broadcaster",
+                   "--controller-manager", "/controller_manager"],
+    )
+
+    unitree_guide_controller = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["unitree_guide_controller", "--controller-manager", "/controller_manager"],
+    )
+
     return [
-        Node(
+        robot_state_publisher,
-            package='robot_state_publisher',
+        controller_manager,
-            executable='robot_state_publisher',
+        joint_state_publisher,
-            name='robot_state_publisher',
+        RegisterEventHandler(
-            parameters=[
+            event_handler=OnProcessExit(
-                {
+                target_action=joint_state_publisher,
-                    'publish_frequency': 20.0,
+                on_exit=[imu_sensor_broadcaster],
-                    'use_tf_static': True,
+            )
-                    'robot_description': robot_description,
-                    'ignore_timestamp': True
-                }
-            ],
         ),
-        Node(
+        RegisterEventHandler(
-            package="controller_manager",
+            event_handler=OnProcessExit(
-            executable="ros2_control_node",
+                target_action=imu_sensor_broadcaster,
-            parameters=[robot_controllers],
+                on_exit=[unitree_guide_controller],
-            remappings=[
+            )
-                ("~/robot_description", "/robot_description"),
-            ],
-            output="both",
-        ),
-        Node(
-            package="controller_manager",
-            executable="spawner",
-            arguments=["joint_state_broadcaster",
-                       "--controller-manager", "/controller_manager"],
-        ),
-        Node(
-            package="controller_manager",
-            executable="spawner",
-            arguments=["imu_sensor_broadcaster",
-                       "--controller-manager", "/controller_manager"],
         ),
     ]
 

descriptions/go2_description/package.xml

@@ -15,7 +15,6 @@
     <exec_depend>joint_state_publisher</exec_depend>
     <exec_depend>robot_state_publisher</exec_depend>
     <exec_depend>imu_sensor_broadcaster</exec_depend>
-
     <export>
         <build_type>ament_cmake</build_type>
     </export>