From bfc5532ce3f6ad70517fbad26c1e8c1e8b4457c6 Mon Sep 17 00:00:00 2001 From: Huang Zhenbiao Date: Tue, 24 Sep 2024 21:50:46 +0800 Subject: [PATCH] ocs2 controller adding --- .../ocs2_quadruped_controller/CMakeLists.txt | 2 +- .../ocs2_quadruped_controller/README.md | 2 + .../estimator/LinearKalmanFilter.h | 7 +- .../estimator/StateEstimateBase.h | 6 +- .../src/Ocs2QuadrupedController.cpp | 106 ++++++++++++++++++ .../src/Ocs2QuadrupedController.h | 104 +++++++++++------ .../src/SafetyChecker.h | 34 ++++++ .../src/estimator/LinearKalmanFilter.cpp | 82 ++------------ .../src/estimator/StateEstimateBase.cpp | 2 +- .../go1_description/xacro/ros2_control.xacro | 7 ++ hardwares/hardware_unitree_mujoco/README.md | 2 +- 11 files changed, 238 insertions(+), 116 deletions(-) create mode 100644 controllers/ocs2_quadruped_controller/src/SafetyChecker.h diff --git a/controllers/ocs2_quadruped_controller/CMakeLists.txt b/controllers/ocs2_quadruped_controller/CMakeLists.txt index 07e3409..a207689 100644 --- a/controllers/ocs2_quadruped_controller/CMakeLists.txt +++ b/controllers/ocs2_quadruped_controller/CMakeLists.txt @@ -31,7 +31,7 @@ add_library(${PROJECT_NAME} SHARED src/Ocs2QuadrupedController.cpp # src/estimator/FromTopicEstimate.cpp - # src/estimator/LinearKalmanFilter.cpp + src/estimator/LinearKalmanFilter.cpp src/estimator/StateEstimateBase.cpp src/wbc/HierarchicalWbc.cpp diff --git a/controllers/ocs2_quadruped_controller/README.md b/controllers/ocs2_quadruped_controller/README.md index 7ed237e..d3e9016 100644 --- a/controllers/ocs2_quadruped_controller/README.md +++ b/controllers/ocs2_quadruped_controller/README.md @@ -1,5 +1,7 @@ # 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). + ## 2. Build ```bash cd ~/ros2_ws 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 664bcb0..60576c6 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 @@ -18,7 +18,8 @@ namespace ocs2::legged_robot { class KalmanFilterEstimate final : public StateEstimateBase { public: KalmanFilterEstimate(PinocchioInterface pinocchioInterface, CentroidalModelInfo info, - const PinocchioEndEffectorKinematics &eeKinematics); + const PinocchioEndEffectorKinematics &eeKinematics, + const rclcpp_lifecycle::LifecycleNode::SharedPtr &node); vector_t update(const rclcpp::Time &time, const rclcpp::Duration &period) override; @@ -49,10 +50,6 @@ namespace ocs2::legged_robot { vector_t xHat_, ps_, vs_; // Topic - ros::Subscriber sub_; - realtime_tools::RealtimeBuffer buffer_; - tf2_ros::Buffer tfBuffer_; - tf2_ros::TransformListener tfListener_; tf2::Transform world2odom_; std::string frameOdom_, frameGuess_; bool topicUpdated_; 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 f5c34b0..3b1cbe3 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 @@ -14,6 +14,7 @@ #include #include #include +#include namespace ocs2::legged_robot { class StateEstimateBase { @@ -21,7 +22,8 @@ namespace ocs2::legged_robot { virtual ~StateEstimateBase() = default; StateEstimateBase(PinocchioInterface pinocchioInterface, CentroidalModelInfo info, - const PinocchioEndEffectorKinematics &eeKinematics, rclcpp::Node::SharedPtr node); + const PinocchioEndEffectorKinematics &eeKinematics, + rclcpp_lifecycle::LifecycleNode::SharedPtr node); virtual void updateJointStates(const vector_t &jointPos, const vector_t &jointVel); @@ -58,7 +60,7 @@ namespace ocs2::legged_robot { rclcpp::Publisher::SharedPtr posePub_; rclcpp::Time lastPub_; - rclcpp::Node::SharedPtr node_; + rclcpp_lifecycle::LifecycleNode::SharedPtr node_; }; template diff --git a/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.cpp b/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.cpp index 0ca7f8c..98763df 100644 --- a/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.cpp +++ b/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.cpp @@ -3,3 +3,109 @@ // #include "Ocs2QuadrupedController.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace ocs2::legged_robot { + controller_interface::CallbackReturn Ocs2QuadrupedController::on_init() { + verbose_ = false; + loadData::loadCppDataType(task_file_, "legged_robot_interface.verbose", verbose_); + + setUpLeggedInterface(); + setupMpc(); + setupMrt(); + + // Visualization + CentroidalModelPinocchioMapping pinocchioMapping(legged_interface_->getCentroidalModelInfo()); + eeKinematicsPtr_ = std::make_shared( + legged_interface_->getPinocchioInterface(), pinocchioMapping, + legged_interface_->modelSettings().contactNames3DoF); + // robotVisualizer_ = std::make_shared(leggedInterface_->getPinocchioInterface(), + // leggedInterface_->getCentroidalModelInfo(), *eeKinematicsPtr_, nh); + // 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 + safetyChecker_ = std::make_shared(legged_interface_->getCentroidalModelInfo()); + + return CallbackReturn::SUCCESS; + } + + void Ocs2QuadrupedController::setUpLeggedInterface() { + legged_interface_ = std::make_shared(task_file_, urdf_file_, reference_file_); + 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()); + rbdConversions_ = std::make_shared(legged_interface_->getPinocchioInterface(), + legged_interface_->getCentroidalModelInfo()); + + const std::string robotName = "legged_robot"; + // Gait receiver + auto gaitReceiverPtr = + std::make_shared(this->get_node(), + legged_interface_->getSwitchedModelReferenceManagerPtr()-> + getGaitSchedule(), robotName); + // ROS ReferenceManager + auto rosReferenceManagerPtr = std::make_shared( + robotName, legged_interface_->getReferenceManagerPtr()); + rosReferenceManagerPtr->subscribe(this->get_node()); + mpc_->getSolverPtr()->addSynchronizedModule(gaitReceiverPtr); + 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(); + + controllerRunning_ = true; + mpcThread_ = std::thread([&]() { + while (controllerRunning_) { + try { + executeAndSleep( + [&]() { + if (mpcRunning_) { + mpcTimer_.startTimer(); + mpcMrtInterface_->advanceMpc(); + mpcTimer_.endTimer(); + } + }, + legged_interface_->mpcSettings().mpcDesiredFrequency_); + } catch (const std::exception &e) { + controllerRunning_ = false; + RCLCPP_WARN(get_node()->get_logger(), "[Ocs2 MPC thread] Error : %s", e.what()); + } + } + }); + setThreadPriority(legged_interface_->sqpSettings().threadPriority, mpcThread_); + } + + void Ocs2QuadrupedController::setupStateEstimate() { + stateEstimate_ = std::make_shared(legged_interface_->getPinocchioInterface(), + legged_interface_->getCentroidalModelInfo(), + *eeKinematicsPtr_, this->get_node()); + dynamic_cast(*stateEstimate_).loadSettings(task_file_, verbose_); + currentObservation_.time = 0; + } +} diff --git a/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.h b/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.h index 907609d..ebb1315 100644 --- a/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.h +++ b/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.h @@ -4,54 +4,94 @@ #ifndef OCS2QUADRUPEDCONTROLLER_H #define OCS2QUADRUPEDCONTROLLER_H +#include "SafetyChecker.h" #include +#include +#include +#include +#include +#include +namespace ocs2::legged_robot { + class Ocs2QuadrupedController final : public controller_interface::ControllerInterface { + public: + CONTROLLER_INTERFACE_PUBLIC + Ocs2QuadrupedController() = default; -class Ocs2QuadrupedController final : public controller_interface::ControllerInterface { + CONTROLLER_INTERFACE_PUBLIC + controller_interface::InterfaceConfiguration command_interface_configuration() const override; -public: - CONTROLLER_INTERFACE_PUBLIC - Ocs2QuadrupedController() = default; + CONTROLLER_INTERFACE_PUBLIC + controller_interface::InterfaceConfiguration state_interface_configuration() const override; - CONTROLLER_INTERFACE_PUBLIC - controller_interface::InterfaceConfiguration command_interface_configuration() const override; + CONTROLLER_INTERFACE_PUBLIC + controller_interface::return_type update( + const rclcpp::Time &time, const rclcpp::Duration &period) override; - CONTROLLER_INTERFACE_PUBLIC - controller_interface::InterfaceConfiguration state_interface_configuration() const override; + CONTROLLER_INTERFACE_PUBLIC + controller_interface::CallbackReturn on_init() override; - CONTROLLER_INTERFACE_PUBLIC - controller_interface::return_type update( - const rclcpp::Time &time, const rclcpp::Duration &period) override; + CONTROLLER_INTERFACE_PUBLIC + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State &previous_state) override; - CONTROLLER_INTERFACE_PUBLIC - controller_interface::CallbackReturn on_init() override; + CONTROLLER_INTERFACE_PUBLIC + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State &previous_state) override; - CONTROLLER_INTERFACE_PUBLIC - controller_interface::CallbackReturn on_configure( - const rclcpp_lifecycle::State &previous_state) override; + CONTROLLER_INTERFACE_PUBLIC + controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State &previous_state) override; - CONTROLLER_INTERFACE_PUBLIC - controller_interface::CallbackReturn on_activate( - const rclcpp_lifecycle::State &previous_state) override; + CONTROLLER_INTERFACE_PUBLIC + controller_interface::CallbackReturn on_cleanup( + const rclcpp_lifecycle::State &previous_state) override; - CONTROLLER_INTERFACE_PUBLIC - controller_interface::CallbackReturn on_deactivate( - const rclcpp_lifecycle::State &previous_state) override; + CONTROLLER_INTERFACE_PUBLIC + controller_interface::CallbackReturn on_shutdown( + const rclcpp_lifecycle::State &previous_state) override; - CONTROLLER_INTERFACE_PUBLIC - controller_interface::CallbackReturn on_cleanup( - const rclcpp_lifecycle::State &previous_state) override; + CONTROLLER_INTERFACE_PUBLIC + controller_interface::CallbackReturn on_error( + const rclcpp_lifecycle::State &previous_state) override; - CONTROLLER_INTERFACE_PUBLIC - controller_interface::CallbackReturn on_shutdown( - const rclcpp_lifecycle::State &previous_state) override; + protected: + void setUpLeggedInterface(); - CONTROLLER_INTERFACE_PUBLIC - controller_interface::CallbackReturn on_error( - const rclcpp_lifecycle::State &previous_state) override; + void setupMpc(); -}; + void setupMrt(); + void setupStateEstimate(); + SystemObservation currentObservation_; + + std::string task_file_; + std::string urdf_file_; + std::string reference_file_; + + bool verbose_; + + std::shared_ptr legged_interface_; + std::shared_ptr eeKinematicsPtr_; + + // Whole Body Control + std::shared_ptr wbc_; + std::shared_ptr safetyChecker_; + + // Nonlinear MPC + std::shared_ptr mpc_; + std::shared_ptr mpcMrtInterface_; + + std::shared_ptr rbdConversions_; + std::shared_ptr stateEstimate_; + + private: + std::thread mpcThread_; + std::atomic_bool controllerRunning_{}, mpcRunning_{}; + benchmark::RepeatedTimer mpcTimer_; + benchmark::RepeatedTimer wbcTimer_; + }; +} #endif //OCS2QUADRUPEDCONTROLLER_H diff --git a/controllers/ocs2_quadruped_controller/src/SafetyChecker.h b/controllers/ocs2_quadruped_controller/src/SafetyChecker.h new file mode 100644 index 0000000..6b4e1cd --- /dev/null +++ b/controllers/ocs2_quadruped_controller/src/SafetyChecker.h @@ -0,0 +1,34 @@ +// +// Created by qiayuan on 2022/7/26. +// + +#pragma once + +#include +#include +#include + +namespace ocs2::legged_robot { + class SafetyChecker { + public: + explicit SafetyChecker(const CentroidalModelInfo &info) : info_(info) { + } + + bool check(const SystemObservation &observation, const vector_t & /*optimized_state*/, + const vector_t & /*optimized_input*/) { + return checkOrientation(observation); + } + + protected: + bool checkOrientation(const SystemObservation &observation) { + vector_t pose = centroidal_model::getBasePose(observation.state, info_); + if (pose(5) > M_PI_2 || pose(5) < -M_PI_2) { + std::cerr << "[SafetyChecker] Orientation safety check failed!" << std::endl; + return false; + } + return true; + } + + const CentroidalModelInfo &info_; + }; +} // namespace legged diff --git a/controllers/ocs2_quadruped_controller/src/estimator/LinearKalmanFilter.cpp b/controllers/ocs2_quadruped_controller/src/estimator/LinearKalmanFilter.cpp index 60753ea..4fb5e80 100644 --- a/controllers/ocs2_quadruped_controller/src/estimator/LinearKalmanFilter.cpp +++ b/controllers/ocs2_quadruped_controller/src/estimator/LinearKalmanFilter.cpp @@ -4,6 +4,7 @@ #include #include +#include #include #include @@ -13,13 +14,13 @@ namespace ocs2::legged_robot { KalmanFilterEstimate::KalmanFilterEstimate(PinocchioInterface pinocchioInterface, CentroidalModelInfo info, - const PinocchioEndEffectorKinematics &eeKinematics) - : StateEstimateBase(std::move(pinocchioInterface), std::move(info), eeKinematics), + const PinocchioEndEffectorKinematics &eeKinematics, + const rclcpp_lifecycle::LifecycleNode::SharedPtr &node) + : StateEstimateBase(std::move(pinocchioInterface), std::move(info), eeKinematics, std::move(node)), numContacts_(info_.numThreeDofContacts + info_.numSixDofContacts), dimContacts_(3 * numContacts_), numState_(6 + dimContacts_), numObserve_(2 * dimContacts_ + numContacts_), - tfListener_(tfBuffer_), topicUpdated_(false) { xHat_.setZero(numState_); ps_.setZero(dimContacts_); @@ -45,12 +46,10 @@ namespace ocs2::legged_robot { eeKinematics_->setPinocchioInterface(pinocchioInterface_); world2odom_.setRotation(tf2::Quaternion::getIdentity()); - sub_ = ros::NodeHandle().subscribe("/tracking_camera/odom/sample", 10, - &KalmanFilterEstimate::callback, this); } - vector_t KalmanFilterEstimate::update(const ros::Time &time, const ros::Duration &period) { - scalar_t dt = period.toSec(); + vector_t KalmanFilterEstimate::update(const rclcpp::Time &time, const rclcpp::Duration &period) { + scalar_t dt = period.seconds(); a_.block(0, 3, 3, 3) = dt * matrix3_t::Identity(); b_.block(0, 0, 3, 3) = 0.5 * dt * dt * matrix3_t::Identity(); b_.block(3, 0, 3, 3) = dt * matrix3_t::Identity(); @@ -75,8 +74,8 @@ namespace ocs2::legged_robot { // Only set angular velocity, let linear velocity be zero vPino.tail(actuatedDofNum) = rbdState_.segment(6 + info_.generalizedCoordinatesNum, actuatedDofNum); - pinocchio::forwardKinematics(model, data, qPino, vPino); - pinocchio::updateFramePlacements(model, data); + forwardKinematics(model, data, qPino, vPino); + updateFramePlacements(model, data); const auto eePos = eeKinematics_->getPosition(vector_t()); const auto eeVel = eeKinematics_->getVelocity(vector_t(), vector_t()); @@ -159,71 +158,6 @@ namespace ocs2::legged_robot { return rbdState_; } - void KalmanFilterEstimate::updateFromTopic() { - auto *msg = buffer_.readFromRT(); - - tf2::Transform world2sensor; - world2sensor.setOrigin(tf2::Vector3(msg->pose.pose.position.x, msg->pose.pose.position.y, - msg->pose.pose.position.z)); - world2sensor.setRotation(tf2::Quaternion(msg->pose.pose.orientation.x, msg->pose.pose.orientation.y, - msg->pose.pose.orientation.z, - msg->pose.pose.orientation.w)); - - if (world2odom_.getRotation() == tf2::Quaternion::getIdentity()) // First received - { - tf2::Transform odom2sensor; - try { - geometry_msgs::TransformStamped tf_msg = tfBuffer_.lookupTransform( - "odom", msg->child_frame_id, msg->header.stamp); - tf2::fromMsg(tf_msg.transform, odom2sensor); - } catch (tf2::TransformException &ex) { - ROS_WARN("%s", ex.what()); - return; - } - world2odom_ = world2sensor * odom2sensor.inverse(); - } - tf2::Transform base2sensor; - try { - geometry_msgs::TransformStamped tf_msg = tfBuffer_.lookupTransform( - "base", msg->child_frame_id, msg->header.stamp); - tf2::fromMsg(tf_msg.transform, base2sensor); - } catch (tf2::TransformException &ex) { - ROS_WARN("%s", ex.what()); - return; - } - tf2::Transform odom2base = world2odom_.inverse() * world2sensor * base2sensor.inverse(); - vector3_t newPos(odom2base.getOrigin().x(), odom2base.getOrigin().y(), odom2base.getOrigin().z()); - - const auto &model = pinocchioInterface_.getModel(); - auto &data = pinocchioInterface_.getData(); - - vector_t qPino(info_.generalizedCoordinatesNum); - qPino.head<3>() = newPos; - qPino.segment<3>(3) = rbdState_.head<3>(); - qPino.tail(info_.actuatedDofNum) = rbdState_.segment(6, info_.actuatedDofNum); - forwardKinematics(model, data, qPino); - updateFramePlacements(model, data); - - xHat_.segment<3>(0) = newPos; - for (size_t i = 0; i < numContacts_; ++i) { - xHat_.segment<3>(6 + i * 3) = eeKinematics_->getPosition(vector_t())[i]; - xHat_(6 + i * 3 + 2) -= footRadius_; - if (contactFlag_[i]) { - feetHeights_[i] = xHat_(6 + i * 3 + 2); - } - } - - auto odom = getOdomMsg(); - odom.header = msg->header; - odom.child_frame_id = "base"; - publishMsgs(odom); - } - - void KalmanFilterEstimate::callback(const nav_msgs::Odometry::ConstPtr &msg) { - buffer_.writeFromNonRT(*msg); - topicUpdated_ = true; - } - nav_msgs::msg::Odometry KalmanFilterEstimate::getOdomMsg() { nav_msgs::msg::Odometry odom; odom.pose.pose.position.x = xHat_.segment<3>(0)(0); diff --git a/controllers/ocs2_quadruped_controller/src/estimator/StateEstimateBase.cpp b/controllers/ocs2_quadruped_controller/src/estimator/StateEstimateBase.cpp index 026f8c5..8a8bd82 100644 --- a/controllers/ocs2_quadruped_controller/src/estimator/StateEstimateBase.cpp +++ b/controllers/ocs2_quadruped_controller/src/estimator/StateEstimateBase.cpp @@ -15,7 +15,7 @@ namespace ocs2::legged_robot { StateEstimateBase::StateEstimateBase(PinocchioInterface pinocchioInterface, CentroidalModelInfo info, const PinocchioEndEffectorKinematics &eeKinematics, - rclcpp::Node::SharedPtr node) + rclcpp_lifecycle::LifecycleNode::SharedPtr node) : pinocchioInterface_(std::move(pinocchioInterface)), info_(std::move(info)), eeKinematics_(eeKinematics.clone()), diff --git a/descriptions/go1_description/xacro/ros2_control.xacro b/descriptions/go1_description/xacro/ros2_control.xacro index ca6a705..a2e4209 100644 --- a/descriptions/go1_description/xacro/ros2_control.xacro +++ b/descriptions/go1_description/xacro/ros2_control.xacro @@ -164,6 +164,13 @@ + + + + + + + diff --git a/hardwares/hardware_unitree_mujoco/README.md b/hardwares/hardware_unitree_mujoco/README.md index cc1ae2a..1d88bee 100644 --- a/hardwares/hardware_unitree_mujoco/README.md +++ b/hardwares/hardware_unitree_mujoco/README.md @@ -2,7 +2,7 @@ This package contains the hardware interface based on [unitree_sdk2](https://github.com/unitreerobotics/unitree_sdk2) to control the Unitree robot in Mujoco simulator. -In theory, it also can communicate with real robot, but it is not tested yet. You can use go2 simulation in [unitree_mujoco](https://github.com/unitreerobotics/unitree_mujoco). +In theory, it also can communicate with real robot, but it is not tested yet. You can use go2 simulation in [unitree_mujoco](https://github.com/legubiao/unitree_mujoco). In this simulation, I add foot force sensor support. ## 1. Interfaces