take FSM out as separate library

controllers/ocs2_quadruped_controller/CMakeLists.txt

@@ -12,6 +12,7 @@ set(CONTROLLER_INCLUDE_DEPENDS
         pluginlib
         rcpputils
         controller_interface
+        controller_common
 
         ocs2_legged_robot_ros
         ocs2_self_collision
@@ -30,6 +31,7 @@ endforeach ()
 
 add_library(${PROJECT_NAME} SHARED
         src/Ocs2QuadrupedController.cpp
+        src/FSM/StateOCS2.cpp
 
         src/estimator/GroundTruth.cpp
         src/estimator/LinearKalmanFilter.cpp

controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/FSM/StateOCS2.h

@@ -0,0 +1,107 @@
+//
+// Created by tlab-uav on 25-2-27.
+//
+
+#ifndef STATEOCS2_H
+#define STATEOCS2_H
+
+#include <SafetyChecker.h>
+#include <ocs2_centroidal_model/CentroidalModelRbdConversions.h>
+#include <ocs2_core/misc/Benchmark.h>
+#include <ocs2_legged_robot_ros/visualization/LeggedRobotVisualizer.h>
+#include <ocs2_mpc/SystemObservation.h>
+#include <ocs2_msgs/msg/detail/mpc_observation__struct.hpp>
+#include <ocs2_quadruped_controller/control/TargetManager.h>
+#include <ocs2_quadruped_controller/estimator/StateEstimateBase.h>
+#include <ocs2_quadruped_controller/interface/LeggedInterface.h>
+#include <ocs2_quadruped_controller/wbc/WbcBase.h>
+#include <rclcpp/duration.hpp>
+#include <ocs2_ros_interfaces/common/RosMsgConversions.h>
+
+#include "controller_common/FSM/FSMState.h"
+
+namespace ocs2 {
+    class MPC_MRT_Interface;
+    class MPC_BASE;
+    class PinocchioEndEffectorKinematics;
+}
+
+struct CtrlComponent;
+
+namespace ocs2::legged_robot {
+    class StateOCS2 final : public FSMState {
+    public:
+        StateOCS2(CtrlInterfaces &ctrl_interfaces,
+                  const std::shared_ptr<rclcpp_lifecycle::LifecycleNode> &node,
+                  const std::string &package_share_directory,
+                  const std::vector<std::string> &joint_names,
+                  const std::vector<std::string> &feet_names,
+                  double default_kp,
+                  double default_kd
+        );
+
+        void enter() override;
+
+        void run(const rclcpp::Time &time,
+                 const rclcpp::Duration &period) override;
+
+        void exit() override;
+
+        FSMStateName checkChange() override;
+
+        void setupStateEstimate(const std::string &estimator_type);
+
+    private:
+        void updateStateEstimation(const rclcpp::Duration &period);
+
+        void setupLeggedInterface();
+
+        void setupMpc();
+
+        void setupMrt();
+
+        std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node_;
+        rclcpp::Publisher<ocs2_msgs::msg::MpcObservation>::SharedPtr observation_publisher_;
+        std::shared_ptr<StateEstimateBase> estimator_;
+        std::shared_ptr<TargetManager> target_manager_;
+        std::shared_ptr<LeggedRobotVisualizer> visualizer_;
+
+        std::shared_ptr<LeggedInterface> legged_interface_;
+        std::shared_ptr<PinocchioEndEffectorKinematics> eeKinematicsPtr_;
+
+        // Whole Body Control
+        std::shared_ptr<WbcBase> wbc_;
+        std::shared_ptr<SafetyChecker> safety_checker_;
+
+        // Nonlinear MPC
+        std::shared_ptr<MPC_BASE> mpc_;
+        std::shared_ptr<MPC_MRT_Interface> mpc_mrt_interface_;
+
+        std::shared_ptr<CentroidalModelRbdConversions> rbd_conversions_;
+
+        SystemObservation observation_;
+
+        vector_t measured_rbd_state_;
+        std::thread mpc_thread_;
+        std::atomic_bool controller_running_{}, mpc_running_{};
+        benchmark::RepeatedTimer mpc_timer_;
+        benchmark::RepeatedTimer wbc_timer_;
+
+        std::string task_file_;
+        std::string urdf_file_;
+        std::string reference_file_;
+        std::string gait_file_;
+
+        std::vector<std::string> joint_names_;
+        std::vector<std::string> feet_names_;
+        double default_kp_;
+        double default_kd_;
+
+        bool verbose_;
+        bool mpc_need_updated_;
+        vector_t optimized_state_, optimized_input_;
+    };
+}
+
+
+#endif //STATEOCS2_H

controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/control/CtrlComponent.h

@@ -1,75 +0,0 @@
-//
-// Created by biao on 24-9-10.
-//
-
-#ifndef INTERFACE_H
-#define INTERFACE_H
-
-#include <vector>
-#include <hardware_interface/loaned_command_interface.hpp>
-#include <hardware_interface/loaned_state_interface.hpp>
-#include <control_input_msgs/msg/inputs.hpp>
-#include <ocs2_mpc/SystemObservation.h>
-#include <ocs2_legged_robot_ros/visualization/LeggedRobotVisualizer.h>
-
-#include "TargetManager.h"
-#include "ocs2_quadruped_controller/estimator/StateEstimateBase.h"
-
-struct CtrlComponent {
-    std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> >
-    joint_torque_command_interface_;
-    std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> >
-    joint_position_command_interface_;
-    std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> >
-    joint_velocity_command_interface_;
-    std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> >
-    joint_kp_command_interface_;
-    std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> >
-    joint_kd_command_interface_;
-
-
-    std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> >
-    joint_effort_state_interface_;
-    std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> >
-    joint_position_state_interface_;
-    std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> >
-    joint_velocity_state_interface_;
-
-    std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> >
-    imu_state_interface_;
-
-    std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> >
-    foot_force_state_interface_;
-
-    std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> >
-    odom_state_interface_;
-
-    control_input_msgs::msg::Inputs control_inputs_;
-    ocs2::SystemObservation observation_;
-    int frequency_{};
-    bool reset = true;
-
-    std::shared_ptr<ocs2::legged_robot::StateEstimateBase> estimator_;
-    std::shared_ptr<ocs2::legged_robot::TargetManager> target_manager_;
-    std::shared_ptr<ocs2::legged_robot::LeggedRobotVisualizer> visualizer_;
-
-    CtrlComponent() {
-    }
-
-    void clear() {
-        joint_torque_command_interface_.clear();
-        joint_position_command_interface_.clear();
-        joint_velocity_command_interface_.clear();
-        joint_kd_command_interface_.clear();
-        joint_kp_command_interface_.clear();
-
-        joint_effort_state_interface_.clear();
-        joint_position_state_interface_.clear();
-        joint_velocity_state_interface_.clear();
-
-        imu_state_interface_.clear();
-        foot_force_state_interface_.clear();
-    }
-};
-
-#endif //INTERFACE_H

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

@@ -4,15 +4,14 @@
 
 #ifndef GAITMANAGER_H
 #define GAITMANAGER_H
+#include <controller_common/CtrlInterfaces.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,
+        GaitManager(CtrlInterfaces &ctrl_interfaces,
                     std::shared_ptr<GaitSchedule> gait_schedule_ptr);
 
         void preSolverRun(scalar_t initTime, scalar_t finalTime,
@@ -27,7 +26,7 @@ namespace ocs2::legged_robot {
     private:
         void getTargetGait();
 
-        CtrlComponent &ctrl_component_;
+        CtrlInterfaces &ctrl_interfaces_;
         std::shared_ptr<GaitSchedule> gait_schedule_ptr_;
 
         ModeSequenceTemplate target_gait_;

controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/control/TargetManager.h

@@ -7,22 +7,21 @@
 
 
 #include <memory>
+#include <controller_common/CtrlInterfaces.h>
 #include <ocs2_mpc/SystemObservation.h>
 #include <ocs2_oc/synchronized_module/ReferenceManagerInterface.h>
 
-struct CtrlComponent;
-
 namespace ocs2::legged_robot {
     class TargetManager {
     public:
-        TargetManager(CtrlComponent &ctrl_component,
+        TargetManager(CtrlInterfaces &ctrl_component,
                       const std::shared_ptr<ReferenceManagerInterface> &referenceManagerPtr,
-                      const std::string& task_file,
+                      const std::string &task_file,
-                      const std::string& reference_file);
+                      const std::string &reference_file);
 
         ~TargetManager() = default;
 
-        void update();
+        void update(SystemObservation &observation);
 
     private:
         TargetTrajectories targetPoseToTargetTrajectories(const vector_t &targetPose,
@@ -45,7 +44,8 @@ namespace ocs2::legged_robot {
 
             return {timeTrajectory, stateTrajectory, inputTrajectory};
         }
-        CtrlComponent &ctrl_component_;
+
+        CtrlInterfaces &ctrl_component_;
         std::shared_ptr<ReferenceManagerInterface> referenceManagerPtr_;
 
         vector_t default_joint_state_{};

controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/FromOdomTopic.h

@@ -11,7 +11,7 @@ namespace ocs2::legged_robot
     class FromOdomTopic final : public StateEstimateBase
     {
     public:
-        FromOdomTopic(CentroidalModelInfo info, CtrlComponent& ctrl_component,
+        FromOdomTopic(CentroidalModelInfo info, CtrlInterfaces& ctrl_component,
                       const rclcpp_lifecycle::LifecycleNode::SharedPtr& node);
 
         vector_t update(const rclcpp::Time& time, const rclcpp::Duration& period) override;

controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/GroundTruth.h

@@ -11,7 +11,7 @@
 namespace ocs2::legged_robot {
     class GroundTruth final : public StateEstimateBase {
     public:
-        GroundTruth(CentroidalModelInfo info, CtrlComponent &ctrl_component,
+        GroundTruth(CentroidalModelInfo info, CtrlInterfaces &ctrl_component,
                     const rclcpp_lifecycle::LifecycleNode::SharedPtr &node);
 
         vector_t update(const rclcpp::Time &time, const rclcpp::Duration &period) override;

controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/LinearKalmanFilter.h

@@ -16,7 +16,7 @@ namespace ocs2::legged_robot {
     public:
         KalmanFilterEstimate(PinocchioInterface pinocchio_interface, CentroidalModelInfo info,
                              const PinocchioEndEffectorKinematics &ee_kinematics,
-                             CtrlComponent &ctrl_component,
+                             CtrlInterfaces &ctrl_component,
                              const rclcpp_lifecycle::LifecycleNode::SharedPtr &node);
 
         vector_t update(const rclcpp::Time &time, const rclcpp::Duration &period) override;

controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/StateEstimateBase.h

@@ -3,6 +3,7 @@
 //
 #pragma once
 
+#include <controller_common/CtrlInterfaces.h>
 #include <rclcpp/rclcpp.hpp>
 
 #include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
@@ -15,14 +16,12 @@
 #include <ocs2_pinocchio_interface/PinocchioEndEffectorKinematics.h>
 #include <rclcpp_lifecycle/lifecycle_node.hpp>
 
-struct CtrlComponent;
-
 namespace ocs2::legged_robot {
     class StateEstimateBase {
     public:
         virtual ~StateEstimateBase() = default;
 
-        StateEstimateBase(CentroidalModelInfo info, CtrlComponent &ctrl_component,
+        StateEstimateBase(CentroidalModelInfo info, CtrlInterfaces &ctrl_component,
                           rclcpp_lifecycle::LifecycleNode::SharedPtr node);
 
         virtual void updateJointStates();
@@ -44,7 +43,7 @@ namespace ocs2::legged_robot {
 
         void publishMsgs(const nav_msgs::msg::Odometry &odom) const;
 
-        CtrlComponent &ctrl_component_;
+        CtrlInterfaces &ctrl_component_;
         CentroidalModelInfo info_;
 
         vector3_t zyx_offset_ = vector3_t::Zero();

controllers/ocs2_quadruped_controller/package.xml

@@ -11,6 +11,7 @@
 
   <depend>backward_ros</depend>
   <depend>controller_interface</depend>
+  <depend>controller_common</depend>
   <depend>pluginlib</depend>
   <depend>control_input_msgs</depend>
   <depend>qpoases_colcon</depend>

controllers/ocs2_quadruped_controller/src/FSM/StateOCS2.cpp

@@ -0,0 +1,268 @@
+//
+// Created by tlab-uav on 25-2-27.
+//
+
+#include "ocs2_quadruped_controller/FSM/StateOCS2.h"
+
+#include <angles/angles.h>
+#include <ocs2_core/reference/TargetTrajectories.h>
+#include <ocs2_core/thread_support/ExecuteAndSleep.h>
+#include <ocs2_core/thread_support/SetThreadPriority.h>
+#include <ocs2_ros_interfaces/common/RosMsgConversions.h>
+#include <rclcpp/rate.hpp>
+#include <ocs2_core/misc/LoadData.h>
+#include <ocs2_legged_robot_ros/visualization/LeggedRobotVisualizer.h>
+#include <ocs2_mpc/MPC_MRT_Interface.h>
+#include <ocs2_quadruped_controller/control/GaitManager.h>
+#include <ocs2_quadruped_controller/control/TargetManager.h>
+#include <ocs2_quadruped_controller/estimator/FromOdomTopic.h>
+#include <ocs2_quadruped_controller/estimator/GroundTruth.h>
+#include <ocs2_quadruped_controller/estimator/LinearKalmanFilter.h>
+#include <ocs2_quadruped_controller/wbc/WeightedWbc.h>
+#include <ocs2_sqp/SqpMpc.h>
+
+namespace ocs2::legged_robot {
+    StateOCS2::StateOCS2(CtrlInterfaces &ctrl_interfaces,
+                         const std::shared_ptr<rclcpp_lifecycle::LifecycleNode> &node,
+                         const std::string &package_share_directory,
+                         const std::vector<std::string> &joint_names,
+                         const std::vector<std::string> &feet_names,
+                         double default_kp,
+                         double default_kd)
+        : FSMState(
+              FSMStateName::OCS2, "OCS2 State", ctrl_interfaces),
+          node_(node),
+          joint_names_(joint_names),
+          feet_names_(feet_names),
+          default_kp_(default_kp),
+          default_kd_(default_kd) {
+        urdf_file_ = package_share_directory + "/urdf/robot.urdf";
+        task_file_ = package_share_directory + "/config/ocs2/task.info";
+        reference_file_ = package_share_directory + "/config/ocs2/reference.info";
+        gait_file_ = package_share_directory + "/config/ocs2/gait.info";
+
+        // Load verbose parameter from the task file
+        verbose_ = false;
+        loadData::loadCppDataType(task_file_, "legged_robot_interface.verbose", verbose_);
+
+        setupLeggedInterface();
+        setupMpc();
+        setupMrt();
+
+        // Visualization
+        CentroidalModelPinocchioMapping pinocchio_mapping(legged_interface_->getCentroidalModelInfo());
+        eeKinematicsPtr_ = std::make_shared<PinocchioEndEffectorKinematics>(
+            legged_interface_->getPinocchioInterface(), pinocchio_mapping,
+            legged_interface_->modelSettings().contactNames3DoF);
+
+        visualizer_ = std::make_shared<LeggedRobotVisualizer>(
+            legged_interface_->getPinocchioInterface(), legged_interface_->getCentroidalModelInfo(), *eeKinematicsPtr_,
+            node_);
+
+        // selfCollisionVisualization_.reset(new LeggedSelfCollisionVisualization(leggedInterface_->getPinocchioInterface(),
+        //                                                                        leggedInterface_->getGeometryInterface(), pinocchioMapping, nh));
+        // Whole body control
+        wbc_ = std::make_shared<WeightedWbc>(legged_interface_->getPinocchioInterface(),
+                                             legged_interface_->getCentroidalModelInfo(),
+                                             *eeKinematicsPtr_);
+        wbc_->loadTasksSetting(task_file_, verbose_);
+
+        // Safety Checker
+        safety_checker_ = std::make_shared<SafetyChecker>(legged_interface_->getCentroidalModelInfo());
+
+        observation_publisher_ = node_->create_publisher<ocs2_msgs::msg::MpcObservation>(
+            "legged_robot_mpc_observation", 10);
+    }
+
+    void StateOCS2::enter() {
+        if (mpc_running_ == false) {
+            // Initial state
+            observation_.state.setZero(
+                static_cast<long>(legged_interface_->getCentroidalModelInfo().stateDim));
+            updateStateEstimation(rclcpp::Duration(0, 1 / ctrl_interfaces_.frequency_ * 1000000000));
+            observation_.input.setZero(
+                static_cast<long>(legged_interface_->getCentroidalModelInfo().inputDim));
+            observation_.mode = STANCE;
+
+            const TargetTrajectories target_trajectories({observation_.time},
+                                                         {observation_.state},
+                                                         {observation_.input});
+
+            // Set the first observation and command and wait for optimization to finish
+            mpc_mrt_interface_->setCurrentObservation(observation_);
+            mpc_mrt_interface_->getReferenceManager().setTargetTrajectories(target_trajectories);
+            RCLCPP_INFO(node_->get_logger(), "Waiting for the initial policy ...");
+            while (!mpc_mrt_interface_->initialPolicyReceived()) {
+                mpc_mrt_interface_->advanceMpc();
+                rclcpp::WallRate(legged_interface_->mpcSettings().mrtDesiredFrequency_).sleep();
+            }
+            RCLCPP_INFO(node_->get_logger(), "Initial policy has been received.");
+
+            mpc_running_ = true;
+        }
+    }
+
+    void StateOCS2::run(const rclcpp::Time &time,
+                        const rclcpp::Duration &period) {
+        if (mpc_running_ == false) {
+            return;
+        }
+
+        // State Estimate
+        updateStateEstimation(period);
+
+        // Compute target trajectory
+        target_manager_->update(observation_);
+
+        // Update the current state of the system
+        mpc_mrt_interface_->setCurrentObservation(observation_);
+
+        // Load the latest MPC policy
+        mpc_mrt_interface_->updatePolicy();
+        mpc_need_updated_ = true;
+
+        // Evaluate the current policy
+        size_t planned_mode = 0; // The mode that is active at the time the policy is evaluated at.
+        mpc_mrt_interface_->evaluatePolicy(observation_.time, observation_.state,
+                                           optimized_state_,
+                                           optimized_input_, planned_mode);
+
+        // Whole body control
+        observation_.input = optimized_input_;
+
+        wbc_timer_.startTimer();
+        vector_t x = wbc_->update(optimized_state_, optimized_input_, measured_rbd_state_, planned_mode,
+                                  period.seconds());
+        wbc_timer_.endTimer();
+
+        vector_t torque = x.tail(12);
+
+        vector_t pos_des = centroidal_model::getJointAngles(optimized_state_,
+                                                            legged_interface_->getCentroidalModelInfo());
+        vector_t vel_des = centroidal_model::getJointVelocities(optimized_input_,
+                                                                legged_interface_->getCentroidalModelInfo());
+
+        for (int i = 0; i < joint_names_.size(); i++) {
+            std::ignore = ctrl_interfaces_.joint_torque_command_interface_[i].get().set_value(torque(i));
+            std::ignore = ctrl_interfaces_.joint_position_command_interface_[i].get().set_value(pos_des(i));
+            std::ignore = ctrl_interfaces_.joint_velocity_command_interface_[i].get().set_value(vel_des(i));
+            std::ignore = ctrl_interfaces_.joint_kp_command_interface_[i].get().set_value(default_kp_);
+            std::ignore = ctrl_interfaces_.joint_kd_command_interface_[i].get().set_value(default_kd_);
+        }
+
+        // Visualization
+        visualizer_->update(observation_, mpc_mrt_interface_->getPolicy(),
+                            mpc_mrt_interface_->getCommand());
+
+        observation_publisher_->publish(ros_msg_conversions::createObservationMsg(observation_));
+    }
+
+    void StateOCS2::exit() {
+        mpc_running_ = false;
+        mpc_thread_.join();
+        RCLCPP_INFO(node_->get_logger(), "MRT thread stopped.");
+    }
+
+    FSMStateName StateOCS2::checkChange() {
+        // Safety check, if failed, stop the controller
+        if (!safety_checker_->check(observation_, optimized_state_, optimized_input_)) {
+            RCLCPP_ERROR(node_->get_logger(), "[Legged Controller] Safety check failed, stopping the controller.");
+            for (int i = 0; i < joint_names_.size(); i++) {
+                std::ignore = ctrl_interfaces_.joint_torque_command_interface_[i].get().set_value(0);
+                std::ignore = ctrl_interfaces_.joint_position_command_interface_[i].get().set_value(0);
+                std::ignore = ctrl_interfaces_.joint_velocity_command_interface_[i].get().set_value(0);
+                std::ignore = ctrl_interfaces_.joint_kp_command_interface_[i].get().set_value(0.0);
+                std::ignore = ctrl_interfaces_.joint_kd_command_interface_[i].get().set_value(0.35);
+            }
+            return FSMStateName::PASSIVE;
+        }
+        return FSMStateName::OCS2;
+    }
+
+    void StateOCS2::setupStateEstimate(const std::string &estimator_type) {
+        if (estimator_type == "ground_truth") {
+            estimator_ = std::make_shared<GroundTruth>(legged_interface_->getCentroidalModelInfo(),
+                                                       ctrl_interfaces_,
+                                                       node_);
+            RCLCPP_INFO(node_->get_logger(), "Using Ground Truth Estimator");
+        } else if (estimator_type == "linear_kalman") {
+            estimator_ = std::make_shared<KalmanFilterEstimate>(
+                legged_interface_->getPinocchioInterface(),
+                legged_interface_->getCentroidalModelInfo(),
+                *eeKinematicsPtr_, ctrl_interfaces_,
+                node_);
+            dynamic_cast<KalmanFilterEstimate &>(*estimator_).loadSettings(task_file_, verbose_);
+            RCLCPP_INFO(node_->get_logger(), "Using Kalman Filter Estimator");
+        } else {
+            estimator_ = std::make_shared<FromOdomTopic>(
+                legged_interface_->getCentroidalModelInfo(), ctrl_interfaces_, node_);
+            RCLCPP_INFO(node_->get_logger(), "Using Odom Topic Based Estimator");
+        }
+        observation_.time = 0;
+    }
+
+    void StateOCS2::updateStateEstimation(const rclcpp::Duration &period) {
+        measured_rbd_state_ = estimator_->update(node_->now(), period);
+        observation_.time += period.seconds();
+        const scalar_t yaw_last = observation_.state(9);
+        observation_.state = rbd_conversions_->computeCentroidalStateFromRbdModel(measured_rbd_state_);
+        observation_.state(9) = yaw_last + angles::shortest_angular_distance(
+                                    yaw_last, observation_.state(9));
+        // ctrl_comp_.observation_.mode = ctrl_comp_.estimator_->getMode();
+    }
+
+    void StateOCS2::setupMrt() {
+        mpc_mrt_interface_ = std::make_shared<MPC_MRT_Interface>(*mpc_);
+        mpc_mrt_interface_->initRollout(&legged_interface_->getRollout());
+        mpc_timer_.reset();
+
+        controller_running_ = true;
+        mpc_thread_ = std::thread([&] {
+            while (controller_running_) {
+                try {
+                    executeAndSleep(
+                        [&] {
+                            if (mpc_running_ && mpc_need_updated_) {
+                                mpc_need_updated_ = false;
+                                mpc_timer_.startTimer();
+                                mpc_mrt_interface_->advanceMpc();
+                                mpc_timer_.endTimer();
+                            }
+                        },
+                        legged_interface_->mpcSettings().mpcDesiredFrequency_);
+                } catch (const std::exception &e) {
+                    controller_running_ = false;
+                    RCLCPP_WARN(node_->get_logger(), "[Ocs2 MPC thread] Error : %s", e.what());
+                }
+            }
+        });
+        setThreadPriority(legged_interface_->sqpSettings().threadPriority, mpc_thread_);
+        RCLCPP_INFO(node_->get_logger(), "MRT initialized. MPC thread started.");
+    }
+
+    void StateOCS2::setupLeggedInterface() {
+        legged_interface_ = std::make_shared<LeggedInterface>(task_file_, urdf_file_, reference_file_);
+        legged_interface_->setupJointNames(joint_names_, feet_names_);
+        legged_interface_->setupOptimalControlProblem(task_file_, urdf_file_, reference_file_, verbose_);
+    }
+
+    void StateOCS2::setupMpc() {
+        mpc_ = std::make_shared<SqpMpc>(legged_interface_->mpcSettings(), legged_interface_->sqpSettings(),
+                                        legged_interface_->getOptimalControlProblem(),
+                                        legged_interface_->getInitializer());
+        rbd_conversions_ = std::make_shared<CentroidalModelRbdConversions>(legged_interface_->getPinocchioInterface(),
+                                                                           legged_interface_->getCentroidalModelInfo());
+
+        // Initialize the reference manager
+        const auto gait_manager_ptr = std::make_shared<GaitManager>(
+            ctrl_interfaces_, legged_interface_->getSwitchedModelReferenceManagerPtr()->
+            getGaitSchedule());
+        gait_manager_ptr->init(gait_file_);
+        mpc_->getSolverPtr()->addSynchronizedModule(gait_manager_ptr);
+        mpc_->getSolverPtr()->setReferenceManager(legged_interface_->getReferenceManagerPtr());
+
+        target_manager_ = std::make_shared<TargetManager>(ctrl_interfaces_,
+                                                          legged_interface_->getReferenceManagerPtr(),
+                                                          task_file_, reference_file_);
+    }
+}

controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.cpp

@@ -6,25 +6,16 @@
 #include <ament_index_cpp/get_package_share_directory.hpp>
 
 #include <ocs2_core/misc/LoadData.h>
-#include <ocs2_core/thread_support/ExecuteAndSleep.h>
-#include <ocs2_core/thread_support/SetThreadPriority.h>
 #include <ocs2_legged_robot_ros/gait/GaitReceiver.h>
-#include <ocs2_quadruped_controller/estimator/LinearKalmanFilter.h>
-#include <ocs2_quadruped_controller/wbc/WeightedWbc.h>
 #include <ocs2_ros_interfaces/synchronized_module/RosReferenceManager.h>
-#include <ocs2_ros_interfaces/common/RosMsgConversions.h>
 #include <ocs2_sqp/SqpMpc.h>
 #include <angles/angles.h>
 #include <ocs2_quadruped_controller/control/GaitManager.h>
-#include <ocs2_quadruped_controller/estimator/GroundTruth.h>
-#include <ocs2_quadruped_controller/estimator/FromOdomTopic.h>
 
-namespace ocs2::legged_robot
+namespace ocs2::legged_robot {
-{
     using config_type = controller_interface::interface_configuration_type;
 
-    controller_interface::InterfaceConfiguration Ocs2QuadrupedController::command_interface_configuration() const
+    controller_interface::InterfaceConfiguration Ocs2QuadrupedController::command_interface_configuration() const {
-    {
         controller_interface::InterfaceConfiguration conf = {config_type::INDIVIDUAL, {}};
 
         conf.names.reserve(joint_names_.size() * command_interface_types_.size());
@@ -41,35 +32,26 @@ namespace ocs2::legged_robot
         return conf;
     }
 
-    controller_interface::InterfaceConfiguration Ocs2QuadrupedController::state_interface_configuration() const
+    controller_interface::InterfaceConfiguration Ocs2QuadrupedController::state_interface_configuration() const {
-    {
         controller_interface::InterfaceConfiguration conf = {config_type::INDIVIDUAL, {}};
 
         conf.names.reserve(joint_names_.size() * state_interface_types_.size());
-        for (const auto& joint_name : joint_names_)
+        for (const auto &joint_name: joint_names_) {
-        {
+            for (const auto &interface_type: state_interface_types_) {
-            for (const auto& interface_type : state_interface_types_)
-            {
                 conf.names.push_back(joint_name + "/" += interface_type);
             }
         }
 
-        for (const auto& interface_type : imu_interface_types_)
+        for (const auto &interface_type: imu_interface_types_) {
-        {
             conf.names.push_back(imu_name_ + "/" += interface_type);
         }
 
-        if (estimator_type_ == "ground_truth")
+        if (estimator_type_ == "ground_truth") {
-        {
+            for (const auto &interface_type: odom_interface_types_) {
-            for (const auto& interface_type : odom_interface_types_)
-            {
                 conf.names.push_back(odom_name_ + "/" += interface_type);
             }
-        }
+        } else if (estimator_type_ == "linear_kalman") {
-        else if (estimator_type_ == "linear_kalman")
+            for (const auto &interface_type: foot_force_interface_types_) {
-        {
-            for (const auto& interface_type : foot_force_interface_types_)
-            {
                 conf.names.push_back(foot_force_name_ + "/" += interface_type);
             }
         }
@@ -77,375 +59,156 @@ namespace ocs2::legged_robot
         return conf;
     }
 
-    controller_interface::return_type Ocs2QuadrupedController::update(const rclcpp::Time& time,
+    controller_interface::return_type Ocs2QuadrupedController::update(const rclcpp::Time &time,
-                                                                      const rclcpp::Duration& period)
+                                                                      const rclcpp::Duration &period) {
-    {
+        if (mode_ == FSMMode::NORMAL) {
-        // State Estimate
+            current_state_->run(time, period);
-        updateStateEstimation(period);
+            next_state_name_ = current_state_->checkChange();
-
+            if (next_state_name_ != current_state_->state_name) {
-        // Compute target trajectory
+                mode_ = FSMMode::CHANGE;
-        ctrl_comp_.target_manager_->update();
+                next_state_ = getNextState(next_state_name_);
-
+                RCLCPP_INFO(get_node()->get_logger(), "Switched from %s to %s",
-        // Update the current state of the system
+                            current_state_->state_name_string.c_str(), next_state_->state_name_string.c_str());
-        mpc_mrt_interface_->setCurrentObservation(ctrl_comp_.observation_);
-
-        // Load the latest MPC policy
-        mpc_mrt_interface_->updatePolicy();
-        mpc_need_updated_ = true;
-
-        // Evaluate the current policy
-        vector_t optimized_state, optimized_input;
-        size_t planned_mode = 0; // The mode that is active at the time the policy is evaluated at.
-        mpc_mrt_interface_->evaluatePolicy(ctrl_comp_.observation_.time, ctrl_comp_.observation_.state, optimized_state,
-                                           optimized_input, planned_mode);
-
-        // Whole body control
-        ctrl_comp_.observation_.input = optimized_input;
-
-        wbc_timer_.startTimer();
-        vector_t x = wbc_->update(optimized_state, optimized_input, measured_rbd_state_, planned_mode,
-                                  period.seconds());
-        wbc_timer_.endTimer();
-
-        vector_t torque = x.tail(12);
-
-        vector_t pos_des = centroidal_model::getJointAngles(optimized_state,
-                                                            legged_interface_->getCentroidalModelInfo());
-        vector_t vel_des = centroidal_model::getJointVelocities(optimized_input,
-                                                                legged_interface_->getCentroidalModelInfo());
-
-        // Safety check, if failed, stop the controller
-        if (!safety_checker_->check(ctrl_comp_.observation_, optimized_state, optimized_input))
-        {
-            RCLCPP_ERROR(get_node()->get_logger(), "[Legged Controller] Safety check failed, stopping the controller.");
-            for (int i = 0; i < joint_names_.size(); i++)
-            {
-                ctrl_comp_.joint_torque_command_interface_[i].get().set_value(0);
-                ctrl_comp_.joint_position_command_interface_[i].get().set_value(0);
-                ctrl_comp_.joint_velocity_command_interface_[i].get().set_value(0);
-                ctrl_comp_.joint_kp_command_interface_[i].get().set_value(0.0);
-                ctrl_comp_.joint_kd_command_interface_[i].get().set_value(0.35);
             }
-            return controller_interface::return_type::ERROR;
+        } else if (mode_ == FSMMode::CHANGE) {
+            current_state_->exit();
+            current_state_ = next_state_;
+
+            current_state_->enter();
+            mode_ = FSMMode::NORMAL;
         }
 
-        for (int i = 0; i < joint_names_.size(); i++)
-        {
-            ctrl_comp_.joint_torque_command_interface_[i].get().set_value(torque(i));
-            ctrl_comp_.joint_position_command_interface_[i].get().set_value(pos_des(i));
-            ctrl_comp_.joint_velocity_command_interface_[i].get().set_value(vel_des(i));
-            ctrl_comp_.joint_kp_command_interface_[i].get().set_value(default_kp_);
-            ctrl_comp_.joint_kd_command_interface_[i].get().set_value(default_kd_);
-        }
-
-        // Visualization
-        ctrl_comp_.visualizer_->update(ctrl_comp_.observation_, mpc_mrt_interface_->getPolicy(),
-                                       mpc_mrt_interface_->getCommand());
-
-        observation_publisher_->publish(ros_msg_conversions::createObservationMsg(ctrl_comp_.observation_));
-
         return controller_interface::return_type::OK;
     }
 
-    controller_interface::CallbackReturn Ocs2QuadrupedController::on_init()
+    controller_interface::CallbackReturn Ocs2QuadrupedController::on_init() {
-    {
         // Initialize OCS2
         robot_pkg_ = auto_declare<std::string>("robot_pkg", robot_pkg_);
         const std::string package_share_directory = ament_index_cpp::get_package_share_directory(robot_pkg_);
 
-        urdf_file_ = package_share_directory + "/urdf/robot.urdf";
+        get_node()->get_parameter("update_rate", ctrl_interfaces_.frequency_);
-        task_file_ = package_share_directory + "/config/ocs2/task.info";
+        RCLCPP_INFO(get_node()->get_logger(), "Controller Manager Update Rate: %d Hz", ctrl_interfaces_.frequency_);
-        reference_file_ = package_share_directory + "/config/ocs2/reference.info";
-        gait_file_ = package_share_directory + "/config/ocs2/gait.info";
-
-        get_node()->get_parameter("update_rate", ctrl_comp_.frequency_);
-        RCLCPP_INFO(get_node()->get_logger(), "Controller Manager Update Rate: %d Hz", ctrl_comp_.frequency_);
-
-        // Load verbose parameter from the task file
-        verbose_ = false;
-        loadData::loadCppDataType(task_file_, "legged_robot_interface.verbose", verbose_);
 
         // Hardware Parameters
         command_prefix_ = auto_declare<std::string>("command_prefix", command_prefix_);
-        joint_names_ = auto_declare<std::vector<std::string>>("joints", joint_names_);
+        joint_names_ = auto_declare<std::vector<std::string> >("joints", joint_names_);
-        feet_names_ = auto_declare<std::vector<std::string>>("feet", feet_names_);
+        feet_names_ = auto_declare<std::vector<std::string> >("feet", feet_names_);
         command_interface_types_ =
-            auto_declare<std::vector<std::string>>("command_interfaces", command_interface_types_);
+                auto_declare<std::vector<std::string> >("command_interfaces", command_interface_types_);
         state_interface_types_ =
-            auto_declare<std::vector<std::string>>("state_interfaces", state_interface_types_);
+                auto_declare<std::vector<std::string> >("state_interfaces", state_interface_types_);
 
         // IMU Sensor
         imu_name_ = auto_declare<std::string>("imu_name", imu_name_);
-        imu_interface_types_ = auto_declare<std::vector<std::string>>("imu_interfaces", state_interface_types_);
+        imu_interface_types_ = auto_declare<std::vector<std::string> >("imu_interfaces", state_interface_types_);
 
         // Odometer Sensor (Ground Truth)
         estimator_type_ = auto_declare<std::string>("estimator_type", estimator_type_);
-        if (estimator_type_ == "ground_truth")
+        if (estimator_type_ == "ground_truth") {
-        {
             odom_name_ = auto_declare<std::string>("odom_name", odom_name_);
-            odom_interface_types_ = auto_declare<std::vector<std::string>>("odom_interfaces", state_interface_types_);
+            odom_interface_types_ = auto_declare<std::vector<std::string> >("odom_interfaces", state_interface_types_);
-        }
+        } else {
-        else
-        {
             // Foot Force Sensor
             foot_force_name_ = auto_declare<std::string>("foot_force_name", foot_force_name_);
             foot_force_interface_types_ =
-                auto_declare<std::vector<std::string>>("foot_force_interfaces", state_interface_types_);
+                    auto_declare<std::vector<std::string> >("foot_force_interfaces", state_interface_types_);
         }
 
         // PD gains
         default_kp_ = auto_declare<double>("default_kp", default_kp_);
         default_kd_ = auto_declare<double>("default_kd", default_kd_);
 
-        setupLeggedInterface();
+        state_list_.passive = std::make_shared<StatePassive>(ctrl_interfaces_);
-        setupMpc();
+        state_list_.fixedDown = std::make_shared<StateOCS2>(ctrl_interfaces_, get_node(),
-        setupMrt();
+                                                            package_share_directory, joint_names_, feet_names_,
-
+                                                            default_kp_, default_kd_);
-        // Visualization
+        state_list_.fixedDown->setupStateEstimate(estimator_type_);
-        CentroidalModelPinocchioMapping pinocchio_mapping(legged_interface_->getCentroidalModelInfo());
-        eeKinematicsPtr_ = std::make_shared<PinocchioEndEffectorKinematics>(
-            legged_interface_->getPinocchioInterface(), pinocchio_mapping,
-            legged_interface_->modelSettings().contactNames3DoF);
-
-        ctrl_comp_.visualizer_ = std::make_shared<LeggedRobotVisualizer>(
-            legged_interface_->getPinocchioInterface(), legged_interface_->getCentroidalModelInfo(), *eeKinematicsPtr_,
-            get_node());
-
-        // selfCollisionVisualization_.reset(new LeggedSelfCollisionVisualization(leggedInterface_->getPinocchioInterface(),
-        //                                                                        leggedInterface_->getGeometryInterface(), pinocchioMapping, nh));
-
-        // State estimation
-        setupStateEstimate();
-
-        // Whole body control
-        wbc_ = std::make_shared<WeightedWbc>(legged_interface_->getPinocchioInterface(),
-                                             legged_interface_->getCentroidalModelInfo(),
-                                             *eeKinematicsPtr_);
-        wbc_->loadTasksSetting(task_file_, verbose_);
-
-        // Safety Checker
-        safety_checker_ = std::make_shared<SafetyChecker>(legged_interface_->getCentroidalModelInfo());
 
         return CallbackReturn::SUCCESS;
     }
 
     controller_interface::CallbackReturn Ocs2QuadrupedController::on_configure(
-        const rclcpp_lifecycle::State& /*previous_state*/)
+        const rclcpp_lifecycle::State & /*previous_state*/) {
-    {
         control_input_subscription_ = get_node()->create_subscription<control_input_msgs::msg::Inputs>(
-            "/control_input", 10, [this](const control_input_msgs::msg::Inputs::SharedPtr msg)
+            "/control_input", 10, [this](const control_input_msgs::msg::Inputs::SharedPtr msg) {
-            {
                 // Handle message
-                ctrl_comp_.control_inputs_.command = msg->command;
+                ctrl_interfaces_.control_inputs_.command = msg->command;
-                ctrl_comp_.control_inputs_.lx = msg->lx;
+                ctrl_interfaces_.control_inputs_.lx = msg->lx;
-                ctrl_comp_.control_inputs_.ly = msg->ly;
+                ctrl_interfaces_.control_inputs_.ly = msg->ly;
-                ctrl_comp_.control_inputs_.rx = msg->rx;
+                ctrl_interfaces_.control_inputs_.rx = msg->rx;
-                ctrl_comp_.control_inputs_.ry = msg->ry;
+                ctrl_interfaces_.control_inputs_.ry = msg->ry;
             });
 
-        observation_publisher_ = get_node()->create_publisher<ocs2_msgs::msg::MpcObservation>(
-            "legged_robot_mpc_observation", 10);
-
         return CallbackReturn::SUCCESS;
     }
 
     controller_interface::CallbackReturn Ocs2QuadrupedController::on_activate(
-        const rclcpp_lifecycle::State& /*previous_state*/)
+        const rclcpp_lifecycle::State & /*previous_state*/) {
-    {
         // clear out vectors in case of restart
-        ctrl_comp_.clear();
+        ctrl_interfaces_.clear();
 
         // assign command interfaces
-        for (auto& interface : command_interfaces_)
+        for (auto &interface: command_interfaces_) {
-        {
             std::string interface_name = interface.get_interface_name();
-            if (const size_t pos = interface_name.find('/'); pos != std::string::npos)
+            if (const size_t pos = interface_name.find('/'); pos != std::string::npos) {
-            {
                 command_interface_map_[interface_name.substr(pos + 1)]->push_back(interface);
-            }
+            } else {
-            else
-            {
                 command_interface_map_[interface_name]->push_back(interface);
             }
         }
 
         // assign state interfaces
-        for (auto& interface : state_interfaces_)
+        for (auto &interface: state_interfaces_) {
-        {
+            if (interface.get_prefix_name() == imu_name_) {
-            if (interface.get_prefix_name() == imu_name_)
+                ctrl_interfaces_.imu_state_interface_.emplace_back(interface);
-            {
+            } else if (interface.get_prefix_name() == foot_force_name_) {
-                ctrl_comp_.imu_state_interface_.emplace_back(interface);
+                ctrl_interfaces_.foot_force_state_interface_.emplace_back(interface);
-            }
+            } else if (interface.get_prefix_name() == odom_name_) {
-            else if (interface.get_prefix_name() == foot_force_name_)
+                ctrl_interfaces_.odom_state_interface_.emplace_back(interface);
-            {
+            } else {
-                ctrl_comp_.foot_force_state_interface_.emplace_back(interface);
-            }
-            else if (interface.get_prefix_name() == odom_name_)
-            {
-                ctrl_comp_.odom_state_interface_.emplace_back(interface);
-            }
-            else
-            {
                 state_interface_map_[interface.get_interface_name()]->push_back(interface);
             }
         }
 
-        if (mpc_running_ == false)
+        current_state_ = state_list_.passive;
-        {
+        current_state_->enter();
-            // Initial state
+        next_state_ = current_state_;
-            ctrl_comp_.observation_.state.setZero(
+        next_state_name_ = current_state_->state_name;
-                static_cast<long>(legged_interface_->getCentroidalModelInfo().stateDim));
+        mode_ = FSMMode::NORMAL;
-            updateStateEstimation(rclcpp::Duration(0, 1 / ctrl_comp_.frequency_ * 1000000000));
-            ctrl_comp_.observation_.input.setZero(
-                static_cast<long>(legged_interface_->getCentroidalModelInfo().inputDim));
-            ctrl_comp_.observation_.mode = STANCE;
-
-            const TargetTrajectories target_trajectories({ctrl_comp_.observation_.time},
-                                                         {ctrl_comp_.observation_.state},
-                                                         {ctrl_comp_.observation_.input});
-
-            // Set the first observation and command and wait for optimization to finish
-            mpc_mrt_interface_->setCurrentObservation(ctrl_comp_.observation_);
-            mpc_mrt_interface_->getReferenceManager().setTargetTrajectories(target_trajectories);
-            RCLCPP_INFO(get_node()->get_logger(), "Waiting for the initial policy ...");
-            while (!mpc_mrt_interface_->initialPolicyReceived())
-            {
-                mpc_mrt_interface_->advanceMpc();
-                rclcpp::WallRate(legged_interface_->mpcSettings().mrtDesiredFrequency_).sleep();
-            }
-            RCLCPP_INFO(get_node()->get_logger(), "Initial policy has been received.");
-
-            mpc_running_ = true;
-        }
 
         return CallbackReturn::SUCCESS;
     }
 
     controller_interface::CallbackReturn Ocs2QuadrupedController::on_deactivate(
-        const rclcpp_lifecycle::State& /*previous_state*/)
+        const rclcpp_lifecycle::State & /*previous_state*/) {
-    {
         release_interfaces();
         return CallbackReturn::SUCCESS;
     }
 
     controller_interface::CallbackReturn Ocs2QuadrupedController::on_cleanup(
-        const rclcpp_lifecycle::State& /*previous_state*/)
+        const rclcpp_lifecycle::State & /*previous_state*/) {
-    {
         return CallbackReturn::SUCCESS;
     }
 
     controller_interface::CallbackReturn Ocs2QuadrupedController::on_shutdown(
-        const rclcpp_lifecycle::State& /*previous_state*/)
+        const rclcpp_lifecycle::State & /*previous_state*/) {
-    {
         return CallbackReturn::SUCCESS;
     }
 
     controller_interface::CallbackReturn Ocs2QuadrupedController::on_error(
-        const rclcpp_lifecycle::State& /*previous_state*/)
+        const rclcpp_lifecycle::State & /*previous_state*/) {
-    {
         return CallbackReturn::SUCCESS;
     }
 
-    void Ocs2QuadrupedController::setupLeggedInterface()
+    std::shared_ptr<FSMState> Ocs2QuadrupedController::getNextState(FSMStateName stateName) const {
-    {
+        switch (stateName) {
-        legged_interface_ = std::make_shared<LeggedInterface>(task_file_, urdf_file_, reference_file_);
+            case FSMStateName::PASSIVE:
-        legged_interface_->setupJointNames(joint_names_, feet_names_);
+                return state_list_.passive;
-        legged_interface_->setupOptimalControlProblem(task_file_, urdf_file_, reference_file_, verbose_);
+            case FSMStateName::FIXEDDOWN:
+                return state_list_.fixedDown;
+            default:
+                return state_list_.invalid;
         }
-
-    void Ocs2QuadrupedController::setupMpc()
-    {
-        mpc_ = std::make_shared<SqpMpc>(legged_interface_->mpcSettings(), legged_interface_->sqpSettings(),
-                                        legged_interface_->getOptimalControlProblem(),
-                                        legged_interface_->getInitializer());
-        rbd_conversions_ = std::make_shared<CentroidalModelRbdConversions>(legged_interface_->getPinocchioInterface(),
-                                                                           legged_interface_->getCentroidalModelInfo());
-
-        // Initialize the reference manager
-        const auto gait_manager_ptr = std::make_shared<GaitManager>(
-            ctrl_comp_, legged_interface_->getSwitchedModelReferenceManagerPtr()->
-                                           getGaitSchedule());
-        gait_manager_ptr->init(gait_file_);
-        mpc_->getSolverPtr()->addSynchronizedModule(gait_manager_ptr);
-        mpc_->getSolverPtr()->setReferenceManager(legged_interface_->getReferenceManagerPtr());
-
-        ctrl_comp_.target_manager_ = std::make_shared<TargetManager>(ctrl_comp_,
-                                                                     legged_interface_->getReferenceManagerPtr(),
-                                                                     task_file_, reference_file_);
-    }
-
-    void Ocs2QuadrupedController::setupMrt()
-    {
-        mpc_mrt_interface_ = std::make_shared<MPC_MRT_Interface>(*mpc_);
-        mpc_mrt_interface_->initRollout(&legged_interface_->getRollout());
-        mpc_timer_.reset();
-
-        controller_running_ = true;
-        mpc_thread_ = std::thread([&]
-        {
-            while (controller_running_)
-            {
-                try
-                {
-                    executeAndSleep(
-                        [&]
-                        {
-                            if (mpc_running_ && mpc_need_updated_)
-                            {
-                                mpc_need_updated_ = false;
-                                mpc_timer_.startTimer();
-                                mpc_mrt_interface_->advanceMpc();
-                                mpc_timer_.endTimer();
-                            }
-                        },
-                        legged_interface_->mpcSettings().mpcDesiredFrequency_);
-                }
-                catch (const std::exception& e)
-                {
-                    controller_running_ = false;
-                    RCLCPP_WARN(get_node()->get_logger(), "[Ocs2 MPC thread] Error : %s", e.what());
-                }
-            }
-        });
-        setThreadPriority(legged_interface_->sqpSettings().threadPriority, mpc_thread_);
-        RCLCPP_INFO(get_node()->get_logger(), "MRT initialized. MPC thread started.");
-    }
-
-    void Ocs2QuadrupedController::setupStateEstimate()
-    {
-        if (estimator_type_ == "ground_truth")
-        {
-            ctrl_comp_.estimator_ = std::make_shared<GroundTruth>(legged_interface_->getCentroidalModelInfo(),
-                                                                  ctrl_comp_,
-                                                                  get_node());
-            RCLCPP_INFO(get_node()->get_logger(), "Using Ground Truth Estimator");
-        }
-        else if (estimator_type_ == "linear_kalman")
-        {
-            ctrl_comp_.estimator_ = std::make_shared<KalmanFilterEstimate>(legged_interface_->getPinocchioInterface(),
-                                                                           legged_interface_->getCentroidalModelInfo(),
-                                                                           *eeKinematicsPtr_, ctrl_comp_,
-                                                                           get_node());
-            dynamic_cast<KalmanFilterEstimate&>(*ctrl_comp_.estimator_).loadSettings(task_file_, verbose_);
-            RCLCPP_INFO(get_node()->get_logger(), "Using Kalman Filter Estimator");
-        }
-        else
-        {
-            ctrl_comp_.estimator_ = std::make_shared<FromOdomTopic>(legged_interface_->getCentroidalModelInfo(),ctrl_comp_,get_node());
-            RCLCPP_INFO(get_node()->get_logger(), "Using Odom Topic Based Estimator");
-        }
-        ctrl_comp_.observation_.time = 0;
-    }
-
-    void Ocs2QuadrupedController::updateStateEstimation(const rclcpp::Duration& period)
-    {
-        measured_rbd_state_ = ctrl_comp_.estimator_->update(get_node()->now(), period);
-        ctrl_comp_.observation_.time += period.seconds();
-        const scalar_t yaw_last = ctrl_comp_.observation_.state(9);
-        ctrl_comp_.observation_.state = rbd_conversions_->computeCentroidalStateFromRbdModel(measured_rbd_state_);
-        ctrl_comp_.observation_.state(9) = yaw_last + angles::shortest_angular_distance(
-            yaw_last, ctrl_comp_.observation_.state(9));
-        // ctrl_comp_.observation_.mode = ctrl_comp_.estimator_->getMode();
     }
 }
 

controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.h

@@ -5,18 +5,22 @@
 #ifndef OCS2QUADRUPEDCONTROLLER_H
 #define OCS2QUADRUPEDCONTROLLER_H
 
+#include <controller_common/FSM/StatePassive.h>
 #include <controller_interface/controller_interface.hpp>
 #include <control_input_msgs/msg/inputs.hpp>
-#include <ocs2_centroidal_model/CentroidalModelRbdConversions.h>
 #include <ocs2_mpc/MPC_MRT_Interface.h>
 #include <ocs2_quadruped_controller/estimator/StateEstimateBase.h>
 #include <ocs2_quadruped_controller/interface/LeggedInterface.h>
 #include <ocs2_quadruped_controller/wbc/WbcBase.h>
-#include <ocs2_msgs/msg/mpc_observation.hpp>
+#include <ocs2_quadruped_controller/FSM/StateOCS2.h>
-#include "SafetyChecker.h"
-#include "ocs2_quadruped_controller/control/CtrlComponent.h"
 
 namespace ocs2::legged_robot {
+    struct FSMStateList {
+        std::shared_ptr<FSMState> invalid;
+        std::shared_ptr<StatePassive> passive;
+        std::shared_ptr<StateOCS2> fixedDown;
+    };
+
     class Ocs2QuadrupedController final : public controller_interface::ControllerInterface {
     public:
         Ocs2QuadrupedController() = default;
@@ -51,17 +55,17 @@ namespace ocs2::legged_robot {
             const rclcpp_lifecycle::State &previous_state) override;
 
     protected:
-        void setupLeggedInterface();
 
-        void setupMpc();
+        std::shared_ptr<FSMState> getNextState(FSMStateName stateName) const;
 
-        void setupMrt();
+        FSMMode mode_ = FSMMode::NORMAL;
+        std::string state_name_;
+        FSMStateName next_state_name_ = FSMStateName::INVALID;
+        FSMStateList state_list_;
+        std::shared_ptr<FSMState> current_state_;
+        std::shared_ptr<FSMState> next_state_;
 
-        void setupStateEstimate();
+        CtrlInterfaces ctrl_interfaces_;
-
-        void updateStateEstimation(const rclcpp::Duration &period);
-
-        CtrlComponent ctrl_comp_;
         std::vector<std::string> joint_names_;
         std::vector<std::string> feet_names_;
         std::vector<std::string> command_interface_types_;
@@ -70,18 +74,18 @@ namespace ocs2::legged_robot {
         std::unordered_map<
             std::string, std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface> > *>
         command_interface_map_ = {
-            {"effort", &ctrl_comp_.joint_torque_command_interface_},
+            {"effort", &ctrl_interfaces_.joint_torque_command_interface_},
-            {"position", &ctrl_comp_.joint_position_command_interface_},
+            {"position", &ctrl_interfaces_.joint_position_command_interface_},
-            {"velocity", &ctrl_comp_.joint_velocity_command_interface_},
+            {"velocity", &ctrl_interfaces_.joint_velocity_command_interface_},
-            {"kp", &ctrl_comp_.joint_kp_command_interface_},
+            {"kp", &ctrl_interfaces_.joint_kp_command_interface_},
-            {"kd", &ctrl_comp_.joint_kd_command_interface_}
+            {"kd", &ctrl_interfaces_.joint_kd_command_interface_}
         };
         std::unordered_map<
             std::string, std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> > *>
         state_interface_map_ = {
-            {"position", &ctrl_comp_.joint_position_state_interface_},
+            {"position", &ctrl_interfaces_.joint_position_state_interface_},
-            {"effort", &ctrl_comp_.joint_effort_state_interface_},
+            {"effort", &ctrl_interfaces_.joint_effort_state_interface_},
-            {"velocity", &ctrl_comp_.joint_velocity_state_interface_}
+            {"velocity", &ctrl_interfaces_.joint_velocity_state_interface_}
         };
 
         std::string robot_pkg_;
@@ -104,35 +108,6 @@ namespace ocs2::legged_robot {
         double default_kd_ = 6;
 
         rclcpp::Subscription<control_input_msgs::msg::Inputs>::SharedPtr control_input_subscription_;
-        rclcpp::Publisher<ocs2_msgs::msg::MpcObservation>::SharedPtr observation_publisher_;
-
-        std::string task_file_;
-        std::string urdf_file_;
-        std::string reference_file_;
-        std::string gait_file_;
-
-        bool verbose_;
-        bool mpc_need_updated_;
-
-        std::shared_ptr<LeggedInterface> legged_interface_;
-        std::shared_ptr<PinocchioEndEffectorKinematics> eeKinematicsPtr_;
-
-        // Whole Body Control
-        std::shared_ptr<WbcBase> wbc_;
-        std::shared_ptr<SafetyChecker> safety_checker_;
-
-        // Nonlinear MPC
-        std::shared_ptr<MPC_BASE> mpc_;
-        std::shared_ptr<MPC_MRT_Interface> mpc_mrt_interface_;
-
-        std::shared_ptr<CentroidalModelRbdConversions> rbd_conversions_;
-
-    private:
-        vector_t measured_rbd_state_;
-        std::thread mpc_thread_;
-        std::atomic_bool controller_running_{}, mpc_running_{};
-        benchmark::RepeatedTimer mpc_timer_;
-        benchmark::RepeatedTimer wbc_timer_;
     };
 }
 

controllers/ocs2_quadruped_controller/src/control/GaitManager.cpp

@@ -10,9 +10,9 @@
 
 namespace ocs2::legged_robot
 {
-    GaitManager::GaitManager(CtrlComponent& ctrl_component,
+    GaitManager::GaitManager(CtrlInterfaces& ctrl_interfaces,
                              std::shared_ptr<GaitSchedule> gait_schedule_ptr)
-        : ctrl_component_(ctrl_component),
+        : ctrl_interfaces_(ctrl_interfaces),
           gait_schedule_ptr_(std::move(gait_schedule_ptr)),
           target_gait_({0.0, 1.0}, {STANCE})
     {
@@ -48,13 +48,12 @@ namespace ocs2::legged_robot
 
     void GaitManager::getTargetGait()
     {
-        if (ctrl_component_.control_inputs_.command == 0) return;
+        if (ctrl_interfaces_.control_inputs_.command == 0) return;
-        if (ctrl_component_.control_inputs_.command == last_command_) return;
+        if (ctrl_interfaces_.control_inputs_.command == last_command_) return;
-        last_command_ = ctrl_component_.control_inputs_.command;
+        last_command_ = ctrl_interfaces_.control_inputs_.command;
-        target_gait_ = gait_list_[ctrl_component_.control_inputs_.command - 1];
+        target_gait_ = gait_list_[ctrl_interfaces_.control_inputs_.command -2];
         RCLCPP_INFO(rclcpp::get_logger("GaitManager"), "Switch to gait: %s",
-                    gait_name_list_[ctrl_component_.control_inputs_.command - 1].c_str());
+                    gait_name_list_[ctrl_interfaces_.control_inputs_.command - 2].c_str());
         gait_updated_ = true;
-        ctrl_component_.reset = false;
     }
 }

controllers/ocs2_quadruped_controller/src/control/TargetManager.cpp

@@ -7,11 +7,9 @@
 #include <ocs2_core/misc/LoadData.h>
 #include <ocs2_robotic_tools/common/RotationTransforms.h>
 
-#include "ocs2_quadruped_controller/control/CtrlComponent.h"
-
 namespace ocs2::legged_robot
 {
-    TargetManager::TargetManager(CtrlComponent& ctrl_component,
+    TargetManager::TargetManager(CtrlInterfaces& ctrl_component,
                                  const std::shared_ptr<ReferenceManagerInterface>& referenceManagerPtr,
                                  const std::string& task_file,
                                  const std::string& reference_file)
@@ -26,16 +24,15 @@ namespace ocs2::legged_robot
         loadData::loadCppDataType(reference_file, "targetDisplacementVelocity", target_displacement_velocity_);
     }
 
-    void TargetManager::update()
+    void TargetManager::update(SystemObservation &observation)
     {
-        if (ctrl_component_.reset) return;
         vector_t cmdGoal = vector_t::Zero(6);
         cmdGoal[0] = ctrl_component_.control_inputs_.ly * target_displacement_velocity_;
         cmdGoal[1] = -ctrl_component_.control_inputs_.lx * target_displacement_velocity_;
         cmdGoal[2] = ctrl_component_.control_inputs_.ry;
         cmdGoal[3] = -ctrl_component_.control_inputs_.rx * target_rotation_velocity_;
 
-        const vector_t currentPose = ctrl_component_.observation_.state.segment<6>(6);
+        const vector_t currentPose = observation.state.segment<6>(6);
         const Eigen::Matrix<scalar_t, 3, 1> zyx = currentPose.tail(3);
         vector_t cmd_vel_rot = getRotationMatrixFromZyxEulerAngles(zyx) * cmdGoal.head(3);
 
@@ -51,9 +48,9 @@ namespace ocs2::legged_robot
             return target;
         }();
 
-        const scalar_t targetReachingTime = ctrl_component_.observation_.time + time_to_target_;
+        const scalar_t targetReachingTime = observation.time + time_to_target_;
         auto trajectories =
-            targetPoseToTargetTrajectories(targetPose, ctrl_component_.observation_, targetReachingTime);
+            targetPoseToTargetTrajectories(targetPose, observation, targetReachingTime);
         trajectories.stateTrajectory[0].head(3) = cmd_vel_rot;
         trajectories.stateTrajectory[1].head(3) = cmd_vel_rot;
 

controllers/ocs2_quadruped_controller/src/estimator/FromOdomTopic.cpp

@@ -5,7 +5,7 @@
 #include "ocs2_quadruped_controller/estimator/FromOdomTopic.h"
 
 namespace ocs2::legged_robot {
-    FromOdomTopic::FromOdomTopic(CentroidalModelInfo info, CtrlComponent &ctrl_component,
+    FromOdomTopic::FromOdomTopic(CentroidalModelInfo info, CtrlInterfaces &ctrl_component,
                                  const rclcpp_lifecycle::LifecycleNode::SharedPtr &node) : StateEstimateBase(
         std::move(info), ctrl_component,
         node) {

controllers/ocs2_quadruped_controller/src/estimator/GroundTruth.cpp

@@ -4,11 +4,9 @@
 
 #include "ocs2_quadruped_controller/estimator/GroundTruth.h"
 
-#include <ocs2_quadruped_controller/control/CtrlComponent.h>
-
 namespace ocs2::legged_robot
 {
-    GroundTruth::GroundTruth(CentroidalModelInfo info, CtrlComponent& ctrl_component,
+    GroundTruth::GroundTruth(CentroidalModelInfo info, CtrlInterfaces& ctrl_component,
                              const rclcpp_lifecycle::LifecycleNode::SharedPtr& node)
         : StateEstimateBase(
             std::move(info), ctrl_component,

controllers/ocs2_quadruped_controller/src/estimator/LinearKalmanFilter.cpp

@@ -16,7 +16,7 @@ namespace ocs2::legged_robot {
     KalmanFilterEstimate::KalmanFilterEstimate(PinocchioInterface pinocchio_interface,
                                                CentroidalModelInfo info,
                                                const PinocchioEndEffectorKinematics &ee_kinematics,
-                                               CtrlComponent &ctrl_component,
+                                               CtrlInterfaces &ctrl_component,
                                                const rclcpp_lifecycle::LifecycleNode::SharedPtr &node)
         : StateEstimateBase(std::move(info), ctrl_component,
                             node),

controllers/ocs2_quadruped_controller/src/estimator/StateEstimateBase.cpp

@@ -9,11 +9,10 @@
 #include <ocs2_robotic_tools/common/RotationDerivativesTransforms.h>
 
 #include <memory>
-#include "ocs2_quadruped_controller/control/CtrlComponent.h"
 
 namespace ocs2::legged_robot {
     StateEstimateBase::StateEstimateBase(CentroidalModelInfo info,
-                                         CtrlComponent &ctrl_component,
+                                         CtrlInterfaces &ctrl_component,
                                          rclcpp_lifecycle::LifecycleNode::SharedPtr node)
         : ctrl_component_(ctrl_component),
           info_(std::move(info)),

descriptions/unitree/go2_description/config/robot_control.yaml

@@ -125,16 +125,6 @@ ocs2_quadruped_controller:
       - linear_acceleration.y
       - linear_acceleration.z
 
-    estimator_type: "ground_truth"
-    odom_name: "odometer"
-    odom_interfaces:
-      - position.x
-      - position.y
-      - position.z
-      - velocity.x
-      - velocity.y
-      - velocity.z
-
     foot_force_name: "foot_force"
     foot_force_interfaces:
       - FL

libraries/controller_common/include/controller_common/CtrlInterfaces.h

@@ -33,6 +33,13 @@ struct CtrlInterfaces {
     std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> >
     imu_state_interface_;
 
+    std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> >
+    foot_force_state_interface_;
+
+    std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> >
+    odom_state_interface_;
+
+
     control_input_msgs::msg::Inputs control_inputs_;
     int frequency_{};
 
@@ -50,6 +57,8 @@ struct CtrlInterfaces {
         joint_velocity_state_interface_.clear();
 
         imu_state_interface_.clear();
+        imu_state_interface_.clear();
+        foot_force_state_interface_.clear();
     }
 };
 

libraries/controller_common/include/controller_common/FSM/BaseFixedStand.h

@@ -5,8 +5,6 @@
 #ifndef BASEFIXEDSTAND_H
 #define BASEFIXEDSTAND_H
 
-#include <rclcpp/time.hpp>
-
 #include "FSMState.h"
 
 class BaseFixedStand : public FSMState {
@@ -18,7 +16,8 @@ public:
 
     void enter() override;
 
-    void run() override;
+    void run(const rclcpp::Time &time,
+             const rclcpp::Duration &period) override;
 
     void exit() override;
 

libraries/controller_common/include/controller_common/FSM/FSMState.h

@@ -9,6 +9,7 @@
 #include <utility>
 #include <controller_common/common/enumClass.h>
 #include <controller_common/CtrlInterfaces.h>
+#include <rclcpp/time.hpp>
 
 class FSMState {
 public:
@@ -22,7 +23,8 @@ public:
 
     virtual void enter() = 0;
 
-    virtual void run() = 0;
+    virtual void run(const rclcpp::Time &time,
+                     const rclcpp::Duration &period) = 0;
 
     virtual void exit() = 0;
 

libraries/controller_common/include/controller_common/FSM/StateFixedDown.h

@@ -5,8 +5,6 @@
 #ifndef STATEFIXEDDOWN_H
 #define STATEFIXEDDOWN_H
 
-#include <rclcpp/time.hpp>
-
 #include "FSMState.h"
 
 class StateFixedDown final : public FSMState {
@@ -19,7 +17,8 @@ public:
 
     void enter() override;
 
-    void run() override;
+    void run(const rclcpp::Time &time,
+             const rclcpp::Duration &period) override;
 
     void exit() override;
 

libraries/controller_common/include/controller_common/FSM/StatePassive.h

@@ -6,14 +6,14 @@
 #define STATEPASSIVE_H
 #include "FSMState.h"
 
-
 class StatePassive final : public FSMState {
 public:
     explicit StatePassive(CtrlInterfaces &ctrl_interfaces);
 
     void enter() override;
 
-    void run() override;
+    void run(const rclcpp::Time &time,
+             const rclcpp::Duration &period) override;
 
     void exit() override;
 

libraries/controller_common/src/FSM/BaseFixedStand.cpp

@@ -31,7 +31,7 @@ void BaseFixedStand::enter() {
     ctrl_interfaces_.control_inputs_.command = 0;
 }
 
-void BaseFixedStand::run() {
+void BaseFixedStand::run(const rclcpp::Time &/*time*/, const rclcpp::Duration &/*period*/) {
     percent_ += 1 / duration_;
     phase = std::tanh(percent_);
     for (int i = 0; i < 12; i++) {

libraries/controller_common/src/FSM/StateFixedDown.cpp

@@ -32,7 +32,7 @@ void StateFixedDown::enter() {
     }
 }
 
-void StateFixedDown::run() {
+void StateFixedDown::run(const rclcpp::Time &/*time*/, const rclcpp::Duration &/*period*/) {
     percent_ += 1 / duration_;
     phase = std::tanh(percent_);
     for (int i = 0; i < 12; i++) {

libraries/controller_common/src/FSM/StatePassive.cpp

@@ -29,7 +29,7 @@ void StatePassive::enter() {
     ctrl_interfaces_.control_inputs_.command = 0;
 }
 
-void StatePassive::run() {
+void StatePassive::run(const rclcpp::Time &/*time*/, const rclcpp::Duration &/*period*/) {
 }
 
 void StatePassive::exit() {