Initial commit
This commit is contained in:
zhangshun
320
src/QuadrupedRobotSimulation/KinematicsHelper.cpp
Normal file
320
src/QuadrupedRobotSimulation/KinematicsHelper.cpp
Normal file
@@ -0,0 +1,320 @@
|
||||
// KinematicsHelper.cpp
|
||||
#include "KinematicsHelper.h"
|
||||
#include "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::cout << "[DEBUG] json KinematicsHelper::QuadrupedRobot_CalculateAllPointsFromMotorAngles(const std::string &jsonInput) " << std::endl;
|
||||
|
||||
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)
|
||||
{
|
||||
|
||||
std::cout << "[DEBUG] json KinematicsHelper::QuadrupedRobot_PerformForwardKinematics(const std::string &jsonInput) " << std::endl;
|
||||
|
||||
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();
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user