diff --git a/descriptions/go2_description/xacro/ros2_control.xacro b/descriptions/go2_description/xacro/ros2_control.xacro
index ca6a705..a2e4209 100644
--- a/descriptions/go2_description/xacro/ros2_control.xacro
+++ b/descriptions/go2_description/xacro/ros2_control.xacro
@@ -164,6 +164,13 @@
+
+
+
+
+
+
+
diff --git a/hardwares/hardware_unitree_mujoco/README.md b/hardwares/hardware_unitree_mujoco/README.md
index 95eecb3..cc1ae2a 100644
--- a/hardwares/hardware_unitree_mujoco/README.md
+++ b/hardwares/hardware_unitree_mujoco/README.md
@@ -4,6 +4,28 @@ 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/unitreerobotics/unitree_mujoco).
+## 1. Interfaces
+
+Required hardware interfaces:
+
+* command:
+ * joint position
+ * joint velocity
+ * joint effort
+ * KP
+ * KD
+* state:
+ * joint effort
+ * joint position
+ * joint velocity
+ * imu sensor
+ * linear acceleration
+ * angular velocity
+ * orientation
+ * foot force sensor
+
+## 2. Build
+
Tested environment:
* Ubuntu 24.04
* ROS2 Jazzy
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 a40ce9c..9b8129f 100644
--- a/hardwares/hardware_unitree_mujoco/include/hardware_unitree_mujoco/HardwareUnitree.h
+++ b/hardwares/hardware_unitree_mujoco/include/hardware_unitree_mujoco/HardwareUnitree.h
@@ -37,6 +37,7 @@ protected:
std::vector joint_effort_;
std::vector imu_states_;
+ std::vector foot_force_;
std::unordered_map > joint_interfaces = {
{"position", {}},
diff --git a/hardwares/hardware_unitree_mujoco/src/HardwareUnitree.cpp b/hardwares/hardware_unitree_mujoco/src/HardwareUnitree.cpp
index 85d442e..4772a52 100644
--- a/hardwares/hardware_unitree_mujoco/src/HardwareUnitree.cpp
+++ b/hardwares/hardware_unitree_mujoco/src/HardwareUnitree.cpp
@@ -29,6 +29,7 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Hardwa
joint_effort_.assign(12, 0);
imu_states_.assign(10, 0);
+ foot_force_.assign(4, 0);
for (const auto &joint: info_.joints) {
for (const auto &interface: joint.state_interfaces) {
@@ -81,6 +82,12 @@ std::vector HardwareUnitree::export_state_in
info_.sensors[0].name, info_.sensors[0].state_interfaces[i].name, &imu_states_[i]);
}
+ // 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]);
+ }
+
return state_interfaces;
}
@@ -128,6 +135,12 @@ return_type HardwareUnitree::read(const rclcpp::Time & /*time*/, const rclcpp::D
imu_states_[8] = low_state_.imu_state().accelerometer()[1];
imu_states_[9] = low_state_.imu_state().accelerometer()[2];
+ // contact states
+ foot_force_[0] = low_state_.foot_force()[0];
+ foot_force_[1] = low_state_.foot_force()[1];
+ foot_force_[2] = low_state_.foot_force()[2];
+ foot_force_[3] = low_state_.foot_force()[3];
+
return return_type::OK;
}
@@ -166,7 +179,7 @@ void HardwareUnitree::initLowCmd() {
}
void HardwareUnitree::lowStateMessageHandle(const void *messages) {
- low_state_ = *(unitree_go::msg::dds_::LowState_ *) messages;
+ low_state_ = *static_cast(messages);
}
#include "pluginlib/class_list_macros.hpp"