caowangrenbo
cf70b1a1d7
Source: DeepRoboticsLab/sdk_deploy (BSD-3-Clause), M20 sim2real part only. Packages: - invicta_msgs: JointCommandArray, MotionCommand, MotionState; standard messages (sensor_msgs/JointState, sensor_msgs/Imu) elsewhere; shared motion value defs. - invicta_deploy: state machine (idle/standup/rl/joint-damping/lie-down), ONNX policy with per-gait models, auto RL entry after standup with readiness checks, safety loop, gait->model table; no protocol/network code. - invicta_remote: vendor basic_server protocol (UDP + JSON, proto.md V1.2.0), header/MsgId, 0xE0xx error codes, motion/usage-mode/gait translation, telemetry 1002/3,4,5,6 (2/10/2/2 Hz); publishes /motion_command, subscribes /motion_state. - invicta_hardware: joint/IMU bridge skeleton (SocketCAN I/O is TODO), publishes sensor_msgs/JointState + sensor_msgs/Imu, consumes /joint_commands. Verified on ROS Noetic (Ubuntu 20.04) in docker: clean catkin_make, 200 Hz joint state / 50 Hz RL commands / 20 Hz state report, protocol conformance (MotionParam, gait preconditions, error codes), telemetry push, and end-to-end switching of two policy models with a real remote controller.
99 lines
2.5 KiB
C++
99 lines
2.5 KiB
C++
/**
|
|
* @file basic_function.hpp
|
|
* @brief basic function
|
|
* @author DeepRobotics
|
|
* @version 1.0
|
|
* @date 2025-11-07
|
|
*
|
|
* @copyright Copyright (c) 2025 DeepRobotics
|
|
*
|
|
*/
|
|
#pragma once
|
|
#include "common_types.h"
|
|
#include "custom_types.h"
|
|
|
|
using namespace types;
|
|
|
|
inline float Deg2Rad(float deg){
|
|
return deg /180.f * M_PI;
|
|
}
|
|
|
|
inline float Rad2Deg(float rad){
|
|
return rad / M_PI * 180.;
|
|
}
|
|
|
|
inline float LimitNumber(float data, float limit){
|
|
limit = fabs(limit);
|
|
if(data > limit) data = limit;
|
|
else if(data < -limit) data = -limit;
|
|
return data;
|
|
}
|
|
|
|
inline float LimitNumber(float data, float low, float high){
|
|
if(low > high){
|
|
std::cerr << "error limit range" << std::endl;
|
|
return data;
|
|
}
|
|
if(data > high) return high;
|
|
if(data < low) return low;
|
|
return data;
|
|
}
|
|
|
|
inline Mat3f RpyToRm(const Vec3f &rpy){
|
|
Eigen::AngleAxisf yawAngle(rpy(2), Vec3f::UnitZ());
|
|
Eigen::AngleAxisf pitchAngle(rpy(1), Vec3f::UnitY());
|
|
Eigen::AngleAxisf rollAngle(rpy(0), Vec3f::UnitX());
|
|
Eigen::Quaternion<float> q = yawAngle*pitchAngle*rollAngle;
|
|
return q.matrix();
|
|
}
|
|
|
|
inline float NormalizeAngle(float angle) {
|
|
float result = std::fmod(angle, 2 * M_PI);
|
|
if (result < 0) {
|
|
result += 2 * M_PI;
|
|
}
|
|
if (result > M_PI) {
|
|
result -= 2 * M_PI;
|
|
}
|
|
return result;
|
|
}
|
|
|
|
inline float Sign(const float &i){
|
|
if (i > 0) {
|
|
return 1.;
|
|
} else if (i == 0) {
|
|
return 0.;
|
|
} else {
|
|
return -1.;
|
|
}
|
|
}
|
|
|
|
inline double GetTimestampMs(){
|
|
static timespec startup_timestamp;
|
|
timespec now_timestamp;
|
|
if (startup_timestamp.tv_sec + startup_timestamp.tv_nsec == 0) {
|
|
clock_gettime(CLOCK_MONOTONIC,&startup_timestamp);
|
|
}
|
|
clock_gettime(CLOCK_MONOTONIC,&now_timestamp);
|
|
return (now_timestamp.tv_sec-startup_timestamp.tv_sec)*1e3 + (now_timestamp.tv_nsec-startup_timestamp.tv_nsec)/1e6;
|
|
}
|
|
|
|
inline float GetCubicSplinePos(float x0, float v0, float xf, float vf, float t, float T){
|
|
if(t >= T) return xf;
|
|
float a, b, c, d;
|
|
d = x0;
|
|
c = v0;
|
|
a = (vf*T - 2*xf + v0*T + 2*x0) / pow(T, 3);
|
|
b = (3*xf - vf*T - 2*v0*T - 3*x0) / pow(T, 2);
|
|
return a*pow(t, 3)+b*pow(t, 2)+c*t+d;
|
|
}
|
|
|
|
inline float GetCubicSplineVel(float x0, float v0, float xf, float vf, float t, float T){
|
|
if(t >= T) return 0;
|
|
float a, b, c;
|
|
c = v0;
|
|
a = (vf*T - 2*xf + v0*T + 2*x0) / pow(T, 3);
|
|
b = (3*xf - vf*T - 2*v0*T - 3*x0) / pow(T, 2);
|
|
return 3.*a*pow(t, 2) + 2.*b*t + c;
|
|
}
|