EmbodiedAI
/
unitree_ros
mirror of https://github.com/unitreerobotics/unitree_ros.git
3
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Make unitree_joint_control_tools header-only.

This commit is contained in:
dohi 2024-08-20 12:50:04 +09:00
parent 8bca7f8907
commit 9e1ace58f5
3 changed files with 28 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
unitree_legged_control/CMakeLists.txt
View File

@ -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})

31
unitree_legged_control/include/unitree_joint_control_tool.h
View File

@ -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

30
unitree_legged_control/src/unitree_joint_control_tool.cpp
View File

@ -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;
}