add gait manager

controllers/ocs2_quadruped_controller/CMakeLists.txt

@@ -51,6 +51,8 @@ add_library(${PROJECT_NAME} SHARED
         src/interface/LeggedRobotPreComputation.cpp
         src/interface/LeggedInterface.cpp
 
+        src/control/GaitManager.cpp
+
 )
 target_include_directories(${PROJECT_NAME}
         PUBLIC

controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/control/GaitManager.h

@@ -0,0 +1,42 @@
+//
+// Created by tlab-uav on 24-9-26.
+//
+
+#ifndef GAITMANAGER_H
+#define GAITMANAGER_H
+#include <ocs2_legged_robot/gait/GaitSchedule.h>
+#include <ocs2_oc/synchronized_module/SolverSynchronizedModule.h>
+
+#include "CtrlComponent.h"
+
+namespace ocs2::legged_robot {
+    class GaitManager final : public SolverSynchronizedModule {
+    public:
+        GaitManager(CtrlComponent &ctrl_component,
+                    std::shared_ptr<GaitSchedule> gait_schedule_ptr);
+
+        void preSolverRun(scalar_t initTime, scalar_t finalTime,
+                          const vector_t &currentState,
+                          const ReferenceManagerInterface &referenceManager) override;
+
+        void postSolverRun(const PrimalSolution &primalSolution) override {
+        }
+
+        void init(const std::string &gait_file);
+
+    private:
+        void getTargetGait();
+
+        CtrlComponent &ctrl_component_;
+        std::shared_ptr<GaitSchedule> gait_schedule_ptr_;
+
+        ModeSequenceTemplate target_gait_;
+        bool gait_updated_{false};
+        bool verbose_{false};
+        std::vector<ModeSequenceTemplate> gait_list_;
+        std::vector<std::string> gait_name_list_;
+    };
+}
+
+
+#endif //GAITMANAGER_H

controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.cpp

@@ -13,6 +13,7 @@
 #include <ocs2_ros_interfaces/synchronized_module/RosReferenceManager.h>
 #include <ocs2_sqp/SqpMpc.h>
 #include <angles/angles.h>
+#include <ocs2_quadruped_controller/control/GaitManager.h>
 
 namespace ocs2::legged_robot {
     using config_type = controller_interface::interface_configuration_type;
@@ -53,13 +54,9 @@ namespace ocs2::legged_robot {
 
     controller_interface::return_type Ocs2QuadrupedController::update(const rclcpp::Time &time,
                                                                       const rclcpp::Duration &period) {
+
         // State Estimate
-        const vector_t rbd_state = ctrl_comp_.estimator_->update(time, period);
+        updateStateEstimation(time, period);
-        currentObservation_.time += period.seconds();
-        const scalar_t yaw_last = currentObservation_.state(9);
-        currentObservation_.state = rbdConversions_->computeCentroidalStateFromRbdModel(rbd_state);
-        currentObservation_.state(9) = yaw_last + angles::shortest_angular_distance(yaw_last, currentObservation_.state(9));
-        currentObservation_.mode = stateEstimate_->getMode();
 
         // Update the current state of the system
         mpcMrtInterface_->setCurrentObservation(currentObservation_);
@@ -69,20 +66,22 @@ namespace ocs2::legged_robot {
 
         // Evaluate the current policy
         vector_t optimizedState, optimizedInput;
-        size_t plannedMode = 0;  // The mode that is active at the time the policy is evaluated at.
+        size_t plannedMode = 0; // The mode that is active at the time the policy is evaluated at.
-        mpcMrtInterface_->evaluatePolicy(currentObservation_.time, currentObservation_.state, optimizedState, optimizedInput, plannedMode);
+        mpcMrtInterface_->evaluatePolicy(currentObservation_.time, currentObservation_.state, optimizedState,
+                                         optimizedInput, plannedMode);
 
         // Whole body control
         currentObservation_.input = optimizedInput;
 
         wbcTimer_.startTimer();
-        vector_t x = wbc_->update(optimizedState, optimizedInput, rbd_state, plannedMode, period.seconds());
+        vector_t x = wbc_->update(optimizedState, optimizedInput, measuredRbdState_, plannedMode, period.seconds());
         wbcTimer_.endTimer();
 
         vector_t torque = x.tail(12);
 
         vector_t posDes = centroidal_model::getJointAngles(optimizedState, legged_interface_->getCentroidalModelInfo());
-        vector_t velDes = centroidal_model::getJointVelocities(optimizedInput, legged_interface_->getCentroidalModelInfo());
+        vector_t velDes = centroidal_model::getJointVelocities(optimizedInput,
+                                                               legged_interface_->getCentroidalModelInfo());
 
         // Safety check, if failed, stop the controller
         if (!safetyChecker_->check(currentObservation_, optimizedState, optimizedInput)) {
@@ -105,6 +104,7 @@ namespace ocs2::legged_robot {
         urdf_file_ = auto_declare<std::string>("urdf_file", urdf_file_);
         task_file_ = auto_declare<std::string>("task_file", task_file_);
         reference_file_ = auto_declare<std::string>("reference_file", reference_file_);
+        gait_file_ = auto_declare<std::string>("gait_file", gait_file_);
 
         verbose_ = false;
         loadData::loadCppDataType(task_file_, "legged_robot_interface.verbose", verbose_);
@@ -194,6 +194,30 @@ namespace ocs2::legged_robot {
             }
         }
 
+        if (mpcRunning_ == false) {
+            // Initial state
+            currentObservation_.state.setZero(static_cast<long>(legged_interface_->getCentroidalModelInfo().stateDim));
+            updateStateEstimation(get_node()->now(), rclcpp::Duration(0, 200000));
+            currentObservation_.input.setZero(static_cast<long>(legged_interface_->getCentroidalModelInfo().inputDim));
+            currentObservation_.mode = STANCE;
+
+            TargetTrajectories target_trajectories({currentObservation_.time}, {currentObservation_.state},
+                                                   {currentObservation_.input});
+
+            // Set the first observation and command and wait for optimization to finish
+            mpcMrtInterface_->setCurrentObservation(currentObservation_);
+            mpcMrtInterface_->getReferenceManager().setTargetTrajectories(target_trajectories);
+            RCLCPP_INFO(get_node()->get_logger(), "Waiting for the initial policy ...");
+            while (!mpcMrtInterface_->initialPolicyReceived()) {
+                std::cout<<legged_interface_->mpcSettings().mrtDesiredFrequency_<<std::endl;
+                mpcMrtInterface_->advanceMpc();
+                rclcpp::WallRate(legged_interface_->mpcSettings().mrtDesiredFrequency_).sleep();
+            }
+            RCLCPP_INFO(get_node()->get_logger(), "Initial policy has been received.");
+
+            mpcRunning_ = true;
+        }
+
         return CallbackReturn::SUCCESS;
     }
 
@@ -234,16 +258,16 @@ namespace ocs2::legged_robot {
 
         // Todo Handle Gait Receive.
         // Gait receiver
-        // auto gaitReceiverPtr =
+        const auto gait_manager_ptr = std::make_shared<GaitManager>(
-        //         std::make_shared<GaitReceiver>(this->get_node(),
+            ctrl_comp_, legged_interface_->getSwitchedModelReferenceManagerPtr()->
-        //                                        legged_interface_->getSwitchedModelReferenceManagerPtr()->
+            getGaitSchedule());
-        //                                        getGaitSchedule(), robotName);
+        gait_manager_ptr->init(gait_file_);
 
         // ROS ReferenceManager
         // auto rosReferenceManagerPtr = std::make_shared<RosReferenceManager>(
         //     robotName, legged_interface_->getReferenceManagerPtr());
         // rosReferenceManagerPtr->subscribe(this->get_node());
-        // mpc_->getSolverPtr()->addSynchronizedModule(gaitReceiverPtr);
+        // mpc_->getSolverPtr()->addSynchronizedModule(gait_manager_ptr);
         // mpc_->getSolverPtr()->setReferenceManager(rosReferenceManagerPtr);
         // observationPublisher_ = nh.advertise<ocs2_msgs::msg::mpc_observation>(robotName + "_mpc_observation", 1);
     }
@@ -282,7 +306,18 @@ namespace ocs2::legged_robot {
         dynamic_cast<KalmanFilterEstimate &>(*ctrl_comp_.estimator_).loadSettings(task_file_, verbose_);
         currentObservation_.time = 0;
     }
+
+    void Ocs2QuadrupedController::updateStateEstimation(const rclcpp::Time &time, const rclcpp::Duration &period) {
+        measuredRbdState_ = ctrl_comp_.estimator_->update(time, period);
+        currentObservation_.time += period.seconds();
+        const scalar_t yaw_last = currentObservation_.state(9);
+        currentObservation_.state = rbdConversions_->computeCentroidalStateFromRbdModel(measuredRbdState_);
+        currentObservation_.state(9) = yaw_last + angles::shortest_angular_distance(
+                                           yaw_last, currentObservation_.state(9));
+        std::cout << ctrl_comp_.estimator_->getMode() << std::endl;
+        currentObservation_.mode = ctrl_comp_.estimator_->getMode();
+    }
 }
 
 #include "pluginlib/class_list_macros.hpp"
-PLUGINLIB_EXPORT_CLASS(ocs2::legged_robot::Ocs2QuadrupedController, controller_interface::ControllerInterface);
+PLUGINLIB_EXPORT_CLASS(ocs2::legged_robot::Ocs2QuadrupedController, controller_interface::ControllerInterface);

controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.h

@@ -67,6 +67,9 @@ namespace ocs2::legged_robot {
 
         void setupStateEstimate();
 
+        void updateStateEstimation(const rclcpp::Time &time,
+                                   const rclcpp::Duration &period);
+
         CtrlComponent ctrl_comp_;
         std::vector<std::string> joint_names_;
         std::vector<std::string> command_interface_types_;
@@ -104,6 +107,7 @@ namespace ocs2::legged_robot {
         std::string task_file_;
         std::string urdf_file_;
         std::string reference_file_;
+        std::string gait_file_;
 
         bool verbose_;
 
@@ -119,9 +123,9 @@ namespace ocs2::legged_robot {
         std::shared_ptr<MPC_MRT_Interface> mpcMrtInterface_;
 
         std::shared_ptr<CentroidalModelRbdConversions> rbdConversions_;
-        std::shared_ptr<StateEstimateBase> stateEstimate_;
 
     private:
+        vector_t measuredRbdState_;
         std::thread mpcThread_;
         std::atomic_bool controllerRunning_{}, mpcRunning_{};
         benchmark::RepeatedTimer mpcTimer_;

controllers/ocs2_quadruped_controller/src/control/GaitManager.cpp

@@ -0,0 +1,51 @@
+//
+// Created by tlab-uav on 24-9-26.
+//
+
+#include <utility>
+
+#include "ocs2_quadruped_controller/control/GaitManager.h"
+
+#include <ocs2_core/misc/LoadData.h>
+
+namespace ocs2::legged_robot {
+    GaitManager::GaitManager(CtrlComponent &ctrl_component,
+                             std::shared_ptr<GaitSchedule> gait_schedule_ptr)
+        : ctrl_component_(ctrl_component),
+          gait_schedule_ptr_(std::move(gait_schedule_ptr)),
+          target_gait_({0.0, 1.0}, {STANCE}) {
+    }
+
+    void GaitManager::preSolverRun(const scalar_t initTime, const scalar_t finalTime,
+                                   const vector_t &currentState,
+                                   const ReferenceManagerInterface &referenceManager) {
+        getTargetGait();
+        if (gait_updated_) {
+            const auto timeHorizon = finalTime - initTime;
+            gait_schedule_ptr_->insertModeSequenceTemplate(target_gait_, finalTime,
+                                                           timeHorizon);
+            gait_updated_ = false;
+        }
+    }
+
+    void GaitManager::init(const std::string &gait_file) {
+        gait_name_list_.clear();
+        loadData::loadStdVector(gait_file, "list", gait_name_list_, verbose_);
+
+        gait_list_.clear();
+        for (const auto &name: gait_name_list_) {
+            gait_list_.push_back(loadModeSequenceTemplate(gait_file, name, verbose_));
+        }
+
+        RCLCPP_INFO(rclcpp::get_logger("gait_manager"), "GaitManager is ready.");
+    }
+
+    void GaitManager::getTargetGait() {
+        std::cout << "ctrl_component_.control_inputs_.command: " << ctrl_component_.control_inputs_.command << std::endl;
+        if (ctrl_component_.control_inputs_.command == 0) return;
+        target_gait_ = gait_list_[ctrl_component_.control_inputs_.command - 1];
+        RCLCPP_INFO(rclcpp::get_logger("GaitManager"), "Switch to gait: %s",
+                    gait_name_list_[ctrl_component_.control_inputs_.command - 1].c_str());
+        gait_updated_ = true;
+    }
+}

controllers/ocs2_quadruped_controller/src/estimator/StateEstimateBase.cpp

@@ -12,7 +12,6 @@
 #include "ocs2_quadruped_controller/control/CtrlComponent.h"
 
 namespace ocs2::legged_robot {
-    using namespace legged_robot;
 
     StateEstimateBase::StateEstimateBase(PinocchioInterface pinocchioInterface, CentroidalModelInfo info,
                                          const PinocchioEndEffectorKinematics &eeKinematics,

descriptions/go2_description/config/ocs2/gait.info

@@ -0,0 +1,255 @@
+list
+{
+  [0] stance
+  [1] trot
+  [2] standing_trot
+  [3] flying_trot
+  [4] pace
+  [5] standing_pace
+  [6] dynamic_walk
+  [7] static_walk
+  [8] amble
+  [9] lindyhop
+  [10] skipping
+  [11] pawup
+}
+
+stance
+ {
+   modeSequence
+   {
+     [0]     STANCE
+   }
+   switchingTimes
+   {
+     [0]     0.0
+     [1]     0.5
+   }
+}
+
+trot
+ {
+   modeSequence
+   {
+     [0]     LF_RH
+     [1]     RF_LH
+   }
+   switchingTimes
+   {
+     [0]     0.0
+     [1]     0.3
+     [2]     0.6
+   }
+}
+
+standing_trot
+{
+  modeSequence
+  {
+    [0]     LF_RH
+    [1]     STANCE
+    [2]     RF_LH
+    [3]     STANCE
+  }
+  switchingTimes
+  {
+    [0]     0.00
+    [1]     0.25
+    [2]     0.3
+    [3]     0.55
+    [4]     0.6
+  }
+}
+
+flying_trot
+{
+  modeSequence
+  {
+    [0]     LF_RH
+    [1]     FLY
+    [2]     RF_LH
+    [3]     FLY
+  }
+  switchingTimes
+  {
+    [0]     0.00
+    [1]     0.15
+    [2]     0.2
+    [3]     0.35
+    [4]     0.4
+  }
+}
+
+pace
+{
+  modeSequence
+  {
+    [0]     LF_LH
+    [1]     FLY
+    [2]     RF_RH
+    [3]     FLY
+  }
+  switchingTimes
+  {
+    [0]     0.0
+    [1]     0.28
+    [2]     0.30
+    [3]     0.58
+    [4]     0.60
+  }
+}
+
+standing_pace
+{
+  modeSequence
+  {
+    [0]     LF_LH
+    [1]     STANCE
+    [2]     RF_RH
+    [3]     STANCE
+  }
+  switchingTimes
+  {
+    [0]     0.0
+    [1]     0.30
+    [2]     0.35
+    [3]     0.65
+    [4]     0.70
+  }
+}
+
+dynamic_walk
+{
+  modeSequence
+  {
+    [0]     LF_RF_RH
+    [1]     RF_RH
+    [2]     RF_LH_RH
+    [3]     LF_RF_LH
+    [4]     LF_LH
+    [5]     LF_LH_RH
+  }
+  switchingTimes
+  {
+    [0]     0.0
+    [1]     0.2
+    [2]     0.3
+    [3]     0.5
+    [4]     0.7
+    [5]     0.8
+    [6]     1.0
+  }
+}
+
+static_walk
+{
+  modeSequence
+  {
+    [0]     LF_RF_RH
+    [1]     RF_LH_RH
+    [2]     LF_RF_LH
+    [3]     LF_LH_RH
+  }
+  switchingTimes
+  {
+    [0]     0.0
+    [1]     0.3
+    [2]     0.6
+    [3]     0.9
+    [4]     1.2
+  }
+}
+
+amble
+{
+  modeSequence
+  {
+    [0]     RF_LH
+    [1]     LF_LH
+    [2]     LF_RH
+    [3]     RF_RH
+  }
+  switchingTimes
+  {
+    [0]     0.0
+    [1]     0.15
+    [2]     0.40
+    [3]     0.55
+    [4]     0.80
+  }
+}
+
+lindyhop
+{
+  modeSequence
+  {
+    [0]     LF_RH
+    [1]     STANCE
+    [2]     RF_LH
+    [3]     STANCE
+    [4]     LF_LH
+    [5]     RF_RH
+    [6]     LF_LH
+    [7]     STANCE
+    [8]     RF_RH
+    [9]     LF_LH
+    [10]     RF_RH
+    [11]     STANCE
+  }
+  switchingTimes
+  {
+    [0]     0.00 ; Step 1
+    [1]     0.35 ; Stance
+    [2]     0.45 ; Step 2
+    [3]     0.80 ; Stance
+    [4]     0.90  ; Tripple step
+    [5]     1.125 ;
+    [6]     1.35  ;
+    [7]     1.70  ; Stance
+    [8]     1.80  ; Tripple step
+    [9]     2.025 ;
+    [10]    2.25  ;
+    [11]    2.60  ; Stance
+    [12]    2.70  ;
+  }
+}
+
+skipping
+{
+  modeSequence
+  {
+    [0]     LF_RH
+    [1]     FLY
+    [2]     LF_RH
+    [3]     FLY
+    [4]     RF_LH
+    [5]     FLY
+    [6]     RF_LH
+    [7]     FLY
+  }
+  switchingTimes
+  {
+    [0]     0.00
+    [1]     0.27
+    [2]     0.30
+    [3]     0.57
+    [4]     0.60
+    [5]     0.87
+    [6]     0.90
+    [7]     1.17
+    [8]     1.20
+  }
+}
+
+pawup
+{
+   modeSequence
+   {
+     [0]     RF_LH_RH
+   }
+   switchingTimes
+   {
+     [0]     0.0
+     [1]     2.0
+   }
+}

descriptions/go2_description/launch/ocs2_control.launch.py

@@ -54,7 +54,9 @@ def launch_setup(context, *args, **kwargs):
                         'task_file': os.path.join(get_package_share_directory(package_description), 'config', 'ocs2',
                                                   'task.info'),
                         'reference_file': os.path.join(get_package_share_directory(package_description), 'config',
-                                                       'ocs2', 'reference.info')
+                                                       'ocs2', 'reference.info'),
+                        'gait_file': os.path.join(get_package_share_directory(package_description), 'config',
+                                                       'ocs2', 'gait.info')
                     }],
         output="both",
     )