From e3e96db9149fb6b251be16c90ced6a845d232a8b Mon Sep 17 00:00:00 2001 From: LuoBin <2646887443@qq.com> Date: Tue, 17 Jan 2023 02:35:00 -0500 Subject: [PATCH] v3.8.3 support B1 --- README.md | 59 +-- unitree_legged_msgs/msg/BmsState.msg | 6 +- unitree_legged_msgs/msg/HighCmd.msg | 4 +- unitree_legged_msgs/msg/LowState.msg | 7 +- unitree_legged_real/CMakeLists.txt | 19 +- unitree_legged_real/include/convert.h | 361 +++++++++--------- .../launch/keyboard_control.launch | 8 - .../src/exe/control_via_keyboard.cpp | 111 ------ .../src/exe/example_position.cpp | 90 +++-- unitree_legged_real/src/exe/example_walk.cpp | 97 +++-- unitree_legged_real/src/exe/ros_udp.cpp | 18 +- unitree_legged_real/src/exe/state_sub.cpp | 39 -- unitree_legged_real/src/exe/twist_sub.cpp | 112 ------ 13 files changed, 317 insertions(+), 614 deletions(-) delete mode 100644 unitree_legged_real/launch/keyboard_control.launch delete mode 100644 unitree_legged_real/src/exe/control_via_keyboard.cpp delete mode 100644 unitree_legged_real/src/exe/state_sub.cpp delete mode 100644 unitree_legged_real/src/exe/twist_sub.cpp diff --git a/README.md b/README.md index e18d9e9..149cd46 100644 --- a/README.md +++ b/README.md @@ -1,9 +1,9 @@ -Packages Version: v3.8.0 +Packages Version: v3.8.3 # Introduction This package can send control command to real robot from ROS. You can do low-level control(namely control all joints on robot) and high-level control(namely control the walking direction and speed of robot). -This version is suitable for unitree_legged_sdk v3.5.1, namely Go1 robot. +This version is suitable for unitree_legged_sdk v3.8.3, namely B1 robot. ## Packages: @@ -12,51 +12,24 @@ Basic message function: `unitree_legged_msgs` The interface between ROS and real robot: `unitree_legged_real` ## Environment -We recommand users to run this package in Ubuntu 18.04 and ROS melodic environment +We recommand users to run this package in Ubuntu 20.04 and ROS neotic environment -## Dependencies +## s * [unitree_legged_sdk](https://github.com/unitreerobotics/unitree_legged_sdk/releases) ### Notice -The newest release [v3.8.0](https://github.com/unitreerobotics/unitree_legged_sdk/releases/tag/3.8.0) only supports for robot: Go1. - -Check release [v3.3.4](https://github.com/unitreerobotics/unitree_legged_sdk/releases/tag/3.3.4) for A1 support. - -# Configuration -Before compiling this package, please download the corresponding unitree_legged_sdk as noted above, and put it to your own workspace's source folder(e.g. `~/catkin_ws/src`). Be careful with the sdk folder name. It should be "unitree_legged_sdk" without version tag. +The release [v3.8.3](https://github.com/unitreerobotics/unitree_legged_sdk/releases/tag/v3.8.3) only supports for robot: B1. # Build -You can use catkin_make to build ROS packages. First copy the package folder to `~/catkin_ws/src`, then: +You can use catkin_make to build ROS packages. First copy the package folder to `~/catkin_ws/src`, then download the corresponding unitree_legged_sdk into `~/catkin_ws/src/unitree_ros_to_real`.Be careful with the sdk folder name. It should be "unitree_legged_sdk" without version tag: ``` cd ~/catkin_ws catkin_make ``` -# Setup the net connection -First, please connect the network cable between your PC and robot. Then run `ifconfig` in a terminal, you will find your port name. For example, `enx000ec6612921`. - -Then, open the `ipconfig.sh` file under the folder `unitree_legged_real`, modify the port name to your own. And run the following commands: -``` -sudo chmod +x ipconfig.sh -sudo ./ipconfig.sh -``` -If you run the `ifconfig` again, you will find that port has `inet` and `netmask` now. -In order to set your port automatically, you can modify `interfaces`: -``` -sudo gedit /etc/network/interfaces -``` -And add the following 4 lines at the end: -``` -auto enx000ec6612921 -iface enx000ec6612921 inet static -address 192.168.123.162 -netmask 255.255.255.0 -``` -Where the port name have to be changed to your own. - # Run the package -You can control your real Go1 robot from ROS by this package. +You can control your real B1 robot from ROS by this package. -Before you run expamle program, please run command +Before you run expamle program, please run ``` roslaunch unitree_legged_real real.launch ctrl_level:=highlevel @@ -68,25 +41,15 @@ roslaunch unitree_legged_real real.launch ctrl_level:=lowlevel It depends which control mode you want to use. -Then, if you want to run high-level control mode, you can run example_walk node like this +Then, if you want to run high-level control mode, you can run example_walk node ``` rosrun unitree_legged_real example_walk ``` -If you want to run low-level control mode, you can run example_position program node like this +If you want to run low-level control mode, you can run example_position program node ``` rosrun unitree_legged_real example_postion ``` -You can also run the node state_sub to subscribe the feedback information from Go1 robot -``` -rosrun unitree_legged_real state_sub -``` - -You can also run the launch file that enables you control robot via keyboard like you can do in turtlesim package -``` -roslaunch unitree_legged_real keyboard_control.launch -``` - And before you do the low-level control, please press L2+A to sit the robot down and then press L1+L2+start to make the robot into -mode in which you can do joint-level control, finally make sure you hang the robot up before you run low-level control. +mode in which you can do low-level control, finally make sure you hang the robot up before you run low-level control. diff --git a/unitree_legged_msgs/msg/BmsState.msg b/unitree_legged_msgs/msg/BmsState.msg index 7d54c56..5a38959 100644 --- a/unitree_legged_msgs/msg/BmsState.msg +++ b/unitree_legged_msgs/msg/BmsState.msg @@ -4,6 +4,6 @@ uint8 bms_status uint8 SOC # SOC 0-100% int32 current # mA uint16 cycle -int8[2] BQ_NTC # x1 degrees centigrade -int8[2] MCU_NTC # x1 degrees centigrade -uint16[10] cell_vol # cell voltage mV \ No newline at end of file +int8[8] BQ_NTC # x1 degrees centigrade +int8[8] MCU_NTC # x1 degrees centigrade +uint16[30] cell_vol # cell voltage mV \ No newline at end of file diff --git a/unitree_legged_msgs/msg/HighCmd.msg b/unitree_legged_msgs/msg/HighCmd.msg index 407299f..7c98550 100644 --- a/unitree_legged_msgs/msg/HighCmd.msg +++ b/unitree_legged_msgs/msg/HighCmd.msg @@ -14,7 +14,9 @@ float32 bodyHeight float32[2] position float32[3] euler float32[2] velocity -float32 yawSpeed +float32 yawSpeed +float32[2] dComXy +float32[2] dstandFootXy BmsCmd bms LED[4] led uint8[40] wirelessRemote diff --git a/unitree_legged_msgs/msg/LowState.msg b/unitree_legged_msgs/msg/LowState.msg index c5c26a2..5572540 100644 --- a/unitree_legged_msgs/msg/LowState.msg +++ b/unitree_legged_msgs/msg/LowState.msg @@ -16,10 +16,5 @@ uint8[40] wirelessRemote uint32 reserve uint32 crc -# Old version Aliengo does not have: -Cartesian[4] eeForceRaw -Cartesian[4] eeForce #it's a 1-DOF force infact, but we use 3-DOF here just for visualization -Cartesian position # will delete -Cartesian velocity # will delete -Cartesian velocity_w # will delete + diff --git a/unitree_legged_real/CMakeLists.txt b/unitree_legged_real/CMakeLists.txt index d0f1fff..9877c5a 100755 --- a/unitree_legged_real/CMakeLists.txt +++ b/unitree_legged_real/CMakeLists.txt @@ -18,7 +18,7 @@ else() set(ARCH arm64) endif() -link_directories(${CMAKE_SOURCE_DIR}/unitree_legged_sdk/lib/cpp/${ARCH}) +link_directories(${CMAKE_SOURCE_DIR}/unitree_ros_to_real/unitree_legged_sdk/lib/cpp/${ARCH}) set(EXTRA_LIBS -pthread libunitree_legged_sdk.so) @@ -27,16 +27,11 @@ set(CMAKE_CXX_FLAGS "-O3 -fPIC") include_directories( include ${catkin_INCLUDE_DIRS} - ${CMAKE_SOURCE_DIR}/unitree_legged_sdk/include + ${CMAKE_SOURCE_DIR}/unitree_ros_to_real/unitree_legged_sdk/include ) - - - - - add_executable(example_walk src/exe/example_walk.cpp) target_link_libraries(example_walk ${EXTRA_LIBS} ${catkin_LIBRARIES}) add_dependencies(example_walk ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) @@ -45,19 +40,9 @@ add_executable(example_position src/exe/example_position.cpp) target_link_libraries(example_position ${EXTRA_LIBS} ${catkin_LIBRARIES}) add_dependencies(example_position ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -add_executable(state_sub src/exe/state_sub.cpp) -target_link_libraries(state_sub ${EXTRA_LIBS} ${catkin_LIBRARIES}) -add_dependencies(state_sub ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - add_executable(ros_udp src/exe/ros_udp.cpp) target_link_libraries(ros_udp ${EXTRA_LIBS} ${catkin_LIBRARIES}) add_dependencies(ros_udp ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -add_executable(control_via_keyboard src/exe/control_via_keyboard.cpp) -target_link_libraries(control_via_keyboard ${EXTRA_LIBS} ${catkin_LIBRARIES}) -add_dependencies(control_via_keyboard ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) -add_executable(twist_sub src/exe/twist_sub.cpp) -target_link_libraries(twist_sub ${EXTRA_LIBS} ${catkin_LIBRARIES}) -add_dependencies(twist_sub ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) diff --git a/unitree_legged_real/include/convert.h b/unitree_legged_real/include/convert.h index f71a838..9c7f910 100755 --- a/unitree_legged_real/include/convert.h +++ b/unitree_legged_real/include/convert.h @@ -6,18 +6,20 @@ Use of this source code is governed by the MPL-2.0 license, see LICENSE. #ifndef _CONVERT_H_ #define _CONVERT_H_ -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include "unitree_legged_msgs/BmsCmd.h" +#include "unitree_legged_msgs/BmsState.h" +#include "unitree_legged_msgs/Cartesian.h" +#include "unitree_legged_msgs/HighCmd.h" +#include "unitree_legged_msgs/HighState.h" +#include "unitree_legged_msgs/IMU.h" +#include "unitree_legged_msgs/LED.h" +#include "unitree_legged_msgs/LowCmd.h" +#include "unitree_legged_msgs/LowState.h" +#include "unitree_legged_msgs/MotorCmd.h" +#include "unitree_legged_msgs/MotorState.h" + #include "unitree_legged_sdk/unitree_legged_sdk.h" -#include -#include +#include "ros/ros.h" UNITREE_LEGGED_SDK::BmsCmd rosMsg2Cmd(const unitree_legged_msgs::BmsCmd &msg) { @@ -25,57 +27,75 @@ UNITREE_LEGGED_SDK::BmsCmd rosMsg2Cmd(const unitree_legged_msgs::BmsCmd &msg) cmd.off = msg.off; - for (int i(0); i < 3; i++) - { + for (std::size_t i(0); i < 3; i++) cmd.reserve[i] = msg.reserve[i]; - } + return cmd; } -UNITREE_LEGGED_SDK::HighCmd rosMsg2Cmd(const unitree_legged_msgs::HighCmd::ConstPtr &msg) +unitree_legged_msgs::BmsState state2rosMsg(UNITREE_LEGGED_SDK::BmsState &state) { - UNITREE_LEGGED_SDK::HighCmd cmd; + unitree_legged_msgs::BmsState ros_msg; - for (int i(0); i < 2; i++) + for (std::size_t i(0); i < 8; i++) { - cmd.head[i] = msg->head[i]; - cmd.SN[i] = msg->SN[i]; - cmd.version[i] = msg->version[i]; - cmd.position[i] = msg->position[i]; - cmd.velocity[i] = msg->velocity[i]; + ros_msg.BQ_NTC[i] = state.BQ_NTC[i]; + ros_msg.MCU_NTC[i] = state.MCU_NTC[i]; } - for (int i(0); i < 3; i++) + for (std::size_t i(0); i < 30; i++) + ros_msg.cell_vol[i] = state.cell_vol[i]; + + + ros_msg.version_h = state.version_h; + ros_msg.version_l = state.version_l; + ros_msg.bms_status = state.bms_status; + ros_msg.SOC = state.SOC; + ros_msg.current = state.current; + ros_msg.cycle = state.cycle; + + return ros_msg; +} + +unitree_legged_msgs::Cartesian state2rosMsg(UNITREE_LEGGED_SDK::Cartesian &state) +{ + unitree_legged_msgs::Cartesian ros_msg; + + ros_msg.x = state.x; + ros_msg.y = state.y; + ros_msg.z = state.z; + + return ros_msg; +} + +unitree_legged_msgs::IMU state2rosMsg(UNITREE_LEGGED_SDK::IMU &state) +{ + unitree_legged_msgs::IMU ros_msg; + + for (std::size_t i(0); i < 4; i++) { - cmd.euler[i] = msg->euler[i]; + ros_msg.quaternion[i] = state.quaternion[i]; } - for (int i(0); i < 4; i++) + for (std::size_t i(0); i < 3; i++) { - cmd.led[i].r = msg->led[i].r; - cmd.led[i].g = msg->led[i].g; - cmd.led[i].b = msg->led[i].b; + ros_msg.gyroscope[i] = state.gyroscope[i]; + ros_msg.accelerometer[i] = state.accelerometer[i]; + ros_msg.rpy[i] = state.rpy[i]; } - for (int i(0); i < 40; i++) - { - cmd.wirelessRemote[i] = msg->wirelessRemote[i]; - } + ros_msg.temperature = state.temperature; - cmd.levelFlag = msg->levelFlag; - cmd.frameReserve = msg->frameReserve; - cmd.bandWidth = msg->bandWidth; - cmd.mode = msg->mode; - cmd.gaitType = msg->gaitType; - cmd.speedLevel = msg->speedLevel; - cmd.footRaiseHeight = msg->footRaiseHeight; - cmd.bodyHeight = msg->bodyHeight; - cmd.yawSpeed = msg->yawSpeed; - cmd.reserve = msg->reserve; - cmd.crc = msg->crc; + return ros_msg; +} - cmd.bms = rosMsg2Cmd(msg->bms); +UNITREE_LEGGED_SDK::LED rosMsg2Cmd(const unitree_legged_msgs::LED &msg) +{ + UNITREE_LEGGED_SDK::LED cmd; + cmd.r = msg.r; + cmd.g = msg.g; + cmd.b = msg.b; return cmd; } @@ -91,7 +111,7 @@ UNITREE_LEGGED_SDK::MotorCmd rosMsg2Cmd(const unitree_legged_msgs::MotorCmd &msg cmd.Kp = msg.Kp; cmd.Kd = msg.Kd; - for (int i(0); i < 3; i++) + for (std::size_t i(0); i < 3; i++) { cmd.reserve[i] = msg.reserve[i]; } @@ -99,37 +119,6 @@ UNITREE_LEGGED_SDK::MotorCmd rosMsg2Cmd(const unitree_legged_msgs::MotorCmd &msg return cmd; } -UNITREE_LEGGED_SDK::LowCmd rosMsg2Cmd(const unitree_legged_msgs::LowCmd::ConstPtr &msg) -{ - UNITREE_LEGGED_SDK::LowCmd cmd; - - for (int i(0); i < 2; i++) - { - cmd.head[i] = msg->head[i]; - cmd.SN[i] = msg->SN[i]; - cmd.version[i] = msg->version[i]; - } - - for (int i(0); i < 40; i++) - { - cmd.wirelessRemote[i] = msg->wirelessRemote[i]; - } - - for (int i(0); i < 20; i++) - { - cmd.motorCmd[i] = rosMsg2Cmd(msg->motorCmd[i]); - } - - cmd.bms = rosMsg2Cmd(msg->bms); - - cmd.levelFlag = msg->levelFlag; - cmd.frameReserve = msg->frameReserve; - cmd.bandWidth = msg->bandWidth; - cmd.reserve = msg->reserve; - cmd.crc = msg->crc; - - return cmd; -} unitree_legged_msgs::MotorState state2rosMsg(UNITREE_LEGGED_SDK::MotorState &state) { @@ -151,113 +140,62 @@ unitree_legged_msgs::MotorState state2rosMsg(UNITREE_LEGGED_SDK::MotorState &sta return ros_msg; } -unitree_legged_msgs::IMU state2rosMsg(UNITREE_LEGGED_SDK::IMU &state) +UNITREE_LEGGED_SDK::HighCmd rosMsg2Cmd(const unitree_legged_msgs::HighCmd &msg) { - unitree_legged_msgs::IMU ros_msg; + UNITREE_LEGGED_SDK::HighCmd cmd; - for (int i(0); i < 4; i++) + for (std::size_t i(0); i < 2; i++) { - ros_msg.quaternion[i] = state.quaternion[i]; + cmd.head[i] = msg.head[i]; + cmd.SN[i] = msg.SN[i]; + cmd.version[i] = msg.version[i]; + cmd.position[i] = msg.position[i]; + cmd.velocity[i] = msg.velocity[i]; + cmd.dComXy[i] = msg.dComXy[i]; + cmd.dstandFootXy[i] = msg.dstandFootXy[i]; } - for (int i(0); i < 3; i++) - { - ros_msg.gyroscope[i] = state.gyroscope[i]; - ros_msg.accelerometer[i] = state.accelerometer[i]; - ros_msg.rpy[i] = state.rpy[i]; - } + for (std::size_t i(0); i < 3; i++) + cmd.euler[i] = msg.euler[i]; + - ros_msg.temperature = state.temperature; + for (std::size_t i(0); i < 4; i++) + cmd.led[i] = rosMsg2Cmd(msg.led[i]); + - return ros_msg; -} + for (std::size_t i(0); i < 40; i++) + cmd.wirelessRemote[i] = msg.wirelessRemote[i]; + -unitree_legged_msgs::BmsState state2rosMsg(UNITREE_LEGGED_SDK::BmsState &state) -{ - unitree_legged_msgs::BmsState ros_msg; + cmd.levelFlag = msg.levelFlag; + cmd.frameReserve = msg.frameReserve; + cmd.bandWidth = msg.bandWidth; + cmd.mode = msg.mode; + cmd.gaitType = msg.gaitType; + cmd.speedLevel = msg.speedLevel; + cmd.footRaiseHeight = msg.footRaiseHeight; + cmd.bodyHeight = msg.bodyHeight; + cmd.yawSpeed = msg.yawSpeed; + cmd.reserve = msg.reserve; + cmd.crc = msg.crc; - for (int i(0); i < 2; i++) - { - ros_msg.BQ_NTC[i] = state.BQ_NTC[i]; - ros_msg.MCU_NTC[i] = state.MCU_NTC[i]; - } + cmd.bms = rosMsg2Cmd(msg.bms); - for (int i(0); i < 10; i++) - { - ros_msg.cell_vol[i] = state.cell_vol[i]; - } - - ros_msg.version_h = state.version_h; - ros_msg.version_l = state.version_l; - ros_msg.bms_status = state.bms_status; - ros_msg.SOC = state.SOC; - ros_msg.current = state.current; - ros_msg.cycle = state.cycle; - - return ros_msg; -} - -unitree_legged_msgs::LowState state2rosMsg(UNITREE_LEGGED_SDK::LowState &state) -{ - unitree_legged_msgs::LowState ros_msg; - - for (int i(0); i < 2; i++) - { - ros_msg.head[i] = state.head[i]; - ros_msg.SN[i] = state.SN[i]; - ros_msg.version[i] = state.version[i]; - } - - for (int i(0); i < 4; i++) - { - ros_msg.footForce[i] = state.footForce[i]; - ros_msg.footForceEst[i] = state.footForceEst[i]; - } - - for (int i(0); i < 40; i++) - { - ros_msg.wirelessRemote[i] = state.wirelessRemote[i]; - } - - for (int i(0); i < 20; i++) - { - ros_msg.motorState[i] = state2rosMsg(state.motorState[i]); - } - - ros_msg.imu = state2rosMsg(state.imu); - - ros_msg.bms = state2rosMsg(state.bms); - - ros_msg.tick = state.tick; - ros_msg.reserve = state.reserve; - ros_msg.crc = state.crc; - - return ros_msg; -} - -unitree_legged_msgs::Cartesian state2rosMsg(UNITREE_LEGGED_SDK::Cartesian &state) -{ - unitree_legged_msgs::Cartesian ros_msg; - - ros_msg.x = state.x; - ros_msg.y = state.y; - ros_msg.z = state.z; - - return ros_msg; + return cmd; } unitree_legged_msgs::HighState state2rosMsg(UNITREE_LEGGED_SDK::HighState &state) { unitree_legged_msgs::HighState ros_msg; - for (int i(0); i < 2; i++) + for (std::size_t i(0); i < 2; i++) { ros_msg.head[i] = state.head[i]; ros_msg.SN[i] = state.SN[i]; ros_msg.version[i] = state.version[i]; } - for (int i(0); i < 4; i++) + for (std::size_t i(0); i < 4; i++) { ros_msg.footForce[i] = state.footForce[i]; ros_msg.footForceEst[i] = state.footForceEst[i]; @@ -266,18 +204,18 @@ unitree_legged_msgs::HighState state2rosMsg(UNITREE_LEGGED_SDK::HighState &state ros_msg.footSpeed2Body[i] = state2rosMsg(state.footSpeed2Body[i]); } - for (int i(0); i < 3; i++) + for (std::size_t i(0); i < 3; i++) { ros_msg.position[i] = state.position[i]; ros_msg.velocity[i] = state.velocity[i]; } - for (int i(0); i < 40; i++) + for (std::size_t i(0); i < 40; i++) { ros_msg.wirelessRemote[i] = state.wirelessRemote[i]; } - for (int i(0); i < 20; i++) + for (std::size_t i(0); i < 20; i++) { ros_msg.motorState[i] = state2rosMsg(state.motorState[i]); } @@ -301,34 +239,85 @@ unitree_legged_msgs::HighState state2rosMsg(UNITREE_LEGGED_SDK::HighState &state return ros_msg; } -UNITREE_LEGGED_SDK::HighCmd rosMsg2Cmd(const geometry_msgs::Twist::ConstPtr &msg) +unitree_legged_msgs::LowState state2rosMsg(UNITREE_LEGGED_SDK::LowState &state) { - UNITREE_LEGGED_SDK::HighCmd cmd; + unitree_legged_msgs::LowState ros_msg; - cmd.head[0] = 0xFE; - cmd.head[1] = 0xEF; - cmd.levelFlag = UNITREE_LEGGED_SDK::HIGHLEVEL; - cmd.mode = 0; - cmd.gaitType = 0; - cmd.speedLevel = 0; - cmd.footRaiseHeight = 0; - cmd.bodyHeight = 0; - cmd.euler[0] = 0; - cmd.euler[1] = 0; - cmd.euler[2] = 0; - cmd.velocity[0] = 0.0f; - cmd.velocity[1] = 0.0f; - cmd.yawSpeed = 0.0f; - cmd.reserve = 0; + for (std::size_t i(0); i < 2; i++) + { + ros_msg.head[i] = state.head[i]; + ros_msg.SN[i] = state.SN[i]; + ros_msg.version[i] = state.version[i]; + } - cmd.velocity[0] = msg->linear.x; - cmd.velocity[1] = msg->linear.y; - cmd.yawSpeed = msg->angular.z; + for (std::size_t i(0); i < 4; i++) + { + ros_msg.footForce[i] = state.footForce[i]; + ros_msg.footForceEst[i] = state.footForceEst[i]; + } - cmd.mode = 2; - cmd.gaitType = 1; + for (std::size_t i(0); i < 40; i++) + { + ros_msg.wirelessRemote[i] = state.wirelessRemote[i]; + } + + for (std::size_t i(0); i < 20; i++) + { + ros_msg.motorState[i] = state2rosMsg(state.motorState[i]); + } + + ros_msg.imu = state2rosMsg(state.imu); + + ros_msg.bms = state2rosMsg(state.bms); + + ros_msg.tick = state.tick; + ros_msg.reserve = state.reserve; + ros_msg.crc = state.crc; + + return ros_msg; +} + + +UNITREE_LEGGED_SDK::LowCmd rosMsg2Cmd(const unitree_legged_msgs::LowCmd &msg) +{ + UNITREE_LEGGED_SDK::LowCmd cmd; + + for (std::size_t i(0); i < 2; i++) + { + cmd.head[i] = msg.head[i]; + cmd.SN[i] = msg.SN[i]; + cmd.version[i] = msg.version[i]; + } + + for (std::size_t i(0); i < 40; i++) + { + cmd.wirelessRemote[i] = msg.wirelessRemote[i]; + } + + for (std::size_t i(0); i < 20; i++) + { + cmd.motorCmd[i] = rosMsg2Cmd(msg.motorCmd[i]); + } + + cmd.bms = rosMsg2Cmd(msg.bms); + + cmd.levelFlag = msg.levelFlag; + cmd.frameReserve = msg.frameReserve; + cmd.bandWidth = msg.bandWidth; + cmd.reserve = msg.reserve; + cmd.crc = msg.crc; return cmd; } + + + + + + + + + + #endif // _CONVERT_H_ diff --git a/unitree_legged_real/launch/keyboard_control.launch b/unitree_legged_real/launch/keyboard_control.launch deleted file mode 100644 index d7b877a..0000000 --- a/unitree_legged_real/launch/keyboard_control.launch +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - \ No newline at end of file diff --git a/unitree_legged_real/src/exe/control_via_keyboard.cpp b/unitree_legged_real/src/exe/control_via_keyboard.cpp deleted file mode 100644 index 53fda22..0000000 --- a/unitree_legged_real/src/exe/control_via_keyboard.cpp +++ /dev/null @@ -1,111 +0,0 @@ -#include "ros/ros.h" -#include -#include - -int getch() -{ - int ch; - struct termios oldt; - struct termios newt; - - // Store old settings, and copy to new settings - tcgetattr(STDIN_FILENO, &oldt); - newt = oldt; - - // Make required changes and apply the settings - newt.c_lflag &= ~(ICANON | ECHO); - newt.c_iflag |= IGNBRK; - newt.c_iflag &= ~(INLCR | ICRNL | IXON | IXOFF); - newt.c_lflag &= ~(ICANON | ECHO | ECHOK | ECHOE | ECHONL | ISIG | IEXTEN); - newt.c_cc[VMIN] = 0; - newt.c_cc[VTIME] = 1; - tcsetattr(fileno(stdin), TCSANOW, &newt); - - // Get the current character - ch = getchar(); - - // Reapply old settings - tcsetattr(STDIN_FILENO, TCSANOW, &oldt); - - return ch; -} - -int main(int argc, char **argv) -{ - ros::init(argc, argv, "keyboard_input_node"); - - ros::NodeHandle nh; - - ros::Rate loop_rate(500); - - ros::Publisher pub = nh.advertise("cmd_vel", 1); - - geometry_msgs::Twist twist; - - long count = 0; - - while (ros::ok()) - { - twist.linear.x = 0.0; - twist.linear.y = 0.0; - twist.linear.z = 0.0; - twist.angular.x = 0.0; - twist.angular.y = 0.0; - twist.angular.z = 0.0; - - int ch = 0; - - ch = getch(); - - printf("%ld\n", count++); - printf("ch = %d\n\n", ch); - - switch (ch) - { - case 'q': - printf("already quit!\n"); - return 0; - - case 'w': - twist.linear.x = 0.5; - printf("move forward!\n"); - break; - - case 's': - twist.linear.x = -0.5; - printf("move backward!\n"); - break; - - case 'a': - twist.linear.y = 0.5; - printf("move left!\n"); - break; - - case 'd': - twist.linear.y = -0.5; - printf("move right!\n"); - break; - - case 'j': - twist.angular.z = 1.0; - printf("turn left!\n"); - break; - - case 'l': - twist.angular.z = -1.0; - printf("turn right!\n"); - break; - - default: - printf("Stop!\n"); - break; - } - - pub.publish(twist); - - ros::spinOnce(); - loop_rate.sleep(); - } - - return 0; -} diff --git a/unitree_legged_real/src/exe/example_position.cpp b/unitree_legged_real/src/exe/example_position.cpp index 7926577..e687bbb 100644 --- a/unitree_legged_real/src/exe/example_position.cpp +++ b/unitree_legged_real/src/exe/example_position.cpp @@ -6,6 +6,18 @@ using namespace UNITREE_LEGGED_SDK; +unitree_legged_msgs::LowState low_state_ros; + +void lowStateCallback(const unitree_legged_msgs::LowState::ConstPtr &state) +{ + static long count = 0; + ROS_INFO("lowStateCallback %ld", count++); + + low_state_ros = *state; +} + + + int main(int argc, char **argv) { ros::init(argc, argv, "example_postition_without_lcm"); @@ -29,16 +41,17 @@ int main(int argc, char **argv) unitree_legged_msgs::LowCmd low_cmd_ros; - bool initiated_flag = false; // initiate need time - int count = 0; + // bool initiated_flag = false; // initiate need time + // int count = 0; ros::Publisher pub = nh.advertise("low_cmd", 1); + ros::Subscriber sub = nh.subscribe("low_state", 1, lowStateCallback); low_cmd_ros.head[0] = 0xFE; low_cmd_ros.head[1] = 0xEF; low_cmd_ros.levelFlag = LOWLEVEL; - for (int i = 0; i < 12; i++) + for (std::size_t i = 0; i < 20; i++) { low_cmd_ros.motorCmd[i].mode = 0x0A; // motor switch to servo (PMSM) mode low_cmd_ros.motorCmd[i].q = PosStopF; // 禁止位置环 @@ -51,37 +64,60 @@ int main(int argc, char **argv) while (ros::ok()) { - if (initiated_flag == true) + printf("FR_joint_pos: %f %f %f\n", low_state_ros.motorState[FR_0].q, low_state_ros.motorState[FR_1].q, low_state_ros.motorState[FR_2].q); + + + motiontime += 2; + + if(motiontime >= 0 && motiontime <= 10) { - motiontime += 2; + qInit[0] = low_state_ros.motorState[FR_0].q; + qInit[1] = low_state_ros.motorState[FR_1].q; + qInit[2] = low_state_ros.motorState[FR_2].q; + } + else if(motiontime > 10 && motiontime < 1000) + { + Kp[0] = 20.0; + Kp[1] = 20.0; + Kp[2] = 20.0; + Kd[0] = 2.0; + Kd[1] = 2.0; + Kd[2] = 2.0; - low_cmd_ros.motorCmd[FR_0].tau = -0.65f; - low_cmd_ros.motorCmd[FL_0].tau = +0.65f; - low_cmd_ros.motorCmd[RR_0].tau = -0.65f; - low_cmd_ros.motorCmd[RL_0].tau = +0.65f; + const float interval = 800.0; - low_cmd_ros.motorCmd[FR_2].q = -M_PI / 2 + 0.5 * sin(2 * M_PI / 5.0 * motiontime * 1e-3); - low_cmd_ros.motorCmd[FR_2].dq = 0.0; - low_cmd_ros.motorCmd[FR_2].Kp = 5.0; - low_cmd_ros.motorCmd[FR_2].Kd = 1.0; + qDes[0] = (1 - (motiontime - 10) / interval) * qInit[0] + (motiontime - 10) / interval * sin_mid_q[0]; + qDes[1] = (1 - (motiontime - 10) / interval) * qInit[1] + (motiontime - 10) / interval * sin_mid_q[1]; + qDes[2] = (1 - (motiontime - 10) / interval) * qInit[2] + (motiontime - 10) / interval * sin_mid_q[2]; + } + else if(motiontime >= 1000) + { + float period = 5.0; - low_cmd_ros.motorCmd[FR_0].q = 0.0; - low_cmd_ros.motorCmd[FR_0].dq = 0.0; - low_cmd_ros.motorCmd[FR_0].Kp = 5.0; - low_cmd_ros.motorCmd[FR_0].Kd = 1.0; + qDes[0] = sin_mid_q[0]; + qDes[1] = sin_mid_q[1] + 0.6 * std::sin(2 * M_PI / period * (motiontime - 400) / 1000.0); + qDes[2] = sin_mid_q[2] + (-0.9) * std::sin(2 * M_PI / period * (motiontime - 400) / 1000.0); - low_cmd_ros.motorCmd[FR_1].q = 0.0; - low_cmd_ros.motorCmd[FR_1].dq = 0.0; - low_cmd_ros.motorCmd[FR_1].Kp = 5.0; - low_cmd_ros.motorCmd[FR_1].Kd = 1.0; } - count++; - if (count > 10) - { - count = 10; - initiated_flag = true; - } + low_cmd_ros.motorCmd[FR_0].tau = -4.0; + low_cmd_ros.motorCmd[FR_0].Kp = Kp[0]; + low_cmd_ros.motorCmd[FR_0].Kd = Kd[0]; + low_cmd_ros.motorCmd[FR_0].q = qDes[0]; + low_cmd_ros.motorCmd[FR_0].dq = 0.0; + + low_cmd_ros.motorCmd[FR_1].tau = 0.0; + low_cmd_ros.motorCmd[FR_1].Kp = Kp[1]; + low_cmd_ros.motorCmd[FR_1].Kd = Kd[1]; + low_cmd_ros.motorCmd[FR_1].q = qDes[1]; + low_cmd_ros.motorCmd[FR_1].dq = 0.0; + + low_cmd_ros.motorCmd[FR_2].tau = 0.0; + low_cmd_ros.motorCmd[FR_2].Kp = Kp[2]; + low_cmd_ros.motorCmd[FR_2].Kd = Kd[2]; + low_cmd_ros.motorCmd[FR_2].q = qDes[2]; + low_cmd_ros.motorCmd[FR_2].dq = 0.0; + pub.publish(low_cmd_ros); diff --git a/unitree_legged_real/src/exe/example_walk.cpp b/unitree_legged_real/src/exe/example_walk.cpp index ce46ddb..7624298 100644 --- a/unitree_legged_real/src/exe/example_walk.cpp +++ b/unitree_legged_real/src/exe/example_walk.cpp @@ -6,6 +6,15 @@ using namespace UNITREE_LEGGED_SDK; +unitree_legged_msgs::HighState high_state_ros; + +void highStateCallback(const unitree_legged_msgs::HighState::ConstPtr &state) +{ + static long count = 0; + ROS_INFO("highStateCallback %ld", count++); + high_state_ros = *state; +} + int main(int argc, char **argv) { ros::init(argc, argv, "example_walk_without_lcm"); @@ -24,9 +33,11 @@ int main(int argc, char **argv) unitree_legged_msgs::HighCmd high_cmd_ros; ros::Publisher pub = nh.advertise("high_cmd", 1000); + ros::Subscriber sub = nh.subscribe("high_state", 1000, highStateCallback); while (ros::ok()) { + printf("rpy: %f %f %f\n", high_state_ros.imu.rpy[0], high_state_ros.imu.rpy[1], high_state_ros.imu.rpy[2]); motiontime += 2; @@ -46,89 +57,77 @@ int main(int argc, char **argv) high_cmd_ros.yawSpeed = 0.0f; high_cmd_ros.reserve = 0; - if (motiontime > 0 && motiontime < 1000) + if (motiontime > 0 && motiontime < 2000) + { + high_cmd_ros.mode = 6; + } + else if(motiontime >= 2000 && motiontime < 3000) { high_cmd_ros.mode = 1; - high_cmd_ros.euler[0] = -0.3; } - if (motiontime > 1000 && motiontime < 2000) + else if(motiontime >= 3000 && motiontime < 4000) { high_cmd_ros.mode = 1; high_cmd_ros.euler[0] = 0.3; } - if (motiontime > 2000 && motiontime < 3000) + else if(motiontime >= 4000 && motiontime < 6000) { high_cmd_ros.mode = 1; - high_cmd_ros.euler[1] = -0.2; + high_cmd_ros.euler[0] = -0.3; } - if (motiontime > 3000 && motiontime < 4000) + else if(motiontime >= 6000 && motiontime < 8000) { high_cmd_ros.mode = 1; - high_cmd_ros.euler[1] = 0.2; + high_cmd_ros.euler[1] = 0.3; } - if (motiontime > 4000 && motiontime < 5000) + else if(motiontime >= 8000 && motiontime < 10000) { high_cmd_ros.mode = 1; - high_cmd_ros.euler[2] = -0.2; + high_cmd_ros.euler[1] = -0.3; } - if (motiontime > 5000 && motiontime < 6000) + else if(motiontime >= 10000 && motiontime < 12000) { high_cmd_ros.mode = 1; - high_cmd_ros.euler[2] = 0.2; + high_cmd_ros.euler[2] = 0.3; } - if (motiontime > 6000 && motiontime < 7000) + else if(motiontime >= 12000 && motiontime < 14000) { high_cmd_ros.mode = 1; - high_cmd_ros.bodyHeight = -0.2; + high_cmd_ros.euler[2] = -0.3; } - if (motiontime > 7000 && motiontime < 8000) + else if(motiontime >= 14000 && motiontime < 15000) { high_cmd_ros.mode = 1; - high_cmd_ros.bodyHeight = 0.1; } - if (motiontime > 8000 && motiontime < 9000) - { - high_cmd_ros.mode = 1; - high_cmd_ros.bodyHeight = 0.0; - } - if (motiontime > 9000 && motiontime < 11000) - { - high_cmd_ros.mode = 5; - } - if (motiontime > 11000 && motiontime < 13000) - { - high_cmd_ros.mode = 6; - } - if (motiontime > 13000 && motiontime < 14000) - { - high_cmd_ros.mode = 0; - } - if (motiontime > 14000 && motiontime < 18000) + else if(motiontime >= 15000 && motiontime < 18000) { high_cmd_ros.mode = 2; - high_cmd_ros.gaitType = 2; - high_cmd_ros.velocity[0] = 0.4f; // -1 ~ +1 - high_cmd_ros.yawSpeed = 2; - high_cmd_ros.footRaiseHeight = 0.1; - // printf("walk\n"); + high_cmd_ros.velocity[0] = 0.3; + high_cmd_ros.yawSpeed = 0.2; } - if (motiontime > 18000 && motiontime < 20000) - { - high_cmd_ros.mode = 0; - high_cmd_ros.velocity[0] = 0; - } - if (motiontime > 20000 && motiontime < 24000) + else if(motiontime >= 18000 && motiontime < 21000) { high_cmd_ros.mode = 2; - high_cmd_ros.gaitType = 1; - high_cmd_ros.velocity[0] = 0.2f; // -1 ~ +1 - high_cmd_ros.bodyHeight = 0.1; - // printf("walk\n"); + high_cmd_ros.velocity[1] = -0.3; + high_cmd_ros.yawSpeed = -0.2; } - if (motiontime > 24000) + else if(motiontime >= 21000 && motiontime < 22000) { high_cmd_ros.mode = 1; } + else if(motiontime >= 22000 && motiontime < 25000) + { + high_cmd_ros.mode = 2; + high_cmd_ros.gaitType = 3; + } + else if(motiontime >= 25000 && motiontime < 26000) + { + high_cmd_ros.mode = 1; + } + else + { + high_cmd_ros.mode = 0; + } pub.publish(high_cmd_ros); diff --git a/unitree_legged_real/src/exe/ros_udp.cpp b/unitree_legged_real/src/exe/ros_udp.cpp index f467aa1..41348ef 100644 --- a/unitree_legged_real/src/exe/ros_udp.cpp +++ b/unitree_legged_real/src/exe/ros_udp.cpp @@ -27,7 +27,7 @@ public: : // low_udp(LOWLEVEL), low_udp(LOWLEVEL, 8091, "192.168.123.10", 8007), - high_udp(8090, "192.168.123.161", 8082, sizeof(HighCmd), sizeof(HighState)) + high_udp(8090, "192.168.123.220", 8082, sizeof(HighCmd), sizeof(HighState)) { high_udp.InitCmdData(high_cmd); low_udp.InitCmdData(low_cmd); @@ -77,9 +77,9 @@ long low_count = 0; void highCmdCallback(const unitree_legged_msgs::HighCmd::ConstPtr &msg) { - printf("highCmdCallback is running !\t%ld\n", ::high_count); + printf("highCmdCallback is running !\t%ld\n", ::high_count++); - custom.high_cmd = rosMsg2Cmd(msg); + custom.high_cmd = rosMsg2Cmd(*msg); unitree_legged_msgs::HighState high_state_ros; @@ -87,15 +87,15 @@ void highCmdCallback(const unitree_legged_msgs::HighCmd::ConstPtr &msg) pub_high.publish(high_state_ros); - printf("highCmdCallback ending !\t%ld\n\n", ::high_count++); + // printf("highCmdCallback ending !\t%ld\n\n", ::high_count++); } void lowCmdCallback(const unitree_legged_msgs::LowCmd::ConstPtr &msg) { - printf("lowCmdCallback is running !\t%ld\n", low_count); + printf("lowCmdCallback is running !\t%ld\n", ::low_count++); - custom.low_cmd = rosMsg2Cmd(msg); + custom.low_cmd = rosMsg2Cmd(*msg); unitree_legged_msgs::LowState low_state_ros; @@ -103,7 +103,7 @@ void lowCmdCallback(const unitree_legged_msgs::LowCmd::ConstPtr &msg) pub_low.publish(low_state_ros); - printf("lowCmdCallback ending!\t%ld\n\n", ::low_count++); + // printf("lowCmdCallback ending!\t%ld\n\n", ::low_count++); } int main(int argc, char **argv) @@ -123,6 +123,8 @@ int main(int argc, char **argv) loop_udpSend.start(); loop_udpRecv.start(); + printf("LOWLEVEL is initialized\n"); + ros::spin(); // printf("low level runing!\n"); @@ -138,6 +140,8 @@ int main(int argc, char **argv) loop_udpSend.start(); loop_udpRecv.start(); + printf("HIGHLEVEL is initialized\n"); + ros::spin(); // printf("high level runing!\n"); diff --git a/unitree_legged_real/src/exe/state_sub.cpp b/unitree_legged_real/src/exe/state_sub.cpp deleted file mode 100644 index b52ed43..0000000 --- a/unitree_legged_real/src/exe/state_sub.cpp +++ /dev/null @@ -1,39 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "unitree_legged_sdk/unitree_legged_sdk.h" -#include - -using namespace UNITREE_LEGGED_SDK; - -void highStateCallback(const unitree_legged_msgs::HighState::ConstPtr &msg) -{ - printf("yaw = %f\n", msg->imu.rpy[2]); -} - -void lowStateCallback(const unitree_legged_msgs::LowState::ConstPtr &msg) -{ - printf("FR_2_pos = %f\n", msg->motorState[FR_2].q); -} - -int main(int argc, char **argv) -{ - ros::init(argc, argv, "node_high_state_sub"); - - ros::NodeHandle nh; - - unitree_legged_msgs::HighState high_state_ros; - - ros::Subscriber high_sub = nh.subscribe("high_state", 1, highStateCallback); - ros::Subscriber low_sub = nh.subscribe("low_state", 1, lowStateCallback); - - ros::spin(); - - return 0; -} \ No newline at end of file diff --git a/unitree_legged_real/src/exe/twist_sub.cpp b/unitree_legged_real/src/exe/twist_sub.cpp deleted file mode 100644 index 7664a09..0000000 --- a/unitree_legged_real/src/exe/twist_sub.cpp +++ /dev/null @@ -1,112 +0,0 @@ -#include -#include -#include -#include -#include -#include "unitree_legged_sdk/unitree_legged_sdk.h" -#include "convert.h" -#include -#include -#include - -using namespace UNITREE_LEGGED_SDK; -class Custom -{ -public: - UDP low_udp; - UDP high_udp; - - HighCmd high_cmd = {0}; - HighState high_state = {0}; - - LowCmd low_cmd = {0}; - LowState low_state = {0}; - -public: - Custom() - : - // low_udp(LOWLEVEL), - low_udp(LOWLEVEL, 8091, "192.168.123.10", 8007), - high_udp(8090, "192.168.123.161", 8082, sizeof(HighCmd), sizeof(HighState)) - { - high_udp.InitCmdData(high_cmd); - low_udp.InitCmdData(low_cmd); - } - - void highUdpSend() - { - // printf("high udp send is running\n"); - - high_udp.SetSend(high_cmd); - high_udp.Send(); - } - - void lowUdpSend() - { - - low_udp.SetSend(low_cmd); - low_udp.Send(); - } - - void lowUdpRecv() - { - - low_udp.Recv(); - low_udp.GetRecv(low_state); - } - - void highUdpRecv() - { - // printf("high udp recv is running\n"); - - high_udp.Recv(); - high_udp.GetRecv(high_state); - } -}; - -Custom custom; - -ros::Subscriber sub_cmd_vel; -ros::Publisher pub_high; - -long cmd_vel_count = 0; - -void cmdVelCallback(const geometry_msgs::Twist::ConstPtr &msg) -{ - printf("cmdVelCallback is running!\t%ld\n", cmd_vel_count); - - custom.high_cmd = rosMsg2Cmd(msg); - - printf("cmd_x_vel = %f\n", custom.high_cmd.velocity[0]); - printf("cmd_y_vel = %f\n", custom.high_cmd.velocity[1]); - printf("cmd_yaw_vel = %f\n", custom.high_cmd.yawSpeed); - - unitree_legged_msgs::HighState high_state_ros; - - high_state_ros = state2rosMsg(custom.high_state); - - pub_high.publish(high_state_ros); - - printf("cmdVelCallback ending!\t%ld\n\n", cmd_vel_count++); -} - -int main(int argc, char **argv) -{ - ros::init(argc, argv, "twist_sub"); - - ros::NodeHandle nh; - - pub_high = nh.advertise("high_state", 1); - - sub_cmd_vel = nh.subscribe("cmd_vel", 1, cmdVelCallback); - - LoopFunc loop_udpSend("high_udp_send", 0.002, 3, boost::bind(&Custom::highUdpSend, &custom)); - LoopFunc loop_udpRecv("high_udp_recv", 0.002, 3, boost::bind(&Custom::highUdpRecv, &custom)); - - loop_udpSend.start(); - loop_udpRecv.start(); - - ros::spin(); - - return 0; -}