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
"$<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)
ament_target_dependencies(
${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_::LowState_ _lowState{}; // default init
unitree_go::msg::dds_::LowCmd_ low_cmd_{}; // default init
unitree_go::msg::dds_::LowState_ low_state_{}; // default init
/*publisher*/
unitree::robot::ChannelPublisherPtr<unitree_go::msg::dds_::LowCmd_> lowcmd_publisher;
unitree::robot::ChannelPublisherPtr<unitree_go::msg::dds_::LowCmd_> low_cmd_publisher_;
/*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

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

@ -2,9 +2,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 "crc32.h"
#define TOPIC_LOWCMD "rt/lowcmd"
#define TOPIC_LOWSTATE "rt/lowstate"
@ -37,21 +37,21 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Hardwa
}
ChannelFactory::Instance()->Init(1, "lo");
lowcmd_publisher =
low_cmd_publisher_ =
std::make_shared<ChannelPublisher<unitree_go::msg::dds_::LowCmd_> >(
TOPIC_LOWCMD);
lowcmd_publisher->InitChannel();
low_cmd_publisher_->InitChannel();
/*create subscriber*/
lowstate_subscriber =
lows_tate_subscriber_ =
std::make_shared<ChannelSubscriber<unitree_go::msg::dds_::LowState_> >(
TOPIC_LOWSTATE);
lowstate_subscriber->InitChannel(
lows_tate_subscriber_->InitChannel(
[this](auto &&PH1) {
low_state_message_handler(std::forward<decltype(PH1)>(PH1));
lowStateMessageHandle(std::forward<decltype(PH1)>(PH1));
},
1);
init_low_cmd();
initLowCmd();
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) {
// joint states
for (int i(0); i < 12; ++i) {
joint_position_[i] = _lowState.motor_state()[i].q();
joint_velocities_[i] = _lowState.motor_state()[i].dq();
joint_effort_[i] = _lowState.motor_state()[i].tau_est();
joint_position_[i] = low_state_.motor_state()[i].q();
joint_velocities_[i] = low_state_.motor_state()[i].dq();
joint_effort_[i] = low_state_.motor_state()[i].tau_est();
}
// imu states
imu_states_[0] = _lowState.imu_state().quaternion()[0]; // w
imu_states_[1] = _lowState.imu_state().quaternion()[1]; // x
imu_states_[2] = _lowState.imu_state().quaternion()[2]; // y
imu_states_[3] = _lowState.imu_state().quaternion()[3]; // z
imu_states_[4] = _lowState.imu_state().gyroscope()[0];
imu_states_[5] = _lowState.imu_state().gyroscope()[1];
imu_states_[6] = _lowState.imu_state().gyroscope()[2];
imu_states_[7] = _lowState.imu_state().accelerometer()[0];
imu_states_[8] = _lowState.imu_state().accelerometer()[1];
imu_states_[9] = _lowState.imu_state().accelerometer()[2];
imu_states_[0] = low_state_.imu_state().quaternion()[0]; // w
imu_states_[1] = low_state_.imu_state().quaternion()[1]; // x
imu_states_[2] = low_state_.imu_state().quaternion()[2]; // y
imu_states_[3] = low_state_.imu_state().quaternion()[3]; // z
imu_states_[4] = low_state_.imu_state().gyroscope()[0];
imu_states_[5] = low_state_.imu_state().gyroscope()[1];
imu_states_[6] = low_state_.imu_state().gyroscope()[2];
imu_states_[7] = low_state_.imu_state().accelerometer()[0];
imu_states_[8] = low_state_.imu_state().accelerometer()[1];
imu_states_[9] = low_state_.imu_state().accelerometer()[2];
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 &) {
// send command
for (int i(0); i < 12; ++i) {
_lowCmd.motor_cmd()[i].mode() = 0x01;
_lowCmd.motor_cmd()[i].q() = static_cast<float>(joint_position_command_[i]);
_lowCmd.motor_cmd()[i].dq() = static_cast<float>(joint_velocities_command_[i]);
_lowCmd.motor_cmd()[i].kp() = static_cast<float>(joint_kp_command_[i]);
_lowCmd.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].mode() = 0x01;
low_cmd_.motor_cmd()[i].q() = static_cast<float>(joint_position_command_[i]);
low_cmd_.motor_cmd()[i].dq() = static_cast<float>(joint_velocities_command_[i]);
low_cmd_.motor_cmd()[i].kp() = static_cast<float>(joint_kp_command_[i]);
low_cmd_.motor_cmd()[i].kd() = static_cast<float>(joint_kd_command_[i]);
low_cmd_.motor_cmd()[i].tau() = static_cast<float>(joint_effort_command_[i]);
}
_lowCmd.crc() = crc32_core(reinterpret_cast<uint32_t *>(&_lowCmd),
(sizeof(unitree_go::msg::dds_::LowCmd_) >> 2) - 1);
lowcmd_publisher->Write(_lowCmd);
low_cmd_.crc() = crc32_core(reinterpret_cast<uint32_t *>(&low_cmd_),
(sizeof(unitree_go::msg::dds_::LowCmd_) >> 2) - 1);
low_cmd_publisher_->Write(low_cmd_);
return return_type::OK;
}
void HardwareUnitree::init_low_cmd() {
_lowCmd.head()[0] = 0xFE;
_lowCmd.head()[1] = 0xEF;
_lowCmd.level_flag() = 0xFF;
_lowCmd.gpio() = 0;
void HardwareUnitree::initLowCmd() {
low_cmd_.head()[0] = 0xFE;
low_cmd_.head()[1] = 0xEF;
low_cmd_.level_flag() = 0xFF;
low_cmd_.gpio() = 0;
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
_lowCmd.motor_cmd()[i].q() = 0;
_lowCmd.motor_cmd()[i].kp() = 0;
_lowCmd.motor_cmd()[i].dq() = 0;
_lowCmd.motor_cmd()[i].kd() = 0;
_lowCmd.motor_cmd()[i].tau() = 0;
low_cmd_.motor_cmd()[i].q() = 0;
low_cmd_.motor_cmd()[i].kp() = 0;
low_cmd_.motor_cmd()[i].dq() = 0;
low_cmd_.motor_cmd()[i].kd() = 0;
low_cmd_.motor_cmd()[i].tau() = 0;
}
}
void HardwareUnitree::low_state_message_handler(const void *messages) {
_lowState = *(unitree_go::msg::dds_::LowState_ *) messages;
void HardwareUnitree::lowStateMessageHandle(const void *messages) {
low_state_ = *(unitree_go::msg::dds_::LowState_ *) messages;
}
#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