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"