initial commit

include/types/common_types.h

@@ -0,0 +1,122 @@
+
+#pragma once
+
+#include <Eigen/Dense>
+#include <iostream>
+#include <iomanip>
+#include <vector>
+#include <deque>
+#include <map>
+#include <cmath>
+#include <memory>
+#include <thread>
+#include <algorithm>
+#include <sstream>
+#include <fstream>
+#include <sys/timerfd.h>
+#include <sys/epoll.h>
+#include <sys/socket.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <unistd.h>
+#include <stdint.h>
+#include <arpa/inet.h>
+#include <netinet/in.h>
+#include <fcntl.h>
+#include <stdio.h>
+#include <csignal>
+#include <pthread.h>
+
+
+namespace types {
+using Vec3f = Eigen::Vector3f;
+using Vec3d = Eigen::Vector3d;
+using Vec4f = Eigen::Vector4f;
+using Vec4d = Eigen::Vector4d;
+using VecXf = Eigen::VectorXf;
+using VecXd = Eigen::VectorXd;
+
+using Mat3f = Eigen::Matrix3f;
+using Mat3d = Eigen::Matrix3d;
+using MatXf = Eigen::MatrixXf;
+using MatXd = Eigen::MatrixXd;
+
+const float gravity = 9.815;
+
+struct RobotBasicState {
+    Vec3f base_rpy;
+    Vec4f base_quat;
+    Mat3f base_rot_mat;
+    Vec3f base_omega;
+    Vec3f base_acc;
+    Vec3f base_lin_vel;
+    MatXf flt_base_acc_mat;
+    VecXf joint_pos;
+    VecXf joint_vel;
+    VecXf joint_tau;
+    // FireAnt sim2sim order: wheel FL/FR/HL/HR, base, knee FL/FR/HL/HR.
+    VecXf contact_force;
+
+    RobotBasicState(int dof = 16) {
+        base_rpy.setZero();
+        base_quat.setZero();
+        base_rot_mat.setIdentity();
+        base_omega.setZero();
+        base_acc.setZero();
+        base_lin_vel.setZero();
+        flt_base_acc_mat = Eigen::MatrixXf::Zero(20, 3);
+        joint_pos = VecXf::Zero(dof);
+        joint_vel = VecXf::Zero(dof);
+        joint_tau = VecXf::Zero(dof);
+        contact_force = VecXf::Zero(9);
+    }
+};
+
+struct RobotAction {
+    VecXf goal_joint_pos;
+    VecXf goal_joint_vel;
+    VecXf kp;
+    VecXf kd;
+    VecXf tau_ff;
+
+    MatXf ConvertToMat() {
+        MatXf res(goal_joint_pos.rows(), 5);
+        res.col(0) = kp;
+        res.col(1) = goal_joint_pos;
+        res.col(2) = kd;
+        res.col(3) = goal_joint_vel;
+        res.col(4) = tau_ff;
+        return res;
+    }
+};
+
+
+struct UserCommand {
+    double time_stamp;
+        uint8_t safe_control_mode; //0 normal, 1 stand, 2 sit, 3 joint damping
+    uint8_t target_mode;
+    float forward_vel_scale = 0;
+    float side_vel_scale = 0;
+    float turnning_vel_scale = 0;
+    float reserved_scale;
+};
+
+enum FromType {
+    inside = 0,
+    user,
+};
+struct NetInfo {
+    std::string ip;
+    int port;
+    FromType fromType;
+};
+
+struct DataInfo : public NetInfo {
+    const char* buffer;
+    ssize_t len;
+};
+struct CmdInfo : public NetInfo {
+    int type;
+    int command;
+};
+};  // namespace types

include/types/custom_types.h

@@ -0,0 +1,52 @@
+#pragma once
+
+#include "common_types.h"
+
+namespace types{
+    enum RobotName{
+        M20 = 11,
+        FireAnt = 12,
+    };
+
+    enum RobotType{
+        QuadrupedWheel = 1,
+    };
+
+    enum RobotMotionState{
+        WaitingForStand = 0,
+        StandingUp      = 1,
+        JointDamping    = 2,
+        LieDown         = 4,
+        RLControlMode   = 6,
+    };
+
+    enum StateName{
+        kInvalid      = -1,
+        kIdle         = 0,
+        kStandUp      = 1,
+        kJointDamping = 2,
+        kLieDown      = 4,
+        kRLControl    = 6,
+    };
+
+    enum RemoteCommandType{
+        kKeyBoard = 0,
+        kGamepad,
+    };
+    
+    enum KeyCode {
+        L1,
+        L2,
+        R1,
+        R2,
+        UNKNOWN
+    };
+
+    inline std::string GetAbsPath(){
+        char buffer[PATH_MAX];
+        if(getcwd(buffer, sizeof(buffer)) != NULL){
+            return std::string(buffer);
+        }
+        return "";
+    }
+};

include/types/dds_types.h

@@ -0,0 +1,69 @@
+#include "common_types.h"
+
+namespace dds{
+    typedef struct{      // BatterySensorData  BatteryBaseInfoTag
+        uint16_t voltage;             ///<总电压            单位10mV
+        int16_t  current;             ///<总电流            单位10mA
+        uint16_t remaining_capacity;  ///<剩余容量          单位10mAH
+        uint16_t nominal_capacity;    ///<标称容量          单位10mAH
+        uint16_t cycles;              ///<循环次数
+        uint16_t production_date;     ///<生产日期
+        uint16_t balanced_low;        ///<均衡低
+        uint16_t balanced_high;       ///<均衡高
+        uint16_t protected_state;     ///<保护状态
+
+        uint8_t software_version;  ///<软件版本
+        uint8_t battery_level;     ///<剩余容量百分比
+        uint8_t mos_state;         ///<Mos状态
+        uint8_t battery_quantity;  ///<电池串数
+        uint8_t battery_ntc;       ///<NTC个数
+
+        float battery_temperature[4];
+    }BatteryInfo_dds;
+
+    typedef struct {
+        float pos, vel, tor, kp, kd;
+        float motor_temperature, driver_temperature;
+        uint16_t status_word, control_word, data_id;  
+    } JointData_dds;
+
+    struct ImuData_dds{
+        float roll, pitch, yaw;
+        float omega_x, omega_y, omega_z;
+        float acc_x, acc_y, acc_z;
+        uint32_t timestamp;
+    };
+
+    // 命令索引
+    enum CommandIndex{
+        kIndexDisable = 1,
+        kIndexEnable = 2,
+        kIndexCalibration = 3,
+        //    kIndexOutEncoderCalibration = 4,
+        kIndexMotorControl = 4,
+        kIndexMotorReset = 5,
+        kIndexSetHome = 6,
+        kIndexSetGear = 7,
+        kIndexSetID = 8,
+        kIndexSetCanTimeout = 9,
+        kIndexSetBandwidth = 10,
+        kIndexSetLimitCurrent = 11,
+        kIndexSetUnderVoltage = 12,
+        kIndexSetOverVoltage = 13,
+        kIndexSetMotorTemperature = 14,
+        kIndexSetDriverTemperature = 15,
+        kIndexSaveConfig = 16,
+        kIndexErrorReset = 17,
+        kIndexWriteAppBackStart = 18,
+        kIndexWriteAppBack = 19,
+        kIndexCheckAppBack = 20,
+        kIndexDfuStart = 21,
+        kIndexGetVersion = 22,
+        kIndexGetStatusWord = 23,
+        kIndexGetConfig = 24,
+        kIndexOutEncoderCalibration = 25,
+        kIndexGetOutEncoder = 26,
+        kIndexCalibReport = 31
+    };
+}
+