fix: clear up

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)
 

src/unitree_rl/include/convert.h

@@ -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 <unitree_legged_msgs/LowCmd.h>
-#include <unitree_legged_msgs/LowState.h>
-#include <unitree_legged_msgs/HighCmd.h>
-#include <unitree_legged_msgs/HighState.h>
-#include <unitree_legged_msgs/MotorCmd.h>
-#include <unitree_legged_msgs/MotorState.h>
-#include <unitree_legged_msgs/IMU.h>
-
-#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_

src/unitree_rl/include/unitree_rl.hpp

@@ -5,8 +5,8 @@
 #include <gazebo_msgs/ModelStates.h>
 #include <sensor_msgs/JointState.h>
 #include <geometry_msgs/Twist.h>
-#include "../lib/model.cpp"
+#include "../library/model/model.hpp"
-#include "../lib/observation_buffer.hpp"
+#include "../library/observation_buffer/observation_buffer.hpp"
 #include "unitree_legged_msgs/MotorCmd.h"
 
 class Unitree_RL : public Model

src/unitree_rl/include/unitree_rl_real.hpp

@@ -5,11 +5,13 @@
 #include <gazebo_msgs/ModelStates.h>
 #include <sensor_msgs/JointState.h>
 #include <geometry_msgs/Twist.h>
-#include "../lib/model.cpp"
+#include "../library/model/model.hpp"
-#include "../lib/observation_buffer.hpp"
+#include "../library/observation_buffer/observation_buffer.hpp"
 #include <unitree_legged_msgs/LowCmd.h>
 #include "unitree_legged_msgs/LowState.h"
-#include "convert.h"
+#include <unitree_legged_msgs/MotorCmd.h>
+#include <unitree_legged_msgs/MotorState.h>
+#include "unitree_legged_sdk/unitree_legged_sdk.h"
 #include <pthread.h>
 
 using namespace UNITREE_LEGGED_SDK;