#ifndef COMMON_H #define COMMON_H #include #include #include "com_define.h" // #include typedef struct ROBOT_CONFIG_ { //utility double dt; double legIsRightLst[LEG_NUMBER]; //kinematics and dynamics double l0; double l1; double l2; double lMin2; double hip[LEG_NUMBER][3]; double legs_offset[LEG_NUMBER]; //dynamics double m; double inertia[3][3]; double leg_m; //control paramters double legStiffSw[3]; double legStiffSt[3]; double legDamp[3]; double legStiffSphSw[3]; double legStiffSphSt[3]; double legDampSph[3]; double jntStiffSw[3]; double jntStiffSt[3]; double jntDamp[3]; double qddKp[3]; double qddKd[3]; //gait parameters double offset_lst[LEG_NUMBER]; double stdP[LEG_NUMBER][3]; double bodyH; // constraints double wdMax; //compensation double fricComp[3]; }ROBOT_CONFIG; typedef union { float array[3]; struct { float x; float y; float z; } axis; } MeasurementVector; typedef union { float array[3]; struct { float roll; float pitch; float yaw; } axis; } EulerAngleVector; typedef union{ double array[9]; } Covariance; /** * @brief Quaternion. */ typedef union { float array[4]; struct { float w; float x; float y; float z; } element; } QuaternionVector; typedef union { float array[3]; struct { float x; float y; float z; } axis; } GPSVector; typedef struct TOP_CONFIG_INFO_{ std::string robot_type; ROBOT_DRIVER_TYPES body_model; }TOP_CONFIG_INFO; namespace robot_control { namespace common { const int GAIT_MSG_CAPACITY = 200; enum class CoreState { BOOT = 0, IDLE = 1, JOINTS_TEST = 2, STAND_STILL = 3, IN_PLANNING = 4 }; enum GaitMsgType //这里如果修改成enum class，需要芳勇配合修改参数接口 { GMT_NO_MSG = 0, GMT_START = 1, GMT_ONLINE = 2, GMT_EM_STOP = 3, GMT_JOINT_CONTROL = 4, GMT_GAIT_MODE = 5 }; enum GaitID //这里如果修改成enum class，需要芳勇配合修改参数接口 { GID_NO_GAIT = 0, GID_IDLE_GAIT = 1, GID_STANDSTILL = 2, GID_RECOVER = 3, GID_HEX_NIMBLE = 4, GID_JOINT_TEST = 5, GID_RL = 6, //强化学习步态 GID_JOINT_TRAJECTORY_CONTROL = 7, GID_POSE_ADJUSTMENT=8, GID_FOOT_ADJUST=9, GID_LEG_TEST=10, GID_QUA_NIMBLE = 11, GID_MAX = 12 }; class GaitMsgHeader { public: short int gait_msg_type; short int gait_id; int data_length; }; class GaitMsg { public: GaitMsgHeader header; char data[GAIT_MSG_CAPACITY]; inline int getAllDataLength() { return header.data_length + sizeof(GaitMsgHeader); } }; class Pose { public: float q[4]; float gyro[3]; float acc[3]; }; class ModelInput { public: unsigned long count; double actual_pos[MOTOR_NUM]; double actual_vel[MOTOR_NUM]; double actual_trq[MOTOR_NUM]; // trq read from the motor current double mea_trq[MOTOR_NUM]; // trq read from the torque sensor Pose body_pose; GaitMsg gait_msg; bool is_stand_up; int planner_stage; public: ModelInput() { is_stand_up = false; } }; struct JointTestData { unsigned int axis_bitmask; float mag; float period; float kp_pos; float kd_pos; float kp_cur; float mu_clb; int test_type; }; class ModelOutput { public: unsigned long count; double target_pos[MOTOR_NUM]; double target_vel[MOTOR_NUM]; double target_trq[MOTOR_NUM]; double target_kp[MOTOR_NUM]; double target_kd[MOTOR_NUM]; CoreState core_state; GaitID current_gait_id; }; typedef struct MC_QUATERNIONS_{ float x; float y; float z; float w; }MC_QUATERNIONS; typedef struct MC_COORDINATE_{ float x; float y; float z; }MC_COORDINATE; typedef struct MC_MOTION_IMU_DATA_{ uint64_t timestamp; MC_QUATERNIONS orientation; double orientation_covariance[9]; MC_COORDINATE angular_velocity; double angular_velocity_covariance[9]; MC_COORDINATE linear_acceleration; double linear_acceleration_covariance[9]; }MC_MOTION_IMU_DATA; typedef struct JOY_KEY_TYPE_{ bool stand_up; int move_mode; int speed_gear; double dynamic_speed_x; double dynamic_speed_y; double static_speed_x; double static_speed_y; }JOY_KEY_TYPE; typedef enum ROBOT_DIRECTION_{ FRONT = 0, REAR, LEFT, RIGHT }ROBOT_DIRECTION; typedef struct ROBOT_TWIST_{ MC_COORDINATE linear; MC_COORDINATE angular; }ROBOT_TWIST; //0: Trot（默认）1: 奔跑 2: 跳跃 3: Saturn标志动作 typedef enum ROBOT_GAIT_{ TROT = 0, RUN, JUMP, SATURN, NIMBLE }ROBOT_GAIT; //int32 model = 1; //0: 自主导航， 1,遥控控制 typedef enum ROBOT_CONTROL_MOVE_MODE_{ NAVIGATION = 0, JOY }ROBOT_CONTROL_MOVE_MODE; typedef enum DRIVER_ENABLE_STATE_{ UNKNOW = 0, ENABLED, ENABLING, ENABLE_FAILED, DISABLED }DRIVER_ENABLE_STATE; typedef enum EQA_STATE_{ EQA_NULL = 0, LISTEN = 1, THINK = 2, EXECUTE = 3 }EQA_STATE; //scene = 1; //0: 平地 , 1: 楼梯 , 2: 沟壑 , 3: 陡坡 , 4: 限高 typedef enum SCENE_TYPE_{ NULL_SCENE = 0, LIE_DOWN = 1, WALKING = 2, STAIRS = 3, CHARGE = 4, PERCEIVE_STAIRS = 5, SNOW = 6, SLIPPY = 7, STONE = 8 }SCENE_TYPE; //机器人场景切换状态枚举 typedef enum SCENE_SWITCH_STATE_{ NULL_SCENE_SWITCH = 0, SWITCHING = 1, SWITCH_SUCCEEDED = 2, SWITCH_FAILED = 3 }SCENE_SWITCH_STATE; typedef enum BODY_POSTURE_STATE_{ NULL_POSTURE = 0, STARTMOVING = 1, MOVING = 2, LIEDOWN = 3, STANDUP = 4, CHARGING=5 //在桩上可能是充电，也可能已经充满了 }BODY_POSTURE_STATE; //相机位置 typedef enum CHARGE_POSITION_STATE_{ NULL_POSITION = 0, POSITION_OFF = 1, //找不到位置默认状态 POSITION_FINDING = 2,//搜寻位置此时需要用户手动控制机器人进行移动 POSITION_ONLINE = 3, //位置信息在线 POSITION_FAILED = 4 //用户调整超时后进入该状态，考虑二维码破损造成的情况 }CHARGE_POSITION_STATE; //上桩过程枚举 typedef enum CHARGE_SWITCH_STATE_{ NULL_CHARGE_SWITCH = 0, EXIT_SUCCEEDED = 1, EXITING_TILE = 2, EXIT_FAILED=3, ENTER_SUCCEEDED = 4, ENTERING_TILE = 5, ENTER_FAILED=6 //尝试上桩超过三次后进入该状态 }CHARGE_SWITCH_STATE; typedef enum AUTO_CHARGE_STATE_{ null_charge = 0, uncharge = 1, charging = 2, charge_finished = 3 }AUTO_CHARGE_STATE; typedef struct ROBOT_BATTERY_INFO_{ float level; //机器人电池当前电量 AUTO_CHARGE_STATE state; //用来表示机器人电池没充电，充电中，充电结束的状态 }ROBOT_BATTERY_INFO; typedef struct BODY_ODOMETRY_{ ROBOT_TWIST twist; //geometry_msgs/Pose Point & Quaternion MC_COORDINATE point; MC_QUATERNIONS quaternion; // 6*6 covariance matrix // repeated pose = 2; }BODY_ODOMETRY; //充电相关状态 typedef struct ROBOT_CHARGE_STATUS_{ //运动控制主机和手机通讯的内容 std::string charge_tile_name; //充电桩唯一标识符（考虑当前是否需要） CHARGE_POSITION_STATE position_state; //检测到二维码位置时进行进行自动充电模式反馈到手机 CHARGE_SWITCH_STATE charge_switch_state; //上桩过程状态反馈到手机 bool charge_succeeded; //电池与充电桩连接成功正在充电标志位该标志位需要BMS与充电桩通信确认 ROBOT_BATTERY_INFO battery_info; bool user_quit_charge_flag; }ROBOT_CHARGE_STATUS; typedef struct JETSON_CHARGE_STATE_{ //回冲功能上传Jetson的信息 long heartbeat; //心跳信息 bool sensnor; //是否打开相机 Jetson 读该信息 }JETSON_CHARGE_STATE; typedef struct JETSON_CHARGE_COMMAND_{ //Jetson主机下发指令到运动主机的信息 bool tag_state; //检测到二维码 Jetson 写该信息 MC_QUATERNIONS position; //充电桩位置信息 Jetson 写该位置 }JETSON_CHARGE_COMMAND; //Jetson回冲相关的状态统一放在这里 typedef struct JETSON_CHARGE_INFO_{ //统一管理接口 JETSON_CHARGE_STATE charge_state; //与充电相关的机器人状态 JETSON_CHARGE_COMMAND charge_command; //充电相关的位置信息 }JETSON_CHARGE_INFO; //视觉落脚点功能，从运动控制主机上传到Jetson主机所用的数据类型,实机 typedef struct JETSON_VISUAL_FOOTHOLD_STATE_{ uint64_t timestamp; robot_control::common::BODY_ODOMETRY robot_odometry; float touch_detection[LEG_NUMBER]; float tip_position_wrt_hip[LEG_NUMBER * 3]; float vf_planned_tip_position_wrt_base[LEG_NUMBER * 3]; //规划出的足端相对机器人base的位置 bool camera_switch; //是否打开相机 }JETSON_VISUAL_FOOTHOLD_STATE; //视觉落脚点功能，从Jetson主机下发数据到运动控制主机的数据类型 typedef struct JETSON_VISUAL_FOOTHOLD_COMMAND_{ uint64_t timestamp; float map_center_point[2]; float body_pos_act[3]; float body_vel_act[3]; //现在机器人身体实际的速度并没有被用到，临时借用一下作为雷达最新的数据 float body_pos_est[3]; float body_vel_est[3]; float body_orn_act[4]; float body_gyr_act[3]; float body_orn_est[4]; float body_gyr_est[3]; unsigned int neigh_height_map[HEIGHT_MAP_HEIGHT][HEIGHT_MAP_WIDTH]; unsigned int neigh_roughness_map[HEIGHT_MAP_HEIGHT][HEIGHT_MAP_WIDTH]; unsigned int neigh_smoothness_map[HEIGHT_MAP_HEIGHT][HEIGHT_MAP_WIDTH]; }JETSON_VISUAL_FOOTHOLD_COMMAND; typedef struct DEPTH_IMAGE_{ int image[DEPTH_IMAGE_HEIGHT][DEPTH_IMAGE_WIDTH]; }DEPTH_IMAGE; typedef struct RGB_IMAGE_{ int image[RGB_IMAGE_HEIGHT][RGB_IMAGE_WIDTH]; }RGB_IMAGE; //视觉落脚点相关的状态统一放在这里 typedef struct JETSON_VISUAL_FOOTHOLD_INFO_{ //统一管理接口 uint64_t timestamp; JETSON_VISUAL_FOOTHOLD_COMMAND vis_foothold_command; float touch_detection[LEG_NUMBER]; float tip_position_wrt_hip[LEG_NUMBER * 3]; //当前机器人足端相对髋关节坐标系的位置 float vf_planned_tip_position_wrt_base[LEG_NUMBER * 3]; //规划出的足端相对机器人base的位置 bool camera_switch; //是否打开相机 }JETSON_VISUAL_FOOTHOLD_INFO; typedef enum _STAIRS_HEIGH_STATE{ STAIRS_HEIGH_NULL = 0, STAIRS_LESS_THAN_16CM = 1, STAIRS_LESS_THAN_20CM = 2, STAIRS_LESS_THAN_30CM = 3, }STAIRS_HEIGH_STATE; typedef struct ROBOT_DRIVER_VALUE_{ double position; double velocity; double torque; double torque_sensor; }ROBOT_DRIVER_VALUE; typedef enum NAV_OR_JOY_MODE_{ nall_control = 0, nav_control = 1, joy_control = 2 }NAV_OR_JOY_MODE; typedef enum LIE_DOWN_ADJUST_{ null = 0, start_to_adjust = 1, adjusting = 2, nimble_stop = 3, finish = 4 }LIE_DOWN_ADJUST_; //停障功能从navigation端接收到的结果 //在目前的使用中，当float<=-2，代表存在障碍物，否则代表不存在障碍物 typedef struct GUARDIAN_VELOCITY_DECAY_RATIO_{ float left_ratio; float front_ratio; float right_ratio; float rear_ratio; }GUARDIAN_VELOCITY_DECAY_RATIO; typedef enum CALIBRATE_POSE_STATE_{ NULL_STATE = 0, START_CALIBRATE = 1, CALIBRATE_SUCCESS = 2, CALIBRATE_FAILED = 3 }CALIBRATE_POSE_STATE; typedef enum TERRAIN_SEG_DETECTION_RESULTS_{ NULL_TERRAIN = 0, WEI_ZHI = 1, CAO_DI = 2, CI_ZHUAN = 3, DI_BAN = 4, DI_TAN = 5, LI_QING_LU_MIAN = 6, LOU_TI = 7, LUAN_SHI_DI_MIAN = 8, LU_ZHUAN = 9, SHUI_NI_DI = 10, TAI_JIE = 11 }TERRAIN_SEG_DETECTION_RESULTS; typedef struct ROBOT_COMMON_STATUS_{ int network; //机器人当前网络状态 BODY_POSTURE_STATE belie_or_stand; //机器人处于站立趴下状态(目前不用，芳勇认为测试中可能会用) bool emergency; //是否是处于紧急状态 bool avoidance; //自动避障状态 long int heartbeat; //心跳 float cur_speed; //当前速度 float cur_height; //当前高度 MC_QUATERNIONS position; //机器人世界坐标系下的位置，xyz与yaw角 float max_height; //机器人最大高度 float max_speed; //机器人最大速度 SCENE_SWITCH_STATE cur_scene_switch_state; //切换场景是否成功 SCENE_TYPE cur_scene; //当前处于的场景,预设站立预设应该是趴下比较合理 DRIVER_ENABLE_STATE driver_enable_state; //机器人里程计信息 BODY_ODOMETRY odometry; float tip_position_wrt_hip[LEG_NUMBER*3];//足端相对髋关节坐标系位置 float vf_planned_tip_position_wrt_base[LEG_NUMBER * 3]; float touch_detection[LEG_NUMBER];//触地检测，范围从0到1.0, 等于1.0为绝对触地，后期会变为概率，但目前还是只有1或者0两种可能 float target_body_orientation[3]; //raw pitch yaw //错误代码 int error_code; bool has_error; unsigned int error_wheel[CODE_WHEEL_NUM]; unsigned int warn_wheel[CODE_WHEEL_NUM]; unsigned int note_wheel[CODE_WHEEL_NUM]; unsigned int joint_error_wheel[JOINT_CODE_WHEEL_NUM]; unsigned int joint_warn_wheel[JOINT_CODE_WHEEL_NUM]; unsigned int joint_note_wheel[JOINT_CODE_WHEEL_NUM]; ROBOT_CHARGE_STATUS charge_status; bool salute_state; NAV_OR_JOY_MODE joy_mode; EQA_STATE eqa_state; bool guardian_switch; //表示当前停障功能的开关的状态 bool visual_foothold_switch; //表示当前视觉落脚点功能开关的状态 GUARDIAN_VELOCITY_DECAY_RATIO guardian_velocity_decay_ratio; //军营用于显示前左右三个方位的障碍 LIE_DOWN_ADJUST_ lie_down_adjust; //用于判断趴下前是否正在做足端位置调整 float speed_bar_on_controller; //机器人遥控器上当前选中的线速度 CALIBRATE_POSE_STATE calibrate_pos_state; int self_test_code; int encoder_calibrate_state; bool nav_pos_state;//当前导航定位位置是否正常 bool nav_init_pos;//是否尝试初始化导航定位 TERRAIN_SEG_DETECTION_RESULTS terrain_seg_result; bool uwb_state; STAIRS_HEIGH_STATE stairs_state; bool weight_load_switch; float cur_weight_load; bool exceeding_weight_load; }ROBOT_COMMON_STATUS; typedef enum JOINT_LIMIT_STATE_{ NULL_LIMIT = 0, LIMITED = 1, NOLIMITED = 2 }JOINT_LIMIT_STATE; //18个关节的关节状态 typedef struct ROBOT_JOINT_STATUS_{ std::string name; float current;//电机数据（电流值） float joint_temperature;//驱动器温度 float motor_temperature;//电机温度 float driver_cpu_temperature;//驱动器cpu温度 float torque_sensor_data;//扭矩传感器数据 double position; double velocity; double effort; double max_limit; double min_limit; JOINT_LIMIT_STATE limit_state; unsigned short driver_error_code; unsigned short motor_error_code; double target_position; double target_velocity; double target_effort; }ROBOT_JOINT_STATUS; typedef struct ROBOT_JOINTS_STATUS_{ ROBOT_JOINT_STATUS joints[MOTOR_NUM]; long int heartbeat; }ROBOT_JOINTS_STATUS; typedef struct ROBOT_PER_CYCLE_STATUS_{ int network;//机器人当前网络状态 bool lie_stand_state; //joystick连接状态 bool emergency; //是否是处于紧急状态 bool avoidance; //自动避障状态 long int heartbeat; //心跳 float speed; MC_QUATERNIONS position; }ROBOT_PER_CYCLE_STATUS; typedef struct ROBOT_TARGET_VALUE_{ ROBOT_DRIVER_VALUE driver[MOTOR_NUM]; long int heartbeat; }ROBOT_TARGET_VALUE; typedef enum enFormOfMovement_ { JOINTS_CONTROL = 0, MOTION_CONTROL = 1 }enFormOfMovement; typedef struct DRIVER_CONFIG_PARAMETER_{ std::string motor_name; double position_scale; double velocity_scale; double torque_scale; bool motor_direction; bool direction_correction; int index_map; bool be_choosed; double init_pos_offset; double init_pos_offset_rough; double position_min_limit; double position_min_safety_limit; double position_max_limit; double position_max_safety_limit; double min_limit_kp; double min_limit_ki; double min_limit_kd; double max_limit_kp; double max_limit_ki; double max_limit_kd; int convert_to_deceleration_end; int one_turn_encorder_value; double driver_kp_scale; double driver_kd_scale; double trq_safety_limit; double target_torque; bool init_pos_check_switch; double init_position; double init_position_error; }DRIVER_CONFIG_PARAMETER; typedef struct SENSOR_CONFIG_PARAMETER_{ double trq_sensor_scale; double trq_sensor_offset; bool trq_sensor_direction; int index_map; }SENSOR_CONFIG_PARAMETER; typedef struct _JOINTS_CONFIG_INFO{ robot_control::common::DRIVER_CONFIG_PARAMETER driver_data[MOTOR_NUM]; robot_control::common::SENSOR_CONFIG_PARAMETER sensor_data[THE_TRQ_SENSOR_NUM]; bool trq_have_been_calibrated; bool joint_position_have_been_calibrated; bool encoder_has_been_reseted; }JOINTS_CONFIG_INFO; typedef struct _ROBOT_SPECIAL_CONFIG_INFO{ std::string bms_address; std::string light_address; EN_DRIVER_ECAT_SLAVES_TYPE driver_type; EN_TRQ_SENSOR_ECAT_SLAVES_TYPE trq_sensor_type; EN_BMS_TYPE bms_type; EN_EMOJI_LIGHT_TYPE emoji_light_type; EN_LAMP_EFFECT_THEME lamp_effect_theme; }ROBOT_SPECIAL_CONFIG_INFO; typedef enum _ROBOT_WORKING_ENVIRINMENT{ ROBOT_BODY_NON = 0, ROBOT_BODY_REAL, ROBOT_BODY_WEBOTS, ROBOT_BODY_GAZEBO }ROBOT_WORKING_ENVIRINMENT; typedef enum LOG_LEVEL_{ LOG_LEVEL_NON = 0, LOG_LEVEL_DEBUG, LOG_LEVEL_NOTE, LOG_LEVEL_WARNING, LOG_LEVEL_ERROR, LOG_LEVEL_ALL, LOG_LEVEL_MAX }LOG_LEVEL; typedef enum LOG_MODULE_NAME_{ MODULE_NON = 0, MODULE_CONTROL_CORE, MODULE_ENTRY, LOG_MODULE_MAX }LOG_MODULE_NAME; typedef struct ROBOT_POSE_ESTIMATE_{ MC_COORDINATE point; MC_QUATERNIONS quaternion; }ROBOT_POSE_ESTIMATE; typedef struct BMS_STATE_INFO_{ float sigle_overvoltage; float remaining_capacity; float battery_voltage_1th; float battery_voltage_2th; float battery_voltage_3th; float battery_voltage_4th; float battery_voltage_5th; float battery_voltage_6th; float total_voltage; float discharging_current; float remain_battery_capacity; float reconfigurable_system_on_chip; float battery_power; bool is_charging; }BMS_STATE_INFO; //Log config } // namespace common } #endif