From d8c4d8eee512bc4d9c9d6eaece5241000ecf8825 Mon Sep 17 00:00:00 2001 From: Huang Zhenbiao Date: Wed, 26 Feb 2025 01:18:37 +0800 Subject: [PATCH] tried to add foot force sensor --- .clang-tidy | 147 +++++ .../go2_description/config/gazebo.yaml | 7 +- .../go2_description/xacro/ft_sensor.xacro | 22 + .../go2_description/xacro/gazebo.xacro | 531 +++++++++--------- .../unitree/go2_description/xacro/leg.xacro | 358 ++++++------ hardwares/gz_quadruped_hardware/README.md | 8 + .../gz_quadruped_hardware/src/gz_system.cpp | 373 +++++++----- .../launch/gazebo.launch.py | 6 +- libraries/gz_quadruped_playground/package.xml | 2 +- .../worlds/default.sdf | 4 - 10 files changed, 883 insertions(+), 575 deletions(-) create mode 100644 .clang-tidy create mode 100644 descriptions/unitree/go2_description/xacro/ft_sensor.xacro create mode 100644 hardwares/gz_quadruped_hardware/README.md diff --git a/.clang-tidy b/.clang-tidy new file mode 100644 index 0000000..fd8c681 --- /dev/null +++ b/.clang-tidy @@ -0,0 +1,147 @@ +# Generated from CLion Inspection settings +--- +Checks: '-*, +bugprone-argument-comment, +bugprone-assert-side-effect, +bugprone-bad-signal-to-kill-thread, +bugprone-branch-clone, +bugprone-copy-constructor-init, +bugprone-dangling-handle, +bugprone-dynamic-static-initializers, +bugprone-fold-init-type, +bugprone-forward-declaration-namespace, +bugprone-forwarding-reference-overload, +bugprone-inaccurate-erase, +bugprone-incorrect-roundings, +bugprone-integer-division, +bugprone-lambda-function-name, +bugprone-macro-parentheses, +bugprone-macro-repeated-side-effects, +bugprone-misplaced-operator-in-strlen-in-alloc, +bugprone-misplaced-pointer-arithmetic-in-alloc, +bugprone-misplaced-widening-cast, +bugprone-move-forwarding-reference, +bugprone-multiple-statement-macro, +bugprone-no-escape, +bugprone-parent-virtual-call, +bugprone-posix-return, +bugprone-reserved-identifier, +bugprone-sizeof-container, +bugprone-sizeof-expression, +bugprone-spuriously-wake-up-functions, +bugprone-string-constructor, +bugprone-string-integer-assignment, +bugprone-string-literal-with-embedded-nul, +bugprone-suspicious-enum-usage, +bugprone-suspicious-include, +bugprone-suspicious-memset-usage, +bugprone-suspicious-missing-comma, +bugprone-suspicious-semicolon, +bugprone-suspicious-string-compare, +bugprone-suspicious-memory-comparison, +bugprone-suspicious-realloc-usage, +bugprone-swapped-arguments, +bugprone-terminating-continue, +bugprone-throw-keyword-missing, +bugprone-too-small-loop-variable, +bugprone-undefined-memory-manipulation, +bugprone-undelegated-constructor, +bugprone-unhandled-self-assignment, +bugprone-unused-raii, +bugprone-unused-return-value, +bugprone-use-after-move, +bugprone-virtual-near-miss, +cert-dcl21-cpp, +cert-dcl58-cpp, +cert-err34-c, +cert-err52-cpp, +cert-err60-cpp, +cert-flp30-c, +cert-msc50-cpp, +cert-msc51-cpp, +cert-str34-c, +cppcoreguidelines-interfaces-global-init, +cppcoreguidelines-narrowing-conversions, +cppcoreguidelines-pro-type-member-init, +cppcoreguidelines-pro-type-static-cast-downcast, +cppcoreguidelines-slicing, +google-default-arguments, +google-explicit-constructor, +google-runtime-operator, +hicpp-exception-baseclass, +hicpp-multiway-paths-covered, +misc-misplaced-const, +misc-new-delete-overloads, +misc-no-recursion, +misc-non-copyable-objects, +misc-throw-by-value-catch-by-reference, +misc-unconventional-assign-operator, +misc-uniqueptr-reset-release, +modernize-avoid-bind, +modernize-concat-nested-namespaces, +modernize-deprecated-headers, +modernize-deprecated-ios-base-aliases, +modernize-loop-convert, +modernize-make-shared, +modernize-make-unique, +modernize-pass-by-value, +modernize-raw-string-literal, +modernize-redundant-void-arg, +modernize-replace-auto-ptr, +modernize-replace-disallow-copy-and-assign-macro, +modernize-replace-random-shuffle, +modernize-return-braced-init-list, +modernize-shrink-to-fit, +modernize-unary-static-assert, +modernize-use-auto, +modernize-use-bool-literals, +modernize-use-emplace, +modernize-use-equals-default, +modernize-use-equals-delete, +modernize-use-nodiscard, +modernize-use-noexcept, +modernize-use-nullptr, +modernize-use-override, +modernize-use-transparent-functors, +modernize-use-uncaught-exceptions, +mpi-buffer-deref, +mpi-type-mismatch, +openmp-use-default-none, +performance-faster-string-find, +performance-for-range-copy, +performance-implicit-conversion-in-loop, +performance-inefficient-algorithm, +performance-inefficient-string-concatenation, +performance-inefficient-vector-operation, +performance-move-const-arg, +performance-move-constructor-init, +performance-no-automatic-move, +performance-noexcept-move-constructor, +performance-trivially-destructible, +performance-type-promotion-in-math-fn, +performance-unnecessary-copy-initialization, +performance-unnecessary-value-param, +portability-simd-intrinsics, +readability-avoid-const-params-in-decls, +readability-const-return-type, +readability-container-size-empty, +readability-convert-member-functions-to-static, +readability-delete-null-pointer, +readability-deleted-default, +readability-inconsistent-declaration-parameter-name, +readability-make-member-function-const, +readability-misleading-indentation, +readability-misplaced-array-index, +readability-non-const-parameter, +readability-redundant-control-flow, +readability-redundant-declaration, +readability-redundant-function-ptr-dereference, +readability-redundant-smartptr-get, +readability-redundant-string-cstr, +readability-redundant-string-init, +readability-simplify-subscript-expr, +readability-static-accessed-through-instance, +readability-static-definition-in-anonymous-namespace, +readability-string-compare, +readability-uniqueptr-delete-release, +readability-use-anyofallof' \ No newline at end of file diff --git a/descriptions/unitree/go2_description/config/gazebo.yaml b/descriptions/unitree/go2_description/config/gazebo.yaml index d15c0d7..e0367fc 100644 --- a/descriptions/unitree/go2_description/config/gazebo.yaml +++ b/descriptions/unitree/go2_description/config/gazebo.yaml @@ -75,7 +75,12 @@ ocs2_quadruped_controller: - linear_acceleration.y - linear_acceleration.z - estimator_type: "odom_topic" + foot_force_name: "foot_force" + foot_force_interfaces: + - FL_ft_sensor + - RL_ft_sensor + - FR_ft_sensor + - RR_ft_sensor unitree_guide_controller: ros__parameters: diff --git a/descriptions/unitree/go2_description/xacro/ft_sensor.xacro b/descriptions/unitree/go2_description/xacro/ft_sensor.xacro new file mode 100644 index 0000000..aa714f9 --- /dev/null +++ b/descriptions/unitree/go2_description/xacro/ft_sensor.xacro @@ -0,0 +1,22 @@ + + + + + + + true + + true + + 1 + 500 + true + ${name}_ft + + child + + + + + + \ No newline at end of file diff --git a/descriptions/unitree/go2_description/xacro/gazebo.xacro b/descriptions/unitree/go2_description/xacro/gazebo.xacro index 6442c9d..911c9e2 100644 --- a/descriptions/unitree/go2_description/xacro/gazebo.xacro +++ b/descriptions/unitree/go2_description/xacro/gazebo.xacro @@ -1,279 +1,294 @@ - - - gz_quadruped_hardware/GazeboSimSystem - + + + + gz_quadruped_hardware/GazeboSimSystem + - - - - - - + + + + + + - - - - + + + + - - - - - - - - - - + + + + + + + + + + - - - - - - - - - - + + + + + + + + + + - - - - - - - - - - + + + + + + + + + + - - - - - - - - - - + + + + + + + + + + - - - - - - - - - - + + + + + + + + + + - - - - - - - - - - + + + + + + + + + + - - - - - - - - - - + + + + + + + + + + - - - - - - - - - - + + + + + + + + + + - - - - - - - - - - + + + + + + + + + + - - - - - - - - - - + + + + + + + + + + - - - - - - - - - - + + + + + + + + + + - - - - - - - - - - - - - + + + + + + + + + + + + - - - $(find go2_description)/config/gazebo.yaml - - - - - odom - base - 1000 - odom - 3 - odom_with_covariance - tf - - + + + + + + - - 0.6 - 0.6 - 1 - + - - - 1 - 500 - true - imu - - - - - 0.0 - 2e-4 - 0.0000075 - 0.0000008 - - - - - 0.0 - 2e-4 - 0.0000075 - 0.0000008 - - - - - 0.0 - 2e-4 - 0.0000075 - 0.0000008 - - - - - - - 0.0 - 1.7e-2 - 0.1 - 0.001 - - - - - 0.0 - 1.7e-2 - 0.1 - 0.001 - - - - - 0.0 - 1.7e-2 - 0.1 - 0.001 - - - - - - - - - true - - - - - - - - 2.094 - - 1280 - 720 - - - 0.1 - 15 - - - gaussian - - 0.0 - 0.007 - - front_camera - camera/camera_info - - true - 15 - true - camera/image - + + + $(find go2_description)/config/gazebo.yaml + + + + + + odom + base + 1000 + odom + 3 + odom_with_covariance + tf + - + + + 0.6 + 0.6 + 1 + + + + + 1 + 500 + true + imu + + + + + 0.0 + 2e-4 + 0.0000075 + 0.0000008 + + + + + 0.0 + 2e-4 + 0.0000075 + 0.0000008 + + + + + 0.0 + 2e-4 + 0.0000075 + 0.0000008 + + + + + + + 0.0 + 1.7e-2 + 0.1 + 0.001 + + + + + 0.0 + 1.7e-2 + 0.1 + 0.001 + + + + + 0.0 + 1.7e-2 + 0.1 + 0.001 + + + + + + + + + true + + + + + + + + + + + + + 2.094 + + 1280 + 720 + + + 0.1 + 15 + + + gaussian + + 0.0 + 0.007 + + front_camera + camera/camera_info + + true + 15 + true + camera/image + + + diff --git a/descriptions/unitree/go2_description/xacro/leg.xacro b/descriptions/unitree/go2_description/xacro/leg.xacro index 9b2f9bf..13150e2 100755 --- a/descriptions/unitree/go2_description/xacro/leg.xacro +++ b/descriptions/unitree/go2_description/xacro/leg.xacro @@ -2,202 +2,202 @@ - + - + - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - + + + + + + + + - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + - 0 - - - - + + + + + - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + - - 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/hardwares/gz_quadruped_hardware/README.md b/hardwares/gz_quadruped_hardware/README.md new file mode 100644 index 0000000..355be96 --- /dev/null +++ b/hardwares/gz_quadruped_hardware/README.md @@ -0,0 +1,8 @@ +# Gazebo Quadruped ROS2 Control Plugin + +This repository is a modified version of [gz_ros2_control](https://github.com/ros-controls/gz_ros2_control) + +```bash +cd ~/ros2_ws +colcon build --packages-up-to gz_quadruped_hardware --symlink-install +``` \ No newline at end of file diff --git a/hardwares/gz_quadruped_hardware/src/gz_system.cpp b/hardwares/gz_quadruped_hardware/src/gz_system.cpp index 2fba691..799d962 100644 --- a/hardwares/gz_quadruped_hardware/src/gz_system.cpp +++ b/hardwares/gz_quadruped_hardware/src/gz_system.cpp @@ -15,6 +15,7 @@ #include "gz_quadruped_hardware/gz_system.hpp" #include +#include #include #include @@ -32,7 +33,7 @@ #include #include #include -#include +#include #include #include #include @@ -46,7 +47,8 @@ #include #include -struct jointData { +struct jointData +{ /// \brief Joint's names. std::string name; @@ -88,7 +90,8 @@ struct jointData { gz_quadruped_hardware::GazeboSimSystemInterface::ControlMethod joint_control_method; }; -class ImuData { +class ImuData +{ public: /// \brief imu's name. std::string name{}; @@ -103,10 +106,11 @@ public: std::array imu_sensor_data_; /// \brief callback to get the IMU topic values - void OnIMU(const GZ_MSGS_NAMESPACE IMU &_msg); + void OnIMU(const GZ_MSGS_NAMESPACE IMU& _msg); }; -void ImuData::OnIMU(const GZ_MSGS_NAMESPACE IMU &_msg) { +void ImuData::OnIMU(const GZ_MSGS_NAMESPACE IMU& _msg) +{ this->imu_sensor_data_[0] = _msg.orientation().x(); this->imu_sensor_data_[1] = _msg.orientation().y(); this->imu_sensor_data_[2] = _msg.orientation().z(); @@ -119,7 +123,41 @@ void ImuData::OnIMU(const GZ_MSGS_NAMESPACE IMU &_msg) { this->imu_sensor_data_[9] = _msg.linear_acceleration().z(); } -class gz_quadruped_hardware::GazeboSimSystemPrivate { +class ForceTorqueData +{ +public: + /// \brief force-torque sensor's name. + std::string name{}; + + /// \brief force-torque sensor's topic name. + std::string topicName{}; + + /// \brief handles to the force torque from within Gazebo + sim::Entity sim_ft_sensors_ = sim::kNullEntity; + + /// \brief An array per FT + std::array ft_sensor_data_; + + /// \brief Current foot end effort + double foot_effort; + + /// \brief callback to get the Force Torque topic values + void OnForceTorque(const GZ_MSGS_NAMESPACE Wrench& _msg); +}; + +void ForceTorqueData::OnForceTorque(const GZ_MSGS_NAMESPACE Wrench& _msg) +{ + this->ft_sensor_data_[0] = _msg.force().x(); + this->ft_sensor_data_[1] = _msg.force().y(); + this->ft_sensor_data_[2] = _msg.force().z(); + this->ft_sensor_data_[3] = _msg.torque().x(); + this->ft_sensor_data_[4] = _msg.torque().y(); + this->ft_sensor_data_[5] = _msg.torque().z(); + this->foot_effort = sqrt(pow(_msg.force().x(), 2) + pow(_msg.force().y(), 2) + pow(_msg.force().z(), 2)); +} + +class gz_quadruped_hardware::GazeboSimSystemPrivate +{ public: GazeboSimSystemPrivate() = default; @@ -135,7 +173,10 @@ public: std::vector joints_; /// \brief vector with the imus . - std::vector > imus_; + std::vector> imus_; + + /// \brief vector with the foot force-torque sensors. + std::vector> ft_sensors_; /// \brief state interfaces that will be exported to the Resource Manager std::vector state_interfaces_; @@ -145,25 +186,24 @@ public: /// \brief Entity component manager, ECM shouldn't be accessed outside those /// methods, otherwise the app will crash - sim::EntityComponentManager *ecm; + sim::EntityComponentManager* ecm; /// \brief controller update rate unsigned int update_rate; /// \brief Gazebo communication node. GZ_TRANSPORT_NAMESPACE Node node; - - /// \brief Gain which converts position error to a velocity command - double position_proportional_gain_; }; -namespace gz_quadruped_hardware { +namespace gz_quadruped_hardware +{ bool GazeboSimSystem::initSim( - rclcpp::Node::SharedPtr &model_nh, - std::map &enableJoints, - const hardware_interface::HardwareInfo &hardware_info, - sim::EntityComponentManager &_ecm, - unsigned int update_rate) { + rclcpp::Node::SharedPtr& model_nh, + std::map& enableJoints, + const hardware_interface::HardwareInfo& hardware_info, + sim::EntityComponentManager& _ecm, + unsigned int update_rate) + { this->dataPtr = std::make_unique(); this->dataPtr->last_update_sim_time_ros_ = rclcpp::Time(); @@ -178,48 +218,20 @@ namespace gz_quadruped_hardware { this->dataPtr->joints_.resize(this->dataPtr->n_dof_); - try { - this->dataPtr->position_proportional_gain_ = - this->nh_->get_parameter("position_proportional_gain").as_double(); - } catch (rclcpp::exceptions::ParameterUninitializedException &ex) { - RCLCPP_ERROR( - this->nh_->get_logger(), - "Parameter 'position_proportional_gain' not initialized, with error %s", ex.what()); - RCLCPP_WARN_STREAM( - this->nh_->get_logger(), - "Using default value: " << this->dataPtr->position_proportional_gain_); - } catch (rclcpp::exceptions::ParameterNotDeclaredException &ex) { - RCLCPP_ERROR( - this->nh_->get_logger(), - "Parameter 'position_proportional_gain' not declared, with error %s", ex.what()); - RCLCPP_WARN_STREAM( - this->nh_->get_logger(), - "Using default value: " << this->dataPtr->position_proportional_gain_); - } catch (rclcpp::ParameterTypeException &ex) { - RCLCPP_ERROR( - this->nh_->get_logger(), - "Parameter 'position_proportional_gain' has wrong type: %s", ex.what()); - RCLCPP_WARN_STREAM( - this->nh_->get_logger(), - "Using default value: " << this->dataPtr->position_proportional_gain_); - } - - RCLCPP_INFO_STREAM( - this->nh_->get_logger(), - "The position_proportional_gain has been set to: " << - this->dataPtr->position_proportional_gain_); - - if (this->dataPtr->n_dof_ == 0) { + if (this->dataPtr->n_dof_ == 0) + { RCLCPP_ERROR_STREAM(this->nh_->get_logger(), "There is no joint available"); return false; } - for (unsigned int j = 0; j < this->dataPtr->n_dof_; j++) { - auto &joint_info = hardware_info.joints[j]; + for (unsigned int j = 0; j < this->dataPtr->n_dof_; j++) + { + auto& joint_info = hardware_info.joints[j]; std::string joint_name = this->dataPtr->joints_[j].name = joint_info.name; auto it_joint = enableJoints.find(joint_name); - if (it_joint == enableJoints.end()) { + if (it_joint == enableJoints.end()) + { RCLCPP_WARN_STREAM( this->nh_->get_logger(), "Skipping joint in the URDF named '" << joint_name << "' which is not in the gazebo model."); @@ -236,21 +248,24 @@ namespace gz_quadruped_hardware { // Create joint position component if one doesn't exist if (!_ecm.EntityHasComponentType( simjoint, - sim::components::JointPosition().TypeId())) { + sim::components::JointPosition().TypeId())) + { _ecm.CreateComponent(simjoint, sim::components::JointPosition()); } // Create joint velocity component if one doesn't exist if (!_ecm.EntityHasComponentType( simjoint, - sim::components::JointVelocity().TypeId())) { + sim::components::JointVelocity().TypeId())) + { _ecm.CreateComponent(simjoint, sim::components::JointVelocity()); } // Create joint transmitted wrench component if one doesn't exist if (!_ecm.EntityHasComponentType( simjoint, - sim::components::JointTransmittedWrench().TypeId())) { + sim::components::JointTransmittedWrench().TypeId())) + { _ecm.CreateComponent(simjoint, sim::components::JointTransmittedWrench()); } @@ -261,11 +276,13 @@ namespace gz_quadruped_hardware { auto it = std::find_if( hardware_info.mimic_joints.begin(), hardware_info.mimic_joints.end(), - [j](const hardware_interface::MimicJoint &mj) { + [j](const hardware_interface::MimicJoint& mj) + { return mj.joint_index == j; }); - if (it != hardware_info.mimic_joints.end()) { + if (it != hardware_info.mimic_joints.end()) + { RCLCPP_INFO_STREAM( this->nh_->get_logger(), "Joint '" << joint_name << "'is mimicking joint '" << @@ -276,13 +293,18 @@ namespace gz_quadruped_hardware { RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\tState:"); auto get_initial_value = - [this, joint_name](const hardware_interface::InterfaceInfo &interface_info) { + [this, joint_name](const hardware_interface::InterfaceInfo& interface_info) + { double initial_value{0.0}; - if (!interface_info.initial_value.empty()) { - try { + if (!interface_info.initial_value.empty()) + { + try + { initial_value = hardware_interface::stod(interface_info.initial_value); RCLCPP_INFO(this->nh_->get_logger(), "\t\t\t found initial value: %f", initial_value); - } catch (std::invalid_argument &) { + } + catch (std::invalid_argument&) + { RCLCPP_ERROR_STREAM( this->nh_->get_logger(), "Failed converting initial_value string to real number for the joint " @@ -301,8 +323,10 @@ namespace gz_quadruped_hardware { double initial_effort = std::numeric_limits::quiet_NaN(); // register the state handles - for (unsigned int i = 0; i < joint_info.state_interfaces.size(); ++i) { - if (joint_info.state_interfaces[i].name == "position") { + for (unsigned int i = 0; i < joint_info.state_interfaces.size(); ++i) + { + if (joint_info.state_interfaces[i].name == "position") + { RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t position"); this->dataPtr->state_interfaces_.emplace_back( joint_name, @@ -311,7 +335,8 @@ namespace gz_quadruped_hardware { initial_position = get_initial_value(joint_info.state_interfaces[i]); this->dataPtr->joints_[j].joint_position = initial_position; } - if (joint_info.state_interfaces[i].name == "velocity") { + if (joint_info.state_interfaces[i].name == "velocity") + { RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t velocity"); this->dataPtr->state_interfaces_.emplace_back( joint_name, @@ -320,7 +345,8 @@ namespace gz_quadruped_hardware { initial_velocity = get_initial_value(joint_info.state_interfaces[i]); this->dataPtr->joints_[j].joint_velocity = initial_velocity; } - if (joint_info.state_interfaces[i].name == "effort") { + if (joint_info.state_interfaces[i].name == "effort") + { RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t effort"); this->dataPtr->state_interfaces_.emplace_back( joint_name, @@ -334,42 +360,55 @@ namespace gz_quadruped_hardware { RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\tCommand:"); // register the command handles - for (unsigned int i = 0; i < joint_info.command_interfaces.size(); ++i) { - if (joint_info.command_interfaces[i].name == "position") { + for (unsigned int i = 0; i < joint_info.command_interfaces.size(); ++i) + { + if (joint_info.command_interfaces[i].name == "position") + { RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t position"); this->dataPtr->command_interfaces_.emplace_back( joint_name, hardware_interface::HW_IF_POSITION, &this->dataPtr->joints_[j].joint_position_cmd); - if (!std::isnan(initial_position)) { + if (!std::isnan(initial_position)) + { this->dataPtr->joints_[j].joint_position_cmd = initial_position; } - } else if (joint_info.command_interfaces[i].name == "velocity") { + } + else if (joint_info.command_interfaces[i].name == "velocity") + { RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t velocity"); this->dataPtr->command_interfaces_.emplace_back( joint_name, hardware_interface::HW_IF_VELOCITY, &this->dataPtr->joints_[j].joint_velocity_cmd); - if (!std::isnan(initial_velocity)) { + if (!std::isnan(initial_velocity)) + { this->dataPtr->joints_[j].joint_velocity_cmd = initial_velocity; } - } else if (joint_info.command_interfaces[i].name == "effort") { + } + else if (joint_info.command_interfaces[i].name == "effort") + { RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t effort"); this->dataPtr->command_interfaces_.emplace_back( joint_name, hardware_interface::HW_IF_EFFORT, &this->dataPtr->joints_[j].joint_effort_cmd); - if (!std::isnan(initial_effort)) { + if (!std::isnan(initial_effort)) + { this->dataPtr->joints_[j].joint_effort_cmd = initial_effort; } - } else if (joint_info.command_interfaces[i].name == "kp") { + } + else if (joint_info.command_interfaces[i].name == "kp") + { RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t kp"); this->dataPtr->command_interfaces_.emplace_back( joint_name, "kp", &this->dataPtr->joints_[j].joint_kp_cmd); this->dataPtr->joints_[j].joint_kp_cmd = 0.0; - } else if (joint_info.command_interfaces[i].name == "kd") { + } + else if (joint_info.command_interfaces[i].name == "kd") + { RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t kd"); this->dataPtr->command_interfaces_.emplace_back( joint_name, @@ -379,13 +418,15 @@ namespace gz_quadruped_hardware { } // independently of existence of command interface set initial value if defined - if (!std::isnan(initial_position)) { + if (!std::isnan(initial_position)) + { this->dataPtr->joints_[j].joint_position = initial_position; this->dataPtr->ecm->CreateComponent( this->dataPtr->joints_[j].sim_joint, sim::components::JointPositionReset({initial_position})); } - if (!std::isnan(initial_velocity)) { + if (!std::isnan(initial_velocity)) + { this->dataPtr->joints_[j].joint_velocity = initial_velocity; this->dataPtr->ecm->CreateComponent( this->dataPtr->joints_[j].sim_joint, @@ -403,12 +444,14 @@ namespace gz_quadruped_hardware { } void GazeboSimSystem::registerSensors( - const hardware_interface::HardwareInfo &hardware_info) { + const hardware_interface::HardwareInfo& hardware_info) + { // Collect gazebo sensor handles size_t n_sensors = hardware_info.sensors.size(); std::vector sensor_components_; - for (unsigned int j = 0; j < n_sensors; j++) { + for (unsigned int j = 0; j < n_sensors; j++) + { hardware_interface::ComponentInfo component = hardware_info.sensors[j]; sensor_components_.push_back(component); } @@ -416,17 +459,20 @@ namespace gz_quadruped_hardware { // So we have resize only once the structures where the data will be stored, and we can safely // use pointers to the structures + // IMU Sensors this->dataPtr->ecm->Each( - [&](const sim::Entity &_entity, - const sim::components::Imu *, - const sim::components::Name *_name) -> bool { + sim::components::Name>( + [&](const sim::Entity& _entity, + const sim::components::Imu*, + const sim::components::Name* _name) -> bool + { auto imuData = std::make_shared(); RCLCPP_INFO_STREAM(this->nh_->get_logger(), "Loading sensor: " << _name->Data()); auto sensorTopicComp = this->dataPtr->ecm->Component< sim::components::SensorTopic>(_entity); - if (sensorTopicComp) { + if (sensorTopicComp) + { RCLCPP_INFO_STREAM(this->nh_->get_logger(), "Topic name: " << sensorTopicComp->Data()); } @@ -436,8 +482,10 @@ namespace gz_quadruped_hardware { imuData->sim_imu_sensors_ = _entity; hardware_interface::ComponentInfo component; - for (auto &comp: sensor_components_) { - if (comp.name == _name->Data()) { + for (auto& comp : sensor_components_) + { + if (comp.name == _name->Data()) + { component = comp; } } @@ -455,7 +503,8 @@ namespace gz_quadruped_hardware { {"linear_acceleration.z", 9}, }; - for (const auto &state_interface: component.state_interfaces) { + for (const auto& state_interface : component.state_interfaces) + { RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t " << state_interface.name); size_t data_index = interface_name_map.at(state_interface.name); @@ -467,75 +516,111 @@ namespace gz_quadruped_hardware { this->dataPtr->imus_.push_back(imuData); return true; }); + + // Foot Force torque sensor + this->dataPtr->ecm->Each( + [&](const sim::Entity& _entity, + const sim::components::ForceTorque*, + const sim::components::Name* _name) -> bool + { + auto ftData = std::make_shared(); + RCLCPP_INFO_STREAM(this->nh_->get_logger(), "Loading Foot Force sensor: " << _name->Data()); + + auto sensorTopicComp = this->dataPtr->ecm->Component< + sim::components::SensorTopic>(_entity); + if (sensorTopicComp) + { + RCLCPP_INFO_STREAM(this->nh_->get_logger(), "Topic name: " << sensorTopicComp->Data()); + } + ftData->name = _name->Data(); + ftData->sim_ft_sensors_ = _entity; + this->dataPtr->state_interfaces_.emplace_back( + "foot_force", + ftData->name, + &ftData->foot_effort); + this->dataPtr->ft_sensors_.push_back(ftData); + return true; + }); } - CallbackReturn GazeboSimSystem::on_init(const hardware_interface::HardwareInfo &info) { - if (SystemInterface::on_init(info) != CallbackReturn::SUCCESS) { + CallbackReturn GazeboSimSystem::on_init(const hardware_interface::HardwareInfo& info) + { + if (SystemInterface::on_init(info) != CallbackReturn::SUCCESS) + { return CallbackReturn::ERROR; } return CallbackReturn::SUCCESS; } CallbackReturn GazeboSimSystem::on_configure( - const rclcpp_lifecycle::State & /*previous_state*/) { + const rclcpp_lifecycle::State& /*previous_state*/) + { RCLCPP_INFO( this->nh_->get_logger(), "System Successfully configured!"); return CallbackReturn::SUCCESS; } - std::vector - GazeboSimSystem::export_state_interfaces() { + std::vector GazeboSimSystem::export_state_interfaces() + { return std::move(this->dataPtr->state_interfaces_); } - std::vector - GazeboSimSystem::export_command_interfaces() { + std::vector GazeboSimSystem::export_command_interfaces() + { return std::move(this->dataPtr->command_interfaces_); } - CallbackReturn GazeboSimSystem::on_activate(const rclcpp_lifecycle::State &previous_state) { + CallbackReturn GazeboSimSystem::on_activate(const rclcpp_lifecycle::State& previous_state) + { return CallbackReturn::SUCCESS; } - CallbackReturn GazeboSimSystem::on_deactivate(const rclcpp_lifecycle::State &previous_state) { + CallbackReturn GazeboSimSystem::on_deactivate(const rclcpp_lifecycle::State& previous_state) + { return CallbackReturn::SUCCESS; } hardware_interface::return_type GazeboSimSystem::read( - const rclcpp::Time & /*time*/, - const rclcpp::Duration & /*period*/) { - - for (unsigned int i = 0; i < this->dataPtr->joints_.size(); ++i) { - if (this->dataPtr->joints_[i].sim_joint == sim::kNullEntity) { + const rclcpp::Time& /*time*/, + const rclcpp::Duration& /*period*/) + { + for (unsigned int i = 0; i < this->dataPtr->joints_.size(); ++i) + { + if (this->dataPtr->joints_[i].sim_joint == sim::kNullEntity) + { continue; } // Get the joint velocity - const auto *jointVelocity = - this->dataPtr->ecm->Component( - this->dataPtr->joints_[i].sim_joint); + const auto* jointVelocity = + this->dataPtr->ecm->Component( + this->dataPtr->joints_[i].sim_joint); // Get the joint force via joint transmitted wrench - const auto *jointWrench = - this->dataPtr->ecm->Component( - this->dataPtr->joints_[i].sim_joint); + const auto* jointWrench = + this->dataPtr->ecm->Component( + this->dataPtr->joints_[i].sim_joint); // Get the joint position - const auto *jointPositions = - this->dataPtr->ecm->Component( - this->dataPtr->joints_[i].sim_joint); + const auto* jointPositions = + this->dataPtr->ecm->Component( + this->dataPtr->joints_[i].sim_joint); this->dataPtr->joints_[i].joint_position = jointPositions->Data()[0]; this->dataPtr->joints_[i].joint_velocity = jointVelocity->Data()[0]; gz::physics::Vector3d force_or_torque; - if (this->dataPtr->joints_[i].joint_type == sdf::JointType::PRISMATIC) { + if (this->dataPtr->joints_[i].joint_type == sdf::JointType::PRISMATIC) + { force_or_torque = { jointWrench->Data().force().x(), jointWrench->Data().force().y(), jointWrench->Data().force().z() }; - } else { + } + else + { // REVOLUTE and CONTINUOUS force_or_torque = { jointWrench->Data().torque().x(), @@ -552,11 +637,14 @@ namespace gz_quadruped_hardware { }); } - for (unsigned int i = 0; i < this->dataPtr->imus_.size(); ++i) { - if (this->dataPtr->imus_[i]->topicName.empty()) { + for (unsigned int i = 0; i < this->dataPtr->imus_.size(); ++i) + { + if (this->dataPtr->imus_[i]->topicName.empty()) + { auto sensorTopicComp = this->dataPtr->ecm->Component< sim::components::SensorTopic>(this->dataPtr->imus_[i]->sim_imu_sensors_); - if (sensorTopicComp) { + if (sensorTopicComp) + { this->dataPtr->imus_[i]->topicName = sensorTopicComp->Data(); RCLCPP_INFO_STREAM( this->nh_->get_logger(), "IMU " << this->dataPtr->imus_[i]->name << @@ -568,34 +656,61 @@ namespace gz_quadruped_hardware { } } } + + for (unsigned int i = 0; i < this->dataPtr->ft_sensors_.size(); ++i) + { + if (this->dataPtr->ft_sensors_[i]->topicName.empty()) + { + auto sensorTopicComp = this->dataPtr->ecm->Component< + sim::components::SensorTopic>(this->dataPtr->ft_sensors_[i]->sim_ft_sensors_); + if (sensorTopicComp) + { + this->dataPtr->ft_sensors_[i]->topicName = sensorTopicComp->Data(); + RCLCPP_INFO_STREAM( + this->nh_->get_logger(), "ForceTorque " << this->dataPtr->ft_sensors_[i]->name << + " has a topic name: " << sensorTopicComp->Data()); + + this->dataPtr->node.Subscribe( + this->dataPtr->ft_sensors_[i]->topicName, &ForceTorqueData::OnForceTorque, + this->dataPtr->ft_sensors_[i].get()); + } + } + } + return hardware_interface::return_type::OK; } hardware_interface::return_type GazeboSimSystem::write( - const rclcpp::Time & /*time*/, - const rclcpp::Duration & /*period*/) { - for (unsigned int i = 0; i < this->dataPtr->joints_.size(); ++i) { - if (this->dataPtr->joints_[i].sim_joint == sim::kNullEntity) { + const rclcpp::Time& /*time*/, + const rclcpp::Duration& /*period*/) + { + for (unsigned int i = 0; i < this->dataPtr->joints_.size(); ++i) + { + if (this->dataPtr->joints_[i].sim_joint == sim::kNullEntity) + { continue; } if (!this->dataPtr->ecm->Component( - this->dataPtr->joints_[i].sim_joint)) { + this->dataPtr->joints_[i].sim_joint)) + { this->dataPtr->ecm->CreateComponent( this->dataPtr->joints_[i].sim_joint, sim::components::JointForceCmd({0})); - } else { + } + else + { const auto jointEffortCmd = - this->dataPtr->ecm->Component( - this->dataPtr->joints_[i].sim_joint); + this->dataPtr->ecm->Component( + this->dataPtr->joints_[i].sim_joint); const double torque = this->dataPtr->joints_[i].joint_effort_cmd + - this->dataPtr->joints_[i].joint_kp_cmd * ( - this->dataPtr->joints_[i].joint_position_cmd - - this->dataPtr->joints_[i].joint_position) - + - this->dataPtr->joints_[i].joint_kd_cmd * ( - this->dataPtr->joints_[i].joint_velocity_cmd - - this->dataPtr->joints_[i].joint_velocity); + this->dataPtr->joints_[i].joint_kp_cmd * ( + this->dataPtr->joints_[i].joint_position_cmd - + this->dataPtr->joints_[i].joint_position) + + + this->dataPtr->joints_[i].joint_kd_cmd * ( + this->dataPtr->joints_[i].joint_velocity_cmd - + this->dataPtr->joints_[i].joint_velocity); *jointEffortCmd = sim::components::JointForceCmd( {torque}); diff --git a/libraries/gz_quadruped_playground/launch/gazebo.launch.py b/libraries/gz_quadruped_playground/launch/gazebo.launch.py index 5204690..0972f64 100644 --- a/libraries/gz_quadruped_playground/launch/gazebo.launch.py +++ b/libraries/gz_quadruped_playground/launch/gazebo.launch.py @@ -120,9 +120,9 @@ def generate_launch_description(): arguments=[ "/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock", "/camera/camera_info@sensor_msgs/msg/CameraInfo@gz.msgs.CameraInfo", - "/odom@nav_msgs/msg/Odometry@gz.msgs.Odometry", - "/odom_with_covariance@nav_msgs/msg/Odometry@gz.msgs.OdometryWithCovariance", - "/tf@tf2_msgs/msg/TFMessage@gz.msgs.Pose_V" + # "/odom@nav_msgs/msg/Odometry@gz.msgs.Odometry", + # "/odom_with_covariance@nav_msgs/msg/Odometry@gz.msgs.OdometryWithCovariance", + # "/tf@tf2_msgs/msg/TFMessage@gz.msgs.Pose_V" ], output="screen", parameters=[ diff --git a/libraries/gz_quadruped_playground/package.xml b/libraries/gz_quadruped_playground/package.xml index 2a14eb4..0d0a7a0 100644 --- a/libraries/gz_quadruped_playground/package.xml +++ b/libraries/gz_quadruped_playground/package.xml @@ -10,8 +10,8 @@ xacro joint_state_publisher robot_state_publisher - leg_pd_controller imu_sensor_broadcaster + gz_quadruped_hardware ament_cmake diff --git a/libraries/gz_quadruped_playground/worlds/default.sdf b/libraries/gz_quadruped_playground/worlds/default.sdf index 5f681d9..06c7f4e 100644 --- a/libraries/gz_quadruped_playground/worlds/default.sdf +++ b/libraries/gz_quadruped_playground/worlds/default.sdf @@ -82,10 +82,6 @@ gz service -s /world/empty/create \ filename="gz-sim-scene-broadcaster-system" name="gz::sim::systems::SceneBroadcaster"> - - true