diff --git a/src/unitree_rl/CMakeLists.txt b/src/unitree_rl/CMakeLists.txt index 34be3c1..4b5161c 100644 --- a/src/unitree_rl/CMakeLists.txt +++ b/src/unitree_rl/CMakeLists.txt @@ -28,8 +28,6 @@ catkin_package( include_directories(library/unitree_legged_sdk_3.2/include) link_directories(library/unitree_legged_sdk_3.2/lib) set(EXTRA_LIBS -pthread libunitree_legged_sdk_amd64.so lcm) -add_executable(lcm_server $ENV{UNITREE_LEGGED_SDK_PATH}/examples/lcm_server.cpp) -target_link_libraries(lcm_server ${EXTRA_LIBS} ${catkin_LIBRARIES}) include_directories( include @@ -41,11 +39,11 @@ include_directories( set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}") -add_library(model lib/model.cpp lib/model.hpp) +add_library(model library/model/model.cpp library/model/model.hpp) target_link_libraries(model "${TORCH_LIBRARIES}") set_property(TARGET model PROPERTY CXX_STANDARD 14) -add_library(observation_buffer lib/observation_buffer.cpp lib/observation_buffer.hpp) +add_library(observation_buffer library/observation_buffer/observation_buffer.cpp library/observation_buffer/observation_buffer.hpp) target_link_libraries(observation_buffer "${TORCH_LIBRARIES}") set_property(TARGET observation_buffer PROPERTY CXX_STANDARD 14) diff --git a/src/unitree_rl/include/convert.h b/src/unitree_rl/include/convert.h deleted file mode 100644 index e2e900d..0000000 --- a/src/unitree_rl/include/convert.h +++ /dev/null @@ -1,187 +0,0 @@ -/************************************************************************ -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 - -#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 // _CONVERT_H_ \ No newline at end of file diff --git a/src/unitree_rl/include/unitree_rl.hpp b/src/unitree_rl/include/unitree_rl.hpp index e791078..3a89efb 100644 --- a/src/unitree_rl/include/unitree_rl.hpp +++ b/src/unitree_rl/include/unitree_rl.hpp @@ -5,8 +5,8 @@ #include #include #include -#include "../lib/model.cpp" -#include "../lib/observation_buffer.hpp" +#include "../library/model/model.hpp" +#include "../library/observation_buffer/observation_buffer.hpp" #include "unitree_legged_msgs/MotorCmd.h" class Unitree_RL : public Model diff --git a/src/unitree_rl/include/unitree_rl_real.hpp b/src/unitree_rl/include/unitree_rl_real.hpp index 957b8b2..e4875ae 100644 --- a/src/unitree_rl/include/unitree_rl_real.hpp +++ b/src/unitree_rl/include/unitree_rl_real.hpp @@ -5,11 +5,13 @@ #include #include #include -#include "../lib/model.cpp" -#include "../lib/observation_buffer.hpp" +#include "../library/model/model.hpp" +#include "../library/observation_buffer/observation_buffer.hpp" #include #include "unitree_legged_msgs/LowState.h" -#include "convert.h" +#include +#include +#include "unitree_legged_sdk/unitree_legged_sdk.h" #include using namespace UNITREE_LEGGED_SDK; diff --git a/src/unitree_rl/lib/model.cpp b/src/unitree_rl/library/model/model.cpp similarity index 100% rename from src/unitree_rl/lib/model.cpp rename to src/unitree_rl/library/model/model.cpp diff --git a/src/unitree_rl/lib/model.hpp b/src/unitree_rl/library/model/model.hpp similarity index 100% rename from src/unitree_rl/lib/model.hpp rename to src/unitree_rl/library/model/model.hpp diff --git a/src/unitree_rl/lib/observation_buffer.cpp b/src/unitree_rl/library/observation_buffer/observation_buffer.cpp similarity index 100% rename from src/unitree_rl/lib/observation_buffer.cpp rename to src/unitree_rl/library/observation_buffer/observation_buffer.cpp diff --git a/src/unitree_rl/lib/observation_buffer.hpp b/src/unitree_rl/library/observation_buffer/observation_buffer.hpp similarity index 100% rename from src/unitree_rl/lib/observation_buffer.hpp rename to src/unitree_rl/library/observation_buffer/observation_buffer.hpp