diff --git a/.gitignore b/.gitignore deleted file mode 100644 index c795b05..0000000 --- a/.gitignore +++ /dev/null @@ -1 +0,0 @@ -build \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index 0600e65..dbf7a00 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -5,6 +5,7 @@ project(z1_sdk) include_directories( include + unitreeArm # ${EIGEN_PATH} ) @@ -12,24 +13,14 @@ 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 +target_link_libraries(example_keyboard_send Z1_SDK_Linux64) + +add_executable(bigDemo examples/bigDemo.cpp) +target_link_libraries(bigDemo Z1_SDK_Linux64) + +add_executable(getJointGripperState examples/getJointGripperState.cpp) +target_link_libraries(getJointGripperState Z1_SDK_Linux64) diff --git a/README.md b/README.md index c16d2d0..55e1d8f 100644 --- a/README.md +++ b/README.md @@ -1,76 +1 @@ -# 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 -``` +see [unitree-z1-docs](http://dev-z1.unitree.com/) \ No newline at end of file diff --git a/examples/bigDemo.cpp b/examples/bigDemo.cpp new file mode 100644 index 0000000..878d7b0 --- /dev/null +++ b/examples/bigDemo.cpp @@ -0,0 +1,119 @@ +#include "unitreeArm.h" + +class Custom : public unitreeArm{ + +public: + Custom(){}; + ~Custom(){}; + void armCtrlByFSM(); + void armCtrlByTraj(); +}; + + +void Custom::armCtrlByFSM() { + Vec6 posture[2]; + + std::cout << "[JOINTCTRL]" << std::endl; + setFsm(ArmFSMState::JOINTCTRL); + sleep(1);// wait for a while for the command to execute + + std::cout << "[TOSTATE] forward" << std::endl; + labelRun("forward"); + sleep(1); + + std::cout << "[MOVEJ]" << std::endl; + posture[0]<<0.5,0.1,0.1,0.5,-0.2,0.5; + MoveJ(posture[0], -1.0); + sleep(1); + + std::cout << "[MOVEL]" << std::endl; + posture[0] << 0,0,0,0.45,-0.2,0.2; + MoveL(posture[0]); + sleep(1); + + std::cout << "[MOVEC]" << std::endl; + posture[0] << 0,0,0,0.4,0,0.3; + posture[1] << 0,0,0,0.45,0.2,0.2; + MoveC(posture[0], posture[1]); + sleep(1); + + std::cout << "[BACKTOSTART]" << std::endl; + backToStart(); + sleep(1); +} + +void Custom::armCtrlByTraj(){ + Vec6 posture[2]; + + std::cout << "[TOSTATE] forward" << std::endl; + labelRun("forward"); + sleep(1); + + int order=1; + + // No.1 trajectory + _trajCmd.trajOrder = order++; + _trajCmd.trajType = TrajType::MoveJ; + _trajCmd.maxSpeed = 0.3; + _trajCmd.gripperPos = -1.0; + posture[0] << 0.5,0.1,0.1,0.5,-0.2,0.5; + // posture[0] << 0.0,0.0,0,0.5,-0.2,0.5; + _trajCmd.posture[0] = Vec6toPosture(posture[0]); + setTraj(_trajCmd); + usleep(500000); + + // No.2 trajectory + _trajCmd.trajOrder = order++; + _trajCmd.trajType = TrajType::MoveL; + _trajCmd.maxSpeed = 0.2; + _trajCmd.gripperPos = 0.0; + posture[0] << 0,0,0,0.45,-0.2,0.2; + _trajCmd.posture[0] = Vec6toPosture(posture[0]); + setTraj(_trajCmd); + usleep(500000); + + // No.4 trajectory + _trajCmd.trajOrder = order++; + _trajCmd.trajType = TrajType::Stop; + _trajCmd.stopTime = 2.0; + _trajCmd.gripperPos = 0.0; + setTraj(_trajCmd); + usleep(500000); + + // No.4 trajectory + _trajCmd.trajOrder = order++; + _trajCmd.trajType = TrajType::MoveC; + _trajCmd.maxSpeed = 0.3; + _trajCmd.gripperPos = -1.0; + posture[0] << 0,0,0,0.45,0,0.4; + posture[1] << 0,0,0,0.45,0.2,0.2; + _trajCmd.posture[0] = Vec6toPosture(posture[0]); + _trajCmd.posture[1] = Vec6toPosture(posture[1]); + setTraj(_trajCmd); + usleep(500000); + + //run + setFsm(ArmFSMState::TRAJECTORY); + + // wait for completion + while (_recvState.state != ArmFSMState::JOINTCTRL){ + usleep(4000); + } +} + + +int main() { + Custom custom; + + custom.backToStart(); + sleep(1); + + if(false){ + custom.armCtrlByFSM(); + }else{ + custom.armCtrlByTraj(); + } + + custom.backToStart(); + return 0; +} \ No newline at end of file diff --git a/examples/example_keyboard_recv.cpp b/examples/example_keyboard_recv.cpp deleted file mode 100755 index 15d35f0..0000000 --- a/examples/example_keyboard_recv.cpp +++ /dev/null @@ -1,53 +0,0 @@ -/***************************************************************** -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 index d316b88..c22c10f 100755 --- a/examples/example_keyboard_send.cpp +++ b/examples/example_keyboard_send.cpp @@ -41,7 +41,7 @@ int main(){ events.push_back(new ValueAction("down", "up", 1.0)); cmdPanel = new UnitreeKeyboardUDPSend(events, emptyAction); - + while(running){ usleep(1000000); } diff --git a/examples/example_lowCmd_send.cpp b/examples/example_lowCmd_send.cpp index aefa302..2516df6 100644 --- a/examples/example_lowCmd_send.cpp +++ b/examples/example_lowCmd_send.cpp @@ -21,7 +21,7 @@ struct SimulationPD : ConfigPD{ K_P[4] = 15; K_P[5] = 15; K_P[6] = 15; - + K_W[0] = 5; K_W[1] = 10; K_W[2] = 10; diff --git a/examples/example_state_recv.cpp b/examples/example_state_recv.cpp deleted file mode 100755 index d31a74d..0000000 --- a/examples/example_state_recv.cpp +++ /dev/null @@ -1,27 +0,0 @@ -/***************************************************************** -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 deleted file mode 100755 index a229aab..0000000 --- a/examples/example_state_send.cpp +++ /dev/null @@ -1,219 +0,0 @@ -/***************************************************************** -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/examples/getJointGripperState.cpp b/examples/getJointGripperState.cpp new file mode 100644 index 0000000..bc6d0fc --- /dev/null +++ b/examples/getJointGripperState.cpp @@ -0,0 +1,50 @@ +#include "unitreeArm.h" + +class Custom : public unitreeArm { +public: + Custom(){}; + ~Custom(){}; + void printJointGripperState(); + Posture _gripperState; +}; + +void Custom::printJointGripperState() { + getJointState(_jointState); + std::cout< + +namespace UNITREE_LEGGED_SDK +{ + + constexpr int HIGHLEVEL = 0x00; + constexpr int LOWLEVEL = 0xff; + constexpr double PosStopF = (2.146E+9f); + constexpr double VelStopF = (16000.0f); + +#pragma pack(1) + + typedef struct + { + float x; + float y; + float z; + } Cartesian; + + typedef struct + { + float quaternion[4]; // quaternion, normalized, (w,x,y,z) + float gyroscope[3]; // angular velocity （unit: rad/s) + float accelerometer[3]; // m/(s2) + float rpy[3]; // 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) + uint32_t reserve[2]; // in reserve[1] returns the brake state. + } 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) ) + uint32_t reserve[3]; // in reserve[0] sends the brake cmd. + } MotorCmd; // motor control + + typedef struct + { + uint8_t levelFlag; // flag to distinguish high level or low level + uint16_t commVersion; + uint16_t robotID; + uint32_t SN; + uint8_t bandWidth; + IMU imu; + MotorState motorState[20]; + int16_t footForce[4]; // force sensors + int16_t footForceEst[4]; // force sensors + uint32_t tick; // reference real-time from motion controller (unit: us) + uint8_t wirelessRemote[40]; // wireless commands + uint32_t reserve; + uint32_t crc; + } LowState; // low level feedback + + typedef struct + { + uint8_t levelFlag; + uint16_t commVersion; + uint16_t robotID; + uint32_t SN; + uint8_t bandWidth; + MotorCmd motorCmd[20]; + LED led[4]; + uint8_t wirelessRemote[40]; + uint32_t reserve; + uint32_t crc; + } LowCmd; // low level control + + typedef struct + { + uint8_t levelFlag; + uint16_t commVersion; + uint16_t robotID; + uint32_t SN; + uint8_t bandWidth; + uint8_t mode; + IMU imu; + float position[3]; // (unit: m), from robot own odometry in inertial frame, usually drift + float velocity[3]; // (unit: m/s), forwardSpeed, sideSpeed, updownSpeed in body frame + float yawSpeed; // (unit: rad/s), rotateSpeed in body frame. + Cartesian footPosition2Body[4]; // foot position relative to body + Cartesian footSpeed2Body[4]; // foot speed relative to body + int16_t footForce[4]; + uint8_t wirelessRemote[40]; + uint32_t reserve; + uint32_t crc; + } HighState; // high level feedback + + typedef struct + { + uint8_t levelFlag; + uint16_t commVersion; + uint16_t robotID; + uint32_t SN; + uint8_t bandWidth; + uint8_t mode; // 0.idle, default stand | 1.force stand (controlled by dBodyHeight + rpy) + // 2.target velocity walking (controlled by velocity + yawSpeed) + // 3.target position walking (controlled by position + rpy[2]) + // 4. path mode walking (reserve for future release) + // 5. position stand down. |6. position stand up |7. damping mode | 8. recovery mode + uint8_t gaitType; // 0.trot | 1. trot running | 2.climb stair + uint8_t speedLevel; // 0. default low speed. 1. medium speed 2. high speed. during walking + float dFootRaiseHeight; // (unit: m), swing foot height adjustment from default swing height. + float dBodyHeight; // (unit: m), body height adjustment from default body height. + float position[2]; // (unit: m), desired x and y position in inertial frame. + float rpy[3]; // (unit: rad), desired yaw-pitch-roll euler angle, expressed in roll(rpy[0]) pitch(rpy[1]) yaw(rpy[2]) + float velocity[2]; // (unit: m/s), forwardSpeed, sideSpeed in body frame. + float yawSpeed; // (unit: rad/s), rotateSpeed in body frame. + LED led[4]; + uint8_t wirelessRemote[40]; + uint32_t reserve; + uint32_t crc; + } HighCmd; // high level control uint8_t mode + +#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 + + constexpr int HIGH_CMD_LENGTH = (sizeof(HighCmd)); + constexpr int HIGH_STATE_LENGTH = (sizeof(HighState)); + +} + +#endif \ 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 index 380f8d5..bc39120 100755 --- a/include/unitree_arm_sdk/common/arm_common.h +++ b/include/unitree_arm_sdk/common/arm_common.h @@ -20,6 +20,7 @@ enum class ArmFSMState{ TEACH, TEACHREPEAT, CALIBRATION, + SETTRAJ, DANCE00, DANCE01, DANCE02, @@ -50,6 +51,13 @@ enum class ArmFSMValue{ UP }; +enum class TrajType{ + MoveJ, + MoveL, + MoveC, + Stop +}; + // 20 Byte struct JointCmd{ float T; @@ -61,55 +69,51 @@ struct JointCmd{ // 16 Byte struct JointState{ + float buf[0]; 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; + double roll; + double pitch; + double yaw; + double x; + double y; + double z; }; -// 48 Byte -struct MoveC{ - Posture middlePosture; - Posture endPosture; +struct TrajCmd{ + TrajType trajType; + Posture posture[2]; + double gripperPos; + double maxSpeed; + double stopTime; + int trajOrder; }; -// 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]; + char name[10]; JointCmd jointCmd[7]; + TrajCmd trajCmd; }; -// 2+4+4+140 = 150 Byte struct SendCmd{ uint8_t head[2]; ArmFSMState state; @@ -117,7 +121,6 @@ struct SendCmd{ ValueUnion valueUnion; }; -// 2+4+112+24 = 142 Byte struct RecvState{ uint8_t head[2]; ArmFSMState state; diff --git a/include/unitree_arm_sdk/common/quadruped_common.h b/include/unitree_arm_sdk/common/b1_common.h similarity index 98% rename from include/unitree_arm_sdk/common/quadruped_common.h rename to include/unitree_arm_sdk/common/b1_common.h index 9401655..b875685 100644 --- a/include/unitree_arm_sdk/common/quadruped_common.h +++ b/include/unitree_arm_sdk/common/b1_common.h @@ -1,5 +1,5 @@ -#ifndef _UNITREE_ARM_QUADRUPED_COMMON_H_ -#define _UNITREE_ARM_QUADRUPED_COMMON_H_ +#ifndef _UNITREE_ARM_B1_COMMON_H_ +#define _UNITREE_ARM_B1_COMMON_H_ #include #include @@ -206,7 +206,6 @@ namespace UNITREE_LEGGED_SDK 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 index cb161e2..57690c3 100755 --- a/include/unitree_arm_sdk/joystick.h +++ b/include/unitree_arm_sdk/joystick.h @@ -4,9 +4,17 @@ #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" +#define CTRL_BY_ALIENGO_JOYSTICK + +#ifdef CTRL_BY_ALIENGO_JOYSTICK +#include "unitree_arm_sdk/common/aliengo_common.h" +#else +#include "unitree_arm_sdk/common/b1_common.h" +#endif + + using namespace UNITREE_LEGGED_SDK; class UnitreeJoystick : public CmdPanel{ diff --git a/include/unitree_arm_sdk/mathTypes.h b/include/unitree_arm_sdk/mathTypes.h new file mode 100644 index 0000000..2ca09b1 --- /dev/null +++ b/include/unitree_arm_sdk/mathTypes.h @@ -0,0 +1,99 @@ +#ifndef MATHTYPES_H +#define MATHTYPES_H + +#include + +/************************/ +/******** Vector ********/ +/************************/ +// 2x1 Vector +using Vec2 = typename Eigen::Matrix; + +// 3x1 Vector +using Vec3 = typename Eigen::Matrix; + +// 4x1 Vector +using Vec4 = typename Eigen::Matrix; + +// 6x1 Vector +using Vec6 = typename Eigen::Matrix; + +// Quaternion +using Quat = typename Eigen::Matrix; + +// 4x1 Integer Vector +using VecInt4 = typename Eigen::Matrix; + +// 12x1 Vector +using Vec12 = typename Eigen::Matrix; + +// 18x1 Vector +using Vec18 = typename Eigen::Matrix; + +// Dynamic Length Vector +using VecX = typename Eigen::Matrix; + +/************************/ +/******** Matrix ********/ +/************************/ +// Rotation Matrix +using RotMat = typename Eigen::Matrix; + +// Homogenous Matrix +using HomoMat = typename Eigen::Matrix; + +// 2x2 Matrix +using Mat2 = typename Eigen::Matrix; + +// 3x3 Matrix +using Mat3 = typename Eigen::Matrix; + +// 3x4 Matrix, each column is a 3x1 vector +using Vec34 = typename Eigen::Matrix; + +// 6x6 Matrix +using Mat6 = typename Eigen::Matrix; + +// 12x12 Matrix +using Mat12 = typename Eigen::Matrix; + +// 3x3 Identity Matrix +#define I3 Eigen::MatrixXd::Identity(3, 3) + +// 6x6 Identity Matrix +#define I6 Eigen::MatrixXd::Identity(6, 6) + +// 12x12 Identity Matrix +#define I12 Eigen::MatrixXd::Identity(12, 12) + +// 18x18 Identity Matrix +#define I18 Eigen::MatrixXd::Identity(18, 18) + +// Dynamic Size Matrix +using MatX = typename Eigen::Matrix; + +/************************/ +/****** Functions *******/ +/************************/ +inline Vec34 vec12ToVec34(Vec12 vec12){ + Vec34 vec34; + for(int i(0); i < 4; ++i){ + vec34.col(i) = vec12.segment(3*i, 3); + } + return vec34; +} + +inline Vec12 vec34ToVec12(Vec34 vec34){ + Vec12 vec12; + for(int i(0); i < 4; ++i){ + vec12.segment(3*i, 3) = vec34.col(i); + } + return vec12; +} + +template +inline VecX stdVecToEigenVec(T stdVec){ + VecX eigenVec = Eigen::VectorXd::Map(&stdVec[0], stdVec.size()); + return eigenVec; +} +#endif // MATHTYPES_H \ No newline at end of file diff --git a/include/unitree_arm_sdk/udp.h b/include/unitree_arm_sdk/udp.h index 5376644..6b38a65 100755 --- a/include/unitree_arm_sdk/udp.h +++ b/include/unitree_arm_sdk/udp.h @@ -10,6 +10,7 @@ #include #include #include +#include "unitree_arm_sdk/timer.h" #include "unitree_arm_sdk/common/arm_motor_common.h" @@ -29,11 +30,13 @@ public: 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); + bool isDisConnect; protected: BlockYN _blockYN = BlockYN::NO; size_t _recvLength; timeval _timeout; timeval _timeoutSaved; + uint16_t _isDisConnectCnt; }; diff --git a/include/unitree_arm_sdk/unitree_arm_sdk.h b/include/unitree_arm_sdk/unitree_arm_sdk.h index 987246a..87df13f 100644 --- a/include/unitree_arm_sdk/unitree_arm_sdk.h +++ b/include/unitree_arm_sdk/unitree_arm_sdk.h @@ -4,10 +4,10 @@ #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" +#include "mathTypes.h" #endif diff --git a/lib/libZ1_SDK_Linux64.so b/lib/libZ1_SDK_Linux64.so index 4f2cf02..d5a3308 100755 Binary files a/lib/libZ1_SDK_Linux64.so and b/lib/libZ1_SDK_Linux64.so differ diff --git a/thirdparty/lcm-1.4.0.zip b/thirdparty/lcm-1.4.0.zip new file mode 100644 index 0000000..ab421d7 Binary files /dev/null and b/thirdparty/lcm-1.4.0.zip differ diff --git a/unitreeArm/unitreeArm.cpp b/unitreeArm/unitreeArm.cpp new file mode 100644 index 0000000..a1cd86c --- /dev/null +++ b/unitreeArm/unitreeArm.cpp @@ -0,0 +1,134 @@ +#include "unitreeArm.h" + +// constructor +// since the SDK communicates with z1_ctrl, the udp is set to the loopback address by default +unitreeArm::unitreeArm():_udp("127.0.0.1", 8071, 8072, sizeof(RecvState), BlockYN::NO, 500000) { + _sendCmd.head[0] = 0xFE; + _sendCmd.head[1] = 0xFF; + + _udpThread = new LoopFunc("udp", 0.004, boost::bind(&unitreeArm::UDPSendRecv, this)); + _udpThread->start(); +} + +unitreeArm::~unitreeArm() { + delete _udpThread; +} + +// udp send and receive function +// it has created a thread in the constructor that used to automatically process the arm state and command datas +void unitreeArm::UDPSendRecv() { + _udp.send((uint8_t*)&_sendCmd, sizeof(SendCmd)); + _udp.recv((uint8_t*)&_recvState, sizeof(RecvState)); +} + +// set current FSM to fsm, this is equivalent to key switching when using keyboard control +// input: fsm [the specific format can be viewed in class ArmFSMState ] +void unitreeArm::setFsm(ArmFSMState fsm) { + while (_recvState.state != fsm){ + _sendCmd.state = fsm; + usleep(deltaTime); + } +} + +void unitreeArm::backToStart() { + setFsm(ArmFSMState::BACKTOSTART); + //for those states that automatically transion to the joint space state, it's recommmanded to wait for it to finish + _sendCmd.state = ArmFSMState::INVALID; + while (_recvState.state != ArmFSMState::JOINTCTRL){ + usleep(deltaTime); + } +} + +// move the arm to the posture indicated by the label +// the specific label name can be viewed in savedArmStates.csv +void unitreeArm::labelRun(std::string label) { + while (_recvState.state != ArmFSMState::TOSTATE){ + _sendCmd.state = ArmFSMState::TOSTATE; + strcpy(_sendCmd.valueUnion.name, label.c_str()); + usleep(deltaTime); + } + _sendCmd.state = ArmFSMState::INVALID; + while (_recvState.state != ArmFSMState::JOINTCTRL){ + usleep(deltaTime); + } +} + +// save current posture to a label +// the label name should be less than 10 chars +void unitreeArm::labelSave(std::string label) { + _sendCmd.state = ArmFSMState::SAVESTATE; + strcpy(_sendCmd.valueUnion.name, label.c_str()); + while (_recvState.state != ArmFSMState::SAVESTATE){ + usleep(deltaTime); + } +} + +// reach the target posture by directly moving the joint +void unitreeArm::MoveJ(Vec6 moveJCmd) { + _sendCmd.valueUnion.trajCmd.posture[0] = Vec6toPosture(moveJCmd); + + _sendCmd.state = ArmFSMState::MOVEJ; + while (_recvState.state != ArmFSMState::MOVEJ){ + usleep(deltaTime); + } + _sendCmd.state = ArmFSMState::INVALID; + while (_recvState.state != ArmFSMState::JOINTCTRL){ + usleep(deltaTime); + } +} + +// reach the target posture by directly moving the joint +void unitreeArm::MoveJ(Vec6 moveJCmd, double gripperPos) { + _sendCmd.valueUnion.jointCmd[6].Pos = gripperPos; + MoveJ(moveJCmd); +} + +// the end effector reaches the target point in a straight line trajectory +void unitreeArm::MoveL(Vec6 moveLCmd) { + _sendCmd.valueUnion.trajCmd.posture[0] = Vec6toPosture(moveLCmd); + + _sendCmd.state = ArmFSMState::MOVEL; + while (_recvState.state != ArmFSMState::MOVEL){ + usleep(deltaTime); + } + _sendCmd.state = ArmFSMState::INVALID; + while (_recvState.state != ArmFSMState::JOINTCTRL){ + usleep(deltaTime); + } +} + +// the end effector reaches the target point in a circular arc trajectory +void unitreeArm::MoveC(Vec6 middleP, Vec6 endP){ + _sendCmd.valueUnion.trajCmd.posture[0] = Vec6toPosture(middleP); + _sendCmd.valueUnion.trajCmd.posture[1] = Vec6toPosture(endP); + + _sendCmd.state = ArmFSMState::MOVEC; + while (_recvState.state != ArmFSMState::MOVEC){ + usleep(deltaTime); + } + _sendCmd.state = ArmFSMState::INVALID; + while (_recvState.state != ArmFSMState::JOINTCTRL){ + usleep(deltaTime); + } +} + +void unitreeArm::getJointState(JointState* jointState) { + for(int i=0;i<7;i++) { + jointState[i] = _recvState.jointState[i]; + } +} + +void unitreeArm::getGripperState(Posture& gripperState) { + gripperState = _recvState.cartesianState; +} + +void unitreeArm::setTraj(TrajCmd trajCmd){ + if(_recvState.state != ArmFSMState::SETTRAJ){ + _sendCmd.valueUnion.trajCmd.trajOrder = 0; + setFsm(ArmFSMState::SETTRAJ); + } + + if(_sendCmd.valueUnion.trajCmd.trajOrder == (trajCmd.trajOrder - 1)){// make sure [order] is sequential + _sendCmd.valueUnion.trajCmd = trajCmd; + } +} diff --git a/unitreeArm/unitreeArm.h b/unitreeArm/unitreeArm.h new file mode 100644 index 0000000..2141ab8 --- /dev/null +++ b/unitreeArm/unitreeArm.h @@ -0,0 +1,58 @@ +#ifndef __UNITREEARM_H +#define __UNITREEARM_H + +#include +#include "unitree_arm_sdk/unitree_arm_sdk.h" + +Vec6 PosturetoVec6(const Posture p){ + Vec6 posture; + posture(0) = p.roll; + posture(1) = p.pitch; + posture(2) = p.yaw; + posture(3) = p.x; + posture(4) = p.y; + posture(5) = p.z; + return posture; +} + +Posture Vec6toPosture(const Vec6 p){ + Posture posture; + posture.roll = p(0); + posture.pitch = p(1); + posture.yaw = p(2); + posture.x = p(3); + posture.y = p(4); + posture.z = p(5); + return posture; +} + +class unitreeArm{ +public: + unitreeArm(); + ~unitreeArm(); + void UDPSendRecv(); + void InitCmdData(SendCmd& sendCmd); + void setFsm(ArmFSMState fsm); + void backToStart(); + void labelRun(std::string label); + void labelSave(std::string label); + void MoveJ(Vec6 moveJCmd); + void MoveJ(Vec6 moveJCmd, double gripperPos); + void MoveL(Vec6 moveLCmd); + void MoveC(Vec6 middleP, Vec6 endP); + void getJointState(JointState* jointstate); + void getGripperState(Posture& gripperState); + void setTraj(TrajCmd trajCmd); + + RecvState _recvState; // the arm state receive from udp + JointState _jointState[7]; // 6 joint angles and one gripper angle + SendCmd _sendCmd; // command to control the arm + TrajCmd _trajCmd; + +private: + UDPPort _udp; + float deltaTime = 4000; //us + LoopFunc *_udpThread; +}; + +#endif \ No newline at end of file