145 lines
4.9 KiB
C++
145 lines
4.9 KiB
C++
// KinematicsHelper.h
|
||
#ifndef KINEMATICS_HELPER_H
|
||
#define KINEMATICS_HELPER_H
|
||
|
||
#include "QuadrupedRobotSimulation/BaseClass.h"
|
||
#include "QuadrupedRobotSimulation/KinematicsSimulation.h"
|
||
#include "QuadrupedRobotSimulation/KinematicsReverse.h"
|
||
#include "QuadrupedRobotSimulation/CompleteJsonExporter.h"
|
||
#include <string>
|
||
#include <memory>
|
||
#include <chrono>
|
||
#include <thread>
|
||
|
||
/**
|
||
* @brief 工具类：提供便捷的正逆解计算方法
|
||
*/
|
||
class KinematicsHelper
|
||
{
|
||
public:
|
||
/**
|
||
* @brief 模拟机器人完整运动
|
||
*/
|
||
static void SimRobot();
|
||
|
||
/**
|
||
* @brief 输入三个电机的值，返回各个连杆的姿态
|
||
* @param jsonInput JSON输入字符串（可选）
|
||
* @return JSON格式的所有点计算结果
|
||
*/
|
||
static json QuadrupedRobot_CalculateAllPointsFromMotorAngles(const std::string &jsonInput = "");
|
||
/**
|
||
* @brief 执行正向运动学计算：计算所有轨迹
|
||
* @param jsonInput JSON输入字符串（可选）
|
||
* @return JSON格式的正向运动学计算结果
|
||
*/
|
||
static json QuadrupedRobot_PerformForwardKinematics(const std::string &jsonInput = "");
|
||
/**
|
||
* @brief 单腿运动计算
|
||
* @param thigh_angle_deg 大腿电机角度（度）
|
||
* @param shank_angle_deg 小腿电机角度（度）
|
||
* @param ankle_angle_deg 脚踝电机角度（度）
|
||
* @param leg_type 腿类型："LF"、"LH"、"RF"、"RH"
|
||
* @param json JSON输入字符串（可选）
|
||
* @return JSON格式的单腿计算结果
|
||
*/
|
||
static std::string QuadrupedRobot_CalculateAllPointsOnlyOneLegFromMotorAngles(
|
||
double thigh_angle_deg, double shank_angle_deg, double ankle_angle_deg,
|
||
const std::string &leg_type = "LF", const std::string &json = "");
|
||
|
||
/**
|
||
* @brief 获得RobotGaitDataManager机器人管理句柄
|
||
* @param json JSON输入字符串（可选）
|
||
* @return 共享指针指向RobotGaitDataManager对象
|
||
*/
|
||
static std::shared_ptr<RobotGaitDataManager> RobotGaitDataManagerFromJson(const std::string &jsonInput = "", const std::string &robotID = "0");
|
||
|
||
/**
|
||
* @brief 从JSON字符串创建配置对象
|
||
* @param jsonInput JSON输入字符串（可选）
|
||
* @return QuadrupedRobotConfiguration对象
|
||
*/
|
||
static QuadrupedRobotConfiguration CreateConfigFromJson(const std::string &jsonInput = "");
|
||
|
||
/**
|
||
* @brief 根据机器人的机构数据和机构类型，计算机器人四个腿的所有轨迹数据
|
||
* @param config 机器人配置
|
||
* @return JSON格式的正向运动学计算结果
|
||
*/
|
||
static std::string PerformForwardKinematics(const QuadrupedRobotConfiguration &config);
|
||
|
||
/**
|
||
* @brief 导出完整数据为JSON字符串
|
||
* @param config 机器人配置
|
||
* @return JSON格式的完整导出数据
|
||
*/
|
||
static std::string ExportCompleteData(const QuadrupedRobotConfiguration &config);
|
||
|
||
/**
|
||
* @brief 批量逆向计算
|
||
* @param motor_data 电机数据映射
|
||
* @param leg_type 腿类型
|
||
* @param max_frames 最大帧数（可选）
|
||
* @param jsonInput 配置JSON（可选）
|
||
* @return 逆向计算结果向量
|
||
*/
|
||
static std::vector<ReverseCalculationResult> BatchReverseCalculation(
|
||
const std::map<std::string, std::vector<double>> &motor_data,
|
||
const std::string &leg_type = "LF",
|
||
std::optional<int> max_frames = std::nullopt,
|
||
const std::string &jsonInput = "");
|
||
|
||
/**
|
||
* @brief 计算并验证支撑点
|
||
* @param pointsDict 点字典
|
||
* @param groundHeight 地面高度
|
||
* @param tolerance 容差
|
||
* @param json 配置JSON（可选）
|
||
* @return 支撑点检查结果
|
||
*/
|
||
static SupportCheckResult CheckSupportPoint(
|
||
const std::map<std::string, std::vector<double>> &pointsDict,
|
||
double groundHeight = 0.0,
|
||
double tolerance = 1.0,
|
||
const std::string &json = "");
|
||
|
||
/**
|
||
* @brief 生成线框播放数据
|
||
* @param frameRate 帧率
|
||
* @param modelCode 模型代码
|
||
* @param positions 位置数组
|
||
* @param quaternions 四元数数组
|
||
* @return P_OPERATION对象
|
||
*/
|
||
static P_OPERATION MakeOperation(
|
||
int frameRate,
|
||
const std::string &modelCode,
|
||
const std::vector<std::vector<double>> &positions,
|
||
const std::vector<std::vector<double>> &quaternions);
|
||
|
||
/**
|
||
* @brief 等待函数（用于模拟延迟）
|
||
* @param milliseconds 等待的毫秒数
|
||
*/
|
||
static void Wait(int milliseconds)
|
||
{
|
||
std::this_thread::sleep_for(std::chrono::milliseconds(milliseconds));
|
||
}
|
||
|
||
private:
|
||
/**
|
||
* @brief 从JSON创建默认配置
|
||
* @return 默认机器人步态请求
|
||
*/
|
||
static RobotGaitRequest CreateDefaultRequest();
|
||
|
||
/**
|
||
* @brief 加载并验证JSON数据
|
||
* @param json JSON字符串
|
||
* @return 验证后的RobotGaitRequest对象
|
||
*/
|
||
static RobotGaitRequest LoadAndValidateJson(const std::string &jsonInput);
|
||
};
|
||
|
||
#endif // KINEMATICS_HELPER_H
|