diff --git a/README.md b/README.md index c27ea4c..2898263 100644 --- a/README.md +++ b/README.md @@ -15,11 +15,14 @@ Real robot control related: `unitree_legged_real` # Dependencies * [ROS](https://www.ros.org/) melodic or ROS kinetic(has not been tested) * [Gazebo8](http://gazebosim.org/) -* [unitree_legged_sdk](https://github.com/unitreerobotics) -* [aliengo_sdk](https://github.com/unitreerobotics) +* [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 @@ -27,14 +30,18 @@ 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 +# amd64, arm32, arm64 export UNITREE_PLATFORM="amd64" ``` # Build -Please run the following command to install relative packages. +If you just need to send message from the ROS on robot's NX or TX2, you can just copy `unitree_legged_msgs` and `unitree_legged_real` to `catkin_ws/src` folder and run `catkin_make` to compile. + +If you would like to fully compile the `unitree_ros`, please run the following command to install relative packages. If your ROS is melodic: ``` @@ -61,6 +68,7 @@ catkin_make If you face a dependency problem, you can just run `catkin_make` again. + # Detail of Packages ## unitree_legged_control: It contains the joints controllers for Gazebo simulation, which allows user to control joints with position, velocity and torque. @@ -148,4 +156,4 @@ 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`. +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/CMakeLists.txt b/unitree_legged_real/CMakeLists.txt old mode 100644 new mode 100755 index c34799e..9c91639 --- a/unitree_legged_real/CMakeLists.txt +++ b/unitree_legged_real/CMakeLists.txt @@ -4,34 +4,46 @@ project(unitree_legged_real) add_compile_options(-std=c++11) find_package(catkin REQUIRED COMPONENTS - controller_manager roscpp - tf geometry_msgs unitree_legged_msgs ) -find_package(Eigen3 REQUIRED) - -catkin_package( -) +catkin_package() include_directories( include ${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIR} - $ENV{UNITREE_LEGGED_SDK_PATH}/include - $ENV{ALIENGO_SDK_PATH}/include ) -link_directories($ENV{UNITREE_LEGGED_SDK_PATH}/lib) -link_directories($ENV{ALIENGO_SDK_PATH}/lib) -string(CONCAT LEGGED_SDK_NAME libunitree_legged_sdk_$ENV{UNITREE_PLATFORM}.so) -set(EXTRA_LIBS ${LEGGED_SDK_NAME} libaliengo_comm.so lcm) - 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}) @@ -46,13 +58,4 @@ add_dependencies(torque_lcm ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTE 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}) - - -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}) - -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}) \ No newline at end of file +add_dependencies(walk_lcm ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) \ No newline at end of file diff --git a/unitree_legged_real/include/convert.h b/unitree_legged_real/include/convert.h old mode 100644 new mode 100755 index 87eded1..833b29b --- a/unitree_legged_real/include/convert.h +++ b/unitree_legged_real/include/convert.h @@ -6,7 +6,6 @@ Use of this source code is governed by the MPL-2.0 license, see LICENSE. #ifndef _CONVERT_H_ #define _CONVERT_H_ -#include "unitree_legged_sdk/unitree_legged_sdk.h" #include #include #include @@ -14,12 +13,158 @@ Use of this source code is governed by the MPL-2.0 license, see LICENSE. #include #include #include + +#ifdef SDK3_1 #include "aliengo_sdk/aliengo_sdk.hpp" -enum firmworkVersion{ - V3_1, - V3_2 -}; +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) { @@ -41,25 +186,6 @@ unitree_legged_msgs::IMU ToRos(UNITREE_LEGGED_SDK::IMU& lcm) return ros; } -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(UNITREE_LEGGED_SDK::MotorState& lcm) { unitree_legged_msgs::MotorState ros; @@ -77,23 +203,6 @@ unitree_legged_msgs::MotorState ToRos(UNITREE_LEGGED_SDK::MotorState& lcm) 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; - -// printf("dq: %f\n", lcm.velocity); - - return ros; -} - UNITREE_LEGGED_SDK::MotorCmd ToLcm(unitree_legged_msgs::MotorCmd& ros, UNITREE_LEGGED_SDK::MotorCmd lcmType) { UNITREE_LEGGED_SDK::MotorCmd lcm; @@ -109,17 +218,6 @@ UNITREE_LEGGED_SDK::MotorCmd ToLcm(unitree_legged_msgs::MotorCmd& ros, UNITREE_L return lcm; } -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(UNITREE_LEGGED_SDK::LowState& lcm) { unitree_legged_msgs::LowState ros; @@ -145,30 +243,6 @@ unitree_legged_msgs::LowState ToRos(UNITREE_LEGGED_SDK::LowState& lcm) return ros; } -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; -} - UNITREE_LEGGED_SDK::LowCmd ToLcm(unitree_legged_msgs::LowCmd& ros, UNITREE_LEGGED_SDK::LowCmd lcmType) { UNITREE_LEGGED_SDK::LowCmd lcm; @@ -193,24 +267,6 @@ UNITREE_LEGGED_SDK::LowCmd ToLcm(unitree_legged_msgs::LowCmd& ros, UNITREE_LEGGE return lcm; } -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(UNITREE_LEGGED_SDK::HighState& lcm) { unitree_legged_msgs::HighState ros; @@ -247,41 +303,6 @@ unitree_legged_msgs::HighState ToRos(UNITREE_LEGGED_SDK::HighState& lcm) return ros; } -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; -} - UNITREE_LEGGED_SDK::HighCmd ToLcm(unitree_legged_msgs::HighCmd& ros, UNITREE_LEGGED_SDK::HighCmd lcmType) { UNITREE_LEGGED_SDK::HighCmd lcm; @@ -312,30 +333,6 @@ UNITREE_LEGGED_SDK::HighCmd ToLcm(unitree_legged_msgs::HighCmd& ros, UNITREE_LEG lcm.crc = ros.crc; return lcm; } +#endif -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 \ No newline at end of file +#endif // _CONVERT_H_ \ 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 old mode 100644 new mode 100755 index 0db0b27..55f7683 --- a/unitree_legged_real/src/exe/position_mode.cpp +++ b/unitree_legged_real/src/exe/position_mode.cpp @@ -10,11 +10,14 @@ Use of this source code is governed by the MPL-2.0 license, see LICENSE. #include #include #include -#include "unitree_legged_sdk/unitree_legged_sdk.h" -#include "aliengo_sdk/aliengo_sdk.hpp" #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) @@ -164,12 +167,13 @@ int main(int argc, char *argv[]){ std::string firmwork; ros::param::get("/firmwork", firmwork); - if(firmwork == "3_1"){ + #ifdef SDK3_1 aliengo::Control control(aliengo::LOWLEVEL); aliengo::LCM roslcm; mainHelper(argc, argv, roslcm); - } - else if(firmwork == "3_2"){ + #endif + + #ifdef SDK3_2 std::string robot_name; UNITREE_LEGGED_SDK::LeggedType rname; ros::param::get("/robot_name", robot_name); @@ -179,8 +183,8 @@ int main(int argc, char *argv[]){ rname = UNITREE_LEGGED_SDK::LeggedType::Aliengo; // UNITREE_LEGGED_SDK::Control control(rname, UNITREE_LEGGED_SDK::LOWLEVEL); - UNITREE_LEGGED_SDK::InitEnvironment(); + // 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 old mode 100644 new mode 100755 index a16de3c..b88a217 --- a/unitree_legged_real/src/exe/torque_mode.cpp +++ b/unitree_legged_real/src/exe/torque_mode.cpp @@ -9,11 +9,14 @@ Use of this source code is governed by the MPL-2.0 license, see LICENSE. #include #include #include -#include "unitree_legged_sdk/unitree_legged_sdk.h" -#include "aliengo_sdk/aliengo_sdk.hpp" #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) @@ -89,12 +92,13 @@ int main(int argc, char *argv[]){ std::string firmwork; ros::param::get("/firmwork", firmwork); - if(firmwork == "3_1"){ + #ifdef SDK3_1 aliengo::Control control(aliengo::LOWLEVEL); aliengo::LCM roslcm; mainHelper(argc, argv, roslcm); - } - else if(firmwork == "3_2"){ + #endif + + #ifdef SDK3_2 std::string robot_name; UNITREE_LEGGED_SDK::LeggedType rname; ros::param::get("/robot_name", robot_name); @@ -103,8 +107,8 @@ int main(int argc, char *argv[]){ else if(strcasecmp(robot_name.c_str(), "Aliengo") == 0) rname = UNITREE_LEGGED_SDK::LeggedType::Aliengo; - UNITREE_LEGGED_SDK::InitEnvironment(); + // 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 old mode 100644 new mode 100755 index 4757b8a..fcddb9a --- a/unitree_legged_real/src/exe/velocity_mode.cpp +++ b/unitree_legged_real/src/exe/velocity_mode.cpp @@ -10,11 +10,14 @@ Use of this source code is governed by the MPL-2.0 license, see LICENSE. #include #include #include -#include "unitree_legged_sdk/unitree_legged_sdk.h" -#include "aliengo_sdk/aliengo_sdk.hpp" #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) @@ -92,12 +95,13 @@ int main(int argc, char *argv[]){ std::string firmwork; ros::param::get("/firmwork", firmwork); - if(firmwork == "3_1"){ + #ifdef SDK3_1 aliengo::Control control(aliengo::LOWLEVEL); aliengo::LCM roslcm; mainHelper(argc, argv, roslcm); - } - else if(firmwork == "3_2"){ + #endif + + #ifdef SDK3_2 std::string robot_name; UNITREE_LEGGED_SDK::LeggedType rname; ros::param::get("/robot_name", robot_name); @@ -107,8 +111,8 @@ int main(int argc, char *argv[]){ rname = UNITREE_LEGGED_SDK::LeggedType::Aliengo; // UNITREE_LEGGED_SDK::Control control(rname, UNITREE_LEGGED_SDK::LOWLEVEL); - UNITREE_LEGGED_SDK::InitEnvironment(); + // 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 old mode 100644 new mode 100755 index ebac273..ee81f05 --- a/unitree_legged_real/src/exe/walk_mode.cpp +++ b/unitree_legged_real/src/exe/walk_mode.cpp @@ -10,11 +10,14 @@ Use of this source code is governed by the MPL-2.0 license, see LICENSE. #include #include #include -#include "unitree_legged_sdk/unitree_legged_sdk.h" -#include "aliengo_sdk/aliengo_sdk.hpp" #include "convert.h" -// using namespace UNITREE_LEGGED_SDK; +#ifdef SDK3_1 +using namespace aliengo; +#endif +#ifdef SDK3_2 +using namespace UNITREE_LEGGED_SDK; +#endif template void* update_loop(void* param) @@ -136,12 +139,13 @@ int main(int argc, char *argv[]){ std::string firmwork; ros::param::get("/firmwork", firmwork); - if(firmwork == "3_1"){ + #ifdef SDK3_1 aliengo::Control control(aliengo::HIGHLEVEL); aliengo::LCM roslcm; mainHelper(argc, argv, roslcm); - } - else if(firmwork == "3_2"){ + #endif + + #ifdef SDK3_2 std::string robot_name; UNITREE_LEGGED_SDK::LeggedType rname; ros::param::get("/robot_name", robot_name); @@ -150,9 +154,9 @@ int main(int argc, char *argv[]){ else if(strcasecmp(robot_name.c_str(), "Aliengo") == 0) rname = UNITREE_LEGGED_SDK::LeggedType::Aliengo; - UNITREE_LEGGED_SDK::InitEnvironment(); + // UNITREE_LEGGED_SDK::InitEnvironment(); UNITREE_LEGGED_SDK::LCM roslcm(UNITREE_LEGGED_SDK::HIGHLEVEL); mainHelper(argc, argv, roslcm); - } + #endif } \ No newline at end of file