├── LICENSE ├── README.md ├── unitree_actuator_sdk ├── .gitignore ├── .vscode │ └── settings.json ├── CMakeLists.txt ├── ChangeID_Tool │ ├── Linux │ │ └── ChangeID │ └── Windows │ │ ├── ChangeID_Win64.exe │ │ └── libUnitree_motor_SDK_Win64.dll ├── LICENSE ├── ReadMe.md ├── include │ ├── LSerial.h │ ├── motor_ctrl.h │ └── motor_msg.h ├── lib │ ├── libUnitree_motor_SDK_ARM32.so │ ├── libUnitree_motor_SDK_ARM64.so │ ├── libUnitree_motor_SDK_Linux32.so │ ├── libUnitree_motor_SDK_Linux64.so │ └── libUnitree_motor_SDK_Win64.dll ├── script │ ├── check.py │ └── typedef.py ├── src │ ├── check.c │ └── check.cpp └── unitree_motor_ctrl │ ├── CMakeLists.txt │ ├── include │ └── unitree_motor_ctrl │ │ ├── LSerial.h │ │ ├── motor_ctrl.h │ │ └── motor_msg.h │ ├── lib │ └── libUnitree_motor_SDK_Linux.so │ ├── package.xml │ ├── script │ ├── __pycache__ │ │ └── typedef.cpython-36.pyc │ ├── check.py │ └── typedef.py │ └── src │ └── check.cpp ├── unitree_guide ├── CMakeLists.txt ├── include │ ├── FSM │ │ ├── FSM.h │ │ ├── FSMState.h │ │ ├── State_BalanceTest.h │ │ ├── State_FixedStand.h │ │ ├── State_FreeStand.h │ │ ├── State_Passive.h │ │ ├── State_StepTest.h │ │ ├── State_SwingTest.h │ │ ├── State_Trotting.h │ │ └── State_move_base.h │ ├── Gait │ │ ├── FeetEndCal.h │ │ ├── GaitGenerator.h │ │ └── WaveGenerator.h │ ├── common │ │ ├── LowPassFilter.h │ │ ├── PyPlot.h │ │ ├── enumClass.h │ │ ├── mathTools.h │ │ ├── mathTypes.h │ │ ├── timeMarker.h │ │ ├── unitreeLeg.h │ │ └── unitreeRobot.h │ ├── control │ │ ├── BalanceCtrl.h │ │ ├── ControlFrame.h │ │ ├── CtrlComponents.h │ │ └── Estimator.h │ ├── interface │ │ ├── CmdPanel.h │ │ ├── IOInterface.h │ │ ├── IOROS.h │ │ ├── IOSDK.h │ │ ├── KeyBoard.h │ │ └── WirelessHandle.h │ ├── message │ │ ├── LowlevelCmd.h │ │ ├── LowlevelState.h │ │ └── unitree_joystick.h │ └── thirdParty │ │ ├── matplotlibcpp.h │ │ └── quadProgpp │ │ ├── Array.hh │ │ └── QuadProg++.hh ├── launch │ └── gazeboSim.launch ├── library │ ├── unitree_legged_sdk-3.8.0 │ │ ├── example │ │ │ ├── example_joystick.cpp │ │ │ ├── example_position.cpp │ │ │ ├── example_torque.cpp │ │ │ ├── example_velocity.cpp │ │ │ └── example_walk.cpp │ │ ├── example_py │ │ │ ├── example_position.py │ │ │ ├── example_torque.py │ │ │ ├── example_velocity.py │ │ │ └── example_walk.py │ │ ├── include │ │ │ └── unitree_legged_sdk │ │ │ │ ├── a1_const.h │ │ │ │ ├── aliengo_const.h │ │ │ │ ├── comm.h │ │ │ │ ├── go1_const.h │ │ │ │ ├── joystick.h │ │ │ │ ├── loop.h │ │ │ │ ├── quadruped.h │ │ │ │ ├── safety.h │ │ │ │ ├── udp.h │ │ │ │ └── unitree_legged_sdk.h │ │ └── lib │ │ │ ├── cpp │ │ │ ├── amd64 │ │ │ │ └── libunitree_legged_sdk.a │ │ │ └── arm64 │ │ │ │ └── libunitree_legged_sdk.a │ │ │ └── python │ │ │ ├── amd64 │ │ │ └── robot_interface.cpython-38-x86_64-linux-gnu.so │ │ │ └── arm64 │ │ │ ├── robot_interface.cpython-37m-aarch64-linux-gnu.so │ │ │ └── robot_interface.cpython-38-aarch64-linux-gnu.so │ ├── unitree_legged_sdk_3.2 │ │ ├── include │ │ │ └── unitree_legged_sdk │ │ │ │ ├── a1_const.h │ │ │ │ ├── aliengo_const.h │ │ │ │ ├── comm.h │ │ │ │ ├── lcm.h │ │ │ │ ├── lcm_server.h │ │ │ │ ├── loop.h │ │ │ │ ├── quadruped.h │ │ │ │ ├── safety.h │ │ │ │ ├── udp.h │ │ │ │ ├── unitree_joystick.h │ │ │ │ └── unitree_legged_sdk.h │ │ └── lib │ │ │ ├── libunitree_legged_sdk_amd64.so │ │ │ ├── libunitree_legged_sdk_arm32.so │ │ │ └── libunitree_legged_sdk_arm64.so │ └── unitree_legged_sdk_3.4 │ │ ├── include │ │ └── unitree_legged_sdk │ │ │ ├── a1_const.h │ │ │ ├── aliengo_const.h │ │ │ ├── comm.h │ │ │ ├── lcm.h │ │ │ ├── loop.h │ │ │ ├── quadruped.h │ │ │ ├── safety.h │ │ │ ├── udp.h │ │ │ └── unitree_legged_sdk.h │ │ └── lib │ │ ├── libunitree_legged_sdk_arm64.so │ │ └── libunitree_legged_sdk_x86.so ├── package.xml └── src │ ├── FSM │ ├── FSM.cpp │ ├── FSMState.cpp │ ├── State_BalanceTest.cpp │ ├── State_FixedStand.cpp │ ├── State_FreeStand.cpp │ ├── State_Passive.cpp │ ├── State_StepTest.cpp │ ├── State_SwingTest.cpp │ ├── State_Trotting.cpp │ └── State_move_base.cpp │ ├── Gait │ ├── FeetEndCal.cpp │ ├── GaitGenerator.cpp │ └── WaveGenerator.cpp │ ├── common │ ├── LowPassFilter.cpp │ ├── unitreeLeg.cpp │ └── unitreeRobot.cpp │ ├── control │ ├── BalanceCtrl.cpp │ ├── ControlFrame.cpp │ └── Estimator.cpp │ ├── interface │ ├── IOROS.cpp │ ├── IOSDK.cpp │ ├── KeyBoard.cpp │ └── WirelessHandle.cpp │ ├── main.cpp │ └── quadProgpp │ ├── Array.cc │ └── QuadProg++.cc └── unitree_move_base ├── .gitignore ├── CMakeLists.txt ├── config ├── base_local_planner_params.yaml ├── costmap_common_params.yaml ├── global_costmap_params.yaml ├── local_costmap_params.yaml ├── move_base.rviz └── pointCloud_to_laserScan_params.yaml ├── launch ├── gazebo_move_base.launch ├── move_base.launch ├── pointCloud2LaserScan.launch ├── robotTF.launch └── rvizMoveBase.launch ├── package.xml └── worlds ├── indoor.world ├── model.config ├── model.sdf └── smallRoom.world /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2016-2022 HangZhou YuShu TECHNOLOGY CO.,LTD. ("Unitree Robotics") 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | * Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | * Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | * Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | A more detailed unitree_guide document is available at unitree [developer site](https://support.unitree.com/home/zh/Algorithm_Practice).( only Chinese version now ) 2 | 3 | # Overview 4 | 5 | The unitree_guide is an open source project for controlling the quadruped robot of Unitree Robotics, and it is also the software project accompanying [《四足机器人控制算法--建模、控制与实践》](https://detail.tmall.com/item.htm?spm=a212k0.12153887.0.0.5487687dBgiovR&id=704510718152) published by Unitree Robotics. 6 | 7 | # Quick Start 8 | The following will quickly introduce the use of unitree_guide in the gazebo simulator. For more usage, please refer to 《四足机器人控制算法--建模、控制与实践》. 9 | ## Environment 10 | We recommand users to run this project in Ubuntu 18.04 and ROS melodic environment. 11 | ## Dependencies 12 | 1. [unitree_guide](https://github.com/unitreerobotics/unitree_guide)
13 | 2. [unitree_ros](https://github.com/unitreerobotics/unitree_ros)
14 | 3. [unitree_legged_msgs](https://github.com/unitreerobotics/unitree_ros_to_real)(Note that: unitree_legged_real package should not be a part of dependencies)
15 | 16 | Put these three packages in the src folder of a ROS workspace. 17 | 18 | ## build 19 | Open a terminal and switch the directory to the ros workspace containing unitree_guide, then run the following command to build the project: 20 | ``` 21 | catkin_make 22 | ``` 23 | If you have any error in this step, you can raise an issue to us. 24 | ## run 25 | In the same terminal, run the following command step by step: 26 | ``` 27 | source ./devel/setup.bash 28 | ``` 29 | To open the gazebo simulator, run: 30 | ``` 31 | roslaunch unitree_guide gazeboSim.launch 32 | ``` 33 | 34 | For starting the controller, open an another terminal and switch to the same directory, then run the following command: 35 | ``` 36 | ./devel/lib/unitree_guide/junior_ctrl 37 | ``` 38 | 39 | ## usage 40 | After starting the controller, the robot will lie on the ground of the simulator, then press the '2' key on the keyboard to switch the robot's finite state machine (FSM) from **Passive**(initial state) to **FixedStand**, then press the '4' key to switch the FSM from **FixedStand** to **Trotting**, now you can press the 'w' 'a' 's' 'd' key to control the translation of the robot, and press the 'j' 'l' key to control the rotation of the robot. Press the Spacebar, the robot will stop and stand on the ground 41 | . (If there is no response, you need to click on the terminal opened for starting the controller and then repeat the previous operation) 42 | 43 | # Note 44 | Unitree_guide provides a basic quadruped robot controller for beginners. To achive better performance, additional fine tuning of parameters or more advanced methods (such as MPC etc.) might be required. Any contribution and good idea from the robotics community are all welcome. Feel free to raise an issue ~
45 | -------------------------------------------------------------------------------- /unitree_actuator_sdk/.gitignore: -------------------------------------------------------------------------------- 1 | build 2 | bin 3 | script/__pycache__/ 4 | -------------------------------------------------------------------------------- /unitree_actuator_sdk/.vscode/settings.json: -------------------------------------------------------------------------------- 1 | { 2 | "python.pythonPath": "/home/bian/.platformio/penv/bin/python3", 3 | "python.linting.pylintEnabled": true, 4 | "python.linting.enabled": true, 5 | "files.associations": { 6 | "array": "c", 7 | "*.tcc": "c", 8 | "memory": "c", 9 | "istream": "c", 10 | "functional": "c", 11 | "tuple": "c", 12 | "utility": "c" 13 | } 14 | } -------------------------------------------------------------------------------- /unitree_actuator_sdk/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(motor_ctrl) 3 | 4 | include_directories(include) 5 | 6 | link_directories(lib) 7 | 8 | # detect 32/64 bits 9 | set(BITNESS 32) 10 | if(CMAKE_SIZEOF_VOID_P EQUAL 8) 11 | set(BITNESS 64) 12 | endif() 13 | 14 | IF(CMAKE_SYSTEM_NAME MATCHES "Linux") 15 | IF(BITNESS EQUAL 64) 16 | set(EXTRA_LIBS libUnitree_motor_SDK_Linux64.so) 17 | ELSEIF(BITNESS EQUAL 32) 18 | set(EXTRA_LIBS libUnitree_motor_SDK_Linux32.so) 19 | ENDIF() 20 | ELSEIF(CMAKE_SYSTEM_NAME MATCHES "Windows") 21 | set(EXTRA_LIBS libUnitree_motor_SDK_Win64.dll) 22 | ENDIF() 23 | 24 | add_executable(check_c src/check.c) 25 | target_link_libraries(check_c ${EXTRA_LIBS}) 26 | 27 | # add_executable(check3motor src/check3motor.c) 28 | # target_link_libraries(check3motor ${EXTRA_LIBS}) 29 | 30 | add_executable(check_cpp src/check.cpp) 31 | target_link_libraries(check_cpp ${EXTRA_LIBS}) 32 | 33 | set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) -------------------------------------------------------------------------------- /unitree_actuator_sdk/ChangeID_Tool/Linux/ChangeID: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/unitreerobotics/unitree_guide/fdf4d23de6affe8ee38fb4d892f61053fa1fcbcb/unitree_actuator_sdk/ChangeID_Tool/Linux/ChangeID -------------------------------------------------------------------------------- /unitree_actuator_sdk/ChangeID_Tool/Windows/ChangeID_Win64.exe: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/unitreerobotics/unitree_guide/fdf4d23de6affe8ee38fb4d892f61053fa1fcbcb/unitree_actuator_sdk/ChangeID_Tool/Windows/ChangeID_Win64.exe -------------------------------------------------------------------------------- /unitree_actuator_sdk/ChangeID_Tool/Windows/libUnitree_motor_SDK_Win64.dll: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/unitreerobotics/unitree_guide/fdf4d23de6affe8ee38fb4d892f61053fa1fcbcb/unitree_actuator_sdk/ChangeID_Tool/Windows/libUnitree_motor_SDK_Win64.dll -------------------------------------------------------------------------------- /unitree_actuator_sdk/ReadMe.md: -------------------------------------------------------------------------------- 1 | # Introduction 2 | This library is used for the communication between PC and the motor control board, so that the user can control the motors by PC. This library includes Linux and Windows version, where the Linux version also support ROS. And we offer the usage examples of C, C++ and Python. 3 | 4 | # Dependencies 5 | * [CMake](http://www.cmake.org) (version 2.8.3 or higher) 6 | 7 | # Files 8 | ## /lib 9 | Including the library files of Linux and Windows separately. If you wish to use the SDK under ARM32/64, please modify the CMakeList manually to select the correst `.so` file. 10 | ## /include 11 | Including the head files. Where ```LSerial.h``` contains the declarations of serial port operation functions. The ```motor_msg.h``` contains the command structure for motor communication. The ```motor_ctrl.h``` contains the encoding and decoding functions. 12 | ## /ChangeID_Tool 13 | Including the tool to change motor's ID of Linux and Windows separately. Please follow guidances of the executable file. 14 | ## /src 15 | Including the example source files of C and C++. The example can control motors to run under desired command for desired time, and then stop. Please watch out that only the check.c is the full control example with all functions. check.cpp and also the Python example do not contain some comments and important tutorials. 16 | ## /script 17 | Including the example source files of Python. This example's function is same with the C/C++ example. The ```typedef.py``` declares the data structure of all library functions and structures in Python style, so that the ```check.py``` can call the library correctly. 18 | ## /unitree_motor_ctrl 19 | It is a package of ROS and the library file is just the library file of Linux. 20 | ## /build 21 | Build directory 22 | ## /bin 23 | The directory of final executable file. 24 | 25 | # Usage 26 | ## C/C++ under Liunx 27 | ### Build 28 | ```bash 29 | mkdir build 30 | cd build 31 | cmake .. 32 | make 33 | ``` 34 | ### Run 35 | ```bash 36 | cd ../bin 37 | sudo ./check_c 38 | ``` 39 | and 40 | ```bash 41 | cd ../bin 42 | sudo ./check_cpp 43 | ``` 44 | ## Python under Linux 45 | ```bash 46 | cd script 47 | sudo python3 check.py 48 | ``` 49 | 50 | ## C/C++ under ROS 51 | ### Build 52 | Under the catkin workspace, run: 53 | ```bash 54 | catkin_make 55 | ``` 56 | ### Run 57 | As we cannot use ```sudo rosrun```, please run as follows: 58 | 59 | First, at a terminal, run: 60 | ```bash 61 | roscore 62 | ``` 63 | In another terminal, run: 64 | ```bash 65 | sudo su 66 | source devel/setup.bash 67 | rosrun unitree_motor_ctrl unitree_motor_ctrl_node 68 | ``` 69 | ## Python under ROS 70 | First, at a terminal, run: 71 | ```bash 72 | roscore 73 | ``` 74 | In another terminal, run: 75 | ```bash 76 | sudo su 77 | source devel/setup.bash 78 | rosrun unitree_motor_ctrl check.py 79 | ``` 80 | ## C/C++ under Windows 81 | ### Build 82 | We take MinGW as an example. First, select the "MinGW Makefiles" in CMake GUI and generate the makefiles to ```build``` directory. Then open the cmd.exe, run: 83 | ```bash 84 | cd build 85 | mingw32-make.exe 86 | ``` 87 | ### Run 88 | Put the generated .exe file(/bin) and the .dll file(/lib) to same directory, and double click the .exe file to run. 89 | ## Python under Windows 90 | Open the cmd.exe, then: 91 | ```bash 92 | cd script 93 | check.py 94 | ``` -------------------------------------------------------------------------------- /unitree_actuator_sdk/include/LSerial.h: -------------------------------------------------------------------------------- 1 | #ifndef LSERIAL 2 | #define LSERIAL 3 | 4 | #ifdef __cplusplus 5 | extern "C"{ 6 | #endif 7 | 8 | #include "motor_ctrl.h" 9 | 10 | #if defined(__linux__) 11 | extern int open_set(char*); //Input serial port name. (The Baud rate is fixed to 4800000), return the handle of serial port;【输入串口名。（波特率固定为4800000），返回串口句柄】 12 | extern int close_serial(int); //Input serial port handle. Close the serial port【输入串口句柄，关闭串口】 13 | extern int broadcast(int, MOTOR_send*); //Input serial port handle and the pointer of message to be sent. Broadcast to all motors, and the motors do not have any respond【输入串口句柄和发送数据的指针，对电机广播，不接受返回】 14 | extern int send_recv(int, MOTOR_send*, MOTOR_recv*); //Input serial port handle, the pointer of message to be sent and the pointer of received message.【输入串口句柄，发送数据的指针，接收数据的指针】 15 | //Synchronous send and receive, send 34 bytes and receive 78 bytes【该函数为同步收发，每发一命令（34字节）必须接收一命令（78字节）】 16 | //The return value is the status of send and receive. 0:send fail and receive fail, 1:send success and receive fail, 11:send success and receive success 17 | //【返回值为发送接收状态，0：发送接收失败，1：发送成功接受失败，11：发送成功接收成功】 18 | #elif defined(__WIN32__) 19 | #include 20 | extern HANDLE open_set(char*); //Input serial port name. (The Baud rate is fixed to 4800000), return the handle of serial port;【输入串口名。（波特率固定为4800000），返回串口句柄】 21 | extern int close_serial(HANDLE); //Input serial port handle. Close the serial port【输入串口句柄，关闭串口】 22 | extern int broadcast(HANDLE, MOTOR_send*); //Input serial port handle and the pointer of message to be sent. Broadcast to all motors, and the motors do not have any respond【输入串口句柄和发送数据的指针，对电机广播，不接受返回】 23 | extern int send_recv(HANDLE, MOTOR_send*, MOTOR_recv*); //Input serial port handle, the pointer of message to be sent and the pointer of received message.【输入串口句柄，发送数据的指针，接收数据的指针】 24 | //Synchronous send and receive, send 34 bytes and receive 78 bytes【该函数为同步收发，每发一命令（34字节）必须接收一命令（78字节）】 25 | //The return value is the status of send and receive. 0:send fail and receive fail, 1:send success and receive fail, 11:send success and receive success 26 | //【返回值为发送接收状态，0：发送接收失败，1：发送成功接受失败，11：发送成功接收成功】 27 | #endif 28 | 29 | #ifdef __cplusplus 30 | } 31 | #endif 32 | 33 | #endif -------------------------------------------------------------------------------- /unitree_actuator_sdk/include/motor_ctrl.h: -------------------------------------------------------------------------------- 1 | #ifndef MOTOR_CTRL 2 | #define MOTOR_CTRL 3 | 4 | #ifdef __cplusplus 5 | extern "C"{ 6 | #endif 7 | 8 | #include "motor_msg.h" //The data structure of motor communication messages【电机通信协议】 9 | #include 10 | 11 | //Define the message to be send. 【定义发送数据】 12 | typedef struct { 13 | MasterComdDataV3 motor_send_data; //The data to be sent to motor. Details are shown in motor_msg.h【电机控制数据结构体，详见motor_msg.h】 14 | int hex_len; //The Bytes count of the message to be sent, it should be 34 for this motor【发送命令字节数, 本电机应为34】 15 | long long send_time; //The time that message was sent发送该命令的时间, 微秒(us) 16 | //The values of motor commands 【待发送的各项数据】 17 | unsigned short id; //Motor ID【电机ID】 18 | unsigned short mode; //The control mode, 0:free, 5:Open loop slow turning, 10:close loop control【电机控制模式，0:空闲, 5:开环缓慢转动, 10:闭环控制】 19 | //The following parameters are just motor's parameters, do not concern the reducer. The real torque command to control board is: 20 | //【以下参数均为电机本身参数，与减速器无关。实际传递给控制板的指令力矩为：】 21 | //K_P*delta_Pos + K_W*delta_W + T 22 | float T; //Desired output torque of motor【期望电机本身的输出力矩（Nm）】 23 | float W; //Desired output speed of motor【期望电机本身的速度(rad/s)】 24 | float Pos; //Desired shaft position of motor【期望电机本身的位置（rad）】 25 | float K_P; //The position stiffness【电机本身的位置刚度系数】 26 | float K_W; //The speed stiffness【电机本身的速度刚度系数】 27 | }MOTOR_send; 28 | 29 | //Define the data structure of received message. 【定义接收数据结构】 30 | typedef struct 31 | { 32 | ServoComdDataV3 motor_recv_data; //The data received from motor. Details are shown in motor_msg.h【电机接收数据结构体，详见motor_msg.h】 33 | int hex_len; //The Bytes count of the received message, it should be 78 for this motor【接收信息的字节数, 本电机应为78】 34 | long long resv_time; //The time of receiving this message, microsecond(us)【接收该命令的时间, 微秒(us)】 35 | int correct; //Whether the received data is correct(1:correct, 0:wrong)【接收数据是否完整（1完整，0不完整）】 36 | //解读得出的电机数据 37 | unsigned char motor_id; //Motor ID【电机ID】 38 | unsigned char mode; //The control mode, 0:free, 5:Open loop slow turning, 10:close loop control【电机控制模式，0:空闲, 5:开环缓慢转动, 10:闭环控制】 39 | int Temp; //Temperature【温度】 40 | unsigned char MError; //Error code【错误码】 41 | 42 | float T; //The output torque of motor【当前实际电机输出力矩】 43 | float W; //The motor shaft speed(without filter)【前实际电机速度（无滤波器）】 44 | float LW; //The motor shaft speed(with filter)【当前实际电机速度（有滤波器）】 45 | int Acc; //The acceleration of motor shaft【电机转子加速度】 46 | float Pos; //The motor shaft position(control board zero fixed)【当前电机位置（主控0点修正，电机关节还是以编码器0点为准）】 47 | 48 | float gyro[3]; //The data of 6 axis inertial sensor on the control board【电机驱动板6轴传感器数据】 49 | float acc[3]; 50 | 51 | }MOTOR_recv; 52 | 53 | extern long long getSystemTime(); //Get the current system time, microsecond(us)【获取当前系统时间（微秒us）】 54 | extern int modify_data(MOTOR_send*); //Compile the data to the data structure of motor【将数据处理为stm32需求的格式】 55 | extern int extract_data(MOTOR_recv*); //Extract the parameter values from received data【将接收到的数据解读】 56 | uint32_t crc32_core(uint32_t*, uint32_t); //Calculate the CRC. Inputs are: pointer to the data to be calculated, Bytes count/4,(ignore remainder)【计算CRC校验码，输入为：待校验数据指针，数据包含的4字节整形数量（余数舍去）】 57 | 58 | #ifdef __cplusplus 59 | } 60 | #endif 61 | 62 | #endif -------------------------------------------------------------------------------- /unitree_actuator_sdk/include/motor_msg.h: -------------------------------------------------------------------------------- 1 | #ifndef MOTOR_MSG 2 | #define MOTOR_MSG 3 | 4 | #ifdef __cplusplus 5 | extern "C"{ 6 | #endif 7 | 8 | #include 9 | typedef int16_t q15_t; 10 | 11 | #pragma pack(1) 12 | 13 | // 发送用单个数据数据结构 14 | typedef union{ 15 | int32_t L; 16 | uint8_t u8[4]; 17 | uint16_t u16[2]; 18 | uint32_t u32; 19 | float F; 20 | }COMData32; 21 | 22 | typedef struct { 23 | // 定义 数据包头 24 | unsigned char start[2]; // 包头 25 | unsigned char motorID; // 电机ID 0,1,2 0xBB 表示向所有电机广播（此时无返回） 26 | unsigned char reserved; 27 | }COMHead; 28 | 29 | #pragma pack() 30 | 31 | #pragma pack(1) 32 | 33 | typedef struct { 34 | 35 | uint8_t fan_d; // 关节上的散热风扇转速 36 | uint8_t Fmusic; // 电机发声频率 /64*1000 15.625f 频率分度 37 | uint8_t Hmusic; // 电机发声强度 推荐值4 声音强度 0.1 分度 38 | uint8_t reserved4; 39 | 40 | uint8_t FRGB[4]; // 足端LED 41 | 42 | }LowHzMotorCmd; 43 | 44 | typedef struct { // 以 4个字节一组排列 ，不然编译器会凑整 45 | // 定义 数据 46 | uint8_t mode; // 关节模式选择 47 | uint8_t ModifyBit; // 电机控制参数修改位 48 | uint8_t ReadBit; // 电机控制参数发送位 49 | uint8_t reserved; 50 | 51 | COMData32 Modify; // 电机参数修改 的数据 52 | //实际给FOC的指令力矩为： 53 | //K_P*delta_Pos + K_W*delta_W + T 54 | q15_t T; // 期望关节的输出力矩（电机本身的力矩）x256, 7 + 8 描述 55 | q15_t W; // 期望关节速度 （电机本身的速度） x128, 8 + 7描述 56 | int32_t Pos; // 期望关节位置 x 16384/6.2832, 14位编码器（主控0点修正，电机关节还是以编码器0点为准） 57 | 58 | q15_t K_P; // 关节刚度系数 x2048 4+11 描述 59 | q15_t K_W; // 关节速度系数 x1024 5+10 描述 60 | 61 | uint8_t LowHzMotorCmdIndex; // 电机低频率控制命令的索引, 0-7, 分别代表LowHzMotorCmd中的8个字节 62 | uint8_t LowHzMotorCmdByte; // 电机低频率控制命令的字节 63 | 64 | COMData32 Res[1]; // 通讯 保留字节 用于实现别的一些通讯内容 65 | 66 | }MasterComdV3; // 加上数据包的包头 和CRC 34字节 67 | 68 | typedef struct { 69 | // 定义 电机控制命令数据包 70 | COMHead head; 71 | MasterComdV3 Mdata; 72 | COMData32 CRCdata; 73 | }MasterComdDataV3;//返回数据 74 | 75 | #pragma pack() 76 | 77 | #pragma pack(1) 78 | 79 | typedef struct { // 以 4个字节一组排列 ，不然编译器会凑整 80 | // 定义 数据 81 | uint8_t mode; // 当前关节模式 82 | uint8_t ReadBit; // 电机控制参数修改 是否成功位 83 | int8_t Temp; // 电机当前平均温度 84 | uint8_t MError; // 电机错误 标识 85 | 86 | COMData32 Read; // 读取的当前 电机 的控制数据 87 | int16_t T; // 当前实际电机输出力矩 7 + 8 描述 88 | 89 | int16_t W; // 当前实际电机速度（高速） 8 + 7 描述 90 | float LW; // 当前实际电机速度（低速） 91 | 92 | int16_t W2; // 当前实际关节速度（高速） 8 + 7 描述 93 | float LW2; // 当前实际关节速度（低速） 94 | 95 | int16_t Acc; // 电机转子加速度 15+0 描述 惯量较小 96 | int16_t OutAcc; // 输出轴加速度 12+3 描述 惯量较大 97 | 98 | int32_t Pos; // 当前电机位置（主控0点修正，电机关节还是以编码器0点为准） 99 | int32_t Pos2; // 关节编码器位置(输出编码器) 100 | 101 | int16_t gyro[3]; // 电机驱动板6轴传感器数据 102 | int16_t acc[3]; 103 | 104 | // 力传感器的数据 105 | int16_t Fgyro[3]; // 106 | int16_t Facc[3]; 107 | int16_t Fmag[3]; 108 | uint8_t Ftemp; // 8位表示的温度 7位（-28~100度） 1位0.5度分辨率 109 | 110 | int16_t Force16; // 力传感器高16位数据 111 | int8_t Force8; // 力传感器低8位数据 112 | 113 | uint8_t FError; // 足端传感器错误标识 114 | 115 | int8_t Res[1]; // 通讯 保留字节 116 | 117 | }ServoComdV3; // 加上数据包的包头 和CRC 78字节（4+70+4） 118 | 119 | typedef struct { 120 | // 定义 电机控制命令数据包 121 | COMHead head; 122 | ServoComdV3 Mdata; 123 | 124 | COMData32 CRCdata; 125 | 126 | }ServoComdDataV3; //发送数据 127 | 128 | #pragma pack() 129 | 130 | #ifdef __cplusplus 131 | } 132 | #endif 133 | 134 | #endif -------------------------------------------------------------------------------- /unitree_actuator_sdk/lib/libUnitree_motor_SDK_ARM32.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/unitreerobotics/unitree_guide/fdf4d23de6affe8ee38fb4d892f61053fa1fcbcb/unitree_actuator_sdk/lib/libUnitree_motor_SDK_ARM32.so -------------------------------------------------------------------------------- /unitree_actuator_sdk/lib/libUnitree_motor_SDK_ARM64.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/unitreerobotics/unitree_guide/fdf4d23de6affe8ee38fb4d892f61053fa1fcbcb/unitree_actuator_sdk/lib/libUnitree_motor_SDK_ARM64.so -------------------------------------------------------------------------------- /unitree_actuator_sdk/lib/libUnitree_motor_SDK_Linux32.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/unitreerobotics/unitree_guide/fdf4d23de6affe8ee38fb4d892f61053fa1fcbcb/unitree_actuator_sdk/lib/libUnitree_motor_SDK_Linux32.so -------------------------------------------------------------------------------- /unitree_actuator_sdk/lib/libUnitree_motor_SDK_Linux64.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/unitreerobotics/unitree_guide/fdf4d23de6affe8ee38fb4d892f61053fa1fcbcb/unitree_actuator_sdk/lib/libUnitree_motor_SDK_Linux64.so -------------------------------------------------------------------------------- /unitree_actuator_sdk/lib/libUnitree_motor_SDK_Win64.dll: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/unitreerobotics/unitree_guide/fdf4d23de6affe8ee38fb4d892f61053fa1fcbcb/unitree_actuator_sdk/lib/libUnitree_motor_SDK_Win64.dll -------------------------------------------------------------------------------- /unitree_actuator_sdk/script/check.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import os 3 | import sys 4 | from typedef import * 5 | from ctypes import * 6 | import time 7 | 8 | system=platform.system() 9 | if system == 'Windows': 10 | fd = c.open_set(b'\\\\.\\COM4') 11 | libPath = os.path.dirname(os.getcwd()) + '/lib/libUnitree_motor_SDK_Win64.dll' 12 | elif system == 'Linux': 13 | fd = c.open_set(b'/dev/ttyUSB0') 14 | maxbit=sys.maxsize 15 | if maxbit>2**32: 16 | libPath = os.path.dirname(os.getcwd()) + '/lib/libUnitree_motor_SDK_Linux64.so' 17 | print('Linux 64 bits') 18 | else: 19 | libPath = os.path.dirname(os.getcwd()) + '/lib/libUnitree_motor_SDK_Linux32.so' 20 | print('Linux 32 bits') 21 | 22 | c = cdll.LoadLibrary(libPath) 23 | 24 | motor_s = MOTOR_send() 25 | motor_s1 = MOTOR_send() 26 | motor_r = MOTOR_recv() 27 | 28 | motor_s.id = 0 29 | motor_s.mode = 10 30 | motor_s.T = 0.05 #单位：Nm, T<255.9 31 | motor_s.W = 0.0 #单位：rad/s, W<511.9 32 | motor_s.Pos = 0 #单位：rad, Pos<131071.9 33 | motor_s.K_P = 0.0 #K_P<31.9 34 | motor_s.K_W = 0.0 #K_W<63.9 35 | 36 | motor_s1.id = 0 37 | motor_s1.mode = 0 38 | 39 | c.modify_data(byref(motor_s)) 40 | c.modify_data(byref(motor_s1)) 41 | 42 | print('START') 43 | 44 | i = 0 45 | while(i < 5): 46 | c.send_recv(fd, byref(motor_s), byref(motor_r)) 47 | c.extract_data(byref(motor_r)) 48 | print('*******************') 49 | print('Motor torque: ', motor_r.T) 50 | print('Motor position: ', motor_r.Pos) 51 | print('Motor velocity: ', motor_r.W) 52 | time.sleep(1) 53 | i = i + 1 54 | 55 | c.send_recv(fd, byref(motor_s1), byref(motor_r)) 56 | print('END') 57 | 58 | c.close_serial(fd) -------------------------------------------------------------------------------- /unitree_actuator_sdk/script/typedef.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import os 3 | # import typedef 4 | from ctypes import * 5 | import platform 6 | import sys 7 | 8 | system=platform.system() 9 | if system == 'Windows': 10 | libPath = os.path.dirname(os.getcwd()) + '/lib/libUnitree_motor_SDK_Win64.dll' 11 | elif system == 'Linux': 12 | maxbit=sys.maxsize 13 | if maxbit>2**32: 14 | libPath = os.path.dirname(os.getcwd()) + '/lib/libUnitree_motor_SDK_Linux64.so' 15 | else: 16 | libPath = os.path.dirname(os.getcwd()) + '/lib/libUnitree_motor_SDK_Linux32.so' 17 | 18 | 19 | c = cdll.LoadLibrary(libPath) 20 | 21 | class COMData32(Union): 22 | _fields_ = [("L", c_int32), ("u8", c_uint8*4), ("u16", c_uint16*2), ("u32", c_uint32), ("F", c_float)] 23 | 24 | class COMHead(Structure): 25 | _fields_ = [("start", c_ubyte*2), ("motorID", c_ubyte), ("reserved", c_ubyte)] 26 | 27 | class MasterComdV3(Structure): 28 | _pack_ = 1 29 | # _fields_ = [("mode", c_uint8)] 30 | _fields_ = [("mode", c_uint8), ("ModifyBit", c_uint8), ("ReadBit", c_uint8), \ 31 | ("reserved", c_uint8), ("Modify", COMData32), ("T", c_int16), \ 32 | ("W", c_int16), ("Pos", c_int32), ("K_P", c_int16), ("K_W", c_int16), \ 33 | ("LowHzMotorCmdIndex", c_uint8), ("LowHzMotorCmdByte", c_uint8), \ 34 | ("Res", COMData32)] 35 | 36 | 37 | class MasterComdDataV3(Structure): 38 | _pack_ = 1 39 | _fields_ = [("head", COMHead), ("Mdata", MasterComdV3), ("CRCdata", COMData32)] 40 | 41 | class ServoComdV3(Structure): 42 | _pack_ = 1 43 | _fields_ = [("mode", c_uint8), ("ReadBit", c_uint8), ("Temp", c_int8), \ 44 | ("MError", c_uint8), ("Read", COMData32), ("T", c_int16), ("W", c_int16), \ 45 | ("LW", c_float), ("W2", c_int16), ("LW2", c_float), ("Acc", c_int16), \ 46 | ("OutAcc", c_int16), ("Pos", c_int32), ("Pos2", c_int32), \ 47 | ("gyro", c_int16*3), ("acc", c_int16*3), ("Fgyro", c_int16*3), ("Facc", c_int16*3), \ 48 | ("Fmag", c_int16*3), ("Ftemp", c_uint8), ("Force16", c_int16), \ 49 | ("Force8", c_int8), ("FError", c_uint8), ("Res", c_int8)] 50 | 51 | 52 | class ServoComdDataV3(Structure): 53 | _pack_ = 1 54 | _fields_ = [("head", COMHead), ("Mdata", ServoComdV3), ("CRCdata", COMData32)] 55 | 56 | class MOTOR_send(Structure): 57 | _fields_ = [("motor_send_data", MasterComdDataV3), ("hex_len", c_int), \ 58 | ("send_time", c_longlong), ("id", c_ushort), ("mode", c_ushort), \ 59 | ("T", c_float), ("W", c_float), ("Pos", c_float), ("K_P", c_float), ("K_W", c_float)] 60 | 61 | class MOTOR_recv(Structure): 62 | _fields_ = [("motor_recv_data", ServoComdDataV3), ("hex_len", c_int), \ 63 | ("recv_time", c_longlong), ("correct", c_int), ("motor_id", c_ubyte), \ 64 | ("mode", c_ubyte), ("Temp", c_int), ("MError", c_ubyte), \ 65 | ("T", c_float), ("W", c_float), ("LW", c_float), ("Acc", c_int), \ 66 | ("Pos", c_float), ("gyro", c_float*3), ("acc", c_float*3)] 67 | 68 | # motor_ctrl.h 69 | c.getSystemTime.restype = c_longlong 70 | 71 | c.modify_data.restype = c_int32 72 | c.modify_data.argtypes = (POINTER(MOTOR_send), ) 73 | 74 | c.extract_data.restype = c_int 75 | c.extract_data.argtypes = (POINTER(MOTOR_recv), ) 76 | 77 | c.crc32_core.restype = c_uint32 78 | c.crc32_core.argtypes = (POINTER(c_uint32), c_uint32) 79 | 80 | # 此处需要考虑win64平台和linux平台的区别 81 | # LSerial.h 82 | c.open_set.restype = c_int 83 | c.open_set.argtypes = (POINTER(c_char),) 84 | 85 | c.close_serial.restype = c_int 86 | c.close_serial.argtypes = (c_int, ) 87 | 88 | # c.broadcast.restype = c_int 89 | # c.broadcast.argtypes(c_int, POINTER(MOTOR_send)) 90 | 91 | c.send_recv.restype = c_int 92 | c.send_recv.argtypes = (c_int, POINTER(MOTOR_send), POINTER(MOTOR_recv)) 93 | 94 | -------------------------------------------------------------------------------- /unitree_actuator_sdk/src/check.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include //错误定义 3 | #include 4 | #include //Unix标准函数定义, usleep() 5 | #include 6 | #include "LSerial.h" //串口通信函数 7 | #include "motor_ctrl.h" //声明发送数据、接收数据的结构体，以及函数声明 8 | 9 | int main() 10 | { 11 | //发送参数 12 | MOTOR_send motor_s, motor_s1; 13 | // long long start_time = 0; 14 | motor_s.id = 0; 15 | motor_s.mode = 5; 16 | motor_s.T = 0; //单位：Nm, T<255.9 17 | motor_s.W = 21.0; //单位：rad/s, W<511.9 18 | motor_s.Pos = 0.0; //单位：rad, Pos<131071.9 19 | motor_s.K_P = 0.0; //K_P<31.9 20 | motor_s.K_W = 10; //K_W<63.9 21 | 22 | motor_s1.id = 0; 23 | motor_s1.mode = 0; 24 | //接收参数 25 | MOTOR_recv motor_r; 26 | //文件ID 27 | #if defined(__linux__) 28 | int fd; 29 | 30 | #elif defined(__WIN32__) 31 | HANDLE fd; 32 | 33 | #endif 34 | 35 | fd = open_set((char*)"/dev/ttyUSB0"); 36 | // fd = open_set((char*)"\\\\.\\COM4"); 37 | 38 | modify_data(&motor_s); 39 | modify_data(&motor_s1); 40 | 41 | int sta; 42 | sta = send_recv(fd, &motor_s1, &motor_r); 43 | // printf("status2: %d\n", sta); 44 | extract_data(&motor_r); 45 | // show_resv_data_hex(&motor_r); 46 | printf("START\n"); 47 | // show_resv_data(&motor_r); 48 | 49 | 50 | for(int i=0; i<100; i++) 51 | { 52 | send_recv(fd, &motor_s, &motor_r); 53 | // extract_data(&motor_r); 54 | // show_resv_data(&motor_r); 55 | usleep(100000); 56 | } 57 | 58 | sta = send_recv(fd, &motor_s1, &motor_r); 59 | // printf("status2: %d\n", sta); 60 | extract_data(&motor_r); 61 | // show_resv_data_hex(&motor_r); 62 | printf("END\n"); 63 | // show_resv_data(&motor_r); 64 | 65 | close_serial(fd); 66 | #if defined(__WIN32__) 67 | system("pause"); 68 | #endif 69 | 70 | return 0; 71 | } 72 | -------------------------------------------------------------------------------- /unitree_actuator_sdk/unitree_motor_ctrl/include/unitree_motor_ctrl/LSerial.h: -------------------------------------------------------------------------------- 1 | #ifndef LSERIAL 2 | #define LSERIAL 3 | 4 | #ifdef __cplusplus 5 | extern "C"{ 6 | #endif 7 | 8 | #include "motor_ctrl.h" 9 | 10 | #if defined(__linux__) 11 | extern int open_set(char*); //串口名。（波特率固定为4800000），返回串口句柄 12 | extern int close_serial(int); //关闭串口 13 | extern int broadcast(int, MOTOR_send*); //对电机广播，不接受返回 14 | extern int send_recv(int, MOTOR_send*, MOTOR_recv*); //该函数为同步收发，每发一命令（34字节）必须接收一命令（78字节） 15 | //返回值为发送接收状态，0：发送接收失败，1：发送成功接受失败，10：发送失败接收成功（不可能），11：发送成功接收成功 16 | #elif defined(__WIN32__) 17 | #include 18 | extern HANDLE open_set(char*); //串口名。（波特率固定为4800000），返回串口句柄 19 | extern int close_serial(HANDLE); 20 | extern int send_recv(HANDLE, MOTOR_send*, MOTOR_recv*); //该函数为同步收发，每发一命令（34字节）必须接收一命令（78字节） 21 | //返回值为发送接收状态，0：发送接收失败，1：发送成功接受失败，10：发送失败接收成功（不可能），11：发送成功接收成功 22 | #endif 23 | 24 | #ifdef __cplusplus 25 | } 26 | #endif 27 | 28 | #endif -------------------------------------------------------------------------------- /unitree_actuator_sdk/unitree_motor_ctrl/include/unitree_motor_ctrl/motor_ctrl.h: -------------------------------------------------------------------------------- 1 | #ifndef MOTOR_CTRL 2 | #define MOTOR_CTRL 3 | 4 | #ifdef __cplusplus 5 | extern "C"{ 6 | #endif 7 | 8 | #include "motor_msg.h" //电机通信协议 9 | #include 10 | 11 | typedef struct { 12 | // 定义 发送格式化数据 13 | MasterComdDataV3 motor_send_data; //电机控制数据结构体，详见motor_msg.h 14 | int hex_len; //发送的16进制命令数组长度, 34 15 | long long send_time; //The time that message was sent发送该命令的时间, 微秒(us) 16 | // 待发送的各项数据 17 | unsigned short id; //电机ID，0代表全部电机 18 | unsigned short mode; //0:空闲, 5:开环转动, 10:闭环FOC控制 19 | //实际给FOC的指令力矩为： 20 | //K_P*delta_Pos + K_W*delta_W + T 21 | float T; //期望关节的输出力矩（电机本身的力矩）（Nm） 22 | float W; //期望关节速度（电机本身的速度）(rad/s) 23 | float Pos; //期望关节位置（rad） 24 | float K_P; //关节刚度系数 25 | float K_W; //关节速度系数 26 | }MOTOR_send; 27 | 28 | typedef struct 29 | { 30 | // 定义 接收数据 31 | ServoComdDataV3 motor_recv_data; //电机接收数据结构体，详见motor_msg.h 32 | int hex_len; //接收的16进制命令数组长度, 78 33 | long long resv_time; //接收该命令的时间, 微秒(us) 34 | int correct; //接收数据是否完整（1完整，0不完整） 35 | //解读得出的电机数据 36 | unsigned char motor_id; //电机ID 37 | unsigned char mode; //0:空闲, 5:开环转动, 10:闭环FOC控制 38 | int Temp; //温度 39 | unsigned char MError; //错误码 40 | 41 | float T; // 当前实际电机输出力矩 42 | float W; // 当前实际电机速度（高速） 43 | float LW; // 当前实际电机速度（低速） 44 | int Acc; // 电机转子加速度 45 | float Pos; // 当前电机位置（主控0点修正，电机关节还是以编码器0点为准） 46 | 47 | float gyro[3]; // 电机驱动板6轴传感器数据 48 | float acc[3]; 49 | 50 | }MOTOR_recv; 51 | 52 | extern long long getSystemTime(); //获取当前系统时间（微秒us） 53 | extern int modify_data(MOTOR_send*); //将数据处理为stm32需求的格式 54 | extern int extract_data(MOTOR_recv*); //将接收到的数据解读 55 | uint32_t crc32_core(uint32_t*, uint32_t); //待校验数组指针，数组长度（余数舍去） 56 | 57 | #ifdef __cplusplus 58 | } 59 | #endif 60 | 61 | #endif -------------------------------------------------------------------------------- /unitree_actuator_sdk/unitree_motor_ctrl/lib/libUnitree_motor_SDK_Linux.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/unitreerobotics/unitree_guide/fdf4d23de6affe8ee38fb4d892f61053fa1fcbcb/unitree_actuator_sdk/unitree_motor_ctrl/lib/libUnitree_motor_SDK_Linux.so -------------------------------------------------------------------------------- /unitree_actuator_sdk/unitree_motor_ctrl/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | unitree_motor_ctrl 4 | 0.0.0 5 | The unitree_motor_ctrl package 6 | 7 | 8 | 9 | 10 | bian 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | roscpp 53 | roscpp 54 | roscpp 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | -------------------------------------------------------------------------------- /unitree_actuator_sdk/unitree_motor_ctrl/script/__pycache__/typedef.cpython-36.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/unitreerobotics/unitree_guide/fdf4d23de6affe8ee38fb4d892f61053fa1fcbcb/unitree_actuator_sdk/unitree_motor_ctrl/script/__pycache__/typedef.cpython-36.pyc -------------------------------------------------------------------------------- /unitree_actuator_sdk/unitree_motor_ctrl/script/check.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import os 3 | import sys 4 | from typedef import * 5 | from ctypes import cdll 6 | import time 7 | 8 | 9 | system=platform.system() 10 | if system == 'Windows': 11 | fd = c.open_set(b'\\\\.\\COM4') 12 | libPath = os.path.dirname(os.getcwd()) + '/lib/libUnitree_motor_SDK_Win64.dll' 13 | elif system == 'Linux': 14 | fd = c.open_set(b'/dev/ttyUSB0') 15 | maxbit=sys.maxsize 16 | if maxbit>2**32: 17 | libPath = os.path.dirname(os.getcwd()) + '/lib/libUnitree_motor_SDK_Linux64.so' 18 | else: 19 | libPath = os.path.dirname(os.getcwd()) + '/lib/libUnitree_motor_SDK_Linux32.so' 20 | 21 | c = cdll.LoadLibrary(libPath) 22 | 23 | motor_s = MOTOR_send() 24 | motor_s1 = MOTOR_send() 25 | motor_r = MOTOR_recv() 26 | 27 | motor_s.id = 0 28 | motor_s.mode = 5 29 | motor_s.T = 0 #单位：Nm, T<255.9 30 | motor_s.W = 240.0 #单位：rad/s, W<511.9 31 | motor_s.Pos = 0 #单位：rad, Pos<131071.9 32 | motor_s.K_P = 0 #K_P<31.9 33 | motor_s.K_W = 20 #K_W<63.9 34 | 35 | motor_s1.id = 0 36 | motor_s1.mode = 0 37 | 38 | c.modify_data(byref(motor_s)) 39 | c.modify_data(byref(motor_s1)) 40 | 41 | c.send_recv(fd, byref(motor_s1), byref(motor_r)) 42 | print('START') 43 | 44 | c.send_recv(fd, byref(motor_s), byref(motor_r)) 45 | time.sleep(5) 46 | 47 | c.send_recv(fd, byref(motor_s1), byref(motor_r)) 48 | print('END') 49 | 50 | c.close_serial(fd) -------------------------------------------------------------------------------- /unitree_actuator_sdk/unitree_motor_ctrl/script/typedef.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import os 3 | # import typedef 4 | from ctypes import * 5 | import platform 6 | import sys 7 | 8 | system=platform.system() 9 | if system == 'Windows': 10 | libPath = os.path.dirname(os.getcwd()) + '/lib/libUnitree_motor_SDK_Win64.dll' 11 | elif system == 'Linux': 12 | maxbit=sys.maxsize 13 | if maxbit>2**32: 14 | libPath = os.path.dirname(os.getcwd()) + '/lib/libUnitree_motor_SDK_Linux64.so' 15 | else: 16 | libPath = os.path.dirname(os.getcwd()) + '/lib/libUnitree_motor_SDK_Linux32.so' 17 | 18 | 19 | c = cdll.LoadLibrary(libPath) 20 | 21 | class COMData32(Union): 22 | _fields_ = [("L", c_int32), ("u8", c_uint8*4), ("u16", c_uint16*2), ("u32", c_uint32), ("F", c_float)] 23 | 24 | class COMHead(Structure): 25 | _fields_ = [("start", c_ubyte*2), ("motorID", c_ubyte), ("reserved", c_ubyte)] 26 | 27 | class MasterComdV3(Structure): 28 | _pack_ = 1 29 | # _fields_ = [("mode", c_uint8)] 30 | _fields_ = [("mode", c_uint8), ("ModifyBit", c_uint8), ("ReadBit", c_uint8), \ 31 | ("reserved", c_uint8), ("Modify", COMData32), ("T", c_int16), \ 32 | ("W", c_int16), ("Pos", c_int32), ("K_P", c_int16), ("K_W", c_int16), \ 33 | ("LowHzMotorCmdIndex", c_uint8), ("LowHzMotorCmdByte", c_uint8), \ 34 | ("Res", COMData32)] 35 | 36 | 37 | class MasterComdDataV3(Structure): 38 | _pack_ = 1 39 | _fields_ = [("head", COMHead), ("Mdata", MasterComdV3), ("CRCdata", COMData32)] 40 | 41 | class ServoComdV3(Structure): 42 | _pack_ = 1 43 | _fields_ = [("mode", c_uint8), ("ReadBit", c_uint8), ("Temp", c_int8), \ 44 | ("MError", c_uint8), ("Read", COMData32), ("T", c_int16), ("W", c_int16), \ 45 | ("LW", c_float), ("W2", c_int16), ("LW2", c_float), ("Acc", c_int16), \ 46 | ("OutAcc", c_int16), ("Pos", c_int32), ("Pos2", c_int32), \ 47 | ("gyro", c_int16*3), ("acc", c_int16*3), ("Fgyro", c_int16*3), ("Facc", c_int16*3), \ 48 | ("Fmag", c_int16*3), ("Ftemp", c_uint8), ("Force16", c_int16), \ 49 | ("Force8", c_int8), ("FError", c_uint8), ("Res", c_int8)] 50 | 51 | 52 | class ServoComdDataV3(Structure): 53 | _pack_ = 1 54 | _fields_ = [("head", COMHead), ("Mdata", ServoComdV3), ("CRCdata", COMData32)] 55 | 56 | class MOTOR_send(Structure): 57 | _fields_ = [("motor_send_data", MasterComdDataV3), ("hex_len", c_int), \ 58 | ("id", c_ushort), ("mode", c_ushort), \ 59 | ("T", c_float), ("W", c_float), ("Pos", c_float), ("K_P", c_float), ("K_W", c_float)] 60 | 61 | class MOTOR_recv(Structure): 62 | _fields_ = [("motor_recv_data", ServoComdDataV3), ("hex_len", c_int), \ 63 | ("recv_time", c_longlong), ("correct", c_int), ("motor_id", c_ubyte), \ 64 | ("mode", c_ubyte), ("Temp", c_int), ("MError", c_ubyte), \ 65 | ("T", c_float), ("W", c_float), ("LW", c_float), ("Acc", c_int), \ 66 | ("Pos", c_float), ("gyro", c_float*3), ("acc", c_float*3)] 67 | 68 | # motor_ctrl.h 69 | c.getSystemTime.restype = c_longlong 70 | 71 | c.modify_data.restype = c_int32 72 | c.modify_data.argtypes = (POINTER(MOTOR_send), ) 73 | 74 | c.extract_data.restype = c_int 75 | c.extract_data.argtypes = (POINTER(MOTOR_send), ) 76 | 77 | c.crc32_core.restype = c_uint32 78 | c.crc32_core.argtypes = (POINTER(c_uint32), c_uint32) 79 | 80 | # 此处需要考虑win64平台和linux平台的区别 81 | # LSerial.h 82 | c.open_set.restype = c_int 83 | c.open_set.argtypes = (POINTER(c_char),) 84 | 85 | c.close_serial.restype = c_int 86 | c.close_serial.argtypes = (c_int, ) 87 | 88 | # c.broadcast.restype = c_int 89 | # c.broadcast.argtypes(c_int, POINTER(MOTOR_send)) 90 | 91 | c.send_recv.restype = c_int 92 | c.send_recv.argtypes = (c_int, POINTER(MOTOR_send), POINTER(MOTOR_recv)) 93 | 94 | -------------------------------------------------------------------------------- /unitree_actuator_sdk/unitree_motor_ctrl/src/check.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include //错误定义 3 | #include 4 | #include //Unix标准函数定义, usleep() 5 | #include 6 | #include "LSerial.h" //串口通信函数 7 | #include "motor_ctrl.h" //声明发送数据、接收数据的结构体，以及函数声明 8 | 9 | int main() 10 | { 11 | //发送参数 12 | MOTOR_send motor_s, motor_s1; 13 | // long long start_time = 0; 14 | motor_s.id = 0; 15 | motor_s.mode = 5; 16 | motor_s.T = 0; //单位：Nm, T<255.9 17 | motor_s.W = 21.0; //单位：rad/s, W<511.9 18 | motor_s.Pos = 0.0; //单位：rad, Pos<131071.9 19 | motor_s.K_P = 0.0; //K_P<31.9 20 | motor_s.K_W = 10; //K_W<63.9 21 | 22 | motor_s1.id = 0; 23 | motor_s1.mode = 0; 24 | //接收参数 25 | MOTOR_recv motor_r; 26 | //文件ID 27 | #if defined(__linux__) 28 | int fd; 29 | 30 | #elif defined(__WIN32__) 31 | HANDLE fd; 32 | 33 | #endif 34 | 35 | fd = open_set((char*)"/dev/ttyUSB0"); 36 | // fd = open_set((char*)"\\\\.\\COM4"); 37 | 38 | modify_data(&motor_s); 39 | modify_data(&motor_s1); 40 | 41 | int sta; 42 | sta = send_recv(fd, &motor_s1, &motor_r); 43 | // printf("status2: %d\n", sta); 44 | extract_data(&motor_r); 45 | // show_resv_data_hex(&motor_r); 46 | printf("START\n"); 47 | // show_resv_data(&motor_r); 48 | 49 | 50 | for(int i=0; i<100; i++) 51 | { 52 | send_recv(fd, &motor_s, &motor_r); 53 | // extract_data(&motor_r); 54 | // show_resv_data(&motor_r); 55 | usleep(100000); 56 | } 57 | 58 | sta = send_recv(fd, &motor_s1, &motor_r); 59 | // printf("status2: %d\n", sta); 60 | extract_data(&motor_r); 61 | // show_resv_data_hex(&motor_r); 62 | printf("END\n"); 63 | // show_resv_data(&motor_r); 64 | 65 | close_serial(fd); 66 | #if defined(__WIN32__) 67 | system("pause"); 68 | #endif 69 | 70 | return 0; 71 | } 72 | -------------------------------------------------------------------------------- /unitree_guide/include/FSM/FSM.h: -------------------------------------------------------------------------------- 1 | /********************************************************************** 2 | Copyright (c) 2020-2023, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ***********************************************************************/ 4 | #ifndef FSM_H 5 | #define FSM_H 6 | 7 | // FSM States 8 | #include "FSM/FSMState.h" 9 | #include "FSM/State_FixedStand.h" 10 | #include "FSM/State_Passive.h" 11 | #include "FSM/State_FreeStand.h" 12 | #include "FSM/State_Trotting.h" 13 | #include "FSM/State_BalanceTest.h" 14 | #include "FSM/State_SwingTest.h" 15 | #include "FSM/State_StepTest.h" 16 | #include "common/enumClass.h" 17 | #include "control/CtrlComponents.h" 18 | #ifdef COMPILE_WITH_MOVE_BASE 19 | #include "FSM/State_move_base.h" 20 | #endif // COMPILE_WITH_MOVE_BASE 21 | 22 | struct FSMStateList{ 23 | FSMState *invalid; 24 | State_Passive *passive; 25 | State_FixedStand *fixedStand; 26 | State_FreeStand *freeStand; 27 | State_Trotting *trotting; 28 | State_BalanceTest *balanceTest; 29 | State_SwingTest *swingTest; 30 | State_StepTest *stepTest; 31 | #ifdef COMPILE_WITH_MOVE_BASE 32 | State_move_base *moveBase; 33 | #endif // COMPILE_WITH_MOVE_BASE 34 | 35 | void deletePtr(){ 36 | delete invalid; 37 | delete passive; 38 | delete fixedStand; 39 | delete freeStand; 40 | delete trotting; 41 | delete balanceTest; 42 | delete swingTest; 43 | delete stepTest; 44 | #ifdef COMPILE_WITH_MOVE_BASE 45 | delete moveBase; 46 | #endif // COMPILE_WITH_MOVE_BASE 47 | } 48 | }; 49 | 50 | class FSM{ 51 | public: 52 | FSM(CtrlComponents *ctrlComp); 53 | ~FSM(); 54 | void initialize(); 55 | void run(); 56 | private: 57 | FSMState* getNextState(FSMStateName stateName); 58 | bool checkSafty(); 59 | CtrlComponents *_ctrlComp; 60 | FSMState *_currentState; 61 | FSMState *_nextState; 62 | FSMStateName _nextStateName; 63 | FSMStateList _stateList; 64 | FSMMode _mode; 65 | long long _startTime; 66 | int count; 67 | }; 68 | 69 | 70 | #endif // FSM_H -------------------------------------------------------------------------------- /unitree_guide/include/FSM/FSMState.h: -------------------------------------------------------------------------------- 1 | /********************************************************************** 2 | Copyright (c) 2020-2023, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ***********************************************************************/ 4 | #ifndef FSMSTATE_H 5 | #define FSMSTATE_H 6 | 7 | #include 8 | #include 9 | #include 10 | #include "control/CtrlComponents.h" 11 | #include "message/LowlevelCmd.h" 12 | #include "message/LowlevelState.h" 13 | #include "common/enumClass.h" 14 | #include "common/mathTools.h" 15 | #include "common/mathTypes.h" 16 | #include "common/timeMarker.h" 17 | #include "interface/CmdPanel.h" 18 | 19 | class FSMState{ 20 | public: 21 | FSMState(CtrlComponents *ctrlComp, FSMStateName stateName, std::string stateNameString); 22 | 23 | virtual void enter() = 0; 24 | virtual void run() = 0; 25 | virtual void exit() = 0; 26 | virtual FSMStateName checkChange() {return FSMStateName::INVALID;} 27 | 28 | FSMStateName _stateName; 29 | std::string _stateNameString; 30 | protected: 31 | CtrlComponents *_ctrlComp; 32 | FSMStateName _nextStateName; 33 | 34 | LowlevelCmd *_lowCmd; 35 | LowlevelState *_lowState; 36 | UserValue _userValue; 37 | }; 38 | 39 | #endif // FSMSTATE_H -------------------------------------------------------------------------------- /unitree_guide/include/FSM/State_BalanceTest.h: -------------------------------------------------------------------------------- 1 | /********************************************************************** 2 | Copyright (c) 2020-2023, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ***********************************************************************/ 4 | #ifndef BALANCETEST_H 5 | #define BALANCETEST_H 6 | 7 | #include "FSM/FSMState.h" 8 | 9 | class State_BalanceTest : public FSMState{ 10 | public: 11 | State_BalanceTest(CtrlComponents *ctrlComp); 12 | ~State_BalanceTest(){} 13 | void enter(); 14 | void run(); 15 | void exit(); 16 | FSMStateName checkChange(); 17 | private: 18 | void calcTau(); 19 | 20 | Estimator *_est; 21 | QuadrupedRobot *_robModel; 22 | BalanceCtrl *_balCtrl; 23 | 24 | VecInt4 *_contact; 25 | 26 | RotMat _Rd, _RdInit; 27 | Vec3 _pcd, _pcdInit; 28 | double _kpw; 29 | Mat3 _Kpp, _Kdp, _Kdw; 30 | Vec3 _ddPcd, _dWbd; 31 | 32 | Vec12 _q, _tau; 33 | Vec3 _posBody, _velBody; 34 | RotMat _B2G_RotMat, _G2B_RotMat; 35 | Vec34 _posFeet2BGlobal; 36 | Vec34 _forceFeetGlobal, _forceFeetBody; 37 | 38 | float _xMax, _xMin; 39 | float _yMax, _yMin; 40 | float _zMax, _zMin; 41 | float _yawMax, _yawMin; 42 | }; 43 | 44 | #endif // BALANCETEST_H -------------------------------------------------------------------------------- /unitree_guide/include/FSM/State_FixedStand.h: -------------------------------------------------------------------------------- 1 | /********************************************************************** 2 | Copyright (c) 2020-2023, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ***********************************************************************/ 4 | #ifndef FIXEDSTAND_H 5 | #define FIXEDSTAND_H 6 | 7 | #include "FSM/FSMState.h" 8 | 9 | class State_FixedStand : public FSMState{ 10 | public: 11 | State_FixedStand(CtrlComponents *ctrlComp); 12 | ~State_FixedStand(){} 13 | void enter(); 14 | void run(); 15 | void exit(); 16 | FSMStateName checkChange(); 17 | 18 | private: 19 | float _targetPos[12] = {0.0, 0.67, -1.3, 0.0, 0.67, -1.3, 20 | 0.0, 0.67, -1.3, 0.0, 0.67, -1.3}; 21 | float _startPos[12]; 22 | float _duration = 1000; //steps 23 | float _percent = 0; //% 24 | }; 25 | 26 | #endif // FIXEDSTAND_H -------------------------------------------------------------------------------- /unitree_guide/include/FSM/State_FreeStand.h: -------------------------------------------------------------------------------- 1 | /********************************************************************** 2 | Copyright (c) 2020-2023, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ***********************************************************************/ 4 | #ifndef FREESTAND_H 5 | #define FREESTAND_H 6 | 7 | #include "FSM/FSMState.h" 8 | 9 | class State_FreeStand : public FSMState{ 10 | public: 11 | State_FreeStand(CtrlComponents *ctrlComp); 12 | ~State_FreeStand(){} 13 | void enter(); 14 | void run(); 15 | void exit(); 16 | FSMStateName checkChange(); 17 | private: 18 | Vec3 _initVecOX; 19 | Vec34 _initVecXP; 20 | float _rowMax, _rowMin; 21 | float _pitchMax, _pitchMin; 22 | float _yawMax, _yawMin; 23 | float _heightMax, _heightMin; 24 | 25 | Vec34 _calcOP(float row, float pitch, float yaw, float height); 26 | void _calcCmd(Vec34 vecOP); 27 | }; 28 | 29 | #endif // FREESTAND_H -------------------------------------------------------------------------------- /unitree_guide/include/FSM/State_Passive.h: -------------------------------------------------------------------------------- 1 | /********************************************************************** 2 | Copyright (c) 2020-2023, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ***********************************************************************/ 4 | #ifndef PASSIVE_H 5 | #define PASSIVE_H 6 | 7 | #include "FSMState.h" 8 | 9 | class State_Passive : public FSMState{ 10 | public: 11 | State_Passive(CtrlComponents *ctrlComp); 12 | void enter(); 13 | void run(); 14 | void exit(); 15 | FSMStateName checkChange(); 16 | }; 17 | 18 | #endif // PASSIVE_H -------------------------------------------------------------------------------- /unitree_guide/include/FSM/State_StepTest.h: -------------------------------------------------------------------------------- 1 | /********************************************************************** 2 | Copyright (c) 2020-2023, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ***********************************************************************/ 4 | #ifndef STEPTEST_H 5 | #define STEPTEST_H 6 | 7 | #include "FSM/FSMState.h" 8 | 9 | class State_StepTest : public FSMState{ 10 | public: 11 | State_StepTest(CtrlComponents *ctrlComp); 12 | ~State_StepTest(){} 13 | void enter(); 14 | void run(); 15 | void exit(); 16 | FSMStateName checkChange(); 17 | private: 18 | void calcTau(); 19 | 20 | float _gaitHeight; 21 | 22 | Estimator *_est; 23 | QuadrupedRobot *_robModel; 24 | BalanceCtrl *_balCtrl; 25 | 26 | VecInt4 *_contact; 27 | Vec4 *_phase; 28 | 29 | RotMat _Rd; 30 | Vec3 _pcd; 31 | Mat3 _Kpp, _Kpw, _Kdp, _Kdw; 32 | Mat3 _KpSwing, _KdSwing; 33 | Vec3 _ddPcd, _dWbd; 34 | 35 | Vec12 _q, _tau; 36 | Vec3 _posBody, _velBody; 37 | RotMat _B2G_RotMat, _G2B_RotMat; 38 | Vec34 _posFeet2BGlobal; 39 | Vec34 _posFeetGlobalInit, _posFeetGlobalGoal, _velFeetGlobalGoal; 40 | Vec34 _posFeetGlobal, _velFeetGlobal; 41 | Vec34 _forceFeetGlobal, _forceFeetBody; 42 | }; 43 | 44 | #endif // STEPTEST_H -------------------------------------------------------------------------------- /unitree_guide/include/FSM/State_SwingTest.h: -------------------------------------------------------------------------------- 1 | /********************************************************************** 2 | Copyright (c) 2020-2023, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ***********************************************************************/ 4 | #ifndef STATE_SWINGTEST_H 5 | #define STATE_SWINGTEST_H 6 | 7 | #include "FSM/FSMState.h" 8 | #include "Gait/GaitGenerator.h" 9 | 10 | class State_SwingTest : public FSMState{ 11 | public: 12 | State_SwingTest(CtrlComponents *ctrlComp); 13 | ~State_SwingTest(){}; 14 | void enter(); 15 | void run(); 16 | void exit(); 17 | FSMStateName checkChange(); 18 | private: 19 | void _positionCtrl(); 20 | void _torqueCtrl(); 21 | 22 | Vec34 _initFeetPos, _feetPos; 23 | Vec3 _initPos, _posGoal; 24 | Vec12 _targetPos; 25 | float _xMin, _xMax; 26 | float _yMin, _yMax; 27 | float _zMin, _zMax; 28 | Mat3 _Kp, _Kd; 29 | }; 30 | 31 | #endif // STATE_SWINGTEST_H -------------------------------------------------------------------------------- /unitree_guide/include/FSM/State_Trotting.h: -------------------------------------------------------------------------------- 1 | /********************************************************************** 2 | Copyright (c) 2020-2023, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ***********************************************************************/ 4 | #ifndef TROTTING_H 5 | #define TROTTING_H 6 | 7 | #include "FSM/FSMState.h" 8 | #include "Gait/GaitGenerator.h" 9 | #include "control/BalanceCtrl.h" 10 | 11 | class State_Trotting : public FSMState{ 12 | public: 13 | State_Trotting(CtrlComponents *ctrlComp); 14 | ~State_Trotting(); 15 | void enter(); 16 | void run(); 17 | void exit(); 18 | virtual FSMStateName checkChange(); 19 | void setHighCmd(double vx, double vy, double wz); 20 | private: 21 | void calcTau(); 22 | void calcQQd(); 23 | void calcCmd(); 24 | virtual void getUserCmd(); 25 | void calcBalanceKp(); 26 | bool checkStepOrNot(); 27 | 28 | GaitGenerator *_gait; 29 | Estimator *_est; 30 | QuadrupedRobot *_robModel; 31 | BalanceCtrl *_balCtrl; 32 | 33 | // Rob State 34 | Vec3 _posBody, _velBody; 35 | double _yaw, _dYaw; 36 | Vec34 _posFeetGlobal, _velFeetGlobal; 37 | Vec34 _posFeet2BGlobal; 38 | RotMat _B2G_RotMat, _G2B_RotMat; 39 | Vec12 _q; 40 | 41 | // Robot command 42 | Vec3 _pcd; 43 | Vec3 _vCmdGlobal, _vCmdBody; 44 | double _yawCmd, _dYawCmd; 45 | double _dYawCmdPast; 46 | Vec3 _wCmdGlobal; 47 | Vec34 _posFeetGlobalGoal, _velFeetGlobalGoal; 48 | Vec34 _posFeet2BGoal, _velFeet2BGoal; 49 | RotMat _Rd; 50 | Vec3 _ddPcd, _dWbd; 51 | Vec34 _forceFeetGlobal, _forceFeetBody; 52 | Vec34 _qGoal, _qdGoal; 53 | Vec12 _tau; 54 | 55 | // Control Parameters 56 | double _gaitHeight; 57 | Vec3 _posError, _velError; 58 | Mat3 _Kpp, _Kdp, _Kdw; 59 | double _kpw; 60 | Mat3 _KpSwing, _KdSwing; 61 | Vec2 _vxLim, _vyLim, _wyawLim; 62 | Vec4 *_phase; 63 | VecInt4 *_contact; 64 | 65 | // Calculate average value 66 | AvgCov *_avg_posError = new AvgCov(3, "_posError", true, 1000, 1000, 1); 67 | AvgCov *_avg_angError = new AvgCov(3, "_angError", true, 1000, 1000, 1000); 68 | }; 69 | 70 | #endif // TROTTING_H -------------------------------------------------------------------------------- /unitree_guide/include/FSM/State_move_base.h: -------------------------------------------------------------------------------- 1 | /********************************************************************** 2 | Copyright (c) 2020-2023, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ***********************************************************************/ 4 | #ifdef COMPILE_WITH_MOVE_BASE 5 | 6 | #ifndef STATE_MOVE_BASE_H 7 | #define STATE_MOVE_BASE_H 8 | 9 | #include "FSM/State_Trotting.h" 10 | #include "ros/ros.h" 11 | #include 12 | 13 | class State_move_base : public State_Trotting{ 14 | public: 15 | State_move_base(CtrlComponents *ctrlComp); 16 | ~State_move_base(){} 17 | FSMStateName checkChange(); 18 | private: 19 | void getUserCmd(); 20 | void initRecv(); 21 | void twistCallback(const geometry_msgs::Twist& msg); 22 | ros::NodeHandle _nm; 23 | ros::Subscriber _cmdSub; 24 | double _vx, _vy; 25 | double _wz; 26 | }; 27 | 28 | #endif // STATE_MOVE_BASE_H 29 | 30 | #endif // COMPILE_WITH_MOVE_BASE -------------------------------------------------------------------------------- /unitree_guide/include/Gait/FeetEndCal.h: -------------------------------------------------------------------------------- 1 | /********************************************************************** 2 | Copyright (c) 2020-2023, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ***********************************************************************/ 4 | #ifndef FEETENDCAL_H 5 | #define FEETENDCAL_H 6 | 7 | #include "control/CtrlComponents.h" 8 | #include "message/LowlevelState.h" 9 | 10 | class FeetEndCal{ 11 | public: 12 | FeetEndCal(CtrlComponents *ctrlComp); 13 | ~FeetEndCal(); 14 | Vec3 calFootPos(int legID, Vec2 vxyGoalGlobal, float dYawGoal, float phase); 15 | private: 16 | LowlevelState *_lowState; 17 | Estimator *_est; 18 | QuadrupedRobot *_robModel; 19 | 20 | Vec3 _nextStep, _footPos; 21 | Vec3 _bodyVelGlobal; // linear velocity 22 | Vec3 _bodyAccGlobal; // linear accelerator 23 | Vec3 _bodyWGlobal; // angular velocity 24 | 25 | Vec4 _feetRadius, _feetInitAngle; 26 | float _yaw, _dYaw, _nextYaw; 27 | 28 | float _Tstance, _Tswing; 29 | float _kx, _ky, _kyaw; 30 | }; 31 | 32 | #endif // FEETENDCAL_H -------------------------------------------------------------------------------- /unitree_guide/include/Gait/GaitGenerator.h: -------------------------------------------------------------------------------- 1 | /********************************************************************** 2 | Copyright (c) 2020-2023, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ***********************************************************************/ 4 | #ifndef GAITGENERATOR_H 5 | #define GAITGENERATOR_H 6 | 7 | #include "Gait/WaveGenerator.h" 8 | #include "Gait/FeetEndCal.h" 9 | 10 | #ifdef COMPILE_DEBUG 11 | #include 12 | #endif // COMPILE_DEBUG 13 | 14 | /*cycloid gait*/ 15 | class GaitGenerator{ 16 | public: 17 | GaitGenerator(CtrlComponents *ctrlComp); 18 | ~GaitGenerator(); 19 | void setGait(Vec2 vxyGoalGlobal, float dYawGoal, float gaitHeight); 20 | void run(Vec34 &feetPos, Vec34 &feetVel); 21 | Vec3 getFootPos(int i); 22 | Vec3 getFootVel(int i); 23 | void restart(); 24 | private: 25 | float cycloidXYPosition(float startXY, float endXY, float phase); 26 | float cycloidXYVelocity(float startXY, float endXY, float phase); 27 | float cycloidZPosition(float startZ, float height, float phase); 28 | float cycloidZVelocity(float height, float phase); 29 | 30 | WaveGenerator *_waveG; 31 | Estimator *_est; 32 | FeetEndCal *_feetCal; 33 | QuadrupedRobot *_robModel; 34 | LowlevelState *_state; 35 | float _gaitHeight; 36 | Vec2 _vxyGoal; 37 | float _dYawGoal; 38 | Vec4 *_phase, _phasePast; 39 | VecInt4 *_contact; 40 | Vec34 _startP, _endP, _idealP, _pastP; 41 | bool _firstRun; 42 | 43 | #ifdef COMPILE_DEBUG 44 | PyPlot _testGaitPlot; 45 | #endif // COMPILE_DEBUG 46 | 47 | }; 48 | 49 | #endif // GAITGENERATOR_H -------------------------------------------------------------------------------- /unitree_guide/include/Gait/WaveGenerator.h: -------------------------------------------------------------------------------- 1 | /********************************************************************** 2 | Copyright (c) 2020-2023, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ***********************************************************************/ 4 | #ifndef WAVEGENERATOR_H 5 | #define WAVEGENERATOR_H 6 | 7 | #include "common/mathTypes.h" 8 | #include "common/timeMarker.h" 9 | #include "common/enumClass.h" 10 | #include 11 | 12 | #ifdef COMPILE_DEBUG 13 | #include "common/PyPlot.h" 14 | #endif // COMPILE_DEBUG 15 | 16 | /*generate linear wave, [0, 1]*/ 17 | class WaveGenerator{ 18 | public: 19 | WaveGenerator(double period, double stancePhaseRatio, Vec4 bias); 20 | ~WaveGenerator(); 21 | void calcContactPhase(Vec4 &phaseResult, VecInt4 &contactResult, WaveStatus status); 22 | float getTstance(); 23 | float getTswing(); 24 | float getT(); 25 | private: 26 | void calcWave(Vec4 &phase, VecInt4 &contact, WaveStatus status); 27 | 28 | double _period; 29 | double _stRatio; 30 | Vec4 _bias; 31 | 32 | Vec4 _normalT; // [0, 1) 33 | Vec4 _phase, _phasePast; 34 | VecInt4 _contact, _contactPast; 35 | VecInt4 _switchStatus; // 1: switching, 0: do not switch 36 | WaveStatus _statusPast; 37 | 38 | double _passT; // unit: second 39 | long long _startT; // unit: us 40 | #ifdef COMPILE_DEBUG 41 | PyPlot _testPlot; 42 | #endif // COMPILE_DEBUG 43 | 44 | }; 45 | 46 | #endif // WAVEGENERATOR_H -------------------------------------------------------------------------------- /unitree_guide/include/common/LowPassFilter.h: -------------------------------------------------------------------------------- 1 | /********************************************************************** 2 | Copyright (c) 2020-2023, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ***********************************************************************/ 4 | #ifndef LOWPASSFILTER 5 | #define LOWPASSFILTER 6 | 7 | class LPFilter{ 8 | public: 9 | LPFilter(double samplePeriod, double cutFrequency); 10 | ~LPFilter(); 11 | void addValue(double newValue); 12 | double getValue(); 13 | void clear(); 14 | private: 15 | double _weight; 16 | double _pastValue; 17 | bool _start; 18 | }; 19 | 20 | #endif // LOWPASSFILTER -------------------------------------------------------------------------------- /unitree_guide/include/common/enumClass.h: -------------------------------------------------------------------------------- 1 | /********************************************************************** 2 | Copyright (c) 2020-2023, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ***********************************************************************/ 4 | #ifndef ENUMCLASS_H 5 | #define ENUMCLASS_H 6 | 7 | #include 8 | #include 9 | 10 | enum class CtrlPlatform{ 11 | GAZEBO, 12 | REALROBOT, 13 | }; 14 | 15 | enum class RobotType{ 16 | A1, 17 | Go1 18 | }; 19 | 20 | enum class UserCommand{ 21 | // EXIT, 22 | NONE, 23 | START, // trotting 24 | L2_A, // fixedStand 25 | L2_B, // passive 26 | L2_X, // freeStand 27 | #ifdef COMPILE_WITH_MOVE_BASE 28 | L2_Y, // move_base 29 | #endif // COMPILE_WITH_MOVE_BASE 30 | L1_X, // balanceTest 31 | L1_A, // swingTest 32 | L1_Y // stepTest 33 | }; 34 | 35 | enum class FrameType{ 36 | BODY, 37 | HIP, 38 | GLOBAL 39 | }; 40 | 41 | enum class WaveStatus{ 42 | STANCE_ALL, 43 | SWING_ALL, 44 | WAVE_ALL 45 | }; 46 | 47 | enum class FSMMode{ 48 | NORMAL, 49 | CHANGE 50 | }; 51 | 52 | enum class FSMStateName{ 53 | // EXIT, 54 | INVALID, 55 | PASSIVE, 56 | FIXEDSTAND, 57 | FREESTAND, 58 | TROTTING, 59 | #ifdef COMPILE_WITH_MOVE_BASE 60 | MOVE_BASE, // move_base 61 | #endif // COMPILE_WITH_MOVE_BASE 62 | BALANCETEST, 63 | SWINGTEST, 64 | STEPTEST 65 | }; 66 | 67 | #endif // ENUMCLASS_H -------------------------------------------------------------------------------- /unitree_guide/include/common/mathTypes.h: -------------------------------------------------------------------------------- 1 | /********************************************************************** 2 | Copyright (c) 2020-2023, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ***********************************************************************/ 4 | #ifndef MATHTYPES_H 5 | #define MATHTYPES_H 6 | 7 | #include 8 | 9 | /************************/ 10 | /******** Vector ********/ 11 | /************************/ 12 | // 2x1 Vector 13 | using Vec2 = typename Eigen::Matrix; 14 | 15 | // 3x1 Vector 16 | using Vec3 = typename Eigen::Matrix; 17 | 18 | // 4x1 Vector 19 | using Vec4 = typename Eigen::Matrix; 20 | 21 | // 6x1 Vector 22 | using Vec6 = typename Eigen::Matrix; 23 | 24 | // Quaternion 25 | using Quat = typename Eigen::Matrix; 26 | 27 | // 4x1 Integer Vector 28 | using VecInt4 = typename Eigen::Matrix; 29 | 30 | // 12x1 Vector 31 | using Vec12 = typename Eigen::Matrix; 32 | 33 | // 18x1 Vector 34 | using Vec18 = typename Eigen::Matrix; 35 | 36 | // Dynamic Length Vector 37 | using VecX = typename Eigen::Matrix; 38 | 39 | /************************/ 40 | /******** Matrix ********/ 41 | /************************/ 42 | // Rotation Matrix 43 | using RotMat = typename Eigen::Matrix; 44 | 45 | // Homogenous Matrix 46 | using HomoMat = typename Eigen::Matrix; 47 | 48 | // 2x2 Matrix 49 | using Mat2 = typename Eigen::Matrix; 50 | 51 | // 3x3 Matrix 52 | using Mat3 = typename Eigen::Matrix; 53 | 54 | // 3x3 Identity Matrix 55 | #define I3 Eigen::MatrixXd::Identity(3, 3) 56 | 57 | // 3x4 Matrix, each column is a 3x1 vector 58 | using Vec34 = typename Eigen::Matrix; 59 | 60 | // 6x6 Matrix 61 | using Mat6 = typename Eigen::Matrix; 62 | 63 | // 12x12 Matrix 64 | using Mat12 = typename Eigen::Matrix; 65 | 66 | // 12x12 Identity Matrix 67 | #define I12 Eigen::MatrixXd::Identity(12, 12) 68 | 69 | // 18x18 Identity Matrix 70 | #define I18 Eigen::MatrixXd::Identity(18, 18) 71 | 72 | // Dynamic Size Matrix 73 | using MatX = typename Eigen::Matrix; 74 | 75 | /************************/ 76 | /****** Functions *******/ 77 | /************************/ 78 | inline Vec34 vec12ToVec34(Vec12 vec12){ 79 | Vec34 vec34; 80 | for(int i(0); i < 4; ++i){ 81 | vec34.col(i) = vec12.segment(3*i, 3); 82 | } 83 | return vec34; 84 | } 85 | 86 | inline Vec12 vec34ToVec12(Vec34 vec34){ 87 | Vec12 vec12; 88 | for(int i(0); i < 4; ++i){ 89 | vec12.segment(3*i, 3) = vec34.col(i); 90 | } 91 | return vec12; 92 | } 93 | 94 | #endif // MATHTYPES_H -------------------------------------------------------------------------------- /unitree_guide/include/common/timeMarker.h: -------------------------------------------------------------------------------- 1 | /********************************************************************** 2 | Copyright (c) 2020-2023, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ***********************************************************************/ 4 | #ifndef TIMEMARKER_H 5 | #define TIMEMARKER_H 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | //时间戳 微秒级， 需要#include 12 | inline long long getSystemTime(){ 13 | struct timeval t; 14 | gettimeofday(&t, NULL); 15 | return 1000000 * t.tv_sec + t.tv_usec; 16 | } 17 | //时间戳 秒级， 需要getSystemTime() 18 | inline double getTimeSecond(){ 19 | double time = getSystemTime() * 0.000001; 20 | return time; 21 | } 22 | //等待函数，微秒级，从startTime开始等待waitTime微秒 23 | inline void absoluteWait(long long startTime, long long waitTime){ 24 | if(getSystemTime() - startTime > waitTime){ 25 | std::cout << "[WARNING] The waitTime=" << waitTime << " of function absoluteWait is not enough!" << std::endl 26 | << "The program has already cost " << getSystemTime() - startTime << "us." << std::endl; 27 | } 28 | while(getSystemTime() - startTime < waitTime){ 29 | usleep(50); 30 | } 31 | } 32 | 33 | #endif //TIMEMARKER_H -------------------------------------------------------------------------------- /unitree_guide/include/common/unitreeLeg.h: -------------------------------------------------------------------------------- 1 | /********************************************************************** 2 | Copyright (c) 2020-2023, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ***********************************************************************/ 4 | #ifndef UNITREELEG_H 5 | #define UNITREELEG_H 6 | 7 | #include "common/mathTypes.h" 8 | #include "common/enumClass.h" 9 | 10 | class QuadrupedLeg{ 11 | public: 12 | QuadrupedLeg(int legID, float abadLinkLength, float hipLinkLength, 13 | float kneeLinkLength, Vec3 pHip2B); 14 | ~QuadrupedLeg(){} 15 | Vec3 calcPEe2H(Vec3 q); 16 | Vec3 calcPEe2B(Vec3 q); 17 | Vec3 calcVEe(Vec3 q, Vec3 qd); 18 | Vec3 calcQ(Vec3 pEe, FrameType frame); 19 | Vec3 calcQd(Vec3 q, Vec3 vEe); 20 | Vec3 calcQd(Vec3 pEe, Vec3 vEe, FrameType frame); 21 | Vec3 calcTau(Vec3 q, Vec3 force); 22 | Mat3 calcJaco(Vec3 q); 23 | Vec3 getHip2B(){return _pHip2B;} 24 | protected: 25 | float q1_ik(float py, float pz, float b2y); 26 | float q3_ik(float b3z, float b4z, float b); 27 | float q2_ik(float q1, float q3, float px, 28 | float py, float pz, float b3z, float b4z); 29 | float _sideSign; 30 | const float _abadLinkLength, _hipLinkLength, _kneeLinkLength; 31 | const Vec3 _pHip2B; 32 | }; 33 | 34 | class A1Leg : public QuadrupedLeg{ 35 | public: 36 | A1Leg(const int legID, const Vec3 pHip2B): 37 | QuadrupedLeg(legID, 0.0838, 0.2, 0.2, pHip2B){} 38 | ~A1Leg(){} 39 | }; 40 | 41 | class Go1Leg : public QuadrupedLeg{ 42 | public: 43 | Go1Leg(const int legID, const Vec3 pHip2B): 44 | QuadrupedLeg(legID, 0.08, 0.213, 0.213, pHip2B){} 45 | ~Go1Leg(){} 46 | }; 47 | 48 | #endif // UNITREELEG_H -------------------------------------------------------------------------------- /unitree_guide/include/common/unitreeRobot.h: -------------------------------------------------------------------------------- 1 | /********************************************************************** 2 | Copyright (c) 2020-2023, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ***********************************************************************/ 4 | #ifndef UNITREEROBOT_H 5 | #define UNITREEROBOT_H 6 | 7 | #include "common/unitreeLeg.h" 8 | #include "message/LowlevelState.h" 9 | 10 | class QuadrupedRobot{ 11 | public: 12 | QuadrupedRobot(){}; 13 | ~QuadrupedRobot(){} 14 | 15 | Vec3 getX(LowlevelState &state); 16 | Vec34 getVecXP(LowlevelState &state); 17 | 18 | // Inverse Kinematics(Body/Hip Frame) 19 | Vec12 getQ(const Vec34 &feetPosition, FrameType frame); 20 | Vec12 getQd(const Vec34 &feetPosition, const Vec34 &feetVelocity, FrameType frame); 21 | Vec12 getTau(const Vec12 &q, const Vec34 feetForce); 22 | 23 | // Forward Kinematics 24 | Vec3 getFootPosition(LowlevelState &state, int id, FrameType frame); 25 | Vec3 getFootVelocity(LowlevelState &state, int id); 26 | Vec34 getFeet2BPositions(LowlevelState &state, FrameType frame); 27 | Vec34 getFeet2BVelocities(LowlevelState &state, FrameType frame); 28 | 29 | Mat3 getJaco(LowlevelState &state, int legID); 30 | Vec2 getRobVelLimitX(){return _robVelLimitX;} 31 | Vec2 getRobVelLimitY(){return _robVelLimitY;} 32 | Vec2 getRobVelLimitYaw(){return _robVelLimitYaw;} 33 | Vec34 getFeetPosIdeal(){return _feetPosNormalStand;} 34 | double getRobMass(){return _mass;} 35 | Vec3 getPcb(){return _pcb;} 36 | Mat3 getRobInertial(){return _Ib;} 37 | 38 | protected: 39 | QuadrupedLeg* _Legs[4]; 40 | Vec2 _robVelLimitX; 41 | Vec2 _robVelLimitY; 42 | Vec2 _robVelLimitYaw; 43 | Vec34 _feetPosNormalStand; 44 | double _mass; 45 | Vec3 _pcb; 46 | Mat3 _Ib; 47 | }; 48 | 49 | class A1Robot : public QuadrupedRobot{ 50 | public: 51 | A1Robot(); 52 | ~A1Robot(){} 53 | }; 54 | 55 | class Go1Robot : public QuadrupedRobot{ 56 | public: 57 | Go1Robot(); 58 | ~Go1Robot(){}; 59 | }; 60 | 61 | #endif // UNITREEROBOT_H -------------------------------------------------------------------------------- /unitree_guide/include/control/BalanceCtrl.h: -------------------------------------------------------------------------------- 1 | /********************************************************************** 2 | Copyright (c) 2020-2023, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ***********************************************************************/ 4 | #ifndef BALANCECTRL_H 5 | #define BALANCECTRL_H 6 | 7 | #include "common/mathTypes.h" 8 | #include "thirdParty/quadProgpp/QuadProg++.hh" 9 | #include "common/unitreeRobot.h" 10 | 11 | #ifdef COMPILE_DEBUG 12 | #include "common/PyPlot.h" 13 | #endif // COMPILE_DEBUG 14 | 15 | class BalanceCtrl{ 16 | public: 17 | BalanceCtrl(double mass, Mat3 Ib, Mat6 S, double alpha, double beta); 18 | BalanceCtrl(QuadrupedRobot *robModel); 19 | Vec34 calF(Vec3 ddPcd, Vec3 dWbd, RotMat rotM, Vec34 feetPos2B, VecInt4 contact); 20 | #ifdef COMPILE_DEBUG 21 | void setPyPlot(PyPlot *plot){_testPlot = plot;} 22 | #endif // COMPILE_DEBUG 23 | private: 24 | void calMatrixA(Vec34 feetPos2B, RotMat rotM, VecInt4 contact); 25 | void calVectorBd(Vec3 ddPcd, Vec3 dWbd, RotMat rotM); 26 | void calConstraints(VecInt4 contact); 27 | void solveQP(); 28 | 29 | Mat12 _G, _W, _U; 30 | Mat6 _S; 31 | Mat3 _Ib; 32 | Vec6 _bd; 33 | Vec3 _g; 34 | Vec3 _pcb; 35 | Vec12 _F, _Fprev, _g0T; 36 | double _mass, _alpha, _beta, _fricRatio; 37 | Eigen::MatrixXd _CE, _CI; 38 | Eigen::VectorXd _ce0, _ci0; 39 | Eigen::Matrix _A; 40 | Eigen::Matrix _fricMat; 41 | 42 | quadprogpp::Matrix G, CE, CI; 43 | quadprogpp::Vector g0, ce0, ci0, x; 44 | 45 | #ifdef COMPILE_DEBUG 46 | PyPlot *_testPlot; 47 | #endif // COMPILE_DEBUG 48 | }; 49 | 50 | #endif // BALANCECTRL_H -------------------------------------------------------------------------------- /unitree_guide/include/control/ControlFrame.h: -------------------------------------------------------------------------------- 1 | /********************************************************************** 2 | Copyright (c) 2020-2023, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ***********************************************************************/ 4 | #ifndef CONTROLFRAME_H 5 | #define CONTROLFRAME_H 6 | 7 | #include "FSM/FSM.h" 8 | #include "control/CtrlComponents.h" 9 | 10 | class ControlFrame{ 11 | public: 12 | ControlFrame(CtrlComponents *ctrlComp); 13 | ~ControlFrame(){ 14 | delete _FSMController; 15 | } 16 | void run(); 17 | private: 18 | FSM* _FSMController; 19 | CtrlComponents *_ctrlComp; 20 | }; 21 | 22 | #endif //CONTROLFRAME_H -------------------------------------------------------------------------------- /unitree_guide/include/control/CtrlComponents.h: -------------------------------------------------------------------------------- 1 | /********************************************************************** 2 | Copyright (c) 2020-2023, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ***********************************************************************/ 4 | #ifndef CTRLCOMPONENTS_H 5 | #define CTRLCOMPONENTS_H 6 | 7 | #include "message/LowlevelCmd.h" 8 | #include "message/LowlevelState.h" 9 | #include "interface/IOInterface.h" 10 | #include "interface/CmdPanel.h" 11 | #include "common/unitreeRobot.h" 12 | #include "Gait/WaveGenerator.h" 13 | #include "control/Estimator.h" 14 | #include "control/BalanceCtrl.h" 15 | #include 16 | #include 17 | 18 | #ifdef COMPILE_DEBUG 19 | #include "common/PyPlot.h" 20 | #endif // COMPILE_DEBUG 21 | 22 | struct CtrlComponents{ 23 | public: 24 | CtrlComponents(IOInterface *ioInter):ioInter(ioInter){ 25 | lowCmd = new LowlevelCmd(); 26 | lowState = new LowlevelState(); 27 | contact = new VecInt4; 28 | phase = new Vec4; 29 | *contact = VecInt4(0, 0, 0, 0); 30 | *phase = Vec4(0.5, 0.5, 0.5, 0.5); 31 | } 32 | ~CtrlComponents(){ 33 | delete lowCmd; 34 | delete lowState; 35 | delete ioInter; 36 | delete robotModel; 37 | delete waveGen; 38 | delete estimator; 39 | delete balCtrl; 40 | #ifdef COMPILE_DEBUG 41 | delete plot; 42 | #endif // COMPILE_DEBUG 43 | } 44 | LowlevelCmd *lowCmd; 45 | LowlevelState *lowState; 46 | IOInterface *ioInter; 47 | QuadrupedRobot *robotModel; 48 | WaveGenerator *waveGen; 49 | Estimator *estimator; 50 | BalanceCtrl *balCtrl; 51 | 52 | #ifdef COMPILE_DEBUG 53 | PyPlot *plot; 54 | #endif // COMPILE_DEBUG 55 | 56 | VecInt4 *contact; 57 | Vec4 *phase; 58 | 59 | double dt; 60 | bool *running; 61 | CtrlPlatform ctrlPlatform; 62 | 63 | void sendRecv(){ 64 | ioInter->sendRecv(lowCmd, lowState); 65 | } 66 | 67 | void runWaveGen(){ 68 | waveGen->calcContactPhase(*phase, *contact, _waveStatus); 69 | } 70 | 71 | void setAllStance(){ 72 | _waveStatus = WaveStatus::STANCE_ALL; 73 | } 74 | 75 | void setAllSwing(){ 76 | _waveStatus = WaveStatus::SWING_ALL; 77 | } 78 | 79 | void setStartWave(){ 80 | _waveStatus = WaveStatus::WAVE_ALL; 81 | } 82 | 83 | void geneObj(){ 84 | estimator = new Estimator(robotModel, lowState, contact, phase, dt); 85 | balCtrl = new BalanceCtrl(robotModel); 86 | 87 | #ifdef COMPILE_DEBUG 88 | plot = new PyPlot(); 89 | balCtrl->setPyPlot(plot); 90 | estimator->setPyPlot(plot); 91 | #endif // COMPILE_DEBUG 92 | } 93 | 94 | private: 95 | WaveStatus _waveStatus = WaveStatus::SWING_ALL; 96 | 97 | }; 98 | 99 | #endif // CTRLCOMPONENTS_H -------------------------------------------------------------------------------- /unitree_guide/include/interface/CmdPanel.h: -------------------------------------------------------------------------------- 1 | /********************************************************************** 2 | Copyright (c) 2020-2023, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ***********************************************************************/ 4 | #ifndef CMDPANEL_H 5 | #define CMDPANEL_H 6 | 7 | #include "message/unitree_joystick.h" 8 | #include "common/enumClass.h" 9 | #include 10 | 11 | #ifdef COMPILE_WITH_REAL_ROBOT 12 | #ifdef ROBOT_TYPE_A1 13 | #include "unitree_legged_sdk/unitree_legged_sdk.h" 14 | #endif // ROBOT_TYPE_A1 15 | #ifdef ROBOT_TYPE_Go1 16 | #include "unitree_legged_sdk/unitree_legged_sdk.h" 17 | #endif // ROBOT_TYPE_Go1 18 | #endif // COMPILE_WITH_REAL_ROBOT 19 | 20 | struct UserValue{ 21 | float lx; 22 | float ly; 23 | float rx; 24 | float ry; 25 | float L2; 26 | UserValue(){ 27 | setZero(); 28 | } 29 | void setZero(){ 30 | lx = 0; 31 | ly = 0; 32 | rx = 0; 33 | ry = 0; 34 | L2 = 0; 35 | } 36 | }; 37 | 38 | class CmdPanel{ 39 | public: 40 | CmdPanel(){} 41 | virtual ~CmdPanel(){} 42 | UserCommand getUserCmd(){return userCmd;} 43 | UserValue getUserValue(){return userValue;} 44 | void setPassive(){userCmd = UserCommand::L2_B;} 45 | void setZero(){userValue.setZero();} 46 | #ifdef COMPILE_WITH_REAL_ROBOT 47 | virtual void receiveHandle(UNITREE_LEGGED_SDK::LowState *lowState){}; 48 | #endif // COMPILE_WITH_REAL_ROBOT 49 | protected: 50 | virtual void* run(void *arg){return NULL;} 51 | UserCommand userCmd; 52 | UserValue userValue; 53 | }; 54 | 55 | #endif // CMDPANEL_H -------------------------------------------------------------------------------- /unitree_guide/include/interface/IOInterface.h: -------------------------------------------------------------------------------- 1 | /********************************************************************** 2 | Copyright (c) 2020-2023, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ***********************************************************************/ 4 | #ifndef IOINTERFACE_H 5 | #define IOINTERFACE_H 6 | 7 | #include "message/LowlevelCmd.h" 8 | #include "message/LowlevelState.h" 9 | #include "interface/CmdPanel.h" 10 | #include 11 | 12 | class IOInterface{ 13 | public: 14 | IOInterface(){} 15 | ~IOInterface(){delete cmdPanel;} 16 | virtual void sendRecv(const LowlevelCmd *cmd, LowlevelState *state) = 0; 17 | void zeroCmdPanel(){cmdPanel->setZero();} 18 | void setPassive(){cmdPanel->setPassive();} 19 | 20 | protected: 21 | CmdPanel *cmdPanel; 22 | }; 23 | 24 | #endif //IOINTERFACE_H -------------------------------------------------------------------------------- /unitree_guide/include/interface/IOROS.h: -------------------------------------------------------------------------------- 1 | /********************************************************************** 2 | Copyright (c) 2020-2023, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ***********************************************************************/ 4 | #ifdef COMPILE_WITH_ROS 5 | 6 | #ifndef IOROS_H 7 | #define IOROS_H 8 | 9 | #include "ros/ros.h" 10 | #include "interface/IOInterface.h" 11 | #include "unitree_legged_msgs/LowCmd.h" 12 | #include "unitree_legged_msgs/LowState.h" 13 | #include "unitree_legged_msgs/MotorCmd.h" 14 | #include "unitree_legged_msgs/MotorState.h" 15 | #include 16 | #include 17 | 18 | class IOROS : public IOInterface{ 19 | public: 20 | IOROS(); 21 | ~IOROS(); 22 | void sendRecv(const LowlevelCmd *cmd, LowlevelState *state); 23 | 24 | private: 25 | void sendCmd(const LowlevelCmd *cmd); 26 | void recvState(LowlevelState *state); 27 | ros::NodeHandle _nm; 28 | ros::Subscriber _servo_sub[12], _imu_sub; 29 | ros::Publisher _servo_pub[12]; 30 | unitree_legged_msgs::LowCmd _lowCmd; 31 | unitree_legged_msgs::LowState _lowState; 32 | std::string _robot_name; 33 | 34 | //repeated functions for multi-thread 35 | void initRecv(); 36 | void initSend(); 37 | 38 | //Callback functions for ROS 39 | void imuCallback(const sensor_msgs::Imu & msg); 40 | 41 | void FRhipCallback(const unitree_legged_msgs::MotorState& msg); 42 | void FRthighCallback(const unitree_legged_msgs::MotorState& msg); 43 | void FRcalfCallback(const unitree_legged_msgs::MotorState& msg); 44 | 45 | void FLhipCallback(const unitree_legged_msgs::MotorState& msg); 46 | void FLthighCallback(const unitree_legged_msgs::MotorState& msg); 47 | void FLcalfCallback(const unitree_legged_msgs::MotorState& msg); 48 | 49 | void RRhipCallback(const unitree_legged_msgs::MotorState& msg); 50 | void RRthighCallback(const unitree_legged_msgs::MotorState& msg); 51 | void RRcalfCallback(const unitree_legged_msgs::MotorState& msg); 52 | 53 | void RLhipCallback(const unitree_legged_msgs::MotorState& msg); 54 | void RLthighCallback(const unitree_legged_msgs::MotorState& msg); 55 | void RLcalfCallback(const unitree_legged_msgs::MotorState& msg); 56 | }; 57 | 58 | #endif // IOROS_H 59 | 60 | #endif // COMPILE_WITH_ROS -------------------------------------------------------------------------------- /unitree_guide/include/interface/IOSDK.h: -------------------------------------------------------------------------------- 1 | /********************************************************************** 2 | Copyright (c) 2020-2023, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ***********************************************************************/ 4 | #ifndef IOSDK_H 5 | #define IOSDK_H 6 | 7 | #include "interface/IOInterface.h" 8 | #include "unitree_legged_sdk/unitree_legged_sdk.h" 9 | 10 | #ifdef COMPILE_WITH_MOVE_BASE 11 | #include 12 | #include 13 | #include 14 | #endif // COMPILE_WITH_MOVE_BASE 15 | 16 | 17 | class IOSDK : public IOInterface{ 18 | public: 19 | IOSDK(); 20 | ~IOSDK(){} 21 | void sendRecv(const LowlevelCmd *cmd, LowlevelState *state); 22 | 23 | private: 24 | UNITREE_LEGGED_SDK::UDP _udp; 25 | UNITREE_LEGGED_SDK::Safety _safe; 26 | UNITREE_LEGGED_SDK::LowCmd _lowCmd; 27 | UNITREE_LEGGED_SDK::LowState _lowState; 28 | 29 | #ifdef COMPILE_WITH_MOVE_BASE 30 | ros::NodeHandle _nh; 31 | ros::Publisher _pub; 32 | sensor_msgs::JointState _joint_state; 33 | #endif // COMPILE_WITH_MOVE_BASE 34 | }; 35 | 36 | #endif // IOSDK_H -------------------------------------------------------------------------------- /unitree_guide/include/interface/KeyBoard.h: -------------------------------------------------------------------------------- 1 | /********************************************************************** 2 | Copyright (c) 2020-2023, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ***********************************************************************/ 4 | #ifndef KEYBOARD_H 5 | #define KEYBOARD_H 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include "interface/CmdPanel.h" 15 | #include "common/mathTools.h" 16 | 17 | class KeyBoard : public CmdPanel{ 18 | public: 19 | KeyBoard(); 20 | ~KeyBoard(); 21 | private: 22 | static void* runKeyBoard(void *arg); 23 | void* run(void *arg); 24 | UserCommand checkCmd(); 25 | void changeValue(); 26 | 27 | pthread_t _tid; 28 | float sensitivityLeft = 0.05; 29 | float sensitivityRight = 0.05; 30 | struct termios _oldSettings, _newSettings; 31 | fd_set set; 32 | int res; 33 | int ret; 34 | char _c; 35 | }; 36 | 37 | #endif // KEYBOARD_H -------------------------------------------------------------------------------- /unitree_guide/include/interface/WirelessHandle.h: -------------------------------------------------------------------------------- 1 | /********************************************************************** 2 | Copyright (c) 2020-2023, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ***********************************************************************/ 4 | #ifndef WIRELESSHANDLE_H 5 | #define WIRELESSHANDLE_H 6 | 7 | #include "message/unitree_joystick.h" 8 | #include "interface/CmdPanel.h" 9 | #include "unitree_legged_sdk/comm.h" 10 | 11 | class WirelessHandle : public CmdPanel{ 12 | public: 13 | WirelessHandle(); 14 | ~WirelessHandle(){} 15 | void receiveHandle(UNITREE_LEGGED_SDK::LowState *lowState); 16 | private: 17 | xRockerBtnDataStruct _keyData; 18 | }; 19 | 20 | #endif // WIRELESSHANDLE_H -------------------------------------------------------------------------------- /unitree_guide/include/message/LowlevelState.h: -------------------------------------------------------------------------------- 1 | /********************************************************************** 2 | Copyright (c) 2020-2023, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ***********************************************************************/ 4 | #ifndef LOWLEVELSTATE_HPP 5 | #define LOWLEVELSTATE_HPP 6 | 7 | #include 8 | #include "common/mathTypes.h" 9 | #include "common/mathTools.h" 10 | #include "interface/CmdPanel.h" 11 | #include "common/enumClass.h" 12 | 13 | struct MotorState 14 | { 15 | unsigned int mode; 16 | float q; 17 | float dq; 18 | float ddq; 19 | float tauEst; 20 | 21 | MotorState(){ 22 | q = 0; 23 | dq = 0; 24 | ddq = 0; 25 | tauEst = 0; 26 | } 27 | }; 28 | 29 | struct IMU 30 | { 31 | float quaternion[4]; // w, x, y, z 32 | float gyroscope[3]; 33 | float accelerometer[3]; 34 | 35 | IMU(){ 36 | for(int i = 0; i < 3; i++){ 37 | quaternion[i] = 0; 38 | gyroscope[i] = 0; 39 | accelerometer[i] = 0; 40 | } 41 | quaternion[3] = 0; 42 | } 43 | 44 | RotMat getRotMat(){ 45 | Quat quat; 46 | quat << quaternion[0], quaternion[1], quaternion[2], quaternion[3]; 47 | return quatToRotMat(quat); 48 | } 49 | 50 | Vec3 getAcc(){ 51 | Vec3 acc; 52 | acc << accelerometer[0], accelerometer[1], accelerometer[2]; 53 | return acc; 54 | } 55 | 56 | Vec3 getGyro(){ 57 | Vec3 gyro; 58 | gyro << gyroscope[0], gyroscope[1], gyroscope[2]; 59 | return gyro; 60 | } 61 | 62 | Quat getQuat(){ 63 | Quat q; 64 | q << quaternion[0], quaternion[1], quaternion[2], quaternion[3]; 65 | return q; 66 | } 67 | }; 68 | 69 | struct LowlevelState 70 | { 71 | IMU imu; 72 | MotorState motorState[12]; 73 | UserCommand userCmd; 74 | UserValue userValue; 75 | 76 | Vec34 getQ(){ 77 | Vec34 qLegs; 78 | for(int i(0); i < 4; ++i){ 79 | qLegs.col(i)(0) = motorState[3*i ].q; 80 | qLegs.col(i)(1) = motorState[3*i + 1].q; 81 | qLegs.col(i)(2) = motorState[3*i + 2].q; 82 | } 83 | return qLegs; 84 | } 85 | 86 | Vec34 getQd(){ 87 | Vec34 qdLegs; 88 | for(int i(0); i < 4; ++i){ 89 | qdLegs.col(i)(0) = motorState[3*i ].dq; 90 | qdLegs.col(i)(1) = motorState[3*i + 1].dq; 91 | qdLegs.col(i)(2) = motorState[3*i + 2].dq; 92 | } 93 | return qdLegs; 94 | } 95 | 96 | RotMat getRotMat(){ 97 | return imu.getRotMat(); 98 | } 99 | 100 | Vec3 getAcc(){ 101 | return imu.getAcc(); 102 | } 103 | 104 | Vec3 getGyro(){ 105 | return imu.getGyro(); 106 | } 107 | 108 | Vec3 getAccGlobal(){ 109 | return getRotMat() * getAcc(); 110 | } 111 | 112 | Vec3 getGyroGlobal(){ 113 | return getRotMat() * getGyro(); 114 | } 115 | 116 | double getYaw(){ 117 | return rotMatToRPY(getRotMat())(2); 118 | } 119 | 120 | double getDYaw(){ 121 | return getGyroGlobal()(2); 122 | } 123 | 124 | void setQ(Vec12 q){ 125 | for(int i(0); i<12; ++i){ 126 | motorState[i].q = q(i); 127 | } 128 | } 129 | }; 130 | 131 | #endif //LOWLEVELSTATE_HPP -------------------------------------------------------------------------------- /unitree_guide/include/message/unitree_joystick.h: -------------------------------------------------------------------------------- 1 | /********************************************************************** 2 | Copyright (c) 2020-2023, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ***********************************************************************/ 4 | #ifndef UNITREE_JOYSTICK_H 5 | #define UNITREE_JOYSTICK_H 6 | 7 | #include 8 | // 16b 9 | typedef union { 10 | struct { 11 | uint8_t R1 :1; 12 | uint8_t L1 :1; 13 | uint8_t start :1; 14 | uint8_t select :1; 15 | uint8_t R2 :1; 16 | uint8_t L2 :1; 17 | uint8_t F1 :1; 18 | uint8_t F2 :1; 19 | uint8_t A :1; 20 | uint8_t B :1; 21 | uint8_t X :1; 22 | uint8_t Y :1; 23 | uint8_t up :1; 24 | uint8_t right :1; 25 | uint8_t down :1; 26 | uint8_t left :1; 27 | } components; 28 | uint16_t value; 29 | } xKeySwitchUnion; 30 | 31 | // 40 Byte (now used 24B) 32 | typedef struct { 33 | uint8_t head[2]; 34 | xKeySwitchUnion btn; 35 | float lx; 36 | float rx; 37 | float ry; 38 | float L2; 39 | float ly; 40 | 41 | uint8_t idle[16]; 42 | } xRockerBtnDataStruct; 43 | 44 | #endif // UNITREE_JOYSTICK_H -------------------------------------------------------------------------------- /unitree_guide/include/thirdParty/quadProgpp/QuadProg++.hh: -------------------------------------------------------------------------------- 1 | /* 2 | File $Id: QuadProg++.hh 232 2007-06-21 12:29:00Z digasper $ 3 | 4 | The quadprog_solve() function implements the algorithm of Goldfarb and Idnani 5 | for the solution of a (convex) Quadratic Programming problem 6 | by means of an active-set dual method. 7 | 8 | The problem is in the form: 9 | 10 | min 0.5 * x G x + g0 x 11 | s.t. 12 | CE^T x + ce0 = 0 13 | CI^T x + ci0 >= 0 14 | 15 | The matrix and vectors dimensions are as follows: 16 | G: n * n 17 | g0: n 18 | 19 | CE: n * p 20 | ce0: p 21 | 22 | CI: n * m 23 | ci0: m 24 | 25 | x: n 26 | 27 | The function will return the cost of the solution written in the x vector or 28 | std::numeric_limits::infinity() if the problem is infeasible. In the latter case 29 | the value of the x vector is not correct. 30 | 31 | References: D. Goldfarb, A. Idnani. A numerically stable dual method for solving 32 | strictly convex quadratic programs. Mathematical Programming 27 (1983) pp. 1-33. 33 | 34 | Notes: 35 | 1. pay attention in setting up the vectors ce0 and ci0. 36 | If the constraints of your problem are specified in the form 37 | A^T x = b and C^T x >= d, then you should set ce0 = -b and ci0 = -d. 38 | 2. The matrices have column dimension equal to MATRIX_DIM, 39 | a constant set to 20 in this file (by means of a #define macro). 40 | If the matrices are bigger than 20 x 20 the limit could be 41 | increased by means of a -DMATRIX_DIM=n on the compiler command line. 42 | 3. The matrix G is modified within the function since it is used to compute 43 | the G = L^T L cholesky factorization for further computations inside the function. 44 | If you need the original matrix G you should make a copy of it and pass the copy 45 | to the function. 46 | 47 | Author: Luca Di Gaspero 48 | DIEGM - University of Udine, Italy 49 | luca.digaspero@uniud.it 50 | http://www.diegm.uniud.it/digaspero/ 51 | 52 | The author will be grateful if the researchers using this software will 53 | acknowledge the contribution of this function in their research papers. 54 | 55 | Copyright (c) 2007-2016 Luca Di Gaspero 56 | 57 | This software may be modified and distributed under the terms 58 | of the MIT license. See the LICENSE file for details. 59 | */ 60 | 61 | 62 | #ifndef _QUADPROGPP 63 | #define _QUADPROGPP 64 | 65 | #include "Array.hh" 66 | #include 67 | 68 | namespace quadprogpp { 69 | 70 | double solve_quadprog(Matrix& G, Vector& g0, 71 | const Matrix& CE, const Vector& ce0, 72 | const Matrix& CI, const Vector& ci0, 73 | Vector& x); 74 | 75 | } // namespace quadprogpp 76 | 77 | #endif // #define _QUADPROGPP 78 | -------------------------------------------------------------------------------- /unitree_guide/launch/gazeboSim.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 28 | 29 | 30 | 31 | 33 | 34 | 35 | 36 | 37 | 38 | 44 | 45 | 46 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | -------------------------------------------------------------------------------- /unitree_guide/library/unitree_legged_sdk-3.8.0/example/example_joystick.cpp: -------------------------------------------------------------------------------- 1 | /************************************************************************ 2 | Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rights reserved. 3 | Use of this source code is governed by the MPL-2.0 license, see LICENSE. 4 | ************************************************************************/ 5 | 6 | #include "unitree_legged_sdk/unitree_legged_sdk.h" 7 | #include "unitree_legged_sdk/joystick.h" 8 | #include 9 | #include 10 | #include 11 | 12 | using namespace UNITREE_LEGGED_SDK; 13 | 14 | class Custom 15 | { 16 | public: 17 | Custom(uint8_t level): 18 | safe(LeggedType::Go1), 19 | udp(level, 8090, "192.168.123.10", 8007){ 20 | udp.InitCmdData(cmd); 21 | } 22 | void UDPSend(); 23 | void UDPRecv(); 24 | void RobotControl(); 25 | 26 | Safety safe; 27 | UDP udp; 28 | LowCmd cmd = {0}; 29 | LowState state = {0}; 30 | xRockerBtnDataStruct _keyData; 31 | int motiontime = 0; 32 | float dt = 0.002; // 0.001~0.01 33 | }; 34 | 35 | void Custom::UDPRecv() 36 | { 37 | udp.Recv(); 38 | } 39 | 40 | void Custom::UDPSend() 41 | { 42 | udp.Send(); 43 | } 44 | 45 | void Custom::RobotControl() 46 | { 47 | motiontime++; 48 | udp.GetRecv(state); 49 | 50 | memcpy(&_keyData, &state.wirelessRemote[0], 40); 51 | 52 | if((int)_keyData.btn.components.A == 1){ 53 | std::cout << "The key A is pressed, and the value of lx is " << _keyData.lx << std::endl; 54 | } 55 | 56 | safe.PowerProtect(cmd, state, 1); 57 | udp.SetSend(cmd); 58 | } 59 | 60 | int main(void) 61 | { 62 | std::cout << "Communication level is set to LOW-level." << std::endl 63 | << "WARNING: Make sure the robot is hung up." << std::endl 64 | << "Press Enter to continue..." << std::endl; 65 | std::cin.ignore(); 66 | 67 | Custom custom(LOWLEVEL); 68 | // InitEnvironment(); 69 | LoopFunc loop_control("control_loop", custom.dt, boost::bind(&Custom::RobotControl, &custom)); 70 | LoopFunc loop_udpSend("udp_send", custom.dt, 3, boost::bind(&Custom::UDPSend, &custom)); 71 | LoopFunc loop_udpRecv("udp_recv", custom.dt, 3, boost::bind(&Custom::UDPRecv, &custom)); 72 | 73 | loop_udpSend.start(); 74 | loop_udpRecv.start(); 75 | loop_control.start(); 76 | 77 | while(1){ 78 | sleep(10); 79 | }; 80 | 81 | return 0; 82 | } 83 | -------------------------------------------------------------------------------- /unitree_guide/library/unitree_legged_sdk-3.8.0/example/example_torque.cpp: -------------------------------------------------------------------------------- 1 | /***************************************************************** 2 | Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ******************************************************************/ 4 | 5 | #include "unitree_legged_sdk/unitree_legged_sdk.h" 6 | #include 7 | #include 8 | #include 9 | 10 | using namespace UNITREE_LEGGED_SDK; 11 | 12 | class Custom 13 | { 14 | public: 15 | Custom(uint8_t level): 16 | safe(LeggedType::Go1), 17 | udp(level, 8090, "192.168.123.10", 8007){ 18 | udp.InitCmdData(cmd); 19 | } 20 | void UDPSend(); 21 | void UDPRecv(); 22 | void RobotControl(); 23 | 24 | Safety safe; 25 | UDP udp; 26 | LowCmd cmd = {0}; 27 | LowState state = {0}; 28 | int motiontime = 0; 29 | float dt = 0.002; // 0.001~0.01 30 | }; 31 | 32 | void Custom::UDPRecv() 33 | { 34 | udp.Recv(); 35 | } 36 | 37 | void Custom::UDPSend() 38 | { 39 | udp.Send(); 40 | } 41 | 42 | void Custom::RobotControl() 43 | { 44 | motiontime++; 45 | udp.GetRecv(state); 46 | // printf("%d\n", motiontime); 47 | // gravity compensation 48 | cmd.motorCmd[FR_0].tau = -0.65f; 49 | cmd.motorCmd[FL_0].tau = +0.65f; 50 | cmd.motorCmd[RR_0].tau = -0.65f; 51 | cmd.motorCmd[RL_0].tau = +0.65f; 52 | 53 | if( motiontime >= 500){ 54 | float torque = (0 - state.motorState[FR_1].q)*10.0f + (0 - state.motorState[FR_1].dq)*1.0f; 55 | if(torque > 5.0f) torque = 5.0f; 56 | if(torque < -5.0f) torque = -5.0f; 57 | // if(torque > 15.0f) torque = 15.0f; 58 | // if(torque < -15.0f) torque = -15.0f; 59 | 60 | cmd.motorCmd[FR_1].q = PosStopF; 61 | cmd.motorCmd[FR_1].dq = VelStopF; 62 | cmd.motorCmd[FR_1].Kp = 0; 63 | cmd.motorCmd[FR_1].Kd = 0; 64 | cmd.motorCmd[FR_1].tau = torque; 65 | } 66 | int res = safe.PowerProtect(cmd, state, 1); 67 | if(res < 0) exit(-1); 68 | 69 | udp.SetSend(cmd); 70 | } 71 | 72 | int main(void) 73 | { 74 | std::cout << "Communication level is set to LOW-level." << std::endl 75 | << "WARNING: Make sure the robot is hung up." << std::endl 76 | << "Press Enter to continue..." << std::endl; 77 | std::cin.ignore(); 78 | 79 | Custom custom(LOWLEVEL); 80 | // InitEnvironment(); 81 | LoopFunc loop_control("control_loop", custom.dt, boost::bind(&Custom::RobotControl, &custom)); 82 | LoopFunc loop_udpSend("udp_send", custom.dt, 3, boost::bind(&Custom::UDPSend, &custom)); 83 | LoopFunc loop_udpRecv("udp_recv", custom.dt, 3, boost::bind(&Custom::UDPRecv, &custom)); 84 | 85 | loop_udpSend.start(); 86 | loop_udpRecv.start(); 87 | loop_control.start(); 88 | 89 | while(1){ 90 | sleep(10); 91 | }; 92 | 93 | return 0; 94 | } 95 | -------------------------------------------------------------------------------- /unitree_guide/library/unitree_legged_sdk-3.8.0/example/example_velocity.cpp: -------------------------------------------------------------------------------- 1 | /***************************************************************** 2 | Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ******************************************************************/ 4 | 5 | #include "unitree_legged_sdk/unitree_legged_sdk.h" 6 | #include 7 | #include 8 | 9 | using namespace UNITREE_LEGGED_SDK; 10 | 11 | class Custom 12 | { 13 | public: 14 | Custom(uint8_t level): 15 | safe(LeggedType::Go1), 16 | udp(level, 8090, "192.168.123.10", 8007){ 17 | udp.InitCmdData(cmd); 18 | } 19 | void UDPRecv(); 20 | void UDPSend(); 21 | void RobotControl(); 22 | 23 | Safety safe; 24 | UDP udp; 25 | LowCmd cmd = {0}; 26 | LowState state = {0}; 27 | int Tpi = 0; 28 | int motiontime = 0; 29 | float dt = 0.002; // 0.001~0.01 30 | }; 31 | 32 | void Custom::UDPRecv() 33 | { 34 | udp.Recv(); 35 | } 36 | 37 | void Custom::UDPSend() 38 | { 39 | udp.Send(); 40 | } 41 | 42 | void Custom::RobotControl() 43 | { 44 | motiontime++; 45 | udp.GetRecv(state); 46 | 47 | // gravity compensation 48 | cmd.motorCmd[FR_0].tau = -0.65f; 49 | cmd.motorCmd[FL_0].tau = +0.65f; 50 | cmd.motorCmd[RR_0].tau = -0.65f; 51 | cmd.motorCmd[RL_0].tau = +0.65f; 52 | 53 | if( motiontime >= 500){ 54 | float speed = 2 * sin(3*M_PI*Tpi/1500.0); 55 | 56 | cmd.motorCmd[FR_1].q = PosStopF; 57 | cmd.motorCmd[FR_1].dq = speed; 58 | cmd.motorCmd[FR_1].Kp = 0; 59 | cmd.motorCmd[FR_1].Kd = 4; 60 | // cmd.motorCmd[FR_1].Kd = 6; 61 | cmd.motorCmd[FR_1].tau = 0.0f; 62 | Tpi++; 63 | } 64 | 65 | if(motiontime > 10){ 66 | int res1 = safe.PowerProtect(cmd, state, 1); 67 | // You can uncomment it for position protection 68 | // int res2 = safe.PositionProtect(cmd, state, 10); 69 | if(res1 < 0) exit(-1); 70 | } 71 | 72 | udp.SetSend(cmd); 73 | } 74 | 75 | int main(void) 76 | { 77 | std::cout << "Communication level is set to LOW-level." << std::endl 78 | << "WARNING: Make sure the robot is hung up." << std::endl 79 | << "Press Enter to continue..." << std::endl; 80 | std::cin.ignore(); 81 | 82 | Custom custom(LOWLEVEL); 83 | // InitEnvironment(); 84 | LoopFunc loop_control("control_loop", custom.dt, boost::bind(&Custom::RobotControl, &custom)); 85 | LoopFunc loop_udpSend("udp_send", custom.dt, 3, boost::bind(&Custom::UDPSend, &custom)); 86 | LoopFunc loop_udpRecv("udp_recv", custom.dt, 3, boost::bind(&Custom::UDPRecv, &custom)); 87 | 88 | loop_udpSend.start(); 89 | loop_udpRecv.start(); 90 | loop_control.start(); 91 | 92 | while(1){ 93 | sleep(10); 94 | }; 95 | 96 | return 0; 97 | } 98 | -------------------------------------------------------------------------------- /unitree_guide/library/unitree_legged_sdk-3.8.0/example_py/example_torque.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | import sys 4 | import time 5 | import math 6 | import numpy as np 7 | 8 | sys.path.append('../lib/python/amd64') 9 | import robot_interface as sdk 10 | 11 | 12 | if __name__ == '__main__': 13 | 14 | d = {'FR_0':0, 'FR_1':1, 'FR_2':2, 15 | 'FL_0':3, 'FL_1':4, 'FL_2':5, 16 | 'RR_0':6, 'RR_1':7, 'RR_2':8, 17 | 'RL_0':9, 'RL_1':10, 'RL_2':11 } 18 | PosStopF = math.pow(10,9) 19 | VelStopF = 16000.0 20 | HIGHLEVEL = 0xee 21 | LOWLEVEL = 0xff 22 | dt = 0.002 23 | sin_count = 0 24 | 25 | udp = sdk.UDP(LOWLEVEL, 8080, "192.168.123.10", 8007) 26 | safe = sdk.Safety(sdk.LeggedType.Go1) 27 | 28 | cmd = sdk.LowCmd() 29 | state = sdk.LowState() 30 | udp.InitCmdData(cmd) 31 | 32 | Tpi = 0 33 | motiontime = 0 34 | while True: 35 | time.sleep(0.002) 36 | motiontime += 1 37 | 38 | # freq_Hz = 1 39 | freq_Hz = 2 40 | # freq_Hz = 5; 41 | freq_rad = freq_Hz * 2* math.pi 42 | # t = dt*sin_count 43 | 44 | # print(motiontime) 45 | # print(state.imu.rpy[0]) 46 | 47 | 48 | udp.Recv() 49 | udp.GetRecv(state) 50 | 51 | if( motiontime >= 500): 52 | sin_count += 1 53 | torque = (0 - state.motorState[d['FR_1']].q)*10.0 + (0 - state.motorState[d['FR_1']].dq)*1.0 54 | # torque = (0 - state.motorState[d['FR_1']].q)*20.0 + (0 - state.motorState[d['FR_1']].dq)*2.0 55 | # torque = 2 * sin(t*freq_rad) 56 | torque = np.fmin(np.fmax(torque, -5.0), 5.0) 57 | # torque = np.fmin(np.fmax(torque, -15.0), 15.0) 58 | 59 | 60 | cmd.motorCmd[d['FR_1']].q = PosStopF 61 | cmd.motorCmd[d['FR_1']].dq = VelStopF 62 | cmd.motorCmd[d['FR_1']].Kp = 0 63 | cmd.motorCmd[d['FR_1']].Kd = 0 64 | cmd.motorCmd[d['FR_1']].tau = torque 65 | 66 | if(motiontime > 10): 67 | safe.PowerProtect(cmd, state, 1) 68 | 69 | udp.SetSend(cmd) 70 | udp.Send() 71 | -------------------------------------------------------------------------------- /unitree_guide/library/unitree_legged_sdk-3.8.0/example_py/example_velocity.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | import sys 4 | import time 5 | import math 6 | 7 | sys.path.append('../lib/python/amd64') 8 | import robot_interface as sdk 9 | 10 | 11 | if __name__ == '__main__': 12 | 13 | d = {'FR_0':0, 'FR_1':1, 'FR_2':2, 14 | 'FL_0':3, 'FL_1':4, 'FL_2':5, 15 | 'RR_0':6, 'RR_1':7, 'RR_2':8, 16 | 'RL_0':9, 'RL_1':10, 'RL_2':11 } 17 | PosStopF = math.pow(10,9) 18 | VelStopF = 16000.0 19 | HIGHLEVEL = 0xee 20 | LOWLEVEL = 0xff 21 | 22 | udp = sdk.UDP(LOWLEVEL, 8080, "192.168.123.10", 8007) 23 | safe = sdk.Safety(sdk.LeggedType.Go1) 24 | 25 | cmd = sdk.LowCmd() 26 | state = sdk.LowState() 27 | udp.InitCmdData(cmd) 28 | 29 | Tpi = 0 30 | motiontime = 0 31 | while True: 32 | time.sleep(0.002) 33 | motiontime += 1 34 | 35 | # print(motiontime) 36 | # print(state.imu.rpy[0]) 37 | 38 | 39 | udp.Recv() 40 | udp.GetRecv(state) 41 | 42 | if( motiontime >= 500): 43 | speed = 2 * math.sin(3*math.pi*Tpi/2000.0) 44 | 45 | # cmd.motorCmd[d['FL_2']].q = PosStopF 46 | cmd.motorCmd[d['FL_2']].q = 0 47 | cmd.motorCmd[d['FL_2']].dq = speed 48 | cmd.motorCmd[d['FL_2']].Kp = 0 49 | cmd.motorCmd[d['FL_2']].Kd = 4 50 | cmd.motorCmd[d['FL_2']].tau = 0.0 51 | 52 | Tpi += 1 53 | 54 | if(motiontime > 10): 55 | safe.PowerProtect(cmd, state, 1) 56 | 57 | udp.SetSend(cmd) 58 | udp.Send() 59 | -------------------------------------------------------------------------------- /unitree_guide/library/unitree_legged_sdk-3.8.0/example_py/example_walk.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | import sys 4 | import time 5 | import math 6 | 7 | sys.path.append('../lib/python/amd64') 8 | import robot_interface as sdk 9 | 10 | 11 | if __name__ == '__main__': 12 | 13 | HIGHLEVEL = 0xee 14 | LOWLEVEL = 0xff 15 | 16 | udp = sdk.UDP(HIGHLEVEL, 8080, "192.168.123.161", 8082) 17 | 18 | cmd = sdk.HighCmd() 19 | state = sdk.HighState() 20 | udp.InitCmdData(cmd) 21 | 22 | motiontime = 0 23 | while True: 24 | time.sleep(0.002) 25 | motiontime = motiontime + 1 26 | 27 | udp.Recv() 28 | udp.GetRecv(state) 29 | 30 | # print(motiontime) 31 | # print(state.imu.rpy[0]) 32 | # print(motiontime, state.motorState[0].q, state.motorState[1].q, state.motorState[2].q) 33 | # print(state.imu.rpy[0]) 34 | 35 | cmd.mode = 0 # 0:idle, default stand 1:forced stand 2:walk continuously 36 | cmd.gaitType = 0 37 | cmd.speedLevel = 0 38 | cmd.footRaiseHeight = 0 39 | cmd.bodyHeight = 0 40 | cmd.euler = [0, 0, 0] 41 | cmd.velocity = [0, 0] 42 | cmd.yawSpeed = 0.0 43 | cmd.reserve = 0 44 | 45 | # cmd.mode = 2 46 | # cmd.gaitType = 1 47 | # # cmd.position = [1, 0] 48 | # # cmd.position[0] = 2 49 | # cmd.velocity = [-0.2, 0] # -1 ~ +1 50 | # cmd.yawSpeed = 0 51 | # cmd.bodyHeight = 0.1 52 | 53 | if(motiontime > 0 and motiontime < 1000): 54 | cmd.mode = 1 55 | cmd.euler = [-0.3, 0, 0] 56 | 57 | if(motiontime > 1000 and motiontime < 2000): 58 | cmd.mode = 1 59 | cmd.euler = [0.3, 0, 0] 60 | 61 | if(motiontime > 2000 and motiontime < 3000): 62 | cmd.mode = 1 63 | cmd.euler = [0, -0.2, 0] 64 | 65 | if(motiontime > 3000 and motiontime < 4000): 66 | cmd.mode = 1 67 | cmd.euler = [0, 0.2, 0] 68 | 69 | if(motiontime > 4000 and motiontime < 5000): 70 | cmd.mode = 1 71 | cmd.euler = [0, 0, -0.2] 72 | 73 | if(motiontime > 5000 and motiontime < 6000): 74 | cmd.mode = 1 75 | cmd.euler = [0.2, 0, 0] 76 | 77 | if(motiontime > 6000 and motiontime < 7000): 78 | cmd.mode = 1 79 | cmd.bodyHeight = -0.2 80 | 81 | if(motiontime > 7000 and motiontime < 8000): 82 | cmd.mode = 1 83 | cmd.bodyHeight = 0.1 84 | 85 | if(motiontime > 8000 and motiontime < 9000): 86 | cmd.mode = 1 87 | cmd.bodyHeight = 0.0 88 | 89 | if(motiontime > 9000 and motiontime < 11000): 90 | cmd.mode = 5 91 | 92 | if(motiontime > 11000 and motiontime < 13000): 93 | cmd.mode = 6 94 | 95 | if(motiontime > 13000 and motiontime < 14000): 96 | cmd.mode = 0 97 | 98 | if(motiontime > 14000 and motiontime < 18000): 99 | cmd.mode = 2 100 | cmd.gaitType = 2 101 | cmd.velocity = [0.4, 0] # -1 ~ +1 102 | cmd.yawSpeed = 2 103 | cmd.footRaiseHeight = 0.1 104 | # printf("walk\n") 105 | 106 | if(motiontime > 18000 and motiontime < 20000): 107 | cmd.mode = 0 108 | cmd.velocity = [0, 0] 109 | 110 | if(motiontime > 20000 and motiontime < 24000): 111 | cmd.mode = 2 112 | cmd.gaitType = 1 113 | cmd.velocity = [0.2, 0] # -1 ~ +1 114 | cmd.bodyHeight = 0.1 115 | # printf("walk\n") 116 | 117 | 118 | udp.SetSend(cmd) 119 | udp.Send() 120 | -------------------------------------------------------------------------------- /unitree_guide/library/unitree_legged_sdk-3.8.0/include/unitree_legged_sdk/a1_const.h: -------------------------------------------------------------------------------- 1 | /***************************************************************** 2 | Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ******************************************************************/ 4 | 5 | #ifndef _UNITREE_LEGGED_A1_H_ 6 | #define _UNITREE_LEGGED_A1_H_ 7 | 8 | namespace UNITREE_LEGGED_SDK 9 | { 10 | constexpr double a1_Hip_max = 0.802; // unit:radian ( = 46 degree) 11 | constexpr double a1_Hip_min = -0.802; // unit:radian ( = -46 degree) 12 | constexpr double a1_Thigh_max = 4.19; // unit:radian ( = 240 degree) 13 | constexpr double a1_Thigh_min = -1.05; // unit:radian ( = -60 degree) 14 | constexpr double a1_Calf_max = -0.916; // unit:radian ( = -52.5 degree) 15 | constexpr double a1_Calf_min = -2.7; // unit:radian ( = -154.5 degree) 16 | } 17 | 18 | #endif 19 | -------------------------------------------------------------------------------- /unitree_guide/library/unitree_legged_sdk-3.8.0/include/unitree_legged_sdk/aliengo_const.h: -------------------------------------------------------------------------------- 1 | /***************************************************************** 2 | Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ******************************************************************/ 4 | 5 | #ifndef _UNITREE_LEGGED_ALIENGO_H_ 6 | #define _UNITREE_LEGGED_ALIENGO_H_ 7 | 8 | namespace UNITREE_LEGGED_SDK 9 | { 10 | constexpr double aliengo_Hip_max = 1.047; // unit:radian ( = 60 degree) 11 | constexpr double aliengo_Hip_min = -0.873; // unit:radian ( = -50 degree) 12 | constexpr double aliengo_Thigh_max = 3.927; // unit:radian ( = 225 degree) 13 | constexpr double aliengo_Thigh_min = -0.524; // unit:radian ( = -30 degree) 14 | constexpr double aliengo_Calf_max = -0.611; // unit:radian ( = -35 degree) 15 | constexpr double aliengo_Calf_min = -2.775; // unit:radian ( = -159 degree) 16 | } 17 | 18 | #endif 19 | -------------------------------------------------------------------------------- /unitree_guide/library/unitree_legged_sdk-3.8.0/include/unitree_legged_sdk/go1_const.h: -------------------------------------------------------------------------------- 1 | /***************************************************************** 2 | Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ******************************************************************/ 4 | 5 | #ifndef _UNITREE_LEGGED_GO1_H_ 6 | #define _UNITREE_LEGGED_GO1_H_ 7 | 8 | namespace UNITREE_LEGGED_SDK 9 | { 10 | constexpr double go1_Hip_max = 1.047; // unit:radian ( = 60 degree) 11 | constexpr double go1_Hip_min = -1.047; // unit:radian ( = -60 degree) 12 | constexpr double go1_Thigh_max = 2.966; // unit:radian ( = 170 degree) 13 | constexpr double go1_Thigh_min = -0.663; // unit:radian ( = -38 degree) 14 | constexpr double go1_Calf_max = -0.837; // unit:radian ( = -48 degree) 15 | constexpr double go1_Calf_min = -2.721; // unit:radian ( = -156 degree) 16 | } 17 | 18 | #endif 19 | -------------------------------------------------------------------------------- /unitree_guide/library/unitree_legged_sdk-3.8.0/include/unitree_legged_sdk/joystick.h: -------------------------------------------------------------------------------- 1 | /***************************************************************** 2 | Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rights reserved. 3 | *****************************************************************/ 4 | #ifndef _UNITREE_LEGGED_JOYSTICK_H_ 5 | #define _UNITREE_LEGGED_JOYSTICK_H_ 6 | 7 | #include 8 | // // 16b 9 | // typedef union { 10 | // struct { 11 | // uint8_t R1 :1; 12 | // uint8_t L1 :1; 13 | // uint8_t start :1; 14 | // uint8_t select :1; 15 | // uint8_t R2 :1; 16 | // uint8_t L2 :1; 17 | // uint8_t F1 :1; 18 | // uint8_t F2 :1; 19 | // uint8_t A :1; 20 | // uint8_t B :1; 21 | // uint8_t X :1; 22 | // uint8_t Y :1; 23 | // uint8_t up :1; 24 | // uint8_t right :1; 25 | // uint8_t down :1; 26 | // uint8_t left :1; 27 | // } components; 28 | // uint16_t value; 29 | // } xKeySwitchUnion; 30 | 31 | // // 40 Byte (now used 24B) 32 | // typedef struct { 33 | // uint8_t head[2]; 34 | // xKeySwitchUnion btn; 35 | // float lx; 36 | // float rx; 37 | // float ry; 38 | // float L2; 39 | // float ly; 40 | 41 | // uint8_t idle[16]; 42 | // } xRockerBtnDataStruct; 43 | 44 | #endif // _UNITREE_LEGGED_JOYSTICK_H_ -------------------------------------------------------------------------------- /unitree_guide/library/unitree_legged_sdk-3.8.0/include/unitree_legged_sdk/loop.h: -------------------------------------------------------------------------------- 1 | /***************************************************************** 2 | Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ******************************************************************/ 4 | 5 | #ifndef _UNITREE_LEGGED_LOOP_H_ 6 | #define _UNITREE_LEGGED_LOOP_H_ 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | namespace UNITREE_LEGGED_SDK 18 | { 19 | 20 | constexpr int THREAD_PRIORITY = 95; // real-time priority 21 | 22 | typedef boost::function Callback; 23 | 24 | class Loop { 25 | public: 26 | Loop(std::string name, float period, int bindCPU = -1):_name(name), _period(period), _bindCPU(bindCPU) {} 27 | ~Loop(); 28 | void start(); 29 | void shutdown(); 30 | virtual void functionCB() = 0; 31 | 32 | private: 33 | void entryFunc(); 34 | 35 | std::string _name; 36 | float _period; 37 | int _bindCPU; 38 | bool _bind_cpu_flag = false; 39 | bool _isrunning = false; 40 | std::thread _thread; 41 | }; 42 | 43 | /* 44 | period unit:second 45 | bindCPU change the CPU affinity of this thread 46 | */ 47 | class LoopFunc : public Loop { 48 | public: 49 | LoopFunc(std::string name, float period, const Callback& _cb) 50 | : Loop(name, period), _fp(_cb){} 51 | LoopFunc(std::string name, float period, int bindCPU, const Callback& _cb) 52 | : Loop(name, period, bindCPU), _fp(_cb){} 53 | void functionCB() { (_fp)(); } 54 | private: 55 | boost::function _fp; 56 | }; 57 | 58 | 59 | } 60 | 61 | #endif 62 | -------------------------------------------------------------------------------- /unitree_guide/library/unitree_legged_sdk-3.8.0/include/unitree_legged_sdk/quadruped.h: -------------------------------------------------------------------------------- 1 | /***************************************************************** 2 | Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ******************************************************************/ 4 | 5 | #ifndef _UNITREE_LEGGED_QUADRUPED_H_ 6 | #define _UNITREE_LEGGED_QUADRUPED_H_ 7 | 8 | #include 9 | 10 | using namespace std; 11 | 12 | namespace UNITREE_LEGGED_SDK 13 | { 14 | 15 | enum class LeggedType { 16 | Aliengo, 17 | A1, 18 | Go1, 19 | B1 20 | }; 21 | 22 | 23 | string VersionSDK(); 24 | int InitEnvironment(); // memory lock 25 | 26 | // definition of each leg and joint 27 | constexpr int FR_ = 0; // leg index 28 | constexpr int FL_ = 1; 29 | constexpr int RR_ = 2; 30 | constexpr int RL_ = 3; 31 | 32 | constexpr int FR_0 = 0; // joint index 33 | constexpr int FR_1 = 1; 34 | constexpr int FR_2 = 2; 35 | 36 | constexpr int FL_0 = 3; 37 | constexpr int FL_1 = 4; 38 | constexpr int FL_2 = 5; 39 | 40 | constexpr int RR_0 = 6; 41 | constexpr int RR_1 = 7; 42 | constexpr int RR_2 = 8; 43 | 44 | constexpr int RL_0 = 9; 45 | constexpr int RL_1 = 10; 46 | constexpr int RL_2 = 11; 47 | 48 | } 49 | 50 | #endif 51 | -------------------------------------------------------------------------------- /unitree_guide/library/unitree_legged_sdk-3.8.0/include/unitree_legged_sdk/safety.h: -------------------------------------------------------------------------------- 1 | /***************************************************************** 2 | Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ******************************************************************/ 4 | 5 | #ifndef _UNITREE_LEGGED_SAFETY_H_ 6 | #define _UNITREE_LEGGED_SAFETY_H_ 7 | 8 | #include "comm.h" 9 | #include "quadruped.h" 10 | 11 | namespace UNITREE_LEGGED_SDK 12 | { 13 | 14 | class Safety{ 15 | public: 16 | Safety(LeggedType type); 17 | ~Safety(); 18 | void PositionLimit(LowCmd&); // only effect under Low Level control in Position mode 19 | int PowerProtect(LowCmd&, LowState&, int); /* only effect under Low Level control, input factor: 1~10, 20 | means 10%~100% power limit. If you are new, then use 1; if you are familiar, 21 | then can try bigger number or even comment this function. */ 22 | int PositionProtect(LowCmd&, LowState&, double limit = 0.087); // default limit is 5 degree 23 | private: 24 | int WattLimit, Wcount; // Watt. When limit to 100, you can triger it with 4 hands shaking. 25 | double Hip_max, Hip_min, Thigh_max, Thigh_min, Calf_max, Calf_min; 26 | }; 27 | 28 | 29 | } 30 | 31 | #endif 32 | -------------------------------------------------------------------------------- /unitree_guide/library/unitree_legged_sdk-3.8.0/include/unitree_legged_sdk/udp.h: -------------------------------------------------------------------------------- 1 | /***************************************************************** 2 | Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ******************************************************************/ 4 | 5 | #ifndef _UNITREE_LEGGED_UDP_H_ 6 | #define _UNITREE_LEGGED_UDP_H_ 7 | 8 | #include "comm.h" 9 | #include "unitree_legged_sdk/quadruped.h" 10 | #include 11 | #include 12 | 13 | /* 14 | UDP critical configuration: 15 | 16 | 1. initiativeDisconnect: if need disconnection after connected, another ip/port can access after disconnection 17 | 18 | /--- block will block till data come 19 | 2. recvType ---- block + timeout will block till data come or timeout 20 | \--- non block if no data will return immediately 21 | 22 | /--- Y ip/port will be set later 23 | 3. setIpPort: 24 | \--- N ip/port not specified, as a server wait for connect 25 | */ 26 | 27 | 28 | namespace UNITREE_LEGGED_SDK 29 | { 30 | 31 | constexpr int UDP_CLIENT_PORT = 8080; // local port 32 | constexpr int UDP_SERVER_PORT = 8007; // target port 33 | constexpr char UDP_SERVER_IP_BASIC[] = "192.168.123.10"; // target IP address 34 | constexpr char UDP_SERVER_IP_SPORT[] = "192.168.123.161"; // target IP address 35 | 36 | typedef enum { 37 | nonBlock = 0x00, 38 | block = 0x01, 39 | blockTimeout = 0x02, 40 | } RecvEnum; 41 | 42 | // Notice: User defined data(like struct) should add crc(4Byte) at the end. 43 | class UDP { 44 | public: 45 | UDP(uint8_t level, uint16_t localPort, const char* targetIP, uint16_t targetPort); // udp use dafault length according to level 46 | UDP(uint16_t localPort, const char* targetIP, uint16_t targetPort, 47 | int sendLength, int recvLength, bool initiativeDisconnect = false, RecvEnum recvType = RecvEnum::nonBlock); 48 | UDP(uint16_t localPort, 49 | int sendLength, int recvLength, bool initiativeDisconnect = false, RecvEnum recvType = RecvEnum::nonBlock, bool setIpPort = false); 50 | ~UDP(); 51 | 52 | void SetIpPort(const char* targetIP, uint16_t targetPort); // if not indicated at constructor function 53 | void SetRecvTimeout(int time); // use in RecvEnum::blockTimeout (unit: ms) 54 | 55 | void SetDisconnectTime(float callback_dt, float disconnectTime); // initiativeDisconnect = true, disconnect for another IP to connect 56 | void SetAccessibleTime(float callback_dt, float accessibleTime); // check if can access data 57 | 58 | int Send(); 59 | int Recv(); // directly save in buffer 60 | 61 | void InitCmdData(HighCmd& cmd); 62 | void InitCmdData(LowCmd& cmd); 63 | int SetSend(char*); 64 | int SetSend(HighCmd&); 65 | int SetSend(LowCmd&); 66 | void GetRecv(char*); 67 | void GetRecv(HighState&); 68 | void GetRecv(LowState&); 69 | 70 | UDPState udpState; 71 | char* targetIP; 72 | uint16_t targetPort; 73 | char* localIP; 74 | uint16_t localPort; 75 | bool accessible = false; // can access or not 76 | 77 | private: 78 | void init(uint16_t localPort, const char* targetIP = NULL, uint16_t targetPort = 0); 79 | 80 | int sockFd; 81 | bool connected; // udp works with connect() function, rather than server mode 82 | int sendLength; 83 | int recvLength; 84 | int lose_recv; 85 | 86 | char* recvBuf; 87 | char* recvAvaliable; 88 | char* sendBuf; 89 | pthread_mutex_t sendMutex; 90 | pthread_mutex_t recvMutex; 91 | pthread_mutex_t udpMutex; 92 | 93 | bool nonblock = true; 94 | int blockTimeout = -1; // use time out method or not, (unit: ms) 95 | bool initiativeDisconnect = false; // 96 | 97 | }; 98 | 99 | } 100 | 101 | #endif 102 | -------------------------------------------------------------------------------- /unitree_guide/library/unitree_legged_sdk-3.8.0/include/unitree_legged_sdk/unitree_legged_sdk.h: -------------------------------------------------------------------------------- 1 | /***************************************************************** 2 | Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ******************************************************************/ 4 | 5 | #ifndef _UNITREE_LEGGED_SDK_H_ 6 | #define _UNITREE_LEGGED_SDK_H_ 7 | 8 | #include "comm.h" 9 | #include "safety.h" 10 | #include "udp.h" 11 | #include "loop.h" 12 | #include "quadruped.h" 13 | #include "joystick.h" 14 | #include 15 | 16 | #define UT UNITREE_LEGGED_SDK //short name 17 | 18 | #endif 19 | -------------------------------------------------------------------------------- /unitree_guide/library/unitree_legged_sdk-3.8.0/lib/cpp/amd64/libunitree_legged_sdk.a: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/unitreerobotics/unitree_guide/fdf4d23de6affe8ee38fb4d892f61053fa1fcbcb/unitree_guide/library/unitree_legged_sdk-3.8.0/lib/cpp/amd64/libunitree_legged_sdk.a -------------------------------------------------------------------------------- /unitree_guide/library/unitree_legged_sdk-3.8.0/lib/cpp/arm64/libunitree_legged_sdk.a: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/unitreerobotics/unitree_guide/fdf4d23de6affe8ee38fb4d892f61053fa1fcbcb/unitree_guide/library/unitree_legged_sdk-3.8.0/lib/cpp/arm64/libunitree_legged_sdk.a -------------------------------------------------------------------------------- /unitree_guide/library/unitree_legged_sdk-3.8.0/lib/python/amd64/robot_interface.cpython-38-x86_64-linux-gnu.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/unitreerobotics/unitree_guide/fdf4d23de6affe8ee38fb4d892f61053fa1fcbcb/unitree_guide/library/unitree_legged_sdk-3.8.0/lib/python/amd64/robot_interface.cpython-38-x86_64-linux-gnu.so -------------------------------------------------------------------------------- /unitree_guide/library/unitree_legged_sdk-3.8.0/lib/python/arm64/robot_interface.cpython-37m-aarch64-linux-gnu.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/unitreerobotics/unitree_guide/fdf4d23de6affe8ee38fb4d892f61053fa1fcbcb/unitree_guide/library/unitree_legged_sdk-3.8.0/lib/python/arm64/robot_interface.cpython-37m-aarch64-linux-gnu.so -------------------------------------------------------------------------------- /unitree_guide/library/unitree_legged_sdk-3.8.0/lib/python/arm64/robot_interface.cpython-38-aarch64-linux-gnu.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/unitreerobotics/unitree_guide/fdf4d23de6affe8ee38fb4d892f61053fa1fcbcb/unitree_guide/library/unitree_legged_sdk-3.8.0/lib/python/arm64/robot_interface.cpython-38-aarch64-linux-gnu.so -------------------------------------------------------------------------------- /unitree_guide/library/unitree_legged_sdk_3.2/include/unitree_legged_sdk/a1_const.h: -------------------------------------------------------------------------------- 1 | /************************************************************************ 2 | Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rights reserved. 3 | Use of this source code is governed by the MPL-2.0 license, see LICENSE. 4 | ************************************************************************/ 5 | 6 | #ifndef _UNITREE_LEGGED_A1_H_ 7 | #define _UNITREE_LEGGED_A1_H_ 8 | 9 | namespace UNITREE_LEGGED_SDK 10 | { 11 | constexpr double a1_Hip_max = 0.802; // unit:radian ( = 46 degree) 12 | constexpr double a1_Hip_min = -0.802; // unit:radian ( = -46 degree) 13 | constexpr double a1_Thigh_max = 4.19; // unit:radian ( = 240 degree) 14 | constexpr double a1_Thigh_min = -1.05; // unit:radian ( = -60 degree) 15 | constexpr double a1_Calf_max = -0.916; // unit:radian ( = -52.5 degree) 16 | constexpr double a1_Calf_min = -2.7; // unit:radian ( = -154.5 degree) 17 | } 18 | 19 | #endif 20 | -------------------------------------------------------------------------------- /unitree_guide/library/unitree_legged_sdk_3.2/include/unitree_legged_sdk/aliengo_const.h: -------------------------------------------------------------------------------- 1 | /************************************************************************ 2 | Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rights reserved. 3 | Use of this source code is governed by the MPL-2.0 license, see LICENSE. 4 | ************************************************************************/ 5 | 6 | #ifndef _UNITREE_LEGGED_ALIENGO_H_ 7 | #define _UNITREE_LEGGED_ALIENGO_H_ 8 | 9 | namespace UNITREE_LEGGED_SDK 10 | { 11 | constexpr double aliengo_Hip_max = 1.047; // unit:radian ( = 60 degree) 12 | constexpr double aliengo_Hip_min = -0.873; // unit:radian ( = -50 degree) 13 | constexpr double aliengo_Thigh_max = 3.927; // unit:radian ( = 225 degree) 14 | constexpr double aliengo_Thigh_min = -0.524; // unit:radian ( = -30 degree) 15 | constexpr double aliengo_Calf_max = -0.611; // unit:radian ( = -35 degree) 16 | constexpr double aliengo_Calf_min = -2.775; // unit:radian ( = -159 degree) 17 | } 18 | 19 | #endif -------------------------------------------------------------------------------- /unitree_guide/library/unitree_legged_sdk_3.2/include/unitree_legged_sdk/lcm.h: -------------------------------------------------------------------------------- 1 | /************************************************************************ 2 | Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rights reserved. 3 | Use of this source code is governed by the MPL-2.0 license, see LICENSE. 4 | ************************************************************************/ 5 | 6 | #ifndef _UNITREE_LEGGED_LCM_H_ 7 | #define _UNITREE_LEGGED_LCM_H_ 8 | 9 | #include "comm.h" 10 | #include 11 | #include 12 | 13 | namespace UNITREE_LEGGED_SDK 14 | { 15 | 16 | constexpr char highCmdChannel[] = "LCM_High_Cmd"; 17 | constexpr char highStateChannel[] = "LCM_High_State"; 18 | constexpr char lowCmdChannel[] = "LCM_Low_Cmd"; 19 | constexpr char lowStateChannel[] = "LCM_Low_State"; 20 | 21 | template 22 | class LCMHandler 23 | { 24 | public: 25 | LCMHandler(){ 26 | pthread_mutex_init(&countMut, NULL); 27 | pthread_mutex_init(&recvMut, NULL); 28 | } 29 | 30 | void onMsg(const lcm::ReceiveBuffer* rbuf, const std::string& channel){ 31 | isrunning = true; 32 | 33 | pthread_mutex_lock(&countMut); 34 | counter = 0; 35 | pthread_mutex_unlock(&countMut); 36 | 37 | T *msg = NULL; 38 | msg = (T *)(rbuf->data); 39 | pthread_mutex_lock(&recvMut); 40 | // sourceBuf = *msg; 41 | memcpy(&sourceBuf, msg, sizeof(T)); 42 | pthread_mutex_unlock(&recvMut); 43 | } 44 | 45 | bool isrunning = false; 46 | T sourceBuf = {0}; 47 | pthread_mutex_t countMut; 48 | pthread_mutex_t recvMut; 49 | int counter = 0; 50 | }; 51 | 52 | class LCM { 53 | public: 54 | LCM(uint8_t level); 55 | ~LCM(); 56 | void SubscribeCmd(); 57 | void SubscribeState(); // remember to call this when change control level 58 | int Send(HighCmd&); // lcm send cmd 59 | int Send(LowCmd&); // lcm send cmd 60 | int Send(HighState&); // lcm send state 61 | int Send(LowState&); // lcm send state 62 | int Recv(); // directly save in buffer 63 | void Get(HighCmd&); 64 | void Get(LowCmd&); 65 | void Get(HighState&); 66 | void Get(LowState&); 67 | 68 | LCMHandler highStateLCMHandler; 69 | LCMHandler lowStateLCMHandler; 70 | LCMHandler highCmdLCMHandler; 71 | LCMHandler lowCmdLCMHandler; 72 | private: 73 | uint8_t levelFlag = HIGHLEVEL; // default: high level 74 | lcm::LCM lcm; 75 | lcm::Subscription* subLcm; 76 | int lcmFd; 77 | int LCM_PERIOD = 2000; //default 2ms 78 | }; 79 | 80 | } 81 | 82 | #endif 83 | -------------------------------------------------------------------------------- /unitree_guide/library/unitree_legged_sdk_3.2/include/unitree_legged_sdk/lcm_server.h: -------------------------------------------------------------------------------- 1 | /************************************************************************ 2 | Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rights reserved. 3 | Use of this source code is governed by the MPL-2.0 license, see LICENSE. 4 | ************************************************************************/ 5 | 6 | #ifndef _UNITREE_LEGGED_LCM_SERVER_ 7 | #define _UNITREE_LEGGED_LCM_SERVER_ 8 | 9 | #include "comm.h" 10 | #include "unitree_legged_sdk/unitree_legged_sdk.h" 11 | 12 | namespace UNITREE_LEGGED_SDK 13 | { 14 | // Low command Lcm Server 15 | class Lcm_Server_Low 16 | { 17 | public: 18 | Lcm_Server_Low(LeggedType rname) : udp(LOWLEVEL), mylcm(LOWLEVEL){ 19 | udp.InitCmdData(cmd); 20 | } 21 | void UDPRecv(){ 22 | udp.Recv(); 23 | } 24 | void UDPSend(){ 25 | udp.Send(); 26 | } 27 | void LCMRecv(); 28 | void RobotControl(); 29 | 30 | UDP udp; 31 | LCM mylcm; 32 | LowCmd cmd = {0}; 33 | LowState state = {0}; 34 | }; 35 | void Lcm_Server_Low::LCMRecv() 36 | { 37 | if(mylcm.lowCmdLCMHandler.isrunning){ 38 | pthread_mutex_lock(&mylcm.lowCmdLCMHandler.countMut); 39 | mylcm.lowCmdLCMHandler.counter++; 40 | if(mylcm.lowCmdLCMHandler.counter > 10){ 41 | printf("Error! LCM Time out.\n"); 42 | exit(-1); // can be commented out 43 | } 44 | pthread_mutex_unlock(&mylcm.lowCmdLCMHandler.countMut); 45 | } 46 | mylcm.Recv(); 47 | } 48 | void Lcm_Server_Low::RobotControl() 49 | { 50 | udp.GetRecv(state); 51 | mylcm.Send(state); 52 | mylcm.Get(cmd); 53 | udp.SetSend(cmd); 54 | } 55 | 56 | 57 | 58 | // High command Lcm Server 59 | class Lcm_Server_High 60 | { 61 | public: 62 | Lcm_Server_High(LeggedType rname): udp(HIGHLEVEL), mylcm(HIGHLEVEL){ 63 | udp.InitCmdData(cmd); 64 | } 65 | void UDPRecv(){ 66 | udp.Recv(); 67 | } 68 | void UDPSend(){ 69 | udp.Send(); 70 | } 71 | void LCMRecv(); 72 | void RobotControl(); 73 | 74 | UDP udp; 75 | LCM mylcm; 76 | HighCmd cmd = {0}; 77 | HighState state = {0}; 78 | }; 79 | 80 | void Lcm_Server_High::LCMRecv() 81 | { 82 | if(mylcm.highCmdLCMHandler.isrunning){ 83 | pthread_mutex_lock(&mylcm.highCmdLCMHandler.countMut); 84 | mylcm.highCmdLCMHandler.counter++; 85 | if(mylcm.highCmdLCMHandler.counter > 10){ 86 | printf("Error! LCM Time out.\n"); 87 | exit(-1); // can be commented out 88 | } 89 | pthread_mutex_unlock(&mylcm.highCmdLCMHandler.countMut); 90 | } 91 | mylcm.Recv(); 92 | } 93 | 94 | void Lcm_Server_High::RobotControl() 95 | { 96 | udp.GetRecv(state); 97 | mylcm.Send(state); 98 | mylcm.Get(cmd); 99 | udp.SetSend(cmd); 100 | } 101 | 102 | 103 | 104 | 105 | } 106 | #endif -------------------------------------------------------------------------------- /unitree_guide/library/unitree_legged_sdk_3.2/include/unitree_legged_sdk/loop.h: -------------------------------------------------------------------------------- 1 | /************************************************************************ 2 | Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rights reserved. 3 | Use of this source code is governed by the MPL-2.0 license, see LICENSE. 4 | ************************************************************************/ 5 | 6 | #ifndef _UNITREE_LEGGED_LOOP_H_ 7 | #define _UNITREE_LEGGED_LOOP_H_ 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | namespace UNITREE_LEGGED_SDK 19 | { 20 | 21 | constexpr int THREAD_PRIORITY = 99; // real-time priority 22 | 23 | typedef boost::function Callback; 24 | 25 | class Loop { 26 | public: 27 | Loop(std::string name, float period, int bindCPU = -1):_name(name), _period(period), _bindCPU(bindCPU) {} 28 | ~Loop(); 29 | void start(); 30 | void shutdown(); 31 | virtual void functionCB() = 0; 32 | 33 | private: 34 | void entryFunc(); 35 | 36 | std::string _name; 37 | float _period; 38 | int _bindCPU; 39 | bool _bind_cpu_flag = false; 40 | bool _isrunning = false; 41 | std::thread _thread; 42 | }; 43 | 44 | class LoopFunc : public Loop { 45 | public: 46 | LoopFunc(std::string name, float period, const Callback& _cb) 47 | : Loop(name, period), _fp(_cb){} 48 | LoopFunc(std::string name, float period, int bindCPU, const Callback& _cb) 49 | : Loop(name, period, bindCPU), _fp(_cb){} 50 | void functionCB() { (_fp)(); } 51 | private: 52 | boost::function _fp; 53 | }; 54 | 55 | 56 | } 57 | 58 | #endif 59 | -------------------------------------------------------------------------------- /unitree_guide/library/unitree_legged_sdk_3.2/include/unitree_legged_sdk/quadruped.h: -------------------------------------------------------------------------------- 1 | /************************************************************************ 2 | Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rights reserved. 3 | Use of this source code is governed by the MPL-2.0 license, see LICENSE. 4 | ************************************************************************/ 5 | 6 | #ifndef _UNITREE_LEGGED_QUADRUPED_H_ 7 | #define _UNITREE_LEGGED_QUADRUPED_H_ 8 | 9 | namespace UNITREE_LEGGED_SDK 10 | { 11 | 12 | enum class LeggedType { 13 | Aliengo, 14 | A1 15 | }; 16 | 17 | enum class HighLevelType { 18 | Basic, 19 | Sport 20 | }; 21 | 22 | void InitEnvironment(); // memory lock 23 | 24 | // definition of each leg and joint 25 | constexpr int FR_ = 0; // leg index 26 | constexpr int FL_ = 1; 27 | constexpr int RR_ = 2; 28 | constexpr int RL_ = 3; 29 | 30 | constexpr int FR_0 = 0; // joint index 31 | constexpr int FR_1 = 1; 32 | constexpr int FR_2 = 2; 33 | 34 | constexpr int FL_0 = 3; 35 | constexpr int FL_1 = 4; 36 | constexpr int FL_2 = 5; 37 | 38 | constexpr int RR_0 = 6; 39 | constexpr int RR_1 = 7; 40 | constexpr int RR_2 = 8; 41 | 42 | constexpr int RL_0 = 9; 43 | constexpr int RL_1 = 10; 44 | constexpr int RL_2 = 11; 45 | 46 | } 47 | 48 | #endif 49 | -------------------------------------------------------------------------------- /unitree_guide/library/unitree_legged_sdk_3.2/include/unitree_legged_sdk/safety.h: -------------------------------------------------------------------------------- 1 | /************************************************************************ 2 | Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rights reserved. 3 | Use of this source code is governed by the MPL-2.0 license, see LICENSE. 4 | ************************************************************************/ 5 | 6 | #ifndef _UNITREE_LEGGED_SAFETY_H_ 7 | #define _UNITREE_LEGGED_SAFETY_H_ 8 | 9 | #include "comm.h" 10 | #include "quadruped.h" 11 | 12 | namespace UNITREE_LEGGED_SDK 13 | { 14 | 15 | class Safety{ 16 | public: 17 | Safety(LeggedType type); 18 | ~Safety(); 19 | void PositionLimit(LowCmd&); // only effect under Low Level control in Position mode 20 | void PowerProtect(LowCmd&, LowState&, int); /* only effect under Low Level control, input factor: 1~10, 21 | means 10%~100% power limit. If you are new, then use 1; if you are familiar, 22 | then can try bigger number or even comment this function. */ 23 | void PositionProtect(LowCmd&, LowState&, double limit = 0.087); // default limit is 5 degree 24 | private: 25 | int WattLimit, Wcount; // Watt. When limit to 100, you can triger it with 4 hands shaking. 26 | double Hip_max, Hip_min, Thigh_max, Thigh_min, Calf_max, Calf_min; 27 | }; 28 | 29 | 30 | } 31 | 32 | #endif 33 | -------------------------------------------------------------------------------- /unitree_guide/library/unitree_legged_sdk_3.2/include/unitree_legged_sdk/udp.h: -------------------------------------------------------------------------------- 1 | /************************************************************************ 2 | Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rights reserved. 3 | Use of this source code is governed by the MPL-2.0 license, see LICENSE. 4 | ************************************************************************/ 5 | 6 | #ifndef _UNITREE_LEGGED_UDP_H_ 7 | #define _UNITREE_LEGGED_UDP_H_ 8 | 9 | #include "comm.h" 10 | #include "unitree_legged_sdk/quadruped.h" 11 | #include 12 | 13 | namespace UNITREE_LEGGED_SDK 14 | { 15 | 16 | constexpr int UDP_CLIENT_PORT = 8080; // local port 17 | constexpr int UDP_SERVER_PORT = 8007; // target port 18 | constexpr char UDP_SERVER_IP_BASIC[] = "192.168.123.10"; // target IP address 19 | constexpr char UDP_SERVER_IP_SPORT[] = "192.168.123.161"; // target IP address 20 | 21 | // Notice: User defined data(like struct) should add crc(4Byte) at the end. 22 | class UDP { 23 | public: 24 | UDP(uint8_t level, HighLevelType highControl = HighLevelType::Basic); // unitree dafault IP and Port 25 | UDP(uint16_t localPort, const char* targetIP, uint16_t targetPort, int sendLength, int recvLength); 26 | UDP(uint16_t localPort, uint16_t targetPort, int sendLength, int recvLength); // as server, client IP can change 27 | ~UDP(); 28 | void InitCmdData(HighCmd& cmd); 29 | void InitCmdData(LowCmd& cmd); 30 | void switchLevel(int level); 31 | 32 | int SetSend(HighCmd&); 33 | int SetSend(LowCmd&); 34 | int SetSend(char* cmd); 35 | void GetRecv(HighState&); 36 | void GetRecv(LowState&); 37 | void GetRecv(char*); 38 | int Send(); 39 | int Recv(); // directly save in buffer 40 | 41 | UDPState udpState; 42 | char* targetIP; 43 | uint16_t targetPort; 44 | char* localIP; 45 | uint16_t localPort; 46 | private: 47 | void init(uint16_t localPort, const char* targetIP, uint16_t targetPort); 48 | 49 | uint8_t levelFlag = HIGHLEVEL; // default: high level 50 | int sockFd; 51 | bool connected; // udp only works when connected 52 | int sendLength; 53 | int recvLength; 54 | char* recvTemp; 55 | char* recvBuf; 56 | char* sendBuf; 57 | int lose_recv; 58 | pthread_mutex_t sendMut; 59 | pthread_mutex_t recvMut; 60 | }; 61 | 62 | } 63 | 64 | #endif 65 | -------------------------------------------------------------------------------- /unitree_guide/library/unitree_legged_sdk_3.2/include/unitree_legged_sdk/unitree_joystick.h: -------------------------------------------------------------------------------- 1 | /***************************************************************** 2 | Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rights reserved. 3 | *****************************************************************/ 4 | #ifndef UNITREE_JOYSTICK_H 5 | #define UNITREE_JOYSTICK_H 6 | 7 | #include 8 | // // 16b 9 | // typedef union { 10 | // struct { 11 | // uint8_t R1 :1; 12 | // uint8_t L1 :1; 13 | // uint8_t start :1; 14 | // uint8_t select :1; 15 | // uint8_t R2 :1; 16 | // uint8_t L2 :1; 17 | // uint8_t F1 :1; 18 | // uint8_t F2 :1; 19 | // uint8_t A :1; 20 | // uint8_t B :1; 21 | // uint8_t X :1; 22 | // uint8_t Y :1; 23 | // uint8_t up :1; 24 | // uint8_t right :1; 25 | // uint8_t down :1; 26 | // uint8_t left :1; 27 | // } components; 28 | // uint16_t value; 29 | // } xKeySwitchUnion; 30 | 31 | // // 40 Byte (now used 24B) 32 | // typedef struct { 33 | // uint8_t head[2]; 34 | // xKeySwitchUnion btn; 35 | // float lx; 36 | // float rx; 37 | // float ry; 38 | // float L2; 39 | // float ly; 40 | 41 | // uint8_t idle[16]; 42 | // } xRockerBtnDataStruct; 43 | 44 | #endif // UNITREE_JOYSTICK_H -------------------------------------------------------------------------------- /unitree_guide/library/unitree_legged_sdk_3.2/include/unitree_legged_sdk/unitree_legged_sdk.h: -------------------------------------------------------------------------------- 1 | /************************************************************************ 2 | Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rights reserved. 3 | Use of this source code is governed by the MPL-2.0 license, see LICENSE. 4 | ************************************************************************/ 5 | 6 | #ifndef _UNITREE_LEGGED_SDK_H_ 7 | #define _UNITREE_LEGGED_SDK_H_ 8 | 9 | #include "comm.h" 10 | #include "safety.h" 11 | #include "udp.h" 12 | #include "loop.h" 13 | #include "lcm.h" 14 | #include "quadruped.h" 15 | #include 16 | 17 | #define UT UNITREE_LEGGED_SDK //short name 18 | 19 | #endif 20 | -------------------------------------------------------------------------------- /unitree_guide/library/unitree_legged_sdk_3.2/lib/libunitree_legged_sdk_amd64.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/unitreerobotics/unitree_guide/fdf4d23de6affe8ee38fb4d892f61053fa1fcbcb/unitree_guide/library/unitree_legged_sdk_3.2/lib/libunitree_legged_sdk_amd64.so -------------------------------------------------------------------------------- /unitree_guide/library/unitree_legged_sdk_3.2/lib/libunitree_legged_sdk_arm32.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/unitreerobotics/unitree_guide/fdf4d23de6affe8ee38fb4d892f61053fa1fcbcb/unitree_guide/library/unitree_legged_sdk_3.2/lib/libunitree_legged_sdk_arm32.so -------------------------------------------------------------------------------- /unitree_guide/library/unitree_legged_sdk_3.2/lib/libunitree_legged_sdk_arm64.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/unitreerobotics/unitree_guide/fdf4d23de6affe8ee38fb4d892f61053fa1fcbcb/unitree_guide/library/unitree_legged_sdk_3.2/lib/libunitree_legged_sdk_arm64.so -------------------------------------------------------------------------------- /unitree_guide/library/unitree_legged_sdk_3.4/include/unitree_legged_sdk/a1_const.h: -------------------------------------------------------------------------------- 1 | /***************************************************************** 2 | Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ******************************************************************/ 4 | 5 | #ifndef _UNITREE_LEGGED_A1_H_ 6 | #define _UNITREE_LEGGED_A1_H_ 7 | 8 | namespace UNITREE_LEGGED_SDK 9 | { 10 | constexpr double a1_Hip_max = 0.802; // unit:radian ( = 46 degree) 11 | constexpr double a1_Hip_min = -0.802; // unit:radian ( = -46 degree) 12 | constexpr double a1_Thigh_max = 4.19; // unit:radian ( = 240 degree) 13 | constexpr double a1_Thigh_min = -1.05; // unit:radian ( = -60 degree) 14 | constexpr double a1_Calf_max = -0.916; // unit:radian ( = -52.5 degree) 15 | constexpr double a1_Calf_min = -2.7; // unit:radian ( = -154.5 degree) 16 | } 17 | 18 | #endif -------------------------------------------------------------------------------- /unitree_guide/library/unitree_legged_sdk_3.4/include/unitree_legged_sdk/aliengo_const.h: -------------------------------------------------------------------------------- 1 | /***************************************************************** 2 | Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ******************************************************************/ 4 | 5 | #ifndef _UNITREE_LEGGED_ALIENGO_H_ 6 | #define _UNITREE_LEGGED_ALIENGO_H_ 7 | 8 | namespace UNITREE_LEGGED_SDK 9 | { 10 | constexpr double aliengo_Hip_max = 1.047; // unit:radian ( = 60 degree) 11 | constexpr double aliengo_Hip_min = -0.873; // unit:radian ( = -50 degree) 12 | constexpr double aliengo_Thigh_max = 3.927; // unit:radian ( = 225 degree) 13 | constexpr double aliengo_Thigh_min = -0.524; // unit:radian ( = -30 degree) 14 | constexpr double aliengo_Calf_max = -0.611; // unit:radian ( = -35 degree) 15 | constexpr double aliengo_Calf_min = -2.775; // unit:radian ( = -159 degree) 16 | } 17 | 18 | #endif -------------------------------------------------------------------------------- /unitree_guide/library/unitree_legged_sdk_3.4/include/unitree_legged_sdk/lcm.h: -------------------------------------------------------------------------------- 1 | /***************************************************************** 2 | Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ******************************************************************/ 4 | 5 | #ifndef _UNITREE_LEGGED_LCM_H_ 6 | #define _UNITREE_LEGGED_LCM_H_ 7 | 8 | #include "comm.h" 9 | #include 10 | #include 11 | 12 | namespace UNITREE_LEGGED_SDK 13 | { 14 | 15 | constexpr char highCmdChannel[] = "LCM_High_Cmd"; 16 | constexpr char highStateChannel[] = "LCM_High_State"; 17 | constexpr char lowCmdChannel[] = "LCM_Low_Cmd"; 18 | constexpr char lowStateChannel[] = "LCM_Low_State"; 19 | 20 | template 21 | class LCMHandler 22 | { 23 | public: 24 | LCMHandler(){ 25 | pthread_mutex_init(&countMut, NULL); 26 | pthread_mutex_init(&recvMut, NULL); 27 | } 28 | 29 | void onMsg(const lcm::ReceiveBuffer* rbuf, const std::string& channel){ 30 | isrunning = true; 31 | 32 | pthread_mutex_lock(&countMut); 33 | counter = 0; 34 | pthread_mutex_unlock(&countMut); 35 | 36 | T *msg = NULL; 37 | msg = (T *)(rbuf->data); 38 | pthread_mutex_lock(&recvMut); 39 | // sourceBuf = *msg; 40 | memcpy(&sourceBuf, msg, sizeof(T)); 41 | pthread_mutex_unlock(&recvMut); 42 | } 43 | 44 | bool isrunning = false; 45 | T sourceBuf = {0}; 46 | pthread_mutex_t countMut; 47 | pthread_mutex_t recvMut; 48 | int counter = 0; 49 | }; 50 | 51 | class LCM { 52 | public: 53 | LCM(uint8_t level); 54 | ~LCM(); 55 | void SubscribeCmd(); 56 | void SubscribeState(); // remember to call this when change control level 57 | int Send(HighCmd&); // lcm send cmd 58 | int Send(LowCmd&); // lcm send cmd 59 | int Send(HighState&); // lcm send state 60 | int Send(LowState&); // lcm send state 61 | int Recv(); // directly save in buffer 62 | void Get(HighCmd&); 63 | void Get(LowCmd&); 64 | void Get(HighState&); 65 | void Get(LowState&); 66 | 67 | LCMHandler highStateLCMHandler; 68 | LCMHandler lowStateLCMHandler; 69 | LCMHandler highCmdLCMHandler; 70 | LCMHandler lowCmdLCMHandler; 71 | private: 72 | uint8_t levelFlag = HIGHLEVEL; // default: high level 73 | lcm::LCM lcm; 74 | lcm::Subscription* subLcm; 75 | int lcmFd; 76 | int LCM_PERIOD = 2000; //default 2ms 77 | }; 78 | 79 | } 80 | 81 | #endif 82 | -------------------------------------------------------------------------------- /unitree_guide/library/unitree_legged_sdk_3.4/include/unitree_legged_sdk/loop.h: -------------------------------------------------------------------------------- 1 | /***************************************************************** 2 | Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ******************************************************************/ 4 | 5 | #ifndef _UNITREE_LEGGED_LOOP_H_ 6 | #define _UNITREE_LEGGED_LOOP_H_ 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | namespace UNITREE_LEGGED_SDK 18 | { 19 | 20 | constexpr int THREAD_PRIORITY = 99; // real-time priority 21 | 22 | typedef boost::function Callback; 23 | 24 | class Loop { 25 | public: 26 | Loop(std::string name, float period, int bindCPU = -1):_name(name), _period(period), _bindCPU(bindCPU) {} 27 | ~Loop(); 28 | void start(); 29 | void shutdown(); 30 | virtual void functionCB() = 0; 31 | 32 | private: 33 | void entryFunc(); 34 | 35 | std::string _name; 36 | float _period; 37 | int _bindCPU; 38 | bool _bind_cpu_flag = false; 39 | bool _isrunning = false; 40 | std::thread _thread; 41 | }; 42 | 43 | class LoopFunc : public Loop { 44 | public: 45 | LoopFunc(std::string name, float period, const Callback& _cb) 46 | : Loop(name, period), _fp(_cb){} 47 | LoopFunc(std::string name, float period, int bindCPU, const Callback& _cb) 48 | : Loop(name, period, bindCPU), _fp(_cb){} 49 | void functionCB() { (_fp)(); } 50 | private: 51 | boost::function _fp; 52 | }; 53 | 54 | 55 | } 56 | 57 | #endif 58 | -------------------------------------------------------------------------------- /unitree_guide/library/unitree_legged_sdk_3.4/include/unitree_legged_sdk/quadruped.h: -------------------------------------------------------------------------------- 1 | /***************************************************************** 2 | Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ******************************************************************/ 4 | 5 | #ifndef _UNITREE_LEGGED_QUADRUPED_H_ 6 | #define _UNITREE_LEGGED_QUADRUPED_H_ 7 | 8 | #include 9 | 10 | using namespace std; 11 | 12 | namespace UNITREE_LEGGED_SDK 13 | { 14 | 15 | enum class LeggedType { 16 | Aliengo, 17 | A1, 18 | Go1 19 | }; 20 | 21 | enum class HighLevelType { 22 | Basic, 23 | Sport 24 | }; 25 | 26 | string VersionSDK(); 27 | int InitEnvironment(); 28 | 29 | 30 | // definition of each leg and joint 31 | constexpr int FR_ = 0; // leg index 32 | constexpr int FL_ = 1; 33 | constexpr int RR_ = 2; 34 | constexpr int RL_ = 3; 35 | 36 | constexpr int FR_0 = 0; // joint index 37 | constexpr int FR_1 = 1; 38 | constexpr int FR_2 = 2; 39 | 40 | constexpr int FL_0 = 3; 41 | constexpr int FL_1 = 4; 42 | constexpr int FL_2 = 5; 43 | 44 | constexpr int RR_0 = 6; 45 | constexpr int RR_1 = 7; 46 | constexpr int RR_2 = 8; 47 | 48 | constexpr int RL_0 = 9; 49 | constexpr int RL_1 = 10; 50 | constexpr int RL_2 = 11; 51 | 52 | } 53 | 54 | #endif 55 | -------------------------------------------------------------------------------- /unitree_guide/library/unitree_legged_sdk_3.4/include/unitree_legged_sdk/safety.h: -------------------------------------------------------------------------------- 1 | /***************************************************************** 2 | Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ******************************************************************/ 4 | 5 | #ifndef _UNITREE_LEGGED_SAFETY_H_ 6 | #define _UNITREE_LEGGED_SAFETY_H_ 7 | 8 | #include "comm.h" 9 | #include "quadruped.h" 10 | 11 | namespace UNITREE_LEGGED_SDK 12 | { 13 | 14 | class Safety{ 15 | public: 16 | Safety(LeggedType type); 17 | ~Safety(); 18 | void PositionLimit(LowCmd&); // only effect under Low Level control in Position mode 19 | int PowerProtect(LowCmd&, LowState&, int); /* only effect under Low Level control, input factor: 1~10, 20 | means 10%~100% power limit. If you are new, then use 1; if you are familiar, 21 | then can try bigger number or even comment this function. */ 22 | int PositionProtect(LowCmd&, LowState&, double limit = 0.087); // default limit is 5 degree 23 | private: 24 | int WattLimit, Wcount; // Watt. When limit to 100, you can triger it with 4 hands shaking. 25 | double Hip_max, Hip_min, Thigh_max, Thigh_min, Calf_max, Calf_min; 26 | }; 27 | 28 | 29 | } 30 | 31 | #endif 32 | -------------------------------------------------------------------------------- /unitree_guide/library/unitree_legged_sdk_3.4/include/unitree_legged_sdk/unitree_legged_sdk.h: -------------------------------------------------------------------------------- 1 | /***************************************************************** 2 | Copyright (c) 2020, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ******************************************************************/ 4 | 5 | #ifndef _UNITREE_LEGGED_SDK_H_ 6 | #define _UNITREE_LEGGED_SDK_H_ 7 | 8 | #include "comm.h" 9 | #include "safety.h" 10 | #include "udp.h" 11 | #include "loop.h" 12 | #include "lcm.h" 13 | #include "quadruped.h" 14 | #include 15 | 16 | #define UT UNITREE_LEGGED_SDK //short name 17 | 18 | #endif 19 | -------------------------------------------------------------------------------- /unitree_guide/library/unitree_legged_sdk_3.4/lib/libunitree_legged_sdk_arm64.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/unitreerobotics/unitree_guide/fdf4d23de6affe8ee38fb4d892f61053fa1fcbcb/unitree_guide/library/unitree_legged_sdk_3.4/lib/libunitree_legged_sdk_arm64.so -------------------------------------------------------------------------------- /unitree_guide/library/unitree_legged_sdk_3.4/lib/libunitree_legged_sdk_x86.so: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/unitreerobotics/unitree_guide/fdf4d23de6affe8ee38fb4d892f61053fa1fcbcb/unitree_guide/library/unitree_legged_sdk_3.4/lib/libunitree_legged_sdk_x86.so -------------------------------------------------------------------------------- /unitree_guide/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | unitree_guide 4 | 0.0.0 5 | The unitree_guide is an open source project for controlling the quadruped robot of Unitree Robotics, and it is also the software project accompanying 《四足机器人控制算法--建模、控制与实践》 published by Unitree Robotics. 6 | 7 | unitree 8 | TODO 9 | 10 | catkin 11 | genmsg 12 | controller_manager 13 | joint_state_controller 14 | robot_state_publisher 15 | roscpp 16 | std_msgs 17 | controller_manager 18 | joint_state_controller 19 | robot_state_publisher 20 | roscpp 21 | std_msgs 22 | controller_manager 23 | joint_state_controller 24 | robot_state_publisher 25 | roscpp 26 | std_msgs 27 | unitree_legged_msgs 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /unitree_guide/src/FSM/FSM.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************** 2 | Copyright (c) 2020-2023, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ***********************************************************************/ 4 | #include "FSM/FSM.h" 5 | #include 6 | 7 | FSM::FSM(CtrlComponents *ctrlComp) 8 | :_ctrlComp(ctrlComp){ 9 | 10 | _stateList.invalid = nullptr; 11 | _stateList.passive = new State_Passive(_ctrlComp); 12 | _stateList.fixedStand = new State_FixedStand(_ctrlComp); 13 | _stateList.freeStand = new State_FreeStand(_ctrlComp); 14 | _stateList.trotting = new State_Trotting(_ctrlComp); 15 | _stateList.balanceTest = new State_BalanceTest(_ctrlComp); 16 | _stateList.swingTest = new State_SwingTest(_ctrlComp); 17 | _stateList.stepTest = new State_StepTest(_ctrlComp); 18 | #ifdef COMPILE_WITH_MOVE_BASE 19 | _stateList.moveBase = new State_move_base(_ctrlComp); 20 | #endif // COMPILE_WITH_MOVE_BASE 21 | initialize(); 22 | } 23 | 24 | FSM::~FSM(){ 25 | _stateList.deletePtr(); 26 | } 27 | 28 | void FSM::initialize(){ 29 | _currentState = _stateList.passive; 30 | _currentState -> enter(); 31 | _nextState = _currentState; 32 | _mode = FSMMode::NORMAL; 33 | } 34 | 35 | void FSM::run(){ 36 | _startTime = getSystemTime(); 37 | _ctrlComp->sendRecv(); 38 | _ctrlComp->runWaveGen(); 39 | _ctrlComp->estimator->run(); 40 | if(!checkSafty()){ 41 | _ctrlComp->ioInter->setPassive(); 42 | } 43 | 44 | if(_mode == FSMMode::NORMAL){ 45 | _currentState->run(); 46 | _nextStateName = _currentState->checkChange(); 47 | if(_nextStateName != _currentState->_stateName){ 48 | _mode = FSMMode::CHANGE; 49 | _nextState = getNextState(_nextStateName); 50 | std::cout << "Switched from " << _currentState->_stateNameString 51 | << " to " << _nextState->_stateNameString << std::endl; 52 | } 53 | } 54 | else if(_mode == FSMMode::CHANGE){ 55 | _currentState->exit(); 56 | _currentState = _nextState; 57 | _currentState->enter(); 58 | _mode = FSMMode::NORMAL; 59 | _currentState->run(); 60 | } 61 | 62 | absoluteWait(_startTime, (long long)(_ctrlComp->dt * 1000000)); 63 | } 64 | 65 | FSMState* FSM::getNextState(FSMStateName stateName){ 66 | switch (stateName) 67 | { 68 | case FSMStateName::INVALID: 69 | return _stateList.invalid; 70 | break; 71 | case FSMStateName::PASSIVE: 72 | return _stateList.passive; 73 | break; 74 | case FSMStateName::FIXEDSTAND: 75 | return _stateList.fixedStand; 76 | break; 77 | case FSMStateName::FREESTAND: 78 | return _stateList.freeStand; 79 | break; 80 | case FSMStateName::TROTTING: 81 | return _stateList.trotting; 82 | break; 83 | case FSMStateName::BALANCETEST: 84 | return _stateList.balanceTest; 85 | break; 86 | case FSMStateName::SWINGTEST: 87 | return _stateList.swingTest; 88 | break; 89 | case FSMStateName::STEPTEST: 90 | return _stateList.stepTest; 91 | break; 92 | #ifdef COMPILE_WITH_MOVE_BASE 93 | case FSMStateName::MOVE_BASE: 94 | return _stateList.moveBase; 95 | break; 96 | #endif // COMPILE_WITH_MOVE_BASE 97 | default: 98 | return _stateList.invalid; 99 | break; 100 | } 101 | } 102 | 103 | bool FSM::checkSafty(){ 104 | // The angle with z axis less than 60 degree 105 | if(_ctrlComp->lowState->getRotMat()(2,2) < 0.5 ){ 106 | return false; 107 | }else{ 108 | return true; 109 | } 110 | } -------------------------------------------------------------------------------- /unitree_guide/src/FSM/FSMState.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************** 2 | Copyright (c) 2020-2023, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ***********************************************************************/ 4 | #include "FSM/FSMState.h" 5 | 6 | FSMState::FSMState(CtrlComponents *ctrlComp, FSMStateName stateName, std::string stateNameString) 7 | :_ctrlComp(ctrlComp), _stateName(stateName), _stateNameString(stateNameString){ 8 | _lowCmd = _ctrlComp->lowCmd; 9 | _lowState = _ctrlComp->lowState; 10 | } -------------------------------------------------------------------------------- /unitree_guide/src/FSM/State_BalanceTest.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************** 2 | Copyright (c) 2020-2023, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ***********************************************************************/ 4 | #include "FSM/State_BalanceTest.h" 5 | 6 | State_BalanceTest::State_BalanceTest(CtrlComponents *ctrlComp) 7 | :FSMState(ctrlComp, FSMStateName::BALANCETEST, "balanceTest"), 8 | _est(ctrlComp->estimator), _robModel(ctrlComp->robotModel), 9 | _balCtrl(ctrlComp->balCtrl), _contact(ctrlComp->contact){ 10 | 11 | _xMax = 0.05; 12 | _xMin = -_xMax; 13 | _yMax = 0.05; 14 | _yMin = -_yMax; 15 | _zMax = 0.04; 16 | _zMin = -_zMax; 17 | _yawMax = 20 * M_PI / 180; 18 | _yawMin = -_yawMax; 19 | 20 | _Kpp = Vec3(150, 150, 150).asDiagonal(); 21 | _Kdp = Vec3(25, 25, 25).asDiagonal(); 22 | 23 | _kpw = 200; 24 | _Kdw = Vec3(30, 30, 30).asDiagonal(); 25 | } 26 | 27 | void State_BalanceTest::enter(){ 28 | _pcdInit = _est->getPosition(); 29 | _pcd = _pcdInit; 30 | _RdInit = _lowState->getRotMat(); 31 | 32 | _ctrlComp->setAllStance(); 33 | _ctrlComp->ioInter->zeroCmdPanel(); 34 | } 35 | 36 | void State_BalanceTest::run(){ 37 | _userValue = _lowState->userValue; 38 | 39 | _pcd(0) = _pcdInit(0) + invNormalize(_userValue.ly, _xMin, _xMax); 40 | _pcd(1) = _pcdInit(1) - invNormalize(_userValue.lx, _yMin, _yMax); 41 | _pcd(2) = _pcdInit(2) + invNormalize(_userValue.ry, _zMin, _zMax); 42 | 43 | float yaw = invNormalize(_userValue.rx, _yawMin, _yawMax); 44 | _Rd = rpyToRotMat(0, 0, yaw)*_RdInit; 45 | 46 | _posBody = _est->getPosition(); 47 | _velBody = _est->getVelocity(); 48 | 49 | _B2G_RotMat = _lowState->getRotMat(); 50 | _G2B_RotMat = _B2G_RotMat.transpose(); 51 | 52 | calcTau(); 53 | 54 | _lowCmd->setStableGain(); 55 | _lowCmd->setTau(_tau); 56 | _lowCmd->setQ(_q); 57 | } 58 | 59 | void State_BalanceTest::exit(){ 60 | _ctrlComp->ioInter->zeroCmdPanel(); 61 | } 62 | 63 | FSMStateName State_BalanceTest::checkChange(){ 64 | if(_lowState->userCmd == UserCommand::L2_B){ 65 | return FSMStateName::PASSIVE; 66 | } 67 | else if(_lowState->userCmd == UserCommand::L2_A){ 68 | return FSMStateName::FIXEDSTAND; 69 | } 70 | else{ 71 | return FSMStateName::BALANCETEST; 72 | } 73 | } 74 | 75 | void State_BalanceTest::calcTau(){ 76 | 77 | _ddPcd = _Kpp*(_pcd - _posBody) + _Kdp * (Vec3(0, 0, 0) - _velBody); 78 | _dWbd = _kpw*rotMatToExp(_Rd*_G2B_RotMat) + _Kdw * (Vec3(0, 0, 0) - _lowState->getGyroGlobal()); 79 | 80 | _posFeet2BGlobal = _est->getPosFeet2BGlobal(); 81 | 82 | _forceFeetGlobal = - _balCtrl->calF(_ddPcd, _dWbd, _B2G_RotMat, _posFeet2BGlobal, *_contact); 83 | _forceFeetBody = _G2B_RotMat * _forceFeetGlobal; 84 | 85 | _q = vec34ToVec12(_lowState->getQ()); 86 | _tau = _robModel->getTau(_q, _forceFeetBody); 87 | } -------------------------------------------------------------------------------- /unitree_guide/src/FSM/State_FixedStand.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************** 2 | Copyright (c) 2020-2023, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ***********************************************************************/ 4 | #include 5 | #include "FSM/State_FixedStand.h" 6 | 7 | State_FixedStand::State_FixedStand(CtrlComponents *ctrlComp) 8 | :FSMState(ctrlComp, FSMStateName::FIXEDSTAND, "fixed stand"){} 9 | 10 | void State_FixedStand::enter(){ 11 | for(int i=0; i<4; i++){ 12 | if(_ctrlComp->ctrlPlatform == CtrlPlatform::GAZEBO){ 13 | _lowCmd->setSimStanceGain(i); 14 | } 15 | else if(_ctrlComp->ctrlPlatform == CtrlPlatform::REALROBOT){ 16 | _lowCmd->setRealStanceGain(i); 17 | } 18 | _lowCmd->setZeroDq(i); 19 | _lowCmd->setZeroTau(i); 20 | } 21 | for(int i=0; i<12; i++){ 22 | _lowCmd->motorCmd[i].q = _lowState->motorState[i].q; 23 | _startPos[i] = _lowState->motorState[i].q; 24 | } 25 | _ctrlComp->setAllStance(); 26 | } 27 | 28 | void State_FixedStand::run(){ 29 | _percent += (float)1/_duration; 30 | _percent = _percent > 1 ? 1 : _percent; 31 | for(int j=0; j<12; j++){ 32 | _lowCmd->motorCmd[j].q = (1 - _percent)*_startPos[j] + _percent*_targetPos[j]; 33 | } 34 | } 35 | 36 | void State_FixedStand::exit(){ 37 | _percent = 0; 38 | } 39 | 40 | FSMStateName State_FixedStand::checkChange(){ 41 | if(_lowState->userCmd == UserCommand::L2_B){ 42 | return FSMStateName::PASSIVE; 43 | } 44 | else if(_lowState->userCmd == UserCommand::L2_X){ 45 | return FSMStateName::FREESTAND; 46 | } 47 | else if(_lowState->userCmd == UserCommand::START){ 48 | return FSMStateName::TROTTING; 49 | } 50 | else if(_lowState->userCmd == UserCommand::L1_X){ 51 | return FSMStateName::BALANCETEST; 52 | } 53 | else if(_lowState->userCmd == UserCommand::L1_A){ 54 | return FSMStateName::SWINGTEST; 55 | } 56 | else if(_lowState->userCmd == UserCommand::L1_Y){ 57 | return FSMStateName::STEPTEST; 58 | } 59 | #ifdef COMPILE_WITH_MOVE_BASE 60 | else if(_lowState->userCmd == UserCommand::L2_Y){ 61 | return FSMStateName::MOVE_BASE; 62 | } 63 | #endif // COMPILE_WITH_MOVE_BASE 64 | else{ 65 | return FSMStateName::FIXEDSTAND; 66 | } 67 | } -------------------------------------------------------------------------------- /unitree_guide/src/FSM/State_FreeStand.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************** 2 | Copyright (c) 2020-2023, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ***********************************************************************/ 4 | #include "FSM/State_FreeStand.h" 5 | 6 | State_FreeStand::State_FreeStand(CtrlComponents *ctrlComp) 7 | :FSMState(ctrlComp, FSMStateName::FREESTAND, "free stand"){ 8 | _rowMax = 20 * M_PI / 180; 9 | _rowMin = -_rowMax; 10 | _pitchMax = 15 * M_PI / 180; 11 | _pitchMin = -_pitchMax; 12 | _yawMax = 20 * M_PI / 180; 13 | _yawMin = -_yawMax; 14 | _heightMax = 0.04; 15 | _heightMin = -_heightMax; 16 | } 17 | 18 | void State_FreeStand::enter(){ 19 | for(int i=0; i<4; i++){ 20 | if(_ctrlComp->ctrlPlatform == CtrlPlatform::GAZEBO){ 21 | _lowCmd->setSimStanceGain(i); 22 | } 23 | else if(_ctrlComp->ctrlPlatform == CtrlPlatform::REALROBOT){ 24 | _lowCmd->setRealStanceGain(i); 25 | } 26 | _lowCmd->setZeroDq(i); 27 | _lowCmd->setZeroTau(i); 28 | } 29 | 30 | for(int i=0; i<12; i++){ 31 | _lowCmd->motorCmd[i].q = _lowState->motorState[i].q; 32 | } 33 | _initVecOX = _ctrlComp->robotModel->getX(*_lowState); 34 | _initVecXP = _ctrlComp->robotModel->getVecXP(*_lowState); 35 | 36 | _ctrlComp->setAllStance(); 37 | _ctrlComp->ioInter->zeroCmdPanel(); 38 | } 39 | 40 | void State_FreeStand::run(){ 41 | Vec34 vecOP; 42 | _userValue = _lowState->userValue; 43 | 44 | vecOP = _calcOP( invNormalize(_userValue.lx, _rowMin, _rowMax), 45 | invNormalize(_userValue.ly, _pitchMin, _pitchMax), 46 | -invNormalize(_userValue.rx, _yawMin, _yawMax), 47 | invNormalize(_userValue.ry, _heightMin, _heightMax) ); 48 | _calcCmd(vecOP); 49 | } 50 | 51 | void State_FreeStand::exit(){ 52 | _ctrlComp->ioInter->zeroCmdPanel(); 53 | } 54 | 55 | FSMStateName State_FreeStand::checkChange(){ 56 | if(_lowState->userCmd == UserCommand::L2_A){ 57 | return FSMStateName::FIXEDSTAND; 58 | } 59 | else if(_lowState->userCmd == UserCommand::L2_B){ 60 | return FSMStateName::PASSIVE; 61 | } 62 | else if(_lowState->userCmd == UserCommand::START){ 63 | return FSMStateName::TROTTING; 64 | } 65 | else{ 66 | return FSMStateName::FREESTAND; 67 | } 68 | } 69 | 70 | Vec34 State_FreeStand::_calcOP(float row, float pitch, float yaw, float height){ 71 | Vec3 vecXO = -_initVecOX; 72 | vecXO(2) += height; 73 | 74 | RotMat rotM = rpyToRotMat(row, pitch, yaw); 75 | 76 | HomoMat Tsb = homoMatrix(vecXO, rotM); 77 | HomoMat Tbs = homoMatrixInverse(Tsb); 78 | 79 | Vec4 tempVec4; 80 | Vec34 vecOP; 81 | for(int i(0); i<4; ++i){ 82 | tempVec4 = Tbs * homoVec(_initVecXP.col(i)); 83 | vecOP.col(i) = noHomoVec(tempVec4); 84 | } 85 | 86 | return vecOP; 87 | } 88 | 89 | void State_FreeStand::_calcCmd(Vec34 vecOP){ 90 | Vec12 q = _ctrlComp->robotModel->getQ(vecOP, FrameType::BODY); 91 | _lowCmd->setQ(q); 92 | } -------------------------------------------------------------------------------- /unitree_guide/src/FSM/State_Passive.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************** 2 | Copyright (c) 2020-2023, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ***********************************************************************/ 4 | #include "FSM/State_Passive.h" 5 | 6 | State_Passive::State_Passive(CtrlComponents *ctrlComp) 7 | :FSMState(ctrlComp, FSMStateName::PASSIVE, "passive"){} 8 | 9 | void State_Passive::enter(){ 10 | if(_ctrlComp->ctrlPlatform == CtrlPlatform::GAZEBO){ 11 | for(int i=0; i<12; i++){ 12 | _lowCmd->motorCmd[i].mode = 10; 13 | _lowCmd->motorCmd[i].q = 0; 14 | _lowCmd->motorCmd[i].dq = 0; 15 | _lowCmd->motorCmd[i].Kp = 0; 16 | _lowCmd->motorCmd[i].Kd = 8; 17 | _lowCmd->motorCmd[i].tau = 0; 18 | } 19 | } 20 | else if(_ctrlComp->ctrlPlatform == CtrlPlatform::REALROBOT){ 21 | for(int i=0; i<12; i++){ 22 | _lowCmd->motorCmd[i].mode = 10; 23 | _lowCmd->motorCmd[i].q = 0; 24 | _lowCmd->motorCmd[i].dq = 0; 25 | _lowCmd->motorCmd[i].Kp = 0; 26 | _lowCmd->motorCmd[i].Kd = 3; 27 | _lowCmd->motorCmd[i].tau = 0; 28 | } 29 | } 30 | 31 | _ctrlComp->setAllSwing(); 32 | } 33 | 34 | void State_Passive::run(){ 35 | 36 | } 37 | 38 | void State_Passive::exit(){ 39 | 40 | } 41 | 42 | FSMStateName State_Passive::checkChange(){ 43 | if(_lowState->userCmd == UserCommand::L2_A){ 44 | return FSMStateName::FIXEDSTAND; 45 | } 46 | else{ 47 | return FSMStateName::PASSIVE; 48 | } 49 | } -------------------------------------------------------------------------------- /unitree_guide/src/FSM/State_StepTest.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************** 2 | Copyright (c) 2020-2023, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ***********************************************************************/ 4 | #include "FSM/State_StepTest.h" 5 | 6 | State_StepTest::State_StepTest(CtrlComponents *ctrlComp) 7 | :FSMState(ctrlComp, FSMStateName::STEPTEST, "stepTest"), 8 | _est(ctrlComp->estimator), _robModel(ctrlComp->robotModel), 9 | _balCtrl(ctrlComp->balCtrl), _contact(ctrlComp->contact), 10 | _phase(ctrlComp->phase){ 11 | 12 | _gaitHeight = 0.05; 13 | 14 | _KpSwing = Vec3(600, 600, 200).asDiagonal(); 15 | _KdSwing = Vec3(20, 20, 5).asDiagonal(); 16 | 17 | _Kpp = Vec3(50, 50, 300).asDiagonal(); 18 | _Kpw = Vec3(600, 600, 600).asDiagonal(); 19 | _Kdp = Vec3(5, 5, 20).asDiagonal(); 20 | _Kdw = Vec3(10, 10, 10).asDiagonal(); 21 | } 22 | 23 | void State_StepTest::enter(){ 24 | _pcd = _est->getPosition(); 25 | _Rd = _lowState->getRotMat(); 26 | _posFeetGlobalInit = _est->getFeetPos(); 27 | _posFeetGlobalGoal = _posFeetGlobalInit; 28 | _ctrlComp->setStartWave(); 29 | _ctrlComp->ioInter->zeroCmdPanel(); 30 | } 31 | 32 | void State_StepTest::run(){ 33 | _posBody = _est->getPosition(); 34 | _velBody = _est->getVelocity(); 35 | 36 | _B2G_RotMat = _lowState->getRotMat(); 37 | _G2B_RotMat = _B2G_RotMat.transpose(); 38 | 39 | 40 | for(int i(0); i<4; ++i){ 41 | if((*_contact)(i) == 0){ 42 | _posFeetGlobalGoal(2, i) = _posFeetGlobalInit(2, i) + (1-cos((*_phase)(i)*2*M_PI))*_gaitHeight; 43 | _velFeetGlobalGoal(2, i) = sin((*_phase)(i)*2*M_PI)*2*M_PI*_gaitHeight; 44 | } 45 | } 46 | 47 | calcTau(); 48 | 49 | _lowCmd->setZeroGain(); 50 | _lowCmd->setTau(_tau); 51 | } 52 | 53 | void State_StepTest::exit(){ 54 | _ctrlComp->ioInter->zeroCmdPanel(); 55 | _ctrlComp->setAllSwing(); 56 | } 57 | 58 | FSMStateName State_StepTest::checkChange(){ 59 | if(_lowState->userCmd == UserCommand::L2_B){ 60 | return FSMStateName::PASSIVE; 61 | } 62 | else if(_lowState->userCmd == UserCommand::L2_A){ 63 | return FSMStateName::FIXEDSTAND; 64 | } 65 | else{ 66 | return FSMStateName::STEPTEST; 67 | } 68 | } 69 | 70 | void State_StepTest::calcTau(){ 71 | _ddPcd = _Kpp*(_pcd - _posBody) + _Kdp * (Vec3(0, 0, 0) - _velBody); 72 | _dWbd = _Kpw*rotMatToExp(_Rd*_G2B_RotMat) + _Kdw * (Vec3(0, 0, 0) - _lowState->getGyroGlobal()); 73 | 74 | _posFeet2BGlobal = _est->getPosFeet2BGlobal(); 75 | _forceFeetGlobal = - _balCtrl->calF(_ddPcd, _dWbd, _B2G_RotMat, _posFeet2BGlobal, *_contact); 76 | 77 | _posFeetGlobal = _est->getFeetPos(); 78 | _velFeetGlobal = _est->getFeetVel(); 79 | for(int i(0); i<4; ++i){ 80 | if((*_contact)(i) == 0){ 81 | _forceFeetGlobal.col(i) = _KpSwing*(_posFeetGlobalGoal.col(i) - _posFeetGlobal.col(i)) + _KdSwing*(_velFeetGlobalGoal.col(i)-_velFeetGlobal.col(i)); 82 | } 83 | } 84 | 85 | _forceFeetBody = _G2B_RotMat * _forceFeetGlobal; 86 | 87 | _q = vec34ToVec12(_lowState->getQ()); 88 | _tau = _robModel->getTau(_q, _forceFeetBody); 89 | } -------------------------------------------------------------------------------- /unitree_guide/src/FSM/State_SwingTest.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************** 2 | Copyright (c) 2020-2023, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ***********************************************************************/ 4 | #include "FSM/State_SwingTest.h" 5 | 6 | State_SwingTest::State_SwingTest(CtrlComponents *ctrlComp) 7 | :FSMState(ctrlComp, FSMStateName::SWINGTEST, "swingTest"){ 8 | _xMin = -0.15; 9 | _xMax = 0.10; 10 | _yMin = -0.15; 11 | _yMax = 0.15; 12 | _zMin = -0.05; 13 | _zMax = 0.20; 14 | } 15 | 16 | void State_SwingTest::enter(){ 17 | for(int i=0; i<4; i++){ 18 | if(_ctrlComp->ctrlPlatform == CtrlPlatform::GAZEBO){ 19 | _lowCmd->setSimStanceGain(i); 20 | } 21 | else if(_ctrlComp->ctrlPlatform == CtrlPlatform::REALROBOT){ 22 | _lowCmd->setRealStanceGain(i); 23 | } 24 | _lowCmd->setZeroDq(i); 25 | _lowCmd->setZeroTau(i); 26 | } 27 | _lowCmd->setSwingGain(0); 28 | 29 | _Kp = Vec3(20, 20, 50).asDiagonal(); 30 | _Kd = Vec3( 5, 5, 20).asDiagonal(); 31 | 32 | for(int i=0; i<12; i++){ 33 | _lowCmd->motorCmd[i].q = _lowState->motorState[i].q; 34 | } 35 | 36 | _initFeetPos = _ctrlComp->robotModel->getFeet2BPositions(*_lowState, FrameType::HIP); 37 | _feetPos = _initFeetPos; 38 | _initPos = _initFeetPos.col(0); 39 | 40 | _ctrlComp->setAllSwing(); 41 | } 42 | 43 | void State_SwingTest::run(){ 44 | _userValue = _lowState->userValue; 45 | 46 | if(_userValue.ly > 0){ 47 | _posGoal(0) = invNormalize(_userValue.ly, _initPos(0), _initPos(0)+_xMax, 0, 1); 48 | }else{ 49 | _posGoal(0) = invNormalize(_userValue.ly, _initPos(0)+_xMin, _initPos(0), -1, 0); 50 | } 51 | 52 | if(_userValue.lx > 0){ 53 | _posGoal(1) = invNormalize(_userValue.lx, _initPos(1, 0), _initPos(1)+_yMax, 0, 1); 54 | }else{ 55 | _posGoal(1) = invNormalize(_userValue.lx, _initPos(1)+_yMin, _initPos(1), -1, 0); 56 | } 57 | 58 | if(_userValue.ry > 0){ 59 | _posGoal(2) = invNormalize(_userValue.ry, _initPos(2), _initPos(2)+_zMax, 0, 1); 60 | }else{ 61 | _posGoal(2) = invNormalize(_userValue.ry, _initPos(2)+_zMin, _initPos(2), -1, 0); 62 | } 63 | 64 | _positionCtrl(); 65 | _torqueCtrl(); 66 | } 67 | 68 | void State_SwingTest::exit(){ 69 | _ctrlComp->ioInter->zeroCmdPanel(); 70 | } 71 | 72 | FSMStateName State_SwingTest::checkChange(){ 73 | if(_lowState->userCmd == UserCommand::L2_B){ 74 | return FSMStateName::PASSIVE; 75 | } 76 | else if(_lowState->userCmd == UserCommand::L2_A){ 77 | return FSMStateName::FIXEDSTAND; 78 | } 79 | else{ 80 | return FSMStateName::SWINGTEST; 81 | } 82 | } 83 | 84 | void State_SwingTest::_positionCtrl(){ 85 | _feetPos.col(0) = _posGoal; 86 | _targetPos = _ctrlComp->robotModel->getQ(_feetPos, FrameType::HIP); 87 | _lowCmd->setQ(_targetPos); 88 | } 89 | 90 | void State_SwingTest::_torqueCtrl(){ 91 | Vec3 pos0 = _ctrlComp->robotModel->getFootPosition(*_lowState, 0, FrameType::HIP); 92 | Vec3 vel0 = _ctrlComp->robotModel->getFootVelocity(*_lowState, 0); 93 | 94 | Vec3 force0 = _Kp*(_posGoal - pos0) + _Kd*(-vel0); 95 | 96 | Vec12 torque; 97 | torque.setZero(); 98 | Mat3 jaco0 = _ctrlComp->robotModel->getJaco(*_lowState, 0); 99 | 100 | torque.segment(0, 3) = jaco0.transpose() * force0; 101 | 102 | _lowCmd->setTau(torque); 103 | } -------------------------------------------------------------------------------- /unitree_guide/src/FSM/State_move_base.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************** 2 | Copyright (c) 2020-2023, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ***********************************************************************/ 4 | #ifdef COMPILE_WITH_MOVE_BASE 5 | 6 | #include "FSM/State_move_base.h" 7 | 8 | State_move_base::State_move_base(CtrlComponents *ctrlComp) 9 | :State_Trotting(ctrlComp){ 10 | _stateName = FSMStateName::MOVE_BASE; 11 | _stateNameString = "move_base"; 12 | initRecv(); 13 | } 14 | 15 | FSMStateName State_move_base::checkChange(){ 16 | if(_lowState->userCmd == UserCommand::L2_B){ 17 | return FSMStateName::PASSIVE; 18 | } 19 | else if(_lowState->userCmd == UserCommand::L2_A){ 20 | return FSMStateName::FIXEDSTAND; 21 | } 22 | else{ 23 | return FSMStateName::MOVE_BASE; 24 | } 25 | } 26 | 27 | void State_move_base::getUserCmd(){ 28 | setHighCmd(_vx, _vy, _wz); 29 | ros::spinOnce(); 30 | } 31 | 32 | void State_move_base::twistCallback(const geometry_msgs::Twist& msg){ 33 | _vx = msg.linear.x; 34 | _vy = msg.linear.y; 35 | _wz = msg.angular.z; 36 | } 37 | 38 | void State_move_base::initRecv(){ 39 | _cmdSub = _nm.subscribe("/cmd_vel", 1, &State_move_base::twistCallback, this); 40 | } 41 | 42 | #endif // COMPILE_WITH_MOVE_BASE -------------------------------------------------------------------------------- /unitree_guide/src/Gait/FeetEndCal.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************** 2 | Copyright (c) 2020-2023, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ***********************************************************************/ 4 | #include "Gait/FeetEndCal.h" 5 | 6 | FeetEndCal::FeetEndCal(CtrlComponents *ctrlComp) 7 | : _est(ctrlComp->estimator), _lowState(ctrlComp->lowState), 8 | _robModel(ctrlComp->robotModel){ 9 | _Tstance = ctrlComp->waveGen->getTstance(); 10 | _Tswing = ctrlComp->waveGen->getTswing(); 11 | 12 | _kx = 0.005; 13 | _ky = 0.005; 14 | _kyaw = 0.005; 15 | 16 | Vec34 feetPosBody = _robModel->getFeetPosIdeal(); 17 | for(int i(0); i<4; ++i){ 18 | _feetRadius(i) = sqrt( pow(feetPosBody(0, i), 2) + pow(feetPosBody(1, i), 2) ); 19 | _feetInitAngle(i) = atan2(feetPosBody(1, i), feetPosBody(0, i)); 20 | } 21 | } 22 | 23 | FeetEndCal::~FeetEndCal(){ 24 | 25 | } 26 | 27 | Vec3 FeetEndCal::calFootPos(int legID, Vec2 vxyGoalGlobal, float dYawGoal, float phase){ 28 | _bodyVelGlobal = _est->getVelocity(); 29 | _bodyWGlobal = _lowState->getGyroGlobal(); 30 | 31 | _nextStep(0) = _bodyVelGlobal(0)*(1-phase)*_Tswing + _bodyVelGlobal(0)*_Tstance/2 + _kx*(_bodyVelGlobal(0) - vxyGoalGlobal(0)); 32 | _nextStep(1) = _bodyVelGlobal(1)*(1-phase)*_Tswing + _bodyVelGlobal(1)*_Tstance/2 + _ky*(_bodyVelGlobal(1) - vxyGoalGlobal(1)); 33 | _nextStep(2) = 0; 34 | 35 | _yaw = _lowState->getYaw(); 36 | _dYaw = _lowState->getDYaw(); 37 | _nextYaw = _dYaw*(1-phase)*_Tswing + _dYaw*_Tstance/2 + _kyaw*(dYawGoal - _dYaw); 38 | 39 | _nextStep(0) += _feetRadius(legID) * cos(_yaw + _feetInitAngle(legID) + _nextYaw); 40 | _nextStep(1) += _feetRadius(legID) * sin(_yaw + _feetInitAngle(legID) + _nextYaw); 41 | 42 | _footPos = _est->getPosition() + _nextStep; 43 | _footPos(2) = 0.0; 44 | 45 | return _footPos; 46 | } -------------------------------------------------------------------------------- /unitree_guide/src/Gait/GaitGenerator.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************** 2 | Copyright (c) 2020-2023, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ***********************************************************************/ 4 | #include "Gait/GaitGenerator.h" 5 | 6 | GaitGenerator::GaitGenerator(CtrlComponents *ctrlComp) 7 | : _waveG(ctrlComp->waveGen), _est(ctrlComp->estimator), 8 | _phase(ctrlComp->phase), _contact(ctrlComp->contact), 9 | _robModel(ctrlComp->robotModel), _state(ctrlComp->lowState){ 10 | _feetCal = new FeetEndCal(ctrlComp); 11 | _firstRun = true; 12 | } 13 | 14 | GaitGenerator::~GaitGenerator(){ 15 | } 16 | 17 | void GaitGenerator::setGait(Vec2 vxyGoalGlobal, float dYawGoal, float gaitHeight){ 18 | _vxyGoal = vxyGoalGlobal; 19 | _dYawGoal = dYawGoal; 20 | _gaitHeight = gaitHeight; 21 | } 22 | 23 | void GaitGenerator::restart(){ 24 | _firstRun = true; 25 | _vxyGoal.setZero(); 26 | } 27 | 28 | void GaitGenerator::run(Vec34 &feetPos, Vec34 &feetVel){ 29 | if(_firstRun){ 30 | _startP = _est->getFeetPos(); 31 | _firstRun = false; 32 | } 33 | 34 | for(int i(0); i<4; ++i){ 35 | if((*_contact)(i) == 1){ 36 | if((*_phase)(i) < 0.5){ 37 | _startP.col(i) = _est->getFootPos(i); 38 | } 39 | feetPos.col(i) = _startP.col(i); 40 | feetVel.col(i).setZero(); 41 | } 42 | else{ 43 | _endP.col(i) = _feetCal->calFootPos(i, _vxyGoal, _dYawGoal, (*_phase)(i)); 44 | 45 | feetPos.col(i) = getFootPos(i); 46 | feetVel.col(i) = getFootVel(i); 47 | } 48 | } 49 | _pastP = feetPos; 50 | _phasePast = *_phase; 51 | } 52 | 53 | Vec3 GaitGenerator::getFootPos(int i){ 54 | Vec3 footPos; 55 | 56 | footPos(0) = cycloidXYPosition(_startP.col(i)(0), _endP.col(i)(0), (*_phase)(i)); 57 | footPos(1) = cycloidXYPosition(_startP.col(i)(1), _endP.col(i)(1), (*_phase)(i)); 58 | footPos(2) = cycloidZPosition(_startP.col(i)(2), _gaitHeight, (*_phase)(i)); 59 | 60 | return footPos; 61 | } 62 | 63 | Vec3 GaitGenerator::getFootVel(int i){ 64 | Vec3 footVel; 65 | 66 | footVel(0) = cycloidXYVelocity(_startP.col(i)(0), _endP.col(i)(0), (*_phase)(i)); 67 | footVel(1) = cycloidXYVelocity(_startP.col(i)(1), _endP.col(i)(1), (*_phase)(i)); 68 | footVel(2) = cycloidZVelocity(_gaitHeight, (*_phase)(i)); 69 | 70 | return footVel; 71 | } 72 | 73 | float GaitGenerator::cycloidXYPosition(float start, float end, float phase){ 74 | float phasePI = 2 * M_PI * phase; 75 | return (end - start)*(phasePI - sin(phasePI))/(2*M_PI) + start; 76 | } 77 | 78 | float GaitGenerator::cycloidXYVelocity(float start, float end, float phase){ 79 | float phasePI = 2 * M_PI * phase; 80 | return (end - start)*(1 - cos(phasePI)) / _waveG->getTswing(); 81 | } 82 | 83 | float GaitGenerator::cycloidZPosition(float start, float h, float phase){ 84 | float phasePI = 2 * M_PI * phase; 85 | return h*(1 - cos(phasePI))/2 + start; 86 | } 87 | 88 | float GaitGenerator::cycloidZVelocity(float h, float phase){ 89 | float phasePI = 2 * M_PI * phase; 90 | return h*M_PI * sin(phasePI) / _waveG->getTswing(); 91 | } -------------------------------------------------------------------------------- /unitree_guide/src/Gait/WaveGenerator.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************** 2 | Copyright (c) 2020-2023, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ***********************************************************************/ 4 | #include "Gait/WaveGenerator.h" 5 | #include 6 | #include 7 | #include 8 | 9 | WaveGenerator::WaveGenerator(double period, double stancePhaseRatio, Vec4 bias) 10 | : _period(period), _stRatio(stancePhaseRatio), _bias(bias) 11 | { 12 | 13 | if ((_stRatio >= 1) || (_stRatio <= 0)) 14 | { 15 | std::cout << "[ERROR] The stancePhaseRatio of WaveGenerator should between (0, 1)" << std::endl; 16 | exit(-1); 17 | } 18 | 19 | for (int i(0); i < bias.rows(); ++i) 20 | { 21 | if ((bias(i) > 1) || (bias(i) < 0)) 22 | { 23 | std::cout << "[ERROR] The bias of WaveGenerator should between [0, 1]" << std::endl; 24 | exit(-1); 25 | } 26 | } 27 | 28 | _startT = getSystemTime(); 29 | _contactPast.setZero(); 30 | _phasePast << 0.5, 0.5, 0.5, 0.5; 31 | _statusPast = WaveStatus::SWING_ALL; 32 | } 33 | 34 | WaveGenerator::~WaveGenerator() 35 | { 36 | } 37 | 38 | void WaveGenerator::calcContactPhase(Vec4 &phaseResult, VecInt4 &contactResult, WaveStatus status) 39 | { 40 | 41 | calcWave(_phase, _contact, status); 42 | 43 | if (status != _statusPast) 44 | { 45 | if (_switchStatus.sum() == 0) 46 | { 47 | _switchStatus.setOnes(); 48 | } 49 | calcWave(_phasePast, _contactPast, _statusPast); 50 | // two special case 51 | if ((status == WaveStatus::STANCE_ALL) && (_statusPast == WaveStatus::SWING_ALL)) 52 | { 53 | _contactPast.setOnes(); 54 | } 55 | else if ((status == WaveStatus::SWING_ALL) && (_statusPast == WaveStatus::STANCE_ALL)) 56 | { 57 | _contactPast.setZero(); 58 | } 59 | } 60 | 61 | if (_switchStatus.sum() != 0) 62 | { 63 | for (int i(0); i < 4; ++i) 64 | { 65 | if (_contact(i) == _contactPast(i)) 66 | { 67 | _switchStatus(i) = 0; 68 | } 69 | else 70 | { 71 | _contact(i) = _contactPast(i); 72 | _phase(i) = _phasePast(i); 73 | } 74 | } 75 | if (_switchStatus.sum() == 0) 76 | { 77 | _statusPast = status; 78 | } 79 | } 80 | 81 | phaseResult = _phase; 82 | contactResult = _contact; 83 | } 84 | 85 | float WaveGenerator::getTstance() 86 | { 87 | return _period * _stRatio; 88 | } 89 | 90 | float WaveGenerator::getTswing() 91 | { 92 | return _period * (1 - _stRatio); 93 | } 94 | 95 | float WaveGenerator::getT() 96 | { 97 | return _period; 98 | } 99 | 100 | void WaveGenerator::calcWave(Vec4 &phase, VecInt4 &contact, WaveStatus status) 101 | { 102 | if (status == WaveStatus::WAVE_ALL) 103 | { 104 | _passT = (double)(getSystemTime() - _startT) * 1e-6; 105 | for (int i(0); i < 4; ++i) 106 | { 107 | _normalT(i) = fmod(_passT + _period - _period * _bias(i), _period) / _period; 108 | if (_normalT(i) < _stRatio) 109 | { 110 | contact(i) = 1; 111 | phase(i) = _normalT(i) / _stRatio; 112 | } 113 | else 114 | { 115 | contact(i) = 0; 116 | phase(i) = (_normalT(i) - _stRatio) / (1 - _stRatio); 117 | } 118 | } 119 | } 120 | else if (status == WaveStatus::SWING_ALL) 121 | { 122 | contact.setZero(); 123 | phase << 0.5, 0.5, 0.5, 0.5; 124 | } 125 | else if (status == WaveStatus::STANCE_ALL) 126 | { 127 | contact.setOnes(); 128 | phase << 0.5, 0.5, 0.5, 0.5; 129 | } 130 | } -------------------------------------------------------------------------------- /unitree_guide/src/common/LowPassFilter.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************** 2 | Copyright (c) 2020-2023, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ***********************************************************************/ 4 | #include "common/LowPassFilter.h" 5 | #include 6 | 7 | LPFilter::LPFilter(double samplePeriod, double cutFrequency){ 8 | _weight = 1.0 / ( 1.0 + 1.0/(2.0*M_PI * samplePeriod * cutFrequency) ); 9 | _start = false; 10 | } 11 | 12 | void LPFilter::addValue(double newValue){ 13 | if(!_start){ 14 | _start = true; 15 | _pastValue = newValue; 16 | } 17 | _pastValue = _weight*newValue + (1-_weight)*_pastValue; 18 | } 19 | 20 | double LPFilter::getValue(){ 21 | return _pastValue; 22 | } 23 | 24 | void LPFilter::clear(){ 25 | _start = false; 26 | } -------------------------------------------------------------------------------- /unitree_guide/src/control/ControlFrame.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************** 2 | Copyright (c) 2020-2023, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ***********************************************************************/ 4 | #include "control/ControlFrame.h" 5 | 6 | ControlFrame::ControlFrame(CtrlComponents *ctrlComp):_ctrlComp(ctrlComp){ 7 | _FSMController = new FSM(_ctrlComp); 8 | } 9 | 10 | void ControlFrame::run(){ 11 | _FSMController->run(); 12 | } -------------------------------------------------------------------------------- /unitree_guide/src/interface/IOSDK.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************** 2 | Copyright (c) 2020-2023, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ***********************************************************************/ 4 | #ifdef COMPILE_WITH_REAL_ROBOT 5 | 6 | #include "interface/IOSDK.h" 7 | #include "interface/WirelessHandle.h" 8 | #include 9 | 10 | #ifdef ROBOT_TYPE_Go1 11 | IOSDK::IOSDK():_safe(UNITREE_LEGGED_SDK::LeggedType::Aliengo), _udp(UNITREE_LEGGED_SDK::LOWLEVEL, 8090, "192.168.123.10", 8007){ 12 | std::cout << "The control interface for real robot" << std::endl; 13 | _udp.InitCmdData(_lowCmd); 14 | cmdPanel = new WirelessHandle(); 15 | 16 | #ifdef COMPILE_WITH_MOVE_BASE 17 | _pub = _nh.advertise("/realRobot/joint_states", 20); 18 | _joint_state.name.resize(12); 19 | _joint_state.position.resize(12); 20 | _joint_state.velocity.resize(12); 21 | _joint_state.effort.resize(12); 22 | #endif // COMPILE_WITH_MOVE_BASE 23 | } 24 | #endif 25 | 26 | #ifdef ROBOT_TYPE_A1 27 | IOSDK::IOSDK():_safe(UNITREE_LEGGED_SDK::LeggedType::Aliengo), _udp(UNITREE_LEGGED_SDK::LOWLEVEL){ 28 | std::cout << "The control interface for real robot" << std::endl; 29 | _udp.InitCmdData(_lowCmd); 30 | cmdPanel = new WirelessHandle(); 31 | 32 | #ifdef COMPILE_WITH_MOVE_BASE 33 | _pub = _nh.advertise("/realRobot/joint_states", 20); 34 | _joint_state.name.resize(12); 35 | _joint_state.position.resize(12); 36 | _joint_state.velocity.resize(12); 37 | _joint_state.effort.resize(12); 38 | #endif // COMPILE_WITH_MOVE_BASE 39 | } 40 | #endif 41 | 42 | 43 | void IOSDK::sendRecv(const LowlevelCmd *cmd, LowlevelState *state){ 44 | for(int i(0); i < 12; ++i){ 45 | _lowCmd.motorCmd[i].mode = cmd->motorCmd[i].mode; 46 | _lowCmd.motorCmd[i].q = cmd->motorCmd[i].q; 47 | _lowCmd.motorCmd[i].dq = cmd->motorCmd[i].dq; 48 | _lowCmd.motorCmd[i].Kp = cmd->motorCmd[i].Kp; 49 | _lowCmd.motorCmd[i].Kd = cmd->motorCmd[i].Kd; 50 | _lowCmd.motorCmd[i].tau = cmd->motorCmd[i].tau; 51 | } 52 | 53 | _udp.SetSend(_lowCmd); 54 | _udp.Send(); 55 | 56 | _udp.Recv(); 57 | _udp.GetRecv(_lowState); 58 | 59 | for(int i(0); i < 12; ++i){ 60 | state->motorState[i].q = _lowState.motorState[i].q; 61 | state->motorState[i].dq = _lowState.motorState[i].dq; 62 | state->motorState[i].ddq = _lowState.motorState[i].ddq; 63 | state->motorState[i].tauEst = _lowState.motorState[i].tauEst; 64 | state->motorState[i].mode = _lowState.motorState[i].mode; 65 | } 66 | 67 | for(int i(0); i < 3; ++i){ 68 | state->imu.quaternion[i] = _lowState.imu.quaternion[i]; 69 | state->imu.gyroscope[i] = _lowState.imu.gyroscope[i]; 70 | state->imu.accelerometer[i] = _lowState.imu.accelerometer[i]; 71 | } 72 | state->imu.quaternion[3] = _lowState.imu.quaternion[3]; 73 | 74 | cmdPanel->receiveHandle(&_lowState); 75 | state->userCmd = cmdPanel->getUserCmd(); 76 | state->userValue = cmdPanel->getUserValue(); 77 | 78 | #ifdef COMPILE_WITH_MOVE_BASE 79 | _joint_state.header.stamp = ros::Time::now(); 80 | _joint_state.name = {"FR_hip_joint", "FR_thigh_joint", "FR_calf_joint", 81 | "FL_hip_joint", "FL_thigh_joint", "FL_calf_joint", 82 | "RR_hip_joint", "RR_thigh_joint", "RR_calf_joint", 83 | "RL_hip_joint", "RL_thigh_joint", "RL_calf_joint"}; 84 | for(int i(0); i<12; ++i){ 85 | _joint_state.position[i] = state->motorState[i].q; 86 | _joint_state.velocity[i] = state->motorState[i].dq; 87 | _joint_state.effort[i] = state->motorState[i].tauEst; 88 | } 89 | 90 | _pub.publish(_joint_state); 91 | #endif // COMPILE_WITH_MOVE_BASE 92 | } 93 | 94 | #endif // COMPILE_WITH_REAL_ROBOT -------------------------------------------------------------------------------- /unitree_guide/src/interface/KeyBoard.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************** 2 | Copyright (c) 2020-2023, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ***********************************************************************/ 4 | #include "interface/KeyBoard.h" 5 | #include 6 | 7 | KeyBoard::KeyBoard(){ 8 | userCmd = UserCommand::NONE; 9 | userValue.setZero(); 10 | 11 | tcgetattr( fileno( stdin ), &_oldSettings ); 12 | _newSettings = _oldSettings; 13 | _newSettings.c_lflag &= (~ICANON & ~ECHO); 14 | tcsetattr( fileno( stdin ), TCSANOW, &_newSettings ); 15 | 16 | pthread_create(&_tid, NULL, runKeyBoard, (void*)this); 17 | } 18 | 19 | KeyBoard::~KeyBoard(){ 20 | pthread_cancel(_tid); 21 | pthread_join(_tid, NULL); 22 | tcsetattr( fileno( stdin ), TCSANOW, &_oldSettings ); 23 | } 24 | 25 | UserCommand KeyBoard::checkCmd(){ 26 | switch (_c){ 27 | case '1': 28 | return UserCommand::L2_B; 29 | case '2': 30 | return UserCommand::L2_A; 31 | case '3': 32 | return UserCommand::L2_X; 33 | case '4': 34 | return UserCommand::START; 35 | #ifdef COMPILE_WITH_MOVE_BASE 36 | case '5': 37 | return UserCommand::L2_Y; 38 | #endif // COMPILE_WITH_MOVE_BASE 39 | case '0': 40 | return UserCommand::L1_X; 41 | case '9': 42 | return UserCommand::L1_A; 43 | case '8': 44 | return UserCommand::L1_Y; 45 | case ' ': 46 | userValue.setZero(); 47 | return UserCommand::NONE; 48 | default: 49 | return UserCommand::NONE; 50 | } 51 | } 52 | 53 | void KeyBoard::changeValue(){ 54 | switch (_c){ 55 | case 'w':case 'W': 56 | userValue.ly = min(userValue.ly+sensitivityLeft, 1.0); 57 | break; 58 | case 's':case 'S': 59 | userValue.ly = max(userValue.ly-sensitivityLeft, -1.0); 60 | break; 61 | case 'd':case 'D': 62 | userValue.lx = min(userValue.lx+sensitivityLeft, 1.0); 63 | break; 64 | case 'a':case 'A': 65 | userValue.lx = max(userValue.lx-sensitivityLeft, -1.0); 66 | break; 67 | 68 | case 'i':case 'I': 69 | userValue.ry = min(userValue.ry+sensitivityRight, 1.0); 70 | break; 71 | case 'k':case 'K': 72 | userValue.ry = max(userValue.ry-sensitivityRight, -1.0); 73 | break; 74 | case 'l':case 'L': 75 | userValue.rx = min(userValue.rx+sensitivityRight, 1.0); 76 | break; 77 | case 'j':case 'J': 78 | userValue.rx = max(userValue.rx-sensitivityRight, -1.0); 79 | break; 80 | default: 81 | break; 82 | } 83 | } 84 | 85 | void* KeyBoard::runKeyBoard(void *arg){ 86 | ((KeyBoard*)arg)->run(NULL); 87 | return NULL; 88 | } 89 | 90 | void* KeyBoard::run(void *arg){ 91 | while(1){ 92 | FD_ZERO(&set); 93 | FD_SET( fileno( stdin ), &set ); 94 | 95 | res = select( fileno( stdin )+1, &set, NULL, NULL, NULL); 96 | 97 | if(res > 0){ 98 | ret = read( fileno( stdin ), &_c, 1 ); 99 | userCmd = checkCmd(); 100 | if(userCmd == UserCommand::NONE) 101 | changeValue(); 102 | _c = '\0'; 103 | } 104 | usleep(1000); 105 | } 106 | return NULL; 107 | } -------------------------------------------------------------------------------- /unitree_guide/src/interface/WirelessHandle.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************** 2 | Copyright (c) 2020-2023, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ***********************************************************************/ 4 | #ifdef COMPILE_WITH_REAL_ROBOT 5 | 6 | #include "interface/WirelessHandle.h" 7 | #include "common/mathTools.h" 8 | #include 9 | #include 10 | 11 | WirelessHandle::WirelessHandle(){ 12 | } 13 | 14 | void WirelessHandle::receiveHandle(UNITREE_LEGGED_SDK::LowState *lowState){ 15 | #ifdef ROBOT_TYPE_A1 16 | memcpy(&_keyData, lowState->wirelessRemote, 40); // note: available in the unitree_legged_sdk_3.2 17 | #endif 18 | #ifdef ROBOT_TYPE_Go1 19 | memcpy(&_keyData, &lowState->wirelessRemote[0], 40); 20 | #endif 21 | if(((int)_keyData.btn.components.L2 == 1) && 22 | ((int)_keyData.btn.components.B == 1)){ 23 | userCmd = UserCommand::L2_B; 24 | } 25 | else if(((int)_keyData.btn.components.L2 == 1) && 26 | ((int)_keyData.btn.components.A == 1)){ 27 | userCmd = UserCommand::L2_A; 28 | } 29 | else if(((int)_keyData.btn.components.L2 == 1) && 30 | ((int)_keyData.btn.components.X == 1)){ 31 | userCmd = UserCommand::L2_X; 32 | } 33 | 34 | #ifdef COMPILE_WITH_MOVE_BASE 35 | else if(((int)_keyData.btn.components.L2 == 1) && 36 | ((int)_keyData.btn.components.Y == 1)){ 37 | userCmd = UserCommand::L2_Y; 38 | } 39 | #endif // COMPILE_WITH_MOVE_BASE 40 | 41 | else if(((int)_keyData.btn.components.L1 == 1) && 42 | ((int)_keyData.btn.components.X == 1)){ 43 | userCmd = UserCommand::L1_X; 44 | } 45 | else if(((int)_keyData.btn.components.L1 == 1) && 46 | ((int)_keyData.btn.components.A == 1)){ 47 | userCmd = UserCommand::L1_A; 48 | } 49 | else if(((int)_keyData.btn.components.L1 == 1) && 50 | ((int)_keyData.btn.components.Y == 1)){ 51 | userCmd = UserCommand::L1_Y; 52 | } 53 | else if((int)_keyData.btn.components.start == 1){ 54 | userCmd = UserCommand::START; 55 | } 56 | 57 | userValue.L2 = killZeroOffset(_keyData.L2, 0.08); 58 | userValue.lx = killZeroOffset(_keyData.lx, 0.08); 59 | userValue.ly = killZeroOffset(_keyData.ly, 0.08); 60 | userValue.rx = killZeroOffset(_keyData.rx, 0.08); 61 | userValue.ry = killZeroOffset(_keyData.ry, 0.08); 62 | } 63 | 64 | 65 | #endif // COMPILE_WITH_REAL_ROBOT 66 | -------------------------------------------------------------------------------- /unitree_guide/src/main.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************** 2 | Copyright (c) 2020-2023, Unitree Robotics.Co.Ltd. All rights reserved. 3 | ***********************************************************************/ 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include "control/ControlFrame.h" 10 | #include "control/CtrlComponents.h" 11 | #include "Gait/WaveGenerator.h" 12 | #include "control/BalanceCtrl.h" 13 | 14 | #ifdef COMPILE_WITH_REAL_ROBOT 15 | #include "interface/IOSDK.h" 16 | #endif // COMPILE_WITH_REAL_ROBOT 17 | 18 | #ifdef COMPILE_WITH_ROS 19 | #include "interface/KeyBoard.h" 20 | #include "interface/IOROS.h" 21 | #endif // COMPILE_WITH_ROS 22 | 23 | bool running = true; 24 | 25 | // over watch the ctrl+c command 26 | void ShutDown(int sig) 27 | { 28 | std::cout << "stop the controller" << std::endl; 29 | running = false; 30 | } 31 | 32 | void setProcessScheduler() 33 | { 34 | pid_t pid = getpid(); 35 | sched_param param; 36 | param.sched_priority = sched_get_priority_max(SCHED_FIFO); 37 | if (sched_setscheduler(pid, SCHED_FIFO, ¶m) == -1) 38 | { 39 | std::cout << "[ERROR] Function setProcessScheduler failed." << std::endl; 40 | } 41 | } 42 | 43 | int main(int argc, char **argv) 44 | { 45 | /* set real-time process */ 46 | setProcessScheduler(); 47 | /* set the print format */ 48 | std::cout << std::fixed << std::setprecision(3); 49 | 50 | #ifdef RUN_ROS 51 | ros::init(argc, argv, "unitree_gazebo_servo"); 52 | #endif // RUN_ROS 53 | 54 | IOInterface *ioInter; 55 | CtrlPlatform ctrlPlat; 56 | 57 | #ifdef COMPILE_WITH_SIMULATION 58 | ioInter = new IOROS(); 59 | ctrlPlat = CtrlPlatform::GAZEBO; 60 | #endif // COMPILE_WITH_SIMULATION 61 | 62 | #ifdef COMPILE_WITH_REAL_ROBOT 63 | ioInter = new IOSDK(); 64 | ctrlPlat = CtrlPlatform::REALROBOT; 65 | #endif // COMPILE_WITH_REAL_ROBOT 66 | 67 | CtrlComponents *ctrlComp = new CtrlComponents(ioInter); 68 | ctrlComp->ctrlPlatform = ctrlPlat; 69 | ctrlComp->dt = 0.002; // run at 500hz 70 | ctrlComp->running = &running; 71 | 72 | #ifdef ROBOT_TYPE_A1 73 | ctrlComp->robotModel = new A1Robot(); 74 | #endif 75 | #ifdef ROBOT_TYPE_Go1 76 | ctrlComp->robotModel = new Go1Robot(); 77 | #endif 78 | 79 | ctrlComp->waveGen = new WaveGenerator(0.45, 0.5, Vec4(0, 0.5, 0.5, 0)); // Trot 80 | // ctrlComp->waveGen = new WaveGenerator(1.1, 0.75, Vec4(0, 0.25, 0.5, 0.75)); //Crawl, only for sim 81 | // ctrlComp->waveGen = new WaveGenerator(0.4, 0.6, Vec4(0, 0.5, 0.5, 0)); //Walking Trot, only for sim 82 | // ctrlComp->waveGen = new WaveGenerator(0.4, 0.35, Vec4(0, 0.5, 0.5, 0)); //Running Trot, only for sim 83 | // ctrlComp->waveGen = new WaveGenerator(0.4, 0.7, Vec4(0, 0, 0, 0)); //Pronk, only for sim 84 | 85 | ctrlComp->geneObj(); 86 | 87 | ControlFrame ctrlFrame(ctrlComp); 88 | 89 | signal(SIGINT, ShutDown); 90 | 91 | while (running) 92 | { 93 | ctrlFrame.run(); 94 | } 95 | 96 | delete ctrlComp; 97 | return 0; 98 | } 99 | -------------------------------------------------------------------------------- /unitree_guide/src/quadProgpp/Array.cc: -------------------------------------------------------------------------------- 1 | // $Id: Array.cc 201 2008-05-18 19:47:38Z digasper $ 2 | // This file is part of QuadProg++: 3 | // Copyright (C) 2006--2009 Luca Di Gaspero. 4 | // 5 | // This software may be modified and distributed under the terms 6 | // of the MIT license. See the LICENSE file for details. 7 | 8 | #include "thirdParty/quadProgpp/Array.hh" 9 | 10 | /** 11 | Index utilities 12 | */ 13 | 14 | namespace quadprogpp { 15 | 16 | std::set seq(unsigned int s, unsigned int e) 17 | { 18 | std::set tmp; 19 | for (unsigned int i = s; i <= e; i++) 20 | tmp.insert(i); 21 | 22 | return tmp; 23 | } 24 | 25 | std::set singleton(unsigned int i) 26 | { 27 | std::set tmp; 28 | tmp.insert(i); 29 | 30 | return tmp; 31 | } 32 | 33 | } // namespace quadprogpp 34 | -------------------------------------------------------------------------------- /unitree_move_base/.gitignore: -------------------------------------------------------------------------------- 1 | .vscode/ -------------------------------------------------------------------------------- /unitree_move_base/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(unitree_move_base) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | geometry_msgs 6 | move_base_msgs 7 | roscpp 8 | rospy 9 | tf 10 | visualization_msgs 11 | ) 12 | 13 | catkin_package() 14 | 15 | include_directories( 16 | ${catkin_INCLUDE_DIRS} 17 | ) 18 | -------------------------------------------------------------------------------- /unitree_move_base/config/base_local_planner_params.yaml: -------------------------------------------------------------------------------- 1 | recovery_behavior_enabled: true 2 | clearing_rotation_allowed: true 3 | 4 | controller_frequency: 10.0 5 | 6 | TrajectoryPlannerROS: 7 | max_vel_x: 0.5 8 | min_vel_x: 0.3 9 | max_vel_y: 0.5 10 | min_vel_y: 0.3 11 | max_vel_theta: 1.2 12 | min_vel_theta: -1.2 13 | min_in_place_vel_theta: 0.8 14 | escape_vel: -0.3 15 | acc_lim_x: 0.5 16 | acc_lim_y: 0.5 17 | acc_lim_theta: 0.8 18 | holonomic_robot: true 19 | y_vels: [-0.2, -0.1, 0.1, 0.2] 20 | 21 | yaw_goal_tolerance: 0.8 # about 45.8 degrees 22 | xy_goal_tolerance: 0.4 # 40 cm 23 | latch_xy_goal_tolerance: true ## 24 | 25 | meter_scoring: true 26 | 27 | pdist_scale: 1.4 28 | gdist_scale: 2.8 29 | 30 | heading_scoring: true ## 31 | heading_lookahead: 0.8 32 | heading_scoring_timestep: 2.8 ## 33 | planner_patience: 5.0 34 | oscillation_reset_dist: 0.05 35 | publish_cost_grid_pc: false 36 | prune_plan: true 37 | 38 | controller_frequency: 10.0 39 | sim_time: 3.0 40 | sim_granularity: 0.025 41 | angular_sim_granularity: 0.025 42 | vx_samples: 12 43 | vy_samples: 12 44 | vtheta_samples: 100 45 | dwa: true 46 | path_distance_bias: 32 47 | goal_distance_bias: 24 48 | occdist_scale: 0.01 49 | simple_attractor: false 50 | -------------------------------------------------------------------------------- /unitree_move_base/config/costmap_common_params.yaml: -------------------------------------------------------------------------------- 1 | obstacle_range: 1.0 ## 2 | raytrace_range: 1.5 ## 3 | footprint: [[0.3, 0.15], [0.3, -0.15], [-0.35, -0.15], [-0.35, 0.15]] 4 | # robot_radius: 0.3 5 | inflation_radius: 0.03 6 | max_obstacle_height: 1.0 7 | min_obstacle_height: 0.0 8 | # observation_sources: scan 9 | # scan: {data_type: LaserScan, topic: /merged_laserscan, marking: true, clearing: true, expected_update_rate: 3.3} 10 | observation_sources: faceLaserScan leftLaserScan rightLaserScan 11 | faceLaserScan: {data_type: LaserScan, topic: /faceLaserScan, marking: true, clearing: true, expected_update_rate: 3.3} 12 | leftLaserScan: {data_type: LaserScan, topic: /leftLaserScan, marking: true, clearing: true, expected_update_rate: 3.3} 13 | rightLaserScan: {data_type: LaserScan, topic: /rightLaserScan, marking: true, clearing: true, expected_update_rate: 3.3} 14 | -------------------------------------------------------------------------------- /unitree_move_base/config/global_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | global_costmap: 2 | global_frame: odom ## 3 | robot_base_frame: base 4 | update_frequency: 5.0 ## 5 | publish_frequency: 3.0 6 | static_map: false ## 7 | rolling_window: true ## 8 | resolution: 0.05 9 | transform_tolerance: 1.0 10 | map_type: costmap -------------------------------------------------------------------------------- /unitree_move_base/config/local_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | local_costmap: 2 | global_frame: odom 3 | robot_base_frame: base 4 | update_frequency: 5.0 5 | publish_frequency: 3.0 6 | static_map: false 7 | rolling_window: true 8 | width: 3.0 9 | height: 3.0 10 | resolution: 0.05 11 | transform_tolerance: 1.0 12 | map_type: costmap -------------------------------------------------------------------------------- /unitree_move_base/config/pointCloud_to_laserScan_params.yaml: -------------------------------------------------------------------------------- 1 | transform_tolerance: 0.01 2 | min_height: -0.05 3 | max_height: 0.1 4 | 5 | angle_min: -0.087 6 | angle_max: 0.087 7 | angle_increment: 0.0175 8 | scan_time: 0.3333 9 | range_min: 0.1 10 | range_max: 1.5 11 | use_inf: true 12 | 13 | # Concurrency level, affects number of pointclouds queued for processing and number of threads used 14 | # 0 : Detect number of cores 15 | # 1 : Single threaded 16 | # 2->inf : Parallelism level 17 | concurrency_level: 1 -------------------------------------------------------------------------------- /unitree_move_base/launch/gazebo_move_base.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 28 | 29 | 30 | 31 | 33 | 34 | 35 | 36 | 37 | 38 | 44 | 45 | 46 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /unitree_move_base/launch/move_base.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /unitree_move_base/launch/pointCloud2LaserScan.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | 7 | 8 | target_frame: camera_face 9 | transform_tolerance: 0.01 10 | min_height: -0.05 11 | max_height: 0.1 12 | 13 | angle_min: -0.087 14 | angle_max: 0.087 15 | angle_increment: 0.0175 16 | scan_time: 0.3333 17 | range_min: 0.1 18 | range_max: 1.5 19 | use_inf: true 20 | 21 | # Concurrency level, affects number of pointclouds queued for processing and number of threads used 22 | # 0 : Detect number of cores 23 | # 1 : Single threaded 24 | # 2->inf : Parallelism level 25 | concurrency_level: 1 26 | 27 | 28 | 29 | 30 | 32 | 33 | 34 | 35 | target_frame: camera_laserscan_link_left 36 | transform_tolerance: 0.01 37 | min_height: -0.05 38 | max_height: 0.1 39 | 40 | angle_min: -0.087 41 | angle_max: 0.087 42 | angle_increment: 0.0175 43 | scan_time: 0.3333 44 | range_min: 0.1 45 | range_max: 1.5 46 | use_inf: true 47 | 48 | # Concurrency level, affects number of pointclouds queued for processing and number of threads used 49 | # 0 : Detect number of cores 50 | # 1 : Single threaded 51 | # 2->inf : Parallelism level 52 | concurrency_level: 1 53 | 54 | 55 | 56 | 57 | 59 | 60 | 61 | 62 | target_frame: camera_laserscan_link_right 63 | transform_tolerance: 0.01 64 | min_height: -0.05 65 | max_height: 0.1 66 | 67 | angle_min: -0.087 68 | angle_max: 0.087 69 | angle_increment: 0.0175 70 | scan_time: 0.3333 71 | range_min: 0.1 72 | range_max: 1.5 73 | use_inf: true 74 | 75 | # Concurrency level, affects number of pointclouds queued for processing and number of threads used 76 | # 0 : Detect number of cores 77 | # 1 : Single threaded 78 | # 2->inf : Parallelism level 79 | concurrency_level: 1 80 | 81 | 82 | 83 | 84 | -------------------------------------------------------------------------------- /unitree_move_base/launch/robotTF.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 6 | 7 | 8 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /unitree_move_base/launch/rvizMoveBase.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /unitree_move_base/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | unitree_move_base 4 | 0.0.0 5 | The unitree_move_base package. Just an naive example for move_base in simulation and real robot 6 | 7 | unitree 8 | TODO 9 | 10 | catkin 11 | 12 | controller_manager 13 | joint_state_controller 14 | robot_state_publisher 15 | roscpp 16 | std_msgs 17 | controller_manager 18 | joint_state_controller 19 | robot_state_publisher 20 | roscpp 21 | std_msgs 22 | controller_manager 23 | joint_state_controller 24 | robot_state_publisher 25 | roscpp 26 | std_msgs 27 | unitree_legged_msgs 28 | 29 | geometry_msgs 30 | move_base_msgs 31 | tf 32 | visualization_msgs 33 | geometry_msgs 34 | move_base_msgs 35 | tf 36 | visualization_msgs 37 | 38 | 39 | 40 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /unitree_move_base/worlds/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | building 4 | 1.0 5 | model.sdf 6 | 7 | 8 | 9 | 10 | 11 | 12 | --------------------------------------------------------------------------------