From 1f1dfce57dc50f922df02c3b886260520eac86f9 Mon Sep 17 00:00:00 2001 From: Huang Zhenbiao Date: Tue, 25 Feb 2025 22:44:38 +0800 Subject: [PATCH] add gz quadruped hardware --- .../src/gz_quadruped_plugin.cpp | 65 +------------------ .../gz_quadruped_hardware/src/gz_system.cpp | 35 +--------- 2 files changed, 6 insertions(+), 94 deletions(-) diff --git a/hardwares/gz_quadruped_hardware/src/gz_quadruped_plugin.cpp b/hardwares/gz_quadruped_hardware/src/gz_quadruped_plugin.cpp index d8e270e..8c1ec99 100644 --- a/hardwares/gz_quadruped_hardware/src/gz_quadruped_plugin.cpp +++ b/hardwares/gz_quadruped_hardware/src/gz_quadruped_plugin.cpp @@ -36,7 +36,6 @@ #include #include -#include #include @@ -52,7 +51,7 @@ namespace gz_quadruped_hardware { rclcpp::Node::SharedPtr &node, sim::EntityComponentManager &ecm, std::map enabledJoints) - : hardware_interface::ResourceManager( + : ResourceManager( node->get_node_clock_interface(), node->get_node_logging_interface()), gz_system_loader_("gz_quadruped_hardware", "gz_quadruped_hardware::GazeboSimSystemInterface"), logger_(node->get_logger().get_child("GZResourceManager")) { @@ -160,7 +159,7 @@ namespace gz_quadruped_hardware { /// \brief Last time the update method was called rclcpp::Time last_update_sim_time_ros_ = - rclcpp::Time((int64_t) 0, RCL_ROS_TIME); + rclcpp::Time(static_cast(0), RCL_ROS_TIME); /// \brief ECM pointer sim::EntityComponentManager *ecm{nullptr}; @@ -295,18 +294,6 @@ namespace gz_quadruped_hardware { std::string ns = "/"; - // Hold joints if no control mode is active? - bool hold_joints = true; // default - if (sdfPtr->HasElement("hold_joints")) { - hold_joints = - sdfPtr->GetElement("hold_joints")->Get(); - } - double position_proportional_gain = 0.1; // default - if (sdfPtr->HasElement("position_proportional_gain")) { - position_proportional_gain = - sdfPtr->GetElement("position_proportional_gain")->Get(); - } - if (sdfPtr->HasElement("ros")) { sdf::ElementPtr sdfRos = sdfPtr->GetElement("ros"); @@ -344,7 +331,7 @@ namespace gz_quadruped_hardware { rclcpp::SignalHandlerOptions::None); } - std::string node_name = "gz_quadruped"; + std::string node_name = "gz_quadruped_control"; this->dataPtr->node_ = rclcpp::Node::make_shared(node_name, ns); this->dataPtr->executor_ = std::make_shared(); @@ -370,52 +357,6 @@ namespace gz_quadruped_hardware { return; } - try { - this->dataPtr->node_->declare_parameter("hold_joints", rclcpp::ParameterValue(hold_joints)); - } catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException &e) { - RCLCPP_ERROR( - this->dataPtr->node_->get_logger(), "Parameter 'hold_joints' has already been declared, %s", - e.what()); - } catch (const rclcpp::exceptions::InvalidParametersException &e) { - RCLCPP_ERROR( - this->dataPtr->node_->get_logger(), "Parameter 'hold_joints' has invalid name, %s", - e.what()); - } catch (const rclcpp::exceptions::InvalidParameterValueException &e) { - RCLCPP_ERROR( - this->dataPtr->node_->get_logger(), "Parameter 'hold_joints' value is invalid, %s", - e.what()); - } catch (const rclcpp::exceptions::InvalidParameterTypeException &e) { - RCLCPP_ERROR( - this->dataPtr->node_->get_logger(), "Parameter 'hold_joints' value has wrong type, %s", - e.what()); - } - - try { - this->dataPtr->node_->declare_parameter( - "position_proportional_gain", - rclcpp::ParameterValue(position_proportional_gain)); - } catch (const rclcpp::exceptions::ParameterAlreadyDeclaredException &e) { - RCLCPP_ERROR( - this->dataPtr->node_->get_logger(), - "Parameter 'position_proportional_gain' has already been declared, %s", - e.what()); - } catch (const rclcpp::exceptions::InvalidParametersException &e) { - RCLCPP_ERROR( - this->dataPtr->node_->get_logger(), - "Parameter 'position_proportional_gain' has invalid name, %s", - e.what()); - } catch (const rclcpp::exceptions::InvalidParameterValueException &e) { - RCLCPP_ERROR( - this->dataPtr->node_->get_logger(), - "Parameter 'position_proportional_gain' value is invalid, %s", - e.what()); - } catch (const rclcpp::exceptions::InvalidParameterTypeException &e) { - RCLCPP_ERROR( - this->dataPtr->node_->get_logger(), - "Parameter 'position_proportional_gain' value has wrong type, %s", - e.what()); - } - std::unique_ptr resource_manager_ = std::make_unique(this->dataPtr->node_, _ecm, enabledJoints); diff --git a/hardwares/gz_quadruped_hardware/src/gz_system.cpp b/hardwares/gz_quadruped_hardware/src/gz_system.cpp index f326090..2fba691 100644 --- a/hardwares/gz_quadruped_hardware/src/gz_system.cpp +++ b/hardwares/gz_quadruped_hardware/src/gz_system.cpp @@ -394,7 +394,7 @@ namespace gz_quadruped_hardware { } // check if joint is actuated (has command interfaces) or passive - this->dataPtr->joints_[j].is_actuated = (joint_info.command_interfaces.size() > 0); + this->dataPtr->joints_[j].is_actuated = joint_info.command_interfaces.size() > 0; } registerSensors(hardware_info); @@ -469,8 +469,7 @@ namespace gz_quadruped_hardware { }); } - CallbackReturn - GazeboSimSystem::on_init(const hardware_interface::HardwareInfo &info) { + CallbackReturn GazeboSimSystem::on_init(const hardware_interface::HardwareInfo &info) { if (SystemInterface::on_init(info) != CallbackReturn::SUCCESS) { return CallbackReturn::ERROR; } @@ -506,6 +505,7 @@ namespace gz_quadruped_hardware { 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) { continue; @@ -602,35 +602,6 @@ namespace gz_quadruped_hardware { } } - // set values of all mimic joints with respect to mimicked joint - for (const auto &mimic_joint: this->info_.mimic_joints) { - // Get the joint position - double position_mimicked_joint = - this->dataPtr->ecm->Component( - this->dataPtr->joints_[mimic_joint.mimicked_joint_index].sim_joint)->Data()[0]; - - double position_mimic_joint = - this->dataPtr->ecm->Component( - this->dataPtr->joints_[mimic_joint.joint_index].sim_joint)->Data()[0]; - - double position_error = - position_mimic_joint - position_mimicked_joint * mimic_joint.multiplier; - - double velocity_sp = (-1.0) * position_error * this->dataPtr->update_rate; - - auto vel = - this->dataPtr->ecm->Component( - this->dataPtr->joints_[mimic_joint.joint_index].sim_joint); - - if (vel == nullptr) { - this->dataPtr->ecm->CreateComponent( - this->dataPtr->joints_[mimic_joint.joint_index].sim_joint, - sim::components::JointVelocityCmd({velocity_sp})); - } else if (!vel->Data().empty()) { - vel->Data()[0] = velocity_sp; - } - } - return hardware_interface::return_type::OK; } } // namespace gz_quadruped_hardware