From 7ed46994e458056a2ab4a3f59106fcc5f9847d8a Mon Sep 17 00:00:00 2001 From: Huang Zhenbiao Date: Thu, 16 Jan 2025 16:56:04 +0800 Subject: [PATCH] add support for ground truth estimator --- .../ocs2_quadruped_controller/CMakeLists.txt | 3 +- .../ocs2_quadruped_controller/README.md | 3 + .../control/CtrlComponent.h | 4 + .../estimator/FromTopiceEstimate.h | 33 -- .../estimator/GroundTruth.h | 24 + .../estimator/LinearKalmanFilter.h | 5 +- .../estimator/StateEstimateBase.h | 7 +- .../src/Ocs2QuadrupedController.cpp | 51 +- .../src/Ocs2QuadrupedController.h | 6 + .../src/control/GaitManager.cpp | 1 + .../src/control/TargetManager.cpp | 1 + .../src/estimator/FromTopicEstimate.cpp | 36 -- .../src/estimator/GroundTruth.cpp | 64 +++ .../src/estimator/LinearKalmanFilter.cpp | 5 +- .../src/estimator/StateEstimateBase.cpp | 6 +- .../go1_description/xacro/ft_sensor.xacro | 21 + .../go1_description/xacro/gazebo.xacro | 6 + .../unitree/go1_description/xacro/leg.xacro | 512 +++++++++--------- .../go2_description/config/robot_control.yaml | 10 + .../go2_description/xacro/ros2_control.xacro | 9 + hardwares/hardware_unitree_mujoco/README.md | 2 + .../hardware_unitree_mujoco/HardwareUnitree.h | 6 + .../src/HardwareUnitree.cpp | 48 +- 23 files changed, 506 insertions(+), 357 deletions(-) delete mode 100644 controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/FromTopiceEstimate.h create mode 100644 controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/GroundTruth.h delete mode 100644 controllers/ocs2_quadruped_controller/src/estimator/FromTopicEstimate.cpp create mode 100644 controllers/ocs2_quadruped_controller/src/estimator/GroundTruth.cpp create mode 100644 descriptions/unitree/go1_description/xacro/ft_sensor.xacro diff --git a/controllers/ocs2_quadruped_controller/CMakeLists.txt b/controllers/ocs2_quadruped_controller/CMakeLists.txt index 8b337c6..c62569d 100644 --- a/controllers/ocs2_quadruped_controller/CMakeLists.txt +++ b/controllers/ocs2_quadruped_controller/CMakeLists.txt @@ -12,7 +12,6 @@ set(CONTROLLER_INCLUDE_DEPENDS pluginlib rcpputils controller_interface - realtime_tools ocs2_legged_robot_ros ocs2_self_collision @@ -31,7 +30,7 @@ endforeach () add_library(${PROJECT_NAME} SHARED src/Ocs2QuadrupedController.cpp - # src/estimator/FromTopicEstimate.cpp + src/estimator/GroundTruth.cpp src/estimator/LinearKalmanFilter.cpp src/estimator/StateEstimateBase.cpp diff --git a/controllers/ocs2_quadruped_controller/README.md b/controllers/ocs2_quadruped_controller/README.md index 2fef223..e4e5ca3 100644 --- a/controllers/ocs2_quadruped_controller/README.md +++ b/controllers/ocs2_quadruped_controller/README.md @@ -10,6 +10,9 @@ Tested environment: * Ubuntu 22.04 * ROS2 Humble +*[x] **[2025-01-16]** Add support for ground truth estimator. + + [![](http://i0.hdslb.com/bfs/archive/e758ce019587032449a153cf897a543443b64bba.jpg)](https://www.bilibili.com/video/BV1UcxieuEmH/) ## 1. Interfaces 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 index f3122cd..ec9add8 100644 --- a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/control/CtrlComponent.h +++ b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/control/CtrlComponent.h @@ -41,9 +41,13 @@ struct CtrlComponent { 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_; diff --git a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/FromTopiceEstimate.h b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/FromTopiceEstimate.h deleted file mode 100644 index 90f1471..0000000 --- a/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/FromTopiceEstimate.h +++ /dev/null @@ -1,33 +0,0 @@ -// -// Created by qiayuan on 2022/7/24. -// -#pragma once - -#include "StateEstimateBase.h" - -#include - -#pragma once -namespace ocs2::legged_robot { - using namespace ocs2; - - class FromTopicStateEstimate : public StateEstimateBase { - public: - FromTopicStateEstimate(PinocchioInterface pinocchioInterface, CentroidalModelInfo info, - const PinocchioEndEffectorKinematics &eeKinematics); - - void updateImu(const Eigen::Quaternion &quat, const vector3_t &angularVelLocal, - const vector3_t &linearAccelLocal, - const matrix3_t &orientationCovariance, const matrix3_t &angularVelCovariance, - const matrix3_t &linearAccelCovariance) override { - }; - - vector_t update(const ros::Time &time, const ros::Duration &period) override; - - private: - void callback(const nav_msgs::Odometry::ConstPtr &msg); - - ros::Subscriber sub_; - realtime_tools::RealtimeBuffer buffer_; - }; -} // namespace legged 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 new file mode 100644 index 0000000..1c33a70 --- /dev/null +++ b/controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/GroundTruth.h @@ -0,0 +1,24 @@ +// +// Created by qiayuan on 2022/7/24. +// +#pragma once + +#include "StateEstimateBase.h" +#include + +#include + +namespace ocs2::legged_robot { + class GroundTruth final : public StateEstimateBase { + public: + GroundTruth(CentroidalModelInfo info, CtrlComponent &ctrl_component, + const rclcpp_lifecycle::LifecycleNode::SharedPtr &node); + + vector_t update(const rclcpp::Time &time, const rclcpp::Duration &period) override; + + protected: + nav_msgs::msg::Odometry getOdomMsg(); + vector3_t position_; + vector3_t linear_velocity_; + }; +} 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 82aa31e..b0b25b3 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 @@ -26,6 +26,9 @@ namespace ocs2::legged_robot { protected: nav_msgs::msg::Odometry getOdomMsg(); + PinocchioInterface pinocchio_interface_; + std::unique_ptr ee_kinematics_; + vector_t feet_heights_; // Config @@ -43,4 +46,4 @@ namespace ocs2::legged_robot { matrix_t a_, b_, c_, q_, p_, r_; vector_t xHat_, ps_, vs_; }; -} // namespace legged +} 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 607ee2b..6acb2c6 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 @@ -22,9 +22,7 @@ namespace ocs2::legged_robot { public: virtual ~StateEstimateBase() = default; - StateEstimateBase(PinocchioInterface pinocchio_interface, CentroidalModelInfo info, - const PinocchioEndEffectorKinematics &ee_kinematics, - CtrlComponent &ctrl_component, + StateEstimateBase(CentroidalModelInfo info, CtrlComponent &ctrl_component, rclcpp_lifecycle::LifecycleNode::SharedPtr node); virtual void updateJointStates(); @@ -45,10 +43,7 @@ namespace ocs2::legged_robot { void publishMsgs(const nav_msgs::msg::Odometry &odom) const; CtrlComponent &ctrl_component_; - - PinocchioInterface pinocchio_interface_; CentroidalModelInfo info_; - std::unique_ptr ee_kinematics_; vector3_t zyx_offset_ = vector3_t::Zero(); vector_t rbd_state_; diff --git a/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.cpp b/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.cpp index 30f161c..2941e71 100644 --- a/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.cpp +++ b/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.cpp @@ -15,6 +15,7 @@ #include #include #include +#include namespace ocs2::legged_robot { using config_type = controller_interface::interface_configuration_type; @@ -46,8 +47,14 @@ namespace ocs2::legged_robot { conf.names.push_back(imu_name_ + "/" += interface_type); } - for (const auto &interface_type: foot_force_interface_types_) { - conf.names.push_back(foot_force_name_ + "/" += interface_type); + if (use_odom_) { + for (const auto &interface_type: odom_interface_types_) { + conf.names.push_back(odom_name_ + "/" += interface_type); + } + } else { + for (const auto &interface_type: foot_force_interface_types_) { + conf.names.push_back(foot_force_name_ + "/" += interface_type); + } } return conf; @@ -139,11 +146,22 @@ namespace ocs2::legged_robot { auto_declare >("command_interfaces", command_interface_types_); 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_); - foot_force_name_ = auto_declare("foot_force_name", foot_force_name_); - foot_force_interface_types_ = - auto_declare >("foot_force_interfaces", state_interface_types_); + + // Odometer Sensor (Ground Truth) + use_odom_ = auto_declare("use_odom", use_odom_); + if (use_odom_) { + odom_name_ = auto_declare("odom_name", odom_name_); + 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_); + } // PD gains default_kp_ = auto_declare("default_kp", default_kp_); @@ -220,6 +238,8 @@ namespace ocs2::legged_robot { 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 { state_interface_map_[interface.get_interface_name()]->push_back(interface); } @@ -229,7 +249,7 @@ namespace ocs2::legged_robot { // Initial state ctrl_comp_.observation_.state.setZero( static_cast(legged_interface_->getCentroidalModelInfo().stateDim)); - updateStateEstimation(get_node()->now(), rclcpp::Duration(0, 1/ctrl_comp_.frequency_*1000000000)); + updateStateEstimation(get_node()->now(), rclcpp::Duration(0, 1 / ctrl_comp_.frequency_ * 1000000000)); ctrl_comp_.observation_.input.setZero( static_cast(legged_interface_->getCentroidalModelInfo().inputDim)); ctrl_comp_.observation_.mode = STANCE; @@ -330,10 +350,19 @@ namespace ocs2::legged_robot { } void Ocs2QuadrupedController::setupStateEstimate() { - ctrl_comp_.estimator_ = std::make_shared(legged_interface_->getPinocchioInterface(), - legged_interface_->getCentroidalModelInfo(), - *eeKinematicsPtr_, ctrl_comp_, this->get_node()); - dynamic_cast(*ctrl_comp_.estimator_).loadSettings(task_file_, verbose_); + if (use_odom_) { + ctrl_comp_.estimator_ = std::make_shared(legged_interface_->getCentroidalModelInfo(), + ctrl_comp_, + get_node()); + RCLCPP_INFO(get_node()->get_logger(), "Using Ground Truth Estimator"); + } else { + 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"); + } ctrl_comp_.observation_.time = 0; } @@ -344,7 +373,7 @@ namespace ocs2::legged_robot { 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(); + // 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 a501ca6..85595f3 100644 --- a/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.h +++ b/controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.h @@ -97,10 +97,16 @@ namespace ocs2::legged_robot { // IMU Sensor std::string imu_name_; std::vector imu_interface_types_; + // Foot Force Sensor std::string foot_force_name_; std::vector foot_force_interface_types_; + // Odometer Sensor (Ground Truth) + bool use_odom_ = false; + std::string odom_name_; + std::vector odom_interface_types_; + double default_kp_ = 0; double default_kd_ = 6; diff --git a/controllers/ocs2_quadruped_controller/src/control/GaitManager.cpp b/controllers/ocs2_quadruped_controller/src/control/GaitManager.cpp index bc30667..875d780 100644 --- a/controllers/ocs2_quadruped_controller/src/control/GaitManager.cpp +++ b/controllers/ocs2_quadruped_controller/src/control/GaitManager.cpp @@ -48,5 +48,6 @@ namespace ocs2::legged_robot { RCLCPP_INFO(rclcpp::get_logger("GaitManager"), "Switch to gait: %s", gait_name_list_[ctrl_component_.control_inputs_.command - 1].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 5f5984c..845e116 100644 --- a/controllers/ocs2_quadruped_controller/src/control/TargetManager.cpp +++ b/controllers/ocs2_quadruped_controller/src/control/TargetManager.cpp @@ -25,6 +25,7 @@ namespace ocs2::legged_robot { } void TargetManager::update() { + 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_; diff --git a/controllers/ocs2_quadruped_controller/src/estimator/FromTopicEstimate.cpp b/controllers/ocs2_quadruped_controller/src/estimator/FromTopicEstimate.cpp deleted file mode 100644 index ee55b10..0000000 --- a/controllers/ocs2_quadruped_controller/src/estimator/FromTopicEstimate.cpp +++ /dev/null @@ -1,36 +0,0 @@ -// -// Created by qiayuan on 2022/7/24. -// - -#include "ocs2_quadruped_controller/estimator/FromTopiceEstimate.h" - -namespace ocs2::legged_robot { - FromTopicStateEstimate::FromTopicStateEstimate(PinocchioInterface pinocchioInterface, CentroidalModelInfo info, - const PinocchioEndEffectorKinematics &eeKinematics) - : StateEstimateBase(std::move(pinocchioInterface), std::move(info), eeKinematics) { - ros::NodeHandle nh; - sub_ = nh.subscribe("/ground_truth/state", 10, &FromTopicStateEstimate::callback, this); - } - - void FromTopicStateEstimate::callback(const nav_msgs::Odometry::ConstPtr &msg) { - buffer_.writeFromNonRT(*msg); - } - - vector_t FromTopicStateEstimate::update(const ros::Time & /*time*/, const ros::Duration & /*period*/) { - nav_msgs::Odometry odom = *buffer_.readFromRT(); - - updateAngular(quatToZyx(Eigen::Quaternion(odom.pose.pose.orientation.w, odom.pose.pose.orientation.x, - odom.pose.pose.orientation.y, - odom.pose.pose.orientation.z)), - Eigen::Matrix(odom.twist.twist.angular.x, odom.twist.twist.angular.y, - odom.twist.twist.angular.z)); - updateLinear(Eigen::Matrix(odom.pose.pose.position.x, odom.pose.pose.position.y, - odom.pose.pose.position.z), - Eigen::Matrix(odom.twist.twist.linear.x, odom.twist.twist.linear.y, - odom.twist.twist.linear.z)); - - publishMsgs(odom); - - return rbdState_; - } -} // namespace legged diff --git a/controllers/ocs2_quadruped_controller/src/estimator/GroundTruth.cpp b/controllers/ocs2_quadruped_controller/src/estimator/GroundTruth.cpp new file mode 100644 index 0000000..968aa4d --- /dev/null +++ b/controllers/ocs2_quadruped_controller/src/estimator/GroundTruth.cpp @@ -0,0 +1,64 @@ +// +// Created by qiayuan on 2022/7/24. +// + +#include "ocs2_quadruped_controller/estimator/GroundTruth.h" + +#include + +namespace ocs2::legged_robot { + GroundTruth::GroundTruth(CentroidalModelInfo info, CtrlComponent &ctrl_component, + const rclcpp_lifecycle::LifecycleNode::SharedPtr &node) + : StateEstimateBase( + std::move(info), ctrl_component, + node) { + } + + vector_t GroundTruth::update(const rclcpp::Time &time, const rclcpp::Duration &period) { + updateJointStates(); + updateContact(); + updateImu(); + + position_ = { + ctrl_component_.odom_state_interface_[0].get().get_value(), + ctrl_component_.odom_state_interface_[1].get().get_value(), + ctrl_component_.odom_state_interface_[2].get().get_value() + }; + + linear_velocity_ = { + ctrl_component_.odom_state_interface_[3].get().get_value(), + ctrl_component_.odom_state_interface_[4].get().get_value(), + ctrl_component_.odom_state_interface_[5].get().get_value() + }; + + updateLinear(position_, linear_velocity_); + + auto odom = getOdomMsg(); + odom.header.stamp = time; + odom.header.frame_id = "odom"; + odom.child_frame_id = "base"; + publishMsgs(odom); + + return rbd_state_; + } + + nav_msgs::msg::Odometry GroundTruth::getOdomMsg() { + nav_msgs::msg::Odometry odom; + odom.pose.pose.position.x = position_(0); + odom.pose.pose.position.y = position_(1); + odom.pose.pose.position.z = position_(2); + odom.pose.pose.orientation.x = quat_.x(); + odom.pose.pose.orientation.y = quat_.y(); + odom.pose.pose.orientation.z = quat_.z(); + odom.pose.pose.orientation.w = quat_.w(); + + odom.twist.twist.linear.x = linear_velocity_(0); + odom.twist.twist.linear.y = linear_velocity_(1); + odom.twist.twist.linear.z = linear_velocity_(2); + odom.twist.twist.angular.x = angular_vel_local_.x(); + odom.twist.twist.angular.y = angular_vel_local_.y(); + odom.twist.twist.angular.z = angular_vel_local_.z(); + + return odom; + } +} // namespace legged diff --git a/controllers/ocs2_quadruped_controller/src/estimator/LinearKalmanFilter.cpp b/controllers/ocs2_quadruped_controller/src/estimator/LinearKalmanFilter.cpp index 5b64436..6d606f2 100644 --- a/controllers/ocs2_quadruped_controller/src/estimator/LinearKalmanFilter.cpp +++ b/controllers/ocs2_quadruped_controller/src/estimator/LinearKalmanFilter.cpp @@ -18,8 +18,10 @@ namespace ocs2::legged_robot { const PinocchioEndEffectorKinematics &ee_kinematics, CtrlComponent &ctrl_component, const rclcpp_lifecycle::LifecycleNode::SharedPtr &node) - : StateEstimateBase(std::move(pinocchio_interface), std::move(info), ee_kinematics, ctrl_component, + : StateEstimateBase(std::move(info), ctrl_component, node), + pinocchio_interface_(std::move(pinocchio_interface)), + ee_kinematics_(ee_kinematics.clone()), numContacts_(info_.numThreeDofContacts + info_.numSixDofContacts), dimContacts_(3 * numContacts_), numState_(6 + dimContacts_), @@ -167,7 +169,6 @@ namespace ocs2::legged_robot { odom.pose.pose.orientation.y = quat_.y(); odom.pose.pose.orientation.z = quat_.z(); odom.pose.pose.orientation.w = quat_.w(); - odom.pose.pose.orientation.x = quat_.x(); for (int i = 0; i < 3; ++i) { for (int j = 0; j < 3; ++j) { odom.pose.covariance[i * 6 + j] = p_(i, j); diff --git a/controllers/ocs2_quadruped_controller/src/estimator/StateEstimateBase.cpp b/controllers/ocs2_quadruped_controller/src/estimator/StateEstimateBase.cpp index 992bec3..6f0995c 100644 --- a/controllers/ocs2_quadruped_controller/src/estimator/StateEstimateBase.cpp +++ b/controllers/ocs2_quadruped_controller/src/estimator/StateEstimateBase.cpp @@ -12,15 +12,11 @@ #include "ocs2_quadruped_controller/control/CtrlComponent.h" namespace ocs2::legged_robot { - - StateEstimateBase::StateEstimateBase(PinocchioInterface pinocchio_interface, CentroidalModelInfo info, - const PinocchioEndEffectorKinematics &ee_kinematics, + StateEstimateBase::StateEstimateBase(CentroidalModelInfo info, CtrlComponent &ctrl_component, rclcpp_lifecycle::LifecycleNode::SharedPtr node) : ctrl_component_(ctrl_component), - pinocchio_interface_(std::move(pinocchio_interface)), info_(std::move(info)), - ee_kinematics_(ee_kinematics.clone()), rbd_state_(vector_t::Zero(2 * info_.generalizedCoordinatesNum)), node_(std::move(node)) { odom_pub_ = node_->create_publisher("odom", 10); pose_pub_ = node_->create_publisher("pose", 10); diff --git a/descriptions/unitree/go1_description/xacro/ft_sensor.xacro b/descriptions/unitree/go1_description/xacro/ft_sensor.xacro new file mode 100644 index 0000000..73fec1c --- /dev/null +++ b/descriptions/unitree/go1_description/xacro/ft_sensor.xacro @@ -0,0 +1,21 @@ + + + + + + + true + + true + + 1 + 500 + true + + child + + + + + + \ No newline at end of file diff --git a/descriptions/unitree/go1_description/xacro/gazebo.xacro b/descriptions/unitree/go1_description/xacro/gazebo.xacro index 6f3a38d..03e9c17 100644 --- a/descriptions/unitree/go1_description/xacro/gazebo.xacro +++ b/descriptions/unitree/go1_description/xacro/gazebo.xacro @@ -1,5 +1,6 @@ + @@ -185,4 +186,9 @@ true + + + + + diff --git a/descriptions/unitree/go1_description/xacro/leg.xacro b/descriptions/unitree/go1_description/xacro/leg.xacro index c44fa36..fe135bf 100644 --- a/descriptions/unitree/go1_description/xacro/leg.xacro +++ b/descriptions/unitree/go1_description/xacro/leg.xacro @@ -2,280 +2,280 @@ - + - + - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - + + + + + + + + + + + + + - - - - - - - - + + + + + + + + - - - - - + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - + + + + + + + + + + + + + - - - - - - - - + + + + + + + + - - - - - + + + + + - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - + + + + + + + + + + + + + - - - - - + + + + + - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + - - 0.2 - 0.2 - + + 0.2 + 0.2 + - - 0.2 - 0.2 - 0 - - - + + 0.2 + 0.2 + 0 + + + - - 0.6 - 0.6 - 1 - + + 0.6 + 0.6 + 1 + - - 0.6 - 0.6 - 1 - - - + + 0.6 + 0.6 + 1 + + + - - + + diff --git a/descriptions/unitree/go2_description/config/robot_control.yaml b/descriptions/unitree/go2_description/config/robot_control.yaml index 0de34a4..71538f8 100644 --- a/descriptions/unitree/go2_description/config/robot_control.yaml +++ b/descriptions/unitree/go2_description/config/robot_control.yaml @@ -125,6 +125,16 @@ ocs2_quadruped_controller: - linear_acceleration.y - linear_acceleration.z + use_odom: True + 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/descriptions/unitree/go2_description/xacro/ros2_control.xacro b/descriptions/unitree/go2_description/xacro/ros2_control.xacro index 688e259..72cc1ec 100644 --- a/descriptions/unitree/go2_description/xacro/ros2_control.xacro +++ b/descriptions/unitree/go2_description/xacro/ros2_control.xacro @@ -171,6 +171,15 @@ + + + + + + + + + diff --git a/hardwares/hardware_unitree_mujoco/README.md b/hardwares/hardware_unitree_mujoco/README.md index 1d88bee..fba9834 100644 --- a/hardwares/hardware_unitree_mujoco/README.md +++ b/hardwares/hardware_unitree_mujoco/README.md @@ -4,6 +4,8 @@ This package contains the hardware interface based on [unitree_sdk2](https://git 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. +*[x] **[2025-01-16]** Add odometer states for simulation. + ## 1. Interfaces Required hardware interfaces: diff --git a/hardwares/hardware_unitree_mujoco/include/hardware_unitree_mujoco/HardwareUnitree.h b/hardwares/hardware_unitree_mujoco/include/hardware_unitree_mujoco/HardwareUnitree.h index 9b8129f..7baad49 100644 --- a/hardwares/hardware_unitree_mujoco/include/hardware_unitree_mujoco/HardwareUnitree.h +++ b/hardwares/hardware_unitree_mujoco/include/hardware_unitree_mujoco/HardwareUnitree.h @@ -10,6 +10,7 @@ #include #include +#include #include #include @@ -38,6 +39,7 @@ protected: std::vector imu_states_; std::vector foot_force_; + std::vector high_states_; std::unordered_map > joint_interfaces = { {"position", {}}, @@ -50,13 +52,17 @@ protected: void lowStateMessageHandle(const void *messages); + void highStateMessageHandle(const void *messages); + unitree_go::msg::dds_::LowCmd_ low_cmd_{}; // default init unitree_go::msg::dds_::LowState_ low_state_{}; // default init + unitree_go::msg::dds_::SportModeState_ high_state_{}; // default init /*publisher*/ unitree::robot::ChannelPublisherPtr low_cmd_publisher_; /*subscriber*/ unitree::robot::ChannelSubscriberPtr lows_tate_subscriber_; + unitree::robot::ChannelSubscriberPtr high_state_subscriber_; }; diff --git a/hardwares/hardware_unitree_mujoco/src/HardwareUnitree.cpp b/hardwares/hardware_unitree_mujoco/src/HardwareUnitree.cpp index 4772a52..302e8b4 100644 --- a/hardwares/hardware_unitree_mujoco/src/HardwareUnitree.cpp +++ b/hardwares/hardware_unitree_mujoco/src/HardwareUnitree.cpp @@ -8,6 +8,7 @@ #define TOPIC_LOWCMD "rt/lowcmd" #define TOPIC_LOWSTATE "rt/lowstate" +#define TOPIC_HIGHSTATE "rt/sportmodestate" using namespace unitree::robot; using hardware_interface::return_type; @@ -30,6 +31,7 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Hardwa imu_states_.assign(10, 0); foot_force_.assign(4, 0); + high_states_.assign(6, 0); for (const auto &joint: info_.joints) { for (const auto &interface: joint.state_interfaces) { @@ -38,12 +40,12 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Hardwa } ChannelFactory::Instance()->Init(1, "lo"); + low_cmd_publisher_ = std::make_shared >( TOPIC_LOWCMD); low_cmd_publisher_->InitChannel(); - /*create subscriber*/ lows_tate_subscriber_ = std::make_shared >( TOPIC_LOWSTATE); @@ -54,6 +56,15 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Hardwa 1); initLowCmd(); + high_state_subscriber_ = + std::make_shared >( + TOPIC_HIGHSTATE); + high_state_subscriber_->InitChannel( + [this](auto &&PH1) { + highStateMessageHandle(std::forward(PH1)); + }, + 1); + return SystemInterface::on_init(info); } @@ -83,13 +94,25 @@ std::vector HardwareUnitree::export_state_in } // export foot force sensor state interface - for (uint i = 0; i < info_.sensors[1].state_interfaces.size(); i++) { - state_interfaces.emplace_back( - info_.sensors[1].name, info_.sensors[1].state_interfaces[i].name, &foot_force_[i]); + if (info_.sensors.size() > 1) { + for (uint i = 0; i < info_.sensors[1].state_interfaces.size(); i++) { + state_interfaces.emplace_back( + info_.sensors[1].name, info_.sensors[1].state_interfaces[i].name, &foot_force_[i]); + } + } + + // export odometer state interface + if (info_.sensors.size() > 2) { + // export high state interface + for (uint i = 0; i < info_.sensors[2].state_interfaces.size(); i++) { + state_interfaces.emplace_back( + info_.sensors[2].name, info_.sensors[2].state_interfaces[i].name, &high_states_[i]); + } } - return state_interfaces; + return + state_interfaces; } std::vector HardwareUnitree::export_command_interfaces() { @@ -141,6 +164,17 @@ return_type HardwareUnitree::read(const rclcpp::Time & /*time*/, const rclcpp::D foot_force_[2] = low_state_.foot_force()[2]; foot_force_[3] = low_state_.foot_force()[3]; + // high states + high_states_[0] = high_state_.position()[0]; + high_states_[1] = high_state_.position()[1]; + high_states_[2] = high_state_.position()[2]; + high_states_[3] = high_state_.velocity()[0]; + high_states_[4] = high_state_.velocity()[1]; + high_states_[5] = high_state_.velocity()[2]; + + // RCLCPP_INFO(get_logger(), "high state: %f %f %f %f %f %f", high_states_[0], high_states_[1], high_states_[2], + // high_states_[3], high_states_[4], high_states_[5]); + return return_type::OK; } @@ -182,6 +216,10 @@ void HardwareUnitree::lowStateMessageHandle(const void *messages) { low_state_ = *static_cast(messages); } +void HardwareUnitree::highStateMessageHandle(const void *messages) { + high_state_ = *static_cast(messages); +} + #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS(