EmbodiedAI
/
quadruped_ros2_control
mirror of https://github.com/legubiao/quadruped_ros2_control.git
3
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

add gz quadruped hardware

This commit is contained in:
Huang Zhenbiao 2025-02-25 22:44:38 +08:00
parent c3797c8d6e
commit 1f1dfce57d
2 changed files with 6 additions and 94 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

65
hardwares/gz_quadruped_hardware/src/gz_quadruped_plugin.cpp
View File

@ -36,7 +36,6 @@
#include <hardware_interface/resource_manager.hpp>
#include <hardware_interface/component_parser.hpp>
#include <hardware_interface/types/hardware_interface_type_values.hpp>
#include <pluginlib/class_loader.hpp>
@ -52,7 +51,7 @@ namespace gz_quadruped_hardware {
rclcpp::Node::SharedPtr &node,
sim::EntityComponentManager &ecm,
std::map<std::string, sim::Entity> 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<int64_t>(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<bool>();
}
double position_proportional_gain = 0.1; // default
if (sdfPtr->HasElement("position_proportional_gain")) {
position_proportional_gain =
sdfPtr->GetElement("position_proportional_gain")->Get<double>();
}
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<rclcpp::executors::MultiThreadedExecutor>();
@ -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<hardware_interface::ResourceManager> resource_manager_ =
std::make_unique<GZResourceManager>(this->dataPtr->node_, _ecm, enabledJoints);

35
hardwares/gz_quadruped_hardware/src/gz_system.cpp
View File

@ -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<sim::components::JointPosition>(
this->dataPtr->joints_[mimic_joint.mimicked_joint_index].sim_joint)->Data()[0];
double position_mimic_joint =
this->dataPtr->ecm->Component<sim::components::JointPosition>(
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<sim::components::JointVelocityCmd>(
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