From 952b75cea115eaf0f1f94bb96ee6ee3a5ad649fa Mon Sep 17 00:00:00 2001 From: xiaoliangstd <2303335747@qq.com> Date: Thu, 24 Oct 2024 10:39:54 +0800 Subject: [PATCH] update b2w example --- example/CMakeLists.txt | 3 +- example/b2w/CMakeLists.txt | 5 + example/b2w/b2w_sport_client.cpp | 161 +++++++++++++ example/b2w/b2w_stand_example.cpp | 372 ++++++++++++++++++++++++++++++ 4 files changed, 540 insertions(+), 1 deletion(-) create mode 100644 example/b2w/CMakeLists.txt create mode 100644 example/b2w/b2w_sport_client.cpp create mode 100644 example/b2w/b2w_stand_example.cpp diff --git a/example/CMakeLists.txt b/example/CMakeLists.txt index aa4ef2a..3b07085 100644 --- a/example/CMakeLists.txt +++ b/example/CMakeLists.txt @@ -8,4 +8,5 @@ add_subdirectory(go2) add_subdirectory(b2) add_subdirectory(h1) add_subdirectory(g1) -add_subdirectory(go2w) \ No newline at end of file +add_subdirectory(go2w) +add_subdirectory(b2w) \ No newline at end of file diff --git a/example/b2w/CMakeLists.txt b/example/b2w/CMakeLists.txt new file mode 100644 index 0000000..5fc6263 --- /dev/null +++ b/example/b2w/CMakeLists.txt @@ -0,0 +1,5 @@ +add_executable(b2w_sport_client b2w_sport_client.cpp) +target_link_libraries(b2w_sport_client unitree_sdk2) + +add_executable(b2w_stand_example b2w_stand_example.cpp) +target_link_libraries(b2w_stand_example unitree_sdk2) diff --git a/example/b2w/b2w_sport_client.cpp b/example/b2w/b2w_sport_client.cpp new file mode 100644 index 0000000..b25f114 --- /dev/null +++ b/example/b2w/b2w_sport_client.cpp @@ -0,0 +1,161 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#define TOPIC_HIGHSTATE "rt/lf/sportmodestate" + +using namespace std; + +struct TestOption +{ + std::string name; + int id; +}; + +const vector option_list = + {{"damp", 0}, + {"stand_up", 1}, + {"stand_down", 2}, + {"move", 3}, + {"stop_move", 4}, + {"switch_gait", 5}, + {"switch_gait", 6}, + {"get_state", 6}, + {"recovery", 7}, + }; + +int ConvertToInt(const std::string &str) +{ + try + { + std::stoi(str); + return std::stoi(str); + } + catch (const std::invalid_argument &) + { + return -1; + } + catch (const std::out_of_range &) + { + return -1; + } +} + +class UserInterface +{ +public: + UserInterface(){}; + ~UserInterface(){}; + + void terminalHandle() + { + std::string input; + std::getline(std::cin, input); + + if (input.compare("list") == 0) + { + for (TestOption option : option_list) + { + std::cout << option.name << ", id: " << option.id << std::endl; + } + } + + for (TestOption option : option_list) + { + if (input.compare(option.name) == 0 || ConvertToInt(input) == option.id) + { + test_option_->id = option.id; + test_option_->name = option.name; + std::cout << "Test: " << test_option_->name << ", test_id: " << test_option_->id << std::endl; + } + } + }; + + TestOption *test_option_; +}; + +int main(int argc, char **argv) +{ + if (argc < 2) + { + std::cout << "Usage: " << argv[0] << " networkInterface" << std::endl; + exit(-1); + } + unitree::robot::ChannelFactory::Instance()->Init(0, argv[1]); + + TestOption test_option; + test_option.id = 1; + + unitree::robot::b2::SportClient sport_client; + sport_client.SetTimeout(10.0f); + sport_client.Init(); + + UserInterface user_interface; + user_interface.test_option_ = &test_option; + + std::cout << "Input \"list \" to list all test option ..." << std::endl; + long res_count = 0; + while (1) + { + auto time_start_trick = std::chrono::high_resolution_clock::now(); + static const constexpr auto dt = std::chrono::microseconds(20000); // 50Hz + + user_interface.terminalHandle(); + + int res = 1; + if (test_option.id == 0) + { + res = sport_client.Damp(); + } + else if (test_option.id == 1) + { + res = sport_client.StandUp(); + } + else if (test_option.id == 2) + { + res = sport_client.StandDown(); + } + else if (test_option.id == 3) + { + res = sport_client.Move(0.2, 0, 0);// the robot will move for 0.5 seconds before stopping. + // If the Move command is called in a loop, the robot will continue moving. + // If no Move request is received within 0.5 seconds, the robot will automatically stop. + } + else if (test_option.id == 4) + { + res = sport_client.StopMove(); + } + else if (test_option.id == 5) + { + res = sport_client.SwitchGait(0); + } + else if (test_option.id == 6) + { + res = sport_client.SwitchGait(1); + } + else if (test_option.id == 7) + { + res = sport_client.RecoveryStand(); + } + + if (res < 0) + { + res_count += 1; + std::cout << "Request error for: " << option_list[test_option.id].name << ", code: " << res << ", count: " << res_count << std::endl; + } + else + { + res_count = 0; + std::cout << "Request successed: " << option_list[test_option.id].name << ", code: " << res << std::endl; + } + std::this_thread::sleep_until(time_start_trick + dt); + } + return 0; +} \ No newline at end of file diff --git a/example/b2w/b2w_stand_example.cpp b/example/b2w/b2w_stand_example.cpp new file mode 100644 index 0000000..8e0f257 --- /dev/null +++ b/example/b2w/b2w_stand_example.cpp @@ -0,0 +1,372 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace unitree::common; +using namespace unitree::robot; +using namespace unitree::robot::b2; + +#define TOPIC_LOWCMD "rt/lowcmd" +#define TOPIC_LOWSTATE "rt/lowstate" + +constexpr double PosStopF = (2.146E+9f); +constexpr double VelStopF = (16000.0f); + +class Custom +{ +public: + explicit Custom(){} + ~Custom(){} + + void Init(); + void Start(); + +private: + void InitLowCmd(); + void LowStateMessageHandler(const void* messages); + void LowCmdWrite(); + int queryMotionStatus(); + std::string queryServiceName(std::string form,std::string name); + +private: + float Kp = 1000.0; + float Kd = 10.0; + double time_consume = 0; + int rate_count = 0; + int sin_count = 0; + int motiontime = 0; + float dt = 0.002; // 0.001~0.01 + + MotionSwitcherClient msc; + SportClient sc; + + unitree_go::msg::dds_::LowCmd_ low_cmd{}; // default init + unitree_go::msg::dds_::LowState_ low_state{}; // default init + + /*publisher*/ + ChannelPublisherPtr lowcmd_publisher; + /*subscriber*/ + ChannelSubscriberPtr lowstate_subscriber; + + /*LowCmd write thread*/ + ThreadPtr lowCmdWriteThreadPtr; + + float _targetPos_1[12] = {0.0, 1.36, -2.65, 0.0, 1.36, -2.65, + -0.2, 1.36, -2.65, 0.2, 1.36, -2.65}; + + float _targetPos_2[12] = {0.0, 0.67, -1.3, 0.0, 0.67, -1.3, + 0.0, 0.67, -1.3, 0.0, 0.67, -1.3}; + + float _targetPos_3[12] = {-0.65, 1.36, -2.65, 0.65, 1.36, -2.65, + -0.65, 1.36, -2.65, 0.65, 1.36, -2.65}; + + float _startPos[12]; + float _duration_1 = 800; + float _duration_2 = 800; + float _duration_3 = 2000; + float _duration_4 = 1500; + float _percent_1 = 0; + float _percent_2 = 0; + float _percent_3 = 0; + float _percent_4 = 0; + + bool firstRun = true; + bool done = false; +}; + +uint32_t crc32_core(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; +} + +void Custom::Init() +{ + InitLowCmd(); + + /*create publisher*/ + lowcmd_publisher.reset(new ChannelPublisher(TOPIC_LOWCMD)); + lowcmd_publisher->InitChannel(); + + /*create subscriber*/ + lowstate_subscriber.reset(new ChannelSubscriber(TOPIC_LOWSTATE)); + lowstate_subscriber->InitChannel(std::bind(&Custom::LowStateMessageHandler, this, std::placeholders::_1), 1); + + /*init MotionSwitcherClient*/ + msc.SetTimeout(10.0f); + msc.Init(); + + /*Shut down motion control-related service*/ + while(queryMotionStatus()) + { + std::cout << "Try to deactivate the motion control-related service." << std::endl; + int32_t ret = msc.ReleaseMode(); + if (ret == 0) { + std::cout << "ReleaseMode succeeded." << std::endl; + } else { + std::cout << "ReleaseMode failed. Error code: " << ret << std::endl; + } + sleep(5); + } +} + +void Custom::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++) + { + low_cmd.motor_cmd()[i].mode() = (0x01); // motor switch to servo (PMSM) mode + low_cmd.motor_cmd()[i].q() = (PosStopF); + low_cmd.motor_cmd()[i].kp() = (0); + low_cmd.motor_cmd()[i].dq() = (VelStopF); + low_cmd.motor_cmd()[i].kd() = (0); + low_cmd.motor_cmd()[i].tau() = (0); + } +} + +int Custom::queryMotionStatus() +{ + std::string robotForm,motionName; + int motionStatus; + int32_t ret = msc.CheckMode(robotForm,motionName); + if (ret == 0) { + std::cout << "CheckMode succeeded." << std::endl; + } else { + std::cout << "CheckMode failed. Error code: " << ret << std::endl; + } + if(motionName.empty()) + { + std::cout << "The motion control-related service is deactivated." << std::endl; + motionStatus = 0; + } + else + { + std::string serviceName = queryServiceName(robotForm,motionName); + std::cout << "Service: "<< serviceName<< " is activate" << std::endl; + motionStatus = 1; + } + return motionStatus; +} + +std::string Custom::queryServiceName(std::string form,std::string name) +{ + if(form == "0") + { + if(name == "normal" ) return "sport_mode"; + if(name == "ai" ) return "ai_sport"; + if(name == "advanced" ) return "advanced_sport"; + } + else + { + if(name == "ai-w" ) return "wheeled_sport(go2W)"; + if(name == "normal-w" ) return "wheeled_sport(b2W)"; + } + return ""; +} + +void Custom::Start() +{ + /*loop publishing thread*/ + lowCmdWriteThreadPtr = CreateRecurrentThreadEx("writebasiccmd", UT_CPU_ID_NONE, 2000, &Custom::LowCmdWrite, this); +} + +void Custom::LowStateMessageHandler(const void* message) +{ + low_state = *(unitree_go::msg::dds_::LowState_*)message; +} + +void Custom::LowCmdWrite() +{ + if(_percent_4<1) + { + std::cout<<"Read sensor data example: "<=500) + { + if(firstRun) + { + for(int i = 0; i < 12; i++) + { + _startPos[i] = low_state.motor_state()[i].q(); + } + firstRun = false; + } + + _percent_1 += (float)1 / _duration_1; + _percent_1 = _percent_1 > 1 ? 1 : _percent_1; + if (_percent_1 < 1) + { + for (int j = 0; j < 12; j++) + { + low_cmd.motor_cmd()[j].q() = (1 - _percent_1) * _startPos[j] + _percent_1 * _targetPos_1[j]; + low_cmd.motor_cmd()[j].dq() = 0; + low_cmd.motor_cmd()[j].kp() = Kp; + low_cmd.motor_cmd()[j].kd() = Kd; + low_cmd.motor_cmd()[j].tau() = 0; + } + + } + if ((_percent_1 == 1)&&(_percent_2 < 1)) + { + _percent_2 += (float)1 / _duration_2; + _percent_2 = _percent_2 > 1 ? 1 : _percent_2; + + for (int j = 0; j < 12; j++) + { + low_cmd.motor_cmd()[j].q() = (1 - _percent_2) * _targetPos_1[j] + _percent_2 * _targetPos_2[j]; + low_cmd.motor_cmd()[j].dq() = 0; + low_cmd.motor_cmd()[j].kp() = Kp; + low_cmd.motor_cmd()[j].kd() = Kd; + low_cmd.motor_cmd()[j].tau() = 0; + } + } + + if ((_percent_1 == 1)&&(_percent_2 == 1)&&(_percent_3<1)) + { + _percent_3 += (float)1 / _duration_3; + _percent_3 = _percent_3 > 1 ? 1 : _percent_3; + + for (int j = 0; j < 12; j++) + { + low_cmd.motor_cmd()[j].q() = _targetPos_2[j]; + low_cmd.motor_cmd()[j].dq() = 0; + low_cmd.motor_cmd()[j].kp() = Kp; + low_cmd.motor_cmd()[j].kd() = Kd; + low_cmd.motor_cmd()[j].tau() = 0; + } + if(_percent_3<0.4) + { + for (int j = 12; j < 16; j++) + { + low_cmd.motor_cmd()[j].q() = 0; + low_cmd.motor_cmd()[j].kp() = 0; + low_cmd.motor_cmd()[j].dq() = 3; + + low_cmd.motor_cmd()[j].kd() = Kd; + low_cmd.motor_cmd()[j].tau() = 0; + } + + } + else if((_percent_3>=0.4)&&(_percent_3<0.8)) + { + for (int j = 12; j < 16; j++) + { + low_cmd.motor_cmd()[j].q() = 0; + low_cmd.motor_cmd()[j].kp() = 0; + low_cmd.motor_cmd()[j].dq() = -3; + + low_cmd.motor_cmd()[j].kd() = Kd; + low_cmd.motor_cmd()[j].tau() = 0; + } + + } + else if(_percent_3>=0.8) + { + for (int j = 12; j < 16; j++) + { + low_cmd.motor_cmd()[j].q() = 0; + low_cmd.motor_cmd()[j].kp() = 0; + low_cmd.motor_cmd()[j].dq() = 0; + + low_cmd.motor_cmd()[j].kd() = Kd; + low_cmd.motor_cmd()[j].tau() = 0; + } + } + + } + if ((_percent_1 == 1)&&(_percent_2 == 1)&&(_percent_3==1)&&((_percent_4<=1))) + { + _percent_4 += (float)1 / _duration_4; + _percent_4 = _percent_4 > 1 ? 1 : _percent_4; + for (int j = 0; j < 12; j++) + { + low_cmd.motor_cmd()[j].q() = (1 - _percent_4) * _targetPos_2[j] + _percent_4 * _targetPos_3[j]; + low_cmd.motor_cmd()[j].dq() = 0; + low_cmd.motor_cmd()[j].kp() = Kp; + low_cmd.motor_cmd()[j].kd() = Kd; + low_cmd.motor_cmd()[j].tau() = 0; + } + } + low_cmd.crc() = crc32_core((uint32_t *)&low_cmd, (sizeof(unitree_go::msg::dds_::LowCmd_)>>2)-1); + + lowcmd_publisher->Write(low_cmd); + } + +} + +int main(int argc, const char** argv) +{ + if (argc < 2) + { + std::cout << "Usage: " << argv[0] << " networkInterface" << std::endl; + exit(-1); + } + + std::cout << "WARNING: Make sure the robot is hung up or lying on the ground." << std::endl + << "Press Enter to continue..." << std::endl; + std::cin.ignore(); + + ChannelFactory::Instance()->Init(0, argv[1]); + + Custom custom; + custom.Init(); + custom.Start(); + + while (1) + { + sleep(10); + } + + return 0; +}