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

tried to add foot force sensor

This commit is contained in:
Huang Zhenbiao 2025-02-26 01:18:37 +08:00
parent 1f1dfce57d
commit d8c4d8eee5
10 changed files with 883 additions and 575 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

147
.clang-tidy Normal file
View File

@ -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'

7
descriptions/unitree/go2_description/config/gazebo.yaml
View File

@ -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:

22
descriptions/unitree/go2_description/xacro/ft_sensor.xacro Normal file
View File

@ -0,0 +1,22 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:macro name="ft_sensor" params="name">
<gazebo reference="${name}_foot_fixed">
<!-- Enable feedback for this joint -->
<provideFeedback>true</provideFeedback>
<!-- Prevent ros2_control from lumping this fixed joint with others -->
<disableFixedJointLumping>true</disableFixedJointLumping>
<sensor name="${name}_ft_sensor" type="force_torque">
<always_on>1</always_on>
<update_rate>500</update_rate>
<visualize>true</visualize>
<topic>${name}_ft</topic>
<force_torque>
<frame>child</frame>
<!-- <measure_direction>child_to_parent</measure_direction>-->
</force_torque>
</sensor>
</gazebo>
</xacro:macro>
</robot>

15
descriptions/unitree/go2_description/xacro/gazebo.xacro
View File

@ -1,5 +1,6 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:include filename="$(find go2_description)/xacro/ft_sensor.xacro"/>
<ros2_control name="GazeboSystem" type="system">
<hardware>
<plugin>gz_quadruped_hardware/GazeboSimSystem</plugin>
@ -150,6 +151,14 @@
<state_interface name="linear_acceleration.y"/>
<state_interface name="linear_acceleration.z"/>
</sensor>
<sensor name="foot_force">
<state_interface name="FR_ft_sensor"/>
<state_interface name="FL_ft_sensor"/>
<state_interface name="RR_ft_sensor"/>
<state_interface name="RL_ft_sensor"/>
</sensor>
</ros2_control>
<gazebo>
@ -159,6 +168,7 @@
<plugin filename="gz-sim-imu-system"
name="gz::sim::systems::Imu">
</plugin>
<plugin filename="gz-sim-forcetorque-system" name="gz::sim::systems::ForceTorque"/>
<plugin filename="gz-sim-odometry-publisher-system"
name="gz::sim::systems::OdometryPublisher">
<odom_frame>odom</odom_frame>
@ -244,6 +254,11 @@
<disableFixedJointLumping>true</disableFixedJointLumping>
</gazebo>
<xacro:ft_sensor name="FR"/>
<xacro:ft_sensor name="FL"/>
<xacro:ft_sensor name="RR"/>
<xacro:ft_sensor name="RL"/>
<xacro:arg name="EXTERNAL_SENSORS" default="false"/>
<xacro:if value="$(arg EXTERNAL_SENSORS)">
<gazebo reference="front_camera">

2
descriptions/unitree/go2_description/xacro/leg.xacro
View File

@ -143,7 +143,7 @@
</link>
<joint name="${name}_foot_fixed" type="fixed">0
<joint name="${name}_foot_fixed" type="fixed">
<origin rpy="0 0 0" xyz="0 0 ${-(calf_length)}"/>
<parent link="${name}_calf"/>
<child link="${name}_foot"/>

8
hardwares/gz_quadruped_hardware/README.md Normal file
View File

@ -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
```

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

@ -15,6 +15,7 @@
#include "gz_quadruped_hardware/gz_system.hpp"
#include <gz/msgs/imu.pb.h>
#include <gz/msgs/wrench.pb.h>
#include <limits>
#include <map>
@ -32,7 +33,7 @@
#include <gz/sim/components/JointPositionReset.hh>
#include <gz/sim/components/JointTransmittedWrench.hh>
#include <gz/sim/components/JointType.hh>
#include <gz/sim/components/JointVelocityCmd.hh>
#include <gz/sim/components/ForceTorque.hh>
#include <gz/sim/components/JointVelocity.hh>
#include <gz/sim/components/JointVelocityReset.hh>
#include <gz/sim/components/Name.hh>
@ -46,7 +47,8 @@
#include <hardware_interface/lexical_casts.hpp>
#include <hardware_interface/types/hardware_interface_type_values.hpp>
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{};
@ -106,7 +109,8 @@ public:
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<double, 6> 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;
@ -137,6 +175,9 @@ public:
/// \brief vector with the imus .
std::vector<std::shared_ptr<ImuData>> imus_;
/// \brief vector with the foot force-torque sensors.
std::vector<std::shared_ptr<ForceTorqueData>> ft_sensors_;
/// \brief state interfaces that will be exported to the Resource Manager
std::vector<hardware_interface::StateInterface> state_interfaces_;
@ -152,18 +193,17 @@ public:
/// \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<std::string, sim::Entity>& enableJoints,
const hardware_interface::HardwareInfo& hardware_info,
sim::EntityComponentManager& _ecm,
unsigned int update_rate) {
unsigned int update_rate)
{
this->dataPtr = std::make_unique<GazeboSimSystemPrivate>();
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++) {
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<double>::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<hardware_interface::ComponentInfo> 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<sim::components::Imu,
sim::components::Name>(
[&](const sim::Entity& _entity,
const sim::components::Imu*,
const sim::components::Name *_name) -> bool {
const sim::components::Name* _name) -> bool
{
auto imuData = std::make_shared<ImuData>();
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,47 +516,80 @@ namespace gz_quadruped_hardware {
this->dataPtr->imus_.push_back(imuData);
return true;
});
// Foot Force torque sensor
this->dataPtr->ecm->Each<sim::components::ForceTorque,
sim::components::Name>(
[&](const sim::Entity& _entity,
const sim::components::ForceTorque*,
const sim::components::Name* _name) -> bool
{
auto ftData = std::make_shared<ForceTorqueData>();
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<hardware_interface::StateInterface>
GazeboSimSystem::export_state_interfaces() {
std::vector<hardware_interface::StateInterface> GazeboSimSystem::export_state_interfaces()
{
return std::move(this->dataPtr->state_interfaces_);
}
std::vector<hardware_interface::CommandInterface>
GazeboSimSystem::export_command_interfaces() {
std::vector<hardware_interface::CommandInterface> 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::Duration& /*period*/)
{
for (unsigned int i = 0; i < this->dataPtr->joints_.size(); ++i)
{
if (this->dataPtr->joints_[i].sim_joint == sim::kNullEntity)
{
continue;
}
@ -529,13 +611,16 @@ namespace gz_quadruped_hardware {
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,22 +656,49 @@ 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::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<sim::components::JointForceCmd>(
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<sim::components::JointForceCmd>(
this->dataPtr->joints_[i].sim_joint);

6
libraries/gz_quadruped_playground/launch/gazebo.launch.py
View File

@ -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=[

2
libraries/gz_quadruped_playground/package.xml
View File

@ -10,8 +10,8 @@
<exec_depend>xacro</exec_depend>
<exec_depend>joint_state_publisher</exec_depend>
<exec_depend>robot_state_publisher</exec_depend>
<exec_depend>leg_pd_controller</exec_depend>
<exec_depend>imu_sensor_broadcaster</exec_depend>
<exec_depend>gz_quadruped_hardware</exec_depend>
<export>
<build_type>ament_cmake</build_type>

4
libraries/gz_quadruped_playground/worlds/default.sdf
View File

@ -82,10 +82,6 @@ gz service -s /world/empty/create \
filename="gz-sim-scene-broadcaster-system"
name="gz::sim::systems::SceneBroadcaster">
</plugin>
<plugin
filename="gz-sim-contact-system"
name="gz::sim::systems::Contact">
</plugin>
<light type="directional" name="sun">
<cast_shadows>true</cast_shadows>