├── .gitignore ├── .gitmodules ├── CMakeLists.txt ├── LICENSE ├── README.md ├── README_ZH.md ├── doc ├── MotionControlFlow.png ├── MotionControlFlowEN.png ├── body.png ├── body_length.png ├── demoFlow.png ├── demoFlowEN.png ├── leg.png ├── leg_length.png └── testFlowEN.png ├── example ├── CMakeLists.txt ├── main.cpp ├── motion_sdk_example.cpp └── motion_sdk_example.h ├── include ├── base_socket.hpp ├── command.h ├── command_list.h ├── parse_cmd.h ├── send_to_robot.h ├── udp_server.hpp ├── udp_socket.hpp └── x30_types.h ├── python ├── CMakeLists.txt ├── __pycache__ │ └── motion_example.cpython-38.pyc ├── motion_sdk_example.py └── motion_sdk_pybind.cpp └── src ├── command.cpp ├── command_list.cpp ├── parse_cmd.cpp └── send_to_robot.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | build 2 | *.so 3 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "pybind11"] 2 | path = python/pybind11 3 | url = https://github.com/pybind/pybind11.git -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | 3 | set(ROBOT_NAME "x30") 4 | project(${ROBOT_NAME}_motion_sdk) 5 | # SET(CMAKE_C_COMPILER "aarch64-linux-gnu-gcc") 6 | # SET(CMAKE_CXX_COMPILER "aarch64-linux-gnu-g++") 7 | set(CMAKE_BUILD_TYPE Release) 8 | set(SRC_DIR_LIST "." src) 9 | add_definitions(-w) # warning ignore 10 | set(CMAKE_CXX_FLAGS "-std=c++11 ${CAMKE_CXX_FLAGS}") 11 | message("CMAKE_SYSTEM_PROCESSOR: ${CMAKE_SYSTEM_PROCESSOR}") 12 | 13 | option(BUILD_EXAMPLE "Example for motion sdk in C++" ON) 14 | option(BUILD_PYTHON "Motion sdk python version" ON) 15 | 16 | foreach(VAR ${SRC_DIR_LIST}) 17 | set(TEMP) 18 | aux_source_directory(./src/ TEMP) 19 | set(SRC_LIST ${RCS_SRC_LIST} ${TEMP}) 20 | endforeach(VAR) 21 | 22 | include_directories( 23 | ./include/ 24 | ) 25 | 26 | 27 | add_library(deeprobotics_${ROBOT_NAME}_motion_sdk_${CMAKE_SYSTEM_PROCESSOR} SHARED ${SRC_LIST}) 28 | target_link_libraries(deeprobotics_${ROBOT_NAME}_motion_sdk_${CMAKE_SYSTEM_PROCESSOR} -lpthread -lm -lrt -ldl -lstdc++) 29 | 30 | add_custom_command(TARGET deeprobotics_${ROBOT_NAME}_motion_sdk_${CMAKE_SYSTEM_PROCESSOR} POST_BUILD 31 | COMMAND ${CMAKE_COMMAND} -E copy $ ${CMAKE_BINARY_DIR}/../python/lib/ 32 | COMMENT "Copying deeprobotics_${ROBOT_NAME}_motion_sdk_${CMAKE_SYSTEM_PROCESSOR} to python/lib" 33 | ) 34 | 35 | 36 | 37 | if(BUILD_EXAMPLE) 38 | add_subdirectory(example) 39 | endif() 40 | 41 | if(BUILD_PYTHON) 42 | add_subdirectory(python) 43 | endif() -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2024 DeepRoboticsLab 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Jueying X30 Motion SDK 2 | 3 | [简体中文](./README_ZH.md) 4 | 5 |   6 | ## 1 SDK Change Log 7 | 8 | ### V1.0 (2024-01-20) 9 | First release. 10 | 11 | ### V2.0 (2024-03-29) 12 | Add python version. 13 | 14 |   15 | ## 2 SDK Introduction 16 | **MotionSDK** is used for motion control algorithms development, supports C++ and Python. It provides five control parameters to control the motion of joints: $pos_{goal}$, $vel_{goal}$, $kp$, $kd$, $t_{ff}$. 17 | 18 | When SDK sends joint commands to the robot, the underlying controller will run the joint commands from SDK with priority and distribute the commands to the 12 joints of the robot. The final control signal for the joints can be calculated based on the five control parameters: 19 | $$T=kp*(pos_{goal} - pos_{real})+kd*(vel_{goal} - vel_{real})+t_{ff}$$ 20 | 21 | The driver will convert final joint control signal into a desired current and perform closed-loop control at a frequency of 20kHz. 22 | 23 | When the underlying controller has not received the commands from SDK for more than 1 second, it will retrieve control right, enter a damping protection mode, and after a certain period, clear the joint commands. 24 | 25 | **Example of control parameters for various control methods:** 26 | 27 | Position Control: 28 | $$pos_{goal}=3.14, vel_{goal}=0, kp=30, kd=0, t_{ff} = 0$$ 29 | Velocity Control: 30 | $$pos_{goal}=0, vel_{goal}=5, kp=0, kd=1, t_{ff} = 0$$ 31 | Damping Control: 32 | $$pos_{goal}=0, vel_{goal}=0, kp=0, kd=1, t_{ff} = 0$$ 33 | Torque Control: 34 | $$pos_{goal}=0, vel_{goal}=0, kp=0, kd=0, t_{ff} = 3$$ 35 | Zero Torque Control: 36 | $$pos_{goal}=0, vel_{goal}=0, kp=0, kd=0, t_{ff} = 0$$ 37 | Hybrid Control: 38 | $$pos_{goal}=3.14, vel_{goal}=0, kp=30, kd=1, t_{ff} = 1$$ 39 | 40 |   41 | ## 3 Parameters 42 | 43 | ### 3.1 Body and Joint Coordinate System 44 | 45 | a 46 | 47 | a 48 | 49 | > Caution: The arc-shaped arrow indicates the positive direction of rotation for the joint coordinate system with the same color. 50 | 51 | ### 3.2 Link Lengths of Body 52 | 53 | a 54 | 55 | | Parameter | Value | Description | 56 | | ----------| -------- | -------------------- | 57 | | Lbody| 0.98m | length of body | 58 | | Lhip | 0.582m | distance between the centers of front hip and hind hip | 59 | | Whip | 0.16m | distance between the centers of left hip and right hip | 60 | | Wleg | 0.3935m| distance between the centers of left leg and right leg | 61 | | Wbody| 0.454m | width of body| 62 | 63 | ### 3.3 Link Lengths of Leg 64 | 65 | s 66 | 67 | | Parameter | Value | Description | 68 | | ---------------------- | -------- | ------------------------------ | 69 | | L1 | 0.1167m | distance between the centers of leg and HipX | 70 | | L2 | 0.3m | length of upper leg | 71 | | L3 | 0.31m | length of lower leg | 72 | | Rfoot | 0.039m | foot radius | 73 | 74 | ### 3.4 Joint Parameters 75 | 76 | | **Joint** | **Range** | **Nominal Torque** | **Nominal Velocity** | **Max Torque** | 77 | | ------------ | ------------ | ------------ | ------------ | ------------ | 78 | | HipX | -18.5°~33.5° | 28N·m | 12rad/s | 84N·m | 79 | | HipY | -170°~15° | 28N·m | 12rad/s | 84N·m | 80 | | Knee | 24°~140° | 65N·m | 11rad/s | 180N·m | 81 | 82 | > Caution: Additional dynamics parameters for Jueying X30 are available in the URDF file provided. 83 | 84 |   85 | ## 4 SDK Download 86 | 87 | Clone **x30_motion_sdk** repository to local host: 88 | ```bash 89 | cd xxxxxxxxxx #cd 90 | git clone --recurse-submodules https://github.com/DeepRoboticsLab/x30_motion_sdk.git 91 | ``` 92 | 93 |   94 | ## 5 Configure the Motion Host 95 | 96 | The developer can remotely connect to the motion host via ssh to configure the destination IP address for the motion host to send motion data such as joint data, and SDK-related parameters. 97 | 98 | The default port number for SDK to receive data reported by the motion host is 43897. The port number for the C++ project can be modified in ***/include/parse_cmd.h*** , and for the Python project, it can be modified in ***/python/motion_sdk_example.py*** . 99 | 100 | - Connect the development host to the robot's WiFi. 101 | 102 | - Open the ssh connection software on the development host and enter`ssh ysc@192.168.1.103`, with the password `'` [a single quote], to connect to the motion host remotely. 103 | - Enter the following command to open the network config file: 104 | ```Bash 105 | cd ~/jy_exe/conf/ 106 | vim network.toml 107 | ``` 108 | - In the config file ***network.toml*** , `ip` and `target_port` form a pair consisting of an IP address and a port number. The addresses in the `ips` correspond to the port numbers in `ports` in sequential order. The specific contents of the file are as follows: 109 | ```toml 110 | ip = '192.168.1.103' 111 | target_port = 43897 112 | local_port = 43893 113 | ips = ["192.168.1.105","192.168.1.106","192.168.1.xxx"] 114 | ports = [43897,43897,43897] 115 | ``` 116 | 117 | - When running, motion host will send joint data to the addresses contained in the `ip` and `ips` of the configuration file, along with their corresponding port numbers. 118 | 119 | - If **MotionSDK** is running within the motion host, please ensure that `ip` or `ips` already includes the motion host IP `192.168.1.103`, along with the corresponding port number matching the port number where the program is receiving data. 120 | 121 | - If **MotionSDK** is running on the developer's own development host, please add the static IP of the development host to `ips` as `192.168.1.xxx`, and add the port number of the program receiving data at the corresponding position in `ports`. 122 | 123 | - After the data reporting address is configured, you need to turn on the switch for the motion host to report data to the sdk, first open the config file ***sdk_config.toml***: 124 | ```Bash 125 | cd ~/jy_exe/conf/ 126 | vim sdk_config.toml 127 | ``` 128 | 129 | - Modify the value of `enable_joint_data` in the config file ***sdk_config.toml*** (If this file does not exist, please create it): 130 | ```toml 131 | enable_joint_data = true 132 | ``` 133 | - In the same config file ***sdk_config.toml***, the joint torque limits can be modified as required: 134 | ```toml 135 | torque_limit=[42.0,42.0,90.0] 136 | ``` 137 | The array elements are defined as follows: 138 | 139 | | Array Element | Meaning | Range |Type| 140 | | --- | -------- | ---------- | ---------- | 141 | | torque_limit[0] | hipx torque limit(N·m)| (0,84.0] | double | 142 | | torque_limit[1] | hipy torque limit(N·m)| (0,84.0] | double | 143 | | torque_limit[2] | knee torque limit(N·m)| (0,160.0] | double | 144 | 145 | >Caution: When initially testing a new motion program, it is advisable to set the torque limiting to a lower level. Once the program has been verified without errors, the torque limiting can be gradually increased to ensure the robot remains in a safe state. 146 | 147 | - Restart the motion program to apply the new configuration: 148 | ```bash 149 | cd ~/jy_exe 150 | sudo ./stop.sh 151 | sudo ./restart.sh 152 | ``` 153 | 154 |   155 | ## 6 Compile and Run 156 | 157 | The example codes ***/example/main.cpp*** (C++ version) and ***/python/motion_example.py*** (Python version) provide a simple demo of standing up, and after standing for a while, it returns control right to the underlying controller, and the robot automatically enters damping protection mode. 158 | 159 | a 160 | 161 | **But to ensure the safe use of the SDK, in the original code of the two demos, the code for sending joint control commands is commented out:** 162 | 163 | C++ ( */example/main.cpp* ): 164 | ```c++ 165 | // if(is_message_updated_){ 166 | // send2robot_cmd->set_send(robot_joint_cmd); 167 | // } 168 | ``` 169 | Python ( */python/motion_sdk_example.py* ): 170 | ```python 171 | # sender.set_send(robot_joint_cmd) 172 | ``` 173 | **Caution: Before uncommenting, the developer must make sure that the communication between SDK and the robot is functioning properly (refer to "6.1 Check the Communication"), and make sure that the joint control commands sent by SDK are correct, to avoid posing a risk when executing the control commands!** 174 | 175 | **Caution: When using Jueying X30 to test your motion control algorithms or do experiments, all present personnel should keep at least 5 meters away from the robot and the robot should be hung on the robot hoisting device, to avoid accidental damage to personnel and equipment. If the user needs to approach the robot during experiment, the user must ensure that either the robot is put into an emergency stop state or the motion program is shut down using the `sudo ./stop.sh`.** 176 | 177 | ### 6.1 Check the Communication 178 | 179 | MotionSDK uses UDP to communicate with the robot. 180 | 181 | To check if the robot has successfully sent data to the SDK, developers can print data such as joint data or imu data using the SDK to determine whether the SDK received the data sent by the robot, or observe whether "No data from the robot was received!!!!!!" is printed when running the demo. 182 | 183 | Refer to 6.3 to compile and run the original example codes and observe whether it is normal to print the data sent by the robot in the terminal. 184 | 185 | ### 6.2 Communication Troubleshooting 186 | 187 | If SDK does not receive the data sent by the robot, you can follow the steps below to troubleshoot: 188 | 189 | - First check if the development host is under the same network segment as the robot host (this step can be skipped if you are running the SDK on the robot motion host): 190 | 191 | - Connect the development host to the robot WiFi or Ethernet port, and then ping the motion host on the development host. 192 | 193 | - If the ping shows the response, use SSH to connect to the robot motion host, and ping the static IP address of the development host from the motion host. 194 | 195 | - If there is no reply, try to manually set the IP address of your development host and follow Chapter 5 again to configure the motion host. 196 | 197 | - If the development host is a virtual machine, it is recommended to configure bridged networking for your virtual machine, manually reset its IP address and reboot it. Then follow Chapter 5 again to configure the motion host. 198 | 199 | If SDK still can't receive the data sent by the robot, you can capture the packet on the motion host: 200 | 201 | - If **MotionSDK** is running on the robot motion host, run `sudo tcpdump -x port 43897 -i lo`; 202 | 203 | - If **MotionSDK** is running on development host, run `sudo tcpdump -x port 43897 -i eth1`. 204 | 205 | Wait for 2 minutes after entering the packet capture command,and observe whether the robot has sent raw data to SDK. If not, enter the top command to see if the process *jy_exe* (robot motion program)is running normally. If *jy_exe* is not running normally, refer to the following command to restart the motion program: 206 | ```bash 207 | cd ~/jy_exe 208 | sudo ./stop.sh 209 | sudo ./restart.sh 210 | ``` 211 | ### 6.3 Compile and Develop 212 | 213 | After ensuring that the SDK communicates properly with the robot and confirming that your control commands are correct, you can uncomment the code for sending commands to the robot in the original code, then recompile and run it. 214 | 215 | #### 6.3.1 C++ 216 | Open a new terminal and create an empty ***build*** directory (Or empty all contents in the ***build*** directory if it has already been created): 217 | ```bash 218 | cd xxxxxxxx # cd 219 | mkdir build 220 | ``` 221 | 222 | Navigate to the ***build*** directory and then compile: 223 | ```bash 224 | cd build 225 | cmake .. -DBUILD_EXAMPLE=ON 226 | make -j 227 | ``` 228 | 229 | After compilation, an executable file named ***motion_sdk_example*** is generated in the ***/build/example*** directory. Enter the following codes in terminal to run the program: 230 | ```bash 231 | ./example/motion_sdk_example 232 | ``` 233 | 234 | #### 6.3.2 Python 235 | SDK for Python programmers uses Pybind to create Python bindings for C++ libraries. 236 | If you have compiled the C++ example codes, you can directly navigate to the existing ***build*** directory. If you have never compiled the example codes, please create a new ***build*** directory first: 237 | ```bash 238 | cd xxxxxxxx # cd 239 | mkdir build 240 | ``` 241 | 242 | Navigate to the ***build*** directory and then compile: 243 | ```bash 244 | cd build 245 | cmake .. -DBUILD_PYTHON=ON 246 | make -j 247 | ``` 248 | 249 | Normally, the compiled dynamic library files will be automatically copied to the ***/python/lib*** directory. Then you can navigate to the ***/x30_motion_sdk/python*** directory and directly execute ***motion_sdk_example.py***: 250 | ```bash 251 | cd /python 252 | python motion_sdk_example.py 253 | ``` 254 | 255 |   256 | ## 7 Example Code (C++) 257 | 258 | This chapter explains ***/example/main.cpp*** . 259 | 260 | Timer, used to set the algorithm period and obtain the current time: 261 | 262 | ```cpp 263 | TimeTool my_set_timer; 264 | my_set_timer.time_init(int); ///< Timer initialization, input: cycle; unit: ms 265 | my_set_timer.get_start_time(); ///< Obtain time for algorithm 266 | my_set_timer.time_interrupt() ///< Timer interrupt flag 267 | my_set_timer.get_now_time(double); ///< Get the current time 268 | ``` 269 | 270 | After binding the IP and port of the robot, SDK will acquire control right and can send the joint control commands: 271 | 272 | ```cpp 273 | SendToRobot* send2robot_cmd = new SendToRobot("192.168.1.103",43893); ///< Create a sender thread 274 | send2robot_cmd->robot_state_init(); ///< Reset all joints to zero and gain control right 275 | send2robot_cmd->set_send(RobotCmdSDK); ///< Send joint control command 276 | send2robot_cmd->control_get(int); ///< Return the control right 277 | ``` 278 | 279 | SDK receives the joint data from the robot: 280 | 281 | ```cpp 282 | ParseCommand* robot_data_rec = new ParseCommand; ///< Create a thread for receiving and parsing 283 | robot_data_rec->getRecvState(); ///< Receive data from 12 joints 284 | ``` 285 | 286 | The data SDK received will be saved into `robot_data`: 287 | 288 | ```cpp 289 | RobotDataSDK *robot_data = &robot_data_rec->getRecvState(); ///< Saving joint data to the robot_data 290 | ///< Left front leg：fl_leg[3], the sequence is fl_hipx, fl_Hipy, fl_knee 291 | ///< Right front leg：fr_leg[3], the sequence is fr_hipx, fr_Hipy, fr_knee 292 | ///< Left hind leg：hl_leg[3], the sequence is hl_hipx, hl_Hipy, hl_knee 293 | ///< Right hind leg：hr_leg[3], the sequence is hr_hipx, hr_Hipy, hr_knee 294 | ///< All joints：leg_force[12]/joint_data[12], the sequence is fl_hipx, fl_hipy, fl_knee, fr_hipx, fr_Hipy, fr_knee, hl_hipx, hl_hipy, hl_knee, hr_hipx, hr_hipy, hr_knee 295 | 296 | robot_data->fl_force[] ///< Contact force on left front foot in X-axis, Y-axis and Z-axis 297 | robot_data->fr_force[] ///< Contact force on right front foot in X-axis, Y-axis and Z-axis 298 | robot_data->hl_force[] ///< Contact force on left hind foot in X-axis, Y-axis and Z-axis 299 | robot_data->hr_force[] ///< Contact force on right hind foot in X-axis, Y-axis and Z-axis 300 | robot_data->contact_force[] ///< Contact force on all feet 301 | 302 | robot_data->tick ///< Cycle of operation 303 | 304 | robot_data->imu ///< IMU data 305 | robot_data->imu.acc_x ///< Acceleration on X-axis, unit m/s^2 306 | robot_data->imu.acc_y ///< Acceleration on Y-axis, unit m/s^2 307 | robot_data->imu.acc_z ///< Acceleration on Z-axis, unit m/s^2 308 | robot_data->imu.roll ///< Roll angle, unit deg 309 | robot_data->imu.pitch ///< Pitch angle, unit deg 310 | robot_data->imu.yaw ///< Yaw angle, unit deg 311 | robot_data->imu.omega_x ///< angular velocity on X-axis, unit rad/s 312 | robot_data->imu.omega_y ///< angular velocity on Y-axis, unit rad/s 313 | robot_data->imu.omega_z ///< angular velocity on Z-axis, unit rad/s 314 | robot_data->imu.buffer_byte ///< Buffer data 315 | robot_data->imu.buffer_float ///< Buffer data 316 | robot_data->imu.timestamp ///< Time when the data is obtained 317 | 318 | robot_data->joint_state ///< Motor status 319 | robot_data->joint_state.fl_leg[].position ///< Motor position of left front leg 320 | robot_data->joint_state.fl_leg[].temperature ///< Motor temperature of left front leg 321 | robot_data->joint_state.fl_leg[].torque ///< Motor torque of left front leg 322 | robot_data->joint_state.fl_leg[].velocity ///< Motor velocity of left front leg 323 | robot_data->joint_state.joint_data ///< All joint data 324 | ``` 325 | 326 | Robot joint control command: 327 | 328 | ```cpp 329 | RobotCmdSDK robot_joint_cmd; ///< Target data of each joint 330 | ///< Left front leg：fl_leg[3], the sequence is fl_hipx, fl_Hipy, fl_knee 331 | ///< Right front leg：fr_leg[3], the sequence is fr_hipx, fr_Hipy, fr_knee 332 | ///< Left hind leg：hl_leg[3], the sequence is hl_hipx, hl_Hipy, hl_knee 333 | ///< Right hind leg：hr_leg[3], the sequence is hr_hipx, hr_Hipy, hr_knee 334 | ///< All joints：leg_force[12]/joint_data[12], the sequence is fl_hipx, fl_hipy, fl_knee, fr_hipx, fr_Hipy, fr_knee, hl_hipx, hl_hipy, hl_knee, hr_hipx, hr_hipy, hr_knee 335 | 336 | robot_joint_cmd.fl_leg[]->kd; ///< Kd of left front leg 337 | robot_joint_cmd.fl_leg[]->kp; ///< Kp of left front leg 338 | robot_joint_cmd.fl_leg[]->position; ///< Position of left front leg 339 | robot_joint_cmd.fl_leg[]->torque; ///< Torue of left front leg 340 | robot_joint_cmd.fl_leg[]->velocity; ///< Velocity of left front leg 341 | ``` 342 | 343 | A simple demo that can make the robot stand: 344 | 345 | 1.**PreStanUp**: Draw the robot's legs in and prepare to stand; 346 | 347 | 2.**GetInitData**: Record the current time and joint data; 348 | 349 | 3.**StandUp**: The robot stands up. 350 | 351 | ```cpp 352 | MotionSDKExample robot_set_up_demo; ///< Demo for testing 353 | 354 | /// @brief Spend 1 sec drawing the robot's legs in and preparing to stand 355 | /// @param cmd Send control command 356 | /// @param time Current timestamp 357 | /// @param data_state Real-time status data of robot 358 | robot_set_up_demo.PreStandUp(robot_joint_cmd,now_time,*robot_data); 359 | 360 | /// @brief Only the current time and angle are recorded 361 | /// @param data Current joint data 362 | /// @param time Current timestamp 363 | robot_set_up_demo.GetInitData(robot_data->motor_state,now_time); 364 | 365 | /// @brief Spend 1.5 secs standing up 366 | /// @param cmd Send control command 367 | /// @param time Current timestamp 368 | /// @param data_state Real-time status data of robot 369 | robot_set_up_demo.StandUp(robot_joint_cmd,now_time,*robot_data); 370 | ``` 371 | 372 |   373 | ### Other Precautions 374 | 375 | 1. X30 motion host is an ARM architecture and developers need to be careful if they want to run their programs on the motion host. 376 | 2. WiFi communication latency fluctuation caused by interference in the network environment may have a certain impact on controllers with control frequencies above 500Hz. 377 | -------------------------------------------------------------------------------- /README_ZH.md: -------------------------------------------------------------------------------- 1 | # 绝影X30运动控制SDK 2 | 3 | [English](./README.md) 4 | 5 |   6 | ## 1 SDK更新记录 7 | 8 | ### V1.0（2024-01-20） 9 | **[新增]** 首次发布。 10 | 11 | ### V2.0（2024-03-29） 12 | **[新增]** python版本。 13 | 14 |   15 | ## 2 SDK简介 16 | **MotionSDK** 是用于开发运动控制算法的SDK，支持C++和Python，提供了低速端（即**关节端**）的5个控制参数接口： $pos_{goal}$， $vel_{goal}$， $kp$， $kd$， $t_{ff}$。 17 | 18 | 当SDK有指令下发时，底层控制器会优先执行SDK的控制指令，将指令分发给机器狗的12个关节。底层关节根据5个控制参数可计算出最终关节目标力为： 19 | $$T=kp*(pos_{goal} - pos_{real})+kd*(vel_{goal} - vel_{real})+t_{ff}$$ 20 | 21 | 驱动器端会将最终的关节目标力转化成期望电流，并以20kHz的频率进行闭环控制。 22 | 23 | 当SDK没有指令下发超过1s时，底层控制器会拿回控制权，进入阻尼保护模式一段时间后，清空关节指令。 24 | 25 | **控制流程可以参考下图：** 26 | 27 | a 28 | 29 | **控制参数使用举例：** 30 | 31 | 当做纯位控即位置控制时，电机的输出轴将会稳定在一个固定的位置。例如，如果我们希望电机输出端固定在3.14弧度的位置，下发数据格式示例： 32 | $$pos_{goal}=3.14, vel_{goal}=0, kp=30, kd=0, t_{ff} = 0$$ 33 | 当做速度控制时，下发数据格式示例： 34 | $$pos_{goal}=0, vel_{goal}=5, kp=0, kd=1, t_{ff} = 0$$ 35 | 当做阻尼控制时，下发数据格式示例： 36 | $$pos_{goal}=0, vel_{goal}=0, kp=0, kd=1, t_{ff} = 0$$ 37 | 当做力矩控制时，下发数据格式示例： 38 | $$pos_{goal}=0, vel_{goal}=0, kp=0, kd=0, t_{ff} = 3$$ 39 | 当做零力矩控制时，下发数据格式示例： 40 | $$pos_{goal}=0, vel_{goal}=0, kp=0, kd=0, t_{ff} = 0$$ 41 | 当做混合控制时，下发数据格式示例： 42 | $$pos_{goal}=3.14, vel_{goal}=0, kp=30, kd=1, t_{ff} = 1$$ 43 | 44 |   45 | ## 3 硬件参数 46 | 47 | ### 3.1 身体与关节坐标系 48 | 49 | a 50 | 51 | a 52 | 53 | > 注意：弧形箭头指示与其颜色一致的关节坐标系的旋转正方向。 54 | 55 | ### 3.2 身体连杆参数 56 | 57 | a 58 | 59 | | 参数 | 数值 | 说明 | 60 | | -------------------- | -------- | -------------------- | 61 | | 长度(Lbody) | 0.98m | 身体总长度 | 62 | | 髋前后间距(Lhip) | 0.582m | 前后髋关节中心距离 | 63 | | 髋左右间距(Whip) | 0.16m | 左右髋关节中心的距离 | 64 | | 腿平面左右间距(Wleg) | 0.3935m | 腿平面左右的距离 | 65 | | 宽度(Wbody) | 0.454m | 身体总宽度 | 66 | 67 | ### 3.3 腿部连杆参数 68 | 69 | s 70 | 71 | | 参数 | 数值 | 说明 | 72 | | -------------------------- | -------- | ------------------------------ | 73 | | 腿平面与髋侧摆关节距离(L1) | 0.1167m | 髋侧摆关节与腿平面距离 | 74 | | 大腿长度(L2) | 0.3m | 髋前摆关节中心与膝关节中心距离 | 75 | | 小腿长度(L3) | 0.31m | 膝关节中心与足底圆心距离 | 76 | | 足底半径 | 0.039m | 足底缓冲件半径 | 77 | 78 | ### 3.4 关节参数 79 | 80 | | **关节** | **运动范围** | **额定转矩** | **额定转速** | **峰值转矩** | 81 | | ------------ | ------------ | ------------ | ------------ | ------------ | 82 | | 髋侧摆(HipX) | -18.5°~33.5° | 28Nm | 12rad/s | 84Nm | 83 | | 髋前摆(HipY) | -170°~15° | 28Nm | 12rad/s | 84Nm | 84 | | 膝关节(Knee) | 24°~140° | 65Nm | 11rad/s | 180Nm | 85 | 86 | > 注意：绝影X30四足机器人的其他动力学参数可以在提供的URDF文件中获得。 87 | 88 |   89 | ## 4 SDK包下载 90 | 91 | 使用git工具将 **x30_motion_sdk** 代码仓库克隆到本地: 92 | ```bash 93 | cd xxxxxxxxxx #cd 94 | git clone --recurse-submodules https://github.com/DeepRoboticsLab/x30_motion_sdk.git 95 | ``` 96 |   97 | ## 5 配置SDK参数和数据上报地址 98 | 99 | 开发者可通过ssh远程连接到运动主机，以配置运动主机上报关节等运动数据的目标地址和sdk相关参数。 100 | 101 | SDK接收运动主机上报的数据的端口号默认为`43897`，C++版本的端口号可以在 ***/include/parse_cmd.h*** 中修改，Python版本的端口号可以在 ***/python/motion_sdk_example.py*** 中修改。 102 | 103 | - 首先将开发主机连接到机器狗WiFi。 104 | 105 | - 在开发主机上打开ssh连接软件，输入`ssh ysc@192.168.1.103`，密码为 `'` [英文单引号]，即可远程连接运动主机。 106 | 107 | - 输入以下命令以打开网络配置文件： 108 | ```Bash 109 | cd ~/jy_exe/conf/ 110 | vim network.toml 111 | ``` 112 | 113 | - 配置文件 ***network.toml*** 中，`ip`与`target_port`为一组IP地址和端口号，`ips`中的地址和`ports`中的端口号按顺序分别对应，配置文件具体内容如下： 114 | ```toml 115 | ip = '192.168.1.103' 116 | target_port = 43897 117 | local_port = 43893 118 | ips = ["192.168.1.105","192.168.1.106","192.168.1.xxx"] 119 | ports = [43897,43897,43897] 120 | ``` 121 | 122 | - 运动程序运行时，会向配置文件中的`ip`和`ips`所包含的地址以及其对应的端口号上报运动状态数据。 123 | 124 | - 如果 **MotionSDK** 在机器狗运动主机内运行，请确保配置文件中`ip`或`ips`已包含运动主机IP `192.168.1.103`，以及对应的端口号与程序接收数据的端口号一致； 125 | 126 | - 如果 **MotionSDK** 在开发者自己的开发主机中运行，请在`ips`中添加开发主机的静态IP `192.168.1.xxx`，并在`ports`中对应的位置添加上程序接受数据的端口号。 127 | 128 | - 数据上报地址配置完成后，需要将运动主机向sdk上报数据的开关打开，首先打开配置文件 ***sdk_config.toml*** （如果没有的话，请先新建该文件）: 129 | ```Bash 130 | cd ~/jy_exe/conf/ 131 | vim sdk_config.toml 132 | ``` 133 | 134 | - 修改配置文件 ***sdk_config.toml*** 中`enable_joint_data`的值： 135 | ```toml 136 | enable_joint_data = true 137 | ``` 138 | 139 | - 在同一个配置文件 ***sdk_config.toml*** 中，可按需对关节力矩限幅进行修改： 140 | ```toml 141 | torque_limit=[42.0,42.0,90.0] 142 | ``` 143 | 数组元素定义如下： 144 | 145 | | 数组元素 | 含义 | 取值范围 |数据类型| 146 | | --- | -------- | ---------- | ---------- | 147 | | torque_limit[0] | 对髋关节hipx的力矩限幅(N·m)| (0,84.0] | double | 148 | | torque_limit[1] | 对侧摆关节hipy的力矩限幅(N·m)| (0,84.0] | double | 149 | | torque_limit[2] | 对膝关节knee的力矩限幅(N·m)| (0,160.0] | double | 150 | 151 | > 注意：在刚开始测试一个新的运动程序的时候应将力矩限幅设置得较小一些，待程序验证无误后再放开力矩限幅，确保机器狗处于安全状态。 152 | 153 | - 重启运动程序使配置生效： 154 | ```bash 155 | cd ~/jy_exe 156 | sudo ./stop.sh 157 | sudo ./restart.sh 158 | ``` 159 | 160 |   161 | ## 6 编译开发 162 | 163 | C++版例程 ***/example/main.cpp*** 和Python版例程 ***/python/motion_example.py*** 提供了机器狗站立的简单demo，并在完成站立一段时间后将控制权归还给底层控制器，进入阻尼保护模式： 164 | 165 | a 166 | 167 | **但为了确保SDK的安全使用，在两个例程的原始代码中，下发指令到机器狗的代码是被注释掉的：** 168 | 169 | C++版例程( */example/main.cpp* )： 170 | ```c++ 171 | // if(is_message_updated_){ 172 | // send2robot_cmd->set_send(robot_joint_cmd); 173 | // } 174 | ``` 175 | Python版例程( */python/motion_sdk_example.py* ): 176 | ```python 177 | # sender.set_send(robot_joint_cmd) 178 | ``` 179 | **注意：在取消注释前，开发者务必确保SDK与机器狗正常通讯（可参考“6.1 检查通讯”），并确保自己的下发控制指令正确，否则机器狗执行控制指令时可能会产生危险！** 180 | 181 | **注意：用户在使用绝影X30执行算法和实验的过程中，请与机器狗保持至少5米距离，并将机器狗悬挂在调试架上避免意外造成人员和设备损伤。若实验过程中，机器狗摔倒或者用户想搬动机器狗位置，需要靠近机器狗时，用户应当使得机器狗处于急停状态或者使用 `sudo ./stop.sh` 命令关闭运动程序。** 182 | 183 | ### 6.1 检查通讯 184 | 185 | MotionSDK采用UDP与机器狗进行通讯。 186 | 187 | 针对数据上报，可以在SDK里打印关节数据或陀螺仪数据等信息，以此判断是否收到机器狗上报的SDK数据；或者观察SDK运行时，是否打印“No data from the robot was received!!!!!!”，以此判断是否收到机器狗上报的SDK数据。 188 | 189 | 请参考6.3节对原始代码进行编译和运行，并观察程序运行过程中终端打印机器狗上报数据是否正常。 190 | 191 | ### 6.2 通讯问题排查 192 | 193 | 如果SDK未接收到机器狗上报的数据，可按照下述步骤进行排查： 194 | 195 | - 首先检查开发主机是否与机器狗主机处于同一网段下（如果是在机器狗上运行SDK，此步骤可跳过）：将开发主机连到接机器狗的WiFi网络或尾部网口，然后在开发主机上ping运动主机，ping通之后ssh连接到机器狗运动主机内，在运动主机内ping开发主机的静态IP。如果无法ping通，请尝试手动设置自己开发主机的IP地址，并再次按照第5节对配置文件进行修改。 196 | 197 | - 如果开发者的开发环境为虚拟机，建议把虚拟机网络连接方式改为桥接并手动设置虚拟机IP地址后重启虚拟机，并再次按照第5节对配置文件进行修改。 198 | 199 | 如果仍收不到机器狗上报数据，可在机器狗运动主机上抓包： 200 | 201 | - 如果 **MotionSDK** 在机器狗运动主机内运行，运行`sudo tcpdump -x port 43897 -i lo`; 202 | 203 | - 如果 **MotionSDK** 在开发者的开发主机内运行，运行`sudo tcpdump -x port 43897 -i eth1`。 204 | 205 | 执行抓包命令后等待2分钟，观察机器狗是否有原始数据上报。如果没有，输入top命令查看机器狗本体控制程序进程jy_exe是否正常运行，若jy_exe没有正常运行，参照以下指令重启运动程序： 206 | 207 | ```bash 208 | cd ~/jy_exe 209 | sudo ./stop.sh 210 | sudo ./restart.sh 211 | ``` 212 | 213 | ### 6.3 编译开发 214 | 确保SDK与机器狗正常通讯，并确保自己的下发控制指令正确后，可以将原始代码中下发指令到机器狗的代码取消注释，然后重新编译运行。 215 | 216 | #### 6.3.1 C++版本编译运行 217 | 218 | 打开一个新的终端，新建一个空的 ***build*** 文件夹（如果已经创建过 ***build*** 文件夹，可直接清空文件夹中所有内容）； 219 | ```bash 220 | cd xxxxxxxx # cd 221 | mkdir build 222 | ``` 223 | 224 | 打开 ***build*** 文件夹并编译： 225 | ```bash 226 | cd build 227 | cmake .. -DBUILD_EXAMPLE=ON 228 | make -j 229 | ``` 230 | 231 | 编译结束后，会在 ***/build/example*** 目录下生成一个名为 ***motion_sdk_example*** 的可执行文件，运行该文件时，机器狗将会执行下发的控制指令： 232 | ```bash 233 | ./example/motion_sdk_example 234 | ``` 235 | 236 | #### 6.3.2 Python版本编译运行 237 | 238 | Python版本程序采用pybind的形式生成。 239 | 240 | 若已编译过C++版本的代码，可直接进入之前创建的 ***build*** 文件夹；若是首次编译，需新建一个 ***build*** 文件夹： 241 | ```bash 242 | cd xxxxxxxx # cd 243 | mkdir build 244 | ``` 245 | 246 | 打开 ***build*** 文件夹并编译； 247 | ```bash 248 | cd build 249 | cmake .. -DBUILD_PYTHON=ON 250 | make -j 251 | ``` 252 | 253 | 正常情况下编译好的动态库文件会自动复制到 ***/python/lib*** 目录下，随后可以进入 ***/x30_motion_sdk/python*** 目录直接执行 ***motion_sdk_example.py*** 文件： 254 | ```bash 255 | cd python/ 256 | python motion_sdk_example.py 257 | ``` 258 |   259 | 260 | ## 7 示例代码(C++) 261 | 262 | 本节对 ***/example/main.cpp*** 进行说明。 263 | 264 | 定时器，用于设置算法周期，获得当前时间： 265 | 266 | ```cpp 267 | TimeTool my_set_timer; 268 | my_set_timer.time_init(int); ///< Timer initialization, input: cycle; unit: ms 269 | my_set_timer.get_start_time(); ///< Obtain time for algorithm 270 | my_set_timer.time_interrupt() ///< Timer interrupt flag 271 | my_set_timer.get_now_time(double); ///< Get the current time 272 | ``` 273 | 274 | SDK在绑定机器狗的IP和端口后，获取控制权，发送关节控制指令： 275 | 276 | ```cpp 277 | SendToRobot* send2robot_cmd = new SendToRobot("192.168.1.103",43893); ///< Create a sender thread 278 | send2robot_cmd->robot_state_init(); ///< Reset all joints to zero and gain control right 279 | send2robot_cmd->set_send(RobotCmdSDK); ///< Send joint control command 280 | send2robot_cmd->control_get(int); ///< Return the control right 281 | ``` 282 | 283 | SDK接收机器狗下发的关节数据： 284 | 285 | ```cpp 286 | ParseCommand* robot_data_rec = new ParseCommand; ///< Create a thread for receiving and parsing 287 | robot_data_rec->getRecvState(); ///< Receive data from 12 joints 288 | ``` 289 | 290 | SDK接收到的关节数据将保存在`robot_data`中： 291 | 292 | ```cpp 293 | RobotDataSDK *robot_data = &robot_data_rec->getRecvState(); ///< Saving joint data to the robot_data 294 | ///< Left front leg：fl_leg[3], the sequence is fl_hipx, fl_Hipy, fl_knee 295 | ///< Right front leg：fr_leg[3], the sequence is fr_hipx, fr_Hipy, fr_knee 296 | ///< Left hind leg：hl_leg[3], the sequence is hl_hipx, hl_Hipy, hl_knee 297 | ///< Right hind leg：hr_leg[3], the sequence is hr_hipx, hr_Hipy, hr_knee 298 | ///< All joints：leg_force[12]/joint_data[12], the sequence is fl_hipx, fl_hipy, fl_knee, fr_hipx, fr_Hipy, fr_knee, hl_hipx, hl_hipy, hl_knee, hr_hipx, hr_hipy, hr_knee 299 | 300 | robot_data->fl_force[] ///< Contact force on left front foot in X-axis, Y-axis and Z-axis 301 | robot_data->fr_force[] ///< Contact force on right front foot in X-axis, Y-axis and Z-axis 302 | robot_data->hl_force[] ///< Contact force on left hind foot in X-axis, Y-axis and Z-axis 303 | robot_data->hr_force[] ///< Contact force on right hind foot in X-axis, Y-axis and Z-axis 304 | robot_data->contact_force[] ///< Contact force on all feet 305 | 306 | robot_data->tick ///< Cycle of operation 307 | 308 | robot_data->imu ///< IMU data 309 | robot_data->imu.acc_x ///< Acceleration on X-axis, unit m/s^2 310 | robot_data->imu.acc_y ///< Acceleration on Y-axis, unit m/s^2 311 | robot_data->imu.acc_z ///< Acceleration on Z-axis, unit m/s^2 312 | robot_data->imu.roll ///< Roll angle, unit deg 313 | robot_data->imu.pitch ///< Pitch angle, unit deg 314 | robot_data->imu.yaw ///< Yaw angle, unit deg 315 | robot_data->imu.omega_x ///< angular velocity on X-axis, unit rad/s 316 | robot_data->imu.omega_y ///< angular velocity on Y-axis, unit rad/s 317 | robot_data->imu.omega_z ///< angular velocity on Z-axis, unit rad/s 318 | robot_data->imu.buffer_byte ///< Buffer data 319 | robot_data->imu.buffer_float ///< Buffer data 320 | robot_data->imu.timestamp ///< Time when the data is obtained 321 | 322 | robot_data->joint_state ///< Motor status 323 | robot_data->joint_state.fl_leg[].position ///< Motor position of left front leg 324 | robot_data->joint_state.fl_leg[].temperature ///< Motor temperature of left front leg 325 | robot_data->joint_state.fl_leg[].torque ///< Motor torque of left front leg 326 | robot_data->joint_state.fl_leg[].velocity ///< Motor velocity of left front leg 327 | robot_data->joint_state.joint_data ///< All joint data 328 | ``` 329 | 330 | 机器狗关节控制指令： 331 | 332 | ```cpp 333 | RobotCmdSDK robot_joint_cmd; ///< Target data of each joint 334 | ///< Left front leg：fl_leg[3], the sequence is fl_hipx, fl_Hipy, fl_knee 335 | ///< Right front leg：fr_leg[3], the sequence is fr_hipx, fr_Hipy, fr_knee 336 | ///< Left hind leg：hl_leg[3], the sequence is hl_hipx, hl_Hipy, hl_knee 337 | ///< Right hind leg：hr_leg[3], the sequence is hr_hipx, hr_Hipy, hr_knee 338 | ///< All joints：leg_force[12]/joint_data[12], the sequence is fl_hipx, fl_hipy, fl_knee, fr_hipx, fr_Hipy, fr_knee, hl_hipx, hl_hipy, hl_knee, hr_hipx, hr_hipy, hr_knee 339 | 340 | robot_joint_cmd.fl_leg[]->kd; ///< Kd of left front leg 341 | robot_joint_cmd.fl_leg[]->kp; ///< Kp of left front leg 342 | robot_joint_cmd.fl_leg[]->position; ///< Position of left front leg 343 | robot_joint_cmd.fl_leg[]->torque; ///< Torue of left front leg 344 | robot_joint_cmd.fl_leg[]->velocity; ///< Velocity of left front leg 345 | ``` 346 | 347 | 机器狗站立的简单demo： 348 | 1.PreStanUp：调整姿势，将机器狗的腿收起来，为站立做准备； 349 | 2.GetInitData：记录下当前时间与关节数据； 350 | 3.StandUp：机器狗起立。 351 | 352 | ```cpp 353 | MotionSDKExample robot_set_up_demo; ///< Demo for testing 354 | 355 | /// @brief Spend 1 sec drawing the robot's legs in and preparing to stand 356 | /// @param cmd Send control command 357 | /// @param time Current timestamp 358 | /// @param data_state Real-time status data of robot 359 | robot_set_up_demo.PreStandUp(robot_joint_cmd,now_time,*robot_data); 360 | 361 | /// @brief Only the current time and angle are recorded 362 | /// @param data Current joint data 363 | /// @param time Current timestamp 364 | robot_set_up_demo.GetInitData(robot_data->motor_state,now_time); 365 | 366 | /// @brief Spend 1.5 secs standing up 367 | /// @param cmd Send control command 368 | /// @param time Current timestamp 369 | /// @param data_state Real-time status data of robot 370 | robot_set_up_demo.StandUp(robot_joint_cmd,now_time,*robot_data); 371 | ``` 372 |   373 | ### 其他注意事项 374 | 1. X30运动主机是ARM架构的，如果开发者想在运动主机上编译自己的程序，需要注意。 375 | 2. WiFi通讯受网络环境干扰产生的通讯延迟波动，可能对控制频率在500Hz以上的控制器有一定影响。 376 | -------------------------------------------------------------------------------- /doc/MotionControlFlow.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DeepRoboticsLab/x30_motion_sdk/885e7f9c8bf855f57b6bf39fa73acb18aec9c28d/doc/MotionControlFlow.png -------------------------------------------------------------------------------- /doc/MotionControlFlowEN.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DeepRoboticsLab/x30_motion_sdk/885e7f9c8bf855f57b6bf39fa73acb18aec9c28d/doc/MotionControlFlowEN.png -------------------------------------------------------------------------------- /doc/body.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DeepRoboticsLab/x30_motion_sdk/885e7f9c8bf855f57b6bf39fa73acb18aec9c28d/doc/body.png -------------------------------------------------------------------------------- /doc/body_length.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DeepRoboticsLab/x30_motion_sdk/885e7f9c8bf855f57b6bf39fa73acb18aec9c28d/doc/body_length.png -------------------------------------------------------------------------------- /doc/demoFlow.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DeepRoboticsLab/x30_motion_sdk/885e7f9c8bf855f57b6bf39fa73acb18aec9c28d/doc/demoFlow.png -------------------------------------------------------------------------------- /doc/demoFlowEN.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DeepRoboticsLab/x30_motion_sdk/885e7f9c8bf855f57b6bf39fa73acb18aec9c28d/doc/demoFlowEN.png -------------------------------------------------------------------------------- /doc/leg.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DeepRoboticsLab/x30_motion_sdk/885e7f9c8bf855f57b6bf39fa73acb18aec9c28d/doc/leg.png -------------------------------------------------------------------------------- /doc/leg_length.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DeepRoboticsLab/x30_motion_sdk/885e7f9c8bf855f57b6bf39fa73acb18aec9c28d/doc/leg_length.png -------------------------------------------------------------------------------- /doc/testFlowEN.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DeepRoboticsLab/x30_motion_sdk/885e7f9c8bf855f57b6bf39fa73acb18aec9c28d/doc/testFlowEN.png -------------------------------------------------------------------------------- /example/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(motion_sdk_example) 3 | 4 | 5 | set(SOURCES 6 | motion_sdk_example.cpp 7 | main.cpp 8 | ) 9 | 10 | add_executable(motion_sdk_example ${SOURCES}) 11 | target_link_libraries(motion_sdk_example deeprobotics_${ROBOT_NAME}_motion_sdk_${CMAKE_SYSTEM_PROCESSOR}) 12 | 13 | -------------------------------------------------------------------------------- /example/main.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file main.cpp 3 | * @author your name (you@domain.com) 4 | * @brief 5 | * @version 0.1 6 | * @date 2022-09-13 7 | * 8 | * @copyright Copyright (c) 2022 9 | * 10 | */ 11 | #include "udp_socket.hpp" 12 | #include "udp_server.hpp" 13 | #include "command_list.h" 14 | #include "parse_cmd.h" 15 | #include "send_to_robot.h" 16 | #include "motion_sdk_example.h" 17 | 18 | #include 19 | #include 20 | #include 21 | 22 | using namespace std; 23 | bool is_message_updated_ = false; ///< Flag to check if message has been updated 24 | /** 25 | * @brief Callback function to set message update flag 26 | * 27 | * @param code The code indicating the type of message received 28 | */ 29 | void OnMessageUpdate(uint32_t code){ 30 | if(code == 0x0906){ 31 | is_message_updated_ = true; 32 | } 33 | } 34 | 35 | 36 | 37 | int main(int argc, char* argv[]){ 38 | double now_time,start_time; 39 | RobotCmdSDK robot_joint_cmd; 40 | TimeTool my_set_timer; 41 | memset(&robot_joint_cmd, 0, sizeof(robot_joint_cmd)); 42 | SendToRobot* send2robot_cmd = new SendToRobot(); ///< Create send thread 43 | ParseCommand* robot_data_rec = new ParseCommand; ///< Create a receive resolution thread 44 | robot_data_rec->RegisterCallBack(OnMessageUpdate); 45 | MotionSDKExample robot_set_up_demo; ///< Demos for testing can be deleted by yourself 46 | RobotDataSDK *robot_data = &robot_data_rec->getRecvState(); 47 | 48 | robot_data_rec->startWork(); 49 | my_set_timer.time_init(1); ///< Timer initialization, input: cycle; Unit: ms 50 | 51 | send2robot_cmd->robot_state_init(); ///< Return all joints to zero and gain control 52 | 53 | start_time = my_set_timer.get_start_time(); ///< Obtain time for algorithm usage 54 | robot_set_up_demo.GetInitData(*robot_data, 0.000); ///< Obtain all joint states once before each stage (action) 55 | 56 | /********************************************************/ 57 | int time_tick = 0; 58 | int loopcount=0; 59 | 60 | while(1){ 61 | if (my_set_timer.time_interrupt() == 1){ ///< Time interrupt flag, return 1, cycle not reached, return 0, reach a cycle 62 | continue; 63 | } 64 | now_time = my_set_timer.get_now_time(start_time); ///< Get the current time 65 | if(robot_data_rec->getDataRevState()==0){ //No data received from robot 66 | cout<<" No data from the robot was received! "<printData(); 70 | } 71 | 72 | 73 | /*******A simple demo that stands up (for testing and can be deleted by yourself)*********/ 74 | time_tick++; 75 | if(time_tick < 5000){ 76 | robot_set_up_demo.PreStandUp(robot_joint_cmd, now_time, *robot_data);///< Stand up and prepare for action 77 | } 78 | if(time_tick == 5000){ 79 | robot_set_up_demo.GetInitData(*robot_data, now_time);///< Obtain all joint states once before each stage (action) 80 | } 81 | if(time_tick >= 5000 ){ 82 | robot_set_up_demo.StandUp(robot_joint_cmd, now_time, *robot_data);///< Full stand up 83 | } 84 | if(time_tick >= 10000){ 85 | send2robot_cmd->control_get(1); ///< Return the control right, input: 1. Original algorithm control of the robot 2. SDK control PS: over 5ms, no data set sent_ Send (cmd), you will lose control, you need to resend to obtain control 86 | exit(1); 87 | } 88 | 89 | 90 | /*********A simple demo that stands up (for testing and can be deleted by yourself)*******/ 91 | // if(is_message_updated_){ 92 | // send2robot_cmd->set_send(robot_joint_cmd); 93 | // } 94 | loopcount++; 95 | // cout<joint_state.joint_data[2].tor<<" | "<joint_state.fl_leg[0].pos<<" "<joint_state.fl_leg[1].pos<<" "<joint_state.fl_leg[2].pos<<" "<< robot_joint_cmd.joint_cmd[3].pos<<" "<imu.acc_x<<" "< total_time) 224 | run_time = total_time; 225 | sub_goal_pos = a * pow(run_time, 3) + b * pow(run_time, 2) + c * run_time + d; 226 | 227 | if (run_time + cycle_time > total_time) 228 | run_time = total_time - cycle_time; 229 | sub_goal_pos_next = a * pow(run_time + cycle_time, 3) + 230 | b * pow(run_time + cycle_time, 2) + 231 | c * (run_time + cycle_time) + d; 232 | 233 | if (run_time + cycle_time * 2 > total_time) 234 | run_time = total_time - cycle_time * 2; 235 | sub_goal_pos_next2 = a * pow(run_time + cycle_time * 2, 3) + 236 | b * pow(run_time + cycle_time * 2, 2) + 237 | c * (run_time + cycle_time * 2) + d; 238 | } -------------------------------------------------------------------------------- /example/motion_sdk_example.h: -------------------------------------------------------------------------------- 1 | #include "command_list.h" 2 | #include "parse_cmd.h" 3 | #include 4 | #include 5 | 6 | using namespace std; 7 | using namespace x30; 8 | 9 | 10 | const double kDegree2Radian = 3.1415926 / 180; 11 | 12 | class MotionSDKExample{ 13 | private: 14 | 15 | public: 16 | void setCmd(RobotCmdSDK &cmd){ 17 | } 18 | /** 19 | * @brief Spend 1 s putting the robot's legs away and preparing to stand 20 | * @param cmd Issue control command 21 | * @param time Current timestamp 22 | * @param data_state Real-time status data of robot 23 | */ 24 | void PreStandUp(RobotCmdSDK &cmd, double time,RobotDataSDK &data_state); 25 | 26 | /** 27 | * @brief Spend 1.5s standing 28 | * @param cmd Issue control command 29 | * @param time Current timestamp 30 | * @param data_state Real-time status data of robot 31 | */ 32 | void StandUp(RobotCmdSDK &cmd, double time,RobotDataSDK &data_state); 33 | 34 | /** 35 | * @brief Specifically achieve swinging one leg of the robot to a specified position within a specified time 36 | * @param initial_angle 37 | * @param final_angle 38 | * @param total_time 39 | * @param run_time 40 | * @param cycle_time Control cycle, default is 1ms 41 | * @param side Control which leg, FL is the left front leg, FR is the right front leg, HL is the left and right leg, and HR is the right rear leg 42 | * @param cmd Issue control command 43 | * @param data Real-time status data of robot 44 | */ 45 | void SwingToAngle(double initial_angle[3], double final_angle[3], double total_time, double run_time, double cycle_time, string side, RobotCmdSDK &cmd, RobotDataSDK &data); 46 | 47 | /** 48 | * @brief Interpolation to find the path point, i.e. the target angle for each control cycle 49 | * @param init_pos 50 | * @param init_vel 51 | * @param goal_pos 52 | * @param goal_vel 53 | * @param run_time 54 | * @param cycle_time Control cycle, default is 1ms 55 | * @param total_time 56 | * @param sub_goal_pos Target angle for the control cycle 57 | * @param sub_goal_pos_next Target angle for the next control cycle 58 | * @param sub_goal_pos_next2 Target angle for the next and next control cycle 59 | */ 60 | void CubicSpline(double init_pos, double init_vel, double goal_pos, double goal_vel, double run_time, double cycle_time, double total_time, double &sub_goal_pos, double &sub_goal_pos_next, double &sub_goal_pos_next2); 61 | 62 | /** 63 | * @brief Only the current moment and angle are recorded 64 | * @param data Current joint data 65 | * @param time Current timestamp 66 | */ 67 | void GetInitData(RobotDataSDK data, double time); 68 | }; 69 | 70 | -------------------------------------------------------------------------------- /include/base_socket.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #if defined(__linux__) || defined(__APPLE__) 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #elif _WIN32 10 | #include 11 | #endif 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | #define FDR_UNUSED(expr){ (void)(expr); } 18 | #define FDR_ON_ERROR std::function onError = [](int errorCode, std::string errorMessage){FDR_UNUSED(errorCode); FDR_UNUSED(errorMessage)} 19 | 20 | namespace x30{ 21 | class BaseSocket 22 | { 23 | // Definitions 24 | public: 25 | enum SocketType 26 | { 27 | TCP = SOCK_STREAM, 28 | UDP = SOCK_DGRAM 29 | }; 30 | const uint16_t BUFFER_SIZE = 0xFFFF; 31 | sockaddr_in address; 32 | bool isClosed = false; 33 | 34 | protected: 35 | int sock = 0; 36 | static std::string ipToString(sockaddr_in addr) 37 | { 38 | char ip[INET_ADDRSTRLEN]; 39 | inet_ntop(AF_INET, &(addr.sin_addr), ip, INET_ADDRSTRLEN); 40 | 41 | return std::string(ip); 42 | } 43 | 44 | BaseSocket(FDR_ON_ERROR, SocketType sockType = TCP, int socketId = -1) 45 | { 46 | if (socketId < 0) 47 | { 48 | if ((this->sock = socket(AF_INET, sockType, 0)) < 0) 49 | { 50 | onError(errno, "Socket creating error."); 51 | } 52 | } 53 | else 54 | { 55 | this->sock = socketId; 56 | } 57 | } 58 | 59 | // Methods 60 | public: 61 | virtual void Close() { 62 | if(isClosed) return; 63 | 64 | isClosed = true; 65 | close(this->sock); 66 | } 67 | 68 | std::string remoteAddress() {return ipToString(this->address);} 69 | int remotePort() {return ntohs(this->address.sin_port);} 70 | int fileDescriptor() const { return this->sock; } 71 | }; 72 | 73 | }; -------------------------------------------------------------------------------- /include/command.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file cmmand.h 3 | * @author your name (you@domain.com) 4 | * @brief 5 | * @version 0.1 6 | * @date 2022-09-13 7 | * 8 | * @copyright Copyright (c) 2022 9 | * 10 | */ 11 | 12 | #ifndef X30_COMMAND_H_ 13 | #define X30_COMMAND_H_ 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | namespace x30{ 22 | struct EthCommand{ 23 | uint32_t code; 24 | union{ 25 | uint32_t value; 26 | uint32_t paramters_size; 27 | }; 28 | struct { 29 | /// indicate the massage whether has one more 30 | /// @param 1 multiple value; 31 | /// @param 0 single value; 32 | uint32_t type : 8; 33 | /// the sequence number of massage 34 | uint32_t count : 24; 35 | }; 36 | }; 37 | 38 | namespace command_type{ 39 | enum CommandType{ 40 | kSingleValue = 0, 41 | kMessValues = 1, 42 | }; 43 | } 44 | 45 | const size_t kCommandDataBufferSize = 1024; 46 | 47 | struct CommandMessage{ 48 | EthCommand command; 49 | char data_buffer[kCommandDataBufferSize]; 50 | }; 51 | 52 | class Command { 53 | private: 54 | std::bitset<32> command_code_; 55 | union{ 56 | int32_t command_value_; 57 | size_t command_parameters_size_; 58 | }; 59 | void* command_parameters_; 60 | 61 | public: 62 | Command(); 63 | Command(uint32_t command_code,int32_t command_value); 64 | Command(uint32_t command_code,size_t command_parameters_size,void* command_parameters); 65 | 66 | virtual ~Command(); 67 | 68 | std::bitset<32>& get_command_code(); 69 | int32_t get_command_value(); 70 | size_t get_command_parameters_size(); 71 | void* get_command_parameters(); 72 | 73 | friend std::ostream& operator<<(std::ostream& stream, Command& c); 74 | }; // end of Command 75 | 76 | };//namespace x30 77 | #endif // COMMAND_H_ -------------------------------------------------------------------------------- /include/command_list.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file cmmand.h 3 | * @author your name (you@domain.com) 4 | * @brief 5 | * @version 0.1 6 | * @date 2022-09-13 7 | * 8 | * @copyright Copyright (c) 2022 9 | * 10 | */ 11 | 12 | #ifndef X30_COMMAND_LIST_H_ 13 | #define X30_COMMAND_LIST_H_ 14 | 15 | #include 16 | #include "command.h" 17 | 18 | namespace x30{ 19 | class CommandList { 20 | private: 21 | std::deque command_list_; 22 | pthread_mutex_t mutex_; 23 | /// config list max size 24 | size_t list_capacity_; 25 | public: 26 | CommandList(); 27 | virtual ~CommandList(); 28 | 29 | uint32_t set_command(Command* command); 30 | uint32_t get_command_front(Command& command); 31 | uint32_t get_command_back(Command& command); 32 | }; 33 | };//namespace x30 34 | #endif // COMMAND_H_ -------------------------------------------------------------------------------- /include/parse_cmd.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file robot_types.h 3 | * @author your name (you@domain.com) 4 | * @brief 5 | * @version 0.1 6 | * @date 2022-09-05 7 | * 8 | * @copyright Copyright (c) 2022 9 | * 10 | */ 11 | 12 | 13 | #ifndef X30_PARSE_CMD_H_ 14 | #define X30_PARSE_CMD_H_ 15 | 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | #include "x30_types.h" 28 | #include "udp_server.hpp" 29 | #define LOCAL_PORT 43897 /**< 43897 Local Port Number.*/ 30 | #define STATE_RECEIVE_CODE 0x0906 /**< Command code for receiving robot data.*/ 31 | 32 | 33 | namespace x30{ 34 | /** 35 | * @brief A class used to receive robot data. 36 | */ 37 | class ParseCommand{ 38 | private: 39 | RobotDataSDK state_rec; /**< Used to save received data.*/ 40 | int is_data_recv = 0; 41 | const int local_port; 42 | void setDataRevState(int state){ 43 | is_data_recv = state > 0? 1:0; 44 | } 45 | /// @brief CallBack_. 46 | /// @param int Instruction type, only 0x0906. 47 | std::function CallBack_; 48 | public: 49 | /** 50 | * @brief 51 | * @note 52 | * @retval 53 | */ 54 | ParseCommand(int port=LOCAL_PORT); 55 | /** 56 | * @brief Create the accepted thread using the work() 57 | */ 58 | void startWork(); /**< Create the accepted thread using the work().*/ 59 | 60 | /** 61 | * @brief Receive robot data and save it 62 | */ 63 | void work(); 64 | /// @brief Registering Callbacks. 65 | void RegisterCallBack(std::function CallBack){ 66 | CallBack_ = std::move(CallBack); 67 | } 68 | 69 | RobotDataSDK& getRecvState(); /**< Save the obtained data in st.*/ 70 | 71 | void printData(); /**< Print the recieved data */ 72 | 73 | int getDataRevState(){ 74 | return is_data_recv; 75 | } 76 | }; 77 | }; 78 | #endif // PARSE_CMD_H_ 79 | -------------------------------------------------------------------------------- /include/send_to_robot.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file robot_types.h 3 | * @author your name (you@domain.com) 4 | * @brief 5 | * @version 0.1 6 | * @date 2022-09-05 7 | * 8 | * @copyright Copyright (c) 2022 9 | * 10 | */ 11 | 12 | 13 | #ifndef X30_SEND_TO_ROBOT_H_ 14 | #define X30_SEND_TO_ROBOT_H_ 15 | 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include "command_list.h" 27 | #include "udp_socket.hpp" 28 | #include "x30_types.h" 29 | 30 | #define DEFAULT_ROBOT_IP "192.168.1.103" 31 | #define DEFAULT_ROBOT_PORT 43893 32 | 33 | #define ABLE 2 34 | #define UNABLE 1 35 | 36 | namespace x30{ 37 | /** 38 | * @brief A class used to send robot data. 39 | */ 40 | class SendToRobot{ 41 | private: 42 | // RobotCmdSDK robot_cmd_; /**< View robot_tppes.h.*/ 43 | UDPSocket *udp_socket_; /**< What udpSocket needs*/ 44 | 45 | /** 46 | * @brief Send instructions. 47 | * @param Command 48 | */ 49 | void cmd_done(Command& command); 50 | public: 51 | /// @brief Constructor with IP and port parameters. 52 | /// @param[in] ip The IP address to send data to. 53 | /// @param[in] port The port number to send data to. 54 | SendToRobot(std::string ip = DEFAULT_ROBOT_IP, uint16_t port = DEFAULT_ROBOT_PORT); 55 | /** 56 | * @brief Create the send thread using the work() 57 | */ 58 | void start_work(); 59 | 60 | /** 61 | * @brief Send data to robot. 62 | */ 63 | void work(); 64 | 65 | // /** 66 | // * @brief Initialization timer for receiving data 67 | // */ 68 | // void init(void); 69 | 70 | /** 71 | * @brief Send the set joint target status to the robot.Input:RobotCmdSDK 72 | * @param RobotCmdSDK 73 | */ 74 | void set_send(RobotCmdSDK&); 75 | 76 | /** 77 | * @brief Initialize the robot for the first time after powering on 78 | */ 79 | void robot_state_init(void); 80 | 81 | /** 82 | * @brief Set the command code and command value to send 83 | * @param code 84 | * @param value 85 | */ 86 | void set_cmd(uint32_t code , uint32_t value); 87 | 88 | /** 89 | * @brief Select the control right 90 | * @param 1:Original robot algorithm control 2: SDK control. 91 | */ 92 | void control_get(int mode){ 93 | if(mode == UNABLE){ 94 | RobotCmdSDK cmd; 95 | for(int i = 0; i < 12; i++){ 96 | cmd.joint_cmd[i].pos = 0.0; 97 | cmd.joint_cmd[i].tor = 0.0; 98 | cmd.joint_cmd[i].vel = 0.0; 99 | cmd.joint_cmd[i].kp = 0.0; 100 | cmd.joint_cmd[i].kd = 12.5; 101 | } 102 | set_send(cmd); 103 | sleep(5); 104 | Command command_temp(0x0113,0, 0); 105 | cmd_done(command_temp); 106 | }else if (mode == ABLE){ 107 | Command command_temp(0x0114,0, 0); 108 | cmd_done(command_temp); 109 | } 110 | }// void control_get(void); 111 | }; 112 | 113 | /** 114 | * @brief A class used to get time. 115 | */ 116 | class TimeTool{ 117 | private: 118 | int tfd; /**< Timer descriptor.*/ 119 | int efd; /**< Epoll descriptor.*/ 120 | int fds, ret; /**< Variables used to initialize the timer.*/ 121 | uint64_t value; /**< Variables used to initialize the timer.*/ 122 | struct epoll_event ev, *evptr; /**< Variables used to initialize the timer.*/ 123 | struct itimerspec time_intv; /**< Variables used to initialize the timer.*/ 124 | public: 125 | timespec system_time; /**< A class for accurately obtaining time.*/ 126 | /** 127 | * @brief Initialize timer, input cycle(ms). 128 | * @param Cycle time unit: ms 129 | */ 130 | void time_init(int ms); 131 | 132 | /** 133 | * @brief Acquire interrupt signal 134 | * @return 1:Enter interrupt 0:no 135 | */ 136 | int time_interrupt(); /**< Acquire interrupt signal.*/ 137 | 138 | /** 139 | * @brief How long has it been 140 | * @param Initial time 141 | */ 142 | double get_now_time(double start_time); 143 | 144 | /** 145 | * @brief Get current time 146 | */ 147 | double get_start_time(); /**< Get start time.*/ 148 | }; 149 | 150 | };//namespace x30 151 | #endif // PARSE_CMD_H_ 152 | -------------------------------------------------------------------------------- /include/udp_server.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "udp_socket.hpp" 4 | #include 5 | 6 | namespace x30{ 7 | class UDPServer : public UDPSocket 8 | { 9 | public: 10 | void Bind(std::string IPv4, std::uint16_t port, FDR_ON_ERROR) 11 | { 12 | if (inet_pton(AF_INET, IPv4.c_str(), &this->address.sin_addr) <= 0) 13 | { 14 | onError(errno, "Invalid address. Address type not supported."); 15 | return; 16 | } 17 | 18 | this->address.sin_family = AF_INET; 19 | this->address.sin_port = htons(port); 20 | 21 | if (bind(this->sock, (const sockaddr *)&this->address, sizeof(this->address)) < 0) 22 | { 23 | onError(errno, "Cannot bind the socket."); 24 | return; 25 | } 26 | } 27 | 28 | void Bind(int port, FDR_ON_ERROR) 29 | { 30 | this->Bind("0.0.0.0", port, onError); 31 | } 32 | 33 | void setBroadcast(FDR_ON_ERROR) 34 | { 35 | int broadcast = 1; 36 | if (setsockopt(this->sock, SOL_SOCKET, SO_BROADCAST, &broadcast, sizeof broadcast)) 37 | { 38 | onError(errno, "setsockopt(SO_BROADCAST) failed."); 39 | return; 40 | } 41 | } 42 | }; 43 | 44 | }; -------------------------------------------------------------------------------- /include/udp_socket.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "base_socket.hpp" 4 | #include 5 | #include 6 | 7 | namespace x30{ 8 | class UDPSocket : public BaseSocket 9 | { 10 | public: 11 | std::function onMessageReceived; 12 | std::function onRawMessageReceived; 13 | 14 | explicit UDPSocket(bool useConnect = false, FDR_ON_ERROR, int socketId = -1): BaseSocket(onError, SocketType::UDP, socketId) 15 | { 16 | if (useConnect) 17 | { 18 | 19 | std::thread t(Receive, this); // usage with Connect() 20 | t.detach(); 21 | } 22 | else 23 | { 24 | std::thread t(ReceiveFrom, this); 25 | t.detach(); 26 | } 27 | } 28 | 29 | // SendTo with no connection 30 | void SendTo(const char *bytes, size_t byteslength, std::string host, uint16_t port, FDR_ON_ERROR) 31 | { 32 | sockaddr_in hostAddr; 33 | 34 | struct addrinfo hints, *res, *it; 35 | memset(&hints, 0, sizeof(hints)); 36 | hints.ai_family = AF_INET; 37 | hints.ai_socktype = SOCK_DGRAM; 38 | 39 | int status; 40 | if ((status = getaddrinfo(host.c_str(), NULL, &hints, &res)) != 0) 41 | { 42 | onError(errno, "Invalid address." + std::string(gai_strerror(status))); 43 | return; 44 | } 45 | 46 | for (it = res; it != NULL; it = it->ai_next) 47 | { 48 | if (it->ai_family == AF_INET) 49 | { // IPv4 50 | memcpy((void *)(&hostAddr), (void *)it->ai_addr, sizeof(sockaddr_in)); 51 | break; // for now, just get first ip (ipv4). 52 | } 53 | } 54 | 55 | freeaddrinfo(res); 56 | 57 | hostAddr.sin_port = htons(port); 58 | hostAddr.sin_family = AF_INET; 59 | 60 | if (sendto(this->sock, bytes, byteslength, 0, (sockaddr *)&hostAddr, sizeof(hostAddr)) < 0) 61 | { 62 | onError(errno, "Cannot send message to the address."); 63 | return; 64 | } 65 | } 66 | void SendTo(std::string message, std::string host, uint16_t port, FDR_ON_ERROR) 67 | { 68 | this->SendTo(message.c_str(), message.length(), host, port, onError); 69 | } 70 | 71 | // Send with Connect() 72 | int Send(const char *bytes, size_t byteslength) 73 | { 74 | if (this->isClosed) 75 | return -1; 76 | 77 | int sent = 0; 78 | if ((sent = send(this->sock, bytes, byteslength, 0)) < 0) 79 | { 80 | perror("send"); 81 | } 82 | return sent; 83 | } 84 | int Send(std::string message) 85 | { 86 | return this->Send(message.c_str(), message.length()); 87 | } 88 | 89 | // To use "Send()", need to call Connect() first 90 | void Connect(uint32_t ipv4, uint16_t port, FDR_ON_ERROR) 91 | { 92 | this->address.sin_family = AF_INET; 93 | this->address.sin_port = htons(port); 94 | this->address.sin_addr.s_addr = ipv4; 95 | 96 | // Try to connect. 97 | if (connect(this->sock, (const sockaddr *)&this->address, sizeof(sockaddr_in)) < 0) 98 | { 99 | onError(errno, "Connection failed to the host."); 100 | return; 101 | } 102 | } 103 | void Connect(std::string host, uint16_t port, FDR_ON_ERROR) 104 | { 105 | struct addrinfo hints, *res, *it; 106 | memset(&hints, 0, sizeof(hints)); 107 | hints.ai_family = AF_INET; 108 | hints.ai_socktype = SOCK_DGRAM; 109 | 110 | int status; 111 | if ((status = getaddrinfo(host.c_str(), NULL, &hints, &res)) != 0) 112 | { 113 | onError(errno, "Invalid address." + std::string(gai_strerror(status))); 114 | return; 115 | } 116 | 117 | for (it = res; it != NULL; it = it->ai_next) 118 | { 119 | if (it->ai_family == AF_INET) 120 | { // IPv4 121 | memcpy((void *)(&this->address), (void *)it->ai_addr, sizeof(sockaddr_in)); 122 | break; // for now, just get first ip (ipv4). 123 | } 124 | } 125 | 126 | freeaddrinfo(res); 127 | 128 | this->Connect((uint32_t)this->address.sin_addr.s_addr, port, onError); 129 | } 130 | 131 | private: 132 | static void Receive(UDPSocket *udpSocket) 133 | { 134 | char tempBuffer[udpSocket->BUFFER_SIZE]; 135 | 136 | while (true) 137 | { 138 | int messageLength; 139 | messageLength = recv(udpSocket->sock, tempBuffer, udpSocket->BUFFER_SIZE, 0); 140 | 141 | if (messageLength < 0) 142 | { 143 | perror("recvfrom"); 144 | return; 145 | } 146 | else 147 | { 148 | tempBuffer[messageLength] = '\0'; 149 | if (udpSocket->onMessageReceived) 150 | udpSocket->onMessageReceived(std::string(tempBuffer, messageLength), ipToString(udpSocket->address), ntohs(udpSocket->address.sin_port)); 151 | 152 | if (udpSocket->onRawMessageReceived) 153 | udpSocket->onRawMessageReceived(tempBuffer, messageLength, ipToString(udpSocket->address), ntohs(udpSocket->address.sin_port)); 154 | } 155 | } 156 | } 157 | static void ReceiveFrom(UDPSocket *udpSocket) 158 | { 159 | sockaddr_in hostAddr; 160 | socklen_t hostAddrSize = sizeof(hostAddr); 161 | 162 | char tempBuffer[udpSocket->BUFFER_SIZE]; 163 | 164 | while (true) 165 | { 166 | int messageLength; 167 | messageLength = recvfrom(udpSocket->sock, tempBuffer, udpSocket->BUFFER_SIZE, 0, (sockaddr *)&hostAddr, &hostAddrSize); 168 | 169 | if (messageLength < 0) 170 | { 171 | perror("recvfrom"); 172 | return; 173 | } 174 | else 175 | { 176 | tempBuffer[messageLength] = '\0'; 177 | if (udpSocket->onMessageReceived) 178 | udpSocket->onMessageReceived(std::string(tempBuffer, messageLength), ipToString(hostAddr), ntohs(hostAddr.sin_port)); 179 | 180 | if (udpSocket->onRawMessageReceived) 181 | udpSocket->onRawMessageReceived(tempBuffer, messageLength, ipToString(hostAddr), ntohs(hostAddr.sin_port)); 182 | } 183 | } 184 | } 185 | }; 186 | 187 | }; -------------------------------------------------------------------------------- /include/x30_types.h: -------------------------------------------------------------------------------- 1 | /** 2 | * @file robot_types.h 3 | * @author your name (you@domain.com) 4 | * @brief 5 | * @version 0.1 6 | * @date 2022-09-05 7 | * 8 | * @copyright Copyright (c) 2022 9 | * 10 | */ 11 | 12 | 13 | #ifndef X30_ROBOT_TYPES_H_ 14 | #define X30_ROBOT_TYPES_H_ 15 | 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | namespace x30{ 22 | struct ImuDataSDK{ 23 | int32_t timestamp; 24 | union{ 25 | float buffer_float[9]; 26 | uint8_t buffer_byte[3][12]; 27 | struct{ 28 | float roll, pitch, yaw; 29 | float omega_x, omega_y, omega_z; 30 | float acc_x, acc_y, acc_z; 31 | }; 32 | }; 33 | }; 34 | 35 | typedef struct{ 36 | float pos; 37 | float vel; 38 | float tor; 39 | float temperature; 40 | }SingleJointData; 41 | 42 | // struct JointDataSDK{ 43 | // union{ 44 | // std::array joint_data; 45 | // struct { 46 | // std::array fl_leg; 47 | // std::array fr_leg; 48 | // std::array hl_leg; 49 | // std::array hr_leg; 50 | // }; 51 | // }; 52 | // }; 53 | 54 | typedef struct{ 55 | float pos; 56 | float vel; 57 | float tor; 58 | float kp; 59 | float kd; 60 | }SingleJointCmd; 61 | 62 | struct RobotCmdSDK{ 63 | union{ 64 | std::array joint_cmd; 65 | struct { 66 | std::array fl_leg; 67 | std::array fr_leg; 68 | std::array hl_leg; 69 | std::array hr_leg; 70 | }; 71 | }; 72 | }; 73 | 74 | 75 | typedef struct{ 76 | uint32_t tick; 77 | ImuDataSDK imu; 78 | union{ 79 | std::array joint_data; 80 | struct { 81 | std::array fl_leg; 82 | std::array fr_leg; 83 | std::array hl_leg; 84 | std::array hr_leg; 85 | }; 86 | }; 87 | union{ 88 | struct{ 89 | std::array fl_force; 90 | std::array fr_force; 91 | std::array hl_force; 92 | std::array hr_force; 93 | }; 94 | std::array contact_force; 95 | }; 96 | }RobotDataSDK; 97 | 98 | 99 | }; 100 | 101 | 102 | 103 | 104 | #endif // ROBOT_TYPES_H_ -------------------------------------------------------------------------------- /python/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(deeprobotics_${ROBOT_NAME}_motion_sdk_py) 3 | 4 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -O3 -fPIC") 5 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3 -fPIC") 6 | set(CMAKE_CXX_STANDARD 11) 7 | 8 | include_directories( 9 | ${CMAKE_CURRENT_SOURCE_DIR}/../include 10 | ) 11 | 12 | add_subdirectory(pybind11) 13 | set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/lib) 14 | 15 | pybind11_add_module(${PROJECT_NAME} motion_sdk_pybind.cpp) 16 | target_link_libraries(${PROJECT_NAME} PRIVATE -lpthread -lm -lrt -ldl -lstdc++ deeprobotics_${ROBOT_NAME}_motion_sdk_${CMAKE_SYSTEM_PROCESSOR}) 17 | -------------------------------------------------------------------------------- /python/__pycache__/motion_example.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/DeepRoboticsLab/x30_motion_sdk/885e7f9c8bf855f57b6bf39fa73acb18aec9c28d/python/__pycache__/motion_example.cpython-38.pyc -------------------------------------------------------------------------------- /python/motion_sdk_example.py: -------------------------------------------------------------------------------- 1 | # -*- coding: UTF-8 -*- 2 | import sys 3 | sys.path.append('./lib/') 4 | # print(sys.path) 5 | import deeprobotics_x30_motion_sdk_py as dr 6 | import numpy as np 7 | import time 8 | 9 | is_message_updated = False 10 | kDegree2Radian = np.pi / 180 11 | 12 | goal_angle_fl = np.zeros(3) 13 | goal_angle_hl = np.zeros(3) 14 | goal_angle_fr = np.zeros(3) 15 | goal_angle_hr = np.zeros(3) 16 | init_angle_fl = np.zeros(3) 17 | init_angle_fr = np.zeros(3) 18 | init_angle_hl = np.zeros(3) 19 | init_angle_hr = np.zeros(3) 20 | 21 | class MotionSDKExample: 22 | def __init__(self): 23 | self.init_time = 0.0 24 | 25 | def CubicSpline(self, init_pos, init_vel, goal_pos, goal_vel, 26 | run_time, cycle_time, total_time): 27 | d = init_pos 28 | c = init_vel 29 | a = (goal_vel * total_time - 2 * goal_pos + init_vel * total_time + 30 | 2 * init_pos) /pow(total_time, 3) 31 | b = (3 * goal_pos - goal_vel * total_time - 2 * init_vel * total_time - 32 | 3 * init_pos) / pow(total_time, 2) 33 | 34 | if run_time > total_time: 35 | run_time = total_time 36 | sub_goal_position = a * pow(run_time, 3) + b * pow(run_time, 2) + c * run_time + d 37 | 38 | if run_time + cycle_time > total_time: 39 | run_time = total_time - cycle_time 40 | sub_goal_position_next = a * pow(run_time + cycle_time, 3) + \ 41 | b * pow(run_time + cycle_time, 2) + \ 42 | c * (run_time + cycle_time) + d 43 | 44 | return sub_goal_position, sub_goal_position_next 45 | 46 | 47 | def SwingToAngle(self, init_angle, final_angle, total_time, run_time, cycle_time, side, 48 | cmd, data): 49 | goal_angle = np.zeros(3) 50 | goal_angle_next = np.zeros(3) 51 | goal_velocity = np.zeros(3) 52 | leg_side = 0 53 | 54 | if side == "FL": 55 | leg_side = 0 56 | elif side == "FR": 57 | leg_side = 1 58 | elif side == "HL": 59 | leg_side = 2 60 | elif side == "HR": 61 | leg_side = 3 62 | else: 63 | print("Leg Side Error!!!") 64 | 65 | for j in range(3): 66 | goal_angle[j], goal_angle_next[j] = \ 67 | self.CubicSpline(init_angle[j], 0, final_angle[j], 0, run_time, 68 | cycle_time, total_time) 69 | goal_velocity[j] = (goal_angle_next[j] - goal_angle[j]) / cycle_time 70 | 71 | for i in range(3): 72 | cmd.joint_cmd[3 * leg_side+i].kp = 300. 73 | cmd.joint_cmd[3 * leg_side+i].kd = 4. 74 | cmd.joint_cmd[3 * leg_side+i].pos = goal_angle[i] 75 | cmd.joint_cmd[3 * leg_side+i].vel = goal_velocity[i] 76 | cmd.joint_cmd[3 * leg_side+i].tor = 0 77 | 78 | 79 | def PreStandUp(self, cmd, time, data_state): 80 | standup_time = 4.0 81 | cycle_time = 0.001 82 | goal_angle_fl = [0 * kDegree2Radian, -70 * kDegree2Radian, 150 * kDegree2Radian] 83 | goal_angle_fr = [0 * kDegree2Radian, -70 * kDegree2Radian, 150 * kDegree2Radian] 84 | goal_angle_hl = [0 * kDegree2Radian, -70 * kDegree2Radian, 150 * kDegree2Radian] 85 | goal_angle_hr = [0 * kDegree2Radian, -70 * kDegree2Radian, 150 * kDegree2Radian] 86 | 87 | self.SwingToAngle(init_angle_fl, goal_angle_fl, standup_time, time - self.init_time, 88 | cycle_time, "FL", cmd, data_state) 89 | self.SwingToAngle(init_angle_fr, goal_angle_fr, standup_time, time - self.init_time, 90 | cycle_time, "FR", cmd, data_state) 91 | self.SwingToAngle(init_angle_hl, goal_angle_hl, standup_time, time - self.init_time, 92 | cycle_time, "HL", cmd, data_state) 93 | self.SwingToAngle(init_angle_hr, goal_angle_hr, standup_time, time - self.init_time, 94 | cycle_time, "HR", cmd, data_state) 95 | 96 | 97 | def StandUp(self, cmd, time, data_state): 98 | standup_time = 4.5 99 | cycle_time = 0.001 100 | goal_angle_fl = [0 * kDegree2Radian, -42 * kDegree2Radian, 78 * kDegree2Radian] 101 | goal_angle_fr = [0 * kDegree2Radian, -42 * kDegree2Radian, 78 * kDegree2Radian] 102 | goal_angle_hl = [0 * kDegree2Radian, -42 * kDegree2Radian, 78 * kDegree2Radian] 103 | goal_angle_hr = [0 * kDegree2Radian, -42 * kDegree2Radian, 78 * kDegree2Radian] 104 | 105 | self.SwingToAngle(init_angle_fl, goal_angle_fl, standup_time, time - self.init_time, 106 | cycle_time, "FL", cmd, data_state) 107 | self.SwingToAngle(init_angle_fr, goal_angle_fr, standup_time, time - self.init_time, 108 | cycle_time, "FR", cmd, data_state) 109 | self.SwingToAngle(init_angle_hl, goal_angle_hl, standup_time, time - self.init_time, 110 | cycle_time, "HL", cmd, data_state) 111 | self.SwingToAngle(init_angle_hr, goal_angle_hr, standup_time, time - self.init_time, 112 | cycle_time, "HR", cmd, data_state) 113 | 114 | 115 | def GetInitData(self, data, time): 116 | self.init_time = time 117 | init_angle_fl[0] = data.joint_data[0].pos 118 | init_angle_fl[1] = data.joint_data[1].pos 119 | init_angle_fl[2] = data.joint_data[2].pos 120 | 121 | init_angle_fr[0] = data.joint_data[3].pos 122 | init_angle_fr[1] = data.joint_data[4].pos 123 | init_angle_fr[2] = data.joint_data[5].pos 124 | 125 | init_angle_hl[0] = data.joint_data[6].pos 126 | init_angle_hl[1] = data.joint_data[7].pos 127 | init_angle_hl[2] = data.joint_data[8].pos 128 | 129 | init_angle_hr[0] = data.joint_data[9].pos 130 | init_angle_hr[1] = data.joint_data[10].pos 131 | init_angle_hr[2] = data.joint_data[11].pos 132 | 133 | 134 | def handler(code): 135 | if(code == 0x0906): 136 | is_message_updated = True 137 | 138 | 139 | if __name__ == "__main__" : 140 | timer = dr.TimeTool() 141 | now_time = 0.0 142 | start_time = 0.0 143 | 144 | sender = dr.SendToRobot('192.168.1.103', 43893) 145 | receiver = dr.ParseCommand(43897) 146 | receiver.register_call_back(handler) 147 | robot_data = receiver.get_data() 148 | receiver.start_work() 149 | timer.time_init(1) 150 | sender.robot_state_init() 151 | 152 | start_time = timer.get_start_time() 153 | 154 | robot_set_up_demo = MotionSDKExample() 155 | robot_set_up_demo.GetInitData(receiver.get_data(), 0.000) 156 | 157 | time_tick = 0 158 | robot_joint_cmd = dr.RobotCmdSDK() 159 | 160 | while True: 161 | if timer.time_interrupt() == True: 162 | continue 163 | now_time = timer.get_now_time(start_time) 164 | time_tick = time_tick+1 165 | if receiver.get_receive_state() == False: 166 | print("No data from the robot was received!") 167 | break 168 | else: 169 | if False: 170 | receiver.print_data() 171 | # start_time = time.time() 172 | if time_tick < 5000: 173 | robot_set_up_demo.PreStandUp(robot_joint_cmd, now_time, receiver.get_data()) 174 | if time_tick == 5000: 175 | robot_set_up_demo.GetInitData(receiver.get_data(), now_time) 176 | print("Robot is ready to stand up ") 177 | if time_tick > 5000: 178 | robot_set_up_demo.StandUp(robot_joint_cmd, now_time, receiver.get_data()) 179 | if time_tick >= 10000: 180 | print("Robot is going to exit SDK control ") 181 | sender.control_get(1) 182 | break 183 | # sender.set_send(robot_joint_cmd) 184 | # print("cost_time: ", 1000.*(time.time() - start_time)) 185 | 186 | -------------------------------------------------------------------------------- /python/motion_sdk_pybind.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file motion_sdk_pybind.cpp 3 | * @brief 4 | * @author mazunwang 5 | * @version 1.0 6 | * @date 2024-03-14 7 | * 8 | * @copyright Copyright (c) 2024 DeepRobotics 9 | * 10 | */ 11 | 12 | #include "parse_cmd.h" 13 | #include "send_to_robot.h" 14 | #include "x30_types.h" 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | using namespace x30; 26 | namespace py = pybind11; 27 | 28 | 29 | PYBIND11_MODULE(deeprobotics_x30_motion_sdk_py, m) { 30 | py::class_(m, "ImuDataSDK") 31 | .def(py::init<>()) 32 | .def_readwrite("timestamp", &ImuDataSDK::timestamp) 33 | .def_readwrite("roll", &ImuDataSDK::roll) 34 | .def_readwrite("pitch", &ImuDataSDK::pitch) 35 | .def_readwrite("yaw", &ImuDataSDK::yaw) 36 | .def_readwrite("omega_x", &ImuDataSDK::omega_x) 37 | .def_readwrite("omega_y", &ImuDataSDK::omega_y) 38 | .def_readwrite("omega_z", &ImuDataSDK::omega_z) 39 | .def_readwrite("acc_x", &ImuDataSDK::acc_x) 40 | .def_readwrite("acc_y", &ImuDataSDK::acc_y) 41 | .def_readwrite("acc_z", &ImuDataSDK::acc_z); 42 | 43 | py::class_(m, "SingleJointData") 44 | .def(py::init<>()) 45 | .def_readwrite("pos", &SingleJointData::pos) 46 | .def_readwrite("vel", &SingleJointData::vel) 47 | .def_readwrite("tor", &SingleJointData::tor) 48 | .def_readwrite("temperature", &SingleJointData::temperature); 49 | 50 | py::class_(m, "SingleJointCmd") 51 | .def(py::init<>()) 52 | .def_readwrite("pos", &SingleJointCmd::pos) 53 | .def_readwrite("vel", &SingleJointCmd::vel) 54 | .def_readwrite("tor", &SingleJointCmd::tor) 55 | .def_readwrite("kp", &SingleJointCmd::kp) 56 | .def_readwrite("kd", &SingleJointCmd::kd); 57 | 58 | py::class_(m, "RobotCmdSDK") 59 | .def(py::init<>()) 60 | .def_readwrite("joint_cmd", &RobotCmdSDK::joint_cmd); 61 | 62 | py::class_(m, "RobotDataSDK") 63 | .def(py::init<>()) 64 | .def_readwrite("tick", &RobotDataSDK::tick) 65 | .def_readwrite("imu", &RobotDataSDK::imu) 66 | .def_readwrite("joint_data", &RobotDataSDK::joint_data) 67 | .def_readwrite("contact_force", &RobotDataSDK::contact_force); 68 | 69 | 70 | py::class_(m, "TimeTool") 71 | .def(py::init<>()) 72 | .def("time_init", &TimeTool::time_init) 73 | .def("time_interrupt", &TimeTool::time_interrupt) 74 | .def("get_now_time", &TimeTool::get_now_time) 75 | .def("get_start_time", &TimeTool::get_start_time); 76 | 77 | py::class_(m, "ParseCommand") 78 | .def(py::init()) 79 | .def("register_call_back", &ParseCommand::RegisterCallBack) 80 | .def("start_work", &ParseCommand::startWork) 81 | .def("get_data", &ParseCommand::getRecvState) 82 | .def("get_receive_state", &ParseCommand::getDataRevState) 83 | .def("print_data", &ParseCommand::printData);; 84 | 85 | py::class_(m, "SendToRobot") 86 | .def(py::init()) 87 | .def("set_send", &SendToRobot::set_send) 88 | .def("control_get", &SendToRobot::control_get) 89 | .def("robot_state_init", &SendToRobot::robot_state_init); 90 | } 91 | -------------------------------------------------------------------------------- /src/command.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file command.cpp 3 | * @author your name (you@domain.com) 4 | * @brief 5 | * @version 0.1 6 | * @date 2022-09-13 7 | * 8 | * @copyright Copyright (c) 2022 9 | * 10 | */ 11 | 12 | #include "command.h" 13 | 14 | using namespace x30; 15 | Command::Command() { 16 | command_code_ = 0; 17 | command_value_ = 0; 18 | command_parameters_ = NULL; 19 | } 20 | 21 | Command::Command(uint32_t command_code,int32_t command_value){ 22 | command_code_ = command_code; 23 | command_value_ = command_value; 24 | command_parameters_ = NULL; 25 | } 26 | 27 | Command::Command(uint32_t command_code, 28 | size_t command_parameters_size, 29 | void* command_parameters){ 30 | command_code_ = command_code; 31 | command_parameters_size_ = command_parameters_size; 32 | command_parameters_ = command_parameters; 33 | } 34 | 35 | Command::~Command() { 36 | } 37 | 38 | 39 | std::bitset<32>& Command::get_command_code(){ 40 | return command_code_; 41 | } 42 | 43 | 44 | int32_t Command::get_command_value(){ 45 | return command_value_; 46 | } 47 | 48 | 49 | size_t Command::get_command_parameters_size(){ 50 | return command_parameters_size_; 51 | } 52 | 53 | // 这个返回的参数，必须由使用者释放掉 54 | // 因为底层无法知道它的类型，而无法自动释放。 55 | void* Command::get_command_parameters(){ 56 | return command_parameters_; 57 | } 58 | 59 | -------------------------------------------------------------------------------- /src/command_list.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file command_list.cpp 3 | * @author your name (you@domain.com) 4 | * @brief 5 | * @version 0.1 6 | * @date 2022-09-13 7 | * 8 | * @copyright Copyright (c) 2022 9 | * 10 | */ 11 | 12 | 13 | #include "command_list.h" 14 | using namespace x30; 15 | 16 | CommandList::CommandList() { 17 | pthread_mutex_init(&mutex_,NULL); 18 | list_capacity_ = 100; 19 | } 20 | 21 | CommandList::~CommandList() { 22 | pthread_mutex_lock(&mutex_); 23 | while(!command_list_.empty()){ 24 | delete (command_list_.back()); 25 | command_list_.pop_back(); 26 | } 27 | pthread_mutex_unlock(&mutex_); 28 | pthread_mutex_destroy(&mutex_); 29 | } 30 | 31 | uint32_t CommandList::set_command(Command* command){ 32 | if(command_list_.size() >= list_capacity_) return 1; 33 | pthread_mutex_lock(&mutex_); 34 | 35 | command_list_.push_back(command); 36 | 37 | pthread_mutex_unlock(&mutex_); 38 | return 0; 39 | } 40 | 41 | // return command count in list 42 | uint32_t CommandList::get_command_front(Command& command){ 43 | size_t quantity = command_list_.size(); 44 | if(command_list_.empty()){ 45 | return quantity; 46 | } 47 | pthread_mutex_lock(&mutex_); 48 | 49 | command = *(command_list_.front()); 50 | delete command_list_.front(); 51 | command_list_.pop_front(); 52 | 53 | pthread_mutex_unlock(&mutex_); 54 | return quantity; 55 | } 56 | 57 | // return user command count in list 58 | uint32_t CommandList::get_command_back(Command& command){ 59 | size_t quantity = command_list_.size(); 60 | if(command_list_.empty()){ 61 | return quantity; 62 | } 63 | pthread_mutex_lock(&mutex_); 64 | 65 | command = *(command_list_.back()); 66 | delete command_list_.back(); 67 | command_list_.pop_back(); 68 | 69 | pthread_mutex_unlock(&mutex_); 70 | return quantity; 71 | } 72 | -------------------------------------------------------------------------------- /src/parse_cmd.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file robot_types.h 3 | * @author your name (you@domain.com) 4 | * @brief 5 | * @version 0.1 6 | * @date 2022-09-05 7 | * 8 | * @copyright Copyright (c) 2022 9 | * 10 | */ 11 | 12 | #include "parse_cmd.h" 13 | #include "command_list.h" 14 | using namespace std; 15 | using namespace x30; 16 | 17 | ParseCommand::ParseCommand(int port):local_port(port){ 18 | memset(&state_rec, 0, sizeof(state_rec)); 19 | } 20 | 21 | void ParseCommand::work() 22 | { 23 | CommandMessage cm; 24 | UDPServer udpServer; 25 | timespec test_time; 26 | 27 | udpServer.onRawMessageReceived = [&](const char* message, int length, string ipv4, uint16_t port) { 28 | clock_gettime(1, &test_time); 29 | memcpy(&cm,message,sizeof(cm)); 30 | Command nc(cm.command.code,cm.command.paramters_size, cm.data_buffer); 31 | if(cm.command.type == command_type::CommandType::kMessValues){ 32 | switch (cm.command.code){ 33 | case STATE_RECEIVE_CODE: 34 | clock_gettime(1, &test_time); 35 | memcpy(&state_rec, cm.data_buffer, sizeof(state_rec)); 36 | if(CallBack_){ 37 | CallBack_(STATE_RECEIVE_CODE); 38 | } 39 | break; 40 | default: 41 | break; 42 | } 43 | } 44 | if(message==nullptr||length==0){ 45 | setDataRevState(0); 46 | }else{ 47 | setDataRevState(1); 48 | } 49 | }; 50 | // Bind the server to a port. 51 | udpServer.Bind(local_port, [](int errorCode, string errorMessage) { 52 | // BINDING FAILED: 53 | cout << errorCode << " : " << errorMessage << endl; 54 | }); 55 | 56 | 57 | while (1){ 58 | sleep(1); 59 | } 60 | } 61 | 62 | void ParseCommand::startWork() 63 | { 64 | std::thread work_thread(std::bind(&ParseCommand::work, this)); 65 | work_thread.detach(); 66 | } 67 | 68 | RobotDataSDK& ParseCommand::getRecvState(){ 69 | return state_rec; 70 | } 71 | 72 | void ParseCommand::printData(){ 73 | std::cout.precision(3); 74 | std::cout << "time_tick: " << state_rec.tick << std::endl; 75 | std::cout << "Imu Data: " << std::endl; 76 | std::cout << "rpy: " << state_rec.imu.roll << " " << state_rec.imu.pitch << " " << state_rec.imu.yaw << std::endl; 77 | std::cout << "gyro: " << state_rec.imu.omega_x << " " << state_rec.imu.omega_y << " " << state_rec.imu.omega_z << std::endl; 78 | std::cout << "acc: " << state_rec.imu.acc_x << " " << state_rec.imu.acc_y << " " << state_rec.imu.acc_z << std::endl; 79 | 80 | std::cout << "Leg Data: " << std::endl; 81 | std::cout << "pos: "; 82 | for(int i=0;i<12;++i) std::cout << state_rec.joint_data[i].pos << " "; 83 | std::cout << std::endl; 84 | std::cout << "vel: "; 85 | for(int i=0;i<12;++i) std::cout << state_rec.joint_data[i].vel << " "; 86 | std::cout << std::endl; 87 | std::cout << "tor: "; 88 | for(int i=0;i<12;++i) std::cout << state_rec.joint_data[i].tor << " "; 89 | std::cout << std::endl; 90 | } 91 | -------------------------------------------------------------------------------- /src/send_to_robot.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file robot_types.h 3 | * @author your name (you@domain.com) 4 | * @brief 5 | * @version 0.1 6 | * @date 2022-09-05 7 | * 8 | * @copyright Copyright (c) 2022 9 | * 10 | */ 11 | 12 | 13 | #include "send_to_robot.h" 14 | 15 | using namespace std; 16 | using namespace x30; 17 | 18 | SendToRobot::SendToRobot(std::string ip, uint16_t port){ 19 | udp_socket_ = new UDPSocket(true); 20 | udp_socket_->Connect(ip, port); 21 | } 22 | 23 | // void SendToRobot::init(void){ 24 | // const string IP = ROBOT_IP; 25 | // const uint16_t PORT = ROBOT_PORT; 26 | // udp_socket_ = new UDPSocket(true); 27 | // udp_socket_->Connect(IP, PORT); 28 | // } 29 | 30 | void SendToRobot::cmd_done(Command& command_temp){ 31 | // Command command_temp; 32 | CommandMessage command_message={{0}}; 33 | uint32_t message_size = 0; 34 | command_message.command.code = command_temp.get_command_code().to_ulong(); 35 | 36 | if(command_temp.get_command_parameters()==0){ 37 | // 普通命令 38 | command_message.command.type = command_type::CommandType::kSingleValue; 39 | command_message.command.value = command_temp.get_command_value(); 40 | message_size = sizeof(command_message.command); 41 | } 42 | else{ 43 | // 带大量数据的命令 44 | command_message.command.type = command_type::CommandType::kMessValues; 45 | command_message.command.paramters_size = command_temp.get_command_parameters_size(); 46 | if(command_message.command.paramters_size > kCommandDataBufferSize){ 47 | std::cout <<"[Error E_Speaker] The message of over load !"<< std::endl; 48 | return; 49 | } 50 | // std::cout << "send " << count++ <Send(buffer,message_size); 63 | } 64 | 65 | void SendToRobot::work(){ 66 | 67 | } 68 | 69 | void SendToRobot::start_work(){ 70 | std::thread work_thread(std::bind(&SendToRobot::work, this)); 71 | work_thread.detach(); 72 | } 73 | 74 | void SendToRobot::set_send(RobotCmdSDK& cmd){ 75 | timespec time_sdk; 76 | static float i = 0.00; 77 | // cmd.fr_leg[2].tor = 3; 78 | clock_gettime(0, &time_sdk); 79 | // cout << "send " << time_sdk.tv_sec << ". " << time_sdk.tv_nsec << endl; 80 | size_t cmd_size = sizeof(cmd); 81 | char *buffer = new char[cmd_size]; 82 | memcpy(buffer, &cmd, cmd_size); 83 | Command command_temp(0x0111,sizeof(cmd), buffer); 84 | cmd_done(command_temp); 85 | 86 | } 87 | // void SendToRobot::set_send_pose(void){ 88 | // size_t cmd_size = sizeof(DataSendPose); 89 | // char *buffer = new char[cmd_size]; 90 | // // memcpy(buffer, &cmd, cmd_size); 91 | // // cmd_done(new Command(600,sizeof(DataSendPose), buffer)); 92 | // Command command_temp(600,sizeof(DataSendPose), buffer); 93 | // cmd_done(command_temp); 94 | // } 95 | 96 | void SendToRobot::robot_state_init(){ 97 | // Command command_temp(0x31010C05,0, 0); 98 | // cmd_done(command_temp); 99 | usleep(1000*1000*7); 100 | Command command_temp_1(0x0114,0,0); 101 | 102 | cmd_done(command_temp_1);//PS：超过5ms，未发数据set_send(cmd)，会失去控制权，要重新发送获取控制权 103 | cmd_done(command_temp_1);//PS：超过5ms，未发数据set_send(cmd)，会失去控制权，要重新发送获取控制权 104 | } 105 | void SendToRobot::set_cmd(uint32_t code , uint32_t value){ 106 | Command command_temp(code, value); 107 | cmd_done(command_temp); 108 | } 109 | 110 | 111 | void TimeTool::time_init(int ms){ 112 | tfd = timerfd_create(CLOCK_MONOTONIC, 0); //创建定时器 113 | if(tfd == -1) { 114 | printf("create timer fd fail \r\n"); 115 | } 116 | time_intv.it_value.tv_sec = 0; //设定2s超时 117 | time_intv.it_value.tv_nsec = 1000*1000*ms; 118 | time_intv.it_interval.tv_sec = time_intv.it_value.tv_sec; //每隔2s超时 119 | time_intv.it_interval.tv_nsec = time_intv.it_value.tv_nsec; 120 | 121 | printf("timer start ...\n"); 122 | timerfd_settime(tfd, 0, &time_intv, NULL); //启动定时器 123 | 124 | efd = epoll_create1(0); //创建epoll实例 125 | if (efd == -1) { 126 | printf("create epoll fail \r\n"); 127 | close(tfd); 128 | } 129 | 130 | evptr = (struct epoll_event *)calloc(1, sizeof(struct epoll_event)); 131 | if (evptr == NULL) { 132 | printf("epoll event calloc fail \r\n"); 133 | close(tfd); 134 | close(efd); 135 | } 136 | 137 | ev.data.fd = tfd; 138 | ev.events = EPOLLIN; //监听定时器读事件，当定时器超时时，定时器描述符可读。 139 | epoll_ctl(efd, EPOLL_CTL_ADD, tfd, &ev); //添加到epoll监听队列中 140 | } 141 | 142 | int TimeTool::time_interrupt(){ 143 | fds = epoll_wait(efd, evptr, 1, -1); //阻塞监听，直到有事件发生 144 | if(evptr[0].events & EPOLLIN){ 145 | ret = read(evptr->data.fd, &value, sizeof(uint64_t)); 146 | if (ret == -1) { 147 | printf("read return 1 -1, errno :%d \r\n", errno); 148 | return 1; 149 | } 150 | } 151 | return 0; 152 | } 153 | 154 | double TimeTool::get_now_time(double start_time){ 155 | clock_gettime(1,&system_time); 156 | return system_time.tv_sec + system_time.tv_nsec/1e9 - start_time; 157 | } 158 | 159 | double TimeTool::get_start_time(){ 160 | clock_gettime(1,&system_time); 161 | return system_time.tv_sec + system_time.tv_nsec/1e9; 162 | } 163 | --------------------------------------------------------------------------------