From 9e1ace58f5c36c6786b18e2fd2892befe7b9182a Mon Sep 17 00:00:00 2001
From: dohi <masa0u0masa1215@gmail.com>
Date: Tue, 20 Aug 2024 12:50:04 +0900
Subject: [PATCH] Make unitree_joint_control_tools header-only.

---
 unitree_legged_control/CMakeLists.txt         |  5 +--
 .../include/unitree_joint_control_tool.h      | 31 ++++++++++++++++---
 .../src/unitree_joint_control_tool.cpp        | 30 ------------------
 3 files changed, 28 insertions(+), 38 deletions(-)
 delete mode 100644 unitree_legged_control/src/unitree_joint_control_tool.cpp

diff --git a/unitree_legged_control/CMakeLists.txt b/unitree_legged_control/CMakeLists.txt
index 224450ae..8916b76d 100644
--- a/unitree_legged_control/CMakeLists.txt
+++ b/unitree_legged_control/CMakeLists.txt
@@ -23,9 +23,6 @@ catkin_package(
 
 include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIR})
 
-add_library( unitree_legged_control 
-    src/joint_controller.cpp
-    src/unitree_joint_control_tool.cpp
-)
+add_library(unitree_legged_control SHARED src/joint_controller.cpp)
 add_dependencies(${PROJECT_NAME} unitree_legged_msgs_gencpp)
 target_link_libraries(unitree_legged_control ${catkin_LIBRARIES})
diff --git a/unitree_legged_control/include/unitree_joint_control_tool.h b/unitree_legged_control/include/unitree_joint_control_tool.h
index a417d338..42afbf29 100644
--- a/unitree_legged_control/include/unitree_joint_control_tool.h
+++ b/unitree_legged_control/include/unitree_joint_control_tool.h
@@ -16,7 +16,7 @@ Use of this source code is governed by the MPL-2.0 license, see LICENSE.
 #define posStopF (2.146E+9f)  // stop position control mode
 #define velStopF (16000.0f)   // stop velocity control mode
 
-typedef struct 
+typedef struct
 {
     uint8_t mode;
     double pos;
@@ -26,8 +26,31 @@ typedef struct
     double torque;
 } ServoCmd;
 
-double clamp(double&, double, double);  // eg. clamp(1.5, -1, 1) = 1
-double computeVel(double current_position, double last_position, double last_velocity, double duration);  // get current velocity
-double computeTorque(double current_position, double current_velocity, ServoCmd&);  // get torque
+/* eg. clamp(1.5, -1, 1) = 1 */
+inline double clamp(double &val, double min_val, double max_val)
+{
+    return val = std::min(std::max(val, min_val), max_val);
+}
+
+/* get current velocity */
+inline double computeVel(double currentPos, double lastPos, double lastVel, double period)
+{
+    return lastVel*0.35f + 0.65f*(currentPos-lastPos)/period;
+}
+
+/* get torque */
+inline double computeTorque(double currentPos, double currentVel, ServoCmd &cmd)
+{
+    double targetPos, targetVel, targetTorque, posStiffness, velStiffness, calcTorque;
+    targetPos = cmd.pos;
+    targetVel = cmd.vel;
+    targetTorque = cmd.torque;
+    posStiffness = cmd.posStiffness;
+    velStiffness = cmd.velStiffness;
+    if(fabs(targetPos-posStopF) < 1e-6) posStiffness = 0;
+    if(fabs(targetVel-velStopF) < 1e-6) velStiffness = 0;
+    calcTorque = posStiffness*(targetPos-currentPos) + velStiffness*(targetVel-currentVel) + targetTorque;
+    return calcTorque;
+}
 
 #endif
diff --git a/unitree_legged_control/src/unitree_joint_control_tool.cpp b/unitree_legged_control/src/unitree_joint_control_tool.cpp
deleted file mode 100644
index 529d0204..00000000
--- a/unitree_legged_control/src/unitree_joint_control_tool.cpp
+++ /dev/null
@@ -1,30 +0,0 @@
-#include "unitree_joint_control_tool.h"
-
-float clamp(float &val, float min_val, float max_val)
-{
-    val = std::min(std::max(val, min_val), max_val);
-}
-
-double clamp(double &val, double min_val, double max_val)
-{
-    val = std::min(std::max(val, min_val), max_val);
-}
-
-double computeVel(double currentPos, double lastPos, double lastVel, double period)
-{
-    return lastVel*0.35f + 0.65f*(currentPos-lastPos)/period;
-}
-
-double computeTorque(double currentPos, double currentVel, ServoCmd &cmd)
-{
-    double targetPos, targetVel, targetTorque, posStiffness, velStiffness, calcTorque;
-    targetPos = cmd.pos;
-    targetVel = cmd.vel;
-    targetTorque = cmd.torque;
-    posStiffness = cmd.posStiffness;
-    velStiffness = cmd.velStiffness;
-    if(fabs(targetPos-posStopF) < 1e-6) posStiffness = 0;
-    if(fabs(targetVel-velStopF) < 1e-6) velStiffness = 0;
-    calcTorque = posStiffness*(targetPos-currentPos) + velStiffness*(targetVel-currentVel) + targetTorque;
-    return calcTorque;
-}
\ No newline at end of file