commit ce58cb9c10201474a85a8fc50a6bf9651beb38e7 Author: Bian Zekun <568574579@qq.com> Date: Wed Sep 1 20:46:41 2021 +0800 suitable for sdk 3.2 diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..dbe9c82 --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +.vscode/ \ No newline at end of file diff --git a/unitree_legged_msgs/CMakeLists.txt b/unitree_legged_msgs/CMakeLists.txt new file mode 100644 index 0000000..4ae8a82 --- /dev/null +++ b/unitree_legged_msgs/CMakeLists.txt @@ -0,0 +1,48 @@ +cmake_minimum_required(VERSION 2.8.3) +project(unitree_legged_msgs) + +find_package(catkin REQUIRED COMPONENTS + message_generation + std_msgs + geometry_msgs + sensor_msgs +) + +add_message_files( + FILES + MotorCmd.msg + MotorState.msg + Cartesian.msg + IMU.msg + LED.msg + LowCmd.msg + LowState.msg + HighCmd.msg + HighState.msg +) + +generate_messages( + DEPENDENCIES + std_msgs + geometry_msgs + sensor_msgs +) + +catkin_package( + CATKIN_DEPENDS + message_runtime + std_msgs + geometry_msgs + sensor_msgs +) + +############# +## Install ## +############# + +# Mark topic names header files for installation +install( + DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" +) \ No newline at end of file diff --git a/unitree_legged_msgs/msg/Cartesian.msg b/unitree_legged_msgs/msg/Cartesian.msg new file mode 100644 index 0000000..0fb5429 --- /dev/null +++ b/unitree_legged_msgs/msg/Cartesian.msg @@ -0,0 +1,3 @@ +float32 x +float32 y +float32 z \ No newline at end of file diff --git a/unitree_legged_msgs/msg/HighCmd.msg b/unitree_legged_msgs/msg/HighCmd.msg new file mode 100644 index 0000000..65aa796 --- /dev/null +++ b/unitree_legged_msgs/msg/HighCmd.msg @@ -0,0 +1,19 @@ +uint8 levelFlag +uint16 commVersion # Old version Aliengo does not have +uint16 robotID # Old version Aliengo does not have +uint32 SN # Old version Aliengo does not have +uint8 bandWidth # Old version Aliengo does not have +uint8 mode +float32 forwardSpeed +float32 sideSpeed +float32 rotateSpeed +float32 bodyHeight +float32 footRaiseHeight +float32 yaw +float32 pitch +float32 roll +LED[4] led +uint8[40] wirelessRemote +uint8[40] AppRemote # Old version Aliengo does not have +uint32 reserve # Old version Aliengo does not have +int32 crc \ No newline at end of file diff --git a/unitree_legged_msgs/msg/HighState.msg b/unitree_legged_msgs/msg/HighState.msg new file mode 100644 index 0000000..08fa238 --- /dev/null +++ b/unitree_legged_msgs/msg/HighState.msg @@ -0,0 +1,26 @@ +uint8 levelFlag +uint16 commVersion # Old version Aliengo does not have +uint16 robotID # Old version Aliengo does not have +uint32 SN # Old version Aliengo does not have +uint8 bandWidth # Old version Aliengo does not have +uint8 mode +IMU imu +float32 forwardSpeed +float32 sideSpeed +float32 rotateSpeed +float32 bodyHeight +float32 updownSpeed +float32 forwardPosition # (will be float type next version) # Old version Aliengo is different +float32 sidePosition # (will be float type next version) # Old version Aliengo is different +Cartesian[4] footPosition2Body +Cartesian[4] footSpeed2Body +int16[4] footForce # Old version Aliengo is different +int16[4] footForceEst # Old version Aliengo does not have +uint32 tick +uint8[40] wirelessRemote +uint32 reserve # Old version Aliengo does not have +uint32 crc + +# Under are not defined in SDK yet. # Old version Aliengo does not have +Cartesian[4] eeForce # It's a 1-DOF force in real robot, but 3-DOF is better for visualization. +float32[12] jointP # for visualization \ No newline at end of file diff --git a/unitree_legged_msgs/msg/IMU.msg b/unitree_legged_msgs/msg/IMU.msg new file mode 100644 index 0000000..71c741a --- /dev/null +++ b/unitree_legged_msgs/msg/IMU.msg @@ -0,0 +1,4 @@ +float32[4] quaternion +float32[3] gyroscope +float32[3] accelerometer +int8 temperature \ No newline at end of file diff --git a/unitree_legged_msgs/msg/LED.msg b/unitree_legged_msgs/msg/LED.msg new file mode 100644 index 0000000..6f07b24 --- /dev/null +++ b/unitree_legged_msgs/msg/LED.msg @@ -0,0 +1,3 @@ +uint8 r +uint8 g +uint8 b \ No newline at end of file diff --git a/unitree_legged_msgs/msg/LowCmd.msg b/unitree_legged_msgs/msg/LowCmd.msg new file mode 100644 index 0000000..62f28ba --- /dev/null +++ b/unitree_legged_msgs/msg/LowCmd.msg @@ -0,0 +1,12 @@ +uint8 levelFlag +uint16 commVersion # Old version Aliengo does not have +uint16 robotID # Old version Aliengo does not have +uint32 SN # Old version Aliengo does not have +uint8 bandWidth # Old version Aliengo does not have +MotorCmd[20] motorCmd +LED[4] led +uint8[40] wirelessRemote +uint32 reserve # Old version Aliengo does not have +uint32 crc + +Cartesian[4] ff # will delete # Old version Aliengo does not have \ No newline at end of file diff --git a/unitree_legged_msgs/msg/LowState.msg b/unitree_legged_msgs/msg/LowState.msg new file mode 100644 index 0000000..2614fa6 --- /dev/null +++ b/unitree_legged_msgs/msg/LowState.msg @@ -0,0 +1,20 @@ +uint8 levelFlag +uint16 commVersion # Old version Aliengo does not have +uint16 robotID # Old version Aliengo does not have +uint32 SN # Old version Aliengo does not have +uint8 bandWidth # Old version Aliengo does not have +IMU imu +MotorState[20] motorState +int16[4] footForce # force sensors # Old version Aliengo is different +int16[4] footForceEst # force sensors # Old version Aliengo does not have +uint32 tick # reference real-time from motion controller (unit: us) +uint8[40] wirelessRemote # wireless commands +uint32 reserve # Old version Aliengo does not have +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_msgs/msg/MotorCmd.msg b/unitree_legged_msgs/msg/MotorCmd.msg new file mode 100644 index 0000000..668fd45 --- /dev/null +++ b/unitree_legged_msgs/msg/MotorCmd.msg @@ -0,0 +1,7 @@ +uint8 mode # motor target mode +float32 q # motor target position +float32 dq # motor target velocity +float32 tau # motor target torque +float32 Kp # motor spring stiffness coefficient +float32 Kd # motor damper coefficient +uint32[3] reserve # motor target torque \ No newline at end of file diff --git a/unitree_legged_msgs/msg/MotorState.msg b/unitree_legged_msgs/msg/MotorState.msg new file mode 100644 index 0000000..8e326b0 --- /dev/null +++ b/unitree_legged_msgs/msg/MotorState.msg @@ -0,0 +1,10 @@ +uint8 mode # motor current mode +float32 q # motor current position（rad） +float32 dq # motor current speed（rad/s） +float32 ddq # motor current speed（rad/s） +float32 tauEst # current estimated output torque（N*m） +float32 q_raw # motor current position（rad） +float32 dq_raw # motor current speed（rad/s） +float32 ddq_raw # motor current speed（rad/s） +int8 temperature # motor temperature（slow conduction of temperature leads to lag） +uint32[2] reserve \ No newline at end of file diff --git a/unitree_legged_msgs/package.xml b/unitree_legged_msgs/package.xml new file mode 100644 index 0000000..a0a1d19 --- /dev/null +++ b/unitree_legged_msgs/package.xml @@ -0,0 +1,19 @@ + + + + unitree_legged_msgs + 0.0.0 + + The test messgaes package. + + unitree + TODO + + catkin + message_runtime + message_generation + std_msgs + geometry_msgs + sensor_msgs + + \ No newline at end of file diff --git a/unitree_legged_real/CMakeLists.txt b/unitree_legged_real/CMakeLists.txt new file mode 100755 index 0000000..9c91639 --- /dev/null +++ b/unitree_legged_real/CMakeLists.txt @@ -0,0 +1,61 @@ +cmake_minimum_required(VERSION 2.8.3) +project(unitree_legged_real) + +add_compile_options(-std=c++11) + +find_package(catkin REQUIRED COMPONENTS + roscpp + geometry_msgs + unitree_legged_msgs +) + +catkin_package() + +include_directories( + include + ${Boost_INCLUDE_DIR} + ${catkin_INCLUDE_DIRS} +) + +set(CMAKE_CXX_FLAGS "-O3") + +if( $ENV{UNITREE_SDK_VERSION} STREQUAL "3_1") + include_directories($ENV{ALIENGO_SDK_PATH}/include) + link_directories($ENV{ALIENGO_SDK_PATH}/lib) + string(CONCAT LEGGED_SDK_NAME libaliengo_comm.so) + set(EXTRA_LIBS ${LEGGED_SDK_NAME} lcm) + + add_definitions(-DSDK3_1) + + add_executable(lcm_server_3_1 $ENV{ALIENGO_SDK_PATH}/examples/lcm_server.cpp) + target_link_libraries(lcm_server_3_1 ${EXTRA_LIBS} ${catkin_LIBRARIES}) + add_dependencies(lcm_server_3_1 ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +elseif( $ENV{UNITREE_SDK_VERSION} STREQUAL "3_2") + include_directories($ENV{UNITREE_LEGGED_SDK_PATH}/include) + link_directories($ENV{UNITREE_LEGGED_SDK_PATH}/lib) + string(CONCAT LEGGED_SDK_NAME libunitree_legged_sdk_$ENV{UNITREE_PLATFORM}.so) + set(EXTRA_LIBS ${LEGGED_SDK_NAME} lcm) + + add_definitions(-DSDK3_2) + + add_executable(lcm_server_3_2 $ENV{UNITREE_LEGGED_SDK_PATH}/examples/lcm_server.cpp) + target_link_libraries(lcm_server_3_2 ${EXTRA_LIBS} ${catkin_LIBRARIES}) + add_dependencies(lcm_server_3_2 ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +endif() + +add_executable(position_lcm src/exe/position_mode.cpp) +target_link_libraries(position_lcm ${EXTRA_LIBS} ${catkin_LIBRARIES}) +add_dependencies(position_lcm ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +add_executable(velocity_lcm src/exe/velocity_mode.cpp) +target_link_libraries(velocity_lcm ${EXTRA_LIBS} ${catkin_LIBRARIES}) +add_dependencies(velocity_lcm ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +add_executable(torque_lcm src/exe/torque_mode.cpp) +target_link_libraries(torque_lcm ${EXTRA_LIBS} ${catkin_LIBRARIES}) +add_dependencies(torque_lcm ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +add_executable(walk_lcm src/exe/walk_mode.cpp) +target_link_libraries(walk_lcm ${EXTRA_LIBS} ${catkin_LIBRARIES}) +add_dependencies(walk_lcm ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) \ No newline at end of file diff --git a/unitree_legged_real/README.md b/unitree_legged_real/README.md new file mode 100644 index 0000000..b0eed04 --- /dev/null +++ b/unitree_legged_real/README.md @@ -0,0 +1,87 @@ +Packages Version: v3.2.1 + +# 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.2 and v3.1, namely A1 and Aliengo robot. Currently, all the A1 and Aliengo use v3.2. If your Aliengo is really old, then it may use v3.1. +As for Go1, please use the v3.4 release version of this package and unitree_legged_sdk v3.4. + +# Dependencies +* [unitree_legged_msgs](https://github.com/unitreerobotics/unitree_ros): `unitree_legged_msgs` is a package of [unitree_ros](https://github.com/unitreerobotics/unitree_ros). +* [unitree_legged_sdk](https://github.com/unitreerobotics): If your robot is suitable for `unitree_legged_sdk`, then you do not need `aliengo_sdk`. +* [aliengo_sdk](https://github.com/unitreerobotics): If your robot is suitable for `aliengo_sdk`, then you do not need `unitree_legged_sdk`. + +# Configuration +Make sure the following exist in your `~/.bashrc` file or export them in terminal. `melodic`, `gazebo-8`, `~/catkin_ws`, `amd64` and the paths to `unitree_legged_sdk` should be replaced in your own case. +If your use `unitree_legged_sdk`, then you need to set `UNITREE_SDK_VERSION=3_2` and the path `UNITREE_LEGGED_SDK_PATH`. +Otherwise, if you use `aliengo_sdk`, you need to set `UNITREE_SDK_VERSION=3_1` and the path `ALIENGO_SDK_PATH`. + +``` +source /opt/ros/melodic/setup.bash +source /usr/share/gazebo-8/setup.sh +source ~/catkin_ws/devel/setup.bash +export ROS_PACKAGE_PATH=~/catkin_ws:${ROS_PACKAGE_PATH} +export GAZEBO_PLUGIN_PATH=~/catkin_ws/devel/lib:${GAZEBO_PLUGIN_PATH} +export LD_LIBRARY_PATH=~/catkin_ws/devel/lib:${LD_LIBRARY_PATH} +# 3_1, 3_2 +export UNITREE_SDK_VERSION=3_2 +export UNITREE_LEGGED_SDK_PATH=~/unitree_legged_sdk +export ALIENGO_SDK_PATH=~/aliengo_sdk +# amd64, arm32, arm64 +export UNITREE_PLATFORM="amd64" +``` + +# Build +You can use catkin_make to build ROS packages. First copy the package folder to `~/catkin_ws/src`, then: +``` +cd ~/catkin_ws +catkin_make +``` +Before compiling `unitree_legged_real`, please make sure that the `unitree_legged_msgs` has been compiled. + +# 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 robot(only A1 and Aliengo) from ROS by this package. + +First you have to run the `real_launch` under root account: +``` +sudo su +source /home/yourUserName/catkin_ws/devel/setup.bash +roslaunch unitree_legged_real real.launch rname:=a1 ctrl_level:=highlevel firmwork:=3_2 +``` +Please watchout that the `/home/yourUserName` means the home directory of yourself. These commands will launch a LCM server. The `rname` means robot name, which can be `a1` or `aliengo`(case does not matter), and the default value is `a1`. And the `ctrl_level` means the control level, which can be `lowlevel` or `highlevel`(case does not matter), and the default value is `highlevel`. Under the low level, you can control the joints directly. And under the high level, you can control the robot to move or change its pose. The `firmwork` means the firmwork version of the robot. The default value is `3_2` Now all the A1's firmwork version is `3_2`. + +In order to send message to robot, you need to run the controller in another terminal(also under root account): +``` +rosrun unitree_legged_real position_lcm +``` +We offered some examples. When you run the low level controller, please make sure the robot is hanging up. The low level contains: +``` +position_lcm +velocity_lcm +torque_lcm +``` +The `velocity_lcm` and `torque_lcm` have to run under root account too. Please use the same method as runing `real_launch`. + +And when you run the high level controller, please make sure the robot is standing on the ground. The high level only has `walk_lcm`. \ No newline at end of file diff --git a/unitree_legged_real/include/convert.h b/unitree_legged_real/include/convert.h new file mode 100755 index 0000000..833b29b --- /dev/null +++ b/unitree_legged_real/include/convert.h @@ -0,0 +1,338 @@ +/************************************************************************ +Copyright (c) 2018-2019, Unitree Robotics.Co.Ltd. All rights reserved. +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 + +#ifdef SDK3_1 +#include "aliengo_sdk/aliengo_sdk.hpp" + +unitree_legged_msgs::IMU ToRos(aliengo::IMU& lcm){ + unitree_legged_msgs::IMU ros; + ros.quaternion[0] = lcm.quaternion[0]; + ros.quaternion[1] = lcm.quaternion[1]; + ros.quaternion[2] = lcm.quaternion[2]; + ros.quaternion[3] = lcm.quaternion[3]; + ros.gyroscope[0] = lcm.gyroscope[0]; + ros.gyroscope[1] = lcm.gyroscope[1]; + ros.gyroscope[2] = lcm.gyroscope[2]; + ros.accelerometer[0] = lcm.acceleration[0]; + ros.accelerometer[1] = lcm.acceleration[1]; + ros.accelerometer[2] = lcm.acceleration[2]; + // ros.rpy[0] = lcm.rpy[0]; + // ros.rpy[1] = lcm.rpy[1]; + // ros.rpy[2] = lcm.rpy[2]; + ros.temperature = lcm.temp; + return ros; +} + +unitree_legged_msgs::MotorState ToRos(aliengo::MotorState& lcm){ + unitree_legged_msgs::MotorState ros; + ros.mode = lcm.mode; + ros.q = lcm.position; + ros.dq = lcm.velocity; + ros.ddq = 0; + ros.tauEst = lcm.torque; + ros.q_raw = 0; + ros.dq_raw = 0; + ros.ddq_raw = 0; + ros.temperature = lcm.temperature; + return ros; +} + +aliengo::MotorCmd ToLcm(unitree_legged_msgs::MotorCmd& ros, aliengo::MotorCmd lcmType){ + aliengo::MotorCmd lcm; + lcm.mode = ros.mode; + lcm.position = ros.q; + lcm.velocity = ros.dq; + lcm.positionStiffness = ros.Kp; + lcm.velocityStiffness = ros.Kd; + lcm.torque = ros.tau; + return lcm; +} + +unitree_legged_msgs::LowState ToRos(aliengo::LowState& lcm){ + unitree_legged_msgs::LowState ros; + ros.levelFlag = lcm.levelFlag; + ros.commVersion = 0; + ros.robotID = 0; + ros.SN = 0; + ros.bandWidth = 0; + ros.imu = ToRos(lcm.imu); + for(int i = 0; i < 20; i++){ + ros.motorState[i] = ToRos(lcm.motorState[i]); + } + for(int i = 0; i < 4; i++){ + ros.footForce[i] = lcm.footForce[i]; + ros.footForceEst[i] = 0; + } + ros.tick = lcm.tick; + for(int i = 0; i < 40; i++){ + ros.wirelessRemote[i] = lcm.wirelessRemote[i]; + } + ros.crc = lcm.crc; + + return ros; +} + +aliengo::LowCmd ToLcm(unitree_legged_msgs::LowCmd& ros, aliengo::LowCmd lcmType){ + aliengo::LowCmd lcm; + lcm.levelFlag = ros.levelFlag; + for(int i = 0; i < 20; i++){ + lcm.motorCmd[i] = ToLcm(ros.motorCmd[i], lcm.motorCmd[i]); + } + for(int i = 0; i < 4; i++){ + lcm.led[i].r = ros.led[i].r; + lcm.led[i].g = ros.led[i].g; + lcm.led[i].b = ros.led[i].b; + } + for(int i = 0; i < 40; i++){ + lcm.wirelessRemote[i] = ros.wirelessRemote[i]; + } + lcm.crc = ros.crc; + return lcm; +} + +unitree_legged_msgs::HighState ToRos(aliengo::HighState& lcm){ + unitree_legged_msgs::HighState ros; + ros.levelFlag = lcm.levelFlag; + ros.commVersion = 0; + ros.robotID = 0; + ros.SN = 0; + ros.bandWidth = 0; + ros.mode = lcm.mode; + ros.imu = ToRos(lcm.imu); + ros.forwardSpeed = lcm.forwardSpeed; + ros.sideSpeed = lcm.sideSpeed; + ros.rotateSpeed = lcm.rotateSpeed; + ros.bodyHeight = lcm.bodyHeight; + ros.updownSpeed = lcm.updownSpeed; + ros.forwardPosition = lcm.forwardPosition.x; + ros.sidePosition = lcm.sidePosition.y; + for(int i = 0; i < 4; i++){ + ros.footPosition2Body[i].x = lcm.footPosition2Body[i].x; + ros.footPosition2Body[i].y = lcm.footPosition2Body[i].y; + ros.footPosition2Body[i].z = lcm.footPosition2Body[i].z; + ros.footSpeed2Body[i].x = lcm.footSpeed2Body[i].x; + ros.footSpeed2Body[i].y = lcm.footSpeed2Body[i].y; + ros.footSpeed2Body[i].z = lcm.footSpeed2Body[i].z; + ros.footForce[i] = lcm.footForce[i]; + ros.footForceEst[i] = 0; + } + ros.tick = lcm.tick; + for(int i = 0; i<40; i++){ + ros.wirelessRemote[i] = lcm.wirelessRemote[i]; + } + ros.reserve = 0; + ros.crc = lcm.crc; + return ros; +} + +aliengo::HighCmd ToLcm(unitree_legged_msgs::HighCmd& ros, aliengo::HighCmd lcmType){ + aliengo::HighCmd lcm; + lcm.levelFlag = ros.levelFlag; + lcm.mode = ros.mode; + lcm.forwardSpeed = ros.forwardSpeed; + lcm.sideSpeed = ros.sideSpeed; + lcm.rotateSpeed = ros.rotateSpeed; + lcm.bodyHeight = ros.bodyHeight; + lcm.footRaiseHeight = ros.footRaiseHeight; + lcm.yaw = ros.yaw; + lcm.pitch = ros.pitch; + lcm.roll = ros.roll; + for(int i = 0; i < 4; i++){ + lcm.led[i].r = ros.led[i].r; + lcm.led[i].g = ros.led[i].g; + lcm.led[i].b = ros.led[i].b; + } + for(int i = 0; i < 40; i++){ + lcm.wirelessRemote[i] = ros.wirelessRemote[i]; + } + lcm.crc = ros.crc; + return lcm; +} +#endif + +#ifdef SDK3_2 +#include "unitree_legged_sdk/unitree_legged_sdk.h" + +unitree_legged_msgs::IMU ToRos(UNITREE_LEGGED_SDK::IMU& lcm) +{ + unitree_legged_msgs::IMU ros; + ros.quaternion[0] = lcm.quaternion[0]; + ros.quaternion[1] = lcm.quaternion[1]; + ros.quaternion[2] = lcm.quaternion[2]; + ros.quaternion[3] = lcm.quaternion[3]; + ros.gyroscope[0] = lcm.gyroscope[0]; + ros.gyroscope[1] = lcm.gyroscope[1]; + ros.gyroscope[2] = lcm.gyroscope[2]; + ros.accelerometer[0] = lcm.accelerometer[0]; + ros.accelerometer[1] = lcm.accelerometer[1]; + ros.accelerometer[2] = lcm.accelerometer[2]; + // ros.rpy[0] = lcm.rpy[0]; + // ros.rpy[1] = lcm.rpy[1]; + // ros.rpy[2] = lcm.rpy[2]; + ros.temperature = lcm.temperature; + return ros; +} + +unitree_legged_msgs::MotorState ToRos(UNITREE_LEGGED_SDK::MotorState& lcm) +{ + unitree_legged_msgs::MotorState ros; + ros.mode = lcm.mode; + ros.q = lcm.q; + ros.dq = lcm.dq; + ros.ddq = lcm.ddq; + ros.tauEst = lcm.tauEst; + ros.q_raw = lcm.q_raw; + ros.dq_raw = lcm.dq_raw; + ros.ddq_raw = lcm.ddq_raw; + ros.temperature = lcm.temperature; + ros.reserve[0] = lcm.reserve[0]; + ros.reserve[1] = lcm.reserve[1]; + return ros; +} + +UNITREE_LEGGED_SDK::MotorCmd ToLcm(unitree_legged_msgs::MotorCmd& ros, UNITREE_LEGGED_SDK::MotorCmd lcmType) +{ + UNITREE_LEGGED_SDK::MotorCmd lcm; + lcm.mode = ros.mode; + lcm.q = ros.q; + lcm.dq = ros.dq; + lcm.tau = ros.tau; + lcm.Kp = ros.Kp; + lcm.Kd = ros.Kd; + lcm.reserve[0] = ros.reserve[0]; + lcm.reserve[1] = ros.reserve[1]; + lcm.reserve[2] = ros.reserve[2]; + return lcm; +} + +unitree_legged_msgs::LowState ToRos(UNITREE_LEGGED_SDK::LowState& lcm) +{ + unitree_legged_msgs::LowState ros; + ros.levelFlag = lcm.levelFlag; + ros.commVersion = lcm.commVersion; + ros.robotID = lcm.robotID; + ros.SN = lcm.SN; + ros.bandWidth = lcm.bandWidth; + ros.imu = ToRos(lcm.imu); + for(int i = 0; i<20; i++){ + ros.motorState[i] = ToRos(lcm.motorState[i]); + } + for(int i = 0; i<4; i++){ + ros.footForce[i] = lcm.footForce[i]; + ros.footForceEst[i] = lcm.footForceEst[i]; + } + ros.tick = lcm.tick; + for(int i = 0; i<40; i++){ + ros.wirelessRemote[i] = lcm.wirelessRemote[i]; + } + ros.reserve = lcm.reserve; + ros.crc = lcm.crc; + return ros; +} + +UNITREE_LEGGED_SDK::LowCmd ToLcm(unitree_legged_msgs::LowCmd& ros, UNITREE_LEGGED_SDK::LowCmd lcmType) +{ + UNITREE_LEGGED_SDK::LowCmd lcm; + lcm.levelFlag = ros.levelFlag; + lcm.commVersion = ros.commVersion; + lcm.robotID = ros.robotID; + lcm.SN = ros.SN; + lcm.bandWidth = ros.bandWidth; + for(int i = 0; i<20; i++){ + lcm.motorCmd[i] = ToLcm(ros.motorCmd[i], lcm.motorCmd[i]); + } + for(int i = 0; i<4; i++){ + lcm.led[i].r = ros.led[i].r; + lcm.led[i].g = ros.led[i].g; + lcm.led[i].b = ros.led[i].b; + } + for(int i = 0; i<40; i++){ + lcm.wirelessRemote[i] = ros.wirelessRemote[i]; + } + lcm.reserve = ros.reserve; + lcm.crc = ros.crc; + return lcm; +} + +unitree_legged_msgs::HighState ToRos(UNITREE_LEGGED_SDK::HighState& lcm) +{ + unitree_legged_msgs::HighState ros; + ros.levelFlag = lcm.levelFlag; + ros.commVersion = lcm.commVersion; + ros.robotID = lcm.robotID; + ros.SN = lcm.SN; + ros.bandWidth = lcm.bandWidth; + ros.mode = lcm.mode; + ros.imu = ToRos(lcm.imu); + ros.forwardSpeed = lcm.forwardSpeed; + ros.sideSpeed = lcm.sideSpeed; + ros.rotateSpeed = lcm.rotateSpeed; + ros.bodyHeight = lcm.bodyHeight; + ros.updownSpeed = lcm.updownSpeed; + ros.forwardPosition = lcm.forwardPosition; + ros.sidePosition = lcm.sidePosition; + for(int i = 0; i<4; i++){ + ros.footPosition2Body[i].x = lcm.footPosition2Body[i].x; + ros.footPosition2Body[i].y = lcm.footPosition2Body[i].y; + ros.footPosition2Body[i].z = lcm.footPosition2Body[i].z; + ros.footSpeed2Body[i].x = lcm.footSpeed2Body[i].x; + ros.footSpeed2Body[i].y = lcm.footSpeed2Body[i].y; + ros.footSpeed2Body[i].z = lcm.footSpeed2Body[i].z; + ros.footForce[i] = lcm.footForce[i]; + ros.footForceEst[i] = lcm.footForceEst[i]; + } + ros.tick = lcm.tick; + for(int i = 0; i<40; i++){ + ros.wirelessRemote[i] = lcm.wirelessRemote[i]; + } + ros.reserve = lcm.reserve; + ros.crc = lcm.crc; + return ros; +} + +UNITREE_LEGGED_SDK::HighCmd ToLcm(unitree_legged_msgs::HighCmd& ros, UNITREE_LEGGED_SDK::HighCmd lcmType) +{ + UNITREE_LEGGED_SDK::HighCmd lcm; + lcm.levelFlag = ros.levelFlag; + lcm.commVersion = ros.commVersion; + lcm.robotID = ros.robotID; + lcm.SN = ros.SN; + lcm.bandWidth = ros.bandWidth; + lcm.mode = ros.mode; + lcm.forwardSpeed = ros.forwardSpeed; + lcm.sideSpeed = ros.sideSpeed; + lcm.rotateSpeed = ros.rotateSpeed; + lcm.bodyHeight = ros.bodyHeight; + lcm.footRaiseHeight = ros.footRaiseHeight; + lcm.yaw = ros.yaw; + lcm.pitch = ros.pitch; + lcm.roll = ros.roll; + for(int i = 0; i<4; i++){ + lcm.led[i].r = ros.led[i].r; + lcm.led[i].g = ros.led[i].g; + lcm.led[i].b = ros.led[i].b; + } + for(int i = 0; i<40; i++){ + lcm.wirelessRemote[i] = ros.wirelessRemote[i]; + lcm.AppRemote[i] = ros.AppRemote[i]; + } + lcm.reserve = ros.reserve; + lcm.crc = ros.crc; + return lcm; +} +#endif + +#endif // _CONVERT_H_ \ No newline at end of file diff --git a/unitree_legged_real/ipconfig.sh b/unitree_legged_real/ipconfig.sh new file mode 100755 index 0000000..611374a --- /dev/null +++ b/unitree_legged_real/ipconfig.sh @@ -0,0 +1,8 @@ +#!/bin/bash + +sudo ifconfig lo multicast +sudo route add -net 224.0.0.0 netmask 240.0.0.0 dev lo + +sudo ifconfig enx000ec6612921 down +sudo ifconfig enx000ec6612921 up 192.168.123.162 netmask 255.255.255.0 + diff --git a/unitree_legged_real/launch/real.launch b/unitree_legged_real/launch/real.launch new file mode 100644 index 0000000..a73bb85 --- /dev/null +++ b/unitree_legged_real/launch/real.launch @@ -0,0 +1,12 @@ + + + + + + + + + + + \ No newline at end of file diff --git a/unitree_legged_real/package.xml b/unitree_legged_real/package.xml new file mode 100644 index 0000000..68c09e5 --- /dev/null +++ b/unitree_legged_real/package.xml @@ -0,0 +1,20 @@ + + + unitree_legged_real + 0.0.0 + The unitree_legged_real package + + unitree + TODO + + catkin + roscpp + std_msgs + roscpp + std_msgs + roscpp + std_msgs + unitree_legged_msgs + eigen + + \ No newline at end of file diff --git a/unitree_legged_real/src/exe/position_mode.cpp b/unitree_legged_real/src/exe/position_mode.cpp new file mode 100755 index 0000000..55f7683 --- /dev/null +++ b/unitree_legged_real/src/exe/position_mode.cpp @@ -0,0 +1,190 @@ +/************************************************************************ +Copyright (c) 2018-2019, Unitree Robotics.Co.Ltd. All rights reserved. +Use of this source code is governed by the MPL-2.0 license, see LICENSE. +************************************************************************/ + +#include +#include +#include +#include +#include +#include +#include +#include "convert.h" + +#ifdef SDK3_1 +using namespace aliengo; +#endif +#ifdef SDK3_2 +using namespace UNITREE_LEGGED_SDK; +#endif + +template +void* update_loop(void* param) +{ + TLCM *data = (TLCM *)param; + while(ros::ok){ + data->Recv(); + usleep(2000); + } +} + +double jointLinearInterpolation(double initPos, double targetPos, double rate) +{ + double p; + rate = std::min(std::max(rate, 0.0), 1.0); + p = initPos*(1-rate) + targetPos*rate; + return p; +} + +template +int mainHelper(int argc, char *argv[], TLCM &roslcm) +{ + std::cout << "WARNING: Control level is set to LOW-level." << std::endl + << "Make sure the robot is hung up." << std::endl + << "Press Enter to continue..." << std::endl; + std::cin.ignore(); + + ros::NodeHandle n; + ros::Rate loop_rate(500); + + long motiontime = 0; + int rate_count = 0; + int sin_count = 0; + float qInit[3]={0}; + float qDes[3]={0}; + float sin_mid_q[3] = {0.0, 1.2, -2.0}; + float Kp[3] = {0}; + float Kd[3] = {0}; + TCmd SendLowLCM = {0}; + TState RecvLowLCM = {0}; + unitree_legged_msgs::LowCmd SendLowROS; + unitree_legged_msgs::LowState RecvLowROS; + + bool initiated_flag = false; // initiate need time + int count = 0; + + roslcm.SubscribeState(); + + pthread_t tid; + pthread_create(&tid, NULL, update_loop, &roslcm); + + SendLowROS.levelFlag = LOWLEVEL; + for(int i = 0; i<12; i++){ + SendLowROS.motorCmd[i].mode = 0x0A; // motor switch to servo (PMSM) mode + SendLowROS.motorCmd[i].q = PosStopF; // 禁止位置环 + SendLowROS.motorCmd[i].Kp = 0; + SendLowROS.motorCmd[i].dq = VelStopF; // 禁止速度环 + SendLowROS.motorCmd[i].Kd = 0; + SendLowROS.motorCmd[i].tau = 0; + } + + while (ros::ok()){ + roslcm.Get(RecvLowLCM); + RecvLowROS = ToRos(RecvLowLCM); + printf("FR_2 position: %f\n", RecvLowROS.motorState[FR_2].q); + + if(initiated_flag == true){ + motiontime++; + + SendLowROS.motorCmd[FR_0].tau = -0.65f; + SendLowROS.motorCmd[FL_0].tau = +0.65f; + SendLowROS.motorCmd[RR_0].tau = -0.65f; + SendLowROS.motorCmd[RL_0].tau = +0.65f; + + // printf("%d\n", motiontime); + // printf("%d %f %f %f\n", FR_0, RecvLowROS.motorState[FR_0].q, RecvLowROS.motorState[FR_1].q, RecvLowROS.motorState[FR_2].q); + // printf("%f %f \n", RecvLowROS.motorState[FR_0].mode, RecvLowROS.motorState[FR_1].mode); + if( motiontime >= 0){ + // first, get record initial position + // if( motiontime >= 100 && motiontime < 500){ + if( motiontime >= 0 && motiontime < 10){ + qInit[0] = RecvLowROS.motorState[FR_0].q; + qInit[1] = RecvLowROS.motorState[FR_1].q; + qInit[2] = RecvLowROS.motorState[FR_2].q; + } + if( motiontime >= 10 && motiontime < 400){ + // printf("%f %f %f\n", ); + rate_count++; + double rate = rate_count/200.0; // needs count to 200 + Kp[0] = 5.0; Kp[1] = 5.0; Kp[2] = 5.0; + Kd[0] = 1.0; Kd[1] = 1.0; Kd[2] = 1.0; + + qDes[0] = jointLinearInterpolation(qInit[0], sin_mid_q[0], rate); + qDes[1] = jointLinearInterpolation(qInit[1], sin_mid_q[1], rate); + qDes[2] = jointLinearInterpolation(qInit[2], sin_mid_q[2], rate); + } + double sin_joint1, sin_joint2; + // last, do sine wave + if( motiontime >= 1700){ + sin_count++; + sin_joint1 = 0.6 * sin(3*M_PI*sin_count/1000.0); + sin_joint2 = -0.6 * sin(1.8*M_PI*sin_count/1000.0); + qDes[0] = sin_mid_q[0]; + qDes[1] = sin_mid_q[1]; + qDes[2] = sin_mid_q[2] + sin_joint2; + // qDes[2] = sin_mid_q[2]; + } + + SendLowROS.motorCmd[FR_0].q = qDes[0]; + SendLowROS.motorCmd[FR_0].dq = 0; + SendLowROS.motorCmd[FR_0].Kp = Kp[0]; + SendLowROS.motorCmd[FR_0].Kd = Kd[0]; + SendLowROS.motorCmd[FR_0].tau = -0.65f; + + SendLowROS.motorCmd[FR_1].q = qDes[1]; + SendLowROS.motorCmd[FR_1].dq = 0; + SendLowROS.motorCmd[FR_1].Kp = Kp[1]; + SendLowROS.motorCmd[FR_1].Kd = Kd[1]; + SendLowROS.motorCmd[FR_1].tau = 0.0f; + + SendLowROS.motorCmd[FR_2].q = qDes[2]; + SendLowROS.motorCmd[FR_2].dq = 0; + SendLowROS.motorCmd[FR_2].Kp = Kp[2]; + SendLowROS.motorCmd[FR_2].Kd = Kd[2]; + SendLowROS.motorCmd[FR_2].tau = 0.0f; + + } + + } + + SendLowLCM = ToLcm(SendLowROS, SendLowLCM); + roslcm.Send(SendLowLCM); + ros::spinOnce(); + loop_rate.sleep(); + + count++; + if(count > 10){ + count = 10; + initiated_flag = true; + } + } + return 0; +} + +int main(int argc, char *argv[]){ + ros::init(argc, argv, "position_ros_mode"); + std::string firmwork; + ros::param::get("/firmwork", firmwork); + + #ifdef SDK3_1 + aliengo::Control control(aliengo::LOWLEVEL); + aliengo::LCM roslcm; + mainHelper(argc, argv, roslcm); + #endif + + #ifdef SDK3_2 + std::string robot_name; + UNITREE_LEGGED_SDK::LeggedType rname; + ros::param::get("/robot_name", robot_name); + if(strcasecmp(robot_name.c_str(), "A1") == 0) + rname = UNITREE_LEGGED_SDK::LeggedType::A1; + else if(strcasecmp(robot_name.c_str(), "Aliengo") == 0) + rname = UNITREE_LEGGED_SDK::LeggedType::Aliengo; + + // UNITREE_LEGGED_SDK::Control control(rname, UNITREE_LEGGED_SDK::LOWLEVEL); + // UNITREE_LEGGED_SDK::InitEnvironment(); + UNITREE_LEGGED_SDK::LCM roslcm(LOWLEVEL); + mainHelper(argc, argv, roslcm); + #endif +} \ No newline at end of file diff --git a/unitree_legged_real/src/exe/torque_mode.cpp b/unitree_legged_real/src/exe/torque_mode.cpp new file mode 100755 index 0000000..b88a217 --- /dev/null +++ b/unitree_legged_real/src/exe/torque_mode.cpp @@ -0,0 +1,114 @@ +/************************************************************************ +Copyright (c) 2018-2019, Unitree Robotics.Co.Ltd. All rights reserved. +Use of this source code is governed by the MPL-2.0 license, see LICENSE. +************************************************************************/ + +#include +#include +#include +#include +#include +#include +#include "convert.h" + +#ifdef SDK3_1 +using namespace aliengo; +#endif +#ifdef SDK3_2 +using namespace UNITREE_LEGGED_SDK; +#endif + +template +void* update_loop(void* param) +{ + TLCM *data = (TLCM *)param; + while(ros::ok){ + data->Recv(); + usleep(2000); + } +} + +template +int mainHelper(int argc, char *argv[], TLCM &roslcm) +{ + std::cout << "WARNING: Control level is set to LOW-level." << std::endl + << "Make sure the robot is hung up." << std::endl + << "Press Enter to continue..." << std::endl; + std::cin.ignore(); + + ros::NodeHandle n; + ros::Rate loop_rate(500); + + long motiontime=0; + float torque = 0; + TCmd SendLowLCM = {0}; + TState RecvLowLCM = {0}; + unitree_legged_msgs::LowCmd SendLowROS; + unitree_legged_msgs::LowState RecvLowROS; + + roslcm.SubscribeState(); + + pthread_t tid; + pthread_create(&tid, NULL, update_loop, &roslcm); + + SendLowROS.levelFlag = LOWLEVEL; + for(int i = 0; i<12; i++){ + SendLowROS.motorCmd[i].mode = 0x0A; // motor switch to servo (PMSM) mode + } + + while (ros::ok()){ + motiontime++; + roslcm.Get(RecvLowLCM); + RecvLowROS = ToRos(RecvLowLCM); + printf("FL_1 position: %f\n", RecvLowROS.motorState[FL_1].q); + + // gravity compensation + SendLowROS.motorCmd[FR_0].tau = -0.65f; + SendLowROS.motorCmd[FL_0].tau = +0.65f; + SendLowROS.motorCmd[RR_0].tau = -0.65f; + SendLowROS.motorCmd[RL_0].tau = +0.65f; + + if( motiontime >= 500){ + torque = (0 - RecvLowROS.motorState[FL_1].q)*10.0f + (0 - RecvLowROS.motorState[FL_1].dq)*1.0f; + if(torque > 5.0f) torque = 5.0f; + if(torque < -5.0f) torque = -5.0f; + + SendLowROS.motorCmd[FL_1].q = PosStopF; + SendLowROS.motorCmd[FL_1].dq = VelStopF; + SendLowROS.motorCmd[FL_1].Kp = 0; + SendLowROS.motorCmd[FL_1].Kd = 0; + SendLowROS.motorCmd[FL_1].tau = torque; + } + SendLowLCM = ToLcm(SendLowROS, SendLowLCM); + roslcm.Send(SendLowLCM); + ros::spinOnce(); + loop_rate.sleep(); + } + return 0; +} + +int main(int argc, char *argv[]){ + ros::init(argc, argv, "torque_ros_mode"); + std::string firmwork; + ros::param::get("/firmwork", firmwork); + + #ifdef SDK3_1 + aliengo::Control control(aliengo::LOWLEVEL); + aliengo::LCM roslcm; + mainHelper(argc, argv, roslcm); + #endif + + #ifdef SDK3_2 + std::string robot_name; + UNITREE_LEGGED_SDK::LeggedType rname; + ros::param::get("/robot_name", robot_name); + if(strcasecmp(robot_name.c_str(), "A1") == 0) + rname = UNITREE_LEGGED_SDK::LeggedType::A1; + else if(strcasecmp(robot_name.c_str(), "Aliengo") == 0) + rname = UNITREE_LEGGED_SDK::LeggedType::Aliengo; + + // UNITREE_LEGGED_SDK::InitEnvironment(); + UNITREE_LEGGED_SDK::LCM roslcm(LOWLEVEL); + mainHelper(argc, argv, roslcm); + #endif +} \ No newline at end of file diff --git a/unitree_legged_real/src/exe/velocity_mode.cpp b/unitree_legged_real/src/exe/velocity_mode.cpp new file mode 100755 index 0000000..fcddb9a --- /dev/null +++ b/unitree_legged_real/src/exe/velocity_mode.cpp @@ -0,0 +1,118 @@ +/************************************************************************ +Copyright (c) 2018-2019, Unitree Robotics.Co.Ltd. All rights reserved. +Use of this source code is governed by the MPL-2.0 license, see LICENSE. +************************************************************************/ + +#include +#include +#include +#include +#include +#include +#include +#include "convert.h" + +#ifdef SDK3_1 +using namespace aliengo; +#endif +#ifdef SDK3_2 +using namespace UNITREE_LEGGED_SDK; +#endif + +template +void* update_loop(void* param) +{ + TLCM *data = (TLCM *)param; + while(ros::ok){ + data->Recv(); + usleep(2000); + } +} + +template +int mainHelper(int argc, char *argv[], TLCM &roslcm) +{ + std::cout << "WARNING: Control level is set to LOW-level." << std::endl + << "Make sure the robot is hung up." << std::endl + << "Press Enter to continue..." << std::endl; + std::cin.ignore(); + + ros::NodeHandle n; + ros::Rate loop_rate(500); + + long motiontime=0; + float Kv = 1.5; + float speed = 0; + unsigned long int Tpi =0; + TCmd SendLowLCM = {0}; + TState RecvLowLCM = {0}; + unitree_legged_msgs::LowCmd SendLowROS; + unitree_legged_msgs::LowState RecvLowROS; + + roslcm.SubscribeState(); + + pthread_t tid; + pthread_create(&tid, NULL, update_loop, &roslcm); + + SendLowROS.levelFlag = LOWLEVEL; + for(int i = 0; i<12; i++){ + SendLowROS.motorCmd[i].mode = 0x0A; // motor switch to servo (PMSM) mode + } + + while (ros::ok()){ + motiontime++; + roslcm.Get(RecvLowLCM); + RecvLowROS = ToRos(RecvLowLCM); + printf("motorState[FL_2] velocity: %f\n", RecvLowROS.motorState[FL_2].dq); + + // gravity compensation + SendLowROS.motorCmd[FR_0].tau = -0.65f; + SendLowROS.motorCmd[FL_0].tau = +0.65f; + SendLowROS.motorCmd[RR_0].tau = -0.65f; + SendLowROS.motorCmd[RL_0].tau = +0.65f; + + if( motiontime >= 500){ + SendLowROS.motorCmd[FL_2].q = PosStopF; + SendLowROS.motorCmd[FL_2].dq = speed; + SendLowROS.motorCmd[FL_2].Kp = 0; + SendLowROS.motorCmd[FL_2].Kd = Kv; + SendLowROS.motorCmd[FL_2].tau = 0.0f; + Tpi++; + // try 1 or 3 + speed = 3 * sin(4*M_PI*Tpi/1000.0); + } + SendLowLCM = ToLcm(SendLowROS, SendLowLCM); + roslcm.Send(SendLowLCM); + ros::spinOnce(); + loop_rate.sleep(); + } + return 0; +} + + +int main(int argc, char *argv[]){ + ros::init(argc, argv, "velocity_ros_mode"); + std::string firmwork; + ros::param::get("/firmwork", firmwork); + + #ifdef SDK3_1 + aliengo::Control control(aliengo::LOWLEVEL); + aliengo::LCM roslcm; + mainHelper(argc, argv, roslcm); + #endif + + #ifdef SDK3_2 + std::string robot_name; + UNITREE_LEGGED_SDK::LeggedType rname; + ros::param::get("/robot_name", robot_name); + if(strcasecmp(robot_name.c_str(), "A1") == 0) + rname = UNITREE_LEGGED_SDK::LeggedType::A1; + else if(strcasecmp(robot_name.c_str(), "Aliengo") == 0) + rname = UNITREE_LEGGED_SDK::LeggedType::Aliengo; + + // UNITREE_LEGGED_SDK::Control control(rname, UNITREE_LEGGED_SDK::LOWLEVEL); + // UNITREE_LEGGED_SDK::InitEnvironment(); + UNITREE_LEGGED_SDK::LCM roslcm(LOWLEVEL); + mainHelper(argc, argv, roslcm); + #endif +} \ No newline at end of file diff --git a/unitree_legged_real/src/exe/walk_mode.cpp b/unitree_legged_real/src/exe/walk_mode.cpp new file mode 100755 index 0000000..ee81f05 --- /dev/null +++ b/unitree_legged_real/src/exe/walk_mode.cpp @@ -0,0 +1,162 @@ +/************************************************************************ +Copyright (c) 2018-2019, Unitree Robotics.Co.Ltd. All rights reserved. +Use of this source code is governed by the MPL-2.0 license, see LICENSE. +************************************************************************/ + +#include +#include +#include +#include +#include +#include +#include +#include "convert.h" + +#ifdef SDK3_1 +using namespace aliengo; +#endif +#ifdef SDK3_2 +using namespace UNITREE_LEGGED_SDK; +#endif + +template +void* update_loop(void* param) +{ + TLCM *data = (TLCM *)param; + while(ros::ok){ + data->Recv(); + usleep(2000); + } +} + +template +int mainHelper(int argc, char *argv[], TLCM &roslcm) +{ + std::cout << "WARNING: Control level is set to HIGH-level." << std::endl + << "Make sure the robot is standing on the ground." << std::endl + << "Press Enter to continue..." << std::endl; + std::cin.ignore(); + + ros::NodeHandle n; + ros::Rate loop_rate(500); + + // SetLevel(HIGHLEVEL); + long motiontime = 0; + TCmd SendHighLCM = {0}; + TState RecvHighLCM = {0}; + unitree_legged_msgs::HighCmd SendHighROS; + unitree_legged_msgs::HighState RecvHighROS; + + roslcm.SubscribeState(); + + pthread_t tid; + pthread_create(&tid, NULL, update_loop, &roslcm); + + while (ros::ok()){ + motiontime = motiontime+2; + roslcm.Get(RecvHighLCM); + RecvHighROS = ToRos(RecvHighLCM); + // printf("%f\n", RecvHighROS.forwardSpeed); + + SendHighROS.forwardSpeed = 0.0f; + SendHighROS.sideSpeed = 0.0f; + SendHighROS.rotateSpeed = 0.0f; + SendHighROS.bodyHeight = 0.0f; + + SendHighROS.mode = 0; + SendHighROS.roll = 0; + SendHighROS.pitch = 0; + SendHighROS.yaw = 0; + + if(motiontime>1000 && motiontime<1500){ + SendHighROS.mode = 1; + // SendHighROS.roll = 0.3f; + } + + if(motiontime>1500 && motiontime<2000){ + SendHighROS.mode = 1; + SendHighROS.pitch = 0.3f; + } + + if(motiontime>2000 && motiontime<2500){ + SendHighROS.mode = 1; + SendHighROS.yaw = 0.2f; + } + + if(motiontime>2500 && motiontime<3000){ + SendHighROS.mode = 1; + SendHighROS.bodyHeight = -0.3f; + } + + if(motiontime>3000 && motiontime<3500){ + SendHighROS.mode = 1; + SendHighROS.bodyHeight = 0.3f; + } + + if(motiontime>3500 && motiontime<4000){ + SendHighROS.mode = 1; + SendHighROS.bodyHeight = 0.0f; + } + + if(motiontime>4000 && motiontime<5000){ + SendHighROS.mode = 2; + } + + if(motiontime>5000 && motiontime<8500){ + SendHighROS.mode = 2; + SendHighROS.forwardSpeed = 0.1f; // -1 ~ +1 + } + + if(motiontime>8500 && motiontime<12000){ + SendHighROS.mode = 2; + SendHighROS.forwardSpeed = -0.2f; // -1 ~ +1 + } + + if(motiontime>12000 && motiontime<16000){ + SendHighROS.mode = 2; + SendHighROS.rotateSpeed = 0.1f; // turn + } + + if(motiontime>16000 && motiontime<20000){ + SendHighROS.mode = 2; + SendHighROS.rotateSpeed = -0.1f; // turn + } + + if(motiontime>20000 && motiontime<21000){ + SendHighROS.mode = 1; + } + + SendHighLCM = ToLcm(SendHighROS, SendHighLCM); + roslcm.Send(SendHighLCM); + ros::spinOnce(); + loop_rate.sleep(); + } + return 0; +} + +int main(int argc, char *argv[]){ + ros::init(argc, argv, "walk_ros_mode"); + std::string firmwork; + ros::param::get("/firmwork", firmwork); + + #ifdef SDK3_1 + aliengo::Control control(aliengo::HIGHLEVEL); + aliengo::LCM roslcm; + mainHelper(argc, argv, roslcm); + #endif + + #ifdef SDK3_2 + std::string robot_name; + UNITREE_LEGGED_SDK::LeggedType rname; + ros::param::get("/robot_name", robot_name); + if(strcasecmp(robot_name.c_str(), "A1") == 0) + rname = UNITREE_LEGGED_SDK::LeggedType::A1; + else if(strcasecmp(robot_name.c_str(), "Aliengo") == 0) + rname = UNITREE_LEGGED_SDK::LeggedType::Aliengo; + + // UNITREE_LEGGED_SDK::InitEnvironment(); + UNITREE_LEGGED_SDK::LCM roslcm(UNITREE_LEGGED_SDK::HIGHLEVEL); + mainHelper(argc, argv, roslcm); + #endif + +} \ No newline at end of file