├── .gitignore ├── LICENSE ├── README.md ├── go1-math-motion ├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── include │ └── convert.h ├── launch │ ├── circle.launch │ ├── figure8.launch │ └── forward.launch ├── package.xml └── src │ ├── backward_walk.cpp │ ├── circle_walk.cpp │ ├── circle_walk.py │ ├── figure8.cpp │ ├── forward_walk.cpp │ └── twist_sub.cpp └── unitree_legged_msgs ├── CMakeLists.txt ├── msg ├── BmsCmd.msg ├── BmsState.msg ├── Cartesian.msg ├── HighCmd.msg ├── HighState.msg ├── IMU.msg ├── LED.msg ├── LowCmd.msg ├── LowState.msg ├── MotorCmd.msg └── MotorState.msg └── package.xml /.gitignore: -------------------------------------------------------------------------------- 1 | devel/ 2 | logs/ 3 | build/ 4 | bin/ 5 | lib/ 6 | msg_gen/ 7 | srv_gen/ 8 | msg/*Action.msg 9 | msg/*ActionFeedback.msg 10 | msg/*ActionGoal.msg 11 | msg/*ActionResult.msg 12 | msg/*Feedback.msg 13 | msg/*Goal.msg 14 | msg/*Result.msg 15 | msg/_*.py 16 | build_isolated/ 17 | devel_isolated/ 18 | 19 | # Generated by dynamic reconfigure 20 | *.cfgc 21 | /cfg/cpp/ 22 | /cfg/*.py 23 | 24 | # Ignore generated docs 25 | *.dox 26 | *.wikidoc 27 | 28 | # eclipse stuff 29 | .project 30 | .cproject 31 | 32 | # qcreator stuff 33 | CMakeLists.txt.user 34 | 35 | srv/_*.py 36 | *.pcd 37 | *.pyc 38 | qtcreator-* 39 | *.user 40 | 41 | /planning/cfg 42 | /planning/docs 43 | /planning/src 44 | 45 | *~ 46 | 47 | # Emacs 48 | .#* 49 | 50 | # Catkin custom files 51 | CATKIN_IGNORE 52 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2022 Dennis Baldwin 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 | # go1-math-motion 2 | 3 | Go1 high level control with ROS 4 | 5 | - git clone https://github.com/dbaldwin/go1-math-motion 6 | - cd go1-math-motion 7 | - docker run -p 6080:80 -v ${PWD}:/home/ubuntu/catkin_ws/src --shm-size=512m --name go1-ros-testing -d tiryoh/ros-desktop-vnc:melodic 8 | - http://localhost:6080 9 | - Open terminal and cd ~/catkin_ws/src 10 | - git clone -b v3.8.0 https://github.com/unitreerobotics/unitree_legged_sdk 11 | - cd ~/catkin_ws 12 | - catkin_make 13 | - source ~/catkin_ws/devel/setup.bash 14 | - roslaunch go1-math-motion circle.launch 15 | -------------------------------------------------------------------------------- /go1-math-motion/.gitignore: -------------------------------------------------------------------------------- 1 | devel/ 2 | logs/ 3 | build/ 4 | bin/ 5 | lib/ 6 | msg_gen/ 7 | srv_gen/ 8 | msg/*Action.msg 9 | msg/*ActionFeedback.msg 10 | msg/*ActionGoal.msg 11 | msg/*ActionResult.msg 12 | msg/*Feedback.msg 13 | msg/*Goal.msg 14 | msg/*Result.msg 15 | msg/_*.py 16 | build_isolated/ 17 | devel_isolated/ 18 | 19 | # Generated by dynamic reconfigure 20 | *.cfgc 21 | /cfg/cpp/ 22 | /cfg/*.py 23 | 24 | # Ignore generated docs 25 | *.dox 26 | *.wikidoc 27 | 28 | # eclipse stuff 29 | .project 30 | .cproject 31 | 32 | # qcreator stuff 33 | CMakeLists.txt.user 34 | 35 | srv/_*.py 36 | *.pcd 37 | *.pyc 38 | qtcreator-* 39 | *.user 40 | 41 | /planning/cfg 42 | /planning/docs 43 | /planning/src 44 | 45 | *~ 46 | 47 | # Emacs 48 | .#* 49 | 50 | # Catkin custom files 51 | CATKIN_IGNORE 52 | -------------------------------------------------------------------------------- /go1-math-motion/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(go1-math-motion) 3 | 4 | add_compile_options(-std=c++11) 5 | 6 | ## Find catkin macros and libraries 7 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 8 | ## is used, also find other catkin packages 9 | find_package(catkin REQUIRED COMPONENTS 10 | geometry_msgs 11 | roscpp 12 | rospy 13 | unitree_legged_msgs 14 | ) 15 | 16 | catkin_package() 17 | 18 | message("-- CMAKE_SYSTEM_PROCESSOR: ${CMAKE_SYSTEM_PROCESSOR}") 19 | if("${CMAKE_SYSTEM_PROCESSOR}" MATCHES "x86_64.*") 20 | set(ARCH amd64) 21 | else() 22 | set(ARCH arm64) 23 | endif() 24 | 25 | link_directories(${CMAKE_SOURCE_DIR}/unitree_legged_sdk/lib/cpp/${ARCH}) 26 | 27 | set(EXTRA_LIBS -pthread libunitree_legged_sdk.so) 28 | 29 | set(CMAKE_CXX_FLAGS "-O3 -fPIC") 30 | 31 | include_directories( 32 | include 33 | ${catkin_INCLUDE_DIRS} 34 | ${CMAKE_SOURCE_DIR}/unitree_legged_sdk/include 35 | ) 36 | 37 | add_executable(circle_walk src/circle_walk.cpp) 38 | target_link_libraries(circle_walk ${EXTRA_LIBS} ${catkin_LIBRARIES}) 39 | add_dependencies(circle_walk ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 40 | 41 | add_executable(figure8 src/figure8.cpp) 42 | target_link_libraries(figure8 ${EXTRA_LIBS} ${catkin_LIBRARIES}) 43 | add_dependencies(figure8 ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 44 | 45 | add_executable(forward_walk src/forward_walk.cpp) 46 | target_link_libraries(forward_walk ${EXTRA_LIBS} ${catkin_LIBRARIES}) 47 | add_dependencies(forward_walk ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 48 | 49 | add_executable(twist_sub src/twist_sub.cpp) 50 | target_link_libraries(twist_sub ${EXTRA_LIBS} ${catkin_LIBRARIES}) 51 | add_dependencies(twist_sub ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -------------------------------------------------------------------------------- /go1-math-motion/LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2022 Dennis Baldwin 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 | -------------------------------------------------------------------------------- /go1-math-motion/README.md: -------------------------------------------------------------------------------- 1 | catkin_create_pkg go1-math-motion roscpp rospy geometry_msgs unitree_legged_msgs 2 | -------------------------------------------------------------------------------- /go1-math-motion/include/convert.h: -------------------------------------------------------------------------------- 1 | /************************************************************************ 2 | Copyright (c) 2018-2019, 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 _CONVERT_H_ 7 | #define _CONVERT_H_ 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include "unitree_legged_sdk/unitree_legged_sdk.h" 19 | #include 20 | #include 21 | 22 | UNITREE_LEGGED_SDK::BmsCmd rosMsg2Cmd(const unitree_legged_msgs::BmsCmd &msg) 23 | { 24 | UNITREE_LEGGED_SDK::BmsCmd cmd; 25 | 26 | cmd.off = msg.off; 27 | 28 | for (int i(0); i < 3; i++) 29 | { 30 | cmd.reserve[i] = msg.reserve[i]; 31 | } 32 | 33 | return cmd; 34 | } 35 | 36 | UNITREE_LEGGED_SDK::HighCmd rosMsg2Cmd(const unitree_legged_msgs::HighCmd::ConstPtr &msg) 37 | { 38 | UNITREE_LEGGED_SDK::HighCmd cmd; 39 | 40 | for (int i(0); i < 2; i++) 41 | { 42 | cmd.head[i] = msg->head[i]; 43 | cmd.SN[i] = msg->SN[i]; 44 | cmd.version[i] = msg->version[i]; 45 | cmd.position[i] = msg->position[i]; 46 | cmd.velocity[i] = msg->velocity[i]; 47 | } 48 | 49 | for (int i(0); i < 3; i++) 50 | { 51 | cmd.euler[i] = msg->euler[i]; 52 | } 53 | 54 | for (int i(0); i < 4; i++) 55 | { 56 | cmd.led[i].r = msg->led[i].r; 57 | cmd.led[i].g = msg->led[i].g; 58 | cmd.led[i].b = msg->led[i].b; 59 | } 60 | 61 | for (int i(0); i < 40; i++) 62 | { 63 | cmd.wirelessRemote[i] = msg->wirelessRemote[i]; 64 | } 65 | 66 | cmd.levelFlag = msg->levelFlag; 67 | cmd.frameReserve = msg->frameReserve; 68 | cmd.bandWidth = msg->bandWidth; 69 | cmd.mode = msg->mode; 70 | cmd.gaitType = msg->gaitType; 71 | cmd.speedLevel = msg->speedLevel; 72 | cmd.footRaiseHeight = msg->footRaiseHeight; 73 | cmd.bodyHeight = msg->bodyHeight; 74 | cmd.yawSpeed = msg->yawSpeed; 75 | cmd.reserve = msg->reserve; 76 | cmd.crc = msg->crc; 77 | 78 | cmd.bms = rosMsg2Cmd(msg->bms); 79 | 80 | return cmd; 81 | } 82 | 83 | UNITREE_LEGGED_SDK::MotorCmd rosMsg2Cmd(const unitree_legged_msgs::MotorCmd &msg) 84 | { 85 | UNITREE_LEGGED_SDK::MotorCmd cmd; 86 | 87 | cmd.mode = msg.mode; 88 | cmd.q = msg.q; 89 | cmd.dq = msg.dq; 90 | cmd.tau = msg.tau; 91 | cmd.Kp = msg.Kp; 92 | cmd.Kd = msg.Kd; 93 | 94 | for (int i(0); i < 3; i++) 95 | { 96 | cmd.reserve[i] = msg.reserve[i]; 97 | } 98 | 99 | return cmd; 100 | } 101 | 102 | UNITREE_LEGGED_SDK::LowCmd rosMsg2Cmd(const unitree_legged_msgs::LowCmd::ConstPtr &msg) 103 | { 104 | UNITREE_LEGGED_SDK::LowCmd cmd; 105 | 106 | for (int i(0); i < 2; i++) 107 | { 108 | cmd.head[i] = msg->head[i]; 109 | cmd.SN[i] = msg->SN[i]; 110 | cmd.version[i] = msg->version[i]; 111 | } 112 | 113 | for (int i(0); i < 40; i++) 114 | { 115 | cmd.wirelessRemote[i] = msg->wirelessRemote[i]; 116 | } 117 | 118 | for (int i(0); i < 20; i++) 119 | { 120 | cmd.motorCmd[i] = rosMsg2Cmd(msg->motorCmd[i]); 121 | } 122 | 123 | cmd.bms = rosMsg2Cmd(msg->bms); 124 | 125 | cmd.levelFlag = msg->levelFlag; 126 | cmd.frameReserve = msg->frameReserve; 127 | cmd.bandWidth = msg->bandWidth; 128 | cmd.reserve = msg->reserve; 129 | cmd.crc = msg->crc; 130 | 131 | return cmd; 132 | } 133 | 134 | unitree_legged_msgs::MotorState state2rosMsg(UNITREE_LEGGED_SDK::MotorState &state) 135 | { 136 | unitree_legged_msgs::MotorState ros_msg; 137 | 138 | ros_msg.mode = state.mode; 139 | ros_msg.q = state.q; 140 | ros_msg.dq = state.dq; 141 | ros_msg.ddq = state.ddq; 142 | ros_msg.tauEst = state.tauEst; 143 | ros_msg.q_raw = state.q_raw; 144 | ros_msg.dq_raw = state.dq_raw; 145 | ros_msg.ddq_raw = state.ddq_raw; 146 | ros_msg.temperature = state.temperature; 147 | 148 | ros_msg.reserve[0] = state.reserve[0]; 149 | ros_msg.reserve[1] = state.reserve[1]; 150 | 151 | return ros_msg; 152 | } 153 | 154 | unitree_legged_msgs::IMU state2rosMsg(UNITREE_LEGGED_SDK::IMU &state) 155 | { 156 | unitree_legged_msgs::IMU ros_msg; 157 | 158 | for (int i(0); i < 4; i++) 159 | { 160 | ros_msg.quaternion[i] = state.quaternion[i]; 161 | } 162 | 163 | for (int i(0); i < 3; i++) 164 | { 165 | ros_msg.gyroscope[i] = state.gyroscope[i]; 166 | ros_msg.accelerometer[i] = state.accelerometer[i]; 167 | ros_msg.rpy[i] = state.rpy[i]; 168 | } 169 | 170 | ros_msg.temperature = state.temperature; 171 | 172 | return ros_msg; 173 | } 174 | 175 | unitree_legged_msgs::BmsState state2rosMsg(UNITREE_LEGGED_SDK::BmsState &state) 176 | { 177 | unitree_legged_msgs::BmsState ros_msg; 178 | 179 | for (int i(0); i < 2; i++) 180 | { 181 | ros_msg.BQ_NTC[i] = state.BQ_NTC[i]; 182 | ros_msg.MCU_NTC[i] = state.MCU_NTC[i]; 183 | } 184 | 185 | for (int i(0); i < 10; i++) 186 | { 187 | ros_msg.cell_vol[i] = state.cell_vol[i]; 188 | } 189 | 190 | ros_msg.version_h = state.version_h; 191 | ros_msg.version_l = state.version_l; 192 | ros_msg.bms_status = state.bms_status; 193 | ros_msg.SOC = state.SOC; 194 | ros_msg.current = state.current; 195 | ros_msg.cycle = state.cycle; 196 | 197 | return ros_msg; 198 | } 199 | 200 | unitree_legged_msgs::LowState state2rosMsg(UNITREE_LEGGED_SDK::LowState &state) 201 | { 202 | unitree_legged_msgs::LowState ros_msg; 203 | 204 | for (int i(0); i < 2; i++) 205 | { 206 | ros_msg.head[i] = state.head[i]; 207 | ros_msg.SN[i] = state.SN[i]; 208 | ros_msg.version[i] = state.version[i]; 209 | } 210 | 211 | for (int i(0); i < 4; i++) 212 | { 213 | ros_msg.footForce[i] = state.footForce[i]; 214 | ros_msg.footForceEst[i] = state.footForceEst[i]; 215 | } 216 | 217 | for (int i(0); i < 40; i++) 218 | { 219 | ros_msg.wirelessRemote[i] = state.wirelessRemote[i]; 220 | } 221 | 222 | for (int i(0); i < 20; i++) 223 | { 224 | ros_msg.motorState[i] = state2rosMsg(state.motorState[i]); 225 | } 226 | 227 | ros_msg.imu = state2rosMsg(state.imu); 228 | 229 | ros_msg.bms = state2rosMsg(state.bms); 230 | 231 | ros_msg.tick = state.tick; 232 | ros_msg.reserve = state.reserve; 233 | ros_msg.crc = state.crc; 234 | 235 | return ros_msg; 236 | } 237 | 238 | unitree_legged_msgs::Cartesian state2rosMsg(UNITREE_LEGGED_SDK::Cartesian &state) 239 | { 240 | unitree_legged_msgs::Cartesian ros_msg; 241 | 242 | ros_msg.x = state.x; 243 | ros_msg.y = state.y; 244 | ros_msg.z = state.z; 245 | 246 | return ros_msg; 247 | } 248 | 249 | unitree_legged_msgs::HighState state2rosMsg(UNITREE_LEGGED_SDK::HighState &state) 250 | { 251 | unitree_legged_msgs::HighState ros_msg; 252 | 253 | for (int i(0); i < 2; i++) 254 | { 255 | ros_msg.head[i] = state.head[i]; 256 | ros_msg.SN[i] = state.SN[i]; 257 | ros_msg.version[i] = state.version[i]; 258 | } 259 | 260 | for (int i(0); i < 4; i++) 261 | { 262 | ros_msg.footForce[i] = state.footForce[i]; 263 | ros_msg.footForceEst[i] = state.footForceEst[i]; 264 | ros_msg.rangeObstacle[i] = state.rangeObstacle[i]; 265 | ros_msg.footPosition2Body[i] = state2rosMsg(state.footPosition2Body[i]); 266 | ros_msg.footSpeed2Body[i] = state2rosMsg(state.footSpeed2Body[i]); 267 | } 268 | 269 | for (int i(0); i < 3; i++) 270 | { 271 | ros_msg.position[i] = state.position[i]; 272 | ros_msg.velocity[i] = state.velocity[i]; 273 | } 274 | 275 | for (int i(0); i < 40; i++) 276 | { 277 | ros_msg.wirelessRemote[i] = state.wirelessRemote[i]; 278 | } 279 | 280 | for (int i(0); i < 20; i++) 281 | { 282 | ros_msg.motorState[i] = state2rosMsg(state.motorState[i]); 283 | } 284 | 285 | ros_msg.imu = state2rosMsg(state.imu); 286 | 287 | ros_msg.bms = state2rosMsg(state.bms); 288 | 289 | ros_msg.levelFlag = state.levelFlag; 290 | ros_msg.frameReserve = state.frameReserve; 291 | ros_msg.bandWidth = state.bandWidth; 292 | ros_msg.mode = state.mode; 293 | ros_msg.progress = state.progress; 294 | ros_msg.gaitType = state.gaitType; 295 | ros_msg.footRaiseHeight = state.footRaiseHeight; 296 | ros_msg.bodyHeight = state.bodyHeight; 297 | ros_msg.yawSpeed = state.yawSpeed; 298 | ros_msg.reserve = state.reserve; 299 | ros_msg.crc = state.crc; 300 | 301 | return ros_msg; 302 | } 303 | 304 | UNITREE_LEGGED_SDK::HighCmd rosMsg2Cmd(const geometry_msgs::Twist::ConstPtr &msg) 305 | { 306 | UNITREE_LEGGED_SDK::HighCmd cmd; 307 | 308 | cmd.head[0] = 0xFE; 309 | cmd.head[1] = 0xEF; 310 | cmd.levelFlag = UNITREE_LEGGED_SDK::HIGHLEVEL; 311 | cmd.mode = 0; 312 | cmd.gaitType = 0; 313 | cmd.speedLevel = 0; 314 | cmd.footRaiseHeight = 0; 315 | cmd.bodyHeight = 0; 316 | cmd.euler[0] = 0; 317 | cmd.euler[1] = 0; 318 | cmd.euler[2] = 0; 319 | cmd.velocity[0] = 0.0f; 320 | cmd.velocity[1] = 0.0f; 321 | cmd.yawSpeed = 0.0f; 322 | cmd.reserve = 0; 323 | 324 | cmd.velocity[0] = msg->linear.x; 325 | cmd.velocity[1] = msg->linear.y; 326 | cmd.yawSpeed = msg->angular.z; 327 | 328 | cmd.mode = 2; 329 | cmd.gaitType = 1; 330 | 331 | return cmd; 332 | } 333 | 334 | #endif // _CONVERT_H_ -------------------------------------------------------------------------------- /go1-math-motion/launch/circle.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /go1-math-motion/launch/figure8.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /go1-math-motion/launch/forward.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /go1-math-motion/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | go1-math-motion 4 | 0.0.0 5 | The go1-math-motion package 6 | 7 | 8 | 9 | 10 | ubuntu 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 | geometry_msgs 53 | roscpp 54 | rospy 55 | unitree_legged_msgs 56 | geometry_msgs 57 | roscpp 58 | rospy 59 | unitree_legged_msgs 60 | geometry_msgs 61 | roscpp 62 | rospy 63 | unitree_legged_msgs 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | -------------------------------------------------------------------------------- /go1-math-motion/src/backward_walk.cpp: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | #include 3 | 4 | int main(int argc, char **argv) 5 | { 6 | ros::init(argc, argv, "circle_walk"); 7 | 8 | ros::NodeHandle nh; 9 | 10 | ros::Rate loop_rate(500); 11 | 12 | ros::Publisher pub = nh.advertise("/cmd_vel", 1); 13 | 14 | geometry_msgs::Twist twist; 15 | 16 | while (ros::ok()) 17 | { 18 | twist.linear.x = 1; // radius (meters) 19 | twist.linear.y = 0; 20 | twist.linear.z = 0; 21 | twist.angular.x = 0; 22 | twist.angular.y = 0; 23 | twist.angular.z = 0; // direction (positive = left, negative = right) 24 | 25 | pub.publish(twist); 26 | 27 | ros::spinOnce(); 28 | loop_rate.sleep(); 29 | } 30 | 31 | return 0; 32 | } 33 | -------------------------------------------------------------------------------- /go1-math-motion/src/circle_walk.cpp: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | #include 3 | 4 | int main(int argc, char **argv) 5 | { 6 | ros::init(argc, argv, "circle_walk"); 7 | 8 | ros::NodeHandle nh; 9 | 10 | ros::Rate loop_rate(500); 11 | 12 | ros::Publisher pub = nh.advertise("/cmd_vel", 1); 13 | 14 | geometry_msgs::Twist twist; 15 | 16 | while (ros::ok()) 17 | { 18 | twist.linear.x = 0.5; // radius (meters) 19 | twist.linear.y = 0; 20 | twist.linear.z = 0; 21 | twist.angular.x = 0; 22 | twist.angular.y = 0; 23 | twist.angular.z = 1; // direction (positive = left, negative = right) 24 | 25 | pub.publish(twist); 26 | 27 | ros::spinOnce(); 28 | loop_rate.sleep(); 29 | } 30 | 31 | return 0; 32 | } 33 | -------------------------------------------------------------------------------- /go1-math-motion/src/circle_walk.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | from geometry_msgs.msg import Twist 3 | import rospy 4 | import sys 5 | 6 | def circle(radius): 7 | rospy.init_node("circle_walk", anonymous=True) 8 | pub = rospy.Publisher("/turtle1/cmd_vel", Twist, queue_size=10) 9 | rate = rospy.Rate(10) 10 | vel = Twist() 11 | 12 | while not rospy.is_shutdown(): 13 | vel.linear.x = radius 14 | vel.linear.y = 0 15 | vel.linear.z = 0 16 | vel.angular.x = 0 17 | vel.angular.y = 0 18 | vel.angular.z = 1 19 | pub.publish(vel) 20 | rate.sleep() 21 | 22 | 23 | if __name__ == '__main__': 24 | try: 25 | circle(float(sys.argv[1])) 26 | except rospy.ROSInterruptException: 27 | pass 28 | -------------------------------------------------------------------------------- /go1-math-motion/src/figure8.cpp: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | #include "geometry_msgs/Twist.h" 3 | #include 4 | 5 | #define PI 3.14 6 | 7 | int main(int argc, char **argv) 8 | { 9 | ros::init(argc, argv, "dennis_test"); 10 | ros::NodeHandle nh; 11 | ros::Publisher pub = nh.advertise("/cmd_vel", 10); 12 | ros::Rate rate(10); 13 | 14 | geometry_msgs::Twist vel_msg; 15 | 16 | double t = 0.0; 17 | double T = 10; 18 | 19 | 20 | while(ros::ok()) { 21 | 22 | // Linear velocity 23 | double xdot = 3.0 * cos(4.0*PI*t/T) * 4.0 * PI/T; 24 | double ydot = 3.0 * cos(2.0*PI*t/T) * 2.0 * PI/T; 25 | double lv = sqrt(xdot*xdot + ydot*ydot); 26 | 27 | // Angular velocity 28 | double xdotdot = -3.0 * sin(4*PI*t/T) * (4.0*PI/T) * (4.0*PI/T); 29 | double ydotdot = -3.0 * sin(2*PI*t/T) * (2.0*PI/T) * (2.0*PI/T); 30 | double av = (ydotdot * xdot - xdotdot * ydot) / (xdot*xdot + ydot*ydot); 31 | 32 | vel_msg.linear.x = lv / 5.0; 33 | vel_msg.angular.z = av; 34 | 35 | pub.publish(vel_msg); 36 | 37 | if (t == T) { 38 | t = 0.0; 39 | } else { 40 | t = t + 0.1; 41 | } 42 | 43 | ros::spinOnce(); 44 | rate.sleep(); 45 | } 46 | 47 | 48 | return 0; 49 | 50 | 51 | 52 | } 53 | -------------------------------------------------------------------------------- /go1-math-motion/src/forward_walk.cpp: -------------------------------------------------------------------------------- 1 | #include "ros/ros.h" 2 | #include 3 | 4 | int main(int argc, char **argv) 5 | { 6 | ros::init(argc, argv, "forward_walk"); 7 | 8 | ros::NodeHandle nh; 9 | 10 | ros::Rate loop_rate(500); 11 | 12 | ros::Publisher pub = nh.advertise("/turtle1/cmd_vel", 1); 13 | 14 | geometry_msgs::Twist twist; 15 | 16 | ros::Time beginTime = ros::Time::now(); 17 | ros::Duration duration = ros::Duration(3); 18 | ros::Time endTime = beginTime + duration; 19 | 20 | while (ros::ok() && ros::Time::now() < endTime) 21 | { 22 | twist.linear.x = -1; // forward / back 23 | twist.linear.y = 0; 24 | twist.linear.z = 0; 25 | twist.angular.x = 0; 26 | twist.angular.y = 0; 27 | twist.angular.z = 0; 28 | 29 | pub.publish(twist); 30 | 31 | ros::spinOnce(); 32 | loop_rate.sleep(); 33 | } 34 | 35 | return 0; 36 | } 37 | -------------------------------------------------------------------------------- /go1-math-motion/src/twist_sub.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include "unitree_legged_sdk/unitree_legged_sdk.h" 7 | #include "convert.h" 8 | #include 9 | #include 10 | #include 11 | 12 | using namespace UNITREE_LEGGED_SDK; 13 | class Custom 14 | { 15 | public: 16 | UDP low_udp; 17 | UDP high_udp; 18 | 19 | HighCmd high_cmd = {0}; 20 | HighState high_state = {0}; 21 | 22 | LowCmd low_cmd = {0}; 23 | LowState low_state = {0}; 24 | 25 | public: 26 | Custom() 27 | : 28 | low_udp(LOWLEVEL, 8091, "192.168.123.10", 8007), 29 | high_udp(8090, "192.168.123.161", 8082, sizeof(HighCmd), sizeof(HighState)) 30 | { 31 | high_udp.InitCmdData(high_cmd); 32 | low_udp.InitCmdData(low_cmd); 33 | } 34 | 35 | void highUdpSend() 36 | { 37 | high_udp.SetSend(high_cmd); 38 | high_udp.Send(); 39 | } 40 | 41 | void lowUdpSend() 42 | { 43 | 44 | low_udp.SetSend(low_cmd); 45 | low_udp.Send(); 46 | } 47 | 48 | void lowUdpRecv() 49 | { 50 | 51 | low_udp.Recv(); 52 | low_udp.GetRecv(low_state); 53 | } 54 | 55 | void highUdpRecv() 56 | { 57 | high_udp.Recv(); 58 | high_udp.GetRecv(high_state); 59 | } 60 | }; 61 | 62 | Custom custom; 63 | 64 | ros::Subscriber sub_cmd_vel; 65 | ros::Publisher pub_high; 66 | 67 | long cmd_vel_count = 0; 68 | 69 | void cmdVelCallback(const geometry_msgs::Twist::ConstPtr &msg) 70 | { 71 | printf("cmdVelCallback is running!\t%ld\n", cmd_vel_count); 72 | 73 | custom.high_cmd = rosMsg2Cmd(msg); 74 | 75 | printf("cmd_x_vel = %f\n", custom.high_cmd.velocity[0]); 76 | printf("cmd_y_vel = %f\n", custom.high_cmd.velocity[1]); 77 | printf("cmd_yaw_vel = %f\n", custom.high_cmd.yawSpeed); 78 | 79 | unitree_legged_msgs::HighState high_state_ros; 80 | 81 | high_state_ros = state2rosMsg(custom.high_state); 82 | 83 | pub_high.publish(high_state_ros); 84 | 85 | printf("cmdVelCallback ending!\t%ld\n\n", cmd_vel_count++); 86 | } 87 | 88 | int main(int argc, char **argv) 89 | { 90 | ros::init(argc, argv, "twist_sub"); 91 | 92 | ros::NodeHandle nh; 93 | 94 | pub_high = nh.advertise("high_state", 1); 95 | 96 | sub_cmd_vel = nh.subscribe("cmd_vel", 1, cmdVelCallback); 97 | 98 | LoopFunc loop_udpSend("high_udp_send", 0.002, 3, boost::bind(&Custom::highUdpSend, &custom)); 99 | LoopFunc loop_udpRecv("high_udp_recv", 0.002, 3, boost::bind(&Custom::highUdpRecv, &custom)); 100 | 101 | loop_udpSend.start(); 102 | loop_udpRecv.start(); 103 | 104 | ros::spin(); 105 | 106 | return 0; 107 | } -------------------------------------------------------------------------------- /unitree_legged_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(unitree_legged_msgs) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | message_generation 6 | std_msgs 7 | geometry_msgs 8 | sensor_msgs 9 | ) 10 | 11 | add_message_files( 12 | FILES 13 | MotorCmd.msg 14 | MotorState.msg 15 | BmsCmd.msg 16 | BmsState.msg 17 | Cartesian.msg 18 | IMU.msg 19 | LED.msg 20 | LowCmd.msg 21 | LowState.msg 22 | HighCmd.msg 23 | HighState.msg 24 | ) 25 | 26 | generate_messages( 27 | DEPENDENCIES 28 | std_msgs 29 | geometry_msgs 30 | sensor_msgs 31 | ) 32 | 33 | catkin_package( 34 | CATKIN_DEPENDS 35 | message_runtime 36 | std_msgs 37 | geometry_msgs 38 | sensor_msgs 39 | ) 40 | 41 | ############# 42 | ## Install ## 43 | ############# 44 | 45 | # Mark topic names header files for installation 46 | install( 47 | DIRECTORY include/${PROJECT_NAME}/ 48 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 49 | FILES_MATCHING PATTERN "*.h" 50 | ) -------------------------------------------------------------------------------- /unitree_legged_msgs/msg/BmsCmd.msg: -------------------------------------------------------------------------------- 1 | uint8 off # off 0xA5 2 | uint8[3] reserve -------------------------------------------------------------------------------- /unitree_legged_msgs/msg/BmsState.msg: -------------------------------------------------------------------------------- 1 | uint8 version_h 2 | uint8 version_l 3 | uint8 bms_status 4 | uint8 SOC # SOC 0-100% 5 | int32 current # mA 6 | uint16 cycle 7 | int8[2] BQ_NTC # x1 degrees centigrade 8 | int8[2] MCU_NTC # x1 degrees centigrade 9 | uint16[10] cell_vol # cell voltage mV -------------------------------------------------------------------------------- /unitree_legged_msgs/msg/Cartesian.msg: -------------------------------------------------------------------------------- 1 | float32 x 2 | float32 y 3 | float32 z -------------------------------------------------------------------------------- /unitree_legged_msgs/msg/HighCmd.msg: -------------------------------------------------------------------------------- 1 | uint8[2] head 2 | uint8 levelFlag 3 | uint8 frameReserve 4 | 5 | uint32[2] SN 6 | uint32[2] version 7 | uint16 bandWidth 8 | uint8 mode 9 | 10 | uint8 gaitType 11 | uint8 speedLevel 12 | float32 footRaiseHeight 13 | float32 bodyHeight 14 | float32[2] position 15 | float32[3] euler 16 | float32[2] velocity 17 | float32 yawSpeed 18 | BmsCmd bms 19 | LED[4] led 20 | uint8[40] wirelessRemote 21 | uint32 reserve 22 | 23 | uint32 crc -------------------------------------------------------------------------------- /unitree_legged_msgs/msg/HighState.msg: -------------------------------------------------------------------------------- 1 | 2 | uint8[2] head 3 | uint8 levelFlag 4 | uint8 frameReserve 5 | 6 | uint32[2] SN 7 | uint32[2] version 8 | uint16 bandWidth 9 | IMU imu 10 | MotorState[20] motorState 11 | BmsState bms 12 | int16[4] footForce 13 | int16[4] footForceEst 14 | uint8 mode 15 | float32 progress 16 | uint8 gaitType 17 | float32 footRaiseHeight 18 | float32[3] position 19 | float32 bodyHeight 20 | float32[3] velocity 21 | float32 yawSpeed 22 | float32[4] rangeObstacle 23 | Cartesian[4] footPosition2Body 24 | Cartesian[4] footSpeed2Body 25 | uint8[40] wirelessRemote 26 | uint32 reserve 27 | 28 | uint32 crc -------------------------------------------------------------------------------- /unitree_legged_msgs/msg/IMU.msg: -------------------------------------------------------------------------------- 1 | float32[4] quaternion 2 | float32[3] gyroscope 3 | float32[3] accelerometer 4 | float32[3] rpy 5 | int8 temperature -------------------------------------------------------------------------------- /unitree_legged_msgs/msg/LED.msg: -------------------------------------------------------------------------------- 1 | uint8 r 2 | uint8 g 3 | uint8 b -------------------------------------------------------------------------------- /unitree_legged_msgs/msg/LowCmd.msg: -------------------------------------------------------------------------------- 1 | 2 | uint8[2] head 3 | uint8 levelFlag 4 | uint8 frameReserve 5 | 6 | uint32[2] SN 7 | uint32[2] version 8 | uint16 bandWidth 9 | MotorCmd[20] motorCmd 10 | BmsCmd bms 11 | uint8[40] wirelessRemote 12 | uint32 reserve 13 | 14 | uint32 crc -------------------------------------------------------------------------------- /unitree_legged_msgs/msg/LowState.msg: -------------------------------------------------------------------------------- 1 | 2 | uint8[2] head 3 | uint8 levelFlag 4 | uint8 frameReserve 5 | 6 | uint32[2] SN 7 | uint32[2] version 8 | uint16 bandWidth 9 | IMU imu 10 | MotorState[20] motorState 11 | BmsState bms 12 | int16[4] footForce 13 | int16[4] footForceEst 14 | uint32 tick 15 | uint8[40] wirelessRemote 16 | uint32 reserve 17 | uint32 crc 18 | 19 | # Old version Aliengo does not have: 20 | Cartesian[4] eeForceRaw 21 | Cartesian[4] eeForce #it's a 1-DOF force infact, but we use 3-DOF here just for visualization 22 | Cartesian position # will delete 23 | Cartesian velocity # will delete 24 | Cartesian velocity_w # will delete 25 | -------------------------------------------------------------------------------- /unitree_legged_msgs/msg/MotorCmd.msg: -------------------------------------------------------------------------------- 1 | uint8 mode # motor target mode 2 | float32 q # motor target position 3 | float32 dq # motor target velocity 4 | float32 tau # motor target torque 5 | float32 Kp # motor spring stiffness coefficient 6 | float32 Kd # motor damper coefficient 7 | uint32[3] reserve # motor target torque -------------------------------------------------------------------------------- /unitree_legged_msgs/msg/MotorState.msg: -------------------------------------------------------------------------------- 1 | uint8 mode # motor current mode 2 | float32 q # motor current position（rad） 3 | float32 dq # motor current speed（rad/s） 4 | float32 ddq # motor current speed（rad/s） 5 | float32 tauEst # current estimated output torque（N*m） 6 | float32 q_raw # motor current position（rad） 7 | float32 dq_raw # motor current speed（rad/s） 8 | float32 ddq_raw # motor current speed（rad/s） 9 | int8 temperature # motor temperature（slow conduction of temperature leads to lag） 10 | uint32[2] reserve -------------------------------------------------------------------------------- /unitree_legged_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | unitree_legged_msgs 5 | 0.0.0 6 | 7 | The test messgaes package. 8 | 9 | unitree 10 | TODO 11 | 12 | catkin 13 | message_runtime 14 | message_generation 15 | std_msgs 16 | geometry_msgs 17 | sensor_msgs 18 | 19 | --------------------------------------------------------------------------------