commit 5221e95827ab443589a639cb58397831dfcd8f7d Author: David Zhai <1003429115@qq.com> Date: Wed Jul 20 11:20:01 2022 +0800 first commit diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..c795b05 --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +build \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100755 index 0000000..0600e65 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,35 @@ +cmake_minimum_required(VERSION 3.0) +project(z1_sdk) + +# set(EIGEN_PATH /usr/include/eigen3) + +include_directories( + include + # ${EIGEN_PATH} +) + +link_directories(lib) + +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11 -O3 -pthread") + +# file(GLOB_RECURSE ADD_SRC_LIST +# src/*.cpp +# ) + +# set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/../lib) # SHARED:CMAKE_LIBRARY_OUTPUT_DIRECTORY STATIC:CMAKE_ARCHIVE_OUTPUT_DIRECTORY +# add_library(Z1_SDK_Linux64 SHARED ${ADD_SRC_LIST}) + +add_executable(example_lowCmd_send examples/example_lowCmd_send.cpp) +target_link_libraries(example_lowCmd_send Z1_SDK_Linux64) + +add_executable(example_state_send examples/example_state_send.cpp) +target_link_libraries(example_state_send Z1_SDK_Linux64) + +add_executable(example_state_recv examples/example_state_recv.cpp) +target_link_libraries(example_state_recv Z1_SDK_Linux64) + +add_executable(example_keyboard_recv examples/example_keyboard_recv.cpp) +target_link_libraries(example_keyboard_recv Z1_SDK_Linux64) + +add_executable(example_keyboard_send examples/example_keyboard_send.cpp) +target_link_libraries(example_keyboard_send Z1_SDK_Linux64) \ No newline at end of file diff --git a/README.md b/README.md new file mode 100644 index 0000000..c16d2d0 --- /dev/null +++ b/README.md @@ -0,0 +1,76 @@ +# v3.6.1 +The z1_sdk is mainly used for communication between z1_controller and command panel(keyboard, joystick, user control panel etc). + +## Notice + +support robot: Z1(v3.6.1) +not support robot: Z1(v3.4, v3.5) + +## Dependencies + +- build-essential + +```bash +sudo apt install build-essential +``` + +- Boost (version 1.5.4 or higher) + +```bash +dpkg -S /usr/include/boost/version.hpp # check boost version +sudo apt install libboost-dev # install boost +``` + +- CMake (version 2.8.3 or higher) + +```bash +cmake --version # check cmake version +sudo apt install cmake # install cmake +``` + +- [Eigen](https://gitlab.com/libeigen/eigen/-/releases/3.3.9) (version 3.3.9 or higher) + +```bash +cd eigen-3.3.9 +mkdir build && cd build +cmake .. +sudo make install +sudo ln -s /usr/local/include/eigen3 /usr/include/eigen3 +sudo ln -s /usr/local/include/eigen3/Eigen /usr/local/include/Eigen +``` + +## Example + +There are three examples of command panel control based on SDK in this project, you can use it fllowing the steps below. + +### State change + +- First, set(CTRL_PANEL SDK) # z1_ws/src/z1_controller/CMakeList.txt，and then rebuild the z1_ws to generate z1_ctrl, then open a teminal to run z1_ctrl + + ``` + cd //z1_controller/build + sudo ./z1_ctrl + ``` + +- Sencond, build the z1_sdk, and then open another terminal to run example. + + ``` + cd //z1_sdk && mkdir build && cd build + cmake .. + make -j4 + ./example_state_send + ``` + +### Low level control + +``` +sudo ./z1_ctrl # Running in a terminal +./example_lowCmd_send # Running in another terminal +``` + +### Keyboard control + +``` +sudo ./z1_ctrl # Running in a terminal +./example_keyboard_send # Running in another terminal +``` diff --git a/examples/example_keyboard_recv.cpp b/examples/example_keyboard_recv.cpp new file mode 100755 index 0000000..15d35f0 --- /dev/null +++ b/examples/example_keyboard_recv.cpp @@ -0,0 +1,53 @@ +/***************************************************************** +Copyright (c) 2022, Unitree Robotics.Co.Ltd. All rights reserved. +*****************************************************************/ +#include "unitree_arm_sdk/unitree_arm_sdk.h" + +bool running = true; + +// over watch the ctrl+c command +void ShutDown(int sig){ + std::cout << "[STATE] stop the controller" << std::endl; + running = false; +} + +int main(){ + EmptyAction emptyAction((int)ArmFSMState::INVALID); + std::vector events; + CmdPanel *cmdPanel; + events.push_back(new StateAction("`", (int)ArmFSMState::BACKTOSTART)); + events.push_back(new StateAction("1", (int)ArmFSMState::PASSIVE)); + events.push_back(new StateAction("2", (int)ArmFSMState::JOINTCTRL)); + events.push_back(new StateAction("3", (int)ArmFSMState::CARTESIAN)); + events.push_back(new StateAction("4", (int)ArmFSMState::MOVEJ)); + events.push_back(new StateAction("5", (int)ArmFSMState::MOVEL)); + events.push_back(new StateAction("6", (int)ArmFSMState::MOVEC)); + events.push_back(new StateAction("7", (int)ArmFSMState::TEACH)); + events.push_back(new StateAction("8", (int)ArmFSMState::TEACHREPEAT)); + events.push_back(new StateAction("9", (int)ArmFSMState::SAVESTATE)); + events.push_back(new StateAction("0", (int)ArmFSMState::TOSTATE)); + events.push_back(new StateAction("-", (int)ArmFSMState::TRAJECTORY)); + events.push_back(new StateAction("=", (int)ArmFSMState::CALIBRATION)); + events.push_back(new StateAction("]", (int)ArmFSMState::NEXT)); + events.push_back(new StateAction("/", (int)ArmFSMState::LOWCMD)); + + events.push_back(new ValueAction("q", "a", 1.0)); + events.push_back(new ValueAction("w", "s", 1.0)); + events.push_back(new ValueAction("e", "d", 1.0)); + events.push_back(new ValueAction("r", "f", 1.0)); + events.push_back(new ValueAction("t", "g", 1.0)); + events.push_back(new ValueAction("y", "h", 1.0)); + events.push_back(new ValueAction("down", "up", 1.0)); + + + cmdPanel = new UnitreeKeyboardUDPRecv(events, emptyAction); + while(true){ + std::cout << "state:" << (int)cmdPanel->getState() << std::endl; + std::cout << "value:" << cmdPanel->getValues().at(0) << std::endl; + usleep(1000000); + } + + delete cmdPanel; + + return 0; +} \ No newline at end of file diff --git a/examples/example_keyboard_send.cpp b/examples/example_keyboard_send.cpp new file mode 100755 index 0000000..d316b88 --- /dev/null +++ b/examples/example_keyboard_send.cpp @@ -0,0 +1,52 @@ +/***************************************************************** +Copyright (c) 2022, Unitree Robotics.Co.Ltd. All rights reserved. +*****************************************************************/ +#include "unitree_arm_sdk/unitree_arm_sdk.h" + +bool running = true; + +// over watch the ctrl+c command +void ShutDown(int sig){ + std::cout << "[STATE] stop the controller" << std::endl; + running = false; +} + +int main(){ + EmptyAction emptyAction((int)ArmFSMState::INVALID); + std::vector events; + UnitreeKeyboardUDPSend *cmdPanel; + + events.push_back(new StateAction("`", (int)ArmFSMState::BACKTOSTART)); + events.push_back(new StateAction("1", (int)ArmFSMState::PASSIVE)); + events.push_back(new StateAction("2", (int)ArmFSMState::JOINTCTRL)); + events.push_back(new StateAction("3", (int)ArmFSMState::CARTESIAN)); + events.push_back(new StateAction("4", (int)ArmFSMState::MOVEJ)); + events.push_back(new StateAction("5", (int)ArmFSMState::MOVEL)); + events.push_back(new StateAction("6", (int)ArmFSMState::MOVEC)); + events.push_back(new StateAction("7", (int)ArmFSMState::TEACH)); + events.push_back(new StateAction("8", (int)ArmFSMState::TEACHREPEAT)); + events.push_back(new StateAction("9", (int)ArmFSMState::SAVESTATE)); + events.push_back(new StateAction("0", (int)ArmFSMState::TOSTATE)); + events.push_back(new StateAction("-", (int)ArmFSMState::TRAJECTORY)); + events.push_back(new StateAction("=", (int)ArmFSMState::CALIBRATION)); + events.push_back(new StateAction("]", (int)ArmFSMState::NEXT)); + events.push_back(new StateAction("/", (int)ArmFSMState::LOWCMD)); + + events.push_back(new ValueAction("q", "a", 1.0)); + events.push_back(new ValueAction("w", "s", 1.0)); + events.push_back(new ValueAction("e", "d", 1.0)); + events.push_back(new ValueAction("r", "f", 1.0)); + events.push_back(new ValueAction("t", "g", 1.0)); + events.push_back(new ValueAction("y", "h", 1.0)); + events.push_back(new ValueAction("down", "up", 1.0)); + + cmdPanel = new UnitreeKeyboardUDPSend(events, emptyAction); + + while(running){ + usleep(1000000); + } + + delete cmdPanel; + + return 0; +} \ No newline at end of file diff --git a/examples/example_lowCmd_send.cpp b/examples/example_lowCmd_send.cpp new file mode 100644 index 0000000..aefa302 --- /dev/null +++ b/examples/example_lowCmd_send.cpp @@ -0,0 +1,187 @@ +/***************************************************************** +Copyright (c) 2022, Unitree Robotics.Co.Ltd. All rights reserved. +*****************************************************************/ +#include +#include "unitree_arm_sdk/unitree_arm_sdk.h" + +struct ConfigPD{ + float K_P[7]; + float K_W[7]; + + virtual void setContrlGain()=0; +}; + +struct SimulationPD : ConfigPD{ + + void setContrlGain(){ + K_P[0] = 80; + K_P[1] = 200; + K_P[2] = 200; + K_P[3] = 15; + K_P[4] = 15; + K_P[5] = 15; + K_P[6] = 15; + + K_W[0] = 5; + K_W[1] = 10; + K_W[2] = 10; + K_W[3] = 2; + K_W[4] = 2; + K_W[5] = 2; + K_W[6] = 5; + } +}; + + +struct RealRobotPD : ConfigPD{ + + void setContrlGain(){ + K_P[0] = 20; + K_P[1] = 40; + K_P[2] = 20; + K_P[3] = 160; + K_P[4] = 160; + K_P[5] = 160; + K_P[6] = 20; + + K_W[0] = 6000; + K_W[1] = 6000; + K_W[2] = 6000; + K_W[3] = 6000; + K_W[4] = 6000; + K_W[5] = 6000; + K_W[6] = 3000; + } +}; + +void printUsage() { + printf( + "Usage: controller [sim-or-robot]\n" + "\t sim-or-robot: s for sim, r for robot\n"); +} + +int main(int argc, char *argv []){ + // input number + if (argc != 2) { + printUsage(); + return EXIT_FAILURE; + } + + // PD config + ConfigPD *configPD; + if (argv[1][0] == 's') { + configPD = new SimulationPD(); + configPD->setContrlGain(); + } else if (argv[1][0] == 'r') { + configPD = new RealRobotPD(); + configPD->setContrlGain(); + } else { + printUsage(); + return EXIT_FAILURE; + } + + // UDP object + UDPPort udp("127.0.0.1", 8071, 8072, sizeof(RecvState), BlockYN::NO, 500000); + + // Send object + SendCmd sendCmd; + RecvState recvState; + sendCmd = {0}; + recvState = {0}; + sendCmd.head[0] = 0xFE; + sendCmd.head[1] = 0xFF; + + float deltaTime = 0.004; // s + + // Only Passive state can change to LowCmd state + while (recvState.state != ArmFSMState::PASSIVE){ + sendCmd.state = ArmFSMState::PASSIVE; + udp.send((uint8_t*)&sendCmd, sizeof(SendCmd)); + udp.recv((uint8_t*)&recvState, sizeof(RecvState)); + usleep(deltaTime*1000000); + } + + // LowCmd state + sendCmd.state = ArmFSMState::LOWCMD; + for(uint8_t i(0); i<7; ++i){ + sendCmd.valueUnion.jointCmd[i].Pos = 0; + sendCmd.valueUnion.jointCmd[i].W = 0; + sendCmd.valueUnion.jointCmd[i].T = 0; + sendCmd.valueUnion.jointCmd[i].K_P = configPD->K_P[i]; + sendCmd.valueUnion.jointCmd[i].K_W = configPD->K_W[i]; + } + sendCmd.valueUnion.jointCmd[3].Pos = -M_PI*4.26/180; + sendCmd.valueUnion.jointCmd[3].W = 0; + sendCmd.valueUnion.jointCmd[3].T = 0; + sendCmd.valueUnion.jointCmd[3].K_P = configPD->K_P[3]; + sendCmd.valueUnion.jointCmd[3].K_W = configPD->K_W[3]; + + int count = 0; + float velocity = 1; // rad/s + + for(int i(0); i<1000; ++i){ + if(i<500){ + // // torque + // sendCmd.valueUnion.jointCmd[0].T = 2.5; + // sendCmd.valueUnion.jointCmd[0].K_P = 0; + // sendCmd.valueUnion.jointCmd[0].K_W = 0; + + // // postion + // sendCmd.valueUnion.jointCmd[0].Pos += deltaTime*velocity; + // sendCmd.valueUnion.jointCmd[0].K_P = configPD->K_P[0]; + // sendCmd.valueUnion.jointCmd[0].K_W = configPD->K_W[0]; + + // // velocity + // sendCmd.valueUnion.jointCmd[0].W = velocity; + // sendCmd.valueUnion.jointCmd[0].K_P = 0; + // sendCmd.valueUnion.jointCmd[0].K_W = configPD->K_W[0]; + + // Grepper + sendCmd.valueUnion.jointCmd[6].Pos -= deltaTime*velocity; + sendCmd.valueUnion.jointCmd[6].W = -velocity; + sendCmd.valueUnion.jointCmd[6].K_P = configPD->K_P[6]; + sendCmd.valueUnion.jointCmd[6].K_W = configPD->K_W[6]; + + } else{ + // // torque + // sendCmd.valueUnion.jointCmd[0].T = -2.5; + // sendCmd.valueUnion.jointCmd[0].K_P = 0; + // sendCmd.valueUnion.jointCmd[0].K_W = 0; + + // // postion + // sendCmd.valueUnion.jointCmd[0].Pos -= deltaTime*velocity; + // sendCmd.valueUnion.jointCmd[0].K_P = configPD->K_P[0]; + // sendCmd.valueUnion.jointCmd[0].K_W = configPD->K_W[0]; + + // // velocity + // sendCmd.valueUnion.jointCmd[0].W = -velocity; + // sendCmd.valueUnion.jointCmd[0].K_P = 0; + // sendCmd.valueUnion.jointCmd[0].K_W = configPD->K_W[0]; + + // Grepper + sendCmd.valueUnion.jointCmd[6].Pos += deltaTime*velocity; + sendCmd.valueUnion.jointCmd[6].W = velocity; + sendCmd.valueUnion.jointCmd[6].K_P = configPD->K_P[6]; + sendCmd.valueUnion.jointCmd[6].K_W = configPD->K_W[6]; + } + + udp.send((uint8_t*)&sendCmd, sizeof(SendCmd)); + udp.recv((uint8_t*)&recvState, sizeof(RecvState)); + + // printf("Pos:%f \n", recvState.jointState[0].Pos); + // printf("roll:%f \n", recvState.cartesianState.roll); + // printf("pitch:%f \n", recvState.cartesianState.pitch); + // printf("yaw:%f \n", recvState.cartesianState.yaw); + usleep(deltaTime*1000000); + } + + while (recvState.state != ArmFSMState::BACKTOSTART){ + sendCmd.state = ArmFSMState::BACKTOSTART; + udp.send((uint8_t*)&sendCmd, sizeof(SendCmd)); + udp.recv((uint8_t*)&recvState, sizeof(RecvState)); + usleep(deltaTime*1000000); + } + + delete configPD; + return 0; +} \ No newline at end of file diff --git a/examples/example_state_recv.cpp b/examples/example_state_recv.cpp new file mode 100755 index 0000000..d31a74d --- /dev/null +++ b/examples/example_state_recv.cpp @@ -0,0 +1,27 @@ +/***************************************************************** +Copyright (c) 2022, Unitree Robotics.Co.Ltd. All rights reserved. +*****************************************************************/ +#include "unitree_arm_sdk/unitree_arm_sdk.h" + +int main(){ + UDPPort udp("127.0.0.1", 8072, 8071, sizeof(SendCmd), BlockYN::NO, 500000); + + SendCmd sendCmd; + RecvState recvState; + sendCmd = {0}; + recvState = {0}; + + recvState.head[0] = 0xFE; + recvState.head[1] = 0xFF; + + bool turn = true; + while(true){ + + udp.send((uint8_t*)&recvState, sizeof(RecvState)); + udp.recv((uint8_t*)&sendCmd, sizeof(SendCmd)); + + printf("%d \n", (int)sendCmd.state); + usleep(2000); + } + return 0; +} \ No newline at end of file diff --git a/examples/example_state_send.cpp b/examples/example_state_send.cpp new file mode 100755 index 0000000..a229aab --- /dev/null +++ b/examples/example_state_send.cpp @@ -0,0 +1,219 @@ +/***************************************************************** +Copyright (c) 2022, Unitree Robotics.Co.Ltd. All rights reserved. +*****************************************************************/ +#include "unitree_arm_sdk/unitree_arm_sdk.h" + + +SendCmd sendCmd; +RecvState recvState; +UDPPort udp("127.0.0.1", 8071, 8072, sizeof(RecvState), BlockYN::NO, 500000); + +void UDPCommunication(){ + udp.send((uint8_t*)&sendCmd, sizeof(SendCmd)); + udp.recv((uint8_t*)&recvState, sizeof(RecvState)); +} + +int main(){ + sendCmd = {0}; + recvState = {0}; + sendCmd.head[0] = 0xFE; + sendCmd.head[1] = 0xFF; + sendCmd.state = ArmFSMState::INVALID; + + float deltaTime = 4000; // us + + LoopFunc *udpCommunication; + udpCommunication = new LoopFunc("udp", 0.002, boost::bind(&UDPCommunication)); + udpCommunication->start(); + +std::cout << "[PASSIVE]" << std::endl; + sendCmd.state = ArmFSMState::PASSIVE; + while (recvState.state != ArmFSMState::PASSIVE){ + usleep(deltaTime); + } + +std::cout << "[BACKTOSTART]" << std::endl; + sendCmd.state = ArmFSMState::BACKTOSTART; + while (recvState.state != ArmFSMState::BACKTOSTART){ + usleep(deltaTime); + } + sendCmd.state = ArmFSMState::INVALID; + while (recvState.state != ArmFSMState::JOINTCTRL){ + usleep(deltaTime); + } + +std::cout << "[CARTESIAN]" << std::endl; + sendCmd.state = ArmFSMState::CARTESIAN; + while (recvState.state != ArmFSMState::CARTESIAN){ + usleep(deltaTime); + } + + for(int i(0); i<2000; ++i){ + if(i<500){ + sendCmd.state = ArmFSMState::INVALID; + sendCmd.value = ArmFSMValue::E; + } + else if(i<1000){ + sendCmd.state = ArmFSMState::INVALID; + sendCmd.value = ArmFSMValue::Q; + } + else if(i<1500){ + sendCmd.state = ArmFSMState::INVALID; + sendCmd.value = ArmFSMValue::D; + } + else{ + sendCmd.state = ArmFSMState::INVALID; + sendCmd.value = ArmFSMValue::Q; + } + + // printf("------state:%d-------\n", (int)recvState.state); + // printf("Pos:%f \n", recvState.jointState[0].Pos); + // printf("Pos:%f \n", recvState.jointState[1].Pos); + // printf("Pos:%f \n", recvState.jointState[2].Pos); + // printf("roll:%f \n", recvState.cartesianState.roll); + // printf("pitch:%f \n", recvState.cartesianState.pitch); + // printf("yaw:%f \n", recvState.cartesianState.yaw); + usleep(deltaTime); + } + +std::cout << "[JOINTCTRL]" << std::endl; + sendCmd.state = ArmFSMState::JOINTCTRL; + while (recvState.state != ArmFSMState::JOINTCTRL){ + usleep(deltaTime); + } + + for(int i(0); i<1800; ++i){ + if(i<300){ + sendCmd.state = ArmFSMState::INVALID; + sendCmd.value = ArmFSMValue::Q; + } + else if(i<600){ + sendCmd.state = ArmFSMState::INVALID; + sendCmd.value = ArmFSMValue::W; + } + else if(i<900){ + sendCmd.state = ArmFSMState::INVALID; + sendCmd.value = ArmFSMValue::D; + } + else if(i<1200){ + sendCmd.state = ArmFSMState::INVALID; + sendCmd.value = ArmFSMValue::R; + } + else if(i<1500){ + sendCmd.state = ArmFSMState::INVALID; + sendCmd.value = ArmFSMValue::T; + } + else{ + sendCmd.state = ArmFSMState::INVALID; + sendCmd.value = ArmFSMValue::Y; + } + + // printf("------state:%d-------\n", (int)recvState.state); + // printf("Pos:%f \n", recvState.jointState[0].Pos); + // printf("Pos:%f \n", recvState.jointState[1].Pos); + // printf("Pos:%f \n", recvState.jointState[2].Pos); + // printf("roll:%f \n", recvState.cartesianState.roll); + // printf("pitch:%f \n", recvState.cartesianState.pitch); + // printf("yaw:%f \n", recvState.cartesianState.yaw); + usleep(deltaTime); + } + +std::cout << "[SAVESTATE]" << std::endl; + std::string test = "test"; + sendCmd.state = ArmFSMState::SAVESTATE; + strcpy(sendCmd.valueUnion.saveState, test.c_str()); + usleep(deltaTime); + +std::cout << "[BACKTOSTART]" << std::endl; + sendCmd.state = ArmFSMState::BACKTOSTART; + while(recvState.state != ArmFSMState::BACKTOSTART){ + usleep(deltaTime); + } + sendCmd.state = ArmFSMState::INVALID; + while (recvState.state != ArmFSMState::JOINTCTRL){ + usleep(deltaTime); + } + + +std::cout << "[TOSTATE]" << std::endl; + sendCmd.state = ArmFSMState::TOSTATE; + strcpy(sendCmd.valueUnion.toState, test.c_str()); + while (recvState.state != ArmFSMState::TOSTATE){ + usleep(deltaTime); + } + sendCmd.state = ArmFSMState::INVALID; + while (recvState.state != ArmFSMState::JOINTCTRL){ + usleep(deltaTime); + } + +std::cout << "[BACKTOSTART]" << std::endl; + sendCmd.state = ArmFSMState::BACKTOSTART; + while (recvState.state != ArmFSMState::BACKTOSTART){ + usleep(deltaTime); + } + sendCmd.state = ArmFSMState::INVALID; + while (recvState.state != ArmFSMState::JOINTCTRL){ + usleep(deltaTime); + } + +std::cout << "[MoveJ]" << std::endl; + sendCmd.state = ArmFSMState::MOVEJ; + sendCmd.valueUnion.moveJ.roll = 0; + sendCmd.valueUnion.moveJ.pitch = 0; + sendCmd.valueUnion.moveJ.yaw = 0; + sendCmd.valueUnion.moveJ.x = 0.5; + sendCmd.valueUnion.moveJ.y = -0.3; + sendCmd.valueUnion.moveJ.z = 0.5; + while (recvState.state != ArmFSMState::MOVEJ){ + usleep(deltaTime); + } + sendCmd.state = ArmFSMState::INVALID; + while (recvState.state != ArmFSMState::JOINTCTRL){ + usleep(deltaTime); + } + +std::cout << "[MoveL]" << std::endl; + sendCmd.state = ArmFSMState::MOVEL; + sendCmd.valueUnion.moveL.roll = 0; + sendCmd.valueUnion.moveL.pitch = 0; + sendCmd.valueUnion.moveL.yaw = 0; + sendCmd.valueUnion.moveL.x = 0.5; + sendCmd.valueUnion.moveL.y = 0.4; + sendCmd.valueUnion.moveL.z = 0.3; + while (recvState.state != ArmFSMState::MOVEL){ + usleep(deltaTime); + } + sendCmd.state = ArmFSMState::INVALID; + while (recvState.state != ArmFSMState::JOINTCTRL){ + usleep(deltaTime); + } + +std::cout << "[MoveC]" << std::endl; + sendCmd.state = ArmFSMState::MOVEC; + sendCmd.valueUnion.moveC[0].roll = 0; + sendCmd.valueUnion.moveC[0].pitch = 0; + sendCmd.valueUnion.moveC[0].yaw = 0; + sendCmd.valueUnion.moveC[0].x = 0.5; + sendCmd.valueUnion.moveC[0].y = 0; + sendCmd.valueUnion.moveC[0].z = 0.5; + sendCmd.valueUnion.moveC[1].roll = 0; + sendCmd.valueUnion.moveC[1].pitch = 0; + sendCmd.valueUnion.moveC[1].yaw = 0; + sendCmd.valueUnion.moveC[1].x = 0.5; + sendCmd.valueUnion.moveC[1].y = -0.4; + sendCmd.valueUnion.moveC[1].z = 0.3; + while (recvState.state != ArmFSMState::MOVEC){ + usleep(deltaTime); + } + sendCmd.state = ArmFSMState::INVALID; + while (recvState.state != ArmFSMState::JOINTCTRL){ + usleep(deltaTime); + } + +std::cout << "[BACKTOSTART]" << std::endl; + sendCmd.state = ArmFSMState::BACKTOSTART; + while (recvState.state != ArmFSMState::BACKTOSTART){ + usleep(deltaTime); + } + return 0; +} \ No newline at end of file diff --git a/include/unitree_arm_sdk/cmdPanel.h b/include/unitree_arm_sdk/cmdPanel.h new file mode 100644 index 0000000..3dfbba4 --- /dev/null +++ b/include/unitree_arm_sdk/cmdPanel.h @@ -0,0 +1,150 @@ +/***************************************************************** +Copyright (c) 2022, Unitree Robotics.Co.Ltd. All rights reserved. +*****************************************************************/ +#ifndef _UNITREE_ARM_CMDPANEL_H_ +#define _UNITREE_ARM_CMDPANEL_H_ + +#include +#include +#include "unitree_arm_sdk/loop.h" +#include "unitree_arm_sdk/common/arm_common.h" + + +enum class KeyPress{ + RELEASE, + PRESS, + REPEAT +}; + + +enum class ActionType{ + EMPTY, + STATE, + VALUE +}; + + +struct KeyCmd{ + std::string c; + KeyPress keyPress; +}; + + +class KeyAction{ +public: + KeyAction(ActionType type); + virtual ~KeyAction(){}; + ActionType getType(){return _type;} +protected: + ActionType _type; +}; + + +class StateAction : public KeyAction{ +public: + StateAction(std::string c, int state, KeyPress press = KeyPress::PRESS); + virtual ~StateAction(){}; + int getState(){return _state;}; + bool handleCmd(KeyCmd keyCmd, int &state); +protected: + int _state; + KeyCmd _keyCmdSet; +}; + +class EmptyAction : public StateAction{ +public: + EmptyAction(int state); + ~EmptyAction(){}; +private: + +}; + + +/* +必须为长按, deltaValue为每秒改变量 +valueAction允许共用按键，例如空格停止 +但是正反转、停止键不可重复 +*/ +class ValueAction : public KeyAction{ +public: + ValueAction(std::string cUp, std::string cDown, double deltaValue, double initValue = 0.0); + ValueAction(std::string cUp, std::string cDown, std::string cGoZero, double deltaValue, double initValue = 0.0); + ValueAction(std::string cUp, std::string cDown, double deltaValue, double limit1, double limit2, double initValue = 0.0); + ValueAction(std::string cUp, std::string cDown, std::string cGoZero, double deltaValue, double limit1, double limit2, double initValue = 0.0); + + ~ValueAction(){}; + bool handleCmd(KeyCmd keyCmd); + void setDt(double dt); + double getValue(); + double getDValue(); + double setValue(double value){_value = value;} +private: + double _value; + double _changeDirection; + double _dV = 0.0; + double _dt = 0.0; + double _dVdt = 0.0; + double _lim1, _lim2; + bool _hasLim = false; + bool _hasGoZero = false; + + KeyCmd _upCmdSet; + KeyCmd _downCmdSet; + KeyCmd _goZeroCmdSet; + +}; + + +class CmdPanel{ +public: + CmdPanel(std::vector events, + EmptyAction emptyAction, size_t channelNum = 1, double dt = 0.002); + virtual ~CmdPanel(); + int getState(size_t channelID = 0); + std::vector getValues(); + std::vector getDValues(); + void setValue(std::vector values); + void setValue(double value, size_t id); + virtual std::string getString(std::string slogan); + virtual std::vector stringToArray(std::string slogan); + virtual std::vector > stringToMatrix(std::string slogan); + + virtual SendCmd getSendCmd(); + virtual void getRecvState(RecvState recvState); + +protected: + virtual void _read() = 0; + void _start(); + void _run(); + void _updateState(); + void _pressKeyboard(); + void _releaseKeyboard(); + + static void* _runStatic(void* obj); + static void* _readStatic(void* obj); + + LoopFunc *_runThread; + LoopFunc *_readThread; + + std::vector _stateEvents; + std::vector _valueEvents; + + EmptyAction _emptyAction; + size_t _actionNum = 0; + size_t _stateNum = 0; + size_t _valueNum = 0; + size_t _channelNum; + std::vector _values; + std::vector _dValues; + int _state; + std::vector> _stateQueue; + std::vector _outputState; + std::vector _getState; + double _dt; + KeyCmd _keyCmd; + std::string _cPast = ""; + + bool _running = true; +}; + +#endif // _UNITREE_ARM_CMDPANEL_H_ \ No newline at end of file diff --git a/include/unitree_arm_sdk/common/arm_common.h b/include/unitree_arm_sdk/common/arm_common.h new file mode 100755 index 0000000..380f8d5 --- /dev/null +++ b/include/unitree_arm_sdk/common/arm_common.h @@ -0,0 +1,130 @@ +#ifndef _UNITREE_ARM_ARM_COMMON_H_ +#define _UNITREE_ARM_ARM_COMMON_H_ + +#include + +#pragma pack(1) + +// 4 Byte +enum class ArmFSMState{ + INVALID, + PASSIVE, + JOINTCTRL, + CARTESIAN, + MOVEJ, + MOVEL, + MOVEC, + TRAJECTORY, + TOSTATE, + SAVESTATE, + TEACH, + TEACHREPEAT, + CALIBRATION, + DANCE00, + DANCE01, + DANCE02, + DANCE03, + DANCE04, + DANCE05, + DANCE06, + DANCE07, + DANCE08, + DANCE09, + BACKTOSTART, + GRIPPER_OPEN, + GRIPPER_CLOSE, + NEXT, + LOWCMD +}; + +// 4 Byte +enum class ArmFSMValue{ + INVALID, + Q,A, + W,S, + E,D, + R,F, + T,G, + Y,H, + DOWN, + UP +}; + +// 20 Byte +struct JointCmd{ + float T; + float W; + float Pos; + float K_P; + float K_W; +}; + +// 16 Byte +struct JointState{ + float T; + float W; + float Acc; + float Pos; +}; + +// 140 Byte +union UDPSendCmd{ + uint8_t checkCmd; + JointCmd jointCmd[7]; +}; + +// 16*7=112 Byte +union UDPRecvState{ + uint8_t singleState; + uint8_t selfCheck[10]; + JointState jointState[7]; + uint8_t errorCheck[16]; +}; + +// 24 Byte +struct Posture{ + float roll; + float pitch; + float yaw; + float x; + float y; + float z; +}; + +// 48 Byte +struct MoveC{ + Posture middlePosture; + Posture endPosture; +}; + +// 20*7=140 Byte +union ValueUnion{ + Posture moveJ; + Posture moveL; + Posture moveC[2]; + char teach[10]; + char teachRepeat[10]; + char saveState[10]; + char toState[10]; + JointCmd jointCmd[7]; +}; + +// 2+4+4+140 = 150 Byte +struct SendCmd{ + uint8_t head[2]; + ArmFSMState state; + ArmFSMValue value; + ValueUnion valueUnion; +}; + +// 2+4+112+24 = 142 Byte +struct RecvState{ + uint8_t head[2]; + ArmFSMState state; + JointState jointState[7]; + Posture cartesianState; +}; + +#pragma pack() + +#endif // _UNITREE_ARM_ARM_MSG_H_ \ No newline at end of file diff --git a/include/unitree_arm_sdk/common/arm_motor_common.h b/include/unitree_arm_sdk/common/arm_motor_common.h new file mode 100644 index 0000000..e248955 --- /dev/null +++ b/include/unitree_arm_sdk/common/arm_motor_common.h @@ -0,0 +1,214 @@ +#ifndef _UNITREE_ARM_ARM_MOTOR_COMMON_H_ +#define _UNITREE_ARM_ARM_MOTOR_COMMON_H_ + +#include +typedef int16_t q15_t; + +#pragma pack(1) + +// 发送用单个数据数据结构 +typedef union{ + int32_t L; + uint8_t u8[4]; + uint16_t u16[2]; + uint32_t u32; + float F; +}COMData32; + +typedef struct { + // 定义 数据包头 + unsigned char start[2]; // 包头 + unsigned char motorID; // 电机ID 0,1,2,3 ... 0xBB 表示向所有电机广播（此时无返回） + unsigned char reserved; +}COMHead; + +#pragma pack() + +#pragma pack(1) + +typedef struct { + + uint8_t fan_d; // 关节上的散热风扇转速 + uint8_t Fmusic; // 电机发声频率 /64*1000 15.625f 频率分度 + uint8_t Hmusic; // 电机发声强度 推荐值4 声音强度 0.1 分度 + uint8_t reserved4; + + uint8_t FRGB[4]; // 足端LED + +}LowHzMotorCmd; + +typedef struct { // 以 4个字节一组排列 ，不然编译器会凑整 + // 定义 数据 + uint8_t mode; // 关节模式选择 + uint8_t ModifyBit; // 电机控制参数修改位 + uint8_t ReadBit; // 电机控制参数发送位 + uint8_t reserved; + + COMData32 Modify; // 电机参数修改 的数据 + //实际给FOC的指令力矩为： + //K_P*delta_Pos + K_W*delta_W + T + q15_t T; // 期望关节的输出力矩（电机本身的力矩）x256, 7 + 8 描述 + q15_t W; // 期望关节速度 （电机本身的速度） x128, 8 + 7描述 + int32_t Pos; // 期望关节位置 x 16384/6.2832, 14位编码器（主控0点修正，电机关节还是以编码器0点为准） + + q15_t K_P; // 关节刚度系数 x2048 4+11 描述 + q15_t K_W; // 关节速度系数 x1024 5+10 描述 + + uint8_t LowHzMotorCmdIndex; // 电机低频率控制命令的索引, 0-7, 分别代表LowHzMotorCmd中的8个字节 + uint8_t LowHzMotorCmdByte; // 电机低频率控制命令的字节 + + COMData32 Res[1]; // 通讯 保留字节 用于实现别的一些通讯内容 + +}MasterComdV3; // 加上数据包的包头 和CRC 34字节 + +typedef struct { + // 定义 电机控制命令数据包 + COMHead head; + MasterComdV3 Mdata; + COMData32 CRCdata; +}MasterComdDataV3;//返回数据 + +// typedef struct { +// // 定义 总得485 数据包 + +// MasterComdData M1; +// MasterComdData M2; +// MasterComdData M3; + +// }DMA485TxDataV3; + +#pragma pack() + +#pragma pack(1) + +typedef struct { // 以 4个字节一组排列 ，不然编译器会凑整 + // 定义 数据 + uint8_t mode; // 当前关节模式 + uint8_t ReadBit; // 电机控制参数修改 是否成功位 + int8_t Temp; // 电机当前平均温度 + uint8_t MError; // 电机错误 标识 + + COMData32 Read; // 读取的当前 电机 的控制数据 + int16_t T; // 当前实际电机输出力矩 7 + 8 描述 + + int16_t W; // 当前实际电机速度（高速） 8 + 7 描述 + float LW; // 当前实际电机速度（低速） + + int16_t W2; // 当前实际关节速度（高速） 8 + 7 描述 + float LW2; // 当前实际关节速度（低速） + + int16_t Acc; // 电机转子加速度 15+0 描述 惯量较小 + int16_t OutAcc; // 输出轴加速度 12+3 描述 惯量较大 + + int32_t Pos; // 当前电机位置（主控0点修正，电机关节还是以编码器0点为准） + int32_t Pos2; // 关节编码器位置(输出编码器) + + int16_t gyro[3]; // 电机驱动板6轴传感器数据 + int16_t acc[3]; + + // 力传感器的数据 + int16_t Fgyro[3]; // + int16_t Facc[3]; + int16_t Fmag[3]; + uint8_t Ftemp; // 8位表示的温度 7位（-28~100度） 1位0.5度分辨率 + + int16_t Force16; // 力传感器高16位数据 + int8_t Force8; // 力传感器低8位数据 + + uint8_t FError; // 足端传感器错误标识 + + int8_t Res[1]; // 通讯 保留字节 + +}ServoComdV3; // 加上数据包的包头 和CRC 78字节（4+70+4） + +typedef struct { + // 定义 电机控制命令数据包 + COMHead head; + ServoComdV3 Mdata; + + COMData32 CRCdata; + +}ServoComdDataV3; //发送数据 + +// typedef struct { +// // 定义 总的485 接受数据包 + +// ServoComdDataV3 M[3]; +// // uint8_t nullbyte1; + +// }DMA485RxDataV3; + + +#pragma pack() + +// 00 00 00 00 00 +// 00 00 00 00 00 +// 00 00 00 00 00 +// 00 00 00 +// 数据包默认初始化 +// 主机发送的数据包 +/* + Tx485Data[_FR][i].head.start[0] = 0xFE ; Tx485Data[_FR][i].head.start[1] = 0xEE; // 数据包头 + Tx485Data[_FR][i].Mdata.ModifyBit = 0xFF; Tx485Data[_FR][i].Mdata.mode = 0; // 默认不修改数据 和 电机的默认工作模式 + Tx485Data[_FR][i].head.motorID = i; 0 // 目标电机标号 + Tx485Data[_FR][i].Mdata.T = 0.0f; // 默认目标关节输出力矩 motor1.Extra_Torque = motorRxData[1].Mdata.T*0.390625f; // N.M 转化为 N.CM IQ8描述 + Tx485Data[_FR][i].Mdata.Pos = 0x7FE95C80; // 默认目标关节位置 不启用位置环 14位分辨率 + Tx485Data[_FR][i].Mdata.W = 16000.0f; // 默认目标关节速度 不启用速度环 1+8+7描述 motor1.Target_Speed = motorRxData[1].Mdata.W*0.0078125f; // 单位 rad/s IQ7描述 + Tx485Data[_FR][i].Mdata.K_P = (q15_t)(0.6f*(1<<11)); // 默认关节刚度系数 4+11 描述 motor1.K_Pos = ((float)motorRxData[1].Mdata.K_P)/(1<<11); // 描述刚度的通讯数据格式 4+11 + Tx485Data[_FR][i].Mdata.K_W = (q15_t)(1.0f*(1<<10)); // 默认关节速度系数 5+10 描述 motor1.K_Speed = ((float)motorRxData[1].Mdata.K_W)/(1<<10); // 描述阻尼的通讯数据格式 5+10 +*/ + +enum class MotorType{ + A1Go1, // 4.8M baudrate, K_W x1024 + B1 // 6.0M baudrate, K_W x512 +}; + +struct MOTOR_send{ + // 定义 发送格式化数据 + MasterComdDataV3 motor_send_data; //电机控制数据结构体 + MotorType motorType = MotorType::A1Go1; + int hex_len = 34; //发送的16进制命令数组长度, 34 + // long long send_time; //发送该命令的时间, 微秒(us) + // 待发送的各项数据 + unsigned short id; //电机ID，0xBB代表全部电机 + unsigned short mode; //0:空闲, 5:开环转动, 10:闭环FOC控制 + //实际给FOC的指令力矩为： + //K_P*delta_Pos + K_W*delta_W + T + float T; //期望关节的输出力矩（电机本身的力矩）（Nm） + float W; //期望关节速度（电机本身的速度）(rad/s) + float Pos; //期望关节位置（rad） + float K_P; //关节刚度系数 + float K_W; //关节速度系数 + COMData32 Res; // 通讯 保留字节 用于实现别的一些通讯内容 +}; + +struct MOTOR_recv{ + // 定义 接收数据 + ServoComdDataV3 motor_recv_data; //电机接收数据结构体 + MotorType motorType = MotorType::A1Go1; + int hex_len = 78; //接收的16进制命令数组长度, 78 + // long long resv_time; //接收该命令的时间, 微秒(us) + bool correct = false; //接收数据是否完整（true完整，false不完整） + //解读得出的电机数据 + unsigned char motor_id; //电机ID + unsigned char mode; //0:空闲, 5:开环转动, 10:闭环FOC控制 + int Temp; //温度 + unsigned char MError; //错误码 + + float T; // 当前实际电机输出力矩 + float W; // 当前实际电机速度（高速） + float LW; // 当前实际电机速度（低速） + int Acc; // 电机转子加速度 + float Pos; // 当前电机位置（主控0点修正，电机关节还是以编码器0点为准） + + float gyro[3]; // 电机驱动板6轴传感器数据 + float acc[3]; + + int8_t Res; +}; + + +extern void modify_data(MOTOR_send* motor_s); +extern bool extract_data(MOTOR_recv* motor_r); + +#endif \ No newline at end of file diff --git a/include/unitree_arm_sdk/common/joystick_common.h b/include/unitree_arm_sdk/common/joystick_common.h new file mode 100644 index 0000000..b9c1e06 --- /dev/null +++ b/include/unitree_arm_sdk/common/joystick_common.h @@ -0,0 +1,43 @@ +/***************************************************************** +Copyright (c) 2022, Unitree Robotics.Co.Ltd. All rights reserved. +*****************************************************************/ +#ifndef _UNITREE_ARM_JOYSTICK_COMMON_H_ +#define _UNITREE_ARM_JOYSTICK_COMMON_H_ +#include + +// 16b +typedef union { + struct { + uint8_t R1 :1; + uint8_t L1 :1; + uint8_t start :1; + uint8_t select :1; + uint8_t R2 :1; + uint8_t L2 :1; + uint8_t F1 :1; + uint8_t F2 :1; + uint8_t A :1; + uint8_t B :1; + uint8_t X :1; + uint8_t Y :1; + uint8_t up :1; + uint8_t right :1; + uint8_t down :1; + uint8_t left :1; + } components; + uint16_t value; +} xKeySwitchUnion; + +// 40 Byte (now used 24B) +typedef struct { + uint8_t head[2]; + xKeySwitchUnion btn; + float lx; + float rx; + float ry; + float L2; + float ly; + uint8_t idle[16]; +} xRockerBtnDataStruct; + +#endif \ No newline at end of file diff --git a/include/unitree_arm_sdk/common/quadruped_common.h b/include/unitree_arm_sdk/common/quadruped_common.h new file mode 100644 index 0000000..9401655 --- /dev/null +++ b/include/unitree_arm_sdk/common/quadruped_common.h @@ -0,0 +1,212 @@ +#ifndef _UNITREE_ARM_QUADRUPED_COMMON_H_ +#define _UNITREE_ARM_QUADRUPED_COMMON_H_ + +#include +#include + +namespace UNITREE_LEGGED_SDK +{ + + constexpr int HIGHLEVEL = 0xee; + constexpr int LOWLEVEL = 0xff; + constexpr int TRIGERLEVEL = 0xf0; + constexpr double PosStopF = (2.146E+9f); + constexpr double VelStopF = (16000.0f); + extern const int HIGH_CMD_LENGTH; // sizeof(HighCmd) + extern const int HIGH_STATE_LENGTH; // sizeof(HighState) + extern const int LOW_CMD_LENGTH; // shorter than sizeof(LowCmd), bytes compressed LowCmd length + extern const int LOW_STATE_LENGTH; // shorter than sizeof(LowState), bytes compressed LowState length + +#pragma pack(1) + + typedef struct + { + uint8_t off; // off 0xA5 + std::array reserve; + } BmsCmd; + + typedef struct + { + uint8_t version_h; + uint8_t version_l; + uint8_t bms_status; + uint8_t SOC; // SOC 0-100% + int32_t current; // mA + uint16_t cycle; + std::array BQ_NTC; // x1 degrees centigrade + std::array MCU_NTC; // x1 degrees centigrade + std::array cell_vol; // cell voltage mV + } BmsState; + + typedef struct + { + float x; + float y; + float z; + } Cartesian; + + typedef struct + { + std::array quaternion; // quaternion, normalized, (w,x,y,z) + std::array gyroscope; // angular velocity （unit: rad/s) (raw data) + std::array accelerometer; // m/(s2) (raw data) + std::array rpy; // euler angle（unit: rad) + int8_t temperature; + } IMU; // when under accelerated motion, the attitude of the robot calculated by IMU will drift. + + typedef struct + { + uint8_t r; + uint8_t g; + uint8_t b; + } LED; // foot led brightness: 0~255 + + typedef struct + { + uint8_t mode; // motor working mode + float q; // current angle (unit: radian) + float dq; // current velocity (unit: radian/second) + float ddq; // current acc (unit: radian/second*second) + float tauEst; // current estimated output torque (unit: N.m) + float q_raw; // current angle (unit: radian) + float dq_raw; // current velocity (unit: radian/second) + float ddq_raw; + int8_t temperature; // current temperature (temperature conduction is slow that leads to lag) + std::array reserve; + } MotorState; // motor feedback + + typedef struct + { + uint8_t mode; // desired working mode + float q; // desired angle (unit: radian) + float dq; // desired velocity (unit: radian/second) + float tau; // desired output torque (unit: N.m) + float Kp; // desired position stiffness (unit: N.m/rad ) + float Kd; // desired velocity stiffness (unit: N.m/(rad/s) ) + std::array reserve; + } MotorCmd; // motor control + + typedef struct + { + std::array head; + uint8_t levelFlag; + uint8_t frameReserve; + + std::array SN; + std::array version; + uint16_t bandWidth; + IMU imu; + std::array motorState; + BmsState bms; + std::array footForce; // force sensors + std::array footForceEst; // force sensors + uint32_t tick; // reference real-time from motion controller (unit: us) + std::array wirelessRemote; // wireless commands + uint32_t reserve; + + uint32_t crc; + } LowState; // low level feedback + + typedef struct + { + std::array head; + uint8_t levelFlag; + uint8_t frameReserve; + + std::array SN; + std::array version; + uint16_t bandWidth; + std::array motorCmd; + BmsCmd bms; + std::array wirelessRemote; + uint32_t reserve; + + uint32_t crc; + } LowCmd; // low level control + + typedef struct + { + std::array head; + uint8_t levelFlag; + uint8_t frameReserve; + + std::array SN; + std::array version; + uint16_t bandWidth; + IMU imu; + std::array motorState; + BmsState bms; + std::array footForce; + std::array footForceEst; + uint8_t mode; + float progress; + uint8_t gaitType; // 0.idle 1.trot 2.trot running 3.climb stair 4.trot obstacle + float footRaiseHeight; // (unit: m, default: 0.08m), foot up height while walking + std::array position; // (unit: m), from own odometry in inertial frame, usually drift + float bodyHeight; // (unit: m, default: 0.28m), + std::array velocity; // (unit: m/s), forwardSpeed, sideSpeed, rotateSpeed in body frame + float yawSpeed; // (unit: rad/s), rotateSpeed in body frame + std::array rangeObstacle; + std::array footPosition2Body; // foot position relative to body + std::array footSpeed2Body; // foot speed relative to body + std::array wirelessRemote; + uint32_t reserve; + + uint32_t crc; + } HighState; // high level feedback + + typedef struct + { + std::array head; + uint8_t levelFlag; + uint8_t frameReserve; + + std::array SN; + std::array version; + uint16_t bandWidth; + uint8_t mode; // 0. idle, default stand 1. force stand (controlled by dBodyHeight + ypr) + // 2. target velocity walking (controlled by velocity + yawSpeed) + // 3. target position walking (controlled by position + ypr[0]) + // 4. path mode walking (reserve for future release) + // 5. position stand down. + // 6. position stand up + // 7. damping mode + // 8. recovery stand + // 9. backflip + // 10. jumpYaw + // 11. straightHand + // 12. dance1 + // 13. dance2 + + uint8_t gaitType; // 0.idle 1.trot 2.trot running 3.climb stair 4.trot obstacle + uint8_t speedLevel; // 0. default low speed. 1. medium speed 2. high speed. during walking, only respond MODE 3 + float footRaiseHeight; // (unit: m, default: 0.08m), foot up height while walking, delta value + float bodyHeight; // (unit: m, default: 0.28m), delta value + std::array position; // (unit: m), desired position in inertial frame + std::array euler; // (unit: rad), roll pitch yaw in stand mode + std::array velocity; // (unit: m/s), forwardSpeed, sideSpeed in body frame + float yawSpeed; // (unit: rad/s), rotateSpeed in body frame + BmsCmd bms; + std::array led; + std::array wirelessRemote; + uint32_t reserve; + + uint32_t crc; + } HighCmd; // high level control + +#pragma pack() + + typedef struct + { + unsigned long long TotalCount; // total loop count + unsigned long long SendCount; // total send count + unsigned long long RecvCount; // total receive count + unsigned long long SendError; // total send error + unsigned long long FlagError; // total flag error + unsigned long long RecvCRCError; // total reveive CRC error + unsigned long long RecvLoseError; // total lose package count + } UDPState; // UDP communication state + +} + +#endif \ No newline at end of file diff --git a/include/unitree_arm_sdk/joystick.h b/include/unitree_arm_sdk/joystick.h new file mode 100755 index 0000000..cb161e2 --- /dev/null +++ b/include/unitree_arm_sdk/joystick.h @@ -0,0 +1,28 @@ +#ifndef _UNITREE_ARM_JOYSTICK_H_ +#define _UNITREE_ARM_JOYSTICK_H_ + +#include +#include "unitree_arm_sdk/udp.h" +#include "unitree_arm_sdk/cmdPanel.h" +#include "unitree_arm_sdk/common/quadruped_common.h" +#include "unitree_arm_sdk/common/joystick_common.h" + +using namespace UNITREE_LEGGED_SDK; + +class UnitreeJoystick : public CmdPanel{ +public: + UnitreeJoystick(std::vector events, + EmptyAction emptyAction, size_t channelNum = 1, + double dt = 0.002); + ~UnitreeJoystick(); +private: + void _read(); + void _extractCmd(); + + xRockerBtnDataStruct _keyData; + UDPPort *_udp; + HighCmd _udpCmd; + HighState _udpState; +}; + +#endif // _UNITREE_ARM_JOYSTICK_H_ diff --git a/include/unitree_arm_sdk/keyboard.h b/include/unitree_arm_sdk/keyboard.h new file mode 100644 index 0000000..b2bdcb1 --- /dev/null +++ b/include/unitree_arm_sdk/keyboard.h @@ -0,0 +1,92 @@ +#ifndef _UNITREE_ARM_KEYBOARD_H_ +#define _UNITREE_ARM_KEYBOARD_H_ + +#include +#include +#include + +#include "unitree_arm_sdk/udp.h" +#include "unitree_arm_sdk/common/arm_common.h" +#include "unitree_arm_sdk/cmdPanel.h" + +class Keyboard : public CmdPanel{ +public: + Keyboard(std::vector events, + EmptyAction emptyAction, size_t channelNum = 1, double dt = 0.002); + ~Keyboard(); + std::string getString(std::string slogan); + std::vector stringToArray(std::string slogan); + std::vector > stringToMatrix(std::string slogan); +private: + void _read(); + void _pauseKey(); + void _startKey(); + void _extractCmd(); + + fd_set _set; + char _c = '\0'; + + termios _oldSettings; + termios _newSettings; + timeval _tv; +}; + + +class UnitreeKeyboardUDPSend : public CmdPanel{ +public: + UnitreeKeyboardUDPSend(std::vector events, + EmptyAction emptyAction, size_t channelNum = 1, + double dt = 0.002); + ~UnitreeKeyboardUDPSend(); + std::string getString(std::string slogan); + std::vector stringToArray(std::string slogan); + std::vector > stringToMatrix(std::string slogan); + +private: + // keyboard communication + void _read(); + void _pauseKey(); + void _startKey(); + void _extractCmd(); + + fd_set _set; + char _c = '\0'; + + termios _oldSettings; + termios _newSettings; + timeval _tv; + + // udp communication + void _communicationUDP(); + void _extractCmdKeyboard(); + + UDPPort *_udp; + SendCmd _sendCmd; + RecvState _recvState; + std::vector> posture; + std::string slogan; +}; + + +class UnitreeKeyboardUDPRecv : public CmdPanel{ +public: + UnitreeKeyboardUDPRecv(std::vector events, + EmptyAction emptyAction, size_t channelNum = 1, + double dt = 0.002); + ~UnitreeKeyboardUDPRecv(); + + SendCmd getSendCmd(){return _sendCmd;}; + void getRecvState(RecvState recvState){ + _recvState = recvState; + }; + +private: + void _read(); + void _extractCmd(); + + UDPPort *_udp; + SendCmd _sendCmd; + RecvState _recvState; +}; + +#endif // _UNITREE_ARM_KEYBOARD_H_ \ No newline at end of file diff --git a/include/unitree_arm_sdk/loop.h b/include/unitree_arm_sdk/loop.h new file mode 100755 index 0000000..c56c0b1 --- /dev/null +++ b/include/unitree_arm_sdk/loop.h @@ -0,0 +1,55 @@ +#ifndef _UNITREE_ARM_LOOP_H_ +#define _UNITREE_ARM_LOOP_H_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "unitree_arm_sdk/timer.h" + +// constexpr int THREAD_PRIORITY = 99; // real-time priority + +typedef boost::function Callback; + +class Loop { +public: + Loop(std::string name, float period, int bindCPU = -1); + ~Loop(); + void start(); + void shutdown(); + virtual void functionCB() = 0; + +private: + void entryFunc(); + + std::string _name; + float _period; + int _bindCPU; + bool _bind_cpu_flag = false; + bool _isrunning = false; + std::thread _thread; + + size_t _runTimes = 0; + size_t _timeOutTimes = 0; + + AbsoluteTimer *_timer; +}; + +class LoopFunc : public Loop { +public: + LoopFunc(std::string name, float period, const Callback& _cb) + : Loop(name, period), _fp(_cb){} + LoopFunc(std::string name, float period, int bindCPU, const Callback& _cb) + : Loop(name, period, bindCPU), _fp(_cb){} + void functionCB() { (_fp)(); } +private: + boost::function _fp; +}; + +#endif // _UNITREE_ARM_LOOP_H_ \ No newline at end of file diff --git a/include/unitree_arm_sdk/timer.h b/include/unitree_arm_sdk/timer.h new file mode 100644 index 0000000..40701da --- /dev/null +++ b/include/unitree_arm_sdk/timer.h @@ -0,0 +1,56 @@ +#ifndef _UNITREE_ARM_TIMER_H_ +#define _UNITREE_ARM_TIMER_H_ + +#include +#include +#include +#include +#include + +//时间戳 微秒级， 需要#include +inline long long getSystemTime(){ + struct timeval t; + gettimeofday(&t, NULL); + return 1000000 * t.tv_sec + t.tv_usec; +} +//时间戳 秒级， 需要getSystemTime() +inline double getTimeSecond(){ + double time = getSystemTime() * 0.000001; + return time; +} +/* +等待函数，微秒级，从startTime开始等待waitTime微秒 +目前不推荐使用，建议使用AbsoluteTimer类 +*/ +inline void absoluteWait(long long startTime, long long waitTime){ + if(getSystemTime() - startTime > waitTime){ + std::cout << "[WARNING] The waitTime=" << waitTime << " of function absoluteWait is not enough!" << std::endl + << "The program has already cost " << getSystemTime() - startTime << "us." << std::endl; + } + while(getSystemTime() - startTime < waitTime){ + usleep(50); + } +} + + +/* +waitTimeS = 0 means do not care time out +*/ +class AbsoluteTimer{ +public: + AbsoluteTimer(double waitTimeS); + ~AbsoluteTimer(); + void start(); + bool wait(); +private: + void _updateWaitTime(double waitTimeS); + int _timerFd; + uint64_t _missed; + double _waitTime; + double _startTime; + double _leftTime; + double _nextWaitTime; + itimerspec _timerSpec; +}; + +#endif \ No newline at end of file diff --git a/include/unitree_arm_sdk/udp.h b/include/unitree_arm_sdk/udp.h new file mode 100755 index 0000000..5376644 --- /dev/null +++ b/include/unitree_arm_sdk/udp.h @@ -0,0 +1,66 @@ +#ifndef _UNITREE_ARM_UDP_H_ +#define _UNITREE_ARM_UDP_H_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "unitree_arm_sdk/common/arm_motor_common.h" + +enum class BlockYN{ + YES, + NO +}; + +class IOPort{ +public: + IOPort(BlockYN blockYN, size_t recvLength, size_t timeOutUs){ + resetIO(blockYN, recvLength, timeOutUs); + } + virtual ~IOPort(){} + virtual size_t send(uint8_t *sendMsg, size_t sendLength) = 0; + virtual size_t recv(uint8_t *recvMsg, size_t recvLength) = 0; + virtual size_t recv(uint8_t *recvMsg) = 0; + virtual bool sendRecv(std::vector &sendVec, std::vector &recvVec) = 0; + void resetIO(BlockYN blockYN, size_t recvLength, size_t timeOutUs); +protected: + BlockYN _blockYN = BlockYN::NO; + size_t _recvLength; + timeval _timeout; + timeval _timeoutSaved; +}; + + +class UDPPort : public IOPort{ +public: + UDPPort(std::string toIP, uint toPort, uint ownPort, + size_t recvLength = 0, + BlockYN blockYN = BlockYN::NO, + size_t timeOutUs = 20000); + ~UDPPort(); + size_t send(uint8_t *sendMsg, size_t sendMsgLength); + size_t recv(uint8_t *recvMsg, size_t recvLength); + size_t recv(uint8_t *recvMsg); + bool sendRecv(std::vector &sendVec, std::vector &recvVec); +private: + size_t _recvBlock(uint8_t *recvMsg, size_t recvLength); + size_t _recvUnBlock(uint8_t *recvMsg, size_t recvLength); + sockaddr_in _ownAddr, _toAddr, _fromAddr; + socklen_t _sockaddrSize; + int _sockfd; + int _on = 1; + size_t _sentLength; + + uint8_t _sendBytes[238]; // 7 motors + uint8_t _recvBytes[546]; // 7 motors + + fd_set _rSet; +}; + +#endif // _UNITREE_ARM_UDP_H_ \ No newline at end of file diff --git a/include/unitree_arm_sdk/unitree_arm_sdk.h b/include/unitree_arm_sdk/unitree_arm_sdk.h new file mode 100644 index 0000000..987246a --- /dev/null +++ b/include/unitree_arm_sdk/unitree_arm_sdk.h @@ -0,0 +1,13 @@ +#ifndef _UNITREE_ARM_SDK_H_ +#define _UNITREE_ARM_SDK_H_ + +#include "unitree_arm_sdk/timer.h" +#include "unitree_arm_sdk/common/arm_motor_common.h" +#include "unitree_arm_sdk/common/arm_common.h" +#include "unitree_arm_sdk/common/quadruped_common.h" +#include "unitree_arm_sdk/udp.h" +#include "unitree_arm_sdk/loop.h" +#include "unitree_arm_sdk/joystick.h" +#include "unitree_arm_sdk/keyboard.h" + +#endif diff --git a/lib/libZ1_SDK_Linux64.so b/lib/libZ1_SDK_Linux64.so new file mode 100755 index 0000000..4f2cf02 Binary files /dev/null and b/lib/libZ1_SDK_Linux64.so differ diff --git a/thirdparty/eigen-3.3.9.zip b/thirdparty/eigen-3.3.9.zip new file mode 100644 index 0000000..bc9412a Binary files /dev/null and b/thirdparty/eigen-3.3.9.zip differ diff --git a/thirdparty/rbdl-2.6.0.zip b/thirdparty/rbdl-2.6.0.zip new file mode 100644 index 0000000..31658c4 Binary files /dev/null and b/thirdparty/rbdl-2.6.0.zip differ