diff --git a/controllers/ocs2_quadruped_controller/CMakeLists.txt b/controllers/ocs2_quadruped_controller/CMakeLists.txt index 3798da1..d41b6aa 100644 --- a/controllers/ocs2_quadruped_controller/CMakeLists.txt +++ b/controllers/ocs2_quadruped_controller/CMakeLists.txt @@ -19,11 +19,11 @@ set(CONTROLLER_INCLUDE_DEPENDS control_input_msgs angles nav_msgs - qpOASES ) # find dependencies find_package(ament_cmake REQUIRED) +find_package(qpOASES REQUIRED) foreach (Dependency IN ITEMS ${CONTROLLER_INCLUDE_DEPENDS}) find_package(${Dependency} REQUIRED) endforeach () @@ -61,10 +61,12 @@ target_include_directories(${PROJECT_NAME} "$" PRIVATE src) -ament_target_dependencies( - ${PROJECT_NAME} PUBLIC +ament_target_dependencies(${PROJECT_NAME} ${CONTROLLER_INCLUDE_DEPENDS} ) +target_link_libraries(${PROJECT_NAME} + ${qpOASES_LIBRARY_DIR}/libqpOASES.a +) pluginlib_export_plugin_description_file(controller_interface ocs2_quadruped_controller.xml) diff --git a/controllers/ocs2_quadruped_controller/README.md b/controllers/ocs2_quadruped_controller/README.md index d3e9016..0cf8bc4 100644 --- a/controllers/ocs2_quadruped_controller/README.md +++ b/controllers/ocs2_quadruped_controller/README.md @@ -1,9 +1,38 @@ # OCS2 Quadruped Controller -This is a ros2-control controller based on [legged_control](https://github.com/qiayuanl/legged_control) and [ocs2_ros2](https://github.com/legubiao/ocs2_ros2). +This is a ros2-control controller based on [legged_control](https://github.com/qiayuanl/legged_control) +and [ocs2_ros2](https://github.com/legubiao/ocs2_ros2). ## 2. Build + +### 2.1 Build Dependencies + +* OCS2 ROS2 Libraries + ```bash + colcon build --packages-up-to ocs2_legged_robot_ros + colcon build --packages-up-to ocs2_self_collision + ``` +* qpOASES + ```bash + cd ~/ros2_ws/src/quadruped_ros2 + git submodule update --init --recursive + ``` + also add below line to the Cmakelist.txt of qpOASES + ```cmake + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fPIC") + ``` + +### 2.2 Build OCS2 Quadruped Controller + ```bash cd ~/ros2_ws colcon build --packages-up-to ocs2_quadruped_controller -``` \ No newline at end of file +``` + +## 3. Launch + +* Go2 Robot + ```bash + source ~/ros2_ws/install/setup.bash + ros2 launch go2_description ocs2_control.launch.py + ``` diff --git a/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.cpp b/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.cpp index 889687e..703b33a 100644 --- a/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.cpp +++ b/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.cpp @@ -54,28 +54,27 @@ namespace ocs2::legged_robot { controller_interface::return_type Ocs2QuadrupedController::update(const rclcpp::Time &time, const rclcpp::Duration &period) { - // State Estimate updateStateEstimation(time, period); // Update the current state of the system - mpcMrtInterface_->setCurrentObservation(currentObservation_); + mpc_mrt_interface_->setCurrentObservation(current_observation_); // Load the latest MPC policy - mpcMrtInterface_->updatePolicy(); + mpc_mrt_interface_->updatePolicy(); // 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, + mpc_mrt_interface_->evaluatePolicy(current_observation_.time, current_observation_.state, optimizedState, optimizedInput, plannedMode); // Whole body control - currentObservation_.input = optimizedInput; + current_observation_.input = optimizedInput; - wbcTimer_.startTimer(); + wbc_timer_.startTimer(); vector_t x = wbc_->update(optimizedState, optimizedInput, measuredRbdState_, plannedMode, period.seconds()); - wbcTimer_.endTimer(); + wbc_timer_.endTimer(); vector_t torque = x.tail(12); @@ -84,7 +83,7 @@ namespace ocs2::legged_robot { legged_interface_->getCentroidalModelInfo()); // Safety check, if failed, stop the controller - if (!safetyChecker_->check(currentObservation_, optimizedState, optimizedInput)) { + if (!safety_checker_->check(current_observation_, optimizedState, optimizedInput)) { RCLCPP_ERROR(get_node()->get_logger(), "[Legged Controller] Safety check failed, stopping the controller."); } @@ -100,6 +99,7 @@ namespace ocs2::legged_robot { } controller_interface::CallbackReturn Ocs2QuadrupedController::on_init() { + // Initialize OCS2 urdf_file_ = auto_declare("urdf_file", urdf_file_); task_file_ = auto_declare("task_file", task_file_); @@ -145,7 +145,7 @@ namespace ocs2::legged_robot { wbc_->loadTasksSetting(task_file_, verbose_); // Safety Checker - safetyChecker_ = std::make_shared(legged_interface_->getCentroidalModelInfo()); + safety_checker_ = std::make_shared(legged_interface_->getCentroidalModelInfo()); return CallbackReturn::SUCCESS; } @@ -194,28 +194,29 @@ namespace ocs2::legged_robot { } } - if (mpcRunning_ == false) { + if (mpc_running_ == false) { // Initial state - currentObservation_.state.setZero(static_cast(legged_interface_->getCentroidalModelInfo().stateDim)); + current_observation_.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; + current_observation_.input.setZero(static_cast(legged_interface_->getCentroidalModelInfo().inputDim)); + current_observation_.mode = STANCE; - TargetTrajectories target_trajectories({currentObservation_.time}, {currentObservation_.state}, - {currentObservation_.input}); + const TargetTrajectories target_trajectories({current_observation_.time}, {current_observation_.state}, + {current_observation_.input}); // Set the first observation and command and wait for optimization to finish - mpcMrtInterface_->setCurrentObservation(currentObservation_); - mpcMrtInterface_->getReferenceManager().setTargetTrajectories(target_trajectories); + mpc_mrt_interface_->setCurrentObservation(current_observation_); + mpc_mrt_interface_->getReferenceManager().setTargetTrajectories(target_trajectories); RCLCPP_INFO(get_node()->get_logger(), "Waiting for the initial policy ..."); - while (!mpcMrtInterface_->initialPolicyReceived()) { - std::cout<mpcSettings().mrtDesiredFrequency_<advanceMpc(); + while (!mpc_mrt_interface_->initialPolicyReceived()) { + std::cout<<"Waiting for the initial policy ..."<advanceMpc(); + std::cout<<"Advance MPC"<mpcSettings().mrtDesiredFrequency_).sleep(); } RCLCPP_INFO(get_node()->get_logger(), "Initial policy has been received."); - mpcRunning_ = true; + mpc_running_ = true; } return CallbackReturn::SUCCESS; @@ -251,7 +252,7 @@ namespace ocs2::legged_robot { mpc_ = std::make_shared(legged_interface_->mpcSettings(), legged_interface_->sqpSettings(), legged_interface_->getOptimalControlProblem(), legged_interface_->getInitializer()); - rbdConversions_ = std::make_shared(legged_interface_->getPinocchioInterface(), + rbd_conversions_ = std::make_shared(legged_interface_->getPinocchioInterface(), legged_interface_->getCentroidalModelInfo()); const std::string robotName = "legged_robot"; @@ -263,40 +264,42 @@ namespace ocs2::legged_robot { getGaitSchedule()); gait_manager_ptr->init(gait_file_); + // Todo Here maybe the reason of the nullPointer. // ROS ReferenceManager - // auto rosReferenceManagerPtr = std::make_shared( - // robotName, legged_interface_->getReferenceManagerPtr()); + const auto rosReferenceManagerPtr = std::make_shared( + robotName, legged_interface_->getReferenceManagerPtr()); // rosReferenceManagerPtr->subscribe(this->get_node()); - // mpc_->getSolverPtr()->addSynchronizedModule(gait_manager_ptr); - // mpc_->getSolverPtr()->setReferenceManager(rosReferenceManagerPtr); + mpc_->getSolverPtr()->addSynchronizedModule(gait_manager_ptr); + mpc_->getSolverPtr()->setReferenceManager(rosReferenceManagerPtr); // observationPublisher_ = nh.advertise(robotName + "_mpc_observation", 1); } void Ocs2QuadrupedController::setupMrt() { - mpcMrtInterface_ = std::make_shared(*mpc_); - mpcMrtInterface_->initRollout(&legged_interface_->getRollout()); - mpcTimer_.reset(); + mpc_mrt_interface_ = std::make_shared(*mpc_); + mpc_mrt_interface_->initRollout(&legged_interface_->getRollout()); + mpc_timer_.reset(); - controllerRunning_ = true; - mpcThread_ = std::thread([&] { - while (controllerRunning_) { + controller_running_ = true; + mpc_thread_ = std::thread([&] { + while (controller_running_) { try { executeAndSleep( [&] { - if (mpcRunning_) { - mpcTimer_.startTimer(); - mpcMrtInterface_->advanceMpc(); - mpcTimer_.endTimer(); + if (mpc_running_) { + mpc_timer_.startTimer(); + mpc_mrt_interface_->advanceMpc(); + mpc_timer_.endTimer(); } }, legged_interface_->mpcSettings().mpcDesiredFrequency_); } catch (const std::exception &e) { - controllerRunning_ = false; + controller_running_ = false; RCLCPP_WARN(get_node()->get_logger(), "[Ocs2 MPC thread] Error : %s", e.what()); } } }); - setThreadPriority(legged_interface_->sqpSettings().threadPriority, mpcThread_); + setThreadPriority(legged_interface_->sqpSettings().threadPriority, mpc_thread_); + RCLCPP_INFO(get_node()->get_logger(), "MRT initialized. MPC thread started."); } void Ocs2QuadrupedController::setupStateEstimate() { @@ -304,18 +307,17 @@ namespace ocs2::legged_robot { legged_interface_->getCentroidalModelInfo(), *eeKinematicsPtr_, ctrl_comp_, this->get_node()); dynamic_cast(*ctrl_comp_.estimator_).loadSettings(task_file_, verbose_); - currentObservation_.time = 0; + current_observation_.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(); + current_observation_.time += period.seconds(); + const scalar_t yaw_last = current_observation_.state(9); + current_observation_.state = rbd_conversions_->computeCentroidalStateFromRbdModel(measuredRbdState_); + current_observation_.state(9) = yaw_last + angles::shortest_angular_distance( + yaw_last, current_observation_.state(9)); + current_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 eab2c92..a73fe33 100644 --- a/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.h +++ b/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.h @@ -102,7 +102,7 @@ namespace ocs2::legged_robot { rclcpp::Subscription::SharedPtr control_input_subscription_; - SystemObservation currentObservation_; + SystemObservation current_observation_; std::string task_file_; std::string urdf_file_; @@ -116,20 +116,20 @@ namespace ocs2::legged_robot { // Whole Body Control std::shared_ptr wbc_; - std::shared_ptr safetyChecker_; + std::shared_ptr safety_checker_; // Nonlinear MPC std::shared_ptr mpc_; - std::shared_ptr mpcMrtInterface_; + std::shared_ptr mpc_mrt_interface_; - std::shared_ptr rbdConversions_; + std::shared_ptr rbd_conversions_; private: vector_t measuredRbdState_; - std::thread mpcThread_; - std::atomic_bool controllerRunning_{}, mpcRunning_{}; - benchmark::RepeatedTimer mpcTimer_; - benchmark::RepeatedTimer wbcTimer_; + std::thread mpc_thread_; + std::atomic_bool controller_running_{}, mpc_running_{}; + benchmark::RepeatedTimer mpc_timer_; + benchmark::RepeatedTimer wbc_timer_; }; }