315 lines
12 KiB
C++
315 lines
12 KiB
C++
// KinematicsHelper.cpp
|
||
#include "QuadrupedRobotSimulation/KinematicsHelper.h"
|
||
#include "QuadrupedRobotSimulation/RobotConfig.hpp"
|
||
#include <iostream>
|
||
#include <sstream>
|
||
#include <iomanip>
|
||
|
||
// 声明网络发送函数（需要根据实际网络库实现）
|
||
extern void SendMsg(const std::string &msg);
|
||
static std::unordered_map<std::string, std::shared_ptr<RobotGaitDataManager>> instanceMap;
|
||
|
||
void KinematicsHelper::SimRobot()
|
||
{
|
||
double thigh_angle_deg = 0;
|
||
double shank_angle_deg = 0;
|
||
double ankle_angle_deg = 0;
|
||
std::string leg_type = "LF";
|
||
|
||
auto manager = KinematicsHelper::RobotGaitDataManagerFromJson();
|
||
auto simulation = std::make_shared<ReverseKinematicsCalculator>(manager);
|
||
|
||
nlohmann::json jsonData = KinematicsHelper::QuadrupedRobot_PerformForwardKinematics();
|
||
|
||
try
|
||
{
|
||
// 使用不同的变量名避免冲突
|
||
CompleteExportData exportData = jsonData.get<CompleteExportData>();
|
||
|
||
int pointCount = exportData.MotorData.Legs.at("LH").Frames.size();
|
||
|
||
for (int i = 0; i < pointCount; i++)
|
||
{
|
||
// 左前腿
|
||
leg_type = "LF";
|
||
thigh_angle_deg = exportData.MotorData.Legs.at(leg_type).Frames[i].Thigh.Angle;
|
||
shank_angle_deg = exportData.MotorData.Legs.at(leg_type).Frames[i].Shank.Angle;
|
||
ankle_angle_deg = exportData.MotorData.Legs.at(leg_type).Frames[i].Ankle.Angle;
|
||
|
||
std::string c_Frames = KinematicsHelper::QuadrupedRobot_CalculateAllPointsOnlyOneLegFromMotorAngles(
|
||
thigh_angle_deg, shank_angle_deg, ankle_angle_deg, leg_type);
|
||
|
||
// 发送消息（需要实现SendMsg函数）
|
||
// SendMsg(PP.Prefix + PP.ObjectHeader + Cmd_Protocol.Frames + c_Frames);
|
||
|
||
// 左后腿
|
||
leg_type = "LH";
|
||
thigh_angle_deg = exportData.MotorData.Legs.at(leg_type).Frames[i].Thigh.Angle;
|
||
shank_angle_deg = exportData.MotorData.Legs.at(leg_type).Frames[i].Shank.Angle;
|
||
ankle_angle_deg = exportData.MotorData.Legs.at(leg_type).Frames[i].Ankle.Angle;
|
||
|
||
c_Frames = KinematicsHelper::QuadrupedRobot_CalculateAllPointsOnlyOneLegFromMotorAngles(
|
||
thigh_angle_deg, shank_angle_deg, ankle_angle_deg, leg_type);
|
||
// SendMsg(PP.Prefix + PP.ObjectHeader + Cmd_Protocol.Frames + c_Frames);
|
||
|
||
// 右前腿
|
||
leg_type = "RF";
|
||
thigh_angle_deg = exportData.MotorData.Legs.at(leg_type).Frames[i].Thigh.Angle;
|
||
shank_angle_deg = exportData.MotorData.Legs.at(leg_type).Frames[i].Shank.Angle;
|
||
ankle_angle_deg = exportData.MotorData.Legs.at(leg_type).Frames[i].Ankle.Angle;
|
||
|
||
c_Frames = KinematicsHelper::QuadrupedRobot_CalculateAllPointsOnlyOneLegFromMotorAngles(
|
||
thigh_angle_deg, shank_angle_deg, ankle_angle_deg, leg_type);
|
||
// SendMsg(PP.Prefix + PP.ObjectHeader + Cmd_Protocol.Frames + c_Frames);
|
||
|
||
// 右后腿
|
||
leg_type = "RH";
|
||
thigh_angle_deg = exportData.MotorData.Legs.at(leg_type).Frames[i].Thigh.Angle;
|
||
shank_angle_deg = exportData.MotorData.Legs.at(leg_type).Frames[i].Shank.Angle;
|
||
ankle_angle_deg = exportData.MotorData.Legs.at(leg_type).Frames[i].Ankle.Angle;
|
||
|
||
c_Frames = KinematicsHelper::QuadrupedRobot_CalculateAllPointsOnlyOneLegFromMotorAngles(
|
||
thigh_angle_deg, shank_angle_deg, ankle_angle_deg, leg_type);
|
||
// SendMsg(PP.Prefix + PP.ObjectHeader + Cmd_Protocol.Frames + c_Frames);
|
||
|
||
// 等待200毫秒
|
||
Wait(200);
|
||
}
|
||
}
|
||
catch (const std::exception &e)
|
||
{
|
||
std::cerr << "Error in SimRobot: " << e.what() << std::endl;
|
||
}
|
||
}
|
||
|
||
json KinematicsHelper::QuadrupedRobot_CalculateAllPointsFromMotorAngles(const std::string &jsonInput)
|
||
{
|
||
std::string jsonStr = "{}";
|
||
auto manager = RobotGaitDataManagerFromJson(jsonInput);
|
||
ReverseKinematicsCalculator simulation(manager);
|
||
jsonStr = simulation.CalculateAllPointsFromMotorAnglesJsonStr();
|
||
json jsonObj = json::parse(jsonStr);
|
||
return jsonObj;
|
||
}
|
||
json KinematicsHelper::QuadrupedRobot_PerformForwardKinematics(const std::string &jsonInput)
|
||
{
|
||
auto manager = RobotGaitDataManagerFromJson(jsonInput);
|
||
QuadrupedRobotConfiguration config(manager->GetGaitInfo(), manager->GetSystemParameters());
|
||
QuadrupedRobotSimulation simulation(config);
|
||
std::string jsonStr = simulation.CalculateAllTrajectoriesJsonString();
|
||
json jsonObj = json::parse(jsonStr);
|
||
return jsonObj;
|
||
}
|
||
std::string KinematicsHelper::QuadrupedRobot_CalculateAllPointsOnlyOneLegFromMotorAngles(
|
||
double thigh_angle_deg, double shank_angle_deg, double ankle_angle_deg,
|
||
const std::string &leg_type, const std::string &jsonInput)
|
||
{
|
||
auto manager = RobotGaitDataManagerFromJson(jsonInput);
|
||
ReverseKinematicsCalculator simulation(manager);
|
||
std::string c_Frames_jsonStr = simulation.CalculateAllPointsOnlyOneLegFromMotorAnglesJsonStr(
|
||
thigh_angle_deg, shank_angle_deg, ankle_angle_deg, leg_type);
|
||
return c_Frames_jsonStr;
|
||
}
|
||
|
||
std::shared_ptr<RobotGaitDataManager> KinematicsHelper::RobotGaitDataManagerFromJson(const std::string &jsonInput, const std::string &robotID)
|
||
{
|
||
|
||
std::string RobotID;
|
||
RobotID = robotID;
|
||
// 尝试解析JSON
|
||
nlohmann::json j;
|
||
j = nlohmann::json::parse(jsonInput);
|
||
|
||
if (j.contains("RobotID"))
|
||
{
|
||
RobotID = j.at("RobotID").get<std::string>();
|
||
}
|
||
auto it = instanceMap.find(RobotID);
|
||
if (it != instanceMap.end())
|
||
{
|
||
// 已存在：重新加载默认数据
|
||
auto manager = it->second;
|
||
if (jsonInput.empty())
|
||
{
|
||
manager->LoadDefaultData();
|
||
}
|
||
else
|
||
{
|
||
manager->LoadFromJson(j);
|
||
}
|
||
return manager;
|
||
}
|
||
else
|
||
{
|
||
|
||
// 不存在：创建新的并加载默认数据
|
||
auto manager = std::make_shared<RobotGaitDataManager>();
|
||
|
||
manager->LoadDefaultData();
|
||
|
||
manager->LoadFromJson(jsonInput);
|
||
|
||
instanceMap[robotID] = manager;
|
||
return manager;
|
||
}
|
||
auto manager = std::make_shared<RobotGaitDataManager>();
|
||
|
||
return manager;
|
||
}
|
||
|
||
QuadrupedRobotConfiguration KinematicsHelper::CreateConfigFromJson(const std::string &jsonInput)
|
||
{
|
||
auto manager = RobotGaitDataManagerFromJson(jsonInput);
|
||
return QuadrupedRobotConfiguration(manager->GetGaitInfo(), manager->GetSystemParameters());
|
||
}
|
||
|
||
std::string KinematicsHelper::PerformForwardKinematics(const QuadrupedRobotConfiguration &config)
|
||
{
|
||
QuadrupedRobotSimulation simulation(config);
|
||
std::string jsonStr = simulation.CalculateAllTrajectories_JsonStr();
|
||
return jsonStr;
|
||
}
|
||
|
||
std::string KinematicsHelper::ExportCompleteData(const QuadrupedRobotConfiguration &config)
|
||
{
|
||
QuadrupedRobotSimulation simulation(config);
|
||
return CompleteJsonExporter::ExportCompleteDataJsonString(simulation);
|
||
}
|
||
|
||
std::vector<ReverseCalculationResult> KinematicsHelper::BatchReverseCalculation(
|
||
const std::map<std::string, std::vector<double>> &motor_data,
|
||
const std::string &leg_type,
|
||
std::optional<int> max_frames,
|
||
const std::string &jsonInput)
|
||
{
|
||
auto manager = RobotGaitDataManagerFromJson(jsonInput);
|
||
ReverseKinematicsCalculator calculator(manager);
|
||
return calculator.BatchReverseCalculation(motor_data, leg_type, max_frames);
|
||
}
|
||
|
||
SupportCheckResult KinematicsHelper::CheckSupportPoint(
|
||
const std::map<std::string, std::vector<double>> &pointsDict,
|
||
double groundHeight,
|
||
double tolerance,
|
||
const std::string &jsonInput)
|
||
{
|
||
auto manager = RobotGaitDataManagerFromJson(jsonInput);
|
||
ReverseKinematicsCalculator calculator(manager);
|
||
|
||
// 注意：原C#代码中IsSupportPoint返回bool，但BaseClass.h中有SupportCheckResult结构
|
||
// 这里需要根据实际实现调整
|
||
bool isSupport = calculator.IsSupportPoint(pointsDict, groundHeight, tolerance);
|
||
|
||
SupportCheckResult result;
|
||
result.IsSupport = isSupport;
|
||
// 这里可以添加更多的检查结果填充逻辑
|
||
|
||
return result;
|
||
}
|
||
|
||
P_OPERATION KinematicsHelper::MakeOperation(
|
||
int frameRate,
|
||
const std::string &modelCode,
|
||
const std::vector<std::vector<double>> &positions,
|
||
const std::vector<std::vector<double>> &quaternions)
|
||
{
|
||
return P_OPERATION_Func::MakeOperation(frameRate, modelCode, positions, quaternions);
|
||
}
|
||
|
||
RobotGaitRequest KinematicsHelper::CreateDefaultRequest()
|
||
{
|
||
RobotGaitRequest request;
|
||
|
||
// 设置默认步态信息
|
||
request.req_param.GaitInfo.GaitType = "walk";
|
||
request.req_param.GaitInfo.Period = 2.0;
|
||
request.req_param.GaitInfo.Frequency = 50.0;
|
||
request.req_param.GaitInfo.StepTime = 0.02;
|
||
request.req_param.GaitInfo.SupportTime = 1.0;
|
||
request.req_param.GaitInfo.SwingTime = 1.0;
|
||
request.req_param.GaitInfo.SupportRatio = 0.5;
|
||
request.req_param.GaitInfo.SwingRatio = 0.5;
|
||
request.req_param.GaitInfo.TotalFrames = 100;
|
||
request.req_param.GaitInfo.TotalTime = 2.0;
|
||
|
||
// 设置默认系统参数
|
||
request.req_param.SystemParameters.A_x = 0.0;
|
||
request.req_param.SystemParameters.A_y = 0.0;
|
||
request.req_param.SystemParameters.L10 = 100.0;
|
||
request.req_param.SystemParameters.L20 = 100.0;
|
||
request.req_param.SystemParameters.L30 = 50.0;
|
||
request.req_param.SystemParameters.StepLength = 200.0;
|
||
request.req_param.SystemParameters.StepHeight = 50.0;
|
||
request.req_param.SystemParameters.X = 50.0;
|
||
request.req_param.SystemParameters.DeltaX = 0.0;
|
||
request.req_param.SystemParameters.L21 = 80.0;
|
||
request.req_param.SystemParameters.L22 = 60.0;
|
||
request.req_param.SystemParameters.L23 = 70.0;
|
||
request.req_param.SystemParameters.Beta1 = 30.0;
|
||
request.req_param.SystemParameters.BB2_BC_Angle = 30.0;
|
||
request.req_param.SystemParameters.C1_B_Length = 50.0;
|
||
request.req_param.SystemParameters.CC1 = 40.0;
|
||
request.req_param.SystemParameters.L31 = 60.0;
|
||
request.req_param.SystemParameters.C2C3_Length = 50.0;
|
||
request.req_param.SystemParameters.Lead = 5.0;
|
||
request.req_param.SystemParameters.D1_L_Offset = 20.0;
|
||
request.req_param.SystemParameters.D2_L_Offset = 20.0;
|
||
request.req_param.SystemParameters.C4_D2_Offset = 15.0;
|
||
request.req_param.SystemParameters.ThighMotorReduction = 10.0;
|
||
request.req_param.SystemParameters.ShankMotorReduction = 10.0;
|
||
request.req_param.SystemParameters.AnkleMotorReduction = 10.0;
|
||
|
||
// 设置默认机器人身体位置
|
||
request.req_param.RobotBody.BodyCode = "Body";
|
||
request.req_param.RobotBody.x = 0.0;
|
||
request.req_param.RobotBody.y = 0.0;
|
||
request.req_param.RobotBody.z = 300.0;
|
||
request.req_param.RobotBody.qx = 0.0;
|
||
request.req_param.RobotBody.qy = 0.0;
|
||
request.req_param.RobotBody.qz = 0.0;
|
||
request.req_param.RobotBody.qw = 1.0;
|
||
|
||
// 设置默认模型ID
|
||
// 这里可以根据需要添加默认模型ID
|
||
|
||
// 设置默认参数
|
||
request.req_param.Param.LF.thigh_angle_deg = -49.1033472630546;
|
||
request.req_param.Param.LF.shank_angle_deg = 7.50678016014155;
|
||
request.req_param.Param.LF.ankle_angle_deg = -183.115048990549;
|
||
|
||
request.req_param.Param.LH.thigh_angle_deg = -38.4042396745178;
|
||
request.req_param.Param.LH.shank_angle_deg = -9.70116099369837;
|
||
request.req_param.Param.LH.ankle_angle_deg = 107.000051175049;
|
||
|
||
request.req_param.Param.RF.thigh_angle_deg = 38.4042396745178;
|
||
request.req_param.Param.RF.shank_angle_deg = 9.70116099369837;
|
||
request.req_param.Param.RF.ankle_angle_deg = 107.000051175049;
|
||
|
||
request.req_param.Param.RH.thigh_angle_deg = 49.1033472630546;
|
||
request.req_param.Param.RH.shank_angle_deg = -7.50678016014155;
|
||
request.req_param.Param.RH.ankle_angle_deg = -183.115048990549;
|
||
|
||
return request;
|
||
}
|
||
|
||
RobotGaitRequest KinematicsHelper::LoadAndValidateJson(const std::string &jsonInput)
|
||
{
|
||
if (jsonInput.empty())
|
||
{
|
||
return CreateDefaultRequest();
|
||
}
|
||
|
||
try
|
||
{
|
||
|
||
RobotGaitRequest request;
|
||
request.req_param = RequestParameters::fromJsonString(jsonInput);
|
||
return request;
|
||
}
|
||
catch (const std::exception &e)
|
||
{
|
||
std::cerr << "Error parsing JSON: " << e.what() << std::endl;
|
||
return CreateDefaultRequest();
|
||
}
|
||
}
|