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

hardware code cleanup

This commit is contained in:
Huang Zhenbiao 2024-09-18 22:27:16 +08:00
parent ac6e2278a1
commit 842a62465b
5 changed files with 86 additions and 85 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
hardwares/hardware_unitree_mujoco/CMakeLists.txt
View File

@ -36,7 +36,9 @@ add_library(
target_include_directories(${PROJECT_NAME} PUBLIC target_include_directories(${PROJECT_NAME} PUBLIC
"$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>" "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
"$<INSTALL_INTERFACE:include/${PROJECT_NAME}>") "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>"
PRIVATE src
)
target_link_libraries(${PROJECT_NAME} unitree_sdk2) target_link_libraries(${PROJECT_NAME} unitree_sdk2)
ament_target_dependencies( ament_target_dependencies(
${PROJECT_NAME} ${PROJECT_NAME}

12
hardwares/hardware_unitree_mujoco/include/hardware_unitree_mujoco/HardwareUnitree.h
View File

@ -45,17 +45,17 @@ protected:
}; };
void init_low_cmd(); void initLowCmd();
void low_state_message_handler(const void *messages); void lowStateMessageHandle(const void *messages);
unitree_go::msg::dds_::LowCmd_ _lowCmd{}; // default init unitree_go::msg::dds_::LowCmd_ low_cmd_{}; // default init
unitree_go::msg::dds_::LowState_ _lowState{}; // default init unitree_go::msg::dds_::LowState_ low_state_{}; // default init
/*publisher*/ /*publisher*/
unitree::robot::ChannelPublisherPtr<unitree_go::msg::dds_::LowCmd_> lowcmd_publisher; unitree::robot::ChannelPublisherPtr<unitree_go::msg::dds_::LowCmd_> low_cmd_publisher_;
/*subscriber*/ /*subscriber*/
unitree::robot::ChannelSubscriberPtr<unitree_go::msg::dds_::LowState_> lowstate_subscriber; unitree::robot::ChannelSubscriberPtr<unitree_go::msg::dds_::LowState_> lows_tate_subscriber_;
}; };

34
hardwares/hardware_unitree_mujoco/include/hardware_unitree_mujoco/crc32.h
View File

@ -1,34 +0,0 @@
//
// Created by biao on 24-7-1.
//
#ifndef STAND_GO2_CRC32_H
#define STAND_GO2_CRC32_H
#include <cstdint>
static uint32_t crc32_core(const uint32_t *ptr, uint32_t len) {
unsigned int xbit = 0;
unsigned int data = 0;
unsigned int CRC32 = 0xFFFFFFFF;
const unsigned int dwPolynomial = 0x04c11db7;
for (unsigned int i = 0; i < len; i++) {
xbit = 1 << 31;
data = ptr[i];
for (unsigned int bits = 0; bits < 32; bits++) {
if (CRC32 & 0x80000000) {
CRC32 <<= 1;
CRC32 ^= dwPolynomial;
} else {
CRC32 <<= 1;
}
if (data & xbit) CRC32 ^= dwPolynomial;
xbit >>= 1;
}
}
return CRC32;
}
#endif // STAND_GO2_CRC32_H

85
hardwares/hardware_unitree_mujoco/src/HardwareUnitree.cpp
View File

@ -2,9 +2,9 @@
// Created by biao on 24-9-9. // Created by biao on 24-9-9.
// //
#include <hardware_unitree_mujoco/crc32.h> #include "hardware_unitree_mujoco/HardwareUnitree.h"
#include <hardware_unitree_mujoco/HardwareUnitree.h>
#include <rclcpp/logging.hpp> #include <rclcpp/logging.hpp>
#include "crc32.h"
#define TOPIC_LOWCMD "rt/lowcmd" #define TOPIC_LOWCMD "rt/lowcmd"
#define TOPIC_LOWSTATE "rt/lowstate" #define TOPIC_LOWSTATE "rt/lowstate"
@ -37,21 +37,21 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Hardwa
} }
ChannelFactory::Instance()->Init(1, "lo"); ChannelFactory::Instance()->Init(1, "lo");
lowcmd_publisher = low_cmd_publisher_ =
std::make_shared<ChannelPublisher<unitree_go::msg::dds_::LowCmd_> >( std::make_shared<ChannelPublisher<unitree_go::msg::dds_::LowCmd_> >(
TOPIC_LOWCMD); TOPIC_LOWCMD);
lowcmd_publisher->InitChannel(); low_cmd_publisher_->InitChannel();
/*create subscriber*/ /*create subscriber*/
lowstate_subscriber = lows_tate_subscriber_ =
std::make_shared<ChannelSubscriber<unitree_go::msg::dds_::LowState_> >( std::make_shared<ChannelSubscriber<unitree_go::msg::dds_::LowState_> >(
TOPIC_LOWSTATE); TOPIC_LOWSTATE);
lowstate_subscriber->InitChannel( lows_tate_subscriber_->InitChannel(
[this](auto &&PH1) { [this](auto &&PH1) {
low_state_message_handler(std::forward<decltype(PH1)>(PH1)); lowStateMessageHandle(std::forward<decltype(PH1)>(PH1));
}, },
1); 1);
init_low_cmd(); initLowCmd();
return SystemInterface::on_init(info); return SystemInterface::on_init(info);
@ -109,25 +109,24 @@ std::vector<hardware_interface::CommandInterface> HardwareUnitree::export_comman
} }
return_type HardwareUnitree::read(const rclcpp::Time &time, const rclcpp::Duration &period) { return_type HardwareUnitree::read(const rclcpp::Time &time, const rclcpp::Duration &period) {
// joint states // joint states
for (int i(0); i < 12; ++i) { for (int i(0); i < 12; ++i) {
joint_position_[i] = _lowState.motor_state()[i].q(); joint_position_[i] = low_state_.motor_state()[i].q();
joint_velocities_[i] = _lowState.motor_state()[i].dq(); joint_velocities_[i] = low_state_.motor_state()[i].dq();
joint_effort_[i] = _lowState.motor_state()[i].tau_est(); joint_effort_[i] = low_state_.motor_state()[i].tau_est();
} }
// imu states // imu states
imu_states_[0] = _lowState.imu_state().quaternion()[0]; // w imu_states_[0] = low_state_.imu_state().quaternion()[0]; // w
imu_states_[1] = _lowState.imu_state().quaternion()[1]; // x imu_states_[1] = low_state_.imu_state().quaternion()[1]; // x
imu_states_[2] = _lowState.imu_state().quaternion()[2]; // y imu_states_[2] = low_state_.imu_state().quaternion()[2]; // y
imu_states_[3] = _lowState.imu_state().quaternion()[3]; // z imu_states_[3] = low_state_.imu_state().quaternion()[3]; // z
imu_states_[4] = _lowState.imu_state().gyroscope()[0]; imu_states_[4] = low_state_.imu_state().gyroscope()[0];
imu_states_[5] = _lowState.imu_state().gyroscope()[1]; imu_states_[5] = low_state_.imu_state().gyroscope()[1];
imu_states_[6] = _lowState.imu_state().gyroscope()[2]; imu_states_[6] = low_state_.imu_state().gyroscope()[2];
imu_states_[7] = _lowState.imu_state().accelerometer()[0]; imu_states_[7] = low_state_.imu_state().accelerometer()[0];
imu_states_[8] = _lowState.imu_state().accelerometer()[1]; imu_states_[8] = low_state_.imu_state().accelerometer()[1];
imu_states_[9] = _lowState.imu_state().accelerometer()[2]; imu_states_[9] = low_state_.imu_state().accelerometer()[2];
return return_type::OK; return return_type::OK;
} }
@ -135,39 +134,39 @@ return_type HardwareUnitree::read(const rclcpp::Time &time, const rclcpp::Durati
return_type HardwareUnitree::write(const rclcpp::Time &, const rclcpp::Duration &) { return_type HardwareUnitree::write(const rclcpp::Time &, const rclcpp::Duration &) {
// send command // send command
for (int i(0); i < 12; ++i) { for (int i(0); i < 12; ++i) {
_lowCmd.motor_cmd()[i].mode() = 0x01; low_cmd_.motor_cmd()[i].mode() = 0x01;
_lowCmd.motor_cmd()[i].q() = static_cast<float>(joint_position_command_[i]); low_cmd_.motor_cmd()[i].q() = static_cast<float>(joint_position_command_[i]);
_lowCmd.motor_cmd()[i].dq() = static_cast<float>(joint_velocities_command_[i]); low_cmd_.motor_cmd()[i].dq() = static_cast<float>(joint_velocities_command_[i]);
_lowCmd.motor_cmd()[i].kp() = static_cast<float>(joint_kp_command_[i]); low_cmd_.motor_cmd()[i].kp() = static_cast<float>(joint_kp_command_[i]);
_lowCmd.motor_cmd()[i].kd() = static_cast<float>(joint_kd_command_[i]); low_cmd_.motor_cmd()[i].kd() = static_cast<float>(joint_kd_command_[i]);
_lowCmd.motor_cmd()[i].tau() = static_cast<float>(joint_effort_command_[i]); low_cmd_.motor_cmd()[i].tau() = static_cast<float>(joint_effort_command_[i]);
} }
_lowCmd.crc() = crc32_core(reinterpret_cast<uint32_t *>(&_lowCmd), low_cmd_.crc() = crc32_core(reinterpret_cast<uint32_t *>(&low_cmd_),
(sizeof(unitree_go::msg::dds_::LowCmd_) >> 2) - 1); (sizeof(unitree_go::msg::dds_::LowCmd_) >> 2) - 1);
lowcmd_publisher->Write(_lowCmd); low_cmd_publisher_->Write(low_cmd_);
return return_type::OK; return return_type::OK;
} }
void HardwareUnitree::init_low_cmd() { void HardwareUnitree::initLowCmd() {
_lowCmd.head()[0] = 0xFE; low_cmd_.head()[0] = 0xFE;
_lowCmd.head()[1] = 0xEF; low_cmd_.head()[1] = 0xEF;
_lowCmd.level_flag() = 0xFF; low_cmd_.level_flag() = 0xFF;
_lowCmd.gpio() = 0; low_cmd_.gpio() = 0;
for (int i = 0; i < 20; i++) { for (int i = 0; i < 20; i++) {
_lowCmd.motor_cmd()[i].mode() = low_cmd_.motor_cmd()[i].mode() =
0x01; // motor switch to servo (PMSM) mode 0x01; // motor switch to servo (PMSM) mode
_lowCmd.motor_cmd()[i].q() = 0; low_cmd_.motor_cmd()[i].q() = 0;
_lowCmd.motor_cmd()[i].kp() = 0; low_cmd_.motor_cmd()[i].kp() = 0;
_lowCmd.motor_cmd()[i].dq() = 0; low_cmd_.motor_cmd()[i].dq() = 0;
_lowCmd.motor_cmd()[i].kd() = 0; low_cmd_.motor_cmd()[i].kd() = 0;
_lowCmd.motor_cmd()[i].tau() = 0; low_cmd_.motor_cmd()[i].tau() = 0;
} }
} }
void HardwareUnitree::low_state_message_handler(const void *messages) { void HardwareUnitree::lowStateMessageHandle(const void *messages) {
_lowState = *(unitree_go::msg::dds_::LowState_ *) messages; low_state_ = *(unitree_go::msg::dds_::LowState_ *) messages;
} }
#include "pluginlib/class_list_macros.hpp" #include "pluginlib/class_list_macros.hpp"

34
hardwares/hardware_unitree_mujoco/src/crc32.h Normal file
View File

@ -0,0 +1,34 @@
//
// Created by biao on 24-7-1.
//
#ifndef STAND_GO2_CRC32_H
#define STAND_GO2_CRC32_H
#include <cstdint>
static uint32_t crc32_core(const uint32_t *ptr, uint32_t len) {
unsigned int xbit = 0;
unsigned int data = 0;
unsigned int CRC32 = 0xFFFFFFFF;
const unsigned int dwPolynomial = 0x04c11db7;
for (unsigned int i = 0; i < len; i++) {
xbit = 1 << 31;
data = ptr[i];
for (unsigned int bits = 0; bits < 32; bits++) {
if (CRC32 & 0x80000000) {
CRC32 <<= 1;
CRC32 ^= dwPolynomial;
} else {
CRC32 <<= 1;
}
if (data & xbit) CRC32 ^= dwPolynomial;
xbit >>= 1;
}
}
return CRC32;
}
#endif // STAND_GO2_CRC32_H