From aefd4fb3dda848e505303850c230a8c6ee796157 Mon Sep 17 00:00:00 2001 From: Huang Zhenbiao Date: Thu, 27 Feb 2025 21:43:10 +0800 Subject: [PATCH] take FSM out as separate library --- .../ocs2_quadruped_controller/CMakeLists.txt | 2 + .../ocs2_quadruped_controller/FSM/StateOCS2.h | 107 +++++ .../control/CtrlComponent.h | 75 ---- .../control/GaitManager.h | 7 +- .../control/TargetManager.h | 18 +- .../estimator/FromOdomTopic.h | 2 +- .../estimator/GroundTruth.h | 2 +- .../estimator/LinearKalmanFilter.h | 2 +- .../estimator/StateEstimateBase.h | 7 +- .../ocs2_quadruped_controller/package.xml | 1 + .../src/FSM/StateOCS2.cpp | 268 ++++++++++++ .../src/Ocs2QuadrupedController.cpp | 397 ++++-------------- .../src/Ocs2QuadrupedController.h | 73 ++-- .../src/control/GaitManager.cpp | 15 +- .../src/control/TargetManager.cpp | 13 +- .../src/estimator/FromOdomTopic.cpp | 2 +- .../src/estimator/GroundTruth.cpp | 4 +- .../src/estimator/LinearKalmanFilter.cpp | 2 +- .../src/estimator/StateEstimateBase.cpp | 3 +- .../go2_description/config/robot_control.yaml | 10 - .../controller_common/CtrlInterfaces.h | 9 + .../controller_common/FSM/BaseFixedStand.h | 5 +- .../include/controller_common/FSM/FSMState.h | 4 +- .../controller_common/FSM/StateFixedDown.h | 5 +- .../controller_common/FSM/StatePassive.h | 4 +- .../src/FSM/BaseFixedStand.cpp | 2 +- .../src/FSM/StateFixedDown.cpp | 2 +- .../src/FSM/StatePassive.cpp | 2 +- 28 files changed, 537 insertions(+), 506 deletions(-) create mode 100644 controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/FSM/StateOCS2.h delete mode 100644 controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/control/CtrlComponent.h create mode 100644 controllers/ocs2_quadruped_controller/src/FSM/StateOCS2.cpp diff --git a/controllers/ocs2_quadruped_controller/CMakeLists.txt b/controllers/ocs2_quadruped_controller/CMakeLists.txt index 82b0a21..5d4f59b 100644 --- a/controllers/ocs2_quadruped_controller/CMakeLists.txt +++ b/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 diff --git a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/FSM/StateOCS2.h b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/FSM/StateOCS2.h new file mode 100644 index 0000000..d5bae79 --- /dev/null +++ b/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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#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 &node, + const std::string &package_share_directory, + const std::vector &joint_names, + const std::vector &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 node_; + rclcpp::Publisher::SharedPtr observation_publisher_; + std::shared_ptr estimator_; + std::shared_ptr target_manager_; + std::shared_ptr visualizer_; + + std::shared_ptr legged_interface_; + std::shared_ptr eeKinematicsPtr_; + + // Whole Body Control + std::shared_ptr wbc_; + std::shared_ptr safety_checker_; + + // Nonlinear MPC + std::shared_ptr mpc_; + std::shared_ptr mpc_mrt_interface_; + + std::shared_ptr 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 joint_names_; + std::vector feet_names_; + double default_kp_; + double default_kd_; + + bool verbose_; + bool mpc_need_updated_; + vector_t optimized_state_, optimized_input_; + }; +} + + +#endif //STATEOCS2_H diff --git a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/control/CtrlComponent.h b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/control/CtrlComponent.h deleted file mode 100644 index ec9add8..0000000 --- a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/control/CtrlComponent.h +++ /dev/null @@ -1,75 +0,0 @@ -// -// Created by biao on 24-9-10. -// - -#ifndef INTERFACE_H -#define INTERFACE_H - -#include -#include -#include -#include -#include -#include - -#include "TargetManager.h" -#include "ocs2_quadruped_controller/estimator/StateEstimateBase.h" - -struct CtrlComponent { - std::vector > - joint_torque_command_interface_; - std::vector > - joint_position_command_interface_; - std::vector > - joint_velocity_command_interface_; - std::vector > - joint_kp_command_interface_; - std::vector > - joint_kd_command_interface_; - - - std::vector > - joint_effort_state_interface_; - std::vector > - joint_position_state_interface_; - std::vector > - joint_velocity_state_interface_; - - std::vector > - imu_state_interface_; - - std::vector > - foot_force_state_interface_; - - std::vector > - odom_state_interface_; - - control_input_msgs::msg::Inputs control_inputs_; - ocs2::SystemObservation observation_; - int frequency_{}; - bool reset = true; - - std::shared_ptr estimator_; - std::shared_ptr target_manager_; - std::shared_ptr 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 diff --git a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/control/GaitManager.h b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/control/GaitManager.h index 790e239..2e86ad6 100644 --- a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/control/GaitManager.h +++ b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/control/GaitManager.h @@ -4,15 +4,14 @@ #ifndef GAITMANAGER_H #define GAITMANAGER_H +#include #include #include -#include "CtrlComponent.h" - namespace ocs2::legged_robot { class GaitManager final : public SolverSynchronizedModule { public: - GaitManager(CtrlComponent &ctrl_component, + GaitManager(CtrlInterfaces &ctrl_interfaces, std::shared_ptr 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 gait_schedule_ptr_; ModeSequenceTemplate target_gait_; diff --git a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/control/TargetManager.h b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/control/TargetManager.h index 65afa12..5fffbfd 100644 --- a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/control/TargetManager.h +++ b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/control/TargetManager.h @@ -7,27 +7,26 @@ #include +#include #include #include -struct CtrlComponent; - namespace ocs2::legged_robot { class TargetManager { public: - TargetManager(CtrlComponent &ctrl_component, + TargetManager(CtrlInterfaces &ctrl_component, const std::shared_ptr &referenceManagerPtr, - const std::string& task_file, - const std::string& reference_file); + const std::string &task_file, + const std::string &reference_file); ~TargetManager() = default; - void update(); + void update(SystemObservation &observation); private: TargetTrajectories targetPoseToTargetTrajectories(const vector_t &targetPose, - const SystemObservation &observation, - const scalar_t &targetReachingTime) { + const SystemObservation &observation, + const scalar_t &targetReachingTime) { // desired time trajectory const scalar_array_t timeTrajectory{observation.time, targetReachingTime}; @@ -45,7 +44,8 @@ namespace ocs2::legged_robot { return {timeTrajectory, stateTrajectory, inputTrajectory}; } - CtrlComponent &ctrl_component_; + + CtrlInterfaces &ctrl_component_; std::shared_ptr referenceManagerPtr_; vector_t default_joint_state_{}; diff --git a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/FromOdomTopic.h b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/FromOdomTopic.h index c23c858..2a47aaa 100644 --- a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/FromOdomTopic.h +++ b/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; diff --git a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/GroundTruth.h b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/GroundTruth.h index 1c33a70..1f2456c 100644 --- a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/GroundTruth.h +++ b/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; diff --git a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/LinearKalmanFilter.h b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/LinearKalmanFilter.h index b0b25b3..3bf14d6 100644 --- a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/LinearKalmanFilter.h +++ b/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; diff --git a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/StateEstimateBase.h b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/StateEstimateBase.h index e451755..c6f634f 100644 --- a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/StateEstimateBase.h +++ b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/StateEstimateBase.h @@ -3,6 +3,7 @@ // #pragma once +#include #include #include @@ -15,14 +16,12 @@ #include #include -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(); diff --git a/controllers/ocs2_quadruped_controller/package.xml b/controllers/ocs2_quadruped_controller/package.xml index 9dd3692..f506b6f 100644 --- a/controllers/ocs2_quadruped_controller/package.xml +++ b/controllers/ocs2_quadruped_controller/package.xml @@ -11,6 +11,7 @@ backward_ros controller_interface + controller_common pluginlib control_input_msgs qpoases_colcon diff --git a/controllers/ocs2_quadruped_controller/src/FSM/StateOCS2.cpp b/controllers/ocs2_quadruped_controller/src/FSM/StateOCS2.cpp new file mode 100644 index 0000000..1f03474 --- /dev/null +++ b/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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace ocs2::legged_robot { + StateOCS2::StateOCS2(CtrlInterfaces &ctrl_interfaces, + const std::shared_ptr &node, + const std::string &package_share_directory, + const std::vector &joint_names, + const std::vector &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( + legged_interface_->getPinocchioInterface(), pinocchio_mapping, + legged_interface_->modelSettings().contactNames3DoF); + + visualizer_ = std::make_shared( + 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(legged_interface_->getPinocchioInterface(), + legged_interface_->getCentroidalModelInfo(), + *eeKinematicsPtr_); + wbc_->loadTasksSetting(task_file_, verbose_); + + // Safety Checker + safety_checker_ = std::make_shared(legged_interface_->getCentroidalModelInfo()); + + observation_publisher_ = node_->create_publisher( + "legged_robot_mpc_observation", 10); + } + + void StateOCS2::enter() { + if (mpc_running_ == false) { + // Initial state + observation_.state.setZero( + static_cast(legged_interface_->getCentroidalModelInfo().stateDim)); + updateStateEstimation(rclcpp::Duration(0, 1 / ctrl_interfaces_.frequency_ * 1000000000)); + observation_.input.setZero( + static_cast(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(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( + legged_interface_->getPinocchioInterface(), + legged_interface_->getCentroidalModelInfo(), + *eeKinematicsPtr_, ctrl_interfaces_, + node_); + dynamic_cast(*estimator_).loadSettings(task_file_, verbose_); + RCLCPP_INFO(node_->get_logger(), "Using Kalman Filter Estimator"); + } else { + estimator_ = std::make_shared( + 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_); + 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(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(legged_interface_->mpcSettings(), legged_interface_->sqpSettings(), + legged_interface_->getOptimalControlProblem(), + legged_interface_->getInitializer()); + rbd_conversions_ = std::make_shared(legged_interface_->getPinocchioInterface(), + legged_interface_->getCentroidalModelInfo()); + + // Initialize the reference manager + const auto gait_manager_ptr = std::make_shared( + 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(ctrl_interfaces_, + legged_interface_->getReferenceManagerPtr(), + task_file_, reference_file_); + } +} diff --git a/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.cpp b/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.cpp index 4f16f48..f11e760 100644 --- a/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.cpp +++ b/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.cpp @@ -6,25 +6,16 @@ #include #include -#include -#include #include -#include -#include #include -#include #include #include #include -#include -#include -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& interface_type : state_interface_types_) - { + for (const auto &joint_name: joint_names_) { + 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") - { - for (const auto& interface_type : odom_interface_types_) - { + if (estimator_type_ == "ground_truth") { + for (const auto &interface_type: odom_interface_types_) { conf.names.push_back(odom_name_ + "/" += interface_type); } - } - else if (estimator_type_ == "linear_kalman") - { - for (const auto& interface_type : foot_force_interface_types_) - { + } else if (estimator_type_ == "linear_kalman") { + 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, - const rclcpp::Duration& period) - { - // State Estimate - updateStateEstimation(period); - - // Compute target trajectory - ctrl_comp_.target_manager_->update(); - - // Update the current state of the system - 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); + controller_interface::return_type Ocs2QuadrupedController::update(const rclcpp::Time &time, + const rclcpp::Duration &period) { + if (mode_ == FSMMode::NORMAL) { + current_state_->run(time, period); + next_state_name_ = current_state_->checkChange(); + if (next_state_name_ != current_state_->state_name) { + mode_ = FSMMode::CHANGE; + next_state_ = getNextState(next_state_name_); + RCLCPP_INFO(get_node()->get_logger(), "Switched from %s to %s", + current_state_->state_name_string.c_str(), next_state_->state_name_string.c_str()); } - 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("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"; - 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"; - - 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_); + get_node()->get_parameter("update_rate", ctrl_interfaces_.frequency_); + RCLCPP_INFO(get_node()->get_logger(), "Controller Manager Update Rate: %d Hz", ctrl_interfaces_.frequency_); // Hardware Parameters command_prefix_ = auto_declare("command_prefix", command_prefix_); - joint_names_ = auto_declare>("joints", joint_names_); - feet_names_ = auto_declare>("feet", feet_names_); + joint_names_ = auto_declare >("joints", joint_names_); + feet_names_ = auto_declare >("feet", feet_names_); command_interface_types_ = - auto_declare>("command_interfaces", command_interface_types_); + auto_declare >("command_interfaces", command_interface_types_); state_interface_types_ = - auto_declare>("state_interfaces", state_interface_types_); + auto_declare >("state_interfaces", state_interface_types_); // IMU Sensor imu_name_ = auto_declare("imu_name", imu_name_); - imu_interface_types_ = auto_declare>("imu_interfaces", state_interface_types_); + imu_interface_types_ = auto_declare >("imu_interfaces", state_interface_types_); // Odometer Sensor (Ground Truth) estimator_type_ = auto_declare("estimator_type", estimator_type_); - if (estimator_type_ == "ground_truth") - { + if (estimator_type_ == "ground_truth") { odom_name_ = auto_declare("odom_name", odom_name_); - odom_interface_types_ = auto_declare>("odom_interfaces", state_interface_types_); - } - else - { + odom_interface_types_ = auto_declare >("odom_interfaces", state_interface_types_); + } else { // Foot Force Sensor foot_force_name_ = auto_declare("foot_force_name", foot_force_name_); foot_force_interface_types_ = - auto_declare>("foot_force_interfaces", state_interface_types_); + auto_declare >("foot_force_interfaces", state_interface_types_); } // PD gains default_kp_ = auto_declare("default_kp", default_kp_); default_kd_ = auto_declare("default_kd", default_kd_); - setupLeggedInterface(); - setupMpc(); - setupMrt(); - - // Visualization - CentroidalModelPinocchioMapping pinocchio_mapping(legged_interface_->getCentroidalModelInfo()); - eeKinematicsPtr_ = std::make_shared( - legged_interface_->getPinocchioInterface(), pinocchio_mapping, - legged_interface_->modelSettings().contactNames3DoF); - - ctrl_comp_.visualizer_ = std::make_shared( - 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(legged_interface_->getPinocchioInterface(), - legged_interface_->getCentroidalModelInfo(), - *eeKinematicsPtr_); - wbc_->loadTasksSetting(task_file_, verbose_); - - // Safety Checker - safety_checker_ = std::make_shared(legged_interface_->getCentroidalModelInfo()); + state_list_.passive = std::make_shared(ctrl_interfaces_); + state_list_.fixedDown = std::make_shared(ctrl_interfaces_, get_node(), + package_share_directory, joint_names_, feet_names_, + default_kp_, default_kd_); + state_list_.fixedDown->setupStateEstimate(estimator_type_); 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", 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_comp_.control_inputs_.lx = msg->lx; - ctrl_comp_.control_inputs_.ly = msg->ly; - ctrl_comp_.control_inputs_.rx = msg->rx; - ctrl_comp_.control_inputs_.ry = msg->ry; + ctrl_interfaces_.control_inputs_.command = msg->command; + ctrl_interfaces_.control_inputs_.lx = msg->lx; + ctrl_interfaces_.control_inputs_.ly = msg->ly; + ctrl_interfaces_.control_inputs_.rx = msg->rx; + ctrl_interfaces_.control_inputs_.ry = msg->ry; }); - observation_publisher_ = get_node()->create_publisher( - "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_) - { - if (interface.get_prefix_name() == imu_name_) - { - ctrl_comp_.imu_state_interface_.emplace_back(interface); - } - else if (interface.get_prefix_name() == foot_force_name_) - { - 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 - { + for (auto &interface: state_interfaces_) { + 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_interfaces_.foot_force_state_interface_.emplace_back(interface); + } else if (interface.get_prefix_name() == odom_name_) { + ctrl_interfaces_.odom_state_interface_.emplace_back(interface); + } else { state_interface_map_[interface.get_interface_name()]->push_back(interface); } } - if (mpc_running_ == false) - { - // Initial state - ctrl_comp_.observation_.state.setZero( - static_cast(legged_interface_->getCentroidalModelInfo().stateDim)); - updateStateEstimation(rclcpp::Duration(0, 1 / ctrl_comp_.frequency_ * 1000000000)); - ctrl_comp_.observation_.input.setZero( - static_cast(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; - } + current_state_ = state_list_.passive; + current_state_->enter(); + next_state_ = current_state_; + next_state_name_ = current_state_->state_name; + mode_ = FSMMode::NORMAL; 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() - { - legged_interface_ = std::make_shared(task_file_, urdf_file_, reference_file_); - legged_interface_->setupJointNames(joint_names_, feet_names_); - legged_interface_->setupOptimalControlProblem(task_file_, urdf_file_, reference_file_, verbose_); - } - - void Ocs2QuadrupedController::setupMpc() - { - mpc_ = std::make_shared(legged_interface_->mpcSettings(), legged_interface_->sqpSettings(), - legged_interface_->getOptimalControlProblem(), - legged_interface_->getInitializer()); - rbd_conversions_ = std::make_shared(legged_interface_->getPinocchioInterface(), - legged_interface_->getCentroidalModelInfo()); - - // Initialize the reference manager - const auto gait_manager_ptr = std::make_shared( - 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(ctrl_comp_, - legged_interface_->getReferenceManagerPtr(), - task_file_, reference_file_); - } - - void Ocs2QuadrupedController::setupMrt() - { - mpc_mrt_interface_ = std::make_shared(*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(legged_interface_->getCentroidalModelInfo(), - ctrl_comp_, - get_node()); - RCLCPP_INFO(get_node()->get_logger(), "Using Ground Truth Estimator"); + std::shared_ptr Ocs2QuadrupedController::getNextState(FSMStateName stateName) const { + switch (stateName) { + case FSMStateName::PASSIVE: + return state_list_.passive; + case FSMStateName::FIXEDDOWN: + return state_list_.fixedDown; + default: + return state_list_.invalid; } - else if (estimator_type_ == "linear_kalman") - { - ctrl_comp_.estimator_ = std::make_shared(legged_interface_->getPinocchioInterface(), - legged_interface_->getCentroidalModelInfo(), - *eeKinematicsPtr_, ctrl_comp_, - get_node()); - dynamic_cast(*ctrl_comp_.estimator_).loadSettings(task_file_, verbose_); - RCLCPP_INFO(get_node()->get_logger(), "Using Kalman Filter Estimator"); - } - else - { - ctrl_comp_.estimator_ = std::make_shared(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(); } } diff --git a/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.h b/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.h index 50ec691..4908101 100644 --- a/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.h +++ b/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.h @@ -5,18 +5,22 @@ #ifndef OCS2QUADRUPEDCONTROLLER_H #define OCS2QUADRUPEDCONTROLLER_H +#include #include #include -#include #include #include #include #include -#include -#include "SafetyChecker.h" -#include "ocs2_quadruped_controller/control/CtrlComponent.h" +#include namespace ocs2::legged_robot { + struct FSMStateList { + std::shared_ptr invalid; + std::shared_ptr passive; + std::shared_ptr 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 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 current_state_; + std::shared_ptr next_state_; - void setupStateEstimate(); - - void updateStateEstimation(const rclcpp::Duration &period); - - CtrlComponent ctrl_comp_; + CtrlInterfaces ctrl_interfaces_; std::vector joint_names_; std::vector feet_names_; std::vector command_interface_types_; @@ -70,18 +74,18 @@ namespace ocs2::legged_robot { std::unordered_map< std::string, std::vector > *> command_interface_map_ = { - {"effort", &ctrl_comp_.joint_torque_command_interface_}, - {"position", &ctrl_comp_.joint_position_command_interface_}, - {"velocity", &ctrl_comp_.joint_velocity_command_interface_}, - {"kp", &ctrl_comp_.joint_kp_command_interface_}, - {"kd", &ctrl_comp_.joint_kd_command_interface_} + {"effort", &ctrl_interfaces_.joint_torque_command_interface_}, + {"position", &ctrl_interfaces_.joint_position_command_interface_}, + {"velocity", &ctrl_interfaces_.joint_velocity_command_interface_}, + {"kp", &ctrl_interfaces_.joint_kp_command_interface_}, + {"kd", &ctrl_interfaces_.joint_kd_command_interface_} }; std::unordered_map< std::string, std::vector > *> state_interface_map_ = { - {"position", &ctrl_comp_.joint_position_state_interface_}, - {"effort", &ctrl_comp_.joint_effort_state_interface_}, - {"velocity", &ctrl_comp_.joint_velocity_state_interface_} + {"position", &ctrl_interfaces_.joint_position_state_interface_}, + {"effort", &ctrl_interfaces_.joint_effort_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::SharedPtr control_input_subscription_; - rclcpp::Publisher::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 legged_interface_; - std::shared_ptr eeKinematicsPtr_; - - // Whole Body Control - std::shared_ptr wbc_; - std::shared_ptr safety_checker_; - - // Nonlinear MPC - std::shared_ptr mpc_; - std::shared_ptr mpc_mrt_interface_; - - std::shared_ptr 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_; }; } diff --git a/controllers/ocs2_quadruped_controller/src/control/GaitManager.cpp b/controllers/ocs2_quadruped_controller/src/control/GaitManager.cpp index 1ab2a68..d160d1b 100644 --- a/controllers/ocs2_quadruped_controller/src/control/GaitManager.cpp +++ b/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 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_component_.control_inputs_.command == last_command_) return; - last_command_ = ctrl_component_.control_inputs_.command; - target_gait_ = gait_list_[ctrl_component_.control_inputs_.command - 1]; + if (ctrl_interfaces_.control_inputs_.command == 0) return; + if (ctrl_interfaces_.control_inputs_.command == last_command_) return; + last_command_ = ctrl_interfaces_.control_inputs_.command; + 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; } } diff --git a/controllers/ocs2_quadruped_controller/src/control/TargetManager.cpp b/controllers/ocs2_quadruped_controller/src/control/TargetManager.cpp index 9ca460d..4aae877 100644 --- a/controllers/ocs2_quadruped_controller/src/control/TargetManager.cpp +++ b/controllers/ocs2_quadruped_controller/src/control/TargetManager.cpp @@ -7,11 +7,9 @@ #include #include -#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& 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 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; diff --git a/controllers/ocs2_quadruped_controller/src/estimator/FromOdomTopic.cpp b/controllers/ocs2_quadruped_controller/src/estimator/FromOdomTopic.cpp index 918c494..f1b97f4 100644 --- a/controllers/ocs2_quadruped_controller/src/estimator/FromOdomTopic.cpp +++ b/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) { diff --git a/controllers/ocs2_quadruped_controller/src/estimator/GroundTruth.cpp b/controllers/ocs2_quadruped_controller/src/estimator/GroundTruth.cpp index f1b20d6..361591f 100644 --- a/controllers/ocs2_quadruped_controller/src/estimator/GroundTruth.cpp +++ b/controllers/ocs2_quadruped_controller/src/estimator/GroundTruth.cpp @@ -4,11 +4,9 @@ #include "ocs2_quadruped_controller/estimator/GroundTruth.h" -#include - 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, diff --git a/controllers/ocs2_quadruped_controller/src/estimator/LinearKalmanFilter.cpp b/controllers/ocs2_quadruped_controller/src/estimator/LinearKalmanFilter.cpp index 3418341..5b582ea 100644 --- a/controllers/ocs2_quadruped_controller/src/estimator/LinearKalmanFilter.cpp +++ b/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), diff --git a/controllers/ocs2_quadruped_controller/src/estimator/StateEstimateBase.cpp b/controllers/ocs2_quadruped_controller/src/estimator/StateEstimateBase.cpp index e0cc12b..1ac80fc 100644 --- a/controllers/ocs2_quadruped_controller/src/estimator/StateEstimateBase.cpp +++ b/controllers/ocs2_quadruped_controller/src/estimator/StateEstimateBase.cpp @@ -9,11 +9,10 @@ #include #include -#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)), diff --git a/descriptions/unitree/go2_description/config/robot_control.yaml b/descriptions/unitree/go2_description/config/robot_control.yaml index 61058c1..410b45a 100644 --- a/descriptions/unitree/go2_description/config/robot_control.yaml +++ b/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 diff --git a/libraries/controller_common/include/controller_common/CtrlInterfaces.h b/libraries/controller_common/include/controller_common/CtrlInterfaces.h index 268c9ea..9969aae 100644 --- a/libraries/controller_common/include/controller_common/CtrlInterfaces.h +++ b/libraries/controller_common/include/controller_common/CtrlInterfaces.h @@ -33,6 +33,13 @@ struct CtrlInterfaces { std::vector > imu_state_interface_; + std::vector > + foot_force_state_interface_; + + std::vector > + 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(); } }; diff --git a/libraries/controller_common/include/controller_common/FSM/BaseFixedStand.h b/libraries/controller_common/include/controller_common/FSM/BaseFixedStand.h index 911403f..1c462c7 100644 --- a/libraries/controller_common/include/controller_common/FSM/BaseFixedStand.h +++ b/libraries/controller_common/include/controller_common/FSM/BaseFixedStand.h @@ -5,8 +5,6 @@ #ifndef BASEFIXEDSTAND_H #define BASEFIXEDSTAND_H -#include - #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; diff --git a/libraries/controller_common/include/controller_common/FSM/FSMState.h b/libraries/controller_common/include/controller_common/FSM/FSMState.h index 9804ea8..d195fb1 100644 --- a/libraries/controller_common/include/controller_common/FSM/FSMState.h +++ b/libraries/controller_common/include/controller_common/FSM/FSMState.h @@ -9,6 +9,7 @@ #include #include #include +#include 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; diff --git a/libraries/controller_common/include/controller_common/FSM/StateFixedDown.h b/libraries/controller_common/include/controller_common/FSM/StateFixedDown.h index 4b0d136..a867851 100644 --- a/libraries/controller_common/include/controller_common/FSM/StateFixedDown.h +++ b/libraries/controller_common/include/controller_common/FSM/StateFixedDown.h @@ -5,8 +5,6 @@ #ifndef STATEFIXEDDOWN_H #define STATEFIXEDDOWN_H -#include - #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; diff --git a/libraries/controller_common/include/controller_common/FSM/StatePassive.h b/libraries/controller_common/include/controller_common/FSM/StatePassive.h index 744ac5e..2477075 100644 --- a/libraries/controller_common/include/controller_common/FSM/StatePassive.h +++ b/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; diff --git a/libraries/controller_common/src/FSM/BaseFixedStand.cpp b/libraries/controller_common/src/FSM/BaseFixedStand.cpp index 238830f..5f77d46 100644 --- a/libraries/controller_common/src/FSM/BaseFixedStand.cpp +++ b/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++) { diff --git a/libraries/controller_common/src/FSM/StateFixedDown.cpp b/libraries/controller_common/src/FSM/StateFixedDown.cpp index 631e4bd..abdfde6 100644 --- a/libraries/controller_common/src/FSM/StateFixedDown.cpp +++ b/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++) { diff --git a/libraries/controller_common/src/FSM/StatePassive.cpp b/libraries/controller_common/src/FSM/StatePassive.cpp index 964afe1..a1be9e6 100644 --- a/libraries/controller_common/src/FSM/StatePassive.cpp +++ b/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() {