375 lines
11 KiB
C++
375 lines
11 KiB
C++
// SliderCrankMechanism.cpp
|
||
#include "FourBarMechanism/SliderCrankMechanism.h"
|
||
#include <stdexcept>
|
||
#include <algorithm>
|
||
#include <cstdio>
|
||
|
||
// ==================== MechanismParametersExt 实现 ====================
|
||
|
||
MechanismParametersExt::MechanismParametersExt() : CrankLength(1.0),
|
||
ConnectingRodLength(3.0),
|
||
SliderOffset(0.5),
|
||
InputValue(0.0),
|
||
AngularVelocity(1.0),
|
||
SolutionMode(SolutionMode::Auto),
|
||
CrankAngleRange1(0.0, 0.0),
|
||
CrankAngleRange2(0.0, 0.0) {}
|
||
|
||
// ==================== SliderCrankMechanism 实现 ====================
|
||
|
||
SliderCrankMechanism::SliderCrankMechanism() : name("曲柄滑块机构 (逆解)")
|
||
{
|
||
parameters.CrankLength = 1.0;
|
||
parameters.ConnectingRodLength = 3.0;
|
||
parameters.SliderOffset = 0.5;
|
||
parameters.InputValue = 0.0;
|
||
parameters.AngularVelocity = 1.0;
|
||
parameters.SolutionMode = SolutionMode::Auto;
|
||
}
|
||
|
||
// 辅助方法
|
||
double SliderCrankMechanism::degreesToRadians(double degrees) const
|
||
{
|
||
return degrees * M_PI / 180.0;
|
||
}
|
||
|
||
double SliderCrankMechanism::radiansToDegrees(double radians) const
|
||
{
|
||
return radians * 180.0 / M_PI;
|
||
}
|
||
|
||
std::string SliderCrankMechanism::formatDouble(double value, int precision) const
|
||
{
|
||
char buffer[32];
|
||
snprintf(buffer, sizeof(buffer), "%.*f", precision, value);
|
||
return std::string(buffer);
|
||
}
|
||
|
||
double SliderCrankMechanism::clamp(double value, double min, double max) const
|
||
{
|
||
if (value < min)
|
||
return min;
|
||
if (value > max)
|
||
return max;
|
||
return value;
|
||
}
|
||
|
||
// 基本属性访问
|
||
std::string SliderCrankMechanism::getName() const
|
||
{
|
||
return name;
|
||
}
|
||
|
||
MechanismType SliderCrankMechanism::getType() const
|
||
{
|
||
return MechanismType::CrankSlider; // 使用相同的机构类型
|
||
}
|
||
|
||
MechanismParametersExt SliderCrankMechanism::getParameters() const
|
||
{
|
||
return parameters;
|
||
}
|
||
|
||
void SliderCrankMechanism::setName(const std::string &newName)
|
||
{
|
||
name = newName;
|
||
}
|
||
|
||
void SliderCrankMechanism::setParameters(const MechanismParametersExt ¶ms)
|
||
{
|
||
parameters = params;
|
||
}
|
||
|
||
void SliderCrankMechanism::setLink(double crankLength, double rodLength, double sliderOffset)
|
||
{
|
||
parameters.CrankLength = crankLength;
|
||
parameters.ConnectingRodLength = rodLength;
|
||
parameters.SliderOffset = sliderOffset;
|
||
}
|
||
|
||
void SliderCrankMechanism::setSolutionMode(SolutionMode mode)
|
||
{
|
||
parameters.SolutionMode = mode;
|
||
}
|
||
|
||
// 计算滑块范围
|
||
std::pair<double, double> SliderCrankMechanism::calculateSliderRange()
|
||
{
|
||
double L1 = parameters.CrankLength;
|
||
double L2 = parameters.ConnectingRodLength;
|
||
double h = parameters.SliderOffset;
|
||
|
||
// 检查Grashof条件
|
||
if ((L1 + L2) < std::abs(h))
|
||
return std::make_pair(0.0, 0.0);
|
||
|
||
double maxX = 0.0;
|
||
if ((L1 + L2) * (L1 + L2) > h * h)
|
||
{
|
||
maxX = std::sqrt((L1 + L2) * (L1 + L2) - h * h);
|
||
}
|
||
|
||
double minX = 0.0;
|
||
if ((L2 - L1) * (L2 - L1) > h * h)
|
||
{
|
||
minX = std::sqrt((L2 - L1) * (L2 - L1) - h * h);
|
||
}
|
||
else
|
||
{
|
||
minX = -std::sqrt((L1 + L2) * (L1 + L2) - h * h);
|
||
}
|
||
|
||
// 计算曲柄角度范围
|
||
if ((L1 + L2) * (L1 + L2) > h * h)
|
||
{
|
||
double theta_max = M_PI - std::asin(h / (L1 + L2));
|
||
double theta_min;
|
||
|
||
if ((L2 - L1) * (L2 - L1) > h * h)
|
||
{
|
||
theta_min = std::asin(h / (L1 + L2));
|
||
}
|
||
else
|
||
{
|
||
theta_min = -M_PI + std::asin(h / (L1 + L2));
|
||
}
|
||
|
||
parameters.CrankAngleRange1 = std::make_pair(theta_min, theta_max);
|
||
parameters.CrankAngleRange2 = std::make_pair(-theta_max, -theta_min);
|
||
}
|
||
|
||
return std::make_pair(minX, maxX);
|
||
}
|
||
|
||
// 主要计算函数（逆解）
|
||
MechanismState SliderCrankMechanism::calculate(double sliderX)
|
||
{
|
||
parameters.InputValue = sliderX;
|
||
|
||
double crankLength = parameters.CrankLength;
|
||
double rodLength = parameters.ConnectingRodLength;
|
||
double sliderOffset = parameters.SliderOffset;
|
||
auto solutionMode = parameters.SolutionMode;
|
||
|
||
Vector2D A(0.0, 0.0);
|
||
Vector2D S(sliderX, sliderOffset);
|
||
|
||
MechanismState state;
|
||
state.InputValue = sliderX;
|
||
|
||
try
|
||
{
|
||
// 计算A到S的向量和距离
|
||
Vector2D AS_vec = S - A;
|
||
double AS_distance = AS_vec.length();
|
||
|
||
// 检查机构是否可以装配
|
||
if (AS_distance > crankLength + rodLength ||
|
||
AS_distance < std::abs(crankLength - rodLength))
|
||
{
|
||
state.ErrorMessage = "滑块位置 " + formatDouble(sliderX) + " 导致机构无法装配";
|
||
return state;
|
||
}
|
||
|
||
// 使用余弦定理计算角度
|
||
double cosTheta = (crankLength * crankLength + AS_distance * AS_distance -
|
||
rodLength * rodLength) /
|
||
(2.0 * crankLength * AS_distance);
|
||
cosTheta = clamp(cosTheta, -1.0, 1.0);
|
||
double theta = std::acos(cosTheta);
|
||
|
||
// 计算基准角度
|
||
double alpha = std::atan2(AS_vec.Y, AS_vec.X);
|
||
|
||
// 计算两个可能的解
|
||
double angle1 = alpha + theta;
|
||
double angle2 = alpha - theta;
|
||
|
||
Vector2D B1 = A + Vector2D(
|
||
crankLength * std::cos(angle1),
|
||
crankLength * std::sin(angle1));
|
||
|
||
Vector2D B2 = A + Vector2D(
|
||
crankLength * std::cos(angle2),
|
||
crankLength * std::sin(angle2));
|
||
|
||
Vector2D B, B_alt;
|
||
|
||
// 默认选择解1
|
||
B = B1;
|
||
B_alt = B2;
|
||
|
||
// 根据解模式选择
|
||
if (solutionMode == SolutionMode::Solution2)
|
||
{
|
||
B = B2;
|
||
B_alt = B1;
|
||
}
|
||
else if (solutionMode == SolutionMode::Auto)
|
||
{
|
||
if (!trajectoryB.empty())
|
||
{
|
||
Vector2D lastB = trajectoryB.back();
|
||
double dist1 = Vector2D::Distance(B1, lastB);
|
||
double dist2 = Vector2D::Distance(B2, lastB);
|
||
if (dist2 < dist1)
|
||
{
|
||
B = B2;
|
||
B_alt = B1;
|
||
}
|
||
}
|
||
}
|
||
|
||
// 记录轨迹
|
||
trajectoryB.push_back(B);
|
||
trajectoryBAlt.push_back(B_alt);
|
||
sliderTrajectory.push_back(S);
|
||
|
||
// 限制轨迹点数量
|
||
const size_t MAX_TRAJECTORY_POINTS = 200;
|
||
if (trajectoryB.size() > MAX_TRAJECTORY_POINTS)
|
||
{
|
||
trajectoryB.erase(trajectoryB.begin());
|
||
trajectoryBAlt.erase(trajectoryBAlt.begin());
|
||
sliderTrajectory.erase(sliderTrajectory.begin());
|
||
}
|
||
|
||
// 计算角度
|
||
double crankAngle = std::atan2(B.Y, B.X) * 180.0 / M_PI;
|
||
double crankAngleAlt = std::atan2(B_alt.Y, B_alt.X) * 180.0 / M_PI;
|
||
|
||
// 检查机构是否可以装配
|
||
auto range = calculateSliderRange();
|
||
if (std::abs(range.first - range.second) < 1e-10)
|
||
{
|
||
state.WarningMessage = "警告：机构无法装配！(曲柄+连杆长度小于滑块偏移量)";
|
||
}
|
||
|
||
// 设置状态
|
||
state.Points["A"] = A;
|
||
state.Points["B"] = B;
|
||
state.Points["B_alt"] = B_alt;
|
||
state.Points["S"] = S;
|
||
|
||
state.Poses["AB"] = PoseCalculator::CalculatePoseAndQuaternion(A, B);
|
||
state.Poses["BS"] = PoseCalculator::CalculatePoseAndQuaternion(B, S);
|
||
|
||
// TCP姿态（滑块处）
|
||
Pose7 tcpPose;
|
||
tcpPose.tx = S.X;
|
||
tcpPose.ty = S.Y;
|
||
tcpPose.tz = 0.0;
|
||
tcpPose.qw = 1.0;
|
||
state.Poses["TCP"] = tcpPose;
|
||
|
||
// 角度信息
|
||
state.Angles["当前解角度"] = crankAngle;
|
||
state.Angles["备选解角度"] = crankAngleAlt;
|
||
state.Angles["滑块位置"] = sliderX;
|
||
|
||
return state;
|
||
}
|
||
catch (const std::exception &ex)
|
||
{
|
||
state.ErrorMessage = std::string("计算失败: ") + ex.what();
|
||
return state;
|
||
}
|
||
}
|
||
|
||
// 参数验证
|
||
ValidationResult SliderCrankMechanism::validateParameters()
|
||
{
|
||
ValidationResult result;
|
||
|
||
if (parameters.CrankLength <= 0.0)
|
||
{
|
||
result.Errors.push_back("曲柄长度必须大于0");
|
||
}
|
||
|
||
if (parameters.ConnectingRodLength <= 0.0)
|
||
{
|
||
result.Errors.push_back("连杆长度必须大于0");
|
||
}
|
||
|
||
if (parameters.CrankLength + parameters.ConnectingRodLength <= std::abs(parameters.SliderOffset))
|
||
{
|
||
result.Errors.push_back("不满足Grashof条件: 曲柄+连杆长度必须大于滑块偏移量的绝对值");
|
||
}
|
||
|
||
return result;
|
||
}
|
||
|
||
// 获取输入范围
|
||
std::pair<double, double> SliderCrankMechanism::getInputRange()
|
||
{
|
||
return calculateSliderRange();
|
||
}
|
||
|
||
// 获取轨迹点
|
||
std::vector<Vector2D> SliderCrankMechanism::getTrajectoryPoints()
|
||
{
|
||
return trajectoryB;
|
||
}
|
||
|
||
std::vector<Vector2D> SliderCrankMechanism::getSliderTrajectory() const
|
||
{
|
||
return sliderTrajectory;
|
||
}
|
||
|
||
std::vector<Vector2D> SliderCrankMechanism::getAlternativeTrajectory() const
|
||
{
|
||
return trajectoryBAlt;
|
||
}
|
||
|
||
// 清除轨迹
|
||
void SliderCrankMechanism::clearTrajectory()
|
||
{
|
||
trajectoryB.clear();
|
||
trajectoryBAlt.clear();
|
||
sliderTrajectory.clear();
|
||
}
|
||
|
||
// 获取状态文本
|
||
std::string SliderCrankMechanism::getStatusText()
|
||
{
|
||
auto range = calculateSliderRange();
|
||
std::string status = name + "\n";
|
||
|
||
if (std::abs(range.first - range.second) < 1e-10)
|
||
{
|
||
status += "⚠️ 警告：机构无法装配！(曲柄+连杆长度小于滑块偏移量)\n";
|
||
}
|
||
else
|
||
{
|
||
status += "曲柄=" + formatDouble(parameters.CrankLength) + "m, ";
|
||
status += "连杆=" + formatDouble(parameters.ConnectingRodLength) + "m\n";
|
||
status += "偏移=" + formatDouble(parameters.SliderOffset) + "m\n";
|
||
status += "滑块范围: [" + formatDouble(range.first) + ", " + formatDouble(range.second) + "]m\n";
|
||
status += "行程: " + formatDouble(range.second - range.first) + "m\n";
|
||
|
||
status += "解1角度范围: [" + formatDouble(parameters.CrankAngleRange1.first * 180.0 / M_PI, 1) + "°, ";
|
||
status += formatDouble(parameters.CrankAngleRange1.second * 180.0 / M_PI, 1) + "°]\n";
|
||
status += "解2角度范围: [" + formatDouble(parameters.CrankAngleRange2.first * 180.0 / M_PI, 1) + "°, ";
|
||
status += formatDouble(parameters.CrankAngleRange2.second * 180.0 / M_PI, 1) + "°]";
|
||
}
|
||
|
||
return status;
|
||
}
|
||
|
||
// ==================== 工厂函数实现 ====================
|
||
|
||
SliderCrankMechanism *createSliderCrankMechanism()
|
||
{
|
||
return new SliderCrankMechanism();
|
||
}
|
||
|
||
void deleteSliderCrankMechanism(SliderCrankMechanism *mechanism)
|
||
{
|
||
delete mechanism;
|
||
}
|
||
|
||
SliderCrankMechanismPtr createSliderCrankMechanismSmart()
|
||
{
|
||
return std::make_shared<SliderCrankMechanism>();
|
||
}
|