diff --git a/controllers/ocs2_quadruped_controller/CMakeLists.txt b/controllers/ocs2_quadruped_controller/CMakeLists.txt index be07f08..3798da1 100644 --- a/controllers/ocs2_quadruped_controller/CMakeLists.txt +++ b/controllers/ocs2_quadruped_controller/CMakeLists.txt @@ -51,6 +51,8 @@ add_library(${PROJECT_NAME} SHARED src/interface/LeggedRobotPreComputation.cpp src/interface/LeggedInterface.cpp + src/control/GaitManager.cpp + ) target_include_directories(${PROJECT_NAME} PUBLIC 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 new file mode 100644 index 0000000..6ab4703 --- /dev/null +++ b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/control/GaitManager.h @@ -0,0 +1,42 @@ +// +// Created by tlab-uav on 24-9-26. +// + +#ifndef GAITMANAGER_H +#define GAITMANAGER_H +#include +#include + +#include "CtrlComponent.h" + +namespace ocs2::legged_robot { + class GaitManager final : public SolverSynchronizedModule { + public: + GaitManager(CtrlComponent &ctrl_component, + std::shared_ptr gait_schedule_ptr); + + void preSolverRun(scalar_t initTime, scalar_t finalTime, + const vector_t ¤tState, + const ReferenceManagerInterface &referenceManager) override; + + void postSolverRun(const PrimalSolution &primalSolution) override { + } + + void init(const std::string &gait_file); + + private: + void getTargetGait(); + + CtrlComponent &ctrl_component_; + std::shared_ptr gait_schedule_ptr_; + + ModeSequenceTemplate target_gait_; + bool gait_updated_{false}; + bool verbose_{false}; + std::vector gait_list_; + std::vector gait_name_list_; + }; +} + + +#endif //GAITMANAGER_H diff --git a/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.cpp b/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.cpp index c04c235..889687e 100644 --- a/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.cpp +++ b/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.cpp @@ -13,6 +13,7 @@ #include #include #include +#include namespace ocs2::legged_robot { using config_type = controller_interface::interface_configuration_type; @@ -53,13 +54,9 @@ namespace ocs2::legged_robot { controller_interface::return_type Ocs2QuadrupedController::update(const rclcpp::Time &time, const rclcpp::Duration &period) { + // State Estimate - const vector_t rbd_state = ctrl_comp_.estimator_->update(time, period); - currentObservation_.time += period.seconds(); - const scalar_t yaw_last = currentObservation_.state(9); - currentObservation_.state = rbdConversions_->computeCentroidalStateFromRbdModel(rbd_state); - currentObservation_.state(9) = yaw_last + angles::shortest_angular_distance(yaw_last, currentObservation_.state(9)); - currentObservation_.mode = stateEstimate_->getMode(); + updateStateEstimation(time, period); // Update the current state of the system mpcMrtInterface_->setCurrentObservation(currentObservation_); @@ -69,20 +66,22 @@ namespace ocs2::legged_robot { // Evaluate the current policy vector_t optimizedState, optimizedInput; - size_t plannedMode = 0; // The mode that is active at the time the policy is evaluated at. - mpcMrtInterface_->evaluatePolicy(currentObservation_.time, currentObservation_.state, optimizedState, optimizedInput, plannedMode); + size_t plannedMode = 0; // The mode that is active at the time the policy is evaluated at. + mpcMrtInterface_->evaluatePolicy(currentObservation_.time, currentObservation_.state, optimizedState, + optimizedInput, plannedMode); // Whole body control currentObservation_.input = optimizedInput; wbcTimer_.startTimer(); - vector_t x = wbc_->update(optimizedState, optimizedInput, rbd_state, plannedMode, period.seconds()); + vector_t x = wbc_->update(optimizedState, optimizedInput, measuredRbdState_, plannedMode, period.seconds()); wbcTimer_.endTimer(); vector_t torque = x.tail(12); vector_t posDes = centroidal_model::getJointAngles(optimizedState, legged_interface_->getCentroidalModelInfo()); - vector_t velDes = centroidal_model::getJointVelocities(optimizedInput, legged_interface_->getCentroidalModelInfo()); + vector_t velDes = centroidal_model::getJointVelocities(optimizedInput, + legged_interface_->getCentroidalModelInfo()); // Safety check, if failed, stop the controller if (!safetyChecker_->check(currentObservation_, optimizedState, optimizedInput)) { @@ -105,6 +104,7 @@ namespace ocs2::legged_robot { urdf_file_ = auto_declare("urdf_file", urdf_file_); task_file_ = auto_declare("task_file", task_file_); reference_file_ = auto_declare("reference_file", reference_file_); + gait_file_ = auto_declare("gait_file", gait_file_); verbose_ = false; loadData::loadCppDataType(task_file_, "legged_robot_interface.verbose", verbose_); @@ -194,6 +194,30 @@ namespace ocs2::legged_robot { } } + if (mpcRunning_ == false) { + // Initial state + currentObservation_.state.setZero(static_cast(legged_interface_->getCentroidalModelInfo().stateDim)); + updateStateEstimation(get_node()->now(), rclcpp::Duration(0, 200000)); + currentObservation_.input.setZero(static_cast(legged_interface_->getCentroidalModelInfo().inputDim)); + currentObservation_.mode = STANCE; + + TargetTrajectories target_trajectories({currentObservation_.time}, {currentObservation_.state}, + {currentObservation_.input}); + + // Set the first observation and command and wait for optimization to finish + mpcMrtInterface_->setCurrentObservation(currentObservation_); + mpcMrtInterface_->getReferenceManager().setTargetTrajectories(target_trajectories); + RCLCPP_INFO(get_node()->get_logger(), "Waiting for the initial policy ..."); + while (!mpcMrtInterface_->initialPolicyReceived()) { + std::cout<mpcSettings().mrtDesiredFrequency_<advanceMpc(); + rclcpp::WallRate(legged_interface_->mpcSettings().mrtDesiredFrequency_).sleep(); + } + RCLCPP_INFO(get_node()->get_logger(), "Initial policy has been received."); + + mpcRunning_ = true; + } + return CallbackReturn::SUCCESS; } @@ -234,16 +258,16 @@ namespace ocs2::legged_robot { // Todo Handle Gait Receive. // Gait receiver - // auto gaitReceiverPtr = - // std::make_shared(this->get_node(), - // legged_interface_->getSwitchedModelReferenceManagerPtr()-> - // getGaitSchedule(), robotName); + const auto gait_manager_ptr = std::make_shared( + ctrl_comp_, legged_interface_->getSwitchedModelReferenceManagerPtr()-> + getGaitSchedule()); + gait_manager_ptr->init(gait_file_); // ROS ReferenceManager // auto rosReferenceManagerPtr = std::make_shared( // robotName, legged_interface_->getReferenceManagerPtr()); // rosReferenceManagerPtr->subscribe(this->get_node()); - // mpc_->getSolverPtr()->addSynchronizedModule(gaitReceiverPtr); + // mpc_->getSolverPtr()->addSynchronizedModule(gait_manager_ptr); // mpc_->getSolverPtr()->setReferenceManager(rosReferenceManagerPtr); // observationPublisher_ = nh.advertise(robotName + "_mpc_observation", 1); } @@ -282,7 +306,18 @@ namespace ocs2::legged_robot { dynamic_cast(*ctrl_comp_.estimator_).loadSettings(task_file_, verbose_); currentObservation_.time = 0; } + + void Ocs2QuadrupedController::updateStateEstimation(const rclcpp::Time &time, const rclcpp::Duration &period) { + measuredRbdState_ = ctrl_comp_.estimator_->update(time, period); + currentObservation_.time += period.seconds(); + const scalar_t yaw_last = currentObservation_.state(9); + currentObservation_.state = rbdConversions_->computeCentroidalStateFromRbdModel(measuredRbdState_); + currentObservation_.state(9) = yaw_last + angles::shortest_angular_distance( + yaw_last, currentObservation_.state(9)); + std::cout << ctrl_comp_.estimator_->getMode() << std::endl; + currentObservation_.mode = ctrl_comp_.estimator_->getMode(); + } } #include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS(ocs2::legged_robot::Ocs2QuadrupedController, controller_interface::ControllerInterface); \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(ocs2::legged_robot::Ocs2QuadrupedController, controller_interface::ControllerInterface); diff --git a/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.h b/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.h index 0ff4a67..eab2c92 100644 --- a/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.h +++ b/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.h @@ -67,6 +67,9 @@ namespace ocs2::legged_robot { void setupStateEstimate(); + void updateStateEstimation(const rclcpp::Time &time, + const rclcpp::Duration &period); + CtrlComponent ctrl_comp_; std::vector joint_names_; std::vector command_interface_types_; @@ -104,6 +107,7 @@ namespace ocs2::legged_robot { std::string task_file_; std::string urdf_file_; std::string reference_file_; + std::string gait_file_; bool verbose_; @@ -119,9 +123,9 @@ namespace ocs2::legged_robot { std::shared_ptr mpcMrtInterface_; std::shared_ptr rbdConversions_; - std::shared_ptr stateEstimate_; private: + vector_t measuredRbdState_; std::thread mpcThread_; std::atomic_bool controllerRunning_{}, mpcRunning_{}; benchmark::RepeatedTimer mpcTimer_; diff --git a/controllers/ocs2_quadruped_controller/src/control/GaitManager.cpp b/controllers/ocs2_quadruped_controller/src/control/GaitManager.cpp new file mode 100644 index 0000000..b5e49eb --- /dev/null +++ b/controllers/ocs2_quadruped_controller/src/control/GaitManager.cpp @@ -0,0 +1,51 @@ +// +// Created by tlab-uav on 24-9-26. +// + +#include + +#include "ocs2_quadruped_controller/control/GaitManager.h" + +#include + +namespace ocs2::legged_robot { + GaitManager::GaitManager(CtrlComponent &ctrl_component, + std::shared_ptr gait_schedule_ptr) + : ctrl_component_(ctrl_component), + gait_schedule_ptr_(std::move(gait_schedule_ptr)), + target_gait_({0.0, 1.0}, {STANCE}) { + } + + void GaitManager::preSolverRun(const scalar_t initTime, const scalar_t finalTime, + const vector_t ¤tState, + const ReferenceManagerInterface &referenceManager) { + getTargetGait(); + if (gait_updated_) { + const auto timeHorizon = finalTime - initTime; + gait_schedule_ptr_->insertModeSequenceTemplate(target_gait_, finalTime, + timeHorizon); + gait_updated_ = false; + } + } + + void GaitManager::init(const std::string &gait_file) { + gait_name_list_.clear(); + loadData::loadStdVector(gait_file, "list", gait_name_list_, verbose_); + + gait_list_.clear(); + for (const auto &name: gait_name_list_) { + gait_list_.push_back(loadModeSequenceTemplate(gait_file, name, verbose_)); + } + + RCLCPP_INFO(rclcpp::get_logger("gait_manager"), "GaitManager is ready."); + } + + void GaitManager::getTargetGait() { + std::cout << "ctrl_component_.control_inputs_.command: " << ctrl_component_.control_inputs_.command << std::endl; + if (ctrl_component_.control_inputs_.command == 0) return; + target_gait_ = gait_list_[ctrl_component_.control_inputs_.command - 1]; + RCLCPP_INFO(rclcpp::get_logger("GaitManager"), "Switch to gait: %s", + gait_name_list_[ctrl_component_.control_inputs_.command - 1].c_str()); + gait_updated_ = true; + } +} diff --git a/controllers/ocs2_quadruped_controller/src/estimator/StateEstimateBase.cpp b/controllers/ocs2_quadruped_controller/src/estimator/StateEstimateBase.cpp index 83ace89..62ac0f0 100644 --- a/controllers/ocs2_quadruped_controller/src/estimator/StateEstimateBase.cpp +++ b/controllers/ocs2_quadruped_controller/src/estimator/StateEstimateBase.cpp @@ -12,7 +12,6 @@ #include "ocs2_quadruped_controller/control/CtrlComponent.h" namespace ocs2::legged_robot { - using namespace legged_robot; StateEstimateBase::StateEstimateBase(PinocchioInterface pinocchioInterface, CentroidalModelInfo info, const PinocchioEndEffectorKinematics &eeKinematics, diff --git a/descriptions/go2_description/config/ocs2/gait.info b/descriptions/go2_description/config/ocs2/gait.info new file mode 100644 index 0000000..6ea163b --- /dev/null +++ b/descriptions/go2_description/config/ocs2/gait.info @@ -0,0 +1,255 @@ +list +{ + [0] stance + [1] trot + [2] standing_trot + [3] flying_trot + [4] pace + [5] standing_pace + [6] dynamic_walk + [7] static_walk + [8] amble + [9] lindyhop + [10] skipping + [11] pawup +} + +stance + { + modeSequence + { + [0] STANCE + } + switchingTimes + { + [0] 0.0 + [1] 0.5 + } +} + +trot + { + modeSequence + { + [0] LF_RH + [1] RF_LH + } + switchingTimes + { + [0] 0.0 + [1] 0.3 + [2] 0.6 + } +} + +standing_trot +{ + modeSequence + { + [0] LF_RH + [1] STANCE + [2] RF_LH + [3] STANCE + } + switchingTimes + { + [0] 0.00 + [1] 0.25 + [2] 0.3 + [3] 0.55 + [4] 0.6 + } +} + +flying_trot +{ + modeSequence + { + [0] LF_RH + [1] FLY + [2] RF_LH + [3] FLY + } + switchingTimes + { + [0] 0.00 + [1] 0.15 + [2] 0.2 + [3] 0.35 + [4] 0.4 + } +} + +pace +{ + modeSequence + { + [0] LF_LH + [1] FLY + [2] RF_RH + [3] FLY + } + switchingTimes + { + [0] 0.0 + [1] 0.28 + [2] 0.30 + [3] 0.58 + [4] 0.60 + } +} + +standing_pace +{ + modeSequence + { + [0] LF_LH + [1] STANCE + [2] RF_RH + [3] STANCE + } + switchingTimes + { + [0] 0.0 + [1] 0.30 + [2] 0.35 + [3] 0.65 + [4] 0.70 + } +} + +dynamic_walk +{ + modeSequence + { + [0] LF_RF_RH + [1] RF_RH + [2] RF_LH_RH + [3] LF_RF_LH + [4] LF_LH + [5] LF_LH_RH + } + switchingTimes + { + [0] 0.0 + [1] 0.2 + [2] 0.3 + [3] 0.5 + [4] 0.7 + [5] 0.8 + [6] 1.0 + } +} + +static_walk +{ + modeSequence + { + [0] LF_RF_RH + [1] RF_LH_RH + [2] LF_RF_LH + [3] LF_LH_RH + } + switchingTimes + { + [0] 0.0 + [1] 0.3 + [2] 0.6 + [3] 0.9 + [4] 1.2 + } +} + +amble +{ + modeSequence + { + [0] RF_LH + [1] LF_LH + [2] LF_RH + [3] RF_RH + } + switchingTimes + { + [0] 0.0 + [1] 0.15 + [2] 0.40 + [3] 0.55 + [4] 0.80 + } +} + +lindyhop +{ + modeSequence + { + [0] LF_RH + [1] STANCE + [2] RF_LH + [3] STANCE + [4] LF_LH + [5] RF_RH + [6] LF_LH + [7] STANCE + [8] RF_RH + [9] LF_LH + [10] RF_RH + [11] STANCE + } + switchingTimes + { + [0] 0.00 ; Step 1 + [1] 0.35 ; Stance + [2] 0.45 ; Step 2 + [3] 0.80 ; Stance + [4] 0.90 ; Tripple step + [5] 1.125 ; + [6] 1.35 ; + [7] 1.70 ; Stance + [8] 1.80 ; Tripple step + [9] 2.025 ; + [10] 2.25 ; + [11] 2.60 ; Stance + [12] 2.70 ; + } +} + +skipping +{ + modeSequence + { + [0] LF_RH + [1] FLY + [2] LF_RH + [3] FLY + [4] RF_LH + [5] FLY + [6] RF_LH + [7] FLY + } + switchingTimes + { + [0] 0.00 + [1] 0.27 + [2] 0.30 + [3] 0.57 + [4] 0.60 + [5] 0.87 + [6] 0.90 + [7] 1.17 + [8] 1.20 + } +} + +pawup +{ + modeSequence + { + [0] RF_LH_RH + } + switchingTimes + { + [0] 0.0 + [1] 2.0 + } +} diff --git a/descriptions/go2_description/launch/ocs2_control.launch.py b/descriptions/go2_description/launch/ocs2_control.launch.py index 4bcf455..43c3064 100644 --- a/descriptions/go2_description/launch/ocs2_control.launch.py +++ b/descriptions/go2_description/launch/ocs2_control.launch.py @@ -54,7 +54,9 @@ def launch_setup(context, *args, **kwargs): 'task_file': os.path.join(get_package_share_directory(package_description), 'config', 'ocs2', 'task.info'), 'reference_file': os.path.join(get_package_share_directory(package_description), 'config', - 'ocs2', 'reference.info') + 'ocs2', 'reference.info'), + 'gait_file': os.path.join(get_package_share_directory(package_description), 'config', + 'ocs2', 'gait.info') }], output="both", )