Initial commit

src/FourBarMechanism/CrankRockingBlockMechanism_Forward.cpp

@@ -0,0 +1,480 @@
+#include "CrankRockingBlockMechanism_Forward.h"
+#include <cmath>
+#include <algorithm>
+#include <stdexcept>
+#include <cstdio>
+
+using namespace std;
+
+// ==================== CrankRockingBlockMechanism_Forward 实现 ====================
+
+CrankRockingBlockMechanism_Forward::CrankRockingBlockMechanism_Forward()
+    : name("曲柄摇块机构 (正解)"),
+      type(MechanismType::CrankRockingBlock_Forward),
+      frameCount(0),
+      thetaADelta(0.0),
+      thetaAValueFactor(1.0),
+      thetaAFlip(1)
+{
+    // 设置默认参数
+    parameters.CrankLength2 = 1.0;         // OA - 曲柄长度
+    parameters.ConnectingRodLength2 = 3.0; // AB - 连杆长度
+    parameters.SliderOffset2 = 3.0;        // OC - 摇块中心距
+    parameters.InputValue = 0.0;           // 曲柄角度
+    parameters.AngularVelocity = 2.0;      // 角速度
+}
+
+string CrankRockingBlockMechanism_Forward::getName() const
+{
+    return name;
+}
+
+MechanismType CrankRockingBlockMechanism_Forward::getType() const
+{
+    return type;
+}
+
+MechanismParameters CrankRockingBlockMechanism_Forward::getParameters() const
+{
+    return parameters;
+}
+
+void CrankRockingBlockMechanism_Forward::setParameters(const MechanismParameters &params)
+{
+    parameters = params;
+}
+
+void CrankRockingBlockMechanism_Forward::setLink(double OA, double AB, double OC)
+{
+    parameters.CrankLength2 = OA;
+    parameters.ConnectingRodLength2 = AB;
+    parameters.SliderOffset2 = OC;
+}
+
+void CrankRockingBlockMechanism_Forward::setOA(double length)
+{
+    parameters.CrankLength2 = length;
+}
+
+void CrankRockingBlockMechanism_Forward::setAB(double length)
+{
+    parameters.ConnectingRodLength2 = length;
+}
+
+void CrankRockingBlockMechanism_Forward::setOC(double distance)
+{
+    parameters.SliderOffset2 = distance;
+}
+
+void CrankRockingBlockMechanism_Forward::setThetaADelta(double delta)
+{
+    thetaADelta = delta;
+}
+
+void CrankRockingBlockMechanism_Forward::setThetaAValueFactor(double factor)
+{
+    thetaAValueFactor = factor;
+}
+
+void CrankRockingBlockMechanism_Forward::setThetaAFlip(int flip)
+{
+    thetaAFlip = flip;
+}
+
+MechanismState CrankRockingBlockMechanism_Forward::calculate(double crankAngleDeg)
+{
+    // 应用配置参数
+    double angleDeg = thetaADelta + crankAngleDeg * thetaAValueFactor * thetaAFlip;
+    parameters.InputValue = angleDeg;
+
+    double OA = parameters.CrankLength2;         // 曲柄长度
+    double AB = parameters.ConnectingRodLength2; // 连杆长度
+    double OC = parameters.SliderOffset2;        // 摇块中心距
+
+    double crankAngle = degreesToRadians(angleDeg);
+
+    MechanismState state;
+    state.InputValue = angleDeg;
+
+    try
+    {
+        // 验证机构参数
+        auto validation = validateParameters();
+        if (!validation.isValid())
+        {
+            state.ErrorMessage = validation.getCombinedMessage();
+            return state;
+        }
+
+        // 计算各点坐标
+        Vector2D O(0.0, 0.0); // 固定点
+        Vector2D C(OC, 0.0);  // 摇块中心
+        Vector2D A(           // 曲柄端点
+            OA * cos(crankAngle),
+            OA * sin(crankAngle));
+
+        // 计算滑块B的位置 (B点在直线AC上)
+        Vector2D B = calculatePointB(A, C, AB);
+
+        if (B.X == 0.0 && B.Y == 0.0 && AB > 1e-10)
+        {
+            state.ErrorMessage = "无法计算滑块位置，机构可能无法装配";
+            return state;
+        }
+
+        // 记录轨迹
+        trajectoryB.push_back(B);
+        crankCirclePoints.push_back(A);
+        rockerTrajectory.push_back(B);
+
+        // 限制轨迹点数
+        const size_t MAX_TRAJECTORY_POINTS = 500;
+        if (trajectoryB.size() > MAX_TRAJECTORY_POINTS)
+        {
+            trajectoryB.erase(trajectoryB.begin());
+            crankCirclePoints.erase(crankCirclePoints.begin());
+            rockerTrajectory.erase(rockerTrajectory.begin());
+        }
+
+        // 更新帧计数
+        frameCount++;
+
+        // 计算角度信息
+        double rockerAngle = atan2(B.Y - C.Y, B.X - C.X);
+        double acDistance = Vector2D::Distance(A, C);
+
+        // 设置状态
+        state.Points["O"] = O;
+        state.Points["A"] = A;
+        state.Points["B"] = B;
+        state.Points["C"] = C;
+
+        state.Angles["曲柄角度"] = normalizeAngle(angleDeg);
+        state.Angles["摇块角度"] = normalizeAngle(radiansToDegrees(rockerAngle));
+        state.Angles["AC距离"] = acDistance;
+
+        // 计算姿态
+        state.Poses["OA"] = PoseCalculator::CalculatePoseAndQuaternion(O, A);
+        state.Poses["AB"] = PoseCalculator::CalculatePoseAndQuaternion(A, B);
+        state.Poses["BC"] = PoseCalculator::CalculatePoseAndQuaternion(B, C);
+
+        // TCP姿态（滑块B点）
+        Pose7 tcpPose;
+        tcpPose.tx = B.X;
+        tcpPose.ty = B.Y;
+        tcpPose.tz = 0.0;
+        tcpPose.qw = 1.0;
+        state.Poses["TCP"] = tcpPose;
+
+        // 检查极限位置
+        checkLimitPositions(state, A, B, C, OA, AB, OC);
+
+        return state;
+    }
+    catch (const exception &ex)
+    {
+        state.ErrorMessage = string("计算失败: ") + ex.what();
+        return state;
+    }
+}
+
+Vector2D CrankRockingBlockMechanism_Forward::calculatePointB(
+    const Vector2D &A, const Vector2D &C, double AB)
+{
+    // 直线方程: B在直线AC上，且|AB| = 给定长度
+    Vector2D AC = C - A;
+    double AC_length = AC.Length();
+
+    if (AC_length < 1e-10)
+        return Vector2D::Zero();
+
+    // 单位向量
+    Vector2D AC_unit = AC * (1.0 / AC_length);
+
+    // 有两个可能的解
+    Vector2D B1 = A + AC_unit * AB; // 沿AC方向
+    Vector2D B2 = A - AC_unit * AB; // 反向
+
+    // 选择合理的解（基于连续性）
+    if (trajectoryB.empty())
+    {
+        // 初始状态选择B1（通常滑块在C点附近）
+        return B1;
+    }
+    else
+    {
+        // 选择与上一帧更接近的解
+        Vector2D lastB = trajectoryB.back();
+        double dist1 = Vector2D::Distance(B1, lastB);
+        double dist2 = Vector2D::Distance(B2, lastB);
+        return (dist1 < dist2) ? B1 : B2;
+    }
+}
+
+void CrankRockingBlockMechanism_Forward::checkLimitPositions(
+    MechanismState &state, const Vector2D &A, const Vector2D &B, const Vector2D &C,
+    double OA, double AB, double OC)
+{
+    // 检查死点位置（OA与AC共线）
+    Vector2D OA_vec = A;
+    Vector2D AC_vec = C - A;
+
+    // 计算夹角
+    double dotProduct = OA_vec.Dot(AC_vec);
+    double productLength = OA_vec.Length() * AC_vec.Length();
+
+    if (productLength > 1e-10)
+    {
+        double cosAngle = dotProduct / productLength;
+        double angle = acos(fabs(cosAngle)) * 180.0 / M_PI;
+
+        if (angle < 5.0 || angle > 175.0)
+        {
+            if (!state.WarningMessage.empty())
+                state.WarningMessage += "\n";
+            state.WarningMessage += "警告：接近死点位置";
+        }
+    }
+
+    // 检查机构装配条件
+    double acDistance = Vector2D::Distance(A, C);
+    if (fabs(acDistance - AB) < 0.1)
+    {
+        if (!state.WarningMessage.empty())
+            state.WarningMessage += "\n";
+        state.WarningMessage += "警告：接近极限位置 (AC ≈ AB)";
+    }
+
+    // 检查曲柄角度范围
+    auto range = getValidCrankRange();
+    if (range.first > 0 || range.second < 360)
+    {
+        double currentAngle = normalizeAngle(parameters.InputValue);
+        if (currentAngle < range.first + 10 || currentAngle > range.second - 10)
+        {
+            char buffer[128];
+            snprintf(buffer, sizeof(buffer),
+                     "警告：接近曲柄角度极限 [%.1f°, %.1f°]",
+                     range.first, range.second);
+
+            if (!state.WarningMessage.empty())
+                state.WarningMessage += "\n";
+            state.WarningMessage += buffer;
+        }
+    }
+}
+
+double CrankRockingBlockMechanism_Forward::normalizeAngle(double angle)
+{
+    angle = fmod(angle, 360.0);
+    if (angle < 0)
+        angle += 360.0;
+    return angle;
+}
+
+pair<double, double> CrankRockingBlockMechanism_Forward::getValidCrankRange() const
+{
+    double OA = parameters.CrankLength2;
+    double AB = parameters.ConnectingRodLength2;
+    double OC = parameters.SliderOffset2;
+
+    // 检查机构能否整周旋转
+    bool canFullRotation = checkFullRotationCondition(OA, AB, OC);
+
+    if (canFullRotation)
+    {
+        return make_pair(0.0, 360.0);
+    }
+    else
+    {
+        // 计算曲柄的摆动范围
+        double cosTheta = (OA * OA + OC * OC - AB * AB) / (2 * OA * OC);
+        cosTheta = max(min(cosTheta, 1.0), -1.0);
+        double theta = acos(cosTheta) * 180.0 / M_PI;
+
+        double minAngle = 180.0 - theta;
+        double maxAngle = 180.0 + theta;
+
+        // 确保角度在0-360范围内
+        minAngle = fmod(minAngle + 360.0, 360.0);
+        maxAngle = fmod(maxAngle + 360.0, 360.0);
+
+        return make_pair(minAngle, maxAngle);
+    }
+}
+
+bool CrankRockingBlockMechanism_Forward::checkFullRotationCondition(
+    double OA, double AB, double OC) const
+{
+    // 曲柄摇块机构整周旋转条件：
+    // 1. 曲柄是最短杆
+    // 2. 满足三角形不等式
+
+    double minLength = min(min(OA, AB), OC);
+
+    // 曲柄不是最短杆
+    if (fabs(minLength - OA) > 1e-10)
+        return false;
+
+    // 检查几何约束
+    if (OA + AB < OC || fabs(OA - AB) > OC)
+        return false;
+
+    return true;
+}
+
+pair<double, double> CrankRockingBlockMechanism_Forward::getInputRange()
+{
+    return make_pair(0.0, 360.0);
+}
+
+vector<Vector2D> CrankRockingBlockMechanism_Forward::getTrajectoryPoints() const
+{
+    return trajectoryB;
+}
+
+vector<Vector2D> CrankRockingBlockMechanism_Forward::getCrankCirclePoints() const
+{
+    return crankCirclePoints;
+}
+
+vector<Vector2D> CrankRockingBlockMechanism_Forward::getRockerTrajectory() const
+{
+    return rockerTrajectory;
+}
+
+void CrankRockingBlockMechanism_Forward::clearTrajectory()
+{
+    trajectoryB.clear();
+    crankCirclePoints.clear();
+    rockerTrajectory.clear();
+    frameCount = 0;
+    timer.reset();
+}
+
+ValidationResult CrankRockingBlockMechanism_Forward::validateParameters()
+{
+    ValidationResult result;
+
+    if (parameters.CrankLength2 <= 0.0)
+        result.Errors.push_back("曲柄长度OA必须大于0");
+
+    if (parameters.ConnectingRodLength2 <= 0.0)
+        result.Errors.push_back("连杆长度AB必须大于0");
+
+    if (parameters.SliderOffset2 <= 0.0)
+        result.Errors.push_back("摇块中心距OC必须大于0");
+
+    double OA = parameters.CrankLength2;
+    double AB = parameters.ConnectingRodLength2;
+    double OC = parameters.SliderOffset2;
+
+    // 检查机构装配条件
+    if (OA + AB < OC)
+    {
+        result.Warnings.push_back("机构可能无法装配或不满足整周旋转条件");
+    }
+
+    // 检查曲柄是否为最短杆
+    double minLength = min(min(OA, AB), OC);
+    if (fabs(minLength - OA) > 1e-10)
+    {
+        result.Warnings.push_back("曲柄不是最短杆，可能无法整周旋转");
+    }
+
+    return result;
+}
+
+string CrankRockingBlockMechanism_Forward::getStatusText()
+{
+    double runTime = timer.elapsedSeconds();
+    double fps = (runTime > 0.0) ? frameCount / runTime : 0.0;
+
+    auto range = getValidCrankRange();
+    string rotationInfo;
+
+    if (range.first == 0.0 && range.second == 360.0)
+    {
+        rotationInfo = "曲柄可整周旋转";
+    }
+    else
+    {
+        char buffer[64];
+        snprintf(buffer, sizeof(buffer),
+                 "曲柄摆动范围: [%.1f°, %.1f°]",
+                 range.first, range.second);
+        rotationInfo = buffer;
+    }
+
+    char statusBuffer[512];
+    snprintf(statusBuffer, sizeof(statusBuffer),
+             "%s\n"
+             "曲柄OA: %.3f m\n"
+             "连杆AB: %.3f m\n"
+             "摇块中心OC: %.3f m\n"
+             "曲柄角度: %.1f°\n"
+             "%s\n"
+             "角速度: %.2f rad/s\n"
+             "运行时间: %.1f s\n"
+             "帧率: %.1f FPS\n"
+             "轨迹点数: %zu/500",
+             name.c_str(),
+             parameters.CrankLength2,
+             parameters.ConnectingRodLength2,
+             parameters.SliderOffset2,
+             parameters.InputValue,
+             rotationInfo.c_str(),
+             parameters.AngularVelocity,
+             runTime,
+             fps,
+             trajectoryB.size());
+
+    return string(statusBuffer);
+}
+
+double CrankRockingBlockMechanism_Forward::getThetaADelta() const
+{
+    return thetaADelta;
+}
+
+double CrankRockingBlockMechanism_Forward::getThetaAValueFactor() const
+{
+    return thetaAValueFactor;
+}
+
+int CrankRockingBlockMechanism_Forward::getThetaAFlip() const
+{
+    return thetaAFlip;
+}
+
+double CrankRockingBlockMechanism_Forward::getOA() const
+{
+    return parameters.CrankLength2;
+}
+
+double CrankRockingBlockMechanism_Forward::getAB() const
+{
+    return parameters.ConnectingRodLength2;
+}
+
+double CrankRockingBlockMechanism_Forward::getOC() const
+{
+    return parameters.SliderOffset2;
+}
+
+double CrankRockingBlockMechanism_Forward::degreesToRadians(double degrees) const
+{
+    return degrees * M_PI / 180.0;
+}
+
+double CrankRockingBlockMechanism_Forward::radiansToDegrees(double radians) const
+{
+    return radians * 180.0 / M_PI;
+}
+
+string CrankRockingBlockMechanism_Forward::formatDouble(double value, int precision) const
+{
+    char buffer[32];
+    snprintf(buffer, sizeof(buffer), "%.*f", precision, value);
+    return string(buffer);
+}

src/FourBarMechanism/CrankRockingBlockMechanism_Inverse.cpp

@@ -0,0 +1,261 @@
+#include "CrankRockingBlockMechanism_Inverse.h"
+#include <cmath>
+#include <algorithm>
+#include <stdexcept>
+#include <cstdio>
+
+using namespace std;
+
+// ==================== CrankRockingBlockMechanism_Inverse 实现 ====================
+
+CrankRockingBlockMechanism_Inverse::CrankRockingBlockMechanism_Inverse()
+    : name("曲柄摇块机构 (逆解)"),
+      type(MechanismType::CrankRockingBlock)
+{
+    // 设置默认参数
+    parameters.CrankLength2 = 1.0;         // OA - 曲柄长度
+    parameters.ConnectingRodLength2 = 3.0; // AB - 连杆长度
+    parameters.SliderOffset2 = 3.0;        // OC - 摇块中心距
+    parameters.InputValue = 2.5;           // AC距离
+    parameters.AngularVelocity = 1.0;      // 角速度
+}
+
+string CrankRockingBlockMechanism_Inverse::getName() const
+{
+    return name;
+}
+
+MechanismType CrankRockingBlockMechanism_Inverse::getType() const
+{
+    return type;
+}
+
+MechanismParameters CrankRockingBlockMechanism_Inverse::getParameters() const
+{
+    return parameters;
+}
+
+void CrankRockingBlockMechanism_Inverse::setParameters(const MechanismParameters &params)
+{
+    parameters = params;
+}
+
+void CrankRockingBlockMechanism_Inverse::setLink(double OA, double AB, double OC)
+{
+    parameters.CrankLength2 = OA;
+    parameters.ConnectingRodLength2 = AB;
+    parameters.SliderOffset2 = OC;
+}
+
+void CrankRockingBlockMechanism_Inverse::setOA(double length)
+{
+    parameters.CrankLength2 = length;
+}
+
+void CrankRockingBlockMechanism_Inverse::setAB(double length)
+{
+    parameters.ConnectingRodLength2 = length;
+}
+
+void CrankRockingBlockMechanism_Inverse::setOC(double distance)
+{
+    parameters.SliderOffset2 = distance;
+}
+
+MechanismState CrankRockingBlockMechanism_Inverse::calculate(double acDistance)
+{
+    parameters.InputValue = acDistance;
+
+    double OA = parameters.CrankLength2;         // 曲柄长度
+    double AB = parameters.ConnectingRodLength2; // 连杆长度
+    double OC = parameters.SliderOffset2;        // 摇块中心距
+    double AC = acDistance;
+
+    MechanismState state;
+    state.InputValue = acDistance;
+
+    try
+    {
+        // 验证AC距离范围
+        auto acRange = getACRange();
+
+        if (AC < acRange.first || AC > acRange.second)
+        {
+            char buffer[128];
+            snprintf(buffer, sizeof(buffer),
+                     "AC距离超出有效范围 [%.3f, %.3f]",
+                     acRange.first, acRange.second);
+            state.ErrorMessage = buffer;
+            return state;
+        }
+
+        // 计算角度θ1 (∠AOC)
+        double cosTheta1 = (OA * OA + OC * OC - AC * AC) / (2 * OA * OC);
+        cosTheta1 = max(min(cosTheta1, 1.0), -1.0);
+        double theta1 = acos(cosTheta1);
+
+        // 计算角度φ (∠BAC)
+        double cosPhi = (AC * AC + OC * OC - OA * OA) / (2 * AC * OC);
+        cosPhi = max(min(cosPhi, 1.0), -1.0);
+        double phi = acos(cosPhi);
+
+        // 计算坐标点
+        Vector2D O(0.0, 0.0); // 固定点
+        Vector2D C(OC, 0.0);  // 摇块中心
+        Vector2D A(           // 曲柄端点
+            OA * cos(theta1),
+            OA * sin(theta1));
+
+        // B点在AC延长线上 (沿AC方向距离A点为AB长度)
+        Vector2D AC_vec = C - A;
+        double AC_length = AC_vec.Length();
+
+        if (AC_length < 1e-10)
+        {
+            state.ErrorMessage = "A点和C点重合";
+            return state;
+        }
+
+        Vector2D AC_dir = AC_vec * (1.0 / AC_length);
+        Vector2D B = A + AC_dir * AB;
+
+        // 记录轨迹（限制点数）
+        trajectoryPoints.push_back(B);
+        const size_t MAX_TRAJECTORY_POINTS = 100;
+        if (trajectoryPoints.size() > MAX_TRAJECTORY_POINTS)
+        {
+            trajectoryPoints.erase(trajectoryPoints.begin());
+        }
+
+        // 设置状态
+        state.Points["O"] = O;
+        state.Points["A"] = A;
+        state.Points["B"] = B;
+        state.Points["C"] = C;
+
+        state.Angles["θ1"] = theta1 * 180.0 / M_PI;
+        state.Angles["φ"] = phi * 180.0 / M_PI;
+        state.Angles["AC距离"] = AC;
+
+        // 计算姿态
+        state.Poses["OA"] = PoseCalculator::CalculatePoseAndQuaternion(O, A);
+        state.Poses["AB"] = PoseCalculator::CalculatePoseAndQuaternion(A, B);
+        state.Poses["BC"] = PoseCalculator::CalculatePoseAndQuaternion(B, C);
+
+        // TCP姿态（滑块B点）
+        Pose7 tcpPose;
+        tcpPose.tx = B.X;
+        tcpPose.ty = B.Y;
+        tcpPose.tz = 0.0;
+        tcpPose.qw = 1.0;
+        state.Poses["TCP"] = tcpPose;
+
+        return state;
+    }
+    catch (const exception &ex)
+    {
+        state.ErrorMessage = string("计算失败: ") + ex.what();
+        return state;
+    }
+}
+
+pair<double, double> CrankRockingBlockMechanism_Inverse::getACRange() const
+{
+    double OA = parameters.CrankLength2;
+    double OC = parameters.SliderOffset2;
+    double AB = parameters.ConnectingRodLength2;
+
+    // 理论最小和最大AC距离
+    double minAC = max(fabs(OA - OC), 0.1);
+    double maxAC = OA + OC - 0.1;
+
+    // 考虑连杆长度约束
+    minAC = max(minAC, fabs(AB - OA));
+    maxAC = min(maxAC, AB + OA);
+
+    return make_pair(minAC, maxAC);
+}
+
+pair<double, double> CrankRockingBlockMechanism_Inverse::getInputRange()
+{
+    return getACRange();
+}
+
+vector<Vector2D> CrankRockingBlockMechanism_Inverse::getTrajectoryPoints() const
+{
+    return trajectoryPoints;
+}
+
+void CrankRockingBlockMechanism_Inverse::clearTrajectory()
+{
+    trajectoryPoints.clear();
+}
+
+ValidationResult CrankRockingBlockMechanism_Inverse::validateParameters()
+{
+    ValidationResult result;
+
+    if (parameters.CrankLength2 <= 0.0)
+        result.Errors.push_back("曲柄长度OA必须大于0");
+
+    if (parameters.ConnectingRodLength2 <= 0.0)
+        result.Errors.push_back("连杆长度AB必须大于0");
+
+    if (parameters.SliderOffset2 <= 0.0)
+        result.Errors.push_back("摇块中心距OC必须大于0");
+
+    // 检查三角形不等式
+    double OA = parameters.CrankLength2;
+    double AB = parameters.ConnectingRodLength2;
+    double OC = parameters.SliderOffset2;
+
+    if (OA + AB < OC)
+    {
+        result.Errors.push_back("曲柄+连杆长度必须大于摇块中心距");
+    }
+
+    if (OA + OC < AB)
+    {
+        result.Errors.push_back("曲柄+中心距必须大于连杆长度");
+    }
+
+    if (AB + OC < OA)
+    {
+        result.Errors.push_back("连杆+中心距必须大于曲柄长度");
+    }
+
+    // 检查运动范围
+    auto range = getACRange();
+    if (range.first >= range.second)
+    {
+        result.Errors.push_back("机构参数无效，无法形成有效运动范围");
+    }
+
+    return result;
+}
+
+string CrankRockingBlockMechanism_Inverse::getStatusText()
+{
+    auto range = getACRange();
+
+    char buffer[256];
+    snprintf(buffer, sizeof(buffer),
+             "%s\n"
+             "曲柄OA: %.3f m\n"
+             "连杆AB: %.3f m\n"
+             "摇块中心OC: %.3f m\n"
+             "当前AC距离: %.3f m\n"
+             "AC有效范围: [%.3f, %.3f] m\n"
+             "角速度: %.2f rad/s\n"
+             "轨迹点数: %zu",
+             name.c_str(),
+             parameters.CrankLength2,
+             parameters.ConnectingRodLength2,
+             parameters.SliderOffset2,
+             parameters.InputValue,
+             range.first, range.second,
+             parameters.AngularVelocity,
+             trajectoryPoints.size());
+
+    return string(buffer);
+}

src/FourBarMechanism/CrankSliderMechanism.cpp

@@ -0,0 +1,474 @@
+// mechanism_simulation.cpp
+#include "CrankSliderMechanism.h"
+#include <stdexcept>
+#include <algorithm>
+#include <cstdio>
+
+// ==================== MechanismParameters 实现 ====================
+
+MechanismParameters::MechanismParameters() : CrankLength2(1.0),
+                                             ConnectingRodLength2(3.0),
+                                             SliderOffset2(0.5),
+                                             L1(1.0),
+                                             L2(3.0),
+                                             L3(2.5),
+                                             L4(3.5),
+                                             InputValue(0.0),
+                                             AngularVelocity(2.0) {}
+
+// ==================== MechanismState 实现 ====================
+
+MechanismState::MechanismState() : InputValue(0.0) {}
+
+bool MechanismState::hasError() const
+{
+    return !ErrorMessage.empty();
+}
+
+bool MechanismState::hasWarning() const
+{
+    return !WarningMessage.empty();
+}
+
+// ==================== ValidationResult 实现 ====================
+
+bool ValidationResult::isValid() const
+{
+    return Errors.empty();
+}
+
+std::string ValidationResult::getCombinedMessage() const
+{
+    std::string message;
+
+    if (!Errors.empty())
+    {
+        message += "错误:\n";
+        for (const auto &error : Errors)
+        {
+            message += "  • " + error + "\n";
+        }
+    }
+
+    if (!Warnings.empty())
+    {
+        if (!message.empty())
+            message += "\n";
+        message += "警告:\n";
+        for (const auto &warning : Warnings)
+        {
+            message += "  • " + warning + "\n";
+        }
+    }
+
+    return message;
+}
+
+// ==================== SimpleTimer 实现 ====================
+
+SimpleTimer::SimpleTimer()
+{
+    reset();
+}
+
+void SimpleTimer::reset()
+{
+    start = std::chrono::steady_clock::now();
+}
+
+double SimpleTimer::elapsedSeconds() const
+{
+    auto now = std::chrono::steady_clock::now();
+    std::chrono::duration<double> elapsed = now - start;
+    return elapsed.count();
+}
+
+// ==================== CrankSliderMechanism 私有辅助方法 ====================
+
+double CrankSliderMechanism::degreesToRadians(double degrees) const
+{
+    return degrees * M_PI / 180.0;
+}
+
+double CrankSliderMechanism::radiansToDegrees(double radians) const
+{
+    return radians * 180.0 / M_PI;
+}
+
+std::string CrankSliderMechanism::formatDouble(double value, int precision) const
+{
+    char buffer[32];
+    snprintf(buffer, sizeof(buffer), "%.*f", precision, value);
+    return std::string(buffer);
+}
+
+// ==================== CrankSliderMechanism 公有方法实现 ====================
+
+CrankSliderMechanism::CrankSliderMechanism() : name("曲柄滑块机构 (正解)"),
+                                               type(MechanismType::CrankSlider),
+                                               thetaADelta(0.0),
+                                               thetaAValueFactor(1.0),
+                                               thetaAFlip(1),
+                                               frameCount(0)
+{
+
+    parameters.CrankLength2 = 0.5;
+    parameters.ConnectingRodLength2 = 2.0;
+    parameters.SliderOffset2 = 0.0;
+    parameters.InputValue = 0.0;
+    parameters.AngularVelocity = 2.0;
+}
+
+std::string CrankSliderMechanism::getName() const
+{
+    return name;
+}
+
+MechanismType CrankSliderMechanism::getType() const
+{
+    return type;
+}
+
+MechanismParameters CrankSliderMechanism::getParameters() const
+{
+    return parameters;
+}
+
+void CrankSliderMechanism::setName(const std::string &newName)
+{
+    name = newName;
+}
+
+void CrankSliderMechanism::setParameters(const MechanismParameters &params)
+{
+    parameters = params;
+}
+
+void CrankSliderMechanism::setLink(double crankLength, double rodLength, double sliderOffset)
+{
+    parameters.CrankLength2 = crankLength;
+    parameters.ConnectingRodLength2 = rodLength;
+    parameters.SliderOffset2 = sliderOffset;
+}
+
+// 配置方法
+void CrankSliderMechanism::setThetaADelta(double delta)
+{
+    thetaADelta = delta;
+}
+
+void CrankSliderMechanism::setThetaAValueFactor(double factor)
+{
+    thetaAValueFactor = factor;
+}
+
+void CrankSliderMechanism::setThetaAFlip(int flip)
+{
+    thetaAFlip = flip;
+}
+
+void CrankSliderMechanism::setL_AB(double length)
+{
+    parameters.CrankLength2 = length;
+}
+
+void CrankSliderMechanism::setL_BS(double length)
+{
+    parameters.ConnectingRodLength2 = length;
+}
+
+void CrankSliderMechanism::setS_OFS(double offset)
+{
+    parameters.SliderOffset2 = offset;
+}
+
+void CrankSliderMechanism::setCodeBody(const std::string &code)
+{
+    codeBody = code;
+}
+
+void CrankSliderMechanism::setL1ABModelName(const std::string &modelName)
+{
+    l1ABModelName = modelName;
+}
+
+void CrankSliderMechanism::setL2BSModelName(const std::string &modelName)
+{
+    l2BSModelName = modelName;
+}
+
+void CrankSliderMechanism::setL3SModelName(const std::string &modelName)
+{
+    l3SModelName = modelName;
+}
+
+MechanismState CrankSliderMechanism::calculate(double _angleDeg)
+{
+    double angleDeg = thetaADelta + _angleDeg * thetaAValueFactor * thetaAFlip;
+    parameters.InputValue = angleDeg;
+
+    double angle = degreesToRadians(angleDeg);
+    double crankLength = parameters.CrankLength2;
+    double rodLength = parameters.ConnectingRodLength2;
+    double sliderOffset = parameters.SliderOffset2;
+
+    Vector2D A(0.0, 0.0);
+    MechanismState state;
+    state.InputValue = angleDeg;
+
+    try
+    {
+        // 验证Grashof条件
+        bool grashofCondition = crankLength + rodLength > std::abs(sliderOffset);
+        if (!grashofCondition)
+        {
+            state.ErrorMessage = "错误：不满足Grashof条件\n(曲柄+连杆长度必须大于滑块偏移量的绝对值)";
+            return state;
+        }
+
+        // 检查滑块偏移量
+        double maxOffset = rodLength - crankLength;
+        if (std::abs(sliderOffset) > maxOffset)
+        {
+            state.ErrorMessage = "错误：滑块偏移量过大\n最大允许值: ±" + formatDouble(maxOffset) + "m";
+            return state;
+        }
+        else if (std::abs(sliderOffset) > 0.8 * maxOffset)
+        {
+            state.WarningMessage = "警告：滑块偏移量接近极限值\n建议值: ±" + formatDouble(0.8 * maxOffset) + "m以内";
+        }
+
+        // 计算B点位置
+        Vector2D B = A + Vector2D(
+                             crankLength * std::cos(angle),
+                             crankLength * std::sin(angle));
+
+        // 计算滑块位置（S点）
+        double Bx = B.X;
+        double By = B.Y;
+        double y_s = sliderOffset;
+        double L = rodLength;
+
+        // 解二次方程：x_s^2 - 2*Bx*x_s + Bx^2 + (y_s - By)^2 - L^2 = 0
+        double a = 1.0;
+        double b = -2.0 * Bx;
+        double c = Bx * Bx + (y_s - By) * (y_s - By) - L * L;
+
+        double discriminant = b * b - 4.0 * a * c;
+
+        if (discriminant < 0.0)
+        {
+            state.ErrorMessage = "错误：无法找到滑块位置，机构可能无法装配";
+            return state;
+        }
+
+        double sqrtDiscriminant = std::sqrt(discriminant);
+        double x1 = (-b + sqrtDiscriminant) / (2.0 * a);
+        double x2 = (-b - sqrtDiscriminant) / (2.0 * a);
+
+        // 选择正确的解（基于连续性）
+        // 修正：改进解的选择逻辑
+        double x_s;
+        if (!sliderTrajectory.empty())
+        {
+            // 使用连续性原则：选择与上一个位置最接近的解
+            double lastX = sliderTrajectory.back().X;
+            x_s = (std::abs(x1 - lastX) < std::abs(x2 - lastX)) ? x1 : x2;
+        }
+        else
+        {
+            x_s = (x1 > x2) ? x1 : x2; // 选择较大的解（通常对应正向运动）
+        }
+
+        Vector2D S(x_s, y_s);
+
+        // 记录轨迹点
+        // 记录轨迹点
+        trajectoryB.push_back(B);
+        sliderTrajectory.push_back(S);
+        crankCirclePoints.push_back(B);
+
+        // 限制轨迹点数量
+        const size_t MAX_TRAJECTORY_POINTS = 1000;
+        if (trajectoryB.size() > MAX_TRAJECTORY_POINTS)
+        {
+            trajectoryB.erase(trajectoryB.begin());
+            sliderTrajectory.erase(sliderTrajectory.begin());
+            crankCirclePoints.erase(crankCirclePoints.begin());
+        }
+
+        // 更新帧计数
+        frameCount++;
+
+        // 设置状态
+        state.Points["A"] = A;
+        state.Points["B"] = B;
+        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["曲柄角度"] = std::fmod(angleDeg, 360.0);
+        state.Angles["滑块位置X"] = x_s;
+
+        return state;
+    }
+    catch (const std::exception &ex)
+    {
+        state.ErrorMessage = std::string("计算失败: ") + ex.what();
+        return state;
+    }
+}
+
+ValidationResult CrankSliderMechanism::validateParameters()
+{
+    ValidationResult result;
+
+    if (parameters.CrankLength2 <= 0.0)
+    {
+        result.Errors.push_back("曲柄长度必须大于0");
+    }
+
+    if (parameters.ConnectingRodLength2 <= 0.0)
+    {
+        result.Errors.push_back("连杆长度必须大于0");
+    }
+
+    if (parameters.CrankLength2 + parameters.ConnectingRodLength2 <= std::abs(parameters.SliderOffset2))
+    {
+        result.Errors.push_back("不满足Grashof条件: 曲柄+连杆长度必须大于滑块偏移量的绝对值");
+    }
+
+    double maxOffset = parameters.ConnectingRodLength2 - parameters.CrankLength2;
+    if (std::abs(parameters.SliderOffset2) > maxOffset)
+    {
+        result.Errors.push_back("滑块偏移量过大，最大允许值: ±" + formatDouble(maxOffset) + "m");
+    }
+    else if (std::abs(parameters.SliderOffset2) > 0.8 * maxOffset)
+    {
+        result.Warnings.push_back("滑块偏移量接近极限值，建议值: ±" + formatDouble(0.8 * maxOffset) + "m以内");
+    }
+
+    return result;
+}
+
+std::pair<double, double> CrankSliderMechanism::getInputRange()
+{
+    return std::make_pair(0.0, 360.0);
+}
+
+std::vector<Vector2D> CrankSliderMechanism::getTrajectoryPoints()
+{
+    return trajectoryB;
+}
+
+std::vector<Vector2D> CrankSliderMechanism::getSliderTrajectory() const
+{
+    return sliderTrajectory;
+}
+
+std::vector<Vector2D> CrankSliderMechanism::getCrankCirclePoints() const
+{
+    return crankCirclePoints;
+}
+
+void CrankSliderMechanism::clearTrajectory()
+{
+    trajectoryB.clear();
+    sliderTrajectory.clear();
+    crankCirclePoints.clear();
+    frameCount = 0;
+    timer.reset();
+}
+
+std::string CrankSliderMechanism::getStatusText()
+{
+    double runTime = timer.elapsedSeconds();
+    double fps = (runTime > 0.0) ? frameCount / runTime : 0.0;
+
+    return name + "\n" +
+           "曲柄长度: " + formatDouble(parameters.CrankLength2) + "m\n" +
+           "连杆长度: " + formatDouble(parameters.ConnectingRodLength2) + "m\n" +
+           "滑块偏移: " + formatDouble(parameters.SliderOffset2) + "m\n" +
+           "角速度: " + formatDouble(parameters.AngularVelocity, 1) + "rad/s\n" +
+           "运行时间: " + formatDouble(runTime, 1) + "s\n" +
+           "帧率: " + formatDouble(fps, 1) + " FPS\n" +
+           "轨迹点数: " + std::to_string(trajectoryB.size()) + "/1000";
+}
+
+// 获取配置参数
+double CrankSliderMechanism::getThetaADelta() const
+{
+    return thetaADelta;
+}
+
+double CrankSliderMechanism::getThetaAValueFactor() const
+{
+    return thetaAValueFactor;
+}
+
+int CrankSliderMechanism::getThetaAFlip() const
+{
+    return thetaAFlip;
+}
+
+double CrankSliderMechanism::getL_AB() const
+{
+    return parameters.CrankLength2;
+}
+
+double CrankSliderMechanism::getL_BS() const
+{
+    return parameters.ConnectingRodLength2;
+}
+
+double CrankSliderMechanism::getS_OFS() const
+{
+    return parameters.SliderOffset2;
+}
+
+std::string CrankSliderMechanism::getCodeBody() const
+{
+    return codeBody;
+}
+
+std::string CrankSliderMechanism::getL1ABModelName() const
+{
+    return l1ABModelName;
+}
+
+std::string CrankSliderMechanism::getL2BSModelName() const
+{
+    return l2BSModelName;
+}
+
+std::string CrankSliderMechanism::getL3SModelName() const
+{
+    return l3SModelName;
+}
+
+// ==================== 工厂函数实现 ====================
+
+CrankSliderMechanism *createCrankSliderMechanism()
+{
+    return new CrankSliderMechanism();
+}
+
+void deleteCrankSliderMechanism(CrankSliderMechanism *mechanism)
+{
+    delete mechanism;
+}
+
+CrankSliderMechanismPtr createCrankSliderMechanismSmart()
+{
+    return std::make_shared<CrankSliderMechanism>();
+}

src/FourBarMechanism/FourBarMechanism.cpp

@@ -0,0 +1,329 @@
+// FourBarMechanism.cpp
+#include "FourBarMechanism.h"
+#include <stdexcept>
+#include <algorithm>
+#include <cstdio>
+#include <sstream>
+
+// ==================== FourBarMechanism 实现 ====================
+
+FourBarMechanism::FourBarMechanism() : name("四杆机构 (正解)"),
+                                       frameCount(0),
+                                       startTime(std::chrono::steady_clock::now())
+{
+    parameters.L1 = 1.0;
+    parameters.L2 = 3.0;
+    parameters.L3 = 2.5;
+    parameters.L4 = 3.5;
+    parameters.InputValue = 0.0;
+    parameters.AngularVelocity = 2.0;
+}
+
+double FourBarMechanism::degreesToRadians(double degrees) const
+{
+    return degrees * M_PI / 180.0;
+}
+
+double FourBarMechanism::radiansToDegrees(double radians) const
+{
+    return radians * 180.0 / M_PI;
+}
+
+std::string FourBarMechanism::formatDouble(double value, int precision) const
+{
+    char buffer[32];
+    snprintf(buffer, sizeof(buffer), "%.*f", precision, value);
+    return std::string(buffer);
+}
+
+std::string FourBarMechanism::getName() const
+{
+    return name;
+}
+
+MechanismType FourBarMechanism::getType() const
+{
+    return MechanismType::FourBar;
+}
+
+MechanismParameters FourBarMechanism::getParameters() const
+{
+    return parameters;
+}
+
+void FourBarMechanism::setName(const std::string &newName)
+{
+    name = newName;
+}
+
+void FourBarMechanism::setParameters(const MechanismParameters &params)
+{
+    parameters = params;
+}
+
+void FourBarMechanism::setLink(double l1, double l2, double l3, double l4)
+{
+    parameters.L1 = l1;
+    parameters.L2 = l2;
+    parameters.L3 = l3;
+    parameters.L4 = l4;
+}
+
+bool FourBarMechanism::calculatePointC(const Vector2D &A, const Vector2D &D,
+                                       const Vector2D &B, double l2, double l3,
+                                       Vector2D &C) const
+{
+    C = Vector2D::Zero();
+
+    Vector2D BD = D - B;
+    double d = BD.Length();
+
+    // 检查三角形不等式
+    if (d > l2 + l3 || d < std::abs(l2 - l3))
+        return false;
+
+    // 计算C点位置（几何法）
+    double a = (l2 * l2 - l3 * l3 + d * d) / (2.0 * d);
+    double h_sq = l2 * l2 - a * a;
+
+    if (h_sq < 0)
+        return false;
+
+    double h = std::sqrt(h_sq);
+
+    // 单位向量
+    Vector2D u = BD * (1.0 / d); // u = BD / d
+    Vector2D v(-u.Y, u.X);       // 垂直向量
+
+    // 两个可能的C点
+    Vector2D C1 = B + u * a + v * h;
+    Vector2D C2 = B + u * a - v * h;
+
+    // 选择正确的解（基于连续性）
+    if (trajectoryC.empty())
+    {
+        // 首次计算，根据输入角度选择
+        C = (parameters.InputValue <= 180.0) ? C1 : C2;
+    }
+    else
+    {
+        // 选择与上一个位置最接近的解
+        Vector2D lastC = trajectoryC.back();
+        double dist1 = Vector2D::Distance(C1, lastC);
+        double dist2 = Vector2D::Distance(C2, lastC);
+        C = (dist1 < dist2) ? C1 : C2;
+    }
+
+    return true;
+}
+
+bool FourBarMechanism::checkGrashofCondition() const
+{
+    double lengths[4] = {parameters.L1, parameters.L2, parameters.L3, parameters.L4};
+    std::sort(lengths, lengths + 4);
+
+    double s = lengths[0];
+    double l = lengths[3];
+    double p = lengths[1];
+    double q = lengths[2];
+
+    // Grashof条件：最短杆+最长杆 ≤ 其他两杆之和
+    return (s + l) <= (p + q);
+}
+
+bool FourBarMechanism::isShortestLinkCrank() const
+{
+    double lengths[4] = {parameters.L1, parameters.L2, parameters.L3, parameters.L4};
+    double minLength = *std::min_element(lengths, lengths + 4);
+
+    // 判断最短杆是否为L1（曲柄）
+    return std::abs(minLength - parameters.L1) < 1e-10;
+}
+
+MechanismState FourBarMechanism::calculate(double angleDeg)
+{
+    parameters.InputValue = angleDeg;
+    double angle = degreesToRadians(angleDeg);
+
+    double l1 = parameters.L1;
+    double l2 = parameters.L2;
+    double l3 = parameters.L3;
+    double l4 = parameters.L4;
+
+    Vector2D A(0.0, 0.0);
+    Vector2D D(l4, 0.0);
+
+    MechanismState state;
+    state.InputValue = angleDeg;
+
+    try
+    {
+        // 验证Grashof条件
+        if (!checkGrashofCondition())
+        {
+            state.ErrorMessage = "不满足四杆机构装配条件: s + l > p + q";
+            return state;
+        }
+
+        // 计算B点位置（曲柄末端）
+        Vector2D B = A + Vector2D(
+                             l1 * std::cos(angle),
+                             l1 * std::sin(angle));
+
+        // 计算C点位置（连杆末端）
+        Vector2D C;
+        if (!calculatePointC(A, D, B, l2, l3, C))
+        {
+            state.ErrorMessage = "无法找到连杆位置，机构可能无法装配";
+            return state;
+        }
+
+        // 记录轨迹点
+        trajectoryB.push_back(B);
+        trajectoryC.push_back(C);
+        crankCirclePoints.push_back(B);
+
+        // 限制轨迹点数量
+        const size_t MAX_TRAJECTORY_POINTS = 500;
+        if (trajectoryB.size() > MAX_TRAJECTORY_POINTS)
+        {
+            trajectoryB.erase(trajectoryB.begin());
+            trajectoryC.erase(trajectoryC.begin());
+            crankCirclePoints.erase(crankCirclePoints.begin());
+        }
+
+        // 更新帧计数
+        frameCount++;
+
+        // 计算摇杆角度
+        Vector2D CD = D - C;
+        double rockerAngle = std::atan2(CD.Y, CD.X);
+
+        // 设置状态
+        state.Points["A"] = A;
+        state.Points["B"] = B;
+        state.Points["C"] = C;
+        state.Points["D"] = D;
+
+        // 计算各杆的姿态
+        state.Poses["AB"] = PoseCalculator::CalculatePoseAndQuaternion(A, B);
+        state.Poses["BC"] = PoseCalculator::CalculatePoseAndQuaternion(B, C);
+        state.Poses["CD"] = PoseCalculator::CalculatePoseAndQuaternion(C, D);
+
+        // 角度信息
+        state.Angles["曲柄角度"] = std::fmod(angleDeg, 360.0);
+        state.Angles["摇杆角度"] = radiansToDegrees(rockerAngle);
+
+        // 检查最短杆是否为曲柄
+        if (!isShortestLinkCrank())
+        {
+            state.WarningMessage = "警告: 最短杆不是曲柄，可能无法做整周旋转";
+        }
+
+        return state;
+    }
+    catch (const std::exception &ex)
+    {
+        state.ErrorMessage = std::string("计算失败: ") + ex.what();
+        return state;
+    }
+}
+
+ValidationResult FourBarMechanism::validateParameters()
+{
+    ValidationResult result;
+
+    // 检查杆件长度
+    if (parameters.L1 <= 0.0)
+        result.Errors.push_back("L1（曲柄）长度必须大于0");
+    if (parameters.L2 <= 0.0)
+        result.Errors.push_back("L2（连杆）长度必须大于0");
+    if (parameters.L3 <= 0.0)
+        result.Errors.push_back("L3（摇杆）长度必须大于0");
+    if (parameters.L4 <= 0.0)
+        result.Errors.push_back("L4（机架）长度必须大于0");
+
+    // 检查Grashof条件
+    if (!checkGrashofCondition())
+    {
+        result.Errors.push_back("不满足四杆机构装配条件: s + l > p + q");
+    }
+
+    // 检查最短杆是否为曲柄
+    if (!isShortestLinkCrank())
+    {
+        result.Warnings.push_back("警告: 最短杆不是曲柄，可能无法做整周旋转");
+    }
+
+    return result;
+}
+
+std::pair<double, double> FourBarMechanism::getInputRange()
+{
+    return std::make_pair(0.0, 360.0);
+}
+
+std::vector<Vector2D> FourBarMechanism::getTrajectoryPoints() const
+{
+    return trajectoryB;
+}
+
+std::vector<Vector2D> FourBarMechanism::getTrajectoryC() const
+{
+    return trajectoryC;
+}
+
+std::vector<Vector2D> FourBarMechanism::getCrankCirclePoints() const
+{
+    return crankCirclePoints;
+}
+
+void FourBarMechanism::clearTrajectory()
+{
+    trajectoryB.clear();
+    trajectoryC.clear();
+    crankCirclePoints.clear();
+    frameCount = 0;
+    startTime = std::chrono::steady_clock::now();
+}
+
+std::string FourBarMechanism::getStatusText()
+{
+    auto now = std::chrono::steady_clock::now();
+    std::chrono::duration<double> elapsed = now - startTime;
+    double runTime = elapsed.count();
+    double fps = (runTime > 0.0) ? frameCount / runTime : 0.0;
+
+    std::ostringstream oss;
+    oss << name << "\n"
+        << "L1=" << formatDouble(parameters.L1) << "m, L2=" << formatDouble(parameters.L2) << "m\n"
+        << "L3=" << formatDouble(parameters.L3) << "m, L4=" << formatDouble(parameters.L4) << "m\n"
+        << "角速度: " << formatDouble(parameters.AngularVelocity, 1) << "rad/s\n"
+        << "运行时间: " << formatDouble(runTime, 1) << "s\n"
+        << "帧率: " << formatDouble(fps, 1) << " FPS\n"
+        << "轨迹点数: " << trajectoryB.size() << "/500";
+
+    return oss.str();
+}
+
+double FourBarMechanism::getL1() const { return parameters.L1; }
+double FourBarMechanism::getL2() const { return parameters.L2; }
+double FourBarMechanism::getL3() const { return parameters.L3; }
+double FourBarMechanism::getL4() const { return parameters.L4; }
+
+// ==================== 工厂函数实现 ====================
+
+FourBarMechanism *createFourBarMechanism()
+{
+    return new FourBarMechanism();
+}
+
+void deleteFourBarMechanism(FourBarMechanism *mechanism)
+{
+    delete mechanism;
+}
+
+FourBarMechanismPtr createFourBarMechanismSmart()
+{
+    return std::make_shared<FourBarMechanism>();
+}

src/FourBarMechanism/SliderCrankMechanism.cpp

@@ -0,0 +1,374 @@
+// SliderCrankMechanism.cpp
+#include "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 &params)
+{
+    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>();
+}

src/KinematicsWebAPI.cpp

@@ -0,0 +1,605 @@
+
+#include "KinematicsHelper.h"
+#include "KinematicsWebAPI.h"
+#include "CrankSliderMechanism.h"
+#include "URDFStrings.h"
+
+#include <iostream>
+#include <thread>
+#include <chrono>
+#include <iomanip>
+#include <sstream>
+#include <codecvt>
+#include <locale>
+#include <csignal>
+
+// inversePoseStr2PSteps
+KinematicsWebAPI::KinematicsWebAPI()
+{
+  init_func();
+}
+
+std::string KinematicsWebAPI::init_func()
+{
+  std::string response_string;
+  std::string urdf_string = URDFStrings::abb120_urdf; // 使用新的命名空间访问
+  auto result = RobotManager::initRobot(urdf_string, "9D7EAEF4-1AAB-499E-8783-B6CE016BC6D1");
+  auto result1 = RobotManager::initRobot(urdf_string, "abb_irb120_3_58");
+
+  json error_response = utils::create_api_response(true, 200, "init_func", "", "", "");
+  response_string = error_response.dump();
+  return response_string;
+}
+
+std::string KinematicsWebAPI::func(std::string sanitized_body)
+{
+
+  // 添加调试输出
+  log("func called with body: " + sanitized_body.substr(0, 100));
+
+  assert(!sanitized_body.empty() && "sanitized_body should not be empty");
+  std::string response_string;
+
+  try
+  {
+    json request_json;
+    // 构建响应数据
+    json result;
+    try
+    {
+      request_json = json::parse(sanitized_body);
+    }
+    catch (const std::exception &e)
+    {
+      json error_response = utils::create_api_response(false, 400, "Invalid JSON format", "", "", "");
+      response_string = error_response.dump();
+      return response_string;
+    }
+
+    std::string msg = request_json.value("msg", "");
+    std::string req_code = request_json.value("req_code", "");
+    std::string req_from = request_json.value("req_from", "");
+    std::string req_cmd = request_json.value("req_cmd", "");
+    json req_param = request_json.value("req_param", json::object());
+
+    json res_data;
+
+    // 使用if语句直接处理对应的命令，不通过中间函数
+    if (req_cmd == "Cmd_Kinematics_inverse_pose_str")
+    {
+      try
+      {
+        log("Handling Cmd_Kinematics_inverse_pose_str command");
+
+        std::string pose_str = req_param.value("pose_str", "");
+        std::string q_init_str = req_param.value("q_init_str", "0,0,0,0,0,0");
+        std::string robot_uuid = req_param.value("robot_uuid", "default");
+
+        auto robot = RobotManager::getRobot(robot_uuid);
+        if (!robot || !robot->isInitialized())
+        {
+          res_data = {{"error", "Robot not found or not initialized"}};
+        }
+        else
+        {
+          result = robot->inversePoseStr(pose_str, q_init_str);
+          if (result.empty())
+          {
+            res_data = {{"error", "Inverse kinematics calculation failed"}};
+          }
+          else
+          {
+            res_data = {{"joints", result}, {"success", true}};
+          }
+        }
+      }
+      catch (const std::exception &e)
+      {
+        res_data = {{"error", "Failed to calculate inverse kinematics: " + std::string(e.what())}};
+      }
+    }
+    else if (req_cmd == "Cmd_Kinematics_inverse_pose_str_2PSteps")
+    {
+      try
+      {
+        log("Handling Cmd_Kinematics_inverse_pose_str_2PSteps command");
+
+        std::string pose_str = req_param.value("pose_str", "");
+        std::string q_init_str = req_param.value("q_init_str", "0,0,0,0,0,0");
+        std::string robot_uuid = req_param.value("robot_uuid", "default");
+        std::string steps_str = req_param.value("steps_str", "default");
+        int steps = std::stoi(steps_str);
+        auto robot = RobotManager::getRobot(robot_uuid);
+        if (!robot || !robot->isInitialized())
+        {
+          res_data = {{"error", "Robot not found or not initialized"}};
+        }
+        else
+        {
+          result = robot->inversePoseStr2PSteps(pose_str, q_init_str, steps);
+          if (result.empty())
+          {
+            res_data = {{"error", "Inverse kinematics calculation failed"}};
+          }
+          else
+          {
+            res_data = {{"joints", result}, {"success", true}};
+          }
+        }
+      }
+      catch (const std::exception &e)
+      {
+        res_data = {{"error", "Failed to calculate inverse kinematics: " + std::string(e.what())}};
+      }
+    }
+    else if (req_cmd == "Cmd_Kinematics_inverse_pose_str_NoDifference")
+    {
+      try
+      {
+        log("Handling Cmd_Kinematics_inverse_pose_strNoDifference command");
+
+        std::string pose_str = req_param.value("pose_str", "");
+        std::string q_init_str = req_param.value("q_init_str", "0,0,0,0,0,0");
+        std::string robot_uuid = req_param.value("robot_uuid", "default");
+
+        auto robot = RobotManager::getRobot(robot_uuid);
+        if (!robot || !robot->isInitialized())
+        {
+          res_data = {{"error", "Robot not found or not initialized"}};
+        }
+        else
+        {
+          result = robot->inversePoseStrNoDifference(pose_str, q_init_str);
+          if (result.empty())
+          {
+            res_data = {{"error", "Inverse kinematics calculation failed"}};
+          }
+          else
+          {
+            res_data = {{"joints", result}, {"success", true}};
+          }
+        }
+      }
+      catch (const std::exception &e)
+      {
+        res_data = {{"error", "Failed to calculate inverse kinematics: " + std::string(e.what())}};
+      }
+    }
+    else if (req_cmd == "Cmd_Kinematics_forward_pose_str")
+    {
+      try
+      {
+        log("Handling Cmd_Kinematics_forward_pose_str command");
+
+        std::string joints_str = req_param.value("q_init_str", "0,0,0,0,0,0");
+        std::string robot_uuid = req_param.value("robot_uuid", "default");
+
+        auto robot = RobotManager::getRobot(robot_uuid);
+        if (!robot || !robot->isInitialized())
+        {
+          res_data = {{"error", "Robot not found or not initialized"}};
+        }
+        else
+        {
+          auto joints = robot->parseJointString(joints_str);
+          if (joints.size() != 6)
+          {
+            res_data = {{"error", "Invalid joints format"}};
+          }
+          else
+          {
+            double tcp_pose[7];
+            if (robot->calculateFK_TCP(joints.data(), tcp_pose))
+            {
+              res_data = {
+                  {"position", {tcp_pose[0], tcp_pose[1], tcp_pose[2]}},
+                  {"orientation", {tcp_pose[3], tcp_pose[4], tcp_pose[5], tcp_pose[6]}},
+                  {"joints", joints},
+                  {"success", true}};
+            }
+            else
+            {
+              res_data = {{"error", "Forward kinematics calculation failed"}};
+            }
+          }
+        }
+      }
+      catch (const std::exception &e)
+      {
+        res_data = {{"error", "Failed to calculate forward kinematics: " + std::string(e.what())}};
+      }
+    }
+    else if (req_cmd == "Cmd_SelectCraftTree")
+    {
+      try
+      {
+        log("Handling Cmd_SelectCraftTree command");
+        std::string tree_id = req_param.value("tree_id", "");
+        res_data = {
+            {"success", true},
+            {"tree_id", tree_id},
+            {"message", "Craft tree selected successfully"},
+            {"timestamp", utils::get_current_time()}};
+      }
+      catch (const std::exception &e)
+      {
+        res_data = {{"error", "Failed to select craft tree: " + std::string(e.what())}};
+      }
+    }
+    else if (req_cmd == "Cmd_AddOperationTree")
+    {
+      try
+      {
+        log("Handling Cmd_AddOperationTree command");
+        std::string tree_name = req_param.value("name", "");
+        json operations = req_param.value("operations", json::array());
+
+        res_data = {
+            {"success", true},
+            {"tree_name", tree_name},
+            {"operations_count", operations.size()},
+            {"message", "Operation tree added successfully"},
+            {"timestamp", utils::get_current_time()}};
+      }
+      catch (const std::exception &e)
+      {
+        res_data = {{"error", "Failed to add operation tree: " + std::string(e.what())}};
+      }
+    }
+    else if (req_cmd == "Cmd_InitRobot")
+    {
+      try
+      {
+        log("Handling Cmd_InitRobot command");
+        std::string urdf_base64 = req_param.value("urdf_base64", "");
+        std::string uuid = req_param.value("robot_uuid", "");
+        bool force_update = req_param.value("force_update", true);
+
+        std::string urdf_content = utils::base64_to_urdf(urdf_base64);
+
+        if (!utils::validate_urdf_base64(urdf_base64))
+        {
+          res_data = {
+              {"success", false},
+              {"message", "Invalid URDF format"},
+              {"timestamp", utils::get_current_time()}};
+        }
+        else
+        {
+          auto result = RobotManager::initRobot(urdf_content, uuid, force_update);
+          res_data = {
+              {"success", result.first},
+              {"message", result.second},
+              {"timestamp", utils::get_current_time()}};
+        }
+      }
+      catch (const std::exception &e)
+      {
+        res_data = {{"error", "Failed to initialize robot: " + std::string(e.what())}};
+      }
+    }
+    else if (req_cmd == "Cmd_GetRobot")
+    {
+      try
+      {
+        log("Handling Cmd_GetRobot command");
+
+        std::string uuid = req_param.value("uuid", "");
+        auto robot = RobotManager::getRobot(uuid);
+
+        if (!robot)
+        {
+          res_data = {{"error", "Robot not found"}};
+        }
+        else
+        {
+          res_data = {
+              {"success", true},
+              {"uuid", uuid},
+              {"initialized", robot->isInitialized()},
+              {"joints_count", robot->getNumberOfJoints()},
+              {"timestamp", utils::get_current_time()}};
+        }
+      }
+      catch (const std::exception &e)
+      {
+        res_data = {{"error", "Failed to get robot: " + std::string(e.what())}};
+      }
+    }
+    else if (req_cmd == "Cmd_RemoveRobot")
+    {
+      try
+      {
+        log("Handling Cmd_RemoveRobot command");
+
+        std::string uuid = req_param.value("uuid", "");
+        auto result = RobotManager::removeRobot(uuid);
+
+        res_data = {
+            {"success", result.first},
+            {"message", result.second},
+            {"timestamp", utils::get_current_time()}};
+      }
+      catch (const std::exception &e)
+      {
+        res_data = {{"error", "Failed to remove robot: " + std::string(e.what())}};
+      }
+    }
+    else if (req_cmd == "Cmd_ListRobots")
+    {
+      try
+      {
+        log("Handling Cmd_ListRobots command");
+
+        bool detail = req_param.value("detail", false);
+        auto robots = RobotManager::listRobots(detail);
+
+        res_data = {
+            {"success", true},
+            {"robots", robots},
+            {"count", robots.size()},
+            {"timestamp", utils::get_current_time()}};
+      }
+      catch (const std::exception &e)
+      {
+        res_data = {{"error", "Failed to list robots: " + std::string(e.what())}};
+      }
+    }
+    else if (req_cmd == "Cmd_Kinematics_forward_all_joints")
+    {
+      try
+      {
+        log("Handling Cmd_Kinematics_forward_all_joints command");
+
+        std::string joints_str = req_param.value("joints_str", "0,0,0,0,0,0");
+        std::string robot_uuid = req_param.value("robot_uuid", "default");
+
+        auto robot = RobotManager::getRobot(robot_uuid);
+        if (!robot || !robot->isInitialized())
+        {
+          res_data = {{"error", "Robot not found or not initialized"}};
+        }
+        else
+        {
+          auto joints_poses_array = robot->handleKinematicsForwardAllJoints(joints_str);
+          res_data = {
+              {"OPERATION", joints_poses_array},
+              {"success", true}};
+        }
+      }
+      catch (const std::exception &e)
+      {
+        res_data = {{"error", "Failed to calculate forward kinematics for all joints: " + std::string(e.what())}};
+      }
+    }
+    else if (req_cmd == "Cmd_Spc")
+    {
+      try
+      {
+        log("Handling " + req_cmd + " command");
+        json result = SpcCalculator::Spc(req_param);
+        res_data = result;
+      }
+      catch (const std::exception &e)
+      {
+        res_data = {{"error", "Failed to Cmd_Spc: " + std::string(e.what())}};
+      }
+    }
+    else if (req_cmd == "Cmd_FourBar_CrankSlider")
+    {
+      // 请求参数示例:
+      // {
+      //   "L_AB": 0.5,
+      //   "L_BS": 2.0,
+      //   "S_OFS": 0.0,
+      //   "angleDeg": 45.0
+      // }
+      try
+      {
+        log("Handling " + req_cmd + " command");
+
+        // 从请求参数中提取值
+        double L_AB = req_param.value("L_AB", 0.5);
+        double L_BS = req_param.value("L_BS", 2.0);
+        double S_OFS = req_param.value("S_OFS", 0.0);
+        double angleDeg = req_param.value("angleDeg", 0.0);
+
+        log("Parameters: L_AB=" + std::to_string(L_AB) +
+            ", L_BS=" + std::to_string(L_BS) +
+            ", S_OFS=" + std::to_string(S_OFS) +
+            ", angleDeg=" + std::to_string(angleDeg));
+
+        // 创建曲柄滑块机构实例
+        std::unique_ptr<CrankSliderMechanism> mechanism(createCrankSliderMechanism());
+
+        // 设置连杆参数
+        mechanism->setL_AB(L_AB);
+        mechanism->setL_BS(L_BS);
+        mechanism->setS_OFS(S_OFS);
+
+        // 验证参数
+        ValidationResult validation = mechanism->validateParameters();
+        if (!validation.isValid())
+        {
+          res_data = {
+              {"success", false},
+              {"error", "Invalid parameters"},
+              {"validation_errors", validation.Errors},
+              {"validation_warnings", validation.Warnings}};
+          return res_data;
+        }
+
+        // 执行计算
+        MechanismState state = mechanism->calculate(angleDeg);
+
+        // 检查是否有错误
+        if (state.hasError())
+        {
+          res_data = {
+              {"success", false},
+              {"error", state.ErrorMessage}};
+          return res_data;
+        }
+
+        // 构建响应数据
+        json result;
+        result["success"] = true;
+
+        // 添加点坐标
+        json points_json;
+        for (const auto &point_pair : state.Points)
+        {
+          json point;
+          point["x"] = point_pair.second.X;
+          point["y"] = point_pair.second.Y;
+          points_json[point_pair.first] = point;
+        }
+        result["points"] = points_json;
+
+        // 添加姿态信息
+        json poses_json;
+        for (const auto &pose_pair : state.Poses)
+        {
+          json pose;
+          pose["tx"] = pose_pair.second.tx;
+          pose["ty"] = pose_pair.second.ty;
+          pose["tz"] = pose_pair.second.tz;
+          pose["qx"] = pose_pair.second.qx;
+          pose["qy"] = pose_pair.second.qy;
+          pose["qz"] = pose_pair.second.qz;
+          pose["qw"] = pose_pair.second.qw;
+          poses_json[pose_pair.first] = pose;
+        }
+        result["poses"] = poses_json;
+
+        // 添加角度信息
+        json angles_json;
+        for (const auto &angle_pair : state.Angles)
+        {
+          angles_json[angle_pair.first] = angle_pair.second;
+        }
+        result["angles"] = angles_json;
+
+        // 添加输入值
+        result["input_value"] = state.InputValue;
+
+        // 添加警告信息（如果有）
+        if (state.hasWarning())
+        {
+          result["warning"] = state.WarningMessage;
+        }
+
+        // 添加轨迹信息
+        std::vector<Vector2D> trajectory = mechanism->getTrajectoryPoints();
+        std::vector<Vector2D> slider_trajectory = mechanism->getSliderTrajectory();
+
+        json trajectory_json;
+        for (size_t i = 0; i < trajectory.size(); i++)
+        {
+          json point;
+          point["x"] = trajectory[i].X;
+          point["y"] = trajectory[i].Y;
+          trajectory_json.push_back(point);
+        }
+        result["trajectory"] = trajectory_json;
+
+        json slider_trajectory_json;
+        for (size_t i = 0; i < slider_trajectory.size(); i++)
+        {
+          json point;
+          point["x"] = slider_trajectory[i].X;
+          point["y"] = slider_trajectory[i].Y;
+          slider_trajectory_json.push_back(point);
+        }
+        result["slider_trajectory"] = slider_trajectory_json;
+
+        // 添加参数信息
+        result["parameters"] = {
+            {"L_AB", L_AB},
+            {"L_BS", L_BS},
+            {"S_OFS", S_OFS},
+            {"angleDeg", angleDeg}};
+
+        res_data = result;
+      }
+      catch (const std::exception &e)
+      {
+        res_data = {{"success", false, "error", "Failed to Cmd_FourBar_CrankSlider: " + std::string(e.what())}};
+      }
+    }
+    else if (req_cmd == "Cmd_QuadrupedRobot_CalculateAllPointsFromMotorAngles")
+    {
+
+      // json j = json::parse(jsonStr);
+      std::cout << "[DEBUG 0001] Cmd_QuadrupedRobot_CalculateAllPointsFromMotorAngles 步骤1: 初始化数据结构" << std::endl;
+      std::string jsonInput = req_param.dump();
+      result = KinematicsHelper::QuadrupedRobot_CalculateAllPointsFromMotorAngles(jsonInput);
+      // if (req_param.empty())
+      // {
+      //   result = KinematicsHelper::QuadrupedRobot_CalculateAllPointsFromMotorAngles();
+      // }
+      // else
+      // {
+      //   std::string jsonInput = req_param.dump();
+      //   result = KinematicsHelper::QuadrupedRobot_CalculateAllPointsFromMotorAngles(jsonInput);
+      // }
+
+      res_data = result;
+    }
+    else if (req_cmd == "Cmd_QuadrupedRobot_PerformForwardKinematics")
+    {
+      std::cout << "[DEBUG] Cmd_QuadrupedRobot_PerformForwardKinematics = " << std::endl;
+      std::string jsonInput = req_param.dump();
+      result = KinematicsHelper::QuadrupedRobot_PerformForwardKinematics(jsonInput);
+      // if (req_param.empty())
+      // {
+      //   result = KinematicsHelper::QuadrupedRobot_PerformForwardKinematics();
+      // }
+      // else
+      // {
+      //   std::string jsonInput = req_param.dump();
+      //   result = KinematicsHelper::QuadrupedRobot_PerformForwardKinematics(jsonInput);
+      // }
+
+      res_data = result;
+    }
+    else
+    {
+      // 未知命令，调用默认处理函数
+      res_data["success"] = false;
+      res_data["error"] = "Unknown command: " + req_cmd;
+      res_data["received_params"] = req_param;
+      res_data["timestamp"] = std::time(nullptr);
+    }
+
+    json response = utils::create_api_response(true, 0, msg, req_code, req_from, req_cmd, res_data);
+    response_string = response.dump();
+  }
+  catch (const std::exception &e)
+  {
+    json error_response = utils::create_api_response(false, 500, "Processing error: " + std::string(e.what()), "", "", "");
+    response_string = error_response.dump();
+  }
+
+  return response_string;
+}
+
+void KinematicsWebAPI::log(const std::string &message)
+{
+  std::cout << "[" << getCurrentTimestamp() << "] " << message << std::endl;
+
+  if (onLog)
+  {
+    onLog("[" + getCurrentTimestamp() + "] " + message);
+  }
+}
+
+std::string KinematicsWebAPI::getCurrentTimestamp()
+{
+  return utils::get_current_timestamp();
+}
+
+bool KinematicsWebAPI::is_running() const
+{
+  return running_;
+}

src/QuadrupedRobotSimulation/CompleteJsonExporter.cpp

@@ -0,0 +1,714 @@
+// CompleteJsonExporter.cpp
+#include "CompleteJsonExporter.h"
+#include "BaseClass.h"
+#include "KinematicsSimulation.h"
+#include <algorithm>
+#include <cmath>
+#include <functional>
+#include <sstream>
+#include <iomanip>
+#include <limits>
+
+// 主导出函数
+std::string CompleteJsonExporter::ExportCompleteDataJsonString(const QuadrupedRobotSimulation &simulation)
+{
+    try
+    {
+        // 创建数据对象
+        CompleteExportData exportData;
+
+        // 基本信息
+        exportData.GaitInfo = CreateGaitInfo(simulation);
+
+        // 系统参数
+        exportData.SystemParameters = CreateSystemParameters(simulation);
+
+        // 轨迹数据（所有腿）
+        exportData.TrajectoryData = CreateTrajectoryData(simulation);
+
+        // 电机数据（所有腿）
+        exportData.MotorData = CreateMotorData(simulation);
+
+        // 约束检查数据
+        exportData.ConstraintData = CreateConstraintData(simulation);
+
+        // 统计信息
+        exportData.Statistics = CreateStatistics(simulation);
+
+        // 初始角度数据
+        exportData.InitialAngles = CreateInitialAngles(simulation);
+
+        // 相位信息
+        exportData.PhaseInfo = CreatePhaseInfo(simulation);
+
+        // 运动范围统计
+        exportData.MotionRange = CreateMotionRange(simulation);
+
+        // 序列化为JSON
+        json j = exportData;
+        return j.dump(2); // 缩进2个空格
+    }
+    catch (const std::exception &ex)
+    {
+        std::cerr << "导出JSON数据时出错: " << ex.what() << std::endl;
+        throw;
+    }
+}
+
+// 创建步态信息
+GaitInfo CompleteJsonExporter::CreateGaitInfo(const QuadrupedRobotSimulation &simulation)
+{
+    GaitInfo gaitInfo;
+    gaitInfo.GaitType = simulation.gait_type;
+    gaitInfo.Period = simulation.T;
+    gaitInfo.Frequency = simulation.f;
+    gaitInfo.StepTime = simulation.step;
+    gaitInfo.SupportTime = simulation.t_s;
+    gaitInfo.SwingTime = simulation.t_w;
+    gaitInfo.SupportRatio = simulation.support_ratio;
+    gaitInfo.SwingRatio = simulation.swing_ratio;
+    gaitInfo.TotalFrames = simulation.Trajectory_L.size();
+    gaitInfo.TotalTime = simulation.Trajectory_L.size() * simulation.step;
+
+    return gaitInfo;
+}
+
+// 创建系统参数
+SystemParameters CompleteJsonExporter::CreateSystemParameters(const QuadrupedRobotSimulation &simulation)
+{
+    SystemParameters systemParams;
+
+    // 基本参数
+    systemParams.L10 = simulation.L10;
+    systemParams.L20 = simulation.L20;
+    systemParams.L30 = simulation.L30;
+
+    // 步态设计参数
+    systemParams.StepLength = simulation.s_l;
+    systemParams.StepHeight = simulation.s_h;
+    systemParams.X = simulation.X;
+    systemParams.DeltaX = simulation.deltX;
+
+    // 小腿连杆参数
+    systemParams.L21 = simulation.L21;
+    systemParams.L22 = simulation.L22;
+    systemParams.L23 = simulation.L23;
+    systemParams.Beta1 = simulation.beta1;
+    systemParams.BB2_BC_Angle = simulation.BB2_BC_angle;
+    systemParams.C1_B_Length = simulation.C1_B_length;
+
+    // 脚踝连杆参数
+    systemParams.CC1 = simulation.CC1;
+    systemParams.L31 = simulation.L31;
+    systemParams.C2C3_Length = simulation.C2C3_length;
+    systemParams.Lead = simulation.lead1;
+
+    // 脚部构型参数
+    systemParams.D1_L_Offset = simulation.D1_L_offset;
+    systemParams.D2_L_Offset = simulation.D2_L_offset;
+    systemParams.C4_D2_Offset = simulation.C4_D2_offset;
+
+    // 电机参数
+    systemParams.ThighMotorReduction = simulation.thigh_motor_reduction;
+    systemParams.ShankMotorReduction = simulation.shank_motor_reduction;
+    systemParams.AnkleMotorReduction = simulation.ankle_motor_reduction;
+
+    return systemParams;
+}
+
+// 创建轨迹数据
+TrajectoryData CompleteJsonExporter::CreateTrajectoryData(const QuadrupedRobotSimulation &simulation)
+{
+    int n_frames = simulation.Trajectory_L.size();
+
+    // 计算相位偏移帧数
+    int phase_LH = static_cast<int>(simulation.timeLH * n_frames);
+    int phase_RF = static_cast<int>(simulation.timeRF * n_frames);
+    int phase_RH = static_cast<int>(simulation.timeRH * n_frames);
+
+    TrajectoryData trajectoryData;
+    trajectoryData.TotalFrames = n_frames;
+    trajectoryData.StepTime = simulation.step;
+
+    // 为每条腿创建轨迹数据
+    std::vector<std::string> legCodes = {"LF", "LH", "RF", "RH"};
+    std::vector<int> phaseShifts = {0, phase_LH, phase_RF, phase_RH};
+
+    for (size_t i = 0; i < legCodes.size(); i++)
+    {
+        std::string legCode = legCodes[i];
+        int phaseShift = phaseShifts[i];
+        bool isRightLeg = legCode[0] == 'R'; // 检查是否以'R'开头
+
+        LegTrajectory legTraj;
+        legTraj.LegCode = legCode;
+        legTraj.PhaseShift = phaseShift;
+        legTraj.IsRightLeg = isRightLeg;
+
+        for (int frame = 0; frame < n_frames; frame++)
+        {
+            int shiftedIdx = (frame + phaseShift) % n_frames;
+
+            FrameData frameData;
+            frameData.FrameNumber = frame;
+            frameData.Time = frame * simulation.step;
+            frameData.ShiftedFrameNumber = shiftedIdx;
+
+            // 关键点坐标
+            frameData.A_Point = Point2D(simulation.A_point[0], simulation.A_point[1]);
+            frameData.B_Point = ArrayToPoint(simulation.Trajectory_B[shiftedIdx]);
+            frameData.C_Point = ArrayToPoint(simulation.Trajectory_C[shiftedIdx]);
+            frameData.L_Point = ArrayToPoint(simulation.Trajectory_L[shiftedIdx]);
+            frameData.D1_Point = ArrayToPoint(simulation.Trajectory_D1[shiftedIdx]);
+            frameData.D2_Point = ArrayToPoint(simulation.Trajectory_D2[shiftedIdx]);
+            frameData.C4_Point = ArrayToPoint(simulation.Trajectory_C4[shiftedIdx]);
+            frameData.B1_Point = ArrayToPoint(simulation.Trajectory_B1[shiftedIdx]);
+            frameData.B2_Point = ArrayToPoint(simulation.Trajectory_B2[shiftedIdx]);
+            frameData.C1_Point = ArrayToPoint(simulation.Trajectory_C1[shiftedIdx]);
+            frameData.C2_Point = ArrayToPoint(simulation.Trajectory_C2[shiftedIdx]);
+            frameData.C3_Point = ArrayToPoint(simulation.Trajectory_C3[shiftedIdx]);
+
+            // 角度数据
+            frameData.AB_Horizontal_Angle = simulation.AB_horizontal_angles[shiftedIdx];
+            frameData.AB_BC_Angle = simulation.AB_BC_angles[shiftedIdx];
+            frameData.BC_CL_Angle = simulation.BC_CL_angles[shiftedIdx];
+            frameData.AB1_AB_Angle = simulation.AB1_AB_angles[shiftedIdx];
+
+            // 距离数据
+            frameData.C3_C4_Distance = simulation.C3_C4_distances[shiftedIdx];
+            frameData.C2_C3_Distance = simulation.C2_C3_distances[shiftedIdx];
+            frameData.C2_C4_Distance = simulation.C2_C4_distances[shiftedIdx];
+            frameData.C2_C3_C4_Angle = simulation.C2_C3_C4_angles[shiftedIdx];
+
+            // 如果是右侧腿，调整大腿角度（镜像变换）
+            if (isRightLeg)
+            {
+                frameData.AB_Horizontal_Angle = 540 - frameData.AB_Horizontal_Angle;
+                // 确保角度在0-360度范围内
+                while (frameData.AB_Horizontal_Angle >= 360)
+                {
+                    frameData.AB_Horizontal_Angle -= 360;
+                }
+                while (frameData.AB_Horizontal_Angle < 0)
+                {
+                    frameData.AB_Horizontal_Angle += 360;
+                }
+            }
+
+            legTraj.Frames.push_back(frameData);
+        }
+
+        trajectoryData.Legs[legCode] = legTraj;
+    }
+
+    return trajectoryData;
+}
+
+// 创建电机数据
+MotorDataExport CompleteJsonExporter::CreateMotorData(const QuadrupedRobotSimulation &simulation)
+{
+    MotorDataExport motorData;
+    motorData.StepTime = simulation.step;
+
+    // 获取四条腿的电机数据
+    std::map<std::string, MotorData> legsData = {
+        {"LF", simulation.motor_data_LF},
+        {"LH", simulation.motor_data_LH},
+        {"RF", simulation.motor_data_RF},
+        {"RH", simulation.motor_data_RH}};
+
+    for (const auto &kvp : legsData)
+    {
+        std::string legCode = kvp.first;
+        const MotorData &data = kvp.second;
+
+        LegMotorData legMotorData;
+        legMotorData.LegCode = legCode;
+
+        int n_frames = data.frame_start_times.size();
+        for (int i = 0; i < n_frames; i++)
+        {
+            MotorFrameData frameData;
+            frameData.FrameNumber = i;
+            frameData.StartTime = data.frame_start_times[i];
+            frameData.EndTime = data.frame_end_times[i];
+
+            // 大腿电机数据
+            frameData.Thigh.Angle = data.AB_angles[i];
+            frameData.Thigh.AngleIncrement = data.AB_angle_increments[i];
+            frameData.Thigh.Speed = data.thigh_motor_speeds[i];
+
+            // 小腿电机数据
+            frameData.Shank.Angle = data.AB1_AB_angles[i];
+            frameData.Shank.AngleIncrement = data.AB1_AB_angle_increments[i];
+            frameData.Shank.Speed = data.shank_motor_speeds[i];
+
+            // 脚踝电机数据
+            frameData.Ankle.Angle = data.ankle_motor_angles[i];
+            frameData.Ankle.AngleIncrement = data.ankle_motor_angle_increments[i];
+            frameData.Ankle.Speed = data.ankle_motor_speeds[i];
+
+            // 丝杠相关数据
+            frameData.C3C4_Distance = data.C3C4_distances[i];
+            frameData.C3C4_DistanceIncrement = data.C3C4_distance_increments[i];
+
+            legMotorData.Frames.push_back(frameData);
+        }
+
+        motorData.Legs[legCode] = legMotorData;
+    }
+
+    return motorData;
+}
+
+// 创建约束检查数据
+ConstraintData CompleteJsonExporter::CreateConstraintData(const QuadrupedRobotSimulation &simulation)
+{
+    const double AB1_AB_min = 23.0;
+    const double AB1_AB_max = 130.0;
+    const double C3_C4_min = 125.0;
+    const double C3_C4_max = 205.0;
+
+    ConstraintData constraintData;
+
+    // 计算AB1-AB角度的绝对值的最大最小值
+    double ab1_ab_min_actual = std::numeric_limits<double>::max();
+    double ab1_ab_max_actual = std::numeric_limits<double>::lowest();
+    bool ab1_ab_all_valid = true;
+
+    for (double angle : simulation.AB1_AB_angles)
+    {
+        double abs_angle = std::abs(angle);
+        ab1_ab_min_actual = std::min(ab1_ab_min_actual, abs_angle);
+        ab1_ab_max_actual = std::max(ab1_ab_max_actual, abs_angle);
+
+        if (abs_angle < AB1_AB_min || abs_angle > AB1_AB_max)
+        {
+            ab1_ab_all_valid = false;
+        }
+    }
+
+    // 计算C3-C4距离的最大最小值
+    double c3c4_min_actual = std::numeric_limits<double>::max();
+    double c3c4_max_actual = std::numeric_limits<double>::lowest();
+    bool c3c4_all_valid = true;
+
+    for (double distance : simulation.C3_C4_distances)
+    {
+        c3c4_min_actual = std::min(c3c4_min_actual, distance);
+        c3c4_max_actual = std::max(c3c4_max_actual, distance);
+
+        if (distance < C3_C4_min || distance > C3_C4_max)
+        {
+            c3c4_all_valid = false;
+        }
+    }
+
+    // 计算CL距离的最大最小值
+    double cl_min_actual = std::numeric_limits<double>::max();
+    double cl_max_actual = std::numeric_limits<double>::lowest();
+    bool cl_all_valid = true;
+
+    for (double distance : simulation.CL_distances)
+    {
+        cl_min_actual = std::min(cl_min_actual, distance);
+        cl_max_actual = std::max(cl_max_actual, distance);
+
+        if (std::abs(distance - simulation.L30) >= 1e-6)
+        {
+            cl_all_valid = false;
+        }
+    }
+
+    // 计算点积的最大最小值
+    double dot_min_actual = std::numeric_limits<double>::max();
+    double dot_max_actual = std::numeric_limits<double>::lowest();
+    bool dot_all_valid = true;
+
+    for (double dot_product : simulation.dot_products)
+    {
+        dot_min_actual = std::min(dot_min_actual, dot_product);
+        dot_max_actual = std::max(dot_max_actual, dot_product);
+
+        if (std::abs(dot_product) >= 1e-6)
+        {
+            dot_all_valid = false;
+        }
+    }
+
+    // 设置约束数据
+    constraintData.AB1_AB_Constraint.MinAllowed = AB1_AB_min;
+    constraintData.AB1_AB_Constraint.MaxAllowed = AB1_AB_max;
+    constraintData.AB1_AB_Constraint.MinActual = ab1_ab_min_actual;
+    constraintData.AB1_AB_Constraint.MaxActual = ab1_ab_max_actual;
+    constraintData.AB1_AB_Constraint.IsValid = ab1_ab_all_valid;
+    constraintData.AB1_AB_Constraint.CheckType = "绝对值检查";
+
+    constraintData.C3C4_Constraint.MinAllowed = C3_C4_min;
+    constraintData.C3C4_Constraint.MaxAllowed = C3_C4_max;
+    constraintData.C3C4_Constraint.MinActual = c3c4_min_actual;
+    constraintData.C3C4_Constraint.MaxActual = c3c4_max_actual;
+    constraintData.C3C4_Constraint.IsValid = c3c4_all_valid;
+
+    constraintData.CL_Distance_Constraint.MinAllowed = simulation.L30;
+    constraintData.CL_Distance_Constraint.MaxAllowed = simulation.L30;
+    constraintData.CL_Distance_Constraint.MinActual = cl_min_actual;
+    constraintData.CL_Distance_Constraint.MaxActual = cl_max_actual;
+    constraintData.CL_Distance_Constraint.IsValid = cl_all_valid;
+
+    constraintData.Perpendicularity_Constraint.MaxDotProduct = dot_max_actual;
+    constraintData.Perpendicularity_Constraint.MinDotProduct = dot_min_actual;
+    constraintData.Perpendicularity_Constraint.IsValid = dot_all_valid;
+
+    // 收集约束违反信息
+    std::vector<ConstraintViolation> violations;
+
+    // 检查AB1-AB约束违反
+    for (size_t i = 0; i < simulation.AB1_AB_angles.size(); i++)
+    {
+        double abs_angle = std::abs(simulation.AB1_AB_angles[i]);
+        if (abs_angle < AB1_AB_min || abs_angle > AB1_AB_max)
+        {
+            ConstraintViolation violation;
+            violation.ConstraintType = "AB1_AB_Angle";
+            violation.Frame = static_cast<int>(i);
+            violation.Time = i * simulation.step;
+            violation.Value = simulation.AB1_AB_angles[i];
+            violation.AllowedRange = DoubleToString(AB1_AB_min) + "~" + DoubleToString(AB1_AB_max);
+            violation.Severity = "Error";
+
+            violations.push_back(violation);
+        }
+    }
+
+    // 检查C3-C4约束违反
+    for (size_t i = 0; i < simulation.C3_C4_distances.size(); i++)
+    {
+        double distance = simulation.C3_C4_distances[i];
+        if (distance < C3_C4_min || distance > C3_C4_max)
+        {
+            ConstraintViolation violation;
+            violation.ConstraintType = "C3C4_Distance";
+            violation.Frame = static_cast<int>(i);
+            violation.Time = i * simulation.step;
+            violation.Value = distance;
+            violation.AllowedRange = DoubleToString(C3_C4_min) + "~" + DoubleToString(C3_C4_max);
+            violation.Severity = "Error";
+
+            violations.push_back(violation);
+        }
+    }
+
+    constraintData.ConstraintViolations = violations;
+
+    return constraintData;
+}
+
+// 创建统计信息
+StatisticsData CompleteJsonExporter::CreateStatistics(const QuadrupedRobotSimulation &simulation)
+{
+    StatisticsData statistics;
+
+    // 计算角度统计
+    double ab_horizontal_min = FindMin(simulation.AB_horizontal_angles);
+    double ab_horizontal_max = FindMax(simulation.AB_horizontal_angles);
+
+    double ab_bc_min = FindMin(simulation.AB_BC_angles);
+    double ab_bc_max = FindMax(simulation.AB_BC_angles);
+
+    double bc_cl_min = FindMin(simulation.BC_CL_angles);
+    double bc_cl_max = FindMax(simulation.BC_CL_angles);
+
+    double ab1_ab_min = FindMin(simulation.AB1_AB_angles);
+    double ab1_ab_max = FindMax(simulation.AB1_AB_angles);
+
+    // 角度统计
+    statistics.AngleStatistics.AB_Horizontal.Min = ab_horizontal_min;
+    statistics.AngleStatistics.AB_Horizontal.Max = ab_horizontal_max;
+    statistics.AngleStatistics.AB_Horizontal.Range = ab_horizontal_max - ab_horizontal_min;
+
+    statistics.AngleStatistics.AB_BC.Min = ab_bc_min;
+    statistics.AngleStatistics.AB_BC.Max = ab_bc_max;
+    statistics.AngleStatistics.AB_BC.Range = ab_bc_max - ab_bc_min;
+
+    statistics.AngleStatistics.BC_CL.Min = bc_cl_min;
+    statistics.AngleStatistics.BC_CL.Max = bc_cl_max;
+    statistics.AngleStatistics.BC_CL.Range = bc_cl_max - bc_cl_min;
+
+    statistics.AngleStatistics.AB1_AB.Min = ab1_ab_min;
+    statistics.AngleStatistics.AB1_AB.Max = ab1_ab_max;
+    statistics.AngleStatistics.AB1_AB.Range = ab1_ab_max - ab1_ab_min;
+
+    // 计算距离统计
+    double c3c4_min = FindMin(simulation.C3_C4_distances);
+    double c3c4_max = FindMax(simulation.C3_C4_distances);
+
+    double c2c3_min = FindMin(simulation.C2_C3_distances);
+    double c2c3_max = FindMax(simulation.C2_C3_distances);
+
+    double c2c4_min = FindMin(simulation.C2_C4_distances);
+    double c2c4_max = FindMax(simulation.C2_C4_distances);
+
+    // 距离统计
+    statistics.DistanceStatistics.C3_C4.Min = c3c4_min;
+    statistics.DistanceStatistics.C3_C4.Max = c3c4_max;
+    statistics.DistanceStatistics.C3_C4.Range = c3c4_max - c3c4_min;
+
+    statistics.DistanceStatistics.C2_C3.Min = c2c3_min;
+    statistics.DistanceStatistics.C2_C3.Max = c2c3_max;
+    statistics.DistanceStatistics.C2_C3.Range = c2c3_max - c2c3_min;
+
+    statistics.DistanceStatistics.C2_C4.Min = c2c4_min;
+    statistics.DistanceStatistics.C2_C4.Max = c2c4_max;
+    statistics.DistanceStatistics.C2_C4.Range = c2c4_max - c2c4_min;
+
+    // 计算电机速度统计（使用左前腿的数据）
+    double thigh_min = FindMin(simulation.motor_data_LF.thigh_motor_speeds);
+    double thigh_max = FindMax(simulation.motor_data_LF.thigh_motor_speeds);
+
+    double shank_min = FindMin(simulation.motor_data_LF.shank_motor_speeds);
+    double shank_max = FindMax(simulation.motor_data_LF.shank_motor_speeds);
+
+    double ankle_min = FindMin(simulation.motor_data_LF.ankle_motor_speeds);
+    double ankle_max = FindMax(simulation.motor_data_LF.ankle_motor_speeds);
+
+    // 电机速度统计
+    statistics.MotorSpeedStatistics.Thigh.Min = thigh_min;
+    statistics.MotorSpeedStatistics.Thigh.Max = thigh_max;
+    statistics.MotorSpeedStatistics.Thigh.Range = std::abs(thigh_max - thigh_min);
+
+    statistics.MotorSpeedStatistics.Shank.Min = shank_min;
+    statistics.MotorSpeedStatistics.Shank.Max = shank_max;
+    statistics.MotorSpeedStatistics.Shank.Range = std::abs(shank_max - shank_min);
+
+    statistics.MotorSpeedStatistics.Ankle.Min = ankle_min;
+    statistics.MotorSpeedStatistics.Ankle.Max = ankle_max;
+    statistics.MotorSpeedStatistics.Ankle.Range = std::abs(ankle_max - ankle_min);
+
+    // 轨迹统计
+    statistics.TrajectoryStatistics.TotalFrames = simulation.Trajectory_L.size();
+    statistics.TrajectoryStatistics.TotalTime = simulation.Trajectory_L.size() * simulation.step;
+    statistics.TrajectoryStatistics.SwingFrames = static_cast<int>(std::round(simulation.t_w / simulation.step));
+    statistics.TrajectoryStatistics.SupportFrames = static_cast<int>(std::round(simulation.t_s / simulation.step));
+
+    return statistics;
+}
+
+// 创建初始角度数据
+InitialAnglesData CompleteJsonExporter::CreateInitialAngles(const QuadrupedRobotSimulation &simulation)
+{
+    InitialAnglesData initialAngles;
+
+    // 获取原始初始角度
+    std::map<std::string, RawInitialAngles> rawInitialAngles;
+
+    rawInitialAngles["LF"] = RawInitialAngles{
+        .Thigh = simulation.AB_horizontal_angles[0],
+        .Shank = simulation.AB1_AB_angles[0],
+        .Ankle = simulation.C3_C4_distances[0]};
+
+    // 计算相位偏移帧数
+    int n_frames = simulation.Trajectory_L.size();
+    int phase_LH = static_cast<int>(simulation.timeLH * n_frames);
+    int phase_RF = static_cast<int>(simulation.timeRF * n_frames);
+    int phase_RH = static_cast<int>(simulation.timeRH * n_frames);
+
+    // 添加其他腿的初始角度
+    rawInitialAngles["LH"] = RawInitialAngles{
+        .Thigh = simulation.AB_horizontal_angles[phase_LH],
+        .Shank = simulation.AB1_AB_angles[phase_LH],
+        .Ankle = simulation.C3_C4_distances[phase_LH]};
+
+    rawInitialAngles["RF"] = RawInitialAngles{
+        .Thigh = simulation.AB_horizontal_angles[phase_RF],
+        .Shank = simulation.AB1_AB_angles[phase_RF],
+        .Ankle = simulation.C3_C4_distances[phase_RF]};
+
+    rawInitialAngles["RH"] = RawInitialAngles{
+        .Thigh = simulation.AB_horizontal_angles[phase_RH],
+        .Shank = simulation.AB1_AB_angles[phase_RH],
+        .Ankle = simulation.C3_C4_distances[phase_RH]};
+
+    // 创建每条腿的初始角度数据
+    std::vector<std::string> legCodes = {"LF", "LH", "RF", "RH"};
+    std::vector<MotorData> motorDatas = {
+        simulation.motor_data_LF,
+        simulation.motor_data_LH,
+        simulation.motor_data_RF,
+        simulation.motor_data_RH};
+
+    for (size_t i = 0; i < legCodes.size(); i++)
+    {
+        std::string legCode = legCodes[i];
+        const MotorData &motorData = motorDatas[i];
+        const RawInitialAngles &rawAngles = rawInitialAngles[legCode];
+
+        LegInitialAngles legInitialAngles;
+        legInitialAngles.LegCode = legCode;
+
+        // 调试初始标定位置（原始值）
+        legInitialAngles.RawInitialAngles = rawAngles;
+
+        // 标定后位置（调整后的值）
+        legInitialAngles.CalibratedAngles.Thigh = motorData.AB_angles[0];
+        legInitialAngles.CalibratedAngles.Shank = motorData.AB1_AB_angles[0];
+        legInitialAngles.CalibratedAngles.Ankle = motorData.ankle_motor_angles[0];
+
+        // 调整信息
+        legInitialAngles.Adjustments.ThighAdjustment = motorData.AB_angles[0] - rawAngles.Thigh;
+        legInitialAngles.Adjustments.ShankAdjustment = motorData.AB1_AB_angles[0] - rawAngles.Shank;
+        legInitialAngles.Adjustments.AnkleAdjustment = motorData.ankle_motor_angles[0] - rawAngles.Ankle;
+
+        initialAngles.Legs[legCode] = legInitialAngles;
+    }
+
+    return initialAngles;
+}
+
+// 创建相位信息
+PhaseInfo CompleteJsonExporter::CreatePhaseInfo(const QuadrupedRobotSimulation &simulation)
+{
+    PhaseInfo phaseInfo;
+    int n_frames = simulation.Trajectory_L.size();
+
+    phaseInfo.GaitType = simulation.gait_type;
+    phaseInfo.TimeLF = simulation.timeLF;
+    phaseInfo.TimeLH = simulation.timeLH;
+    phaseInfo.TimeRF = simulation.timeRF;
+    phaseInfo.TimeRH = simulation.timeRH;
+    phaseInfo.PhaseLF = 0;
+    phaseInfo.PhaseLH = static_cast<int>(simulation.timeLH * n_frames);
+    phaseInfo.PhaseRF = static_cast<int>(simulation.timeRF * n_frames);
+    phaseInfo.PhaseRH = static_cast<int>(simulation.timeRH * n_frames);
+    phaseInfo.SupportRatio = simulation.support_ratio;
+    phaseInfo.SwingRatio = simulation.swing_ratio;
+    phaseInfo.SupportTime = simulation.t_s;
+    phaseInfo.SwingTime = simulation.t_w;
+
+    return phaseInfo;
+}
+
+// 创建运动范围数据
+MotionRangeData CompleteJsonExporter::CreateMotionRange(const QuadrupedRobotSimulation &simulation)
+{
+    MotionRangeData motionRange;
+
+    // 为每条腿计算运动范围
+    std::vector<std::string> legCodes = {"LF", "LH", "RF", "RH"};
+    std::vector<int> phaseShifts = {
+        0,
+        static_cast<int>(simulation.timeLH * simulation.Trajectory_L.size()),
+        static_cast<int>(simulation.timeRF * simulation.Trajectory_L.size()),
+        static_cast<int>(simulation.timeRH * simulation.Trajectory_L.size())};
+
+    for (size_t i = 0; i < legCodes.size(); i++)
+    {
+        std::string legCode = legCodes[i];
+        int phaseShift = phaseShifts[i];
+
+        // 计算L点运动范围
+        std::vector<double> x_values, y_values;
+        for (size_t idx = 0; idx < simulation.Trajectory_L.size(); idx++)
+        {
+            int shiftedIdx = (idx + phaseShift) % simulation.Trajectory_L.size();
+            const auto &point = simulation.Trajectory_L[shiftedIdx];
+            x_values.push_back(point[0]);
+            y_values.push_back(point[1]);
+        }
+
+        double x_min = FindMin(x_values);
+        double x_max = FindMax(x_values);
+        double y_min = FindMin(y_values);
+        double y_max = FindMax(y_values);
+
+        LegMotionRange legMotionRange;
+        legMotionRange.LegCode = legCode;
+        legMotionRange.PhaseShift = phaseShift;
+
+        legMotionRange.L_Point.X.Min = x_min;
+        legMotionRange.L_Point.X.Max = x_max;
+        legMotionRange.L_Point.X.Range = x_max - x_min;
+
+        legMotionRange.L_Point.Y.Min = y_min;
+        legMotionRange.L_Point.Y.Max = y_max;
+        legMotionRange.L_Point.Y.Range = y_max - y_min;
+
+        legMotionRange.L_Point.TotalRange = std::sqrt(
+            std::pow(x_max - x_min, 2) +
+            std::pow(y_max - y_min, 2));
+
+        motionRange.Legs[legCode] = legMotionRange;
+    }
+
+    return motionRange;
+}
+
+// 辅助函数：将数组转换为Point2D
+Point2D CompleteJsonExporter::ArrayToPoint(const std::vector<double> &array)
+{
+    if (array.size() >= 2)
+    {
+        return Point2D(array[0], array[1]);
+    }
+    return Point2D(0.0, 0.0);
+}
+
+// 模板函数：查找最小值
+template <typename T>
+T CompleteJsonExporter::FindMin(const std::vector<T> &values)
+{
+    if (values.empty())
+    {
+        return T();
+    }
+    T min_val = values[0];
+    for (const auto &val : values)
+    {
+        if (val < min_val)
+        {
+            min_val = val;
+        }
+    }
+    return min_val;
+}
+
+// 模板函数：查找最大值
+template <typename T>
+T CompleteJsonExporter::FindMax(const std::vector<T> &values)
+{
+    if (values.empty())
+    {
+        return T();
+    }
+    T max_val = values[0];
+    for (const auto &val : values)
+    {
+        if (val > max_val)
+        {
+            max_val = val;
+        }
+    }
+    return max_val;
+}
+
+// 模板函数：检查所有值是否满足条件
+template <typename T>
+bool CompleteJsonExporter::All(const std::vector<T> &values, std::function<bool(const T &)> predicate)
+{
+    for (const auto &val : values)
+    {
+        if (!predicate(val))
+        {
+            return false;
+        }
+    }
+    return true;
+}
+
+// 辅助函数：将double转换为字符串
+std::string CompleteJsonExporter::DoubleToString(double value, int precision)
+{
+    std::ostringstream oss;
+    oss << std::fixed << std::setprecision(precision) << value;
+    return oss.str();
+}

src/QuadrupedRobotSimulation/KinematicsHelper.cpp

@@ -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();
+    }
+}

src/QuadrupedRobotSimulation/RobotConfig.cpp

@@ -0,0 +1,846 @@
+#include "RobotConfig.hpp"
+// 统一的更新函数：手动从JSON更新现有对象
+RequestParameters &RequestParameters::updateFromJson(const nlohmann::json &j, RequestParameters &req_param)
+{
+    if (j.is_null() || !j.is_object())
+        return req_param; // 修正：返回传入的引用
+
+    try
+    {
+        // 更新 GaitInfo
+        if (j.contains("GaitInfo"))
+        {
+            const auto &gaitJson = j.at("GaitInfo");
+            if (gaitJson.is_object())
+            {
+                if (gaitJson.contains("GaitType"))
+                    req_param.GaitInfo.GaitType = gaitJson.at("GaitType").get<std::string>();
+                if (gaitJson.contains("Period"))
+                    req_param.GaitInfo.Period = gaitJson.at("Period").get<double>();
+                if (gaitJson.contains("Frequency"))
+                    req_param.GaitInfo.Frequency = gaitJson.at("Frequency").get<double>();
+                if (gaitJson.contains("StepTime"))
+                    req_param.GaitInfo.StepTime = gaitJson.at("StepTime").get<double>();
+                if (gaitJson.contains("SupportTime"))
+                    req_param.GaitInfo.SupportTime = gaitJson.at("SupportTime").get<double>();
+                if (gaitJson.contains("SwingTime"))
+                    req_param.GaitInfo.SwingTime = gaitJson.at("SwingTime").get<double>();
+                if (gaitJson.contains("SupportRatio"))
+                    req_param.GaitInfo.SupportRatio = gaitJson.at("SupportRatio").get<double>();
+                if (gaitJson.contains("SwingRatio"))
+                    req_param.GaitInfo.SwingRatio = gaitJson.at("SwingRatio").get<double>();
+                if (gaitJson.contains("TotalFrames"))
+                    req_param.GaitInfo.TotalFrames = gaitJson.at("TotalFrames").get<int>();
+                if (gaitJson.contains("TotalTime"))
+                    req_param.GaitInfo.TotalTime = gaitJson.at("TotalTime").get<double>();
+            }
+        }
+
+        // 更新 SystemParameters
+        if (j.contains("SystemParameters"))
+        {
+            const auto &sysJson = j.at("SystemParameters");
+            if (sysJson.is_object())
+            {
+                if (sysJson.contains("A_x"))
+                    req_param.SystemParameters.A_x = sysJson.at("A_x").get<double>();
+                if (sysJson.contains("A_y"))
+                    req_param.SystemParameters.A_y = sysJson.at("A_y").get<double>();
+                if (sysJson.contains("L10"))
+                    req_param.SystemParameters.L10 = sysJson.at("L10").get<double>();
+                if (sysJson.contains("L20"))
+                    req_param.SystemParameters.L20 = sysJson.at("L20").get<double>();
+                if (sysJson.contains("L30"))
+                    req_param.SystemParameters.L30 = sysJson.at("L30").get<double>();
+                if (sysJson.contains("StepLength"))
+                    req_param.SystemParameters.StepLength = sysJson.at("StepLength").get<double>();
+                if (sysJson.contains("StepHeight"))
+                    req_param.SystemParameters.StepHeight = sysJson.at("StepHeight").get<double>();
+                if (sysJson.contains("X"))
+                    req_param.SystemParameters.X = sysJson.at("X").get<double>();
+                if (sysJson.contains("DeltaX"))
+                    req_param.SystemParameters.DeltaX = sysJson.at("DeltaX").get<double>();
+                if (sysJson.contains("L21"))
+                    req_param.SystemParameters.L21 = sysJson.at("L21").get<double>();
+                if (sysJson.contains("L22"))
+                    req_param.SystemParameters.L22 = sysJson.at("L22").get<double>();
+                if (sysJson.contains("L23"))
+                    req_param.SystemParameters.L23 = sysJson.at("L23").get<double>();
+                if (sysJson.contains("Beta1"))
+                    req_param.SystemParameters.Beta1 = sysJson.at("Beta1").get<double>();
+                if (sysJson.contains("BB2_BC_Angle"))
+                    req_param.SystemParameters.BB2_BC_Angle = sysJson.at("BB2_BC_Angle").get<double>();
+                if (sysJson.contains("C1_B_Length"))
+                    req_param.SystemParameters.C1_B_Length = sysJson.at("C1_B_Length").get<double>();
+                if (sysJson.contains("CC1"))
+                    req_param.SystemParameters.CC1 = sysJson.at("CC1").get<double>();
+                if (sysJson.contains("L31"))
+                    req_param.SystemParameters.L31 = sysJson.at("L31").get<double>();
+                if (sysJson.contains("C2C3_Length"))
+                    req_param.SystemParameters.C2C3_Length = sysJson.at("C2C3_Length").get<double>();
+                if (sysJson.contains("Lead"))
+                    req_param.SystemParameters.Lead = sysJson.at("Lead").get<double>();
+                if (sysJson.contains("D1_L_Offset"))
+                    req_param.SystemParameters.D1_L_Offset = sysJson.at("D1_L_Offset").get<double>();
+                if (sysJson.contains("D2_L_Offset"))
+                    req_param.SystemParameters.D2_L_Offset = sysJson.at("D2_L_Offset").get<double>();
+                if (sysJson.contains("C4_D2_Offset"))
+                    req_param.SystemParameters.C4_D2_Offset = sysJson.at("C4_D2_Offset").get<double>();
+                if (sysJson.contains("ThighMotorReduction"))
+                    req_param.SystemParameters.ThighMotorReduction = sysJson.at("ThighMotorReduction").get<double>();
+                if (sysJson.contains("ShankMotorReduction"))
+                    req_param.SystemParameters.ShankMotorReduction = sysJson.at("ShankMotorReduction").get<double>();
+                if (sysJson.contains("AnkleMotorReduction"))
+                    req_param.SystemParameters.AnkleMotorReduction = sysJson.at("AnkleMotorReduction").get<double>();
+            }
+        }
+
+        // 更新 ModelID
+        if (j.contains("ModelID"))
+        {
+            const auto &modelJson = j.at("ModelID");
+            if (modelJson.is_object())
+            {
+                // 更新 LF
+                if (modelJson.contains("LF"))
+                {
+                    const auto &lfJson = modelJson.at("LF");
+                    if (lfJson.is_array())
+                    {
+                        req_param.ModelID.LF.components.clear();
+                        for (const auto &item : lfJson)
+                        {
+                            if (item.is_object())
+                            {
+                                ComponentID comp;
+                                auto it = item.begin();
+                                if (it != item.end())
+                                {
+                                    comp.name = it.key();
+                                    comp.uuid = it.value().get<std::string>();
+                                    req_param.ModelID.LF.components.push_back(comp);
+                                }
+                            }
+                        }
+                    }
+                }
+
+                // 更新 LH
+                if (modelJson.contains("LH"))
+                {
+                    const auto &lhJson = modelJson.at("LH");
+                    if (lhJson.is_array())
+                    {
+                        req_param.ModelID.LH.components.clear();
+                        for (const auto &item : lhJson)
+                        {
+                            if (item.is_object())
+                            {
+                                ComponentID comp;
+                                auto it = item.begin();
+                                if (it != item.end())
+                                {
+                                    comp.name = it.key();
+                                    comp.uuid = it.value().get<std::string>();
+                                    req_param.ModelID.LH.components.push_back(comp);
+                                }
+                            }
+                        }
+                    }
+                }
+
+                // 更新 RF
+                if (modelJson.contains("RF"))
+                {
+                    const auto &rfJson = modelJson.at("RF");
+                    if (rfJson.is_array())
+                    {
+                        req_param.ModelID.RF.components.clear();
+                        for (const auto &item : rfJson)
+                        {
+                            if (item.is_object())
+                            {
+                                ComponentID comp;
+                                auto it = item.begin();
+                                if (it != item.end())
+                                {
+                                    comp.name = it.key();
+                                    comp.uuid = it.value().get<std::string>();
+                                    req_param.ModelID.RF.components.push_back(comp);
+                                }
+                            }
+                        }
+                    }
+                }
+
+                // 更新 RH
+                if (modelJson.contains("RH"))
+                {
+                    const auto &rhJson = modelJson.at("RH");
+                    if (rhJson.is_array())
+                    {
+                        req_param.ModelID.RH.components.clear();
+                        for (const auto &item : rhJson)
+                        {
+                            if (item.is_object())
+                            {
+                                ComponentID comp;
+                                auto it = item.begin();
+                                if (it != item.end())
+                                {
+                                    comp.name = it.key();
+                                    comp.uuid = it.value().get<std::string>();
+                                    req_param.ModelID.RH.components.push_back(comp);
+                                }
+                            }
+                        }
+                    }
+                }
+            }
+        }
+
+        // 更新 Param (LegParameters)
+        if (j.contains("Param"))
+        {
+            const auto &paramJson = j.at("Param");
+            if (paramJson.is_object())
+            {
+                // 更新 LF
+                if (paramJson.contains("LF"))
+                {
+                    const auto &lfJson = paramJson.at("LF");
+                    if (lfJson.is_object())
+                    {
+                        if (lfJson.contains("thigh_angle_deg"))
+                            req_param.Param.LF.thigh_angle_deg = lfJson.at("thigh_angle_deg").get<double>();
+                        if (lfJson.contains("shank_angle_deg"))
+                            req_param.Param.LF.shank_angle_deg = lfJson.at("shank_angle_deg").get<double>();
+                        if (lfJson.contains("ankle_angle_deg"))
+                            req_param.Param.LF.ankle_angle_deg = lfJson.at("ankle_angle_deg").get<double>();
+                    }
+                }
+
+                // 更新 LH
+                if (paramJson.contains("LH"))
+                {
+                    const auto &lhJson = paramJson.at("LH");
+                    if (lhJson.is_object())
+                    {
+                        if (lhJson.contains("thigh_angle_deg"))
+                            req_param.Param.LH.thigh_angle_deg = lhJson.at("thigh_angle_deg").get<double>();
+                        if (lhJson.contains("shank_angle_deg"))
+                            req_param.Param.LH.shank_angle_deg = lhJson.at("shank_angle_deg").get<double>();
+                        if (lhJson.contains("ankle_angle_deg"))
+                            req_param.Param.LH.ankle_angle_deg = lhJson.at("ankle_angle_deg").get<double>();
+                    }
+                }
+
+                // 更新 RF
+                if (paramJson.contains("RF"))
+                {
+                    const auto &rfJson = paramJson.at("RF");
+                    if (rfJson.is_object())
+                    {
+                        if (rfJson.contains("thigh_angle_deg"))
+                            req_param.Param.RF.thigh_angle_deg = rfJson.at("thigh_angle_deg").get<double>();
+                        if (rfJson.contains("shank_angle_deg"))
+                            req_param.Param.RF.shank_angle_deg = rfJson.at("shank_angle_deg").get<double>();
+                        if (rfJson.contains("ankle_angle_deg"))
+                            req_param.Param.RF.ankle_angle_deg = rfJson.at("ankle_angle_deg").get<double>();
+                    }
+                }
+
+                // 更新 RH
+                if (paramJson.contains("RH"))
+                {
+                    const auto &rhJson = paramJson.at("RH");
+                    if (rhJson.is_object())
+                    {
+                        if (rhJson.contains("thigh_angle_deg"))
+                            req_param.Param.RH.thigh_angle_deg = rhJson.at("thigh_angle_deg").get<double>();
+                        if (rhJson.contains("shank_angle_deg"))
+                            req_param.Param.RH.shank_angle_deg = rhJson.at("shank_angle_deg").get<double>();
+                        if (rhJson.contains("ankle_angle_deg"))
+                            req_param.Param.RH.ankle_angle_deg = rhJson.at("ankle_angle_deg").get<double>();
+                    }
+                }
+            }
+        }
+
+        // 更新 RobotBody
+        if (j.contains("RobotBody"))
+        {
+            const auto &bodyJson = j.at("RobotBody");
+            if (bodyJson.is_object())
+            {
+                if (bodyJson.contains("BodyCode"))
+                    req_param.RobotBody.BodyCode = bodyJson.at("BodyCode").get<std::string>();
+                if (bodyJson.contains("x"))
+                    req_param.RobotBody.x = bodyJson.at("x").get<double>();
+                if (bodyJson.contains("y"))
+                    req_param.RobotBody.y = bodyJson.at("y").get<double>();
+                if (bodyJson.contains("z"))
+                    req_param.RobotBody.z = bodyJson.at("z").get<double>();
+                if (bodyJson.contains("qx"))
+                    req_param.RobotBody.qx = bodyJson.at("qx").get<double>();
+                if (bodyJson.contains("qy"))
+                    req_param.RobotBody.qy = bodyJson.at("qy").get<double>();
+                if (bodyJson.contains("qz"))
+                    req_param.RobotBody.qz = bodyJson.at("qz").get<double>();
+                if (bodyJson.contains("qw"))
+                    req_param.RobotBody.qw = bodyJson.at("qw").get<double>();
+            }
+        }
+
+        // 更新 t_percentage
+        if (j.contains("t_percentage"))
+        {
+            req_param.t_percentage = j.at("t_percentage").get<double>();
+        }
+        if (j.contains("RobotID"))
+        {
+            req_param.RobotID = j.at("RobotID").get<std::string>();
+        }
+
+        return req_param; // 返回引用
+    }
+    catch (const nlohmann::json::exception &e)
+    {
+        std::cerr << "[ERROR] 更新RequestParameters失败: " << e.what() << std::endl;
+        std::cerr << "[ERROR] 当前JSON: " << j.dump(2) << std::endl;
+        throw;
+    }
+}
+// GaitInfo 序列化/反序列化实现
+void to_json(nlohmann::json &j, const GaitInfo &g)
+{
+    j = nlohmann::json{
+        {"GaitType", g.GaitType},
+        {"Period", g.Period},
+        {"Frequency", g.Frequency},
+        {"StepTime", g.StepTime},
+        {"SupportTime", g.SupportTime},
+        {"SwingTime", g.SwingTime},
+        {"SupportRatio", g.SupportRatio},
+        {"SwingRatio", g.SwingRatio},
+        {"TotalFrames", g.TotalFrames},
+        {"TotalTime", g.TotalTime}};
+}
+
+void from_json(const nlohmann::json &j, GaitInfo &g)
+{
+    if (j.is_null() || !j.is_object())
+        return;
+
+    if (j.contains("GaitType"))
+        j.at("GaitType").get_to(g.GaitType);
+    if (j.contains("Period"))
+        j.at("Period").get_to(g.Period);
+    if (j.contains("Frequency"))
+        j.at("Frequency").get_to(g.Frequency);
+    if (j.contains("StepTime"))
+        j.at("StepTime").get_to(g.StepTime);
+    if (j.contains("SupportTime"))
+        j.at("SupportTime").get_to(g.SupportTime);
+    if (j.contains("SwingTime"))
+        j.at("SwingTime").get_to(g.SwingTime);
+    if (j.contains("SupportRatio"))
+        j.at("SupportRatio").get_to(g.SupportRatio);
+    if (j.contains("SwingRatio"))
+        j.at("SwingRatio").get_to(g.SwingRatio);
+    if (j.contains("TotalFrames"))
+        j.at("TotalFrames").get_to(g.TotalFrames);
+    if (j.contains("TotalTime"))
+        j.at("TotalTime").get_to(g.TotalTime);
+}
+
+// SystemParameters 序列化/反序列化实现
+void to_json(nlohmann::json &j, const SystemParameters &s)
+{
+    j = nlohmann::json{
+        {"A_x", s.A_x},
+        {"A_y", s.A_y},
+        {"L10", s.L10},
+        {"L20", s.L20},
+        {"L30", s.L30},
+        {"StepLength", s.StepLength},
+        {"StepHeight", s.StepHeight},
+        {"X", s.X},
+        {"DeltaX", s.DeltaX},
+        {"L21", s.L21},
+        {"L22", s.L22},
+        {"L23", s.L23},
+        {"Beta1", s.Beta1},
+        {"BB2_BC_Angle", s.BB2_BC_Angle},
+        {"C1_B_Length", s.C1_B_Length},
+        {"CC1", s.CC1},
+        {"L31", s.L31},
+        {"C2C3_Length", s.C2C3_Length},
+        {"Lead", s.Lead},
+        {"D1_L_Offset", s.D1_L_Offset},
+        {"D2_L_Offset", s.D2_L_Offset},
+        {"C4_D2_Offset", s.C4_D2_Offset},
+        {"ThighMotorReduction", s.ThighMotorReduction},
+        {"ShankMotorReduction", s.ShankMotorReduction},
+        {"AnkleMotorReduction", s.AnkleMotorReduction}};
+}
+
+void from_json(const nlohmann::json &j, SystemParameters &s)
+{
+    if (j.is_null() || !j.is_object())
+        return;
+
+    if (j.contains("A_x"))
+        j.at("A_x").get_to(s.A_x);
+    if (j.contains("A_y"))
+        j.at("A_y").get_to(s.A_y);
+    if (j.contains("L10"))
+        j.at("L10").get_to(s.L10);
+    if (j.contains("L20"))
+        j.at("L20").get_to(s.L20);
+    if (j.contains("L30"))
+        j.at("L30").get_to(s.L30);
+    if (j.contains("StepLength"))
+        j.at("StepLength").get_to(s.StepLength);
+    if (j.contains("StepHeight"))
+        j.at("StepHeight").get_to(s.StepHeight);
+    if (j.contains("X"))
+        j.at("X").get_to(s.X);
+    if (j.contains("DeltaX"))
+        j.at("DeltaX").get_to(s.DeltaX);
+    if (j.contains("L21"))
+        j.at("L21").get_to(s.L21);
+    if (j.contains("L22"))
+        j.at("L22").get_to(s.L22);
+    if (j.contains("L23"))
+        j.at("L23").get_to(s.L23);
+    if (j.contains("Beta1"))
+        j.at("Beta1").get_to(s.Beta1);
+    if (j.contains("BB2_BC_Angle"))
+        j.at("BB2_BC_Angle").get_to(s.BB2_BC_Angle);
+    if (j.contains("C1_B_Length"))
+        j.at("C1_B_Length").get_to(s.C1_B_Length);
+    if (j.contains("CC1"))
+        j.at("CC1").get_to(s.CC1);
+    if (j.contains("L31"))
+        j.at("L31").get_to(s.L31);
+    if (j.contains("C2C3_Length"))
+        j.at("C2C3_Length").get_to(s.C2C3_Length);
+    if (j.contains("Lead"))
+        j.at("Lead").get_to(s.Lead);
+    if (j.contains("D1_L_Offset"))
+        j.at("D1_L_Offset").get_to(s.D1_L_Offset);
+    if (j.contains("D2_L_Offset"))
+        j.at("D2_L_Offset").get_to(s.D2_L_Offset);
+    if (j.contains("C4_D2_Offset"))
+        j.at("C4_D2_Offset").get_to(s.C4_D2_Offset);
+    if (j.contains("ThighMotorReduction"))
+        j.at("ThighMotorReduction").get_to(s.ThighMotorReduction);
+    if (j.contains("ShankMotorReduction"))
+        j.at("ShankMotorReduction").get_to(s.ShankMotorReduction);
+    if (j.contains("AnkleMotorReduction"))
+        j.at("AnkleMotorReduction").get_to(s.AnkleMotorReduction);
+}
+
+// ComponentID 序列化/反序列化实现
+void to_json(nlohmann::json &j, const ComponentID &c)
+{
+    j = nlohmann::json{{c.name, c.uuid}};
+}
+
+void from_json(const nlohmann::json &j, ComponentID &c)
+{
+    if (j.is_null() || !j.is_object())
+        return;
+
+    auto it = j.begin();
+    if (it != j.end())
+    {
+        c.name = it.key();
+        c.uuid = it.value();
+    }
+}
+
+// LegModelIDs 序列化/反序列化实现
+void to_json(nlohmann::json &j, const LegModelIDs &l)
+{
+
+    j = nlohmann::json::array();
+    for (const auto &component : l.components)
+    {
+        j.push_back(component);
+    }
+}
+
+void from_json(const nlohmann::json &j, LegModelIDs &l)
+{
+    l.components.clear();
+    if (j.is_null() || !j.is_array())
+        return;
+
+    for (const auto &item : j)
+    {
+        ComponentID comp;
+        from_json(item, comp);
+        l.components.push_back(comp);
+    }
+}
+
+// ModelID 序列化/反序列化实现
+void to_json(nlohmann::json &j, const ModelID &m)
+{
+    j = nlohmann::json{
+        {"LF", m.LF},
+        {"LH", m.LH},
+        {"RF", m.RF},
+        {"RH", m.RH}};
+}
+
+void from_json(const nlohmann::json &j, ModelID &m)
+{
+    if (j.is_null() || !j.is_object())
+        return;
+
+    if (j.contains("LF"))
+        j.at("LF").get_to(m.LF);
+    if (j.contains("LH"))
+        j.at("LH").get_to(m.LH);
+    if (j.contains("RF"))
+        j.at("RF").get_to(m.RF);
+    if (j.contains("RH"))
+        j.at("RH").get_to(m.RH);
+}
+
+// LegParam 序列化/反序列化实现
+void to_json(nlohmann::json &j, const LegParam &l)
+{
+    j = nlohmann::json{
+        {"thigh_angle_deg", l.thigh_angle_deg},
+        {"shank_angle_deg", l.shank_angle_deg},
+        {"ankle_angle_deg", l.ankle_angle_deg}};
+}
+
+void from_json(const nlohmann::json &j, LegParam &l)
+{
+    if (j.is_null() || !j.is_object())
+        return;
+
+    if (j.contains("thigh_angle_deg"))
+        j.at("thigh_angle_deg").get_to(l.thigh_angle_deg);
+    if (j.contains("shank_angle_deg"))
+        j.at("shank_angle_deg").get_to(l.shank_angle_deg);
+    if (j.contains("ankle_angle_deg"))
+        j.at("ankle_angle_deg").get_to(l.ankle_angle_deg);
+}
+
+// LegParameters 序列化/反序列化实现
+void to_json(nlohmann::json &j, const LegParameters &l)
+{
+    j = nlohmann::json{
+        {"LF", l.LF},
+        {"LH", l.LH},
+        {"RF", l.RF},
+        {"RH", l.RH}};
+}
+
+void from_json(const nlohmann::json &j, LegParameters &l)
+{
+    if (j.is_null() || !j.is_object())
+        return;
+
+    if (j.contains("LF"))
+        j.at("LF").get_to(l.LF);
+    if (j.contains("LH"))
+        j.at("LH").get_to(l.LH);
+    if (j.contains("RF"))
+        j.at("RF").get_to(l.RF);
+    if (j.contains("RH"))
+        j.at("RH").get_to(l.RH);
+}
+
+// RobotBody 序列化/反序列化实现
+void to_json(nlohmann::json &j, const RobotBody &r)
+{
+    j = nlohmann::json{
+        {"BodyCode", r.BodyCode},
+        {"x", r.x},
+        {"y", r.y},
+        {"z", r.z},
+        {"qx", r.qx},
+        {"qy", r.qy},
+        {"qz", r.qz},
+        {"qw", r.qw}};
+}
+
+void from_json(const nlohmann::json &j, RobotBody &r)
+{
+    if (j.is_null() || !j.is_object())
+        return;
+
+    if (j.contains("BodyCode"))
+        j.at("BodyCode").get_to(r.BodyCode);
+    if (j.contains("x"))
+        j.at("x").get_to(r.x);
+    if (j.contains("y"))
+        j.at("y").get_to(r.y);
+    if (j.contains("z"))
+        j.at("z").get_to(r.z);
+    if (j.contains("qx"))
+        j.at("qx").get_to(r.qx);
+    if (j.contains("qy"))
+        j.at("qy").get_to(r.qy);
+    if (j.contains("qz"))
+        j.at("qz").get_to(r.qz);
+    if (j.contains("qw"))
+        j.at("qw").get_to(r.qw);
+}
+
+// RequestParameters 序列化/反序列化实现
+void to_json(nlohmann::json &j, const RequestParameters &c)
+{
+    j = nlohmann::json{
+        {"GaitInfo", c.GaitInfo},
+        {"SystemParameters", c.SystemParameters},
+        {"ModelID", c.ModelID},
+        {"Param", c.Param},
+        {"RobotBody", c.RobotBody},
+        {"RobotID", c.RobotID},
+        {"t_percentage", c.t_percentage}};
+}
+
+void from_json(const nlohmann::json &j, RequestParameters &c)
+{
+    if (j.is_null() || !j.is_object())
+        return;
+
+    try
+    {
+        if (j.contains("GaitInfo"))
+            j.at("GaitInfo").get_to(c.GaitInfo);
+        if (j.contains("SystemParameters"))
+            j.at("SystemParameters").get_to(c.SystemParameters);
+        if (j.contains("ModelID"))
+            j.at("ModelID").get_to(c.ModelID);
+        if (j.contains("Param"))
+            j.at("Param").get_to(c.Param);
+        if (j.contains("RobotBody"))
+            j.at("RobotBody").get_to(c.RobotBody);
+        if (j.contains("t_percentage"))
+            j.at("t_percentage").get_to(c.t_percentage);
+        if (j.contains("RobotID"))
+            j.at("RobotID").get_to(c.RobotID);
+    }
+    catch (const nlohmann::json::exception &e)
+    {
+        std::cerr << "[ERROR] 解析RequestParameters失败: " << e.what() << std::endl;
+        std::cerr << "[ERROR] 当前JSON: " << j.dump(2) << std::endl;
+        throw;
+    }
+}
+void RequestParameters::debugPrint() const
+{
+    std::cout << "========================================" << std::endl;
+    std::cout << "RequestParameters DEBUG PRINT" << std::endl;
+    std::cout << "========================================" << std::endl;
+
+    // GaitInfo
+    std::cout << "=== GaitInfo ===" << std::endl;
+    std::cout << "  GaitType:     " << GaitInfo.GaitType << std::endl;
+    std::cout << "  Period:       " << GaitInfo.Period << std::endl;
+    std::cout << "  Frequency:    " << GaitInfo.Frequency << std::endl;
+    std::cout << "  StepTime:     " << GaitInfo.StepTime << std::endl;
+    std::cout << "  SupportTime:  " << GaitInfo.SupportTime << std::endl;
+    std::cout << "  SwingTime:    " << GaitInfo.SwingTime << std::endl;
+    std::cout << "  SupportRatio: " << GaitInfo.SupportRatio << std::endl;
+    std::cout << "  SwingRatio:   " << GaitInfo.SwingRatio << std::endl;
+    std::cout << "  TotalFrames:  " << GaitInfo.TotalFrames << std::endl;
+    std::cout << "  TotalTime:    " << GaitInfo.TotalTime << std::endl;
+
+    // SystemParameters (关键字段)
+    std::cout << "\n=== SystemParameters (key fields) ===" << std::endl;
+    std::cout << "  A_x: " << SystemParameters.A_x << std::endl;
+    std::cout << "  A_y: " << SystemParameters.A_y << std::endl;
+    std::cout << "  L10: " << SystemParameters.L10 << std::endl;
+    std::cout << "  L20: " << SystemParameters.L20 << std::endl;
+    std::cout << "  L30: " << SystemParameters.L30 << std::endl;
+    std::cout << "  StepLength: " << SystemParameters.StepLength << std::endl;
+    std::cout << "  StepHeight: " << SystemParameters.StepHeight << std::endl;
+    std::cout << "  X: " << SystemParameters.X << std::endl;
+    std::cout << "  DeltaX: " << SystemParameters.DeltaX << std::endl;
+    std::cout << "  L21: " << SystemParameters.L21 << std::endl;
+    std::cout << "  L22: " << SystemParameters.L22 << std::endl;
+    std::cout << "  L23: " << SystemParameters.L23 << std::endl;
+    std::cout << "  Beta1: " << SystemParameters.Beta1 << std::endl;
+    std::cout << "  BB2_BC_Angle: " << SystemParameters.BB2_BC_Angle << std::endl;
+    std::cout << "  C1_B_Length: " << SystemParameters.C1_B_Length << std::endl;
+    std::cout << "  CC1: " << SystemParameters.CC1 << std::endl;
+    std::cout << "  L31: " << SystemParameters.L31 << std::endl;
+    std::cout << "  C2C3_Length: " << SystemParameters.C2C3_Length << std::endl;
+    std::cout << "  Lead: " << SystemParameters.Lead << std::endl;
+    std::cout << "  D1_L_Offset: " << SystemParameters.D1_L_Offset << std::endl;
+    std::cout << "  D2_L_Offset: " << SystemParameters.D2_L_Offset << std::endl;
+    std::cout << "  C4_D2_Offset: " << SystemParameters.C4_D2_Offset << std::endl;
+    std::cout << "  ThighMotorReduction: " << SystemParameters.ThighMotorReduction << std::endl;
+    std::cout << "  ShankMotorReduction: " << SystemParameters.ShankMotorReduction << std::endl;
+    std::cout << "  AnkleMotorReduction: " << SystemParameters.AnkleMotorReduction << std::endl;
+
+    // ModelID详细内容
+    std::cout << "\n=== ModelID Details ===" << std::endl;
+
+    // LF
+    std::cout << "  LF [" << ModelID.LF.components.size() << " components]:" << std::endl;
+    for (size_t i = 0; i < ModelID.LF.components.size(); ++i)
+    {
+        std::cout << "    [" << i << "] " << ModelID.LF.components[i].name
+                  << ": " << ModelID.LF.components[i].uuid << std::endl;
+    }
+
+    // LH
+    std::cout << "  LH [" << ModelID.LH.components.size() << " components]:" << std::endl;
+    for (size_t i = 0; i < ModelID.LH.components.size(); ++i)
+    {
+        std::cout << "    [" << i << "] " << ModelID.LH.components[i].name
+                  << ": " << ModelID.LH.components[i].uuid << std::endl;
+    }
+
+    // RF
+    std::cout << "  RF [" << ModelID.RF.components.size() << " components]:" << std::endl;
+    for (size_t i = 0; i < ModelID.RF.components.size(); ++i)
+    {
+        std::cout << "    [" << i << "] " << ModelID.RF.components[i].name
+                  << ": " << ModelID.RF.components[i].uuid << std::endl;
+    }
+
+    // RH
+    std::cout << "  RH [" << ModelID.RH.components.size() << " components]:" << std::endl;
+    for (size_t i = 0; i < ModelID.RH.components.size(); ++i)
+    {
+        std::cout << "    [" << i << "] " << ModelID.RH.components[i].name
+                  << ": " << ModelID.RH.components[i].uuid << std::endl;
+    }
+
+    // LegParameters
+    std::cout << "\n=== Param (LegParameters) ===" << std::endl;
+
+    std::cout << "  LF:" << std::endl;
+    std::cout << "    thigh_angle_deg: " << Param.LF.thigh_angle_deg << std::endl;
+    std::cout << "    shank_angle_deg: " << Param.LF.shank_angle_deg << std::endl;
+    std::cout << "    ankle_angle_deg: " << Param.LF.ankle_angle_deg << std::endl;
+
+    std::cout << "  LH:" << std::endl;
+    std::cout << "    thigh_angle_deg: " << Param.LH.thigh_angle_deg << std::endl;
+    std::cout << "    shank_angle_deg: " << Param.LH.shank_angle_deg << std::endl;
+    std::cout << "    ankle_angle_deg: " << Param.LH.ankle_angle_deg << std::endl;
+
+    std::cout << "  RF:" << std::endl;
+    std::cout << "    thigh_angle_deg: " << Param.RF.thigh_angle_deg << std::endl;
+    std::cout << "    shank_angle_deg: " << Param.RF.shank_angle_deg << std::endl;
+    std::cout << "    ankle_angle_deg: " << Param.RF.ankle_angle_deg << std::endl;
+
+    std::cout << "  RH:" << std::endl;
+    std::cout << "    thigh_angle_deg: " << Param.RH.thigh_angle_deg << std::endl;
+    std::cout << "    shank_angle_deg: " << Param.RH.shank_angle_deg << std::endl;
+    std::cout << "    ankle_angle_deg: " << Param.RH.ankle_angle_deg << std::endl;
+
+    // RobotBody
+    std::cout << "\n=== RobotBody ===" << std::endl;
+    std::cout << "  BodyCode: " << RobotBody.BodyCode << std::endl;
+    std::cout << "  x: " << RobotBody.x << std::endl;
+    std::cout << "  y: " << RobotBody.y << std::endl;
+    std::cout << "  z: " << RobotBody.z << std::endl;
+    std::cout << "  qx: " << RobotBody.qx << std::endl;
+    std::cout << "  qy: " << RobotBody.qy << std::endl;
+    std::cout << "  qz: " << RobotBody.qz << std::endl;
+    std::cout << "  qw: " << RobotBody.qw << std::endl;
+
+    // Other parameters
+    std::cout << "\n=== Other Parameters  ===" << std::endl;
+    std::cout << "  t_percentage: " << t_percentage << std::endl;
+    std::cout << "  RobotID: " << RobotID << std::endl;
+
+    std::cout << "========================================" << std::endl;
+    std::cout << "END DEBUG PRINT" << std::endl;
+    std::cout << "========================================" << std::endl;
+}
+
+// RequestParameters 成员函数实现
+RequestParameters RequestParameters::fromJsonString(const std::string &jsonStr)
+{
+    try
+    {
+        auto j = nlohmann::json::parse(jsonStr);
+
+        // 如果不包含req_param，尝试直接解析
+        if (j.contains("GaitInfo") || j.contains("SystemParameters") ||
+            j.contains("ModelID") || j.contains("Param") || j.contains("RobotBody") || j.contains("t_percentage") || j.contains("RobotID"))
+        {
+            // 看起来是直接RequestParameters格式
+            return j.get<RequestParameters>();
+        }
+        else
+        {
+
+            if (j.contains("req_param") && !j["req_param"].is_null())
+            {
+                // req_param是一个嵌套的JSON对象，直接获取它
+                auto req_param_json = j["req_param"];
+                if (req_param_json.contains("GaitInfo") || req_param_json.contains("SystemParameters") ||
+                    req_param_json.contains("ModelID") || req_param_json.contains("Param") || req_param_json.contains("RobotBody") || req_param_json.contains("t_percentage") || req_param_json.contains("RobotID"))
+                {
+                    // 看起来是直接RequestParameters格式
+                    return req_param_json.get<RequestParameters>();
+                }
+            }
+        }
+
+        // 返回默认对象
+        return RequestParameters();
+    }
+    catch (const nlohmann::json::exception &e)
+    {
+        std::cerr << "[ERROR] JSON解析失败: " << e.what() << std::endl;
+        return RequestParameters(); // 返回默认对象
+    }
+    catch (...)
+    {
+        std::cerr << "[ERROR] 未知错误解析JSON" << std::endl;
+        return RequestParameters(); // 返回默认对象
+    }
+}
+
+RequestParameters RequestParameters::fromJsonFile(const std::string &filename)
+{
+    std::ifstream file(filename);
+    if (!file.is_open())
+    {
+        throw std::runtime_error("Cannot open file: " + filename);
+    }
+
+    std::stringstream buffer;
+    buffer << file.rdbuf();
+    return fromJsonString(buffer.str());
+}
+
+std::string RequestParameters::toJsonString() const
+{
+    nlohmann::json j = *this;
+    return j.dump(2); // 缩进2个空格
+}
+
+bool RequestParameters::saveToFile(const std::string &filename) const
+{
+    try
+    {
+        std::ofstream file(filename);
+        if (!file.is_open())
+        {
+            return false;
+        }
+        file << toJsonString();
+        return true;
+    }
+    catch (...)
+    {
+        return false;
+    }
+}

src/QuadrupedRobotSimulation/SharedGeometry.cpp

@@ -0,0 +1,211 @@
+// SharedGeometry.cpp
+#include "SharedGeometry.h"
+#include <stdexcept>
+#include <cmath>
+#include <algorithm>
+
+// 如果没有定义M_PI，则定义它
+#ifndef M_PI
+#define M_PI 3.14159265358979323846
+#endif
+
+// ==================== Vector2D 实现 ====================
+
+Vector2D::Vector2D() : X(0.0), Y(0.0) {}
+
+Vector2D::Vector2D(double x, double y) : X(x), Y(y) {}
+
+Vector2D Vector2D::operator+(const Vector2D &other) const
+{
+    return Vector2D(X + other.X, Y + other.Y);
+}
+
+Vector2D Vector2D::operator-(const Vector2D &other) const
+{
+    return Vector2D(X - other.X, Y - other.Y);
+}
+
+Vector2D Vector2D::operator*(double scalar) const
+{
+    return Vector2D(X * scalar, Y * scalar);
+}
+
+double Vector2D::distanceTo(const Vector2D &other) const
+{
+    double dx = X - other.X;
+    double dy = Y - other.Y;
+    return std::sqrt(dx * dx + dy * dy);
+}
+
+double Vector2D::length() const
+{
+    return std::sqrt(X * X + Y * Y);
+}
+
+Vector2D Vector2D::normalized() const
+{
+    double len = length();
+    if (len > 0.0)
+    {
+        return Vector2D(X / len, Y / len);
+    }
+    return Vector2D(0.0, 0.0);
+}
+
+Vector2D Vector2D::Zero()
+{
+    return Vector2D(0.0, 0.0);
+}
+
+double Vector2D::Distance(const Vector2D &v1, const Vector2D &v2)
+{
+    return std::sqrt(std::pow(v2.X - v1.X, 2) + std::pow(v2.Y - v1.Y, 2));
+}
+
+double Vector2D::Cross(const Vector2D &other) const
+{
+    return X * other.Y - Y * other.X;
+}
+
+double Vector2D::Dot(const Vector2D &other) const
+{
+    return X * other.X + Y * other.Y;
+}
+
+// ==================== Pose7 实现 ====================
+
+Pose7::Pose7() : tx(0.0), ty(0.0), tz(0.0), qx(0.0), qy(0.0), qz(0.0), qw(1.0) {}
+
+// ==================== PoseCalculator 实现 ====================
+
+Pose7 PoseCalculator::CalculatePoseAndQuaternion(const Vector2D &start, const Vector2D &end)
+{
+    Pose7 pose;
+    pose.tx = start.X;
+    pose.ty = start.Y;
+    pose.tz = 0.0;
+
+    Vector2D direction = end - start;
+    double length = direction.length();
+
+    if (length > 0.0)
+    {
+        direction = direction * (1.0 / length);
+        double angle = std::atan2(direction.Y, direction.X);
+
+        // 转换为四元数（绕Z轴旋转）
+        double halfAngle = angle * 0.5;
+        pose.qw = std::cos(halfAngle);
+        pose.qz = std::sin(halfAngle);
+        pose.qx = 0.0;
+        pose.qy = 0.0;
+    }
+    else
+    {
+        pose.qw = 1.0;
+        pose.qx = 0.0;
+        pose.qy = 0.0;
+        pose.qz = 0.0;
+    }
+
+    return pose;
+}
+
+std::vector<double> PoseCalculator::CalculatePoseAndQuaternionArray(double x1, double y1, double x2, double y2)
+{
+    double x_D = x1;
+    double y_D = y1;
+    double x_G = x2;
+    double y_G = y2;
+
+    // 方向向量
+    double dx = x_G - x_D;
+    double dy = y_G - y_D;
+
+    double v_norm = std::sqrt(dx * dx + dy * dy);
+
+    // 使用小的epsilon值而不是直接比较0
+    if (std::abs(v_norm) < 1e-10)
+    {
+        throw std::invalid_argument("两个点位置相同，无法计算姿态。");
+    }
+
+    // 单位化方向向量
+    double vx = dx / v_norm;
+    double vy = dy / v_norm;
+
+    // 四元数（绕Z轴旋转）
+    double theta = std::atan2(vy, vx);
+    double halfTheta = theta / 2.0;
+    double qw = std::cos(halfTheta);
+    double qx = 0.0;
+    double qy = 0.0;
+    double qz = std::sin(halfTheta);
+
+    // 返回长度为7的数组
+    return {x_D, y_D, 0.0, qw, qx, qy, qz};
+}
+
+double PoseCalculator::MMToM(double MM)
+{
+    return Round(MM / 1000.0, 6);
+}
+
+double PoseCalculator::Round(double value, int decimals)
+{
+    double factor = std::pow(10.0, decimals);
+    return std::round(value * factor) / factor;
+}
+
+std::string PoseCalculator::ToString(double value, int decimals)
+{
+    std::ostringstream oss;
+    oss << std::fixed << std::setprecision(decimals) << value;
+    return oss.str();
+}
+
+C_ObjStates PoseCalculator::CalculatePoseAndQuaternion_C_ObjStates(const std::string &i,
+                                                                   double x1, double y1,
+                                                                   double x2, double y2)
+{
+    // 如果需要实现这个方法，需要C_ObjStates的定义
+    // 这里先抛出一个异常，提醒需要实现
+    // throw std::runtime_error("C_ObjStates需要额外的定义，请提供C_ObjStates类的定义");
+
+    // 示例代码（需要C_ObjStates类）：
+
+    double x_D = MMToM(x1);
+    double y_D = MMToM(y1);
+    double x_G = MMToM(x2);
+    double y_G = MMToM(y2);
+
+    double dx = x_G - x_D;
+    double dy = y_G - y_D;
+    double v_norm = std::sqrt(dx * dx + dy * dy);
+
+    if (std::abs(v_norm) < 1e-10)
+    {
+        throw std::invalid_argument("两个点位置相同，无法计算姿态。");
+    }
+
+    double vx = dx / v_norm;
+    double vy = dy / v_norm;
+    double theta = std::atan2(vy, vx);
+    double halfTheta = theta / 2.0;
+    double qw = std::cos(halfTheta);
+    double qx = 0.0;
+    double qy = 0.0;
+    double qz = std::sin(halfTheta);
+
+    C_ObjStates c_ObjStates;
+    c_ObjStates.i = i;
+    c_ObjStates.tx = ToString(x_D);
+    c_ObjStates.ty = ToString(y_D);
+    c_ObjStates.tz = "0";
+    c_ObjStates.qx = ToString(qx);
+    c_ObjStates.qy = ToString(qy);
+    c_ObjStates.qz = ToString(qz);
+    c_ObjStates.qw = ToString(qw);
+
+    return c_ObjStates;
+}

src/RobotManager.cpp

@@ -0,0 +1,167 @@
+#include "RobotManager.h"
+#include <sstream>
+#include <iomanip>
+#include <iostream>
+#include <functional>
+
+// 初始化静态成员
+std::unordered_map<std::string, std::shared_ptr<Robot>> RobotManager::_kinematics_table;
+std::unordered_map<std::string, std::string> RobotManager::_urdf_hashes;
+std::recursive_mutex RobotManager::_lock;
+
+// 哈希函数
+std::string calculateHash(const std::string& input) {
+    // 使用标准库的哈希函数组合
+    std::hash<std::string> hasher;
+    size_t h1 = hasher(input);
+    size_t h2 = hasher(input + "salt"); // 添加盐值增加唯一性
+
+    std::stringstream ss;
+    ss << std::hex << h1 << h2;
+    return ss.str();
+}
+
+bool RobotManager::_validateKinematics(const std::shared_ptr<Robot>& robot) {
+    if (!robot || !robot->isInitialized()) {
+        return false;
+    }
+
+    try {
+        // 简单的验证：检查正向运动学计算
+        double test_joints[6] = { 0.1, 0.1, 0.1, 0.1, 0.1, 0.1 };
+        double tcp_pose[7];
+
+        return robot->calculateFK_TCP(test_joints, tcp_pose);
+    }
+    catch (const std::exception& e) {
+        std::cerr << "Robot validation failed: " << e.what() << std::endl;
+        return false;
+    }
+}
+
+std::pair<bool, std::string> RobotManager::initRobot(const std::string& urdf_robot,
+    const std::string& robot_uuid,
+    bool force_update) {
+    std::lock_guard<std::recursive_mutex> lock(_lock);
+
+    try {
+        // 创建机器人实例
+        auto robot = std::make_shared<Robot>();
+
+        // 初始化机器人
+        if (!robot->initRobot(urdf_robot)) {
+            return std::make_pair(false, "Failed to initialize robot from URDF");
+        }
+
+        std::string actual_uuid = robot_uuid;
+        if (actual_uuid.empty()) {
+            // 如果没有提供UUID，使用URDF内容的哈希作为UUID
+            actual_uuid = calculateHash(urdf_robot);
+        }
+
+        // 计算URDF哈希
+        std::string urdf_hash = calculateHash(urdf_robot);
+
+        // 检查是否需要更新
+        if (!force_update && _urdf_hashes.find(actual_uuid) != _urdf_hashes.end() &&
+            _urdf_hashes[actual_uuid] == urdf_hash) {
+            return std::make_pair(true, "Content unchanged, skipping update");
+        }
+
+        // 验证运动学
+        if (!_validateKinematics(robot)) {
+            return std::make_pair(false, "URDF validation failed");
+        }
+
+        // 更新表
+        _kinematics_table[actual_uuid] = robot;
+        _urdf_hashes[actual_uuid] = urdf_hash;
+
+        bool existed = (_urdf_hashes.find(actual_uuid) != _urdf_hashes.end());
+        std::string action = existed ? "updated" : "added";
+        return std::make_pair(true, "Successfully " + action + " robot " + actual_uuid);
+    }
+    catch (const std::exception& e) {
+        return std::make_pair(false, "Operation failed: " + std::string(e.what()));
+    }
+}
+
+std::unordered_map<std::string, std::pair<bool, std::string>>
+RobotManager::batchInit(const std::unordered_map<std::string, std::string>& robot_specs) {
+    std::lock_guard<std::recursive_mutex> lock(_lock);
+
+    std::unordered_map<std::string, std::pair<bool, std::string>> results;
+
+    // 使用传统的迭代器语法
+    for (auto it = robot_specs.begin(); it != robot_specs.end(); ++it) {
+        results[it->first] = initRobot(it->second, it->first);
+    }
+
+    return results;
+}
+
+std::shared_ptr<Robot> RobotManager::getRobot(const std::string& robot_uuid) {
+    std::lock_guard<std::recursive_mutex> lock(_lock);
+
+    auto it = _kinematics_table.find(robot_uuid);
+    if (it != _kinematics_table.end()) {
+        return it->second;
+    }
+    return nullptr;
+}
+
+std::pair<bool, std::string> RobotManager::removeRobot(const std::string& robot_uuid) {
+    std::lock_guard<std::recursive_mutex> lock(_lock);
+
+    if (_kinematics_table.find(robot_uuid) == _kinematics_table.end()) {
+        return std::make_pair(false, "Robot " + robot_uuid + " does not exist");
+    }
+
+    _kinematics_table.erase(robot_uuid);
+    _urdf_hashes.erase(robot_uuid);
+
+    return std::make_pair(true, "Successfully removed robot " + robot_uuid);
+}
+
+std::unordered_map<std::string, std::string> RobotManager::listRobots(bool detail) {
+    std::lock_guard<std::recursive_mutex> lock(_lock);
+
+    std::unordered_map<std::string, std::string> result;
+
+    if (!detail) {
+        for (auto it = _kinematics_table.begin(); it != _kinematics_table.end(); ++it) {
+            result[it->first] = "Robot Instance";
+        }
+        return result;
+    }
+
+    for (auto it = _kinematics_table.begin(); it != _kinematics_table.end(); ++it) {
+        const std::string& uuid = it->first;
+        const auto& robot = it->second;
+
+        std::stringstream info;
+        info << "Joints: " << robot->getNumberOfJoints() << ", "
+            << "Status: " << (_validateKinematics(robot) ? "valid" : "invalid") << ", "
+            << "Hash: " << _urdf_hashes.at(uuid).substr(0, 8) + "...";
+
+        result[uuid] = info.str();
+    }
+
+    return result;
+}
+
+size_t RobotManager::getRobotCount() {
+    std::lock_guard<std::recursive_mutex> lock(_lock);
+    return _kinematics_table.size();
+}
+
+void RobotManager::clearAll() {
+    std::lock_guard<std::recursive_mutex> lock(_lock);
+    _kinematics_table.clear();
+    _urdf_hashes.clear();
+}
+
+bool RobotManager::containsRobot(const std::string& robot_uuid) {
+    std::lock_guard<std::recursive_mutex> lock(_lock);
+    return _kinematics_table.find(robot_uuid) != _kinematics_table.end();
+}

src/function_metadata.h

@@ -0,0 +1,436 @@
+#ifndef FUNCTION_METADATA_H
+#define FUNCTION_METADATA_H
+
+#include <string>
+#include <vector>
+#include <map>
+#include <any>
+#include <functional>
+#include <memory>
+#include <algorithm>
+#include <cctype>
+#include <sstream> // 添加这行
+
+// 参数类型枚举
+enum class ParamType
+{
+    INT,
+    FLOAT,
+    DOUBLE,
+    BOOL,
+    STRING,
+    INT_ARRAY,
+    FLOAT_ARRAY,
+    VOID_PTR,
+    UNKNOWN
+};
+
+// 参数信息结构
+struct ParamInfo
+{
+    std::string name;
+    ParamType type;
+    std::any default_value;
+    bool is_optional;
+
+    ParamInfo(const std::string &n, ParamType t, std::any dv = {}, bool opt = false)
+        : name(n), type(t), default_value(dv), is_optional(opt) {}
+
+    // 获取类型名称
+    std::string getTypeName() const
+    {
+        switch (type)
+        {
+        case ParamType::INT:
+            return "int";
+        case ParamType::FLOAT:
+            return "float";
+        case ParamType::DOUBLE:
+            return "double";
+        case ParamType::BOOL:
+            return "bool";
+        case ParamType::STRING:
+            return "string";
+        case ParamType::INT_ARRAY:
+            return "int[]";
+        case ParamType::FLOAT_ARRAY:
+            return "float[]";
+        case ParamType::VOID_PTR:
+            return "void*";
+        default:
+            return "unknown";
+        }
+    }
+
+    // 类型转换
+    template <typename T>
+    T convert(const std::any &value) const
+    {
+        try
+        {
+            if constexpr (std::is_same_v<T, int>)
+            {
+                if (value.type() == typeid(int))
+                    return std::any_cast<int>(value);
+                if (value.type() == typeid(double))
+                    return static_cast<int>(std::any_cast<double>(value));
+                if (value.type() == typeid(float))
+                    return static_cast<int>(std::any_cast<float>(value));
+                if (value.type() == typeid(std::string))
+                {
+                    try
+                    {
+                        return std::stoi(std::any_cast<std::string>(value));
+                    }
+                    catch (...)
+                    {
+                        return 0;
+                    }
+                }
+                return std::any_cast<T>(value);
+            }
+            else if constexpr (std::is_same_v<T, float>)
+            {
+                if (value.type() == typeid(float))
+                    return std::any_cast<float>(value);
+                if (value.type() == typeid(double))
+                    return static_cast<float>(std::any_cast<double>(value));
+                if (value.type() == typeid(int))
+                    return static_cast<float>(std::any_cast<int>(value));
+                if (value.type() == typeid(std::string))
+                {
+                    try
+                    {
+                        return std::stof(std::any_cast<std::string>(value));
+                    }
+                    catch (...)
+                    {
+                        return 0.0f;
+                    }
+                }
+                return std::any_cast<T>(value);
+            }
+            else if constexpr (std::is_same_v<T, std::string>)
+            {
+                if (value.type() == typeid(std::string))
+                    return std::any_cast<std::string>(value);
+                // 其他类型转换为字符串
+                std::stringstream ss;
+                if (value.type() == typeid(int))
+                    ss << std::any_cast<int>(value);
+                else if (value.type() == typeid(float))
+                    ss << std::any_cast<float>(value);
+                else if (value.type() == typeid(double))
+                    ss << std::any_cast<double>(value);
+                else if (value.type() == typeid(bool))
+                    ss << (std::any_cast<bool>(value) ? "true" : "false");
+                else
+                    return "";
+                return ss.str();
+            }
+            else if constexpr (std::is_same_v<T, bool>)
+            {
+                if (value.type() == typeid(bool))
+                    return std::any_cast<bool>(value);
+                if (value.type() == typeid(int))
+                    return std::any_cast<int>(value) != 0;
+                if (value.type() == typeid(std::string))
+                {
+                    std::string str = std::any_cast<std::string>(value);
+                    std::string lower_str = str;
+                    std::transform(lower_str.begin(), lower_str.end(), lower_str.begin(),
+                                   [](unsigned char c)
+                                   { return std::tolower(c); });
+                    return lower_str == "true" || lower_str == "1" || lower_str == "yes" || lower_str == "on";
+                }
+                return false;
+            }
+            else
+            {
+                return std::any_cast<T>(value);
+            }
+        }
+        catch (const std::bad_any_cast &)
+        {
+            return T();
+        }
+    }
+};
+
+// 函数信息结构
+struct FunctionInfo
+{
+    std::string name;
+    std::string description;
+    std::vector<ParamInfo> params;
+    std::function<std::any(const std::vector<std::any> &)> handler;
+
+    // 参数名称映射，支持多个名称
+    std::map<std::string, std::string> param_aliases;
+
+    FunctionInfo(const std::string &n, const std::string &desc = "")
+        : name(n), description(desc) {}
+
+    // 添加参数
+    FunctionInfo &addParam(const std::string &name, ParamType type,
+                           std::any default_value = {}, bool optional = false)
+    {
+        params.emplace_back(name, type, default_value, optional);
+        return *this;
+    }
+
+    // 添加参数别名
+    FunctionInfo &addAlias(const std::string &original, const std::vector<std::string> &aliases)
+    {
+        for (const auto &alias : aliases)
+        {
+            param_aliases[alias] = original;
+        }
+        return *this;
+    }
+
+    // 设置处理器
+    template <typename Func>
+    FunctionInfo &setHandler(Func &&func)
+    {
+        handler = std::forward<Func>(func);
+        return *this;
+    }
+
+    // 获取规范化的参数名
+    std::string getCanonicalName(const std::string &input_name) const
+    {
+        // 检查别名
+        auto it = param_aliases.find(input_name);
+        if (it != param_aliases.end())
+        {
+            return it->second;
+        }
+
+        // 检查直接匹配（大小写不敏感）
+        std::string lower_input = toLower(input_name);
+
+        for (const auto &param : params)
+        {
+            std::string lower_param = toLower(param.name);
+
+            if (lower_param == lower_input)
+            {
+                return param.name;
+            }
+        }
+
+        return input_name; // 如果没有找到，返回原名称
+    }
+
+    // 验证参数
+    bool validateParams(const std::map<std::string, std::any> &input_params,
+                        std::string &error_msg) const
+    {
+        // 检查必需参数
+        for (const auto &param : params)
+        {
+            if (!param.is_optional)
+            {
+                bool found = false;
+                for (const auto &input : input_params)
+                {
+                    if (getCanonicalName(input.first) == param.name)
+                    {
+                        found = true;
+                        break;
+                    }
+                }
+                if (!found)
+                {
+                    error_msg = "Missing required parameter: " + param.name;
+                    return false;
+                }
+            }
+        }
+        return true;
+    }
+
+private:
+    static std::string toLower(const std::string &str)
+    {
+        std::string result = str;
+        std::transform(result.begin(), result.end(), result.begin(),
+                       [](unsigned char c)
+                       { return ::towlower(c); });
+        return result;
+    }
+};
+
+// 函数注册表
+class FunctionRegistry
+{
+private:
+    std::map<std::string, std::shared_ptr<FunctionInfo>> functions_;
+
+    FunctionRegistry() = default;
+
+public:
+    static FunctionRegistry &instance()
+    {
+        static FunctionRegistry instance;
+        return instance;
+    }
+
+    // 禁止复制
+    FunctionRegistry(const FunctionRegistry &) = delete;
+    FunctionRegistry &operator=(const FunctionRegistry &) = delete;
+
+    // 注册函数
+    void registerFunction(const std::shared_ptr<FunctionInfo> &func_info)
+    {
+        functions_[func_info->name] = func_info;
+    }
+
+    // 获取函数
+    std::shared_ptr<FunctionInfo> getFunction(const std::string &name)
+    {
+        // 直接查找
+        auto it = functions_.find(name);
+        if (it != functions_.end())
+        {
+            return it->second;
+        }
+
+        // 大小写不敏感查找
+        std::string lower_name = toLower(name);
+
+        for (const auto &[func_name, func_info] : functions_)
+        {
+            std::string lower_func = toLower(func_name);
+
+            if (lower_func == lower_name)
+            {
+                return func_info;
+            }
+        }
+
+        return nullptr;
+    }
+
+    // 获取所有函数
+    const std::map<std::string, std::shared_ptr<FunctionInfo>> &getAllFunctions() const
+    {
+        return functions_;
+    }
+
+    // 自动推导参数类型
+    ParamType deduceParamType(const std::any &value)
+    {
+        if (value.type() == typeid(int))
+            return ParamType::INT;
+        if (value.type() == typeid(float))
+            return ParamType::FLOAT;
+        if (value.type() == typeid(double))
+            return ParamType::DOUBLE;
+        if (value.type() == typeid(bool))
+            return ParamType::BOOL;
+        if (value.type() == typeid(std::string))
+            return ParamType::STRING;
+        if (value.type() == typeid(std::vector<int>))
+            return ParamType::INT_ARRAY;
+        if (value.type() == typeid(std::vector<float>))
+            return ParamType::FLOAT_ARRAY;
+        if (value.type() == typeid(void *))
+            return ParamType::VOID_PTR;
+        return ParamType::UNKNOWN;
+    }
+
+    // 智能参数匹配
+    std::map<std::string, std::any> smartMatchParams(
+        const std::shared_ptr<FunctionInfo> &func_info,
+        const std::map<std::string, std::any> &input_params)
+    {
+        std::map<std::string, std::any> matched_params;
+
+        // 1. 首先处理输入参数
+        for (const auto &[input_name, input_value] : input_params)
+        {
+            std::string canonical_name = func_info->getCanonicalName(input_name);
+
+            // 查找对应的参数定义
+            auto param_it = std::find_if(func_info->params.begin(), func_info->params.end(),
+                                         [&canonical_name](const ParamInfo &param)
+                                         {
+                                             return param.name == canonical_name;
+                                         });
+
+            if (param_it != func_info->params.end())
+            {
+                // 类型转换
+                std::any converted_value;
+                switch (param_it->type)
+                {
+                case ParamType::INT:
+                    converted_value = param_it->convert<int>(input_value);
+                    break;
+                case ParamType::FLOAT:
+                    converted_value = param_it->convert<float>(input_value);
+                    break;
+                case ParamType::STRING:
+                    converted_value = param_it->convert<std::string>(input_value);
+                    break;
+                case ParamType::BOOL:
+                    converted_value = param_it->convert<bool>(input_value);
+                    break;
+                default:
+                    converted_value = input_value; // 保持原类型
+                }
+                matched_params[param_it->name] = converted_value;
+            }
+        }
+
+        // 2. 填充默认值
+        for (const auto &param : func_info->params)
+        {
+            if (matched_params.find(param.name) == matched_params.end())
+            {
+                if (!param.default_value.has_value() && !param.is_optional)
+                {
+                    // 必需参数没有提供值，使用类型默认值
+                    switch (param.type)
+                    {
+                    case ParamType::INT:
+                        matched_params[param.name] = 0;
+                        break;
+                    case ParamType::FLOAT:
+                        matched_params[param.name] = 0.0f;
+                        break;
+                    case ParamType::STRING:
+                        matched_params[param.name] = std::string();
+                        break;
+                    case ParamType::BOOL:
+                        matched_params[param.name] = false;
+                        break;
+                    default:
+                        // 空值
+                        break;
+                    }
+                }
+                else if (param.default_value.has_value())
+                {
+                    matched_params[param.name] = param.default_value;
+                }
+            }
+        }
+
+        return matched_params;
+    }
+
+private:
+    static std::string toLower(const std::string &str)
+    {
+        std::string result = str;
+        std::transform(result.begin(), result.end(), result.begin(),
+                       [](unsigned char c)
+                       { return ::towlower(c); });
+        return result;
+    }
+};
+
+#endif // FUNCTION_METADATA_H

src/kdl_parser/include/kdl_parser/kdl_parser.hpp

@@ -0,0 +1,104 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2008, Willow Garage, Inc.
+*  All rights reserved.
+*
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+*
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+*
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/* Author: Wim Meeussen */
+
+#ifndef KDL_PARSER__KDL_PARSER_HPP_
+#define KDL_PARSER__KDL_PARSER_HPP_
+
+#include <kdl/tree.hpp>
+#include <string>
+#include <urdf_model/model.h>
+#include <tinyxml2.h>
+#include <tinyxml.h>  // NOLINT
+
+#include "kdl_parser/visibility_control.hpp"
+
+namespace kdl_parser
+{
+
+/** Constructs a KDL tree from a file, given the file name
+ * \param file The filename from where to read the xml
+ * \param tree The resulting KDL Tree
+ * returns true on success, false on failure
+ */
+	KDL_PARSER_PUBLIC
+		bool treeFromFile(const std::string& file, KDL::Tree& tree);
+
+	KDL_PARSER_PUBLIC
+		bool treeFromFileDocument(const std::string& file, KDL::Tree& tree);
+
+	/** Constructs a KDL tree from the parameter server, given the parameter name
+ * \param param the name of the parameter on the parameter server
+ * \param tree The resulting KDL Tree
+ * returns true on success, false on failure or if built without ROS
+ */
+KDL_PARSER_PUBLIC
+bool treeFromParam(const std::string & param, KDL::Tree & tree);
+
+/** Constructs a KDL tree from a string containing xml
+ * \param xml A string containing the xml description of the robot
+ * \param tree The resulting KDL Tree
+ * returns true on success, false on failure
+ */
+KDL_PARSER_PUBLIC
+bool treeFromString(const std::string & xml, KDL::Tree & tree);
+
+/** Constructs a KDL tree from a TinyXML2 document
+ * \param[in] xml_doc The document containing the xml description of the robot
+ * \param[out] tree The resulting KDL Tree
+ * \return true on success, false on failure
+ */
+KDL_PARSER_PUBLIC
+bool treeFromXml(const tinyxml2::XMLDocument * xml_doc, KDL::Tree & tree);
+
+/** Constructs a KDL tree from a TinyXML document
+ * \param[in] xml_doc The document containing the xml description of the robot
+ * \param[out] tree The resulting KDL Tree
+ * returns true on success, false on failure
+ */
+KDL_PARSER_PUBLIC
+KDL_PARSER_DEPRECATED("TinyXML API is deprecated, use the TinyXML2 version instead")
+bool treeFromXml(TiXmlDocument * xml_doc, KDL::Tree & tree);
+
+/** Constructs a KDL tree from a URDF robot model
+ * \param robot_model The URDF robot model
+ * \param tree The resulting KDL Tree
+ * returns true on success, false on failure
+ */
+KDL_PARSER_PUBLIC
+bool treeFromUrdfModel(const urdf::ModelInterface & robot_model, KDL::Tree & tree);
+}  // namespace kdl_parser
+
+#endif  // KDL_PARSER__KDL_PARSER_HPP_

src/kdl_parser/include/kdl_parser/visibility_control.hpp

@@ -0,0 +1,79 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2017, Open Source Robotics Foundation, Inc.
+*  All rights reserved.
+*
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+*
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the copyright holder nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+*
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/* This header must be included by all kdl_parser headers which declare symbols
+ * which are defined in the kdl_parser library. When not building the kdl_parser
+ * library, i.e. when using the headers in other package's code, the contents
+ * of this header change the visibility of certain symbols which the kdl_parser
+ * library cannot have, but the consuming code must have inorder to link.
+ */
+
+#ifndef KDL_PARSER__VISIBILITY_CONTROL_HPP_
+#define KDL_PARSER__VISIBILITY_CONTROL_HPP_
+
+// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
+//     https://gcc.gnu.org/wiki/Visibility
+
+#if defined _WIN32 || defined __CYGWIN__
+  #ifdef __GNUC__
+    #define KDL_PARSER_EXPORT __attribute__ ((dllexport))
+    #define KDL_PARSER_IMPORT __attribute__ ((dllimport))
+    #define KDL_PARSER_DEPRECATED(msg) __attribute__((deprecated(msg)))
+  #else
+    #define KDL_PARSER_EXPORT __declspec(dllexport)
+    #define KDL_PARSER_IMPORT __declspec(dllimport)
+    #define KDL_PARSER_DEPRECATED(msg) __declspec(deprecated(msg))
+  #endif
+  #ifdef KDL_PARSER_BUILDING_DLL
+    #define KDL_PARSER_PUBLIC KDL_PARSER_EXPORT
+  #else
+    #define KDL_PARSER_PUBLIC KDL_PARSER_IMPORT
+  #endif
+  #define KDL_PARSER_PUBLIC_TYPE KDL_PARSER_PUBLIC
+  #define KDL_PARSER_LOCAL
+#else
+  #define KDL_PARSER_EXPORT __attribute__ ((visibility("default")))
+  #define KDL_PARSER_IMPORT
+  #if __GNUC__ >= 4
+    #define KDL_PARSER_PUBLIC __attribute__ ((visibility("default")))
+    #define KDL_PARSER_LOCAL  __attribute__ ((visibility("hidden")))
+  #else
+    #define KDL_PARSER_PUBLIC
+    #define KDL_PARSER_LOCAL
+  #endif
+  #define KDL_PARSER_PUBLIC_TYPE
+  #define KDL_PARSER_DEPRECATED(msg) __attribute__((deprecated(msg)))
+#endif
+
+#endif  // KDL_PARSER__VISIBILITY_CONTROL_HPP_

src/kdl_parser/include/tinystr.h

@@ -0,0 +1,305 @@
+/*
+www.sourceforge.net/projects/tinyxml
+
+This software is provided 'as-is', without any express or implied
+warranty. In no event will the authors be held liable for any
+damages arising from the use of this software.
+
+Permission is granted to anyone to use this software for any
+purpose, including commercial applications, and to alter it and
+redistribute it freely, subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must
+not claim that you wrote the original software. If you use this
+software in a product, an acknowledgment in the product documentation
+would be appreciated but is not required.
+
+2. Altered source versions must be plainly marked as such, and
+must not be misrepresented as being the original software.
+
+3. This notice may not be removed or altered from any source
+distribution.
+*/
+
+
+#ifndef TIXML_USE_STL
+
+#ifndef TIXML_STRING_INCLUDED
+#define TIXML_STRING_INCLUDED
+
+#include <assert.h>
+#include <string.h>
+
+/*	The support for explicit isn't that universal, and it isn't really
+	required - it is used to check that the TiXmlString class isn't incorrectly
+	used. Be nice to old compilers and macro it here:
+*/
+#if defined(_MSC_VER) && (_MSC_VER >= 1200 )
+	// Microsoft visual studio, version 6 and higher.
+	#define TIXML_EXPLICIT explicit
+#elif defined(__GNUC__) && (__GNUC__ >= 3 )
+	// GCC version 3 and higher.s
+	#define TIXML_EXPLICIT explicit
+#else
+	#define TIXML_EXPLICIT
+#endif
+
+
+/*
+   TiXmlString is an emulation of a subset of the std::string template.
+   Its purpose is to allow compiling TinyXML on compilers with no or poor STL support.
+   Only the member functions relevant to the TinyXML project have been implemented.
+   The buffer allocation is made by a simplistic power of 2 like mechanism : if we increase
+   a string and there's no more room, we allocate a buffer twice as big as we need.
+*/
+class TiXmlString
+{
+  public :
+	// The size type used
+  	typedef size_t size_type;
+
+	// Error value for find primitive
+	static const size_type npos; // = -1;
+
+
+	// TiXmlString empty constructor
+	TiXmlString () : rep_(&nullrep_)
+	{
+	}
+
+	// TiXmlString copy constructor
+	TiXmlString ( const TiXmlString & copy) : rep_(0)
+	{
+		init(copy.length());
+		memcpy(start(), copy.data(), length());
+	}
+
+	// TiXmlString constructor, based on a string
+	TIXML_EXPLICIT TiXmlString ( const char * copy) : rep_(0)
+	{
+		init( static_cast<size_type>( strlen(copy) ));
+		memcpy(start(), copy, length());
+	}
+
+	// TiXmlString constructor, based on a string
+	TIXML_EXPLICIT TiXmlString ( const char * str, size_type len) : rep_(0)
+	{
+		init(len);
+		memcpy(start(), str, len);
+	}
+
+	// TiXmlString destructor
+	~TiXmlString ()
+	{
+		quit();
+	}
+
+	TiXmlString& operator = (const char * copy)
+	{
+		return assign( copy, (size_type)strlen(copy));
+	}
+
+	TiXmlString& operator = (const TiXmlString & copy)
+	{
+		return assign(copy.start(), copy.length());
+	}
+
+
+	// += operator. Maps to append
+	TiXmlString& operator += (const char * suffix)
+	{
+		return append(suffix, static_cast<size_type>( strlen(suffix) ));
+	}
+
+	// += operator. Maps to append
+	TiXmlString& operator += (char single)
+	{
+		return append(&single, 1);
+	}
+
+	// += operator. Maps to append
+	TiXmlString& operator += (const TiXmlString & suffix)
+	{
+		return append(suffix.data(), suffix.length());
+	}
+
+
+	// Convert a TiXmlString into a null-terminated char *
+	const char * c_str () const { return rep_->str; }
+
+	// Convert a TiXmlString into a char * (need not be null terminated).
+	const char * data () const { return rep_->str; }
+
+	// Return the length of a TiXmlString
+	size_type length () const { return rep_->size; }
+
+	// Alias for length()
+	size_type size () const { return rep_->size; }
+
+	// Checks if a TiXmlString is empty
+	bool empty () const { return rep_->size == 0; }
+
+	// Return capacity of string
+	size_type capacity () const { return rep_->capacity; }
+
+
+	// single char extraction
+	const char& at (size_type index) const
+	{
+		assert( index < length() );
+		return rep_->str[ index ];
+	}
+
+	// [] operator
+	char& operator [] (size_type index) const
+	{
+		assert( index < length() );
+		return rep_->str[ index ];
+	}
+
+	// find a char in a string. Return TiXmlString::npos if not found
+	size_type find (char lookup) const
+	{
+		return find(lookup, 0);
+	}
+
+	// find a char in a string from an offset. Return TiXmlString::npos if not found
+	size_type find (char tofind, size_type offset) const
+	{
+		if (offset >= length()) return npos;
+
+		for (const char* p = c_str() + offset; *p != '\0'; ++p)
+		{
+		   if (*p == tofind) return static_cast< size_type >( p - c_str() );
+		}
+		return npos;
+	}
+
+	void clear ()
+	{
+		//Lee:
+		//The original was just too strange, though correct:
+		//	TiXmlString().swap(*this);
+		//Instead use the quit & re-init:
+		quit();
+		init(0,0);
+	}
+
+	/*	Function to reserve a big amount of data when we know we'll need it. Be aware that this
+		function DOES NOT clear the content of the TiXmlString if any exists.
+	*/
+	void reserve (size_type cap);
+
+	TiXmlString& assign (const char* str, size_type len);
+
+	TiXmlString& append (const char* str, size_type len);
+
+	void swap (TiXmlString& other)
+	{
+		Rep* r = rep_;
+		rep_ = other.rep_;
+		other.rep_ = r;
+	}
+
+  private:
+
+	void init(size_type sz) { init(sz, sz); }
+	void set_size(size_type sz) { rep_->str[ rep_->size = sz ] = '\0'; }
+	char* start() const { return rep_->str; }
+	char* finish() const { return rep_->str + rep_->size; }
+
+	struct Rep
+	{
+		size_type size, capacity;
+		char str[1];
+	};
+
+	void init(size_type sz, size_type cap)
+	{
+		if (cap)
+		{
+			// Lee: the original form:
+			//	rep_ = static_cast<Rep*>(operator new(sizeof(Rep) + cap));
+			// doesn't work in some cases of new being overloaded. Switching
+			// to the normal allocation, although use an 'int' for systems
+			// that are overly picky about structure alignment.
+			const size_type bytesNeeded = sizeof(Rep) + cap;
+			const size_type intsNeeded = ( bytesNeeded + sizeof(int) - 1 ) / sizeof( int ); 
+			rep_ = reinterpret_cast<Rep*>( new int[ intsNeeded ] );
+
+			rep_->str[ rep_->size = sz ] = '\0';
+			rep_->capacity = cap;
+		}
+		else
+		{
+			rep_ = &nullrep_;
+		}
+	}
+
+	void quit()
+	{
+		if (rep_ != &nullrep_)
+		{
+			// The rep_ is really an array of ints. (see the allocator, above).
+			// Cast it back before delete, so the compiler won't incorrectly call destructors.
+			delete [] ( reinterpret_cast<int*>( rep_ ) );
+		}
+	}
+
+	Rep * rep_;
+	static Rep nullrep_;
+
+} ;
+
+
+inline bool operator == (const TiXmlString & a, const TiXmlString & b)
+{
+	return    ( a.length() == b.length() )				// optimization on some platforms
+	       && ( strcmp(a.c_str(), b.c_str()) == 0 );	// actual compare
+}
+inline bool operator < (const TiXmlString & a, const TiXmlString & b)
+{
+	return strcmp(a.c_str(), b.c_str()) < 0;
+}
+
+inline bool operator != (const TiXmlString & a, const TiXmlString & b) { return !(a == b); }
+inline bool operator >  (const TiXmlString & a, const TiXmlString & b) { return b < a; }
+inline bool operator <= (const TiXmlString & a, const TiXmlString & b) { return !(b < a); }
+inline bool operator >= (const TiXmlString & a, const TiXmlString & b) { return !(a < b); }
+
+inline bool operator == (const TiXmlString & a, const char* b) { return strcmp(a.c_str(), b) == 0; }
+inline bool operator == (const char* a, const TiXmlString & b) { return b == a; }
+inline bool operator != (const TiXmlString & a, const char* b) { return !(a == b); }
+inline bool operator != (const char* a, const TiXmlString & b) { return !(b == a); }
+
+TiXmlString operator + (const TiXmlString & a, const TiXmlString & b);
+TiXmlString operator + (const TiXmlString & a, const char* b);
+TiXmlString operator + (const char* a, const TiXmlString & b);
+
+
+/*
+   TiXmlOutStream is an emulation of std::ostream. It is based on TiXmlString.
+   Only the operators that we need for TinyXML have been developped.
+*/
+class TiXmlOutStream : public TiXmlString
+{
+public :
+
+	// TiXmlOutStream << operator.
+	TiXmlOutStream & operator << (const TiXmlString & in)
+	{
+		*this += in;
+		return *this;
+	}
+
+	// TiXmlOutStream << operator.
+	TiXmlOutStream & operator << (const char * in)
+	{
+		*this += in;
+		return *this;
+	}
+
+} ;
+
+#endif	// TIXML_STRING_INCLUDED
+#endif	// TIXML_USE_STL

src/kdl_parser/include/urdf_exception/exception.h

@@ -0,0 +1,53 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+* 
+*  Copyright (c) 2008, Willow Garage, Inc.
+*  All rights reserved.
+* 
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+* 
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+* 
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+// URDF exceptions
+#ifndef URDF_INTERFACE_EXCEPTION_H_
+#define URDF_INTERFACE_EXCEPTION_H_
+
+#include <string>
+#include <stdexcept>
+
+namespace urdf
+{
+
+class ParseError: public std::runtime_error
+{
+public:
+  ParseError(const std::string &error_msg) : std::runtime_error(error_msg) {};
+};
+
+}
+
+#endif

src/kdl_parser/include/urdf_model/color.h

@@ -0,0 +1,105 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2008, Willow Garage, Inc.
+*  All rights reserved.
+*
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+*
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+*
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/* Author: Josh Faust */
+
+#ifndef URDF_INTERFACE_COLOR_H
+#define URDF_INTERFACE_COLOR_H
+
+#include <stdexcept>
+#include <string>
+#include <vector>
+#include <math.h>
+
+#include <urdf_model/utils.h>
+#include <urdf_exception/exception.h>
+
+namespace urdf
+{
+
+class Color
+{
+public:
+  Color() {this->clear();};
+  float r;
+  float g;
+  float b;
+  float a;
+
+  void clear()
+  {
+    r = g = b = 0.0f;
+    a = 1.0f;
+  }
+  bool init(const std::string &vector_str)
+  {
+    this->clear();
+    std::vector<std::string> pieces;
+    std::vector<float> rgba;
+    urdf::split_string( pieces, vector_str, " ");
+    for (unsigned int i = 0; i < pieces.size(); ++i)
+    {
+      if (!pieces[i].empty())
+      {
+        try
+        {
+          double piece = strToDouble(pieces[i].c_str());
+          if ((piece < 0) || (piece > 1))
+            throw ParseError("Component [" + pieces[i] + "] is outside the valid range for colors [0, 1]");
+          rgba.push_back(static_cast<float>(piece));
+        }
+        catch (std::runtime_error &/*e*/) {
+          throw ParseError("Unable to parse component [" + pieces[i] + "] to a double (while parsing a color value)");
+        }
+      }
+    }
+
+    if (rgba.size() != 4)
+    {
+      return false;
+    }
+
+    this->r = rgba[0];
+    this->g = rgba[1];
+    this->b = rgba[2];
+    this->a = rgba[3];
+
+    return true;
+  };
+};
+
+}
+
+#endif
+

src/kdl_parser/include/urdf_model/joint.h

@@ -0,0 +1,230 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+* 
+*  Copyright (c) 2008, Willow Garage, Inc.
+*  All rights reserved.
+* 
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+* 
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+* 
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/* Author: Wim Meeussen */
+
+#ifndef URDF_INTERFACE_JOINT_H
+#define URDF_INTERFACE_JOINT_H
+
+#include <string>
+#include <vector>
+
+#include "urdf_model/pose.h"
+#include "urdf_model/types.h"
+
+
+namespace urdf{
+
+class Link;
+
+class JointDynamics
+{
+public:
+  JointDynamics() { this->clear(); };
+  double damping;
+  double friction;
+
+  void clear()
+  {
+    damping = 0;
+    friction = 0;
+  };
+};
+
+class JointLimits
+{
+public:
+  JointLimits() { this->clear(); };
+  double lower;
+  double upper;
+  double effort;
+  double velocity;
+
+  void clear()
+  {
+    lower = 0;
+    upper = 0;
+    effort = 0;
+    velocity = 0;
+  };
+};
+
+/// \brief Parameters for Joint Safety Controllers
+class JointSafety
+{
+public:
+  /// clear variables on construction
+  JointSafety() { this->clear(); };
+
+  /// 
+  /// IMPORTANT:  The safety controller support is very much PR2 specific, not intended for generic usage.
+  /// 
+  /// Basic safety controller operation is as follows
+  /// 
+  /// current safety controllers will take effect on joints outside the position range below:
+  ///
+  /// position range: [JointSafety::soft_lower_limit  + JointLimits::velocity / JointSafety::k_position, 
+  ///                  JointSafety::soft_uppper_limit - JointLimits::velocity / JointSafety::k_position]
+  ///
+  /// if (joint_position is outside of the position range above)
+  ///     velocity_limit_min = -JointLimits::velocity + JointSafety::k_position * (joint_position - JointSafety::soft_lower_limit)
+  ///     velocity_limit_max =  JointLimits::velocity + JointSafety::k_position * (joint_position - JointSafety::soft_upper_limit)
+  /// else
+  ///     velocity_limit_min = -JointLimits::velocity
+  ///     velocity_limit_max =  JointLimits::velocity
+  ///
+  /// velocity range: [velocity_limit_min + JointLimits::effort / JointSafety::k_velocity,
+  ///                  velocity_limit_max - JointLimits::effort / JointSafety::k_velocity]
+  ///
+  /// if (joint_velocity is outside of the velocity range above)
+  ///     effort_limit_min = -JointLimits::effort + JointSafety::k_velocity * (joint_velocity - velocity_limit_min)
+  ///     effort_limit_max =  JointLimits::effort + JointSafety::k_velocity * (joint_velocity - velocity_limit_max)
+  /// else
+  ///     effort_limit_min = -JointLimits::effort
+  ///     effort_limit_max =  JointLimits::effort
+  ///
+  /// Final effort command sent to the joint is saturated by [effort_limit_min,effort_limit_max]
+  ///
+  /// Please see wiki for more details: http://www.ros.org/wiki/pr2_controller_manager/safety_limits
+  /// 
+  double soft_upper_limit;
+  double soft_lower_limit;
+  double k_position;
+  double k_velocity;
+
+  void clear()
+  {
+    soft_upper_limit = 0;
+    soft_lower_limit = 0;
+    k_position = 0;
+    k_velocity = 0;
+  };
+};
+
+
+class JointCalibration
+{
+public:
+  JointCalibration() { this->clear(); };
+  double reference_position;
+  DoubleSharedPtr rising, falling;
+
+  void clear()
+  {
+    reference_position = 0;
+  };
+};
+
+class JointMimic
+{
+public:
+  JointMimic() { this->clear(); };
+  double offset;
+  double multiplier;
+  std::string joint_name;
+
+  void clear()
+  {
+    offset = 0.0;
+    multiplier = 0.0;
+    joint_name.clear();
+  };
+};
+
+
+class Joint
+{
+public:
+
+  Joint() { this->clear(); };
+
+  std::string name;
+  enum
+  {
+    UNKNOWN, REVOLUTE, CONTINUOUS, PRISMATIC, FLOATING, PLANAR, FIXED
+  } type;
+
+  /// \brief     type_       meaning of axis_
+  /// ------------------------------------------------------
+  ///            UNKNOWN     unknown type
+  ///            REVOLUTE    rotation axis
+  ///            PRISMATIC   translation axis
+  ///            FLOATING    N/A
+  ///            PLANAR      plane normal axis
+  ///            FIXED       N/A
+  Vector3 axis;
+
+  /// child Link element
+  ///   child link frame is the same as the Joint frame
+  std::string child_link_name;
+
+  /// parent Link element
+  ///   origin specifies the transform from Parent Link to Joint Frame
+  std::string parent_link_name;
+  /// transform from Parent Link frame to Joint frame
+  Pose  parent_to_joint_origin_transform;
+
+  /// Joint Dynamics
+  JointDynamicsSharedPtr dynamics;
+
+  /// Joint Limits
+  JointLimitsSharedPtr limits;
+
+  /// Unsupported Hidden Feature
+  JointSafetySharedPtr safety;
+
+  /// Unsupported Hidden Feature
+  JointCalibrationSharedPtr calibration;
+
+  /// Option to Mimic another Joint
+  JointMimicSharedPtr mimic;
+
+  void clear()
+  {
+    this->axis.clear();
+    this->child_link_name.clear();
+    this->parent_link_name.clear();
+    this->parent_to_joint_origin_transform.clear();
+    this->dynamics.reset();
+    this->limits.reset();
+    this->safety.reset();
+    this->calibration.reset();
+    this->mimic.reset();
+    this->type = UNKNOWN;
+  };
+};
+
+}
+
+#endif

src/kdl_parser/include/urdf_model/link.h

@@ -0,0 +1,247 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+* 
+*  Copyright (c) 2008, Willow Garage, Inc.
+*  All rights reserved.
+* 
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+* 
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+* 
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/* Author: Wim Meeussen */
+
+#ifndef URDF_INTERFACE_LINK_H
+#define URDF_INTERFACE_LINK_H
+
+#include <string>
+#include <vector>
+#include <map>
+
+#include "joint.h"
+#include "color.h"
+#include "types.h"
+
+namespace urdf{
+
+class Geometry
+{
+public:
+  enum {SPHERE, BOX, CYLINDER, MESH} type;
+
+  virtual ~Geometry(void)
+  {
+  }  
+};
+
+class Sphere : public Geometry
+{
+public:
+  Sphere() { this->clear(); type = SPHERE; };
+  double radius;
+
+  void clear()
+  {
+    radius = 0;
+  };
+};
+
+class Box : public Geometry
+{
+public:
+  Box() { this->clear(); type = BOX; };
+  Vector3 dim;
+
+  void clear()
+  {
+    this->dim.clear();
+  };
+};
+
+class Cylinder : public Geometry
+{
+public:
+  Cylinder() { this->clear(); type = CYLINDER; };
+  double length;
+  double radius;
+
+  void clear()
+  {
+    length = 0;
+    radius = 0;
+  };
+};
+
+class Mesh : public Geometry
+{
+public:
+  Mesh() { this->clear(); type = MESH; };
+  std::string filename;
+  Vector3 scale;
+
+  void clear()
+  {
+    filename.clear();
+    // default scale
+    scale.x = 1;
+    scale.y = 1;
+    scale.z = 1;
+  };
+};
+
+class Material
+{
+public:
+  Material() { this->clear(); };
+  std::string name;
+  std::string texture_filename;
+  Color color;
+
+  void clear()
+  {
+    color.clear();
+    texture_filename.clear();
+    name.clear();
+  };
+};
+
+class Inertial
+{
+public:
+  Inertial() { this->clear(); };
+  Pose origin;
+  double mass;
+  double ixx,ixy,ixz,iyy,iyz,izz;
+
+  void clear()
+  {
+    origin.clear();
+    mass = 0;
+    ixx = ixy = ixz = iyy = iyz = izz = 0;
+  };
+};
+
+class Visual
+{
+public:
+  Visual() { this->clear(); };
+  Pose origin;
+  GeometrySharedPtr geometry;
+
+  std::string material_name;
+  MaterialSharedPtr material;
+
+  void clear()
+  {
+    origin.clear();
+    material_name.clear();
+    material.reset();
+    geometry.reset();
+    name.clear();
+  };
+
+  std::string name;
+};
+
+class Collision
+{
+public:
+  Collision() { this->clear(); };
+  Pose origin;
+  GeometrySharedPtr geometry;
+
+  void clear()
+  {
+    origin.clear();
+    geometry.reset();
+    name.clear();
+  };
+
+  std::string name;
+
+};
+
+
+class Link
+{
+public:
+  Link() { this->clear(); };
+
+  std::string name;
+
+  /// inertial element
+  InertialSharedPtr inertial;
+
+  /// visual element
+  VisualSharedPtr visual;
+
+  /// collision element
+  CollisionSharedPtr collision;
+
+  /// if more than one collision element is specified, all collision elements are placed in this array (the collision member points to the first element of the array)
+  std::vector<CollisionSharedPtr> collision_array;
+
+  /// if more than one visual element is specified, all visual elements are placed in this array (the visual member points to the first element of the array)
+  std::vector<VisualSharedPtr> visual_array;
+
+  /// Parent Joint element
+  ///   explicitly stating "parent" because we want directional-ness for tree structure
+  ///   every link can have one parent
+  JointSharedPtr parent_joint;
+
+  std::vector<JointSharedPtr> child_joints;
+  std::vector<LinkSharedPtr> child_links;
+
+  LinkSharedPtr getParent() const
+  {return parent_link_.lock();};
+
+  void setParent(const LinkSharedPtr &parent)
+  { parent_link_ = parent; }
+  
+  void clear()
+  {
+    this->name.clear();
+    this->inertial.reset();
+    this->visual.reset();
+    this->collision.reset();
+    this->parent_joint.reset();
+    this->child_joints.clear();
+    this->child_links.clear();
+    this->collision_array.clear();
+    this->visual_array.clear();
+  };
+
+private:
+  LinkWeakPtr parent_link_;
+
+};
+
+
+
+
+}
+
+#endif

src/kdl_parser/include/urdf_model/model.h

@@ -0,0 +1,206 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+* 
+*  Copyright (c) 2008, Willow Garage, Inc.
+*  All rights reserved.
+* 
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+* 
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+* 
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/* Author: Wim Meeussen */
+
+#ifndef URDF_INTERFACE_MODEL_H
+#define URDF_INTERFACE_MODEL_H
+
+#include <string>
+#include <map>
+#include <urdf_model/link.h>
+#include <urdf_model/types.h>
+#include <urdf_exception/exception.h>
+
+namespace urdf {
+
+class ModelInterface
+{
+public:
+  LinkConstSharedPtr getRoot(void) const{return this->root_link_;};
+  LinkConstSharedPtr getLink(const std::string& name) const
+  {
+    LinkConstSharedPtr ptr;
+    if (this->links_.find(name) == this->links_.end())
+      ptr.reset();
+    else
+      ptr = this->links_.find(name)->second;
+    return ptr;
+  };
+  
+  JointConstSharedPtr getJoint(const std::string& name) const
+  {
+    JointConstSharedPtr ptr;
+    if (this->joints_.find(name) == this->joints_.end())
+      ptr.reset();
+    else
+      ptr = this->joints_.find(name)->second;
+    return ptr;
+  };
+  
+  
+  const std::string& getName() const {return name_;};
+  void getLinks(std::vector<LinkSharedPtr >& links) const
+  {
+    for (std::map<std::string,LinkSharedPtr>::const_iterator link = this->links_.begin();link != this->links_.end(); link++)
+    {
+      links.push_back(link->second);
+    }
+  };
+  
+  void clear()
+  {
+    name_.clear();
+    this->links_.clear();
+    this->joints_.clear();
+    this->materials_.clear();
+    this->root_link_.reset();
+  };
+  
+  /// non-const getLink()
+  void getLink(const std::string& name, LinkSharedPtr &link) const
+  {
+    LinkSharedPtr ptr;
+    if (this->links_.find(name) == this->links_.end())
+      ptr.reset();
+    else
+      ptr = this->links_.find(name)->second;
+    link = ptr;
+  };
+  
+  /// non-const getMaterial()
+  MaterialSharedPtr getMaterial(const std::string& name) const
+  {
+    MaterialSharedPtr ptr;
+    if (this->materials_.find(name) == this->materials_.end())
+      ptr.reset();
+    else
+      ptr = this->materials_.find(name)->second;
+    return ptr;
+  };
+  
+  void initTree(std::map<std::string, std::string> &parent_link_tree)
+  {
+    // loop through all joints, for every link, assign children links and children joints
+    for (std::map<std::string, JointSharedPtr>::iterator joint = this->joints_.begin();joint != this->joints_.end(); joint++)
+    {
+      std::string parent_link_name = joint->second->parent_link_name;
+      std::string child_link_name = joint->second->child_link_name;
+      
+      if (parent_link_name.empty() || child_link_name.empty())
+      {
+        throw ParseError("Joint [" + joint->second->name + "] is missing a parent and/or child link specification.");
+      }
+      else
+      {
+        // find child and parent links
+        LinkSharedPtr child_link, parent_link;
+        this->getLink(child_link_name, child_link);
+        if (!child_link)
+        {
+          throw ParseError("child link [" + child_link_name + "] of joint [" + joint->first + "] not found");
+        }
+        this->getLink(parent_link_name, parent_link);
+        if (!parent_link)
+        {
+          throw ParseError("parent link [" + parent_link_name + "] of joint [" + joint->first + "] not found.  This is not valid according to the URDF spec. Every link you refer to from a joint needs to be explicitly defined in the robot description. To fix this problem you can either remove this joint [" + joint->first + "] from your urdf file, or add \"<link name=\"" + parent_link_name + "\" />\" to your urdf file.");
+        }
+        
+        //set parent link for child link
+        child_link->setParent(parent_link);
+
+        //set parent joint for child link        
+        child_link->parent_joint = joint->second;
+        
+        //set child joint for parent link
+        parent_link->child_joints.push_back(joint->second);
+
+        //set child link for parent link
+        parent_link->child_links.push_back(child_link);
+
+        // fill in child/parent string map
+        parent_link_tree[child_link->name] = parent_link_name;
+      }
+    }
+  }
+  
+  void initRoot(const std::map<std::string, std::string> &parent_link_tree)
+  { 
+    this->root_link_.reset();
+    
+    // find the links that have no parent in the tree
+    for (std::map<std::string, LinkSharedPtr>::const_iterator l=this->links_.begin(); l!=this->links_.end(); l++)
+    {
+      std::map<std::string, std::string >::const_iterator parent = parent_link_tree.find(l->first);
+      if (parent == parent_link_tree.end())
+      {
+        // store root link
+        if (!this->root_link_)
+        {
+          getLink(l->first, this->root_link_);
+        }
+        // we already found a root link
+        else
+        {
+          throw ParseError("Two root links found: [" + this->root_link_->name + "] and [" + l->first + "]");
+        }
+      }
+    }
+    if (!this->root_link_)
+    {
+      throw ParseError("No root link found. The robot xml is not a valid tree.");
+    }
+  }
+  
+  
+  /// \brief complete list of Links
+  std::map<std::string, LinkSharedPtr> links_;
+  /// \brief complete list of Joints
+  std::map<std::string, JointSharedPtr> joints_;
+  /// \brief complete list of Materials
+  std::map<std::string, MaterialSharedPtr> materials_;
+
+  /// \brief The name of the robot model
+  std::string name_;
+
+  /// \brief The root is always a link (the parent of the tree describing the robot)
+  LinkSharedPtr root_link_;
+
+
+
+};
+
+}
+
+#endif

src/kdl_parser/include/urdf_model/pose.h

@@ -0,0 +1,260 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2008, Willow Garage, Inc.
+*  All rights reserved.
+*
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+*
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+*
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/* Author: Wim Meeussen */
+
+#ifndef URDF_INTERFACE_POSE_H
+#define URDF_INTERFACE_POSE_H
+
+#include <cmath>
+#include <sstream>
+#include <stdexcept>
+#include <string>
+#include <vector>
+
+#include <urdf_exception/exception.h>
+#include <urdf_model/utils.h>
+
+namespace urdf{
+
+class Vector3
+{
+public:
+  Vector3(double _x,double _y, double _z) {this->x=_x;this->y=_y;this->z=_z;};
+  Vector3() {this->clear();};
+  double x;
+  double y;
+  double z;
+
+  void clear() {this->x=this->y=this->z=0.0;};
+  void init(const std::string &vector_str)
+  {
+    this->clear();
+    std::vector<std::string> pieces;
+    std::vector<double> xyz;
+    urdf::split_string( pieces, vector_str, " ");
+    for (unsigned int i = 0; i < pieces.size(); ++i){
+      if (pieces[i] != ""){
+        try {
+          xyz.push_back(strToDouble(pieces[i].c_str()));
+        } catch(std::runtime_error &) {
+          throw ParseError("Unable to parse component [" + pieces[i] + "] to a double (while parsing a vector value)");
+        }
+      }
+    }
+
+    if (xyz.size() != 3)
+      throw ParseError("Parser found " + std::to_string(xyz.size())  + " elements but 3 expected while parsing vector [" + vector_str + "]");
+
+    this->x = xyz[0];
+    this->y = xyz[1];
+    this->z = xyz[2];
+  }
+
+  Vector3 operator+(Vector3 vec)
+  {
+    return Vector3(this->x+vec.x,this->y+vec.y,this->z+vec.z);
+  };
+};
+
+class Rotation
+{
+public:
+  Rotation(double _x,double _y, double _z, double _w) {this->x=_x;this->y=_y;this->z=_z;this->w=_w;};
+  Rotation() {this->clear();};
+  void getQuaternion(double &quat_x,double &quat_y,double &quat_z, double &quat_w) const
+  {
+    quat_x = this->x;
+    quat_y = this->y;
+    quat_z = this->z;
+    quat_w = this->w;
+  };
+  void getRPY(double &roll,double &pitch,double &yaw) const
+  {
+    double sqw;
+    double sqx;
+    double sqy;
+    double sqz;
+
+    sqx = this->x * this->x;
+    sqy = this->y * this->y;
+    sqz = this->z * this->z;
+    sqw = this->w * this->w;
+
+    // Cases derived from https://orbitalstation.wordpress.com/tag/quaternion/
+    double sarg = -2 * (this->x*this->z - this->w*this->y);
+    const double pi_2 = 1.57079632679489661923;
+    if (sarg <= -0.99999) {
+      pitch = -pi_2;
+      roll  = 0;
+      yaw   = 2 * atan2(this->x, -this->y);
+    } else if (sarg >= 0.99999) {
+      pitch = pi_2;
+      roll  = 0;
+      yaw   = 2 * atan2(-this->x, this->y);
+    } else {
+      pitch = asin(sarg);
+      roll  = atan2(2 * (this->y*this->z + this->w*this->x), sqw - sqx - sqy + sqz);
+      yaw   = atan2(2 * (this->x*this->y + this->w*this->z), sqw + sqx - sqy - sqz);
+    }
+
+  };
+  void setFromQuaternion(double quat_x,double quat_y,double quat_z,double quat_w)
+  {
+    this->x = quat_x;
+    this->y = quat_y;
+    this->z = quat_z;
+    this->w = quat_w;
+    this->normalize();
+  };
+  void setFromRPY(double roll, double pitch, double yaw)
+  {
+    double phi, the, psi;
+
+    phi = roll / 2.0;
+    the = pitch / 2.0;
+    psi = yaw / 2.0;
+
+    this->x = sin(phi) * cos(the) * cos(psi) - cos(phi) * sin(the) * sin(psi);
+    this->y = cos(phi) * sin(the) * cos(psi) + sin(phi) * cos(the) * sin(psi);
+    this->z = cos(phi) * cos(the) * sin(psi) - sin(phi) * sin(the) * cos(psi);
+    this->w = cos(phi) * cos(the) * cos(psi) + sin(phi) * sin(the) * sin(psi);
+
+    this->normalize();
+  };
+
+  double x,y,z,w;
+
+  void init(const std::string &rotation_str)
+  {
+    this->clear();
+    Vector3 rpy;
+    rpy.init(rotation_str);
+    setFromRPY(rpy.x, rpy.y, rpy.z);
+  }
+
+  void clear() { this->x=this->y=this->z=0.0;this->w=1.0; }
+
+  void normalize()
+  {
+    double s = sqrt(this->x * this->x +
+                    this->y * this->y +
+                    this->z * this->z +
+                    this->w * this->w);
+    if (s == 0.0)
+    {
+      this->x = 0.0;
+      this->y = 0.0;
+      this->z = 0.0;
+      this->w = 1.0;
+    }
+    else
+    {
+      this->x /= s;
+      this->y /= s;
+      this->z /= s;
+      this->w /= s;
+    }
+  };
+
+  // Multiplication operator (copied from gazebo)
+  Rotation operator*( const Rotation &qt ) const
+  {
+    Rotation c;
+
+    c.x = this->w * qt.x + this->x * qt.w + this->y * qt.z - this->z * qt.y;
+    c.y = this->w * qt.y - this->x * qt.z + this->y * qt.w + this->z * qt.x;
+    c.z = this->w * qt.z + this->x * qt.y - this->y * qt.x + this->z * qt.w;
+    c.w = this->w * qt.w - this->x * qt.x - this->y * qt.y - this->z * qt.z;
+
+    return c;
+  };
+  /// Rotate a vector using the quaternion
+  Vector3 operator*(Vector3 vec) const
+  {
+    Rotation tmp;
+    Vector3 result;
+
+    tmp.w = 0.0;
+    tmp.x = vec.x;
+    tmp.y = vec.y;
+    tmp.z = vec.z;
+
+    tmp = (*this) * (tmp * this->GetInverse());
+
+    result.x = tmp.x;
+    result.y = tmp.y;
+    result.z = tmp.z;
+
+    return result;
+  };
+  // Get the inverse of this quaternion
+  Rotation GetInverse() const
+  {
+    Rotation q;
+
+    double norm = this->w*this->w+this->x*this->x+this->y*this->y+this->z*this->z;
+
+    if (norm > 0.0)
+    {
+      q.w = this->w / norm;
+      q.x = -this->x / norm;
+      q.y = -this->y / norm;
+      q.z = -this->z / norm;
+    }
+
+    return q;
+  };
+
+
+};
+
+class Pose
+{
+public:
+  Pose() { this->clear(); };
+
+  Vector3  position;
+  Rotation rotation;
+
+  void clear()
+  {
+    this->position.clear();
+    this->rotation.clear();
+  };
+};
+
+}
+
+#endif

src/kdl_parser/include/urdf_model/twist.h

@@ -0,0 +1,68 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+* 
+*  Copyright (c) 2008, Willow Garage, Inc.
+*  All rights reserved.
+* 
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+* 
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+* 
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/* Author: John Hsu */
+
+#ifndef URDF_TWIST_H
+#define URDF_TWIST_H
+
+#include <string>
+#include <sstream>
+#include <vector>
+#include <math.h>
+#include <urdf_model/pose.h>
+
+namespace urdf{
+
+
+class Twist
+{
+public:
+  Twist() { this->clear(); };
+
+  Vector3  linear;
+  // Angular velocity represented by Euler angles
+  Vector3  angular;
+
+  void clear()
+  {
+    this->linear.clear();
+    this->angular.clear();
+  };
+};
+
+}
+
+#endif
+

src/kdl_parser/include/urdf_model/types.h

@@ -0,0 +1,91 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2008, Willow Garage, Inc.
+*  All rights reserved.
+*
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+*
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+*
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/* Author: Steve Peters */
+
+#ifndef URDF_MODEL_TYPES_H
+#define URDF_MODEL_TYPES_H
+
+#include <memory>
+
+#define URDF_TYPEDEF_CLASS_POINTER(Class) \
+class Class; \
+typedef std::shared_ptr<Class> Class##SharedPtr; \
+typedef std::shared_ptr<const Class> Class##ConstSharedPtr; \
+typedef std::weak_ptr<Class> Class##WeakPtr
+
+namespace urdf{
+
+// shared pointer used in joint.h
+typedef std::shared_ptr<double> DoubleSharedPtr;
+
+URDF_TYPEDEF_CLASS_POINTER(Box);
+URDF_TYPEDEF_CLASS_POINTER(Collision);
+URDF_TYPEDEF_CLASS_POINTER(Cylinder);
+URDF_TYPEDEF_CLASS_POINTER(Geometry);
+URDF_TYPEDEF_CLASS_POINTER(Inertial);
+URDF_TYPEDEF_CLASS_POINTER(Joint);
+URDF_TYPEDEF_CLASS_POINTER(JointCalibration);
+URDF_TYPEDEF_CLASS_POINTER(JointDynamics);
+URDF_TYPEDEF_CLASS_POINTER(JointLimits);
+URDF_TYPEDEF_CLASS_POINTER(JointMimic);
+URDF_TYPEDEF_CLASS_POINTER(JointSafety);
+URDF_TYPEDEF_CLASS_POINTER(Link);
+URDF_TYPEDEF_CLASS_POINTER(Material);
+URDF_TYPEDEF_CLASS_POINTER(Mesh);
+URDF_TYPEDEF_CLASS_POINTER(Sphere);
+URDF_TYPEDEF_CLASS_POINTER(Visual);
+
+// create *_pointer_cast functions in urdf namespace
+template<class T, class U>
+std::shared_ptr<T> const_pointer_cast(std::shared_ptr<U> const & r)
+{
+  return std::const_pointer_cast<T>(r);
+}
+
+template<class T, class U>
+std::shared_ptr<T> dynamic_pointer_cast(std::shared_ptr<U> const & r)
+{
+  return std::dynamic_pointer_cast<T>(r);
+}
+
+template<class T, class U>
+std::shared_ptr<T> static_pointer_cast(std::shared_ptr<U> const & r)
+{
+  return std::static_pointer_cast<T>(r);
+}
+
+}
+
+#endif

src/kdl_parser/include/urdf_model/utils.h

@@ -0,0 +1,92 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2016, Open Source Robotics Foundation (OSRF)
+*  All rights reserved.
+*
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+*
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the OSRF nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+*
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/* Author: Steve Peters */
+
+#ifndef URDF_INTERFACE_UTILS_H
+#define URDF_INTERFACE_UTILS_H
+
+#include <locale>
+#include <sstream>
+#include <stdexcept>
+#include <string>
+#include <vector>
+
+namespace urdf {
+
+// Replacement for boost::split( ... , ... , boost::is_any_of(" "))
+inline
+void split_string(std::vector<std::string> &result,
+                  const std::string &input,
+                  const std::string &isAnyOf)
+{
+  std::string::size_type start = 0;
+  std::string::size_type end = input.find_first_of(isAnyOf, start);
+  while (end != std::string::npos)
+  {
+    result.push_back(input.substr(start, end-start));
+    start = end + 1;
+    end = input.find_first_of(isAnyOf, start);
+  }
+  if (start < input.length())
+  {
+    result.push_back(input.substr(start));
+  }
+}
+
+// This is a locale-safe version of string-to-double, which is suprisingly
+// difficult to do correctly.  This function ensures that the C locale is used
+// for parsing, as that matches up with what the XSD for double specifies.
+// On success, the double is returned; on failure, a std::runtime_error is
+// thrown.
+static inline double strToDouble(const char *in)
+{
+  std::stringstream ss;
+  ss.imbue(std::locale::classic());
+
+  ss << in;
+
+  double out;
+  ss >> out;
+
+  if (ss.fail() || !ss.eof()) {
+    throw std::runtime_error("Failed converting string to double");
+  }
+
+  return out;
+}
+
+}
+
+#endif

src/kdl_parser/include/urdf_model_state/model_state.h

@@ -0,0 +1,151 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+* 
+*  Copyright (c) 2008, Willow Garage, Inc.
+*  All rights reserved.
+* 
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+* 
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+* 
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/* Author: John Hsu */
+
+#ifndef URDF_MODEL_STATE_H
+#define URDF_MODEL_STATE_H
+
+#include <string>
+#include <vector>
+#include <map>
+
+#include "urdf_model/pose.h"
+#include <urdf_model/twist.h>
+#include "urdf_model_state/types.h"
+
+
+namespace urdf{
+
+//round is not defined in C++98
+//So in Visual Studio <= 2012 is necessary to define it
+#ifdef _MSC_VER
+#if (_MSC_VER <= 1700)
+double round(double value)
+{
+    return (value >= 0.0f)?(floor(value + 0.5f)):(ceil(value - 0.5f));
+}
+#endif 
+#endif
+
+class Time
+{
+public:
+  Time() { this->clear(); };
+
+  void set(double _seconds)
+  {
+    this->sec = (int32_t)(floor(_seconds));
+    this->nsec = (int32_t)(round((_seconds - this->sec) * 1e9));
+    this->Correct();
+  };
+
+  operator double ()
+  {
+    return (static_cast<double>(this->sec) +
+            static_cast<double>(this->nsec)*1e-9);
+  };
+
+  int32_t sec;
+  int32_t nsec;
+
+  void clear()
+  {
+    this->sec = 0;
+    this->nsec = 0;
+  };
+private:
+  void Correct()
+  {
+    // Make any corrections
+    if (this->nsec >= 1e9)
+    {
+      this->sec++;
+      this->nsec = (int32_t)(this->nsec - 1e9);
+    }
+    else if (this->nsec < 0)
+    {
+      this->sec--;
+      this->nsec = (int32_t)(this->nsec + 1e9);
+    }
+  };
+};
+
+
+class JointState
+{
+public:
+  JointState() { this->clear(); };
+
+  /// joint name
+  std::string joint;
+
+  std::vector<double> position;
+  std::vector<double> velocity;
+  std::vector<double> effort;
+
+  void clear()
+  {
+    this->joint.clear();
+    this->position.clear();
+    this->velocity.clear();
+    this->effort.clear();
+  }
+};
+
+class ModelState
+{
+public:
+  ModelState() { this->clear(); };
+
+  /// state name must be unique
+  std::string name;
+
+  Time time_stamp;
+
+  void clear()
+  {
+    this->name.clear();
+    this->time_stamp.set(0);
+    this->joint_states.clear();
+  };
+
+  std::vector<JointStateSharedPtr> joint_states;
+
+};
+
+}
+
+#endif
+

src/kdl_parser/include/urdf_model_state/twist.h

@@ -0,0 +1,42 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+* 
+*  Copyright (c) 2008, Willow Garage, Inc.
+*  All rights reserved.
+* 
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+* 
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+* 
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#ifndef URDF_MODEL_STATE_TWIST_
+#define URDF_MODEL_STATE_TWIST_
+
+#warning "Please Use #include <urdf_model/twist.h>"
+
+#include <urdf_model/twist.h>
+
+#endif

src/kdl_parser/include/urdf_model_state/types.h

@@ -0,0 +1,52 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2008, Willow Garage, Inc.
+*  All rights reserved.
+*
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+*
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+*
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/* Author: Steve Peters */
+
+#ifndef URDF_MODEL_STATE_TYPES_H
+#define URDF_MODEL_STATE_TYPES_H
+
+#include <memory>
+
+
+namespace urdf{
+
+class JointState;
+
+// typedef shared pointers
+typedef std::shared_ptr<JointState> JointStateSharedPtr;
+
+}
+
+#endif

src/kdl_parser/include/urdf_parser/exportdecl.h

@@ -0,0 +1,84 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2008, Willow Garage, Inc.
+*  All rights reserved.
+*
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+*
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+*
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/* Author: Thomas Moulard */
+
+#ifndef URDFDOM_EXPORTDECL_H
+# define URDFDOM_EXPORTDECL_H
+
+// Handle portable symbol export.
+// Defining manually which symbol should be exported is required
+// under Windows whether MinGW or MSVC is used.
+//
+// The headers then have to be able to work in two different modes:
+// - dllexport when one is building the library,
+// - dllimport for clients using the library.
+//
+// On Linux, set the visibility accordingly. If C++ symbol visibility
+// is handled by the compiler, see: http://gcc.gnu.org/wiki/Visibility
+# if defined _WIN32 || defined __CYGWIN__
+// On Microsoft Windows, use dllimport and dllexport to tag symbols.
+#  define URDFDOM_DLLIMPORT __declspec(dllimport)
+#  define URDFDOM_DLLEXPORT __declspec(dllexport)
+#  define URDFDOM_DLLLOCAL
+# else
+// On Linux, for GCC >= 4, tag symbols using GCC extension.
+#  if __GNUC__ >= 4
+#   define URDFDOM_DLLIMPORT __attribute__ ((visibility("default")))
+#   define URDFDOM_DLLEXPORT __attribute__ ((visibility("default")))
+#   define URDFDOM_DLLLOCAL  __attribute__ ((visibility("hidden")))
+#  else
+// Otherwise (GCC < 4 or another compiler is used), export everything.
+#   define URDFDOM_DLLIMPORT
+#   define URDFDOM_DLLEXPORT
+#   define URDFDOM_DLLLOCAL
+#  endif // __GNUC__ >= 4
+# endif // defined _WIN32 || defined __CYGWIN__
+
+# ifdef URDFDOM_STATIC
+// If one is using the library statically, get rid of
+// extra information.
+#  define URDFDOM_DLLAPI
+#  define URDFDOM_LOCAL
+# else
+// Depending on whether one is building or using the
+// library define DLLAPI to import or export.
+#  ifdef URDFDOM_EXPORTS
+#   define URDFDOM_DLLAPI URDFDOM_DLLEXPORT
+#  else
+#   define URDFDOM_DLLAPI URDFDOM_DLLIMPORT
+#  endif // URDFDOM_EXPORTS
+#  define URDFDOM_LOCAL URDFDOM_DLLLOCAL
+# endif // URDFDOM_STATIC
+#endif //! URDFDOM_EXPORTDECL_H

src/kdl_parser/include/urdf_parser/urdf_parser.h

@@ -0,0 +1,150 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+* 
+*  Copyright (c) 2008, Willow Garage, Inc.
+*  All rights reserved.
+* 
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+* 
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+* 
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/* Author: Wim Meeussen */
+
+#ifndef URDF_PARSER_URDF_PARSER_H
+#define URDF_PARSER_URDF_PARSER_H
+
+#include <stdexcept>
+#include <string>
+#include <vector>
+
+#include <tinyxml.h>
+#include <urdf_model/model.h>
+#include <urdf_model/color.h>
+#include <urdf_world/types.h>
+#include <urdf_model/utils.h>
+
+#include "exportdecl.h"
+
+namespace urdf_export_helpers {
+
+URDFDOM_DLLAPI std::string values2str(unsigned int count, const double *values, double (*conv)(double) = NULL);
+URDFDOM_DLLAPI std::string values2str(urdf::Vector3 vec);
+URDFDOM_DLLAPI std::string values2str(urdf::Rotation rot);
+URDFDOM_DLLAPI std::string values2str(urdf::Color c);
+URDFDOM_DLLAPI std::string values2str(double d);
+
+// This lives here (rather than in model.cpp) so we can run tests on it.
+class URDFVersion final
+{
+public:
+  explicit URDFVersion(const char *attr)
+  {
+    // If the passed in attribute is NULL, it means it wasn't specified in the
+    // XML, so we just assume version 1.0.
+    if (attr == nullptr)
+    {
+      major_ = 1;
+      minor_ = 0;
+      return;
+    }
+
+    // We only accept version strings of the type <major>.<minor>
+    std::vector<std::string> split;
+    urdf::split_string(split, std::string(attr), ".");
+    if (split.size() == 2)
+    {
+      major_ = strToUnsigned(split[0].c_str());
+      minor_ = strToUnsigned(split[1].c_str());
+    }
+    else
+    {
+      throw std::runtime_error("The version attribute should be in the form 'x.y'");
+    }
+  }
+
+  bool equal(uint32_t maj, uint32_t min)
+  {
+    return this->major_ == maj && this->minor_ == min;
+  }
+
+  uint32_t getMajor() const
+  {
+    return major_;
+  }
+
+  uint32_t getMinor() const
+  {
+    return minor_;
+  }
+
+private:
+  uint32_t strToUnsigned(const char *str)
+  {
+    if (str[0] == '\0')
+    {
+      // This would get caught below, but we can make a nicer error message
+      throw std::runtime_error("One of the fields of the version attribute is blank");
+    }
+    char *end = const_cast<char *>(str);
+    long value = strtol(str, &end, 10);
+    if (end == str)
+    {
+      // If the pointer didn't move at all, then we couldn't convert any of
+      // the string to an integer.
+      throw std::runtime_error("Version attribute is not an integer");
+    }
+    if (*end != '\0')
+    {
+      // Here, we didn't go all the way to the end of the string, which
+      // means there was junk at the end
+      throw std::runtime_error("Extra characters after the version number");
+    }
+    if (value < 0)
+    {
+      throw std::runtime_error("Version number must be positive");
+    }
+
+    return value;
+  }
+
+  uint32_t major_;
+  uint32_t minor_;
+};
+
+}
+
+namespace urdf{
+
+  URDFDOM_DLLAPI ModelInterfaceSharedPtr parseURDF(const std::string &xml_string);
+  URDFDOM_DLLAPI ModelInterfaceSharedPtr parseURDFFile(const std::string& path);
+  URDFDOM_DLLAPI ModelInterfaceSharedPtr parseURDFFileDocument(const std::string &path);
+  URDFDOM_DLLAPI TiXmlDocument*  exportURDF(ModelInterfaceSharedPtr &model);
+  URDFDOM_DLLAPI TiXmlDocument*  exportURDF(const ModelInterface &model);
+  URDFDOM_DLLAPI bool parsePose(Pose&, TiXmlElement*);
+}
+
+#endif

src/kdl_parser/include/urdf_sensor/sensor.h

@@ -0,0 +1,176 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+* 
+*  Copyright (c) 2008, Willow Garage, Inc.
+*  All rights reserved.
+* 
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+* 
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+* 
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/* Author: John Hsu */
+
+/* example
+
+ <sensor name="my_camera_sensor" update_rate="20">
+   <origin xyz="0 0 0" rpy="0 0 0"/>
+   <camera>
+     <horizontal_hov>1.5708</horizontal_hov>
+     <image width="640" height="480" format="R8G8B8"/>
+     <clip near="0.01" far="50.0"/>
+   </camera>
+ </sensor>
+ <sensor name="my_ray_sensor" update_rate="20">
+   <origin xyz="0 0 0" rpy="0 0 0"/>
+   <ray>
+     <scan>
+       <horizontal samples="100" resolution="1" min_angle="-1.5708" max_angle="1.5708"/>
+       <vertical samples="1" resolution="1" min_angle="0" max_angle="0"/>
+     </scan>
+   </ray>
+ </sensor>
+
+*/
+
+
+
+#ifndef URDF_SENSOR_H
+#define URDF_SENSOR_H
+
+#include <string>
+#include <vector>
+#include <map>
+#include "urdf_model/pose.h"
+#include "urdf_model/joint.h"
+#include "urdf_model/link.h"
+#include "urdf_model/types.h"
+#include "urdf_sensor/types.h"
+
+namespace urdf{
+
+class VisualSensor
+{
+public:
+  enum {CAMERA, RAY} type;
+  virtual ~VisualSensor(void)
+  {
+  }
+};
+
+class Camera : public VisualSensor
+{
+public:
+  Camera() { this->clear(); };
+  unsigned int width, height;
+  /// format is optional: defaults to R8G8B8), but can be
+  /// (L8|R8G8B8|B8G8R8|BAYER_RGGB8|BAYER_BGGR8|BAYER_GBRG8|BAYER_GRBG8)
+  std::string format;
+  double hfov;
+  double near;
+  double far;
+
+  void clear()
+  {
+    hfov = 0;
+    width = 0;
+    height = 0;
+    format.clear();
+    near = 0;
+    far = 0;
+  };
+};
+
+class Ray : public VisualSensor
+{
+public:
+  Ray() { this->clear(); };
+  unsigned int horizontal_samples;
+  double horizontal_resolution;
+  double horizontal_min_angle;
+  double horizontal_max_angle;
+  unsigned int vertical_samples;
+  double vertical_resolution;
+  double vertical_min_angle;
+  double vertical_max_angle;
+
+  void clear()
+  {
+    // set defaults
+    horizontal_samples = 1;
+    horizontal_resolution = 1;
+    horizontal_min_angle = 0;
+    horizontal_max_angle = 0;
+    vertical_samples = 1;
+    vertical_resolution = 1;
+    vertical_min_angle = 0;
+    vertical_max_angle = 0;
+  };
+};
+
+
+class Sensor
+{
+public:
+  Sensor() { this->clear(); };
+
+  /// sensor name must be unique
+  std::string name;
+
+  /// update rate in Hz
+  double update_rate;
+
+  /// transform from parent frame to optical center
+  ///   with z-forward and x-right, y-down
+  Pose origin;
+
+  /// sensor
+  VisualSensorSharedPtr sensor;
+
+
+  /// Parent link element name.  A pointer is stored in parent_link_.
+  std::string parent_link_name;
+
+  LinkSharedPtr getParent() const
+  {return parent_link_.lock();};
+
+  void setParent(LinkSharedPtr parent)
+  {  this->parent_link_ = parent; }
+  
+  void clear()
+  {
+    this->name.clear();
+    this->sensor.reset();
+    this->parent_link_name.clear();
+    this->parent_link_.reset();
+  };
+
+private:
+  LinkWeakPtr parent_link_;
+
+};
+}
+#endif

src/kdl_parser/include/urdf_sensor/types.h

@@ -0,0 +1,52 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2008, Willow Garage, Inc.
+*  All rights reserved.
+*
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+*
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+*
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/* Author: Steve Peters */
+
+#ifndef URDF_SENSOR_TYPES_H
+#define URDF_SENSOR_TYPES_H
+
+#include <memory>
+
+
+namespace urdf{
+
+class VisualSensor;
+
+// typedef shared pointers
+typedef std::shared_ptr<VisualSensor> VisualSensorSharedPtr;
+
+}
+
+#endif

src/kdl_parser/include/urdf_world/types.h

@@ -0,0 +1,52 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2008, Willow Garage, Inc.
+*  All rights reserved.
+*
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+*
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+*
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/* Author: Steve Peters */
+
+#ifndef URDF_WORLD_TYPES_H
+#define URDF_WORLD_TYPES_H
+
+#include <memory>
+
+
+namespace urdf{
+
+class ModelInterface;
+
+// typedef shared pointers
+typedef std::shared_ptr<ModelInterface> ModelInterfaceSharedPtr;
+
+}
+
+#endif

src/kdl_parser/include/urdf_world/world.h

@@ -0,0 +1,110 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+* 
+*  Copyright (c) 2008, Willow Garage, Inc.
+*  All rights reserved.
+* 
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+* 
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+* 
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/* Author: John Hsu */
+
+/* encapsulates components in a world
+   see http://ros.org/wiki/usdf/XML/urdf_world
+   for details
+*/
+/* example world XML
+
+<world name="pr2_with_table">
+  <!-- include the models by including
+       either the complete urdf or
+       referencing the file name.  -->
+  <model name="pr2">
+    ...
+  </model>
+  <include filename="table.urdf" model_name="table_model"/>
+
+  <!-- models in the world -->
+  <entity model="pr2" name="prj">
+    <origin xyz="0 1 0" rpy="0 0 0"/>
+    <twist linear="0 0 0" angular="0 0 0"/>
+  </entity>
+  <entity model="pr2" name="prk">
+    <origin xyz="0 2 0" rpy="0 0 0"/>
+    <twist linear="0 0 0" angular="0 0 0"/>
+  </entity>
+  <entity model="table_model">
+    <origin xyz="0 3 0" rpy="0 0 0"/>
+    <twist linear="0 0 0" angular="0 0 0"/>
+  </entity>
+
+</world>
+
+*/
+
+#ifndef URDF_WORLD_H
+#define URDF_WORLD_H
+
+#include <string>
+#include <vector>
+#include <map>
+
+#include "urdf_model/model.h"
+#include "urdf_model/pose.h"
+#include "urdf_model/twist.h"
+#include "urdf_world/types.h"
+
+namespace urdf{
+
+class Entity
+{
+public:
+  ModelInterfaceSharedPtr model;
+  Pose origin;
+  Twist twist;
+};
+
+class World
+{
+public:
+  World() { this->clear(); };
+
+  /// world name must be unique
+  std::string name;
+
+  std::vector<Entity> models;
+
+  void clear()
+  {
+    this->name.clear();
+  };
+};
+}
+
+#endif
+

src/kdl_parser/src/joint.cpp

@@ -0,0 +1,642 @@
+/*********************************************************************
+* Software Ligcense Agreement (BSD License)
+* 
+*  Copyright (c) 2008, Willow Garage, Inc.
+*  All rights reserved.
+* 
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+* 
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+* 
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/* Author: John Hsu */
+
+#include <locale>
+#include <sstream>
+#include <stdexcept>
+#include <string>
+#include <urdf_model/joint.h>
+//#include <console_bridge/console.h>
+#include <tinyxml.h>
+#include <urdf_parser/urdf_parser.h>
+
+namespace urdf{
+
+bool parsePose(Pose &pose, TiXmlElement* xml);
+
+bool parseJointDynamics(JointDynamics &jd, TiXmlElement* config)
+{
+  jd.clear();
+
+  // Get joint damping
+  const char* damping_str = config->Attribute("damping");
+  if (damping_str == NULL){
+    ////CONSOLE_BRIDGE_logDebug("urdfdom.joint_dynamics: no damping, defaults to 0");
+    jd.damping = 0;
+  }
+  else
+  {
+    try {
+      jd.damping = strToDouble(damping_str);
+    } catch(std::runtime_error &) {
+      //CONSOLE_BRIDGE_logError("damping value (%s) is not a valid float", damping_str);
+      return false;
+    }
+  }
+
+  // Get joint friction
+  const char* friction_str = config->Attribute("friction");
+  if (friction_str == NULL){
+    ////CONSOLE_BRIDGE_logDebug("urdfdom.joint_dynamics: no friction, defaults to 0");
+    jd.friction = 0;
+  }
+  else
+  {
+    try {
+      jd.friction = strToDouble(friction_str);
+    } catch (std::runtime_error &) {
+      ////CONSOLE_BRIDGE_logError("friction value (%s) is not a valid float", friction_str);
+      return false;
+    }
+  }
+
+  if (damping_str == NULL && friction_str == NULL)
+  {
+    ////CONSOLE_BRIDGE_logError("joint dynamics element specified with no damping and no friction");
+    return false;
+  }
+  else{
+    //CONSOLE_BRIDGE_logDebug("urdfdom.joint_dynamics: damping %f and friction %f", jd.damping, jd.friction);
+    return true;
+  }
+}
+
+bool parseJointLimits(JointLimits &jl, TiXmlElement* config)
+{
+  jl.clear();
+
+  // Get lower joint limit
+  const char* lower_str = config->Attribute("lower");
+  if (lower_str == NULL){
+    //CONSOLE_BRIDGE_logDebug("urdfdom.joint_limit: no lower, defaults to 0");
+    jl.lower = 0;
+  }
+  else
+  {
+    try {
+      jl.lower = strToDouble(lower_str);
+    } catch (std::runtime_error &) {
+      ////CONSOLE_BRIDGE_logError("lower value (%s) is not a valid float", lower_str);
+      return false;
+    }
+  }
+
+  // Get upper joint limit
+  const char* upper_str = config->Attribute("upper");
+  if (upper_str == NULL){
+    //CONSOLE_BRIDGE_logDebug("urdfdom.joint_limit: no upper, , defaults to 0");
+    jl.upper = 0;
+  }
+  else
+  {
+    try {
+      jl.upper = strToDouble(upper_str);
+    } catch(std::runtime_error &) {
+      ////CONSOLE_BRIDGE_logError("upper value (%s) is not a valid float", upper_str);
+      return false;
+    }
+  }
+
+  // Get joint effort limit
+  const char* effort_str = config->Attribute("effort");
+  if (effort_str == NULL){
+    ////CONSOLE_BRIDGE_logError("joint limit: no effort");
+    return false;
+  }
+  else
+  {
+    try {
+      jl.effort = strToDouble(effort_str);
+    } catch(std::runtime_error &) {
+      ////CONSOLE_BRIDGE_logError("effort value (%s) is not a valid float", effort_str);
+      return false;
+    }
+  }
+
+  // Get joint velocity limit
+  const char* velocity_str = config->Attribute("velocity");
+  if (velocity_str == NULL){
+    ////CONSOLE_BRIDGE_logError("joint limit: no velocity");
+    return false;
+  }
+  else
+  {
+    try {
+      jl.velocity = strToDouble(velocity_str);
+    } catch(std::runtime_error &) {
+      ////CONSOLE_BRIDGE_logError("velocity value (%s) is not a valid float", velocity_str);
+      return false;
+    }
+  }
+
+  return true;
+}
+
+bool parseJointSafety(JointSafety &js, TiXmlElement* config)
+{
+  js.clear();
+
+  // Get soft_lower_limit joint limit
+  const char* soft_lower_limit_str = config->Attribute("soft_lower_limit");
+  if (soft_lower_limit_str == NULL)
+  {
+    //CONSOLE_BRIDGE_logDebug("urdfdom.joint_safety: no soft_lower_limit, using default value");
+    js.soft_lower_limit = 0;
+  }
+  else
+  {
+    try {
+      js.soft_lower_limit = strToDouble(soft_lower_limit_str);
+    } catch(std::runtime_error &) {
+      ////CONSOLE_BRIDGE_logError("soft_lower_limit value (%s) is not a valid float", soft_lower_limit_str);
+      return false;
+    }
+  }
+
+  // Get soft_upper_limit joint limit
+  const char* soft_upper_limit_str = config->Attribute("soft_upper_limit");
+  if (soft_upper_limit_str == NULL)
+  {
+    //CONSOLE_BRIDGE_logDebug("urdfdom.joint_safety: no soft_upper_limit, using default value");
+    js.soft_upper_limit = 0;
+  }
+  else
+  {
+    try {
+      js.soft_upper_limit = strToDouble(soft_upper_limit_str);
+    } catch(std::runtime_error &) {
+      ////CONSOLE_BRIDGE_logError("soft_upper_limit value (%s) is not a valid float", soft_upper_limit_str);
+      return false;
+    }
+  }
+
+  // Get k_position_ safety "position" gain - not exactly position gain
+  const char* k_position_str = config->Attribute("k_position");
+  if (k_position_str == NULL)
+  {
+    //CONSOLE_BRIDGE_logDebug("urdfdom.joint_safety: no k_position, using default value");
+    js.k_position = 0;
+  }
+  else
+  {
+    try {
+      js.k_position = strToDouble(k_position_str);
+    } catch(std::runtime_error &) {
+      ////CONSOLE_BRIDGE_logError("k_position value (%s) is not a valid float", k_position_str);
+      return false;
+    }
+  }
+  // Get k_velocity_ safety velocity gain
+  const char* k_velocity_str = config->Attribute("k_velocity");
+  if (k_velocity_str == NULL)
+  {
+    ////CONSOLE_BRIDGE_logError("joint safety: no k_velocity");
+    return false;
+  }
+  else
+  {
+    try {
+      js.k_velocity = strToDouble(k_velocity_str);
+    } catch(std::runtime_error &) {
+      ////CONSOLE_BRIDGE_logError("k_velocity value (%s) is not a valid float", k_velocity_str);
+      return false;
+    }
+  }
+
+  return true;
+}
+
+bool parseJointCalibration(JointCalibration &jc, TiXmlElement* config)
+{
+  jc.clear();
+
+  // Get rising edge position
+  const char* rising_position_str = config->Attribute("rising");
+  if (rising_position_str == NULL)
+  {
+    //CONSOLE_BRIDGE_logDebug("urdfdom.joint_calibration: no rising, using default value");
+    jc.rising.reset();
+  }
+  else
+  {
+    try {
+      jc.rising.reset(new double(strToDouble(rising_position_str)));
+    } catch(std::runtime_error &) {
+      ////CONSOLE_BRIDGE_logError("rising value (%s) is not a valid float", rising_position_str);
+      return false;
+    }
+  }
+
+  // Get falling edge position
+  const char* falling_position_str = config->Attribute("falling");
+  if (falling_position_str == NULL)
+  {
+    //CONSOLE_BRIDGE_logDebug("urdfdom.joint_calibration: no falling, using default value");
+    jc.falling.reset();
+  }
+  else
+  {
+    try {
+      jc.falling.reset(new double(strToDouble(falling_position_str)));
+    } catch(std::runtime_error &) {
+      ////CONSOLE_BRIDGE_logError("falling value (%s) is not a valid float", falling_position_str);
+      return false;
+    }
+  }
+
+  return true;
+}
+
+bool parseJointMimic(JointMimic &jm, TiXmlElement* config)
+{
+  jm.clear();
+
+  // Get name of joint to mimic
+  const char* joint_name_str = config->Attribute("joint");
+
+  if (joint_name_str == NULL)
+  {
+    ////CONSOLE_BRIDGE_logError("joint mimic: no mimic joint specified");
+    return false;
+  }
+  else
+    jm.joint_name = joint_name_str;
+  
+  // Get mimic multiplier
+  const char* multiplier_str = config->Attribute("multiplier");
+
+  if (multiplier_str == NULL)
+  {
+    //CONSOLE_BRIDGE_logDebug("urdfdom.joint_mimic: no multiplier, using default value of 1");
+    jm.multiplier = 1;    
+  }
+  else
+  {
+    try {
+      jm.multiplier = strToDouble(multiplier_str);
+    } catch(std::runtime_error &) {
+      ////CONSOLE_BRIDGE_logError("multiplier value (%s) is not a valid float", multiplier_str);
+      return false;
+    }
+  }
+
+  
+  // Get mimic offset
+  const char* offset_str = config->Attribute("offset");
+  if (offset_str == NULL)
+  {
+    //CONSOLE_BRIDGE_logDebug("urdfdom.joint_mimic: no offset, using default value of 0");
+    jm.offset = 0;
+  }
+  else
+  {
+    try {
+      jm.offset = strToDouble(offset_str);
+    } catch(std::runtime_error &) {
+      ////CONSOLE_BRIDGE_logError("offset value (%s) is not a valid float", offset_str);
+      return false;
+    }
+  }
+
+  return true;
+}
+
+bool parseJoint(Joint &joint, TiXmlElement* config)
+{
+  joint.clear();
+
+  // Get Joint Name
+  const char *name = config->Attribute("name");
+  if (!name)
+  {
+    ////CONSOLE_BRIDGE_logError("unnamed joint found");
+    return false;
+  }
+  joint.name = name;
+
+  // Get transform from Parent Link to Joint Frame
+  TiXmlElement *origin_xml = config->FirstChildElement("origin");
+  if (!origin_xml)
+  {
+    //CONSOLE_BRIDGE_logDebug("urdfdom: Joint [%s] missing origin tag under parent describing transform from Parent Link to Joint Frame, (using Identity transform).", joint.name.c_str());
+    joint.parent_to_joint_origin_transform.clear();
+  }
+  else
+  {
+    if (!parsePose(joint.parent_to_joint_origin_transform, origin_xml))
+    {
+      joint.parent_to_joint_origin_transform.clear();
+      ////CONSOLE_BRIDGE_logError("Malformed parent origin element for joint [%s]", joint.name.c_str());
+      return false;
+    }
+  }
+
+  // Get Parent Link
+  TiXmlElement *parent_xml = config->FirstChildElement("parent");
+  if (parent_xml)
+  {
+    const char *pname = parent_xml->Attribute("link");
+    if (!pname)
+    {
+      //CONSOLE_BRIDGE_logInform("no parent link name specified for Joint link [%s]. this might be the root?", joint.name.c_str());
+    }
+    else
+    {
+      joint.parent_link_name = std::string(pname);
+    }
+  }
+
+  // Get Child Link
+  TiXmlElement *child_xml = config->FirstChildElement("child");
+  if (child_xml)
+  {
+    const char *pname = child_xml->Attribute("link");
+    if (!pname)
+    {
+      //CONSOLE_BRIDGE_logInform("no child link name specified for Joint link [%s].", joint.name.c_str());
+    }
+    else
+    {
+      joint.child_link_name = std::string(pname);
+    }
+  }
+
+  // Get Joint type
+  const char* type_char = config->Attribute("type");
+  if (!type_char)
+  {
+    ////CONSOLE_BRIDGE_logError("joint [%s] has no type, check to see if it's a reference.", joint.name.c_str());
+    return false;
+  }
+  
+  std::string type_str = type_char;
+  if (type_str == "planar")
+    joint.type = Joint::PLANAR;
+  else if (type_str == "floating")
+    joint.type = Joint::FLOATING;
+  else if (type_str == "revolute")
+    joint.type = Joint::REVOLUTE;
+  else if (type_str == "continuous")
+    joint.type = Joint::CONTINUOUS;
+  else if (type_str == "prismatic")
+    joint.type = Joint::PRISMATIC;
+  else if (type_str == "fixed")
+    joint.type = Joint::FIXED;
+  else
+  {
+    ////CONSOLE_BRIDGE_logError("Joint [%s] has no known type [%s]", joint.name.c_str(), type_str.c_str());
+    return false;
+  }
+
+  // Get Joint Axis
+  if (joint.type != Joint::FLOATING && joint.type != Joint::FIXED)
+  {
+    // axis
+    TiXmlElement *axis_xml = config->FirstChildElement("axis");
+    if (!axis_xml){
+      //CONSOLE_BRIDGE_logDebug("urdfdom: no axis elemement for Joint link [%s], defaulting to (1,0,0) axis", joint.name.c_str());
+      joint.axis = Vector3(1.0, 0.0, 0.0);
+    }
+    else{
+      if (axis_xml->Attribute("xyz")){
+        try {
+          joint.axis.init(axis_xml->Attribute("xyz"));
+        }
+        catch (ParseError &e) {
+          joint.axis.clear();
+          ////CONSOLE_BRIDGE_logError("Malformed axis element for joint [%s]: %s", joint.name.c_str(), e.what());
+          return false;
+        }
+      }
+    }
+  }
+
+  // Get limit
+  TiXmlElement *limit_xml = config->FirstChildElement("limit");
+  if (limit_xml)
+  {
+    joint.limits.reset(new JointLimits());
+    if (!parseJointLimits(*joint.limits, limit_xml))
+    {
+      //CONSOLE_BRIDGE_logError("Could not parse limit element for joint [%s]", joint.name.c_str());
+      joint.limits.reset();
+      return false;
+    }
+  }
+  else if (joint.type == Joint::REVOLUTE)
+  {
+    //CONSOLE_BRIDGE_logError("Joint [%s] is of type REVOLUTE but it does not specify limits", joint.name.c_str());
+    return false;
+  }
+  else if (joint.type == Joint::PRISMATIC)
+  {
+    //CONSOLE_BRIDGE_logError("Joint [%s] is of type PRISMATIC without limits", joint.name.c_str()); 
+    return false;
+  }
+
+  // Get safety
+  TiXmlElement *safety_xml = config->FirstChildElement("safety_controller");
+  if (safety_xml)
+  {
+    joint.safety.reset(new JointSafety());
+    if (!parseJointSafety(*joint.safety, safety_xml))
+    {
+      //CONSOLE_BRIDGE_logError("Could not parse safety element for joint [%s]", joint.name.c_str());
+      joint.safety.reset();
+      return false;
+    }
+  }
+
+  // Get calibration
+  TiXmlElement *calibration_xml = config->FirstChildElement("calibration");
+  if (calibration_xml)
+  {
+    joint.calibration.reset(new JointCalibration());
+    if (!parseJointCalibration(*joint.calibration, calibration_xml))
+    {
+      //CONSOLE_BRIDGE_logError("Could not parse calibration element for joint  [%s]", joint.name.c_str());
+      joint.calibration.reset();
+      return false;
+    }
+  }
+
+  // Get Joint Mimic
+  TiXmlElement *mimic_xml = config->FirstChildElement("mimic");
+  if (mimic_xml)
+  {
+    joint.mimic.reset(new JointMimic());
+    if (!parseJointMimic(*joint.mimic, mimic_xml))
+    {
+      //CONSOLE_BRIDGE_logError("Could not parse mimic element for joint  [%s]", joint.name.c_str());
+      joint.mimic.reset();
+      return false;
+    }
+  }
+
+  // Get Dynamics
+  TiXmlElement *prop_xml = config->FirstChildElement("dynamics");
+  if (prop_xml)
+  {
+    joint.dynamics.reset(new JointDynamics());
+    if (!parseJointDynamics(*joint.dynamics, prop_xml))
+    {
+      //CONSOLE_BRIDGE_logError("Could not parse joint_dynamics element for joint [%s]", joint.name.c_str());
+      joint.dynamics.reset();
+      return false;
+    }
+  }
+
+  return true;
+}
+
+
+/* exports */
+bool exportPose(Pose &pose, TiXmlElement* xml);
+
+bool exportJointDynamics(JointDynamics &jd, TiXmlElement* xml)
+{
+  TiXmlElement *dynamics_xml = new TiXmlElement("dynamics");
+  dynamics_xml->SetAttribute("damping", urdf_export_helpers::values2str(jd.damping) );
+  dynamics_xml->SetAttribute("friction", urdf_export_helpers::values2str(jd.friction) );
+  xml->LinkEndChild(dynamics_xml);
+  return true;
+}
+
+bool exportJointLimits(JointLimits &jl, TiXmlElement* xml)
+{
+  TiXmlElement *limit_xml = new TiXmlElement("limit");
+  limit_xml->SetAttribute("effort", urdf_export_helpers::values2str(jl.effort) );
+  limit_xml->SetAttribute("velocity", urdf_export_helpers::values2str(jl.velocity) );
+  limit_xml->SetAttribute("lower", urdf_export_helpers::values2str(jl.lower) );
+  limit_xml->SetAttribute("upper", urdf_export_helpers::values2str(jl.upper) );
+  xml->LinkEndChild(limit_xml);
+  return true;
+}
+
+bool exportJointSafety(JointSafety &js, TiXmlElement* xml)
+{
+  TiXmlElement *safety_xml = new TiXmlElement("safety_controller");
+  safety_xml->SetAttribute("k_position", urdf_export_helpers::values2str(js.k_position) );
+  safety_xml->SetAttribute("k_velocity", urdf_export_helpers::values2str(js.k_velocity) );
+  safety_xml->SetAttribute("soft_lower_limit", urdf_export_helpers::values2str(js.soft_lower_limit) );
+  safety_xml->SetAttribute("soft_upper_limit", urdf_export_helpers::values2str(js.soft_upper_limit) );
+  xml->LinkEndChild(safety_xml);
+  return true;
+}
+
+bool exportJointCalibration(JointCalibration &jc, TiXmlElement* xml)
+{
+  if (jc.falling || jc.rising)
+  {
+    TiXmlElement *calibration_xml = new TiXmlElement("calibration");
+    if (jc.falling)
+      calibration_xml->SetAttribute("falling", urdf_export_helpers::values2str(*jc.falling) );
+    if (jc.rising)
+      calibration_xml->SetAttribute("rising", urdf_export_helpers::values2str(*jc.rising) );
+    //calibration_xml->SetAttribute("reference_position", urdf_export_helpers::values2str(jc.reference_position) );
+    xml->LinkEndChild(calibration_xml);
+  }
+  return true;
+}
+
+bool exportJointMimic(JointMimic &jm, TiXmlElement* xml)
+{
+  if (!jm.joint_name.empty())
+  {
+    TiXmlElement *mimic_xml = new TiXmlElement("mimic");
+    mimic_xml->SetAttribute("offset", urdf_export_helpers::values2str(jm.offset) );
+    mimic_xml->SetAttribute("multiplier", urdf_export_helpers::values2str(jm.multiplier) );
+    mimic_xml->SetAttribute("joint", jm.joint_name );
+    xml->LinkEndChild(mimic_xml);
+  }
+  return true;
+}
+
+bool exportJoint(Joint &joint, TiXmlElement* xml)
+{
+  TiXmlElement * joint_xml = new TiXmlElement("joint");
+  joint_xml->SetAttribute("name", joint.name);
+  if (joint.type == urdf::Joint::PLANAR)
+    joint_xml->SetAttribute("type", "planar");
+  else if (joint.type == urdf::Joint::FLOATING)
+    joint_xml->SetAttribute("type", "floating");
+  else if (joint.type == urdf::Joint::REVOLUTE)
+    joint_xml->SetAttribute("type", "revolute");
+  else if (joint.type == urdf::Joint::CONTINUOUS)
+    joint_xml->SetAttribute("type", "continuous");
+  else if (joint.type == urdf::Joint::PRISMATIC)
+    joint_xml->SetAttribute("type", "prismatic");
+  else if (joint.type == urdf::Joint::FIXED)
+    joint_xml->SetAttribute("type", "fixed");
+  else
+    //CONSOLE_BRIDGE_logError("ERROR:  Joint [%s] type [%d] is not a defined type.\n",joint.name.c_str(), joint.type);
+
+  // origin
+  exportPose(joint.parent_to_joint_origin_transform, joint_xml);
+
+  // axis
+  TiXmlElement * axis_xml = new TiXmlElement("axis");
+  axis_xml->SetAttribute("xyz", urdf_export_helpers::values2str(joint.axis));
+  joint_xml->LinkEndChild(axis_xml);
+
+  // parent 
+  TiXmlElement * parent_xml = new TiXmlElement("parent");
+  parent_xml->SetAttribute("link", joint.parent_link_name);
+  joint_xml->LinkEndChild(parent_xml);
+
+  // child
+  TiXmlElement * child_xml = new TiXmlElement("child");
+  child_xml->SetAttribute("link", joint.child_link_name);
+  joint_xml->LinkEndChild(child_xml);
+
+  if (joint.dynamics)
+    exportJointDynamics(*(joint.dynamics), joint_xml);
+  if (joint.limits)
+    exportJointLimits(*(joint.limits), joint_xml);
+  if (joint.safety)
+    exportJointSafety(*(joint.safety), joint_xml);
+  if (joint.calibration)
+    exportJointCalibration(*(joint.calibration), joint_xml);
+  if (joint.mimic)
+    exportJointMimic(*(joint.mimic), joint_xml);
+
+  xml->LinkEndChild(joint_xml);
+  return true;
+}
+
+
+
+}

src/kdl_parser/src/kdl_parser.cpp

@@ -0,0 +1,261 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2008, Willow Garage, Inc.
+*  All rights reserved.
+*
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+*
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+*
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/* Author: Wim Meeussen */
+
+#include "kdl_parser/kdl_parser.hpp"
+
+#include <string>
+#include <vector>
+
+#include <urdf_model/model.h>
+#include <urdf_parser/urdf_parser.h>
+
+#include <kdl/frames_io.hpp>
+
+//#ifdef HAS_ROS
+//#include <ros/console.h>
+//#else
+// forward ROS warnings and errors to stderr
+#define ROS_DEBUG(...) fprintf(stdout, __VA_ARGS__);
+#define ROS_ERROR(...) fprintf(stderr, __VA_ARGS__);
+#define ROS_WARN(...) fprintf(stderr, __VA_ARGS__);
+//#endif
+
+//#ifdef HAS_URDF
+#include <urdf_model/model.h>
+//#include <urdf_model/urdfdom_compatibility.h>
+//#endif
+
+namespace kdl_parser
+{
+// construct vector
+KDL::Vector toKdl(urdf::Vector3 v)
+{
+  return KDL::Vector(v.x, v.y, v.z);
+}
+
+// construct rotation
+KDL::Rotation toKdl(urdf::Rotation r)
+{
+  return KDL::Rotation::Quaternion(r.x, r.y, r.z, r.w);
+}
+
+// construct pose
+KDL::Frame toKdl(urdf::Pose p)
+{
+  return KDL::Frame(toKdl(p.rotation), toKdl(p.position));
+}
+
+// construct joint
+KDL::Joint toKdl(urdf::JointSharedPtr jnt)
+{
+  KDL::Frame F_parent_jnt = toKdl(jnt->parent_to_joint_origin_transform);
+
+  switch (jnt->type) {
+    case urdf::Joint::FIXED: {
+        return KDL::Joint(jnt->name, KDL::Joint::None);
+      }
+    case urdf::Joint::REVOLUTE: {
+        KDL::Vector axis = toKdl(jnt->axis);
+        return KDL::Joint(jnt->name, F_parent_jnt.p, F_parent_jnt.M * axis, KDL::Joint::RotAxis);
+      }
+    case urdf::Joint::CONTINUOUS: {
+        KDL::Vector axis = toKdl(jnt->axis);
+        return KDL::Joint(jnt->name, F_parent_jnt.p, F_parent_jnt.M * axis, KDL::Joint::RotAxis);
+      }
+    case urdf::Joint::PRISMATIC: {
+        KDL::Vector axis = toKdl(jnt->axis);
+        return KDL::Joint(jnt->name, F_parent_jnt.p, F_parent_jnt.M * axis, KDL::Joint::TransAxis);
+      }
+    default: {
+        ROS_WARN("Converting unknown joint type of joint '%s' into a fixed joint", jnt->name.c_str());
+        return KDL::Joint(jnt->name, KDL::Joint::None);
+      }
+  }
+  return KDL::Joint();
+}
+
+// construct inertia
+KDL::RigidBodyInertia toKdl(urdf::InertialSharedPtr i)
+{
+  KDL::Frame origin = toKdl(i->origin);
+
+  // the mass is frame independent
+  double kdl_mass = i->mass;
+
+  // kdl and urdf both specify the com position in the reference frame of the link
+  KDL::Vector kdl_com = origin.p;
+
+  // kdl specifies the inertia matrix in the reference frame of the link,
+  // while the urdf specifies the inertia matrix in the inertia reference frame
+  KDL::RotationalInertia urdf_inertia =
+    KDL::RotationalInertia(i->ixx, i->iyy, i->izz, i->ixy, i->ixz, i->iyz);
+
+  // Rotation operators are not defined for rotational inertia,
+  // so we use the RigidBodyInertia operators (with com = 0) as a workaround
+  KDL::RigidBodyInertia kdl_inertia_wrt_com_workaround =
+    origin.M * KDL::RigidBodyInertia(0, KDL::Vector::Zero(), urdf_inertia);
+
+  // Note that the RigidBodyInertia constructor takes the 3d inertia wrt the com
+  // while the getRotationalInertia method returns the 3d inertia wrt the frame origin
+  // (but having com = Vector::Zero() in kdl_inertia_wrt_com_workaround they match)
+  KDL::RotationalInertia kdl_inertia_wrt_com =
+    kdl_inertia_wrt_com_workaround.getRotationalInertia();
+
+  return KDL::RigidBodyInertia(kdl_mass, kdl_com, kdl_inertia_wrt_com);
+}
+
+
+// recursive function to walk through tree
+bool addChildrenToTree(urdf::LinkConstSharedPtr root, KDL::Tree & tree)
+{
+  std::vector<urdf::LinkSharedPtr> children = root->child_links;
+  ROS_DEBUG("Link %s had %zu children", root->name.c_str(), children.size());
+
+  // constructs the optional inertia
+  KDL::RigidBodyInertia inert(0);
+  if (root->inertial) {
+    inert = toKdl(root->inertial);
+  }
+
+  // constructs the kdl joint
+  KDL::Joint jnt = toKdl(root->parent_joint);
+
+  // construct the kdl segment
+  KDL::Segment sgm(root->name, jnt, toKdl(
+      root->parent_joint->parent_to_joint_origin_transform), inert);
+
+  // add segment to tree
+  tree.addSegment(sgm, root->parent_joint->parent_link_name);
+
+  // recurslively add all children
+  for (size_t i = 0; i < children.size(); i++) {
+    if (!addChildrenToTree(children[i], tree)) {
+      return false;
+    }
+  }
+  return true;
+}
+
+bool treeFromFileDocument(const std::string & file, KDL::Tree & tree)
+{
+  const urdf::ModelInterfaceSharedPtr robot_model = urdf::parseURDFFileDocument(file);
+  return kdl_parser::treeFromUrdfModel(*robot_model, tree);
+}
+
+bool treeFromFile(const std::string& file, KDL::Tree& tree)
+{
+    const urdf::ModelInterfaceSharedPtr robot_model = urdf::parseURDFFile(file);
+    return kdl_parser::treeFromUrdfModel(*robot_model, tree);
+}
+
+bool treeFromParam(const std::string & param, KDL::Tree & tree)
+{
+#if defined(HAS_ROS) && defined(HAS_URDF)
+  urdf::Model robot_model;
+  if (!robot_model.initParam(param)){
+    ROS_ERROR("Could not generate robot model");
+    return false;
+  }
+  return treeFromUrdfModel(robot_model, tree);
+#else
+  return false;
+#endif
+}
+
+
+bool treeFromString(const std::string & xml, KDL::Tree & tree)
+{
+  const urdf::ModelInterfaceSharedPtr robot_model = urdf::parseURDF(xml);
+  if (!robot_model) {
+    ROS_ERROR("Could not generate robot model");
+    return false;
+  }
+  return kdl_parser::treeFromUrdfModel(*robot_model, tree);
+}
+
+bool treeFromXml(const tinyxml2::XMLDocument * xml_doc, KDL::Tree & tree)
+{
+  if (!xml_doc) {
+    ROS_ERROR("Could not parse the xml document");
+    return false;
+  }
+
+  tinyxml2::XMLPrinter printer;
+  xml_doc->Print(&printer);
+
+  return treeFromString(printer.CStr(), tree);
+}
+
+bool treeFromXml(TiXmlDocument * xml_doc, KDL::Tree & tree)
+{
+  if (!xml_doc) {
+    ROS_ERROR("Could not parse the xml document");
+    return false;
+  }
+
+  std::stringstream ss;
+  
+  ss << *xml_doc;
+
+  return treeFromString(ss.str(), tree);
+}
+
+bool treeFromUrdfModel(const urdf::ModelInterface & robot_model, KDL::Tree & tree)
+{
+  if (!robot_model.getRoot()) {
+    return false;
+  }
+
+  tree = KDL::Tree(robot_model.getRoot()->name);
+
+  // warn if root link has inertia. KDL does not support this
+  if (robot_model.getRoot()->inertial) {
+    ROS_WARN("The root link %s has an inertia specified in the URDF, but KDL does not "
+      "support a root link with an inertia.  As a workaround, you can add an extra "
+      "dummy link to your URDF.", robot_model.getRoot()->name.c_str());
+  }
+
+  //  add all children
+  for (size_t i = 0; i < robot_model.getRoot()->child_links.size(); i++) {
+    if (!addChildrenToTree(robot_model.getRoot()->child_links[i], tree)) {
+      return false;
+    }
+  }
+
+  return true;
+}
+
+}  // namespace kdl_parser

src/kdl_parser/src/link.cpp

@@ -0,0 +1,670 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+* 
+*  Copyright (c) 2008, Willow Garage, Inc.
+*  All rights reserved.
+* 
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+* 
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+* 
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/* Author: Wim Meeussen */
+
+
+#include <urdf_parser/urdf_parser.h>
+#include <urdf_model/link.h>
+#include <fstream>
+#include <locale>
+#include <sstream>
+#include <stdexcept>
+#include <string>
+#include <utility>
+#include <vector>
+#include <algorithm>
+#include <tinyxml.h>
+//#include <console_bridge/console.h>
+
+namespace urdf{
+
+bool parsePose(Pose &pose, TiXmlElement* xml);
+
+bool parseMaterial(Material &material, TiXmlElement *config, bool only_name_is_ok)
+{
+  bool has_rgb = false;
+  bool has_filename = false;
+
+  material.clear();
+
+  if (!config->Attribute("name"))
+  {
+    //CONSOLE_BRIDGE_logError("Material must contain a name attribute");
+    return false;
+  }
+  
+  material.name = config->Attribute("name");
+
+  // texture
+  TiXmlElement *t = config->FirstChildElement("texture");
+  if (t)
+  {
+    if (t->Attribute("filename"))
+    {
+      material.texture_filename = t->Attribute("filename");
+      has_filename = true;
+    }
+  }
+
+  // color
+  TiXmlElement *c = config->FirstChildElement("color");
+  if (c)
+  {
+    if (c->Attribute("rgba")) {
+
+      try {
+        material.color.init(c->Attribute("rgba"));
+        has_rgb = true;
+      }
+      catch (ParseError &e) {  
+        material.color.clear();
+        //CONSOLE_BRIDGE_logError(std::string("Material [" + material.name + "] has malformed color rgba values: " + e.what()).c_str());
+      }
+    }
+  }
+
+  if (!has_rgb && !has_filename) {
+    if (!only_name_is_ok) // no need for an error if only name is ok
+    {
+      if (!has_rgb) //CONSOLE_BRIDGE_logError(std::string("Material ["+material.name+"] color has no rgba").c_str());
+      if (!has_filename){ //CONSOLE_BRIDGE_logError(std::string("Material ["+material.name+"] not defined in file").c_str());
+      }
+    }
+    return false;
+  }
+  return true;
+}
+
+
+bool parseSphere(Sphere &s, TiXmlElement *c)
+{
+  s.clear();
+
+  s.type = Geometry::SPHERE;
+  if (!c->Attribute("radius"))
+  {
+    //CONSOLE_BRIDGE_logError("Sphere shape must have a radius attribute");
+    return false;
+  }
+
+  try {
+    s.radius = strToDouble(c->Attribute("radius"));
+  } catch(std::runtime_error &) {
+    std::stringstream stm;
+    stm << "radius [" << c->Attribute("radius") << "] is not a valid float";
+    //CONSOLE_BRIDGE_logError(stm.str().c_str());
+    return false;
+  }
+
+  return true;
+}
+
+bool parseBox(Box &b, TiXmlElement *c)
+{
+  b.clear();
+  
+  b.type = Geometry::BOX;
+  if (!c->Attribute("size"))
+  {
+    //CONSOLE_BRIDGE_logError("Box shape has no size attribute");
+    return false;
+  }
+  try
+  {
+    b.dim.init(c->Attribute("size"));
+  }
+  catch (ParseError &e)
+  {
+    b.dim.clear();
+    //CONSOLE_BRIDGE_logError(e.what());
+    return false;
+  }
+  return true;
+}
+
+bool parseCylinder(Cylinder &y, TiXmlElement *c)
+{
+  y.clear();
+
+  y.type = Geometry::CYLINDER;
+  if (!c->Attribute("length") ||
+      !c->Attribute("radius"))
+  {
+    //CONSOLE_BRIDGE_logError("Cylinder shape must have both length and radius attributes");
+    return false;
+  }
+
+  try {
+    y.length = strToDouble(c->Attribute("length"));
+  } catch(std::runtime_error &) {
+    std::stringstream stm;
+    stm << "length [" << c->Attribute("length") << "] is not a valid float";
+    //CONSOLE_BRIDGE_logError(stm.str().c_str());
+    return false;
+  }
+
+  try {
+    y.radius = strToDouble(c->Attribute("radius"));
+  } catch(std::runtime_error &) {
+    std::stringstream stm;
+    stm << "radius [" << c->Attribute("radius") << "] is not a valid float";
+    //CONSOLE_BRIDGE_logError(stm.str().c_str());
+    return false;
+  }
+
+  return true;
+}
+
+
+bool parseMesh(Mesh &m, TiXmlElement *c)
+{
+  m.clear();
+
+  m.type = Geometry::MESH;
+  if (!c->Attribute("filename")) {
+    //CONSOLE_BRIDGE_logError("Mesh must contain a filename attribute");
+    return false;
+  }
+
+  m.filename = c->Attribute("filename");
+
+  if (c->Attribute("scale")) {
+    try {
+      m.scale.init(c->Attribute("scale"));
+    }
+    catch (ParseError &e) {
+      m.scale.clear();
+      //CONSOLE_BRIDGE_logError("Mesh scale was specified, but could not be parsed: %s", e.what());
+      return false;
+    }
+  }
+  else
+  {
+    m.scale.x = m.scale.y = m.scale.z = 1;
+  }
+  return true;
+}
+
+GeometrySharedPtr parseGeometry(TiXmlElement *g)
+{
+  GeometrySharedPtr geom;
+  if (!g) return geom;
+
+  TiXmlElement *shape = g->FirstChildElement();
+  if (!shape)
+  {
+    //CONSOLE_BRIDGE_logError("Geometry tag contains no child element.");
+    return geom;
+  }
+
+  std::string type_name = shape->ValueStr();
+
+  if (type_name == "sphere")
+  {
+    Sphere *s = new Sphere();
+    geom.reset(s);
+    if (parseSphere(*s, shape))
+      return geom;
+  }
+  else if (type_name == "box")
+  {
+    Box *b = new Box();
+    geom.reset(b);
+    if (parseBox(*b, shape))
+      return geom;
+  }
+  else if (type_name == "cylinder")
+  {
+    Cylinder *c = new Cylinder();
+    geom.reset(c);
+    if (parseCylinder(*c, shape))
+      return geom;
+  }
+  else if (type_name == "mesh")
+  {
+    Mesh *m = new Mesh();
+    geom.reset(m);
+    if (parseMesh(*m, shape))
+      return geom;    
+  }
+  else
+  {
+    //CONSOLE_BRIDGE_logError("Unknown geometry type '%s'", type_name.c_str());
+    return geom;
+  }
+  
+  return GeometrySharedPtr();
+}
+
+bool parseInertial(Inertial &i, TiXmlElement *config)
+{
+  i.clear();
+
+  // Origin
+  TiXmlElement *o = config->FirstChildElement("origin");
+  if (o)
+  {
+    if (!parsePose(i.origin, o))
+      return false;
+  }
+
+  TiXmlElement *mass_xml = config->FirstChildElement("mass");
+  if (!mass_xml)
+  {
+    //CONSOLE_BRIDGE_logError("Inertial element must have a mass element");
+    return false;
+  }
+  if (!mass_xml->Attribute("value"))
+  {
+    //CONSOLE_BRIDGE_logError("Inertial: mass element must have value attribute");
+    return false;
+  }
+
+  try {
+    i.mass = strToDouble(mass_xml->Attribute("value"));
+  } catch(std::runtime_error &) {
+    std::stringstream stm;
+    stm << "Inertial: mass [" << mass_xml->Attribute("value")
+        << "] is not a float";
+    //CONSOLE_BRIDGE_logError(stm.str().c_str());
+    return false;
+  }
+
+  TiXmlElement *inertia_xml = config->FirstChildElement("inertia");
+  if (!inertia_xml)
+  {
+    //CONSOLE_BRIDGE_logError("Inertial element must have inertia element");
+    return false;
+  }
+
+  std::vector<std::pair<std::string, double>> attrs{
+      std::make_pair("ixx", 0.0),
+      std::make_pair("ixy", 0.0),
+      std::make_pair("ixz", 0.0),
+      std::make_pair("iyy", 0.0),
+      std::make_pair("iyz", 0.0),
+      std::make_pair("izz", 0.0)
+  };
+
+  for (auto& attr : attrs)
+  {
+    if (!inertia_xml->Attribute(attr.first))
+    {
+      std::stringstream stm;
+      stm << "Inertial: inertia element missing " << attr.first << " attribute";
+      //CONSOLE_BRIDGE_logError(stm.str().c_str());
+      return false;
+    }
+
+    try {
+      attr.second = strToDouble(inertia_xml->Attribute(attr.first.c_str()));
+    } catch(std::runtime_error &) {
+      std::stringstream stm;
+      stm << "Inertial: inertia element " << attr.first << " is not a valid double";
+      //CONSOLE_BRIDGE_logError(stm.str().c_str());
+      return false;
+    }
+  }
+
+  i.ixx = attrs[0].second;
+  i.ixy = attrs[1].second;
+  i.ixz = attrs[2].second;
+  i.iyy = attrs[3].second;
+  i.iyz = attrs[4].second;
+  i.izz = attrs[5].second;
+
+  return true;
+}
+
+bool parseVisual(Visual &vis, TiXmlElement *config)
+{
+  vis.clear();
+
+  // Origin
+  TiXmlElement *o = config->FirstChildElement("origin");
+  if (o) {
+    if (!parsePose(vis.origin, o))
+      return false;
+  }
+
+  // Geometry
+  TiXmlElement *geom = config->FirstChildElement("geometry");
+  vis.geometry = parseGeometry(geom);
+  if (!vis.geometry)
+    return false;
+
+  const char *name_char = config->Attribute("name");
+  if (name_char)
+    vis.name = name_char;
+
+  // Material
+  TiXmlElement *mat = config->FirstChildElement("material");
+  if (mat) {
+    // get material name
+    if (!mat->Attribute("name")) {
+      //CONSOLE_BRIDGE_logError("Visual material must contain a name attribute");
+      return false;
+    }
+    vis.material_name = mat->Attribute("name");
+    
+    // try to parse material element in place
+    vis.material.reset(new Material());
+    if (!parseMaterial(*vis.material, mat, true))
+    {
+      vis.material.reset();
+    }
+  }
+  
+  return true;
+}
+
+bool parseCollision(Collision &col, TiXmlElement* config)
+{  
+  col.clear();
+
+  // Origin
+  TiXmlElement *o = config->FirstChildElement("origin");
+  if (o) {
+    if (!parsePose(col.origin, o))
+      return false;
+  }
+  
+  // Geometry
+  TiXmlElement *geom = config->FirstChildElement("geometry");
+  col.geometry = parseGeometry(geom);
+  if (!col.geometry)
+    return false;
+
+  const char *name_char = config->Attribute("name");
+  if (name_char)
+    col.name = name_char;
+
+  return true;
+}
+
+bool parseLink(Link &link, TiXmlElement* config)
+{
+  
+  link.clear();
+
+  const char *name_char = config->Attribute("name");
+  if (!name_char)
+  {
+    //CONSOLE_BRIDGE_logError("No name given for the link.");
+    return false;
+  }
+  link.name = std::string(name_char);
+
+  // Inertial (optional)
+  TiXmlElement *i = config->FirstChildElement("inertial");
+  if (i)
+  {
+    link.inertial.reset(new Inertial());
+    if (!parseInertial(*link.inertial, i))
+    {
+      //CONSOLE_BRIDGE_logError("Could not parse inertial element for Link [%s]", link.name.c_str());
+      return false;
+    }
+  }
+
+  // Multiple Visuals (optional)
+  for (TiXmlElement* vis_xml = config->FirstChildElement("visual"); vis_xml; vis_xml = vis_xml->NextSiblingElement("visual"))
+  {
+
+    VisualSharedPtr vis;
+    vis.reset(new Visual());
+    if (parseVisual(*vis, vis_xml))
+    {
+      link.visual_array.push_back(vis);
+    }
+    else
+    {
+      vis.reset();
+      //CONSOLE_BRIDGE_logError("Could not parse visual element for Link [%s]", link.name.c_str());
+      return false;
+    }
+  }
+
+  // Visual (optional)
+  // Assign the first visual to the .visual ptr, if it exists
+  if (!link.visual_array.empty())
+    link.visual = link.visual_array[0];
+  
+  // Multiple Collisions (optional)
+  for (TiXmlElement* col_xml = config->FirstChildElement("collision"); col_xml; col_xml = col_xml->NextSiblingElement("collision"))
+  {
+    CollisionSharedPtr col;
+    col.reset(new Collision());
+    if (parseCollision(*col, col_xml))
+    {      
+      link.collision_array.push_back(col);
+    }
+    else
+    {
+      col.reset();
+      //CONSOLE_BRIDGE_logError("Could not parse collision element for Link [%s]",  link.name.c_str());
+      return false;
+    }
+  }
+  
+  // Collision (optional)  
+  // Assign the first collision to the .collision ptr, if it exists
+  if (!link.collision_array.empty())
+    link.collision = link.collision_array[0];
+
+  return true;
+}
+
+/* exports */
+bool exportPose(Pose &pose, TiXmlElement* xml);
+
+bool exportMaterial(Material &material, TiXmlElement *xml)
+{
+  TiXmlElement *material_xml = new TiXmlElement("material");
+  material_xml->SetAttribute("name", material.name);
+
+  TiXmlElement* texture = new TiXmlElement("texture");
+  if (!material.texture_filename.empty())
+    texture->SetAttribute("filename", material.texture_filename);
+  material_xml->LinkEndChild(texture);
+
+  TiXmlElement* color = new TiXmlElement("color");
+  color->SetAttribute("rgba", urdf_export_helpers::values2str(material.color));
+  material_xml->LinkEndChild(color);
+  xml->LinkEndChild(material_xml);
+  return true;
+}
+
+bool exportSphere(Sphere &s, TiXmlElement *xml)
+{
+  // e.g. add <sphere radius="1"/>
+  TiXmlElement *sphere_xml = new TiXmlElement("sphere");
+  sphere_xml->SetAttribute("radius", urdf_export_helpers::values2str(s.radius));
+  xml->LinkEndChild(sphere_xml);
+  return true;
+}
+
+bool exportBox(Box &b, TiXmlElement *xml)
+{
+  // e.g. add <box size="1 1 1"/>
+  TiXmlElement *box_xml = new TiXmlElement("box");
+  box_xml->SetAttribute("size", urdf_export_helpers::values2str(b.dim));
+  xml->LinkEndChild(box_xml);
+  return true;
+}
+
+bool exportCylinder(Cylinder &y, TiXmlElement *xml)
+{
+  // e.g. add <cylinder radius="1"/>
+  TiXmlElement *cylinder_xml = new TiXmlElement("cylinder");
+  cylinder_xml->SetAttribute("radius", urdf_export_helpers::values2str(y.radius));
+  cylinder_xml->SetAttribute("length", urdf_export_helpers::values2str(y.length));
+  xml->LinkEndChild(cylinder_xml);
+  return true;
+}
+
+bool exportMesh(Mesh &m, TiXmlElement *xml)
+{
+  // e.g. add <mesh filename="my_file" scale="1 1 1"/>
+  TiXmlElement *mesh_xml = new TiXmlElement("mesh");
+  if (!m.filename.empty())
+    mesh_xml->SetAttribute("filename", m.filename);
+  mesh_xml->SetAttribute("scale", urdf_export_helpers::values2str(m.scale));
+  xml->LinkEndChild(mesh_xml);
+  return true;
+}
+
+bool exportGeometry(GeometrySharedPtr &geom, TiXmlElement *xml)
+{
+  TiXmlElement *geometry_xml = new TiXmlElement("geometry");
+  if (urdf::dynamic_pointer_cast<Sphere>(geom))
+  {
+    exportSphere((*(urdf::dynamic_pointer_cast<Sphere>(geom).get())), geometry_xml);
+  }
+  else if (urdf::dynamic_pointer_cast<Box>(geom))
+  {
+    exportBox((*(urdf::dynamic_pointer_cast<Box>(geom).get())), geometry_xml);
+  }
+  else if (urdf::dynamic_pointer_cast<Cylinder>(geom))
+  {
+    exportCylinder((*(urdf::dynamic_pointer_cast<Cylinder>(geom).get())), geometry_xml);
+  }
+  else if (urdf::dynamic_pointer_cast<Mesh>(geom))
+  {
+    exportMesh((*(urdf::dynamic_pointer_cast<Mesh>(geom).get())), geometry_xml);
+  }
+  else
+  {
+    //CONSOLE_BRIDGE_logError("geometry not specified, I'll make one up for you!");
+    Sphere *s = new Sphere();
+    s->radius = 0.03;
+    geom.reset(s);
+    exportSphere((*(urdf::dynamic_pointer_cast<Sphere>(geom).get())), geometry_xml);
+  }
+
+  xml->LinkEndChild(geometry_xml);
+  return true;
+}
+
+bool exportInertial(Inertial &i, TiXmlElement *xml)
+{
+  // adds <inertial>
+  //        <mass value="1"/>
+  //        <pose xyz="0 0 0" rpy="0 0 0"/>
+  //        <inertia ixx="1" ixy="0" />
+  //      </inertial>
+  TiXmlElement *inertial_xml = new TiXmlElement("inertial");
+
+  TiXmlElement *mass_xml = new TiXmlElement("mass");
+  mass_xml->SetAttribute("value", urdf_export_helpers::values2str(i.mass));
+  inertial_xml->LinkEndChild(mass_xml);
+
+  exportPose(i.origin, inertial_xml);
+
+  TiXmlElement *inertia_xml = new TiXmlElement("inertia");
+  inertia_xml->SetAttribute("ixx", urdf_export_helpers::values2str(i.ixx));
+  inertia_xml->SetAttribute("ixy", urdf_export_helpers::values2str(i.ixy));
+  inertia_xml->SetAttribute("ixz", urdf_export_helpers::values2str(i.ixz));
+  inertia_xml->SetAttribute("iyy", urdf_export_helpers::values2str(i.iyy));
+  inertia_xml->SetAttribute("iyz", urdf_export_helpers::values2str(i.iyz));
+  inertia_xml->SetAttribute("izz", urdf_export_helpers::values2str(i.izz));
+  inertial_xml->LinkEndChild(inertia_xml);
+
+  xml->LinkEndChild(inertial_xml);
+  
+  return true;
+}
+
+bool exportVisual(Visual &vis, TiXmlElement *xml)
+{
+  // <visual group="default">
+  //   <origin rpy="0 0 0" xyz="0 0 0"/>
+  //   <geometry>
+  //     <mesh filename="mesh.dae"/>
+  //   </geometry>
+  //   <material name="Grey"/>
+  // </visual>
+  TiXmlElement * visual_xml = new TiXmlElement("visual");
+
+  exportPose(vis.origin, visual_xml);
+
+  exportGeometry(vis.geometry, visual_xml);
+
+  if (vis.material)
+    exportMaterial(*vis.material, visual_xml);
+
+  xml->LinkEndChild(visual_xml);
+
+  return true;
+}
+
+bool exportCollision(Collision &col, TiXmlElement* xml)
+{  
+  // <collision group="default">
+  //   <origin rpy="0 0 0" xyz="0 0 0"/>
+  //   <geometry>
+  //     <mesh filename="mesh.dae"/>
+  //   </geometry>
+  //   <material name="Grey"/>
+  // </collision>
+  TiXmlElement * collision_xml = new TiXmlElement("collision");
+
+  exportPose(col.origin, collision_xml);
+
+  exportGeometry(col.geometry, collision_xml);
+
+  xml->LinkEndChild(collision_xml);
+
+  return true;
+}
+
+bool exportLink(Link &link, TiXmlElement* xml)
+{
+  TiXmlElement * link_xml = new TiXmlElement("link");
+  link_xml->SetAttribute("name", link.name);
+
+  if (link.inertial)
+    exportInertial(*link.inertial, link_xml);
+  for (std::size_t i = 0 ; i < link.visual_array.size() ; ++i)
+    exportVisual(*link.visual_array[i], link_xml);
+  for (std::size_t i = 0 ; i < link.collision_array.size() ; ++i)
+    exportCollision(*link.collision_array[i], link_xml);
+
+  xml->LinkEndChild(link_xml);
+
+  return true;
+}
+
+}

src/kdl_parser/src/model.cpp

@@ -0,0 +1,326 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+* 
+*  Copyright (c) 2008, Willow Garage, Inc.
+*  All rights reserved.
+* 
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+* 
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+* 
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/* Author: Wim Meeussen */
+
+#include <fstream>
+#include <map>
+#include <stdexcept>
+#include <string>
+#include "urdf_parser/urdf_parser.h"
+//#include <console_bridge/console.h>
+
+namespace urdf{
+
+bool parseMaterial(Material &material, TiXmlElement *config, bool only_name_is_ok);
+bool parseLink(Link &link, TiXmlElement *config);
+bool parseJoint(Joint &joint, TiXmlElement *config);
+
+ModelInterfaceSharedPtr  parseURDFFileDocument(const std::string & xml_str)
+{
+    //std::ifstream stream( path.c_str() );
+    //if (!stream)
+    //{
+    //  ////CONSOLE_BRIDGE_logError(("File " + path + " does not exist").c_str());
+    //  return ModelInterfaceSharedPtr();
+    //}
+
+    //std::string xml_str((std::istreambuf_iterator<char>(stream)),
+	   //                  std::istreambuf_iterator<char>());
+    return urdf::parseURDF( xml_str );
+}
+
+ModelInterfaceSharedPtr  parseURDFFile(const std::string& path)
+{
+    std::ifstream stream( path.c_str() );
+    if (!stream)
+    {
+      ////CONSOLE_BRIDGE_logError(("File " + path + " does not exist").c_str());
+      return ModelInterfaceSharedPtr();
+    }
+
+    std::string xml_str((std::istreambuf_iterator<char>(stream)),
+                         std::istreambuf_iterator<char>());
+    return urdf::parseURDF(xml_str);
+}
+
+bool assignMaterial(const VisualSharedPtr& visual, ModelInterfaceSharedPtr& model, const char* link_name)
+{
+  if (visual->material_name.empty())
+    return true;
+
+  const MaterialSharedPtr& material = model->getMaterial(visual->material_name);
+  if (material)
+  {
+    //CONSOLE_BRIDGE_logDebug("urdfdom: setting link '%s' material to '%s'", link_name, visual->material_name.c_str());
+    visual->material = material;
+  }
+  else
+  {
+    if (visual->material)
+    {
+      //CONSOLE_BRIDGE_logDebug("urdfdom: link '%s' material '%s' defined in Visual.", link_name, visual->material_name.c_str());
+      model->materials_.insert(make_pair(visual->material->name, visual->material));
+    }
+    else
+    {
+      //CONSOLE_BRIDGE_logWarn("link '%s' material '%s' undefined.", link_name,visual->material_name.c_str());
+      return false;
+    }
+  }
+  return true;
+}
+
+ModelInterfaceSharedPtr  parseURDF(const std::string &xml_string)
+{
+  ModelInterfaceSharedPtr model(new ModelInterface);
+  model->clear();
+
+  TiXmlDocument xml_doc;
+  xml_doc.Parse(xml_string.c_str());
+  if (xml_doc.Error())
+  {
+    ////CONSOLE_BRIDGE_logError(xml_doc.ErrorDesc());
+    xml_doc.ClearError();
+    model.reset();
+    return model;
+  }
+
+  TiXmlElement *robot_xml = xml_doc.FirstChildElement("robot");
+  if (!robot_xml)
+  {
+    ////CONSOLE_BRIDGE_logError("Could not find the 'robot' element in the xml file");
+    model.reset();
+    return model;
+  }
+
+  // Get robot name
+  const char *name = robot_xml->Attribute("name");
+  if (!name)
+  {
+    ////CONSOLE_BRIDGE_logError("No name given for the robot.");
+    model.reset();
+    return model;
+  }
+  model->name_ = std::string(name);
+
+  try
+  {
+    urdf_export_helpers::URDFVersion version(robot_xml->Attribute("version"));
+    if (!version.equal(1, 0))
+    {
+      throw std::runtime_error("Invalid 'version' specified; only version 1.0 is currently supported");
+    }
+  }
+  catch (const std::runtime_error & err)
+  {
+    ////CONSOLE_BRIDGE_logError(err.what());
+    model.reset();
+    return model;
+  }
+
+  // Get all Material elements
+  for (TiXmlElement* material_xml = robot_xml->FirstChildElement("material"); material_xml; material_xml = material_xml->NextSiblingElement("material"))
+  {
+    MaterialSharedPtr material;
+    material.reset(new Material);
+
+    try {
+      parseMaterial(*material, material_xml, false); // material needs to be fully defined here
+      if (model->getMaterial(material->name))
+      {
+        ////CONSOLE_BRIDGE_logError("material '%s' is not unique.", material->name.c_str());
+        material.reset();
+        model.reset();
+        return model;
+      }
+      else
+      {
+        model->materials_.insert(make_pair(material->name,material));
+        //CONSOLE_BRIDGE_logDebug("urdfdom: successfully added a new material '%s'", material->name.c_str());
+      }
+    }
+    catch (ParseError &/*e*/) {
+      ////CONSOLE_BRIDGE_logError("material xml is not initialized correctly");
+      material.reset();
+      model.reset();
+      return model;
+    }
+  }
+
+  // Get all Link elements
+  for (TiXmlElement* link_xml = robot_xml->FirstChildElement("link"); link_xml; link_xml = link_xml->NextSiblingElement("link"))
+  {
+    LinkSharedPtr link;
+    link.reset(new Link);
+
+    try {
+      parseLink(*link, link_xml);
+      if (model->getLink(link->name))
+      {
+        ////CONSOLE_BRIDGE_logError("link '%s' is not unique.", link->name.c_str());
+        model.reset();
+        return model;
+      }
+      else
+      {
+        // set link visual(s) material
+        //CONSOLE_BRIDGE_logDebug("urdfdom: setting link '%s' material", link->name.c_str());
+        if (link->visual)
+        {
+          assignMaterial(link->visual, model, link->name.c_str());
+        }
+        for (const auto& visual : link->visual_array)
+        {
+          assignMaterial(visual, model, link->name.c_str());
+        }
+
+        model->links_.insert(make_pair(link->name,link));
+        //CONSOLE_BRIDGE_logDebug("urdfdom: successfully added a new link '%s'", link->name.c_str());
+      }
+    }
+    catch (ParseError &/*e*/) {
+      ////CONSOLE_BRIDGE_logError("link xml is not initialized correctly");
+      model.reset();
+      return model;
+    }
+  }
+  if (model->links_.empty()){
+    ////CONSOLE_BRIDGE_logError("No link elements found in urdf file");
+    model.reset();
+    return model;
+  }
+
+  // Get all Joint elements
+  for (TiXmlElement* joint_xml = robot_xml->FirstChildElement("joint"); joint_xml; joint_xml = joint_xml->NextSiblingElement("joint"))
+  {
+    JointSharedPtr joint;
+    joint.reset(new Joint);
+
+    if (parseJoint(*joint, joint_xml))
+    {
+      if (model->getJoint(joint->name))
+      {
+        ////CONSOLE_BRIDGE_logError("joint '%s' is not unique.", joint->name.c_str());
+        model.reset();
+        return model;
+      }
+      else
+      {
+        model->joints_.insert(make_pair(joint->name,joint));
+        //CONSOLE_BRIDGE_logDebug("urdfdom: successfully added a new joint '%s'", joint->name.c_str());
+      }
+    }
+    else
+    {
+      ////CONSOLE_BRIDGE_logError("joint xml is not initialized correctly");
+      model.reset();
+      return model;
+    }
+  }
+
+
+  // every link has children links and joints, but no parents, so we create a
+  // local convenience data structure for keeping child->parent relations
+  std::map<std::string, std::string> parent_link_tree;
+  parent_link_tree.clear();
+
+  // building tree: name mapping
+  try 
+  {
+    model->initTree(parent_link_tree);
+  }
+  catch(ParseError &e)
+  {
+    ////CONSOLE_BRIDGE_logError("Failed to build tree: %s", e.what());
+    model.reset();
+    return model;
+  }
+
+  // find the root link
+  try
+  {
+    model->initRoot(parent_link_tree);
+  }
+  catch(ParseError &e)
+  {
+    ////CONSOLE_BRIDGE_logError("Failed to find root link: %s", e.what());
+    model.reset();
+    return model;
+  }
+  
+  return model;
+}
+
+bool exportMaterial(Material &material, TiXmlElement *config);
+bool exportLink(Link &link, TiXmlElement *config);
+bool exportJoint(Joint &joint, TiXmlElement *config);
+TiXmlDocument*  exportURDF(const ModelInterface &model)
+{
+  TiXmlDocument *doc = new TiXmlDocument();
+
+  TiXmlElement *robot = new TiXmlElement("robot");
+  robot->SetAttribute("name", model.name_);
+  doc->LinkEndChild(robot);
+
+
+  for (std::map<std::string, MaterialSharedPtr>::const_iterator m=model.materials_.begin(); m!=model.materials_.end(); m++)
+  {
+    //CONSOLE_BRIDGE_logDebug("urdfdom: exporting material [%s]\n",m->second->name.c_str());
+    exportMaterial(*(m->second), robot);
+  }
+
+  for (std::map<std::string, LinkSharedPtr>::const_iterator l=model.links_.begin(); l!=model.links_.end(); l++)  
+  {
+    //CONSOLE_BRIDGE_logDebug("urdfdom: exporting link [%s]\n",l->second->name.c_str());
+    exportLink(*(l->second), robot);
+  }
+  	
+  for (std::map<std::string, JointSharedPtr>::const_iterator j=model.joints_.begin(); j!=model.joints_.end(); j++)  
+  {
+    //CONSOLE_BRIDGE_logDebug("urdfdom: exporting joint [%s]\n",j->second->name.c_str());
+    exportJoint(*(j->second), robot);
+  }
+
+  return doc;
+}
+    
+TiXmlDocument*  exportURDF(ModelInterfaceSharedPtr &model)
+{
+  return exportURDF(*model);
+}
+
+
+}
+

src/kdl_parser/src/pose.cpp

@@ -0,0 +1,135 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+* 
+*  Copyright (c) 2008, Willow Garage, Inc.
+*  All rights reserved.
+* 
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+* 
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+* 
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/* Author: Wim Meeussen, John Hsu */
+
+
+#include <urdf_model/pose.h>
+#include <fstream>
+#include <sstream>
+#include <algorithm>
+//#include <console_bridge/console.h>
+#include <tinyxml.h>
+#include <urdf_parser/urdf_parser.h>
+
+namespace urdf_export_helpers {
+
+std::string values2str(unsigned int count, const double *values, double (*conv)(double))
+{
+    std::stringstream ss;
+    for (unsigned int i = 0 ; i < count ; i++)
+    {
+        if (i > 0)
+            ss << " ";
+        ss << (conv ? conv(values[i]) : values[i]);
+    }
+    return ss.str();
+}
+std::string values2str(urdf::Vector3 vec)
+{
+    double xyz[3];
+    xyz[0] = vec.x;
+    xyz[1] = vec.y;
+    xyz[2] = vec.z;
+    return values2str(3, xyz);
+}
+std::string values2str(urdf::Rotation rot)
+{
+    double rpy[3];
+    rot.getRPY(rpy[0], rpy[1], rpy[2]);
+    return values2str(3, rpy);
+}
+std::string values2str(urdf::Color c)
+{
+    double rgba[4];
+    rgba[0] = c.r;
+    rgba[1] = c.g;
+    rgba[2] = c.b;
+    rgba[3] = c.a;
+    return values2str(4, rgba);
+}
+std::string values2str(double d)
+{
+    return values2str(1, &d);
+}
+}
+
+namespace urdf{
+
+bool parsePose(Pose &pose, TiXmlElement* xml)
+{
+  pose.clear();
+  if (xml)
+  {
+    const char* xyz_str = xml->Attribute("xyz");
+    if (xyz_str != NULL)
+    {
+      try {
+        pose.position.init(xyz_str);
+      }
+      catch (ParseError &e) {
+        ////CONSOLE_BRIDGE_logError(e.what());
+        return false;
+      }
+    }
+
+    const char* rpy_str = xml->Attribute("rpy");
+    if (rpy_str != NULL)
+    {
+      try {
+        pose.rotation.init(rpy_str);
+      }
+      catch (ParseError &e) {
+        ////CONSOLE_BRIDGE_logError(e.what());
+        return false;
+      }
+    }
+  }
+  return true;
+}
+
+bool exportPose(Pose &pose, TiXmlElement* xml)
+{
+  TiXmlElement *origin = new TiXmlElement("origin");
+  std::string pose_xyz_str = urdf_export_helpers::values2str(pose.position);
+  std::string pose_rpy_str = urdf_export_helpers::values2str(pose.rotation);
+  origin->SetAttribute("xyz", pose_xyz_str);
+  origin->SetAttribute("rpy", pose_rpy_str);
+  xml->LinkEndChild(origin);
+  return true;
+}
+
+}
+
+

src/kdl_parser/src/tinystr.cpp

@@ -0,0 +1,111 @@
+/*
+www.sourceforge.net/projects/tinyxml
+
+This software is provided 'as-is', without any express or implied
+warranty. In no event will the authors be held liable for any
+damages arising from the use of this software.
+
+Permission is granted to anyone to use this software for any
+purpose, including commercial applications, and to alter it and
+redistribute it freely, subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must
+not claim that you wrote the original software. If you use this
+software in a product, an acknowledgment in the product documentation
+would be appreciated but is not required.
+
+2. Altered source versions must be plainly marked as such, and
+must not be misrepresented as being the original software.
+
+3. This notice may not be removed or altered from any source
+distribution.
+*/
+
+
+#ifndef TIXML_USE_STL
+
+#include "tinystr.h"
+
+// Error value for find primitive
+const TiXmlString::size_type TiXmlString::npos = static_cast< TiXmlString::size_type >(-1);
+
+
+// Null rep.
+TiXmlString::Rep TiXmlString::nullrep_ = { 0, 0, { '\0' } };
+
+
+void TiXmlString::reserve (size_type cap)
+{
+	if (cap > capacity())
+	{
+		TiXmlString tmp;
+		tmp.init(length(), cap);
+		memcpy(tmp.start(), data(), length());
+		swap(tmp);
+	}
+}
+
+
+TiXmlString& TiXmlString::assign(const char* str, size_type len)
+{
+	size_type cap = capacity();
+	if (len > cap || cap > 3*(len + 8))
+	{
+		TiXmlString tmp;
+		tmp.init(len);
+		memcpy(tmp.start(), str, len);
+		swap(tmp);
+	}
+	else
+	{
+		memmove(start(), str, len);
+		set_size(len);
+	}
+	return *this;
+}
+
+
+TiXmlString& TiXmlString::append(const char* str, size_type len)
+{
+	size_type newsize = length() + len;
+	if (newsize > capacity())
+	{
+		reserve (newsize + capacity());
+	}
+	memmove(finish(), str, len);
+	set_size(newsize);
+	return *this;
+}
+
+
+TiXmlString operator + (const TiXmlString & a, const TiXmlString & b)
+{
+	TiXmlString tmp;
+	tmp.reserve(a.length() + b.length());
+	tmp += a;
+	tmp += b;
+	return tmp;
+}
+
+TiXmlString operator + (const TiXmlString & a, const char* b)
+{
+	TiXmlString tmp;
+	TiXmlString::size_type b_len = static_cast<TiXmlString::size_type>( strlen(b) );
+	tmp.reserve(a.length() + b_len);
+	tmp += a;
+	tmp.append(b, b_len);
+	return tmp;
+}
+
+TiXmlString operator + (const char* a, const TiXmlString & b)
+{
+	TiXmlString tmp;
+	TiXmlString::size_type a_len = static_cast<TiXmlString::size_type>( strlen(a) );
+	tmp.reserve(a_len + b.length());
+	tmp.append(a, a_len);
+	tmp += b;
+	return tmp;
+}
+
+
+#endif	// TIXML_USE_STL

src/kdl_parser/src/tinyxmlerror.cpp

@@ -0,0 +1,52 @@
+/*
+www.sourceforge.net/projects/tinyxml
+Original code (2.0 and earlier )copyright (c) 2000-2006 Lee Thomason (www.grinninglizard.com)
+
+This software is provided 'as-is', without any express or implied 
+warranty. In no event will the authors be held liable for any 
+damages arising from the use of this software.
+
+Permission is granted to anyone to use this software for any 
+purpose, including commercial applications, and to alter it and 
+redistribute it freely, subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must
+not claim that you wrote the original software. If you use this
+software in a product, an acknowledgment in the product documentation
+would be appreciated but is not required.
+
+2. Altered source versions must be plainly marked as such, and
+must not be misrepresented as being the original software.
+
+3. This notice may not be removed or altered from any source
+distribution.
+*/
+
+#include "tinyxml.h"
+
+// The goal of the seperate error file is to make the first
+// step towards localization. tinyxml (currently) only supports
+// english error messages, but the could now be translated.
+//
+// It also cleans up the code a bit.
+//
+
+const char* TiXmlBase::errorString[ TiXmlBase::TIXML_ERROR_STRING_COUNT ] =
+{
+	"No error",
+	"Error",
+	"Failed to open file",
+	"Error parsing Element.",
+	"Failed to read Element name",
+	"Error reading Element value.",
+	"Error reading Attributes.",
+	"Error: empty tag.",
+	"Error reading end tag.",
+	"Error parsing Unknown.",
+	"Error parsing Comment.",
+	"Error parsing Declaration.",
+	"Error document empty.",
+	"Error null (0) or unexpected EOF found in input stream.",
+	"Error parsing CDATA.",
+	"Error when TiXmlDocument added to document, because TiXmlDocument can only be at the root.",
+};

src/kdl_parser/src/twist.cpp

@@ -0,0 +1,84 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+* 
+*  Copyright (c) 2008, Willow Garage, Inc.
+*  All rights reserved.
+* 
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+* 
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+* 
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/* Author: John Hsu */
+
+
+#include <urdf_model/twist.h>
+#include <fstream>
+#include <sstream>
+#include <algorithm>
+#include <tinyxml.h>
+//#include <console_bridge/console.h>
+
+namespace urdf{
+
+bool parseTwist(Twist &twist, TiXmlElement* xml)
+{
+  twist.clear();
+  if (xml)
+  {
+    const char* linear_char = xml->Attribute("linear");
+    if (linear_char != NULL)
+    {
+      try {
+        twist.linear.init(linear_char);
+      }
+      catch (ParseError &e) {
+        twist.linear.clear();
+        //CONSOLE_BRIDGE_logError("Malformed linear string [%s]: %s", linear_char, e.what());
+        return false;
+      }
+    }
+
+    const char* angular_char = xml->Attribute("angular");
+    if (angular_char != NULL)
+    {
+      try {
+        twist.angular.init(angular_char);
+      }
+      catch (ParseError &e) {
+        twist.angular.clear();
+        //CONSOLE_BRIDGE_logError("Malformed angular [%s]: %s", angular_char, e.what());
+        return false;
+      }
+    }
+  }
+  return true;
+}
+
+}
+
+
+

src/kdl_parser/src/urdf_model_state.cpp

@@ -0,0 +1,159 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2008, Willow Garage, Inc.
+*  All rights reserved.
+*
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+*
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+*
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/* Author: John Hsu */
+
+
+#include <urdf_model_state/model_state.h>
+#include <urdf_model/utils.h>
+#include <fstream>
+#include <locale>
+#include <sstream>
+#include <stdexcept>
+#include <string>
+#include <algorithm>
+#include <tinyxml.h>
+//#include <console_bridge/console.h>
+
+namespace urdf {
+
+    bool parseModelState(ModelState& ms, TiXmlElement* config)
+    {
+        ms.clear();
+
+        const char* name_char = config->Attribute("name");
+        if (!name_char)
+        {
+            /*CONSOLE_BRIDGE_logError("No name given for the model_state.");*/
+
+            return false;
+        }
+        ms.name = std::string(name_char);
+
+        const char* time_stamp_char = config->Attribute("time_stamp");
+        if (time_stamp_char)
+        {
+            try {
+                ms.time_stamp.set(strToDouble(time_stamp_char));
+            }
+            catch (std::runtime_error&) {
+                //CONSOLE_BRIDGE_logError("Parsing time stamp [%s] failed", time_stamp_char);
+                return false;
+            }
+        }
+
+        TiXmlElement* joint_state_elem = config->FirstChildElement("joint_state");
+        if (joint_state_elem)
+        {
+            JointStateSharedPtr joint_state;
+            joint_state.reset(new JointState());
+
+            const char* joint_char = joint_state_elem->Attribute("joint");
+            if (joint_char)
+                joint_state->joint = std::string(joint_char);
+            else
+            {
+                //CONSOLE_BRIDGE_logError("No joint name given for the model_state.");
+                return false;
+            }
+
+            /*// parse position*/
+            const char* position_char = joint_state_elem->Attribute("position");
+            if (position_char)
+            {
+
+                std::vector<std::string> pieces;
+                urdf::split_string(pieces, position_char, " ");
+                for (unsigned int i = 0; i < pieces.size(); ++i) {
+                    if (pieces[i] != "") {
+                        try {
+                            joint_state->position.push_back(strToDouble(pieces[i].c_str()));
+                        }
+                        catch (std::runtime_error&) {
+                            throw ParseError("position element (" + pieces[i] + ") is not a valid float");
+                        }
+                    }
+                }
+            }
+            /* parse velocity*/
+
+            const char* velocity_char = joint_state_elem->Attribute("velocity");
+            /**/
+            int i = 0;
+
+            if (velocity_char)
+            {
+
+                std::vector<std::string> pieces;
+                urdf::split_string(pieces, velocity_char, " ");
+                for (unsigned int i = 0; i < pieces.size(); ++i) {
+                    if (pieces[i] != "") {
+                        try {
+                            joint_state->velocity.push_back(strToDouble(pieces[i].c_str()));
+                        }
+                        catch (std::runtime_error&) {
+                            throw ParseError("velocity element (" + pieces[i] + ") is not a valid float");
+                        }
+                    }
+                }
+            }
+
+            // parse effort
+
+            const char* effort_char = joint_state_elem->Attribute("effort");
+            /**/
+
+            if(effort_char)
+            {
+                std::vector<std::string> pieces;
+                urdf::split_string(pieces, effort_char, " ");
+                for (unsigned int i = 0; i < pieces.size(); ++i) {
+                    if (pieces[i] != "") {
+                        try {
+                            joint_state->effort.push_back(strToDouble(pieces[i].c_str()));
+                        }
+                        catch (std::runtime_error&) {
+                            throw ParseError("effort element (" + pieces[i] + ") is not a valid float");
+                        }
+                    }
+                }
+            }
+
+            /*// add to vector*/
+            ms.joint_states.push_back(joint_state);
+            /* */
+        }
+        return false;
+    }
+}

src/kdl_parser/src/urdf_sensor.cpp

@@ -0,0 +1,360 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+* 
+*  Copyright (c) 2008, Willow Garage, Inc.
+*  All rights reserved.
+* 
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+* 
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+* 
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/* Author: John Hsu */
+
+
+#include <urdf_sensor/sensor.h>
+#include <fstream>
+#include <locale>
+#include <sstream>
+#include <stdexcept>
+#include <string>
+#include <algorithm>
+#include <tinyxml.h>
+#include <console_bridge/console.h>
+
+namespace urdf{
+
+bool parsePose(Pose &pose, TiXmlElement* xml);
+
+bool parseCamera(Camera &camera, TiXmlElement* config)
+{
+  camera.clear();
+  camera.type = VisualSensor::CAMERA;
+
+  TiXmlElement *image = config->FirstChildElement("image");
+  if (image)
+  {
+    const char* width_char = image->Attribute("width");
+    if (width_char)
+    {
+      try
+      {
+        camera.width = std::stoul(width_char);
+      }
+      catch (std::invalid_argument &e)
+      {
+        //CONSOLE_BRIDGE_logError("Camera image width [%s] is not a valid int: %s", width_char, e.what());
+        return false;
+      }
+      catch (std::out_of_range &e)
+      {
+        //CONSOLE_BRIDGE_logError("Camera image width [%s] is out of range: %s", width_char, e.what());
+        return false;
+      }
+    }
+    else
+    {
+      //CONSOLE_BRIDGE_logError("Camera sensor needs an image width attribute");
+      return false;
+    }
+
+    const char* height_char = image->Attribute("height");
+    if (height_char)
+    {
+      try
+      {
+        camera.height = std::stoul(height_char);
+      }
+      catch (std::invalid_argument &e)
+      {
+        //CONSOLE_BRIDGE_logError("Camera image height [%s] is not a valid int: %s", height_char, e.what());
+        return false;
+      }
+      catch (std::out_of_range &e)
+      {
+        //CONSOLE_BRIDGE_logError("Camera image height [%s] is out of range: %s", height_char, e.what());
+        return false;
+      }
+    }
+    else
+    {
+      //CONSOLE_BRIDGE_logError("Camera sensor needs an image height attribute");
+      return false;
+    }
+
+    const char* format_char = image->Attribute("format");
+    if (format_char)
+      camera.format = std::string(format_char);
+    else
+    {
+      //CONSOLE_BRIDGE_logError("Camera sensor needs an image format attribute");
+      return false;
+    }    
+
+    const char* hfov_char = image->Attribute("hfov");
+    if (hfov_char)
+    {
+      try {
+        camera.hfov = strToDouble(hfov_char);
+      } catch(std::runtime_error &) {
+        //CONSOLE_BRIDGE_logError("Camera image hfov [%s] is not a valid float", hfov_char);
+        return false;
+      }
+    }
+    else
+    {
+      //CONSOLE_BRIDGE_logError("Camera sensor needs an image hfov attribute");
+      return false;
+    }
+
+    const char* near_char = image->Attribute("near");
+    if (near_char)
+    {
+      try {
+        camera.near = strToDouble(near_char);
+      } catch(std::runtime_error &) {
+        //CONSOLE_BRIDGE_logError("Camera image near [%s] is not a valid float", near_char);
+        return false;
+      }
+    }
+    else
+    {
+      //CONSOLE_BRIDGE_logError("Camera sensor needs an image near attribute");
+      return false;
+    }
+
+    const char* far_char = image->Attribute("far");
+    if (far_char)
+    {
+      try {
+        camera.far = strToDouble(far_char);
+      } catch(std::runtime_error &) {
+        //CONSOLE_BRIDGE_logError("Camera image far [%s] is not a valid float", far_char);
+        return false;
+      }
+    }
+    else
+    {
+      //CONSOLE_BRIDGE_logError("Camera sensor needs an image far attribute");
+      return false;
+    }
+    
+  }
+  else
+  {
+    //CONSOLE_BRIDGE_logError("Camera sensor has no <image> element");
+    return false;
+  }
+  return true;
+}
+
+bool parseRay(Ray &ray, TiXmlElement* config)
+{
+  ray.clear();
+  ray.type = VisualSensor::RAY;
+
+  TiXmlElement *horizontal = config->FirstChildElement("horizontal");
+  if (horizontal)
+  {
+    const char* samples_char = horizontal->Attribute("samples");
+    if (samples_char)
+    {
+      try
+      {
+        ray.horizontal_samples = std::stoul(samples_char);
+      }
+      catch (std::invalid_argument &e)
+      {
+        //CONSOLE_BRIDGE_logError("Ray horizontal samples [%s] is not a valid float: %s", samples_char, e.what());
+        return false;
+      }
+      catch (std::out_of_range &e)
+      {
+        //CONSOLE_BRIDGE_logError("Ray horizontal samples [%s] is out of range: %s", samples_char, e.what());
+        return false;
+      }
+    }
+
+    const char* resolution_char = horizontal->Attribute("resolution");
+    if (resolution_char)
+    {
+      try {
+        ray.horizontal_resolution = strToDouble(resolution_char);
+      } catch(std::runtime_error &) {
+        //CONSOLE_BRIDGE_logError("Ray horizontal resolution [%s] is not a valid float", resolution_char);
+        return false;
+      }
+    }
+
+    const char* min_angle_char = horizontal->Attribute("min_angle");
+    if (min_angle_char)
+    {
+      try {
+        ray.horizontal_min_angle = strToDouble(min_angle_char);
+      } catch(std::runtime_error &) {
+        //CONSOLE_BRIDGE_logError("Ray horizontal min_angle [%s] is not a valid float", min_angle_char);
+        return false;
+      }
+    }
+
+    const char* max_angle_char = horizontal->Attribute("max_angle");
+    if (max_angle_char)
+    {
+      try {
+        ray.horizontal_max_angle = strToDouble(max_angle_char);
+      } catch(std::runtime_error &) {
+        //CONSOLE_BRIDGE_logError("Ray horizontal max_angle [%s] is not a valid float", max_angle_char);
+        return false;
+      }
+    }
+  }
+  
+  TiXmlElement *vertical = config->FirstChildElement("vertical");
+  if (vertical)
+  {
+    const char* samples_char = vertical->Attribute("samples");
+    if (samples_char)
+    {
+      try
+      {
+        ray.vertical_samples = std::stoul(samples_char);
+      }
+      catch (std::invalid_argument &e)
+      {
+        //CONSOLE_BRIDGE_logError("Ray vertical samples [%s] is not a valid float: %s", samples_char, e.what());
+        return false;
+      }
+      catch (std::out_of_range &e)
+      {
+        //CONSOLE_BRIDGE_logError("Ray vertical samples [%s] is out of range: %s", samples_char, e.what());
+        return false;
+      }
+    }
+
+    const char* resolution_char = vertical->Attribute("resolution");
+    if (resolution_char)
+    {
+      try {
+        ray.vertical_resolution = strToDouble(resolution_char);
+      } catch(std::runtime_error &) {
+        //CONSOLE_BRIDGE_logError("Ray vertical resolution [%s] is not a valid float", resolution_char);
+        return false;
+      }
+    }
+
+    const char* min_angle_char = vertical->Attribute("min_angle");
+    if (min_angle_char)
+    {
+      try {
+        ray.vertical_min_angle = strToDouble(min_angle_char);
+      } catch(std::runtime_error &) {
+        //CONSOLE_BRIDGE_logError("Ray vertical min_angle [%s] is not a valid float", min_angle_char);
+        return false;
+      }
+    }
+
+    const char* max_angle_char = vertical->Attribute("max_angle");
+    if (max_angle_char)
+    {
+      try {
+        ray.vertical_max_angle = strToDouble(max_angle_char);
+      } catch(std::runtime_error &) {
+        //CONSOLE_BRIDGE_logError("Ray vertical max_angle [%s] is not a valid float", max_angle_char);
+        return false;
+      }
+    }
+  }
+  return false;
+}
+
+VisualSensorSharedPtr parseVisualSensor(TiXmlElement *g)
+{
+  VisualSensorSharedPtr visual_sensor;
+
+  // get sensor type
+  TiXmlElement *sensor_xml;
+  if (g->FirstChildElement("camera"))
+  {
+    Camera *camera = new Camera();
+    visual_sensor.reset(camera);
+    sensor_xml = g->FirstChildElement("camera");
+    if (!parseCamera(*camera, sensor_xml))
+      visual_sensor.reset();
+  }
+  else if (g->FirstChildElement("ray"))
+  {
+    Ray *ray = new Ray();
+    visual_sensor.reset(ray);
+    sensor_xml = g->FirstChildElement("ray");
+    if (!parseRay(*ray, sensor_xml))
+      visual_sensor.reset();
+  }
+  else
+  {
+    //CONSOLE_BRIDGE_logError("No know sensor types [camera|ray] defined in <sensor> block");
+  }
+  return visual_sensor;
+}
+
+
+bool parseSensor(Sensor &sensor, TiXmlElement* config)
+{
+  sensor.clear();
+
+  const char *name_char = config->Attribute("name");
+  if (!name_char)
+  {
+    //CONSOLE_BRIDGE_logError("No name given for the sensor.");
+    return false;
+  }
+  sensor.name = std::string(name_char);
+
+  // parse parent_link_name
+  const char *parent_link_name_char = config->Attribute("parent_link_name");
+  if (!parent_link_name_char)
+  {
+    //CONSOLE_BRIDGE_logError("No parent_link_name given for the sensor.");
+    return false;
+  }
+  sensor.parent_link_name = std::string(parent_link_name_char);
+
+  // parse origin
+  TiXmlElement *o = config->FirstChildElement("origin");
+  if (o)
+  {
+    if (!parsePose(sensor.origin, o))
+      return false;
+  }
+
+  // parse sensor
+  sensor.sensor = parseVisualSensor(config);
+  return true;
+}
+
+
+}
+
+

src/kdl_parser/src/world.cpp

@@ -0,0 +1,70 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+* 
+*  Copyright (c) 2008, Willow Garage, Inc.
+*  All rights reserved.
+* 
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+* 
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the Willow Garage nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+* 
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/* Author: Wim Meeussen */
+
+
+#include <urdf_world/world.h>
+#include <urdf_model/model.h>
+#include <urdf_parser/urdf_parser.h>
+#include <fstream>
+#include <sstream>
+#include <algorithm>
+#include <tinyxml.h>
+//#include <console_bridge/console.h>
+
+namespace urdf{
+
+bool parseWorld(World &/*world*/, TiXmlElement* /*config*/)
+{
+
+  // to be implemented
+
+  return true;
+}
+
+bool exportWorld(World &world, TiXmlElement* xml)
+{
+  TiXmlElement * world_xml = new TiXmlElement("world");
+  world_xml->SetAttribute("name", world.name);
+
+  // to be implemented
+  // exportModels(*world.models, world_xml);
+
+  xml->LinkEndChild(world_xml);
+
+  return true;
+}
+
+}

src/main.cpp

@@ -0,0 +1,75 @@
+#include <iostream>
+#include "math_utils.h"
+#include "smart_json_wrapper.h"
+
+// #ifdef __EMSCRIPTEN__
+// #include <emscripten.h>
+// #endif
+
+// // WebAssembly 初始化函数
+// #ifdef __cplusplus
+// extern "C"
+// {
+// #endif
+
+//     EMSCRIPTEN_KEEPALIVE
+//     void initialize()
+//     {
+//         std::cout << "Smart WASM Math Library Initialized" << std::endl;
+//     }
+
+//     EMSCRIPTEN_KEEPALIVE
+//     int test_math_functions()
+//     {
+//         // 测试所有数学函数
+//         int result = 0;
+
+//         // 测试加法
+//         result = add(10, 20);
+//         std::cout << "add(10, 20) = " << result << std::endl;
+
+//         // 测试减法
+//         result = subtract(30, 15);
+//         std::cout << "subtract(30, 15) = " << result << std::endl;
+
+//         // 测试乘法
+//         float fresult = multiply(2.5f, 4.0f);
+//         std::cout << "multiply(2.5, 4.0) = " << fresult << std::endl;
+
+//         // 测试除法
+//         fresult = divide(10.0f, 2.0f);
+//         std::cout << "divide(10.0, 2.0) = " << fresult << std::endl;
+
+//         // 测试斐波那契
+//         result = fibonacci(10);
+//         std::cout << "fibonacci(10) = " << result << std::endl;
+
+//         // 测试问候语
+//         const char *greeting = get_greeting();
+//         std::cout << "Greeting: " << greeting << std::endl;
+
+//         return 0;
+//     }
+
+// #ifdef __cplusplus
+// }
+// #endif
+
+// 主函数 - 用于本地测试
+int main()
+{
+    std::cout << "=========================================" << std::endl;
+    std::cout << "     Smart WASM Math Library v1.0.0     " << std::endl;
+    std::cout << "=========================================" << std::endl;
+    std::cout << std::endl;
+
+    // 测试数学函数
+    // test_math_functions();
+
+    std::cout << std::endl;
+    std::cout << "=========================================" << std::endl;
+    std::cout << "  Build for WebAssembly with JSON API   " << std::endl;
+    std::cout << "=========================================" << std::endl;
+
+    return 0;
+}

src/math_utils.cpp

@@ -0,0 +1,89 @@
+#include "math_utils.h"
+#include <cmath>
+#include <cstdlib>
+#include <iostream>
+
+// 包含 emscripten 头文件
+#ifdef __EMSCRIPTEN__
+#include <emscripten.h>
+#else
+// 非 Emscripten 环境的替代定义
+#define EMSCRIPTEN_KEEPALIVE
+#endif
+
+// 所有函数都加上 EMSCRIPTEN_KEEPALIVE
+EMSCRIPTEN_KEEPALIVE
+int add(int a, int b)
+{
+    return a + b;
+}
+
+EMSCRIPTEN_KEEPALIVE
+int subtract(int a, int b)
+{
+    return a - b;
+}
+
+EMSCRIPTEN_KEEPALIVE
+float multiply(float a, float b)
+{
+    return a * b;
+}
+
+EMSCRIPTEN_KEEPALIVE
+float divide(float a, float b)
+{
+    if (b == 0.0f)
+    {
+        std::cerr << "Error: Division by zero" << std::endl;
+        return 0.0f;
+    }
+    return a / b;
+}
+
+EMSCRIPTEN_KEEPALIVE
+int fibonacci(int n)
+{
+    if (n <= 1)
+        return n;
+    return fibonacci(n - 1) + fibonacci(n - 2);
+}
+
+EMSCRIPTEN_KEEPALIVE
+void *create_buffer(int size)
+{
+    if (size <= 0)
+        return nullptr;
+    return malloc(size * sizeof(char));
+}
+
+EMSCRIPTEN_KEEPALIVE
+void destroy_buffer(void *p)
+{
+    if (p)
+    {
+        free(p);
+    }
+}
+
+EMSCRIPTEN_KEEPALIVE
+int compute_sum(int *arr, int size)
+{
+    if (arr == nullptr || size <= 0)
+    {
+        return 0;
+    }
+
+    int sum = 0;
+    for (int i = 0; i < size; i++)
+    {
+        sum += arr[i];
+    }
+    return sum;
+}
+
+EMSCRIPTEN_KEEPALIVE
+const char *get_greeting()
+{
+    return "Hello from Smart WebAssembly!";
+}

src/math_utils.h

@@ -0,0 +1,30 @@
+#ifndef MATH_UTILS_H
+#define MATH_UTILS_H
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+    // 基础数学运算
+    int add(int a, int b);
+    int subtract(int a, int b);
+    float multiply(float a, float b);
+    float divide(float a, float b);
+    int fibonacci(int n);
+
+    // 内存操作
+    void *create_buffer(int size);
+    void destroy_buffer(void *p);
+
+    // 数组运算
+    int compute_sum(int *arr, int size);
+
+    // 字符串操作
+    const char *get_greeting();
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // MATH_UTILS_H

src/smart_json_wrapper.cpp

@@ -0,0 +1,943 @@
+#include "smart_json_wrapper.h"
+#include "KinematicsWebAPI.h"
+#include <emscripten.h>
+#include <cmath>
+#include <chrono>
+#include <sstream>
+#include <iomanip>
+#include <regex>
+#include <algorithm>
+#include <memory>
+#include <iostream>
+#include <cstring>
+#include <cctype>
+
+// ========== 内存管理辅助函数 ==========
+namespace
+{
+    // 创建 WASM 内存中的字符串（使用 malloc 分配，与 JavaScript 的 _free 兼容）
+    char *createWasmString(const std::string &str)
+    {
+        if (str.empty())
+        {
+            char *buffer = (char *)malloc(1);
+            if (buffer)
+            {
+                buffer[0] = '\0';
+            }
+            return buffer;
+        }
+
+        size_t len = str.length();
+        char *buffer = (char *)malloc(len + 1); // +1 for null terminator
+
+        if (buffer)
+        {
+            // 使用 std::copy 复制字符串内容
+            std::copy(str.begin(), str.end(), buffer);
+            buffer[len] = '\0'; // 添加 null 终止符
+        }
+
+        return buffer;
+    }
+
+    // 安全释放 WASM 字符串（使用 free 释放，与 JavaScript 的 _free 兼容）
+    void freeWasmString(const char *str)
+    {
+        if (str)
+        {
+            free(const_cast<char *>(str));
+        }
+    }
+}
+
+// ========== SmartResponse 实现 ==========
+std::string SmartResponse::toJson() const
+{
+    std::stringstream ss;
+    ss << "{";
+    ss << "\"success\":" << (success ? "true" : "false") << ",";
+    ss << "\"code\":" << code << ",";
+    ss << "\"msg\":\"" << msg << "\",";
+    ss << "\"req_code\":\"" << req_code << "\",";
+    ss << "\"req_cmd\":\"" << req_cmd << "\",";
+    ss << "\"execution_time\":" << std::fixed << std::setprecision(3) << execution_time << ",";
+
+    // 处理结果数据
+    ss << "\"res_data\":";
+    if (res_data.has_value())
+    {
+        try
+        {
+            if (res_data.type() == typeid(int))
+            {
+                ss << std::any_cast<int>(res_data);
+            }
+            else if (res_data.type() == typeid(float))
+            {
+                ss << std::fixed << std::setprecision(3) << std::any_cast<float>(res_data);
+            }
+            else if (res_data.type() == typeid(double))
+            {
+                ss << std::fixed << std::setprecision(3) << std::any_cast<double>(res_data);
+            }
+            else if (res_data.type() == typeid(bool))
+            {
+                ss << (std::any_cast<bool>(res_data) ? "true" : "false");
+            }
+            else if (res_data.type() == typeid(std::string))
+            {
+                ss << "\"" << std::any_cast<std::string>(res_data) << "\"";
+            }
+            else if (res_data.type() == typeid(std::vector<int>))
+            {
+                auto arr = std::any_cast<std::vector<int>>(res_data);
+                ss << "[";
+                for (size_t i = 0; i < arr.size(); ++i)
+                {
+                    if (i > 0)
+                        ss << ",";
+                    ss << arr[i];
+                }
+                ss << "]";
+            }
+            else
+            {
+                // 默认处理为字符串
+                ss << "\"complex_data\"";
+            }
+        }
+        catch (const std::bad_any_cast &)
+        {
+            ss << "null";
+        }
+    }
+    else
+    {
+        ss << "null";
+    }
+
+    // 执行信息
+    if (!execution_info.empty())
+    {
+        ss << ",\"execution_info\":{";
+        bool first = true;
+        for (const auto &[key, value] : execution_info)
+        {
+            if (!first)
+                ss << ",";
+            ss << "\"" << key << "\":\"" << value << "\"";
+            first = false;
+        }
+        ss << "}";
+    }
+
+    ss << "}";
+    return ss.str();
+}
+
+SmartResponse SmartResponse::createSuccess(const std::string &req_code,
+                                           const std::string &req_cmd,
+                                           const std::any &data)
+{
+    SmartResponse resp;
+    resp.success = true;
+    resp.code = 0;
+    resp.msg = "操作成功";
+    resp.req_code = req_code;
+    resp.req_cmd = req_cmd;
+    resp.res_data = data;
+    return resp;
+}
+
+SmartResponse SmartResponse::createError(const std::string &req_code,
+                                         const std::string &req_cmd,
+                                         int code,
+                                         const std::string &msg)
+{
+    SmartResponse resp;
+    resp.success = false;
+    resp.code = code;
+    resp.msg = msg;
+    resp.req_code = req_code;
+    resp.req_cmd = req_cmd;
+    return resp;
+}
+
+// ========== 字符串工具函数 ==========
+std::string SmartJsonProcessor::trim(const std::string &str)
+{
+    size_t first = str.find_first_not_of(" \t\n\r");
+    if (first == std::string::npos)
+        return "";
+    size_t last = str.find_last_not_of(" \t\n\r");
+    return str.substr(first, (last - first + 1));
+}
+
+std::vector<std::string> SmartJsonProcessor::split(const std::string &str, char delimiter)
+{
+    std::vector<std::string> tokens;
+    std::stringstream ss(str);
+    std::string token;
+    while (std::getline(ss, token, delimiter))
+    {
+        token = trim(token);
+        if (!token.empty())
+        {
+            tokens.push_back(token);
+        }
+    }
+    return tokens;
+}
+
+// ========== SmartJsonProcessor 实现 ==========
+SmartJsonProcessor::SmartJsonProcessor()
+{
+    registerMathFunctions();
+}
+
+SmartJsonProcessor::~SmartJsonProcessor()
+{
+    // 清理资源
+}
+
+// ========== JSON 解析 ==========
+std::map<std::string, std::any> SmartJsonProcessor::parseJsonParams(const std::string &json_str)
+{
+    std::map<std::string, std::any> params;
+
+    // 简化JSON解析，解析键值对
+    std::regex param_regex("\"([^\"]+)\"\\s*:\\s*([^,}\\s\"]+|\"[^\"]*\")");
+    std::sregex_iterator it(json_str.begin(), json_str.end(), param_regex);
+    std::sregex_iterator end;
+
+    while (it != end)
+    {
+        std::smatch match = *it;
+        std::string key = match[1].str();
+        std::string value = match[2].str();
+
+        // 移除引号
+        if (!value.empty() && value[0] == '"' && value.back() == '"')
+        {
+            value = value.substr(1, value.length() - 2);
+        }
+
+        params[key] = inferJsonValue(key, value);
+        ++it;
+    }
+
+    // 特别处理数组参数
+    std::regex array_regex("\"([^\"]+)\"\\s*:\\s*\\[([^\\]]*)\\]");
+    it = std::sregex_iterator(json_str.begin(), json_str.end(), array_regex);
+
+    while (it != end)
+    {
+        std::smatch match = *it;
+        std::string key = match[1].str();
+        std::string array_str = match[2].str();
+
+        // 尝试解析为整数数组
+        auto int_array = parseIntArray(array_str);
+        if (!int_array.empty())
+        {
+            params[key] = int_array;
+        }
+        else
+        {
+            // 尝试解析为浮点数数组
+            auto float_array = parseFloatArray(array_str);
+            if (!float_array.empty())
+            {
+                params[key] = float_array;
+            }
+        }
+        ++it;
+    }
+
+    return params;
+}
+
+std::any SmartJsonProcessor::inferJsonValue(const std::string &key, const std::string &value_str)
+{
+    if (value_str.empty())
+    {
+        return std::string();
+    }
+
+    // 根据键名猜测类型
+    std::string lower_key = key;
+    // std::transform(lower_key.begin(), lower_key.end(), lower_key.begin(), ::towlower);
+    // std::transform(lower_key.begin(), lower_key.end(), lower_key.begin(), std::tolower);
+
+    // 布尔值
+    if (value_str == "true" || value_str == "false")
+    {
+        return value_str == "true";
+    }
+
+    // 尝试解析为数字
+    try
+    {
+        // 检查是否是整数
+        // if (value_str.find('.') == std::string::npos && value_str.find('e') == std::string::npos)
+        // {
+        //     return std::stoi(value_str);
+        // }
+        // else
+        // {
+        //     return std::stof(value_str);
+        // }
+    }
+    catch (...)
+    {
+        // 不是数字，保持字符串
+    }
+
+    return value_str;
+}
+
+std::vector<int> SmartJsonProcessor::parseIntArray(const std::string &array_str)
+{
+    std::vector<int> result;
+    auto tokens = split(array_str, ',');
+
+    for (const auto &token : tokens)
+    {
+        try
+        {
+            result.push_back(std::stoi(trim(token)));
+        }
+        catch (...)
+        {
+            // 转换失败，清空数组并返回
+            result.clear();
+            break;
+        }
+    }
+
+    return result;
+}
+
+std::vector<float> SmartJsonProcessor::parseFloatArray(const std::string &array_str)
+{
+    std::vector<float> result;
+    auto tokens = split(array_str, ',');
+
+    for (const auto &token : tokens)
+    {
+        try
+        {
+            result.push_back(std::stof(trim(token)));
+        }
+        catch (...)
+        {
+            // 转换失败，清空数组并返回
+            result.clear();
+            break;
+        }
+    }
+
+    return result;
+}
+
+// ========== 数学函数注册 ==========
+void SmartJsonProcessor::registerMathFunctions()
+{
+    auto &registry = FunctionRegistry::instance();
+
+    // 1. add 函数
+    auto add_func = std::make_shared<FunctionInfo>("add", "整数加法");
+    add_func->addParam("a", ParamType::INT, 0)
+        .addParam("b", ParamType::INT, 0)
+        .addAlias("a", {"num1", "first", "x", "operand1", "left"})
+        .addAlias("b", {"num2", "second", "y", "operand2", "right"})
+        .setHandler([](const std::vector<std::any> &args) -> std::any
+                    {
+                if (args.size() >= 2) {
+                    int a = 0, b = 0;
+                    try {
+                        a = std::any_cast<int>(args[0]);
+                        b = std::any_cast<int>(args[1]);
+                    } catch (...) {
+                        // 尝试转换
+                        if (args[0].type() == typeid(float)) a = static_cast<int>(std::any_cast<float>(args[0]));
+                        if (args[1].type() == typeid(float)) b = static_cast<int>(std::any_cast<float>(args[1]));
+                    }
+                    return add(a, b);
+                }
+                return 0; });
+    registry.registerFunction(add_func);
+
+    // 2. subtract 函数
+    auto sub_func = std::make_shared<FunctionInfo>("subtract", "整数减法");
+    sub_func->addParam("a", ParamType::INT, 0)
+        .addParam("b", ParamType::INT, 0)
+        .addAlias("a", {"num1", "minuend", "x"})
+        .addAlias("b", {"num2", "subtrahend", "y"})
+        .setHandler([](const std::vector<std::any> &args) -> std::any
+                    {
+                if (args.size() >= 2) {
+                    int a = 0, b = 0;
+                    try {
+                        a = std::any_cast<int>(args[0]);
+                        b = std::any_cast<int>(args[1]);
+                    } catch (...) {
+                        if (args[0].type() == typeid(float)) a = static_cast<int>(std::any_cast<float>(args[0]));
+                        if (args[1].type() == typeid(float)) b = static_cast<int>(std::any_cast<float>(args[1]));
+                    }
+                    return subtract(a, b);
+                }
+                return 0; });
+    registry.registerFunction(sub_func);
+
+    // 3. multiply 函数
+    auto mul_func = std::make_shared<FunctionInfo>("multiply", "乘法运算");
+    mul_func->addParam("a", ParamType::FLOAT, 0.0f)
+        .addParam("b", ParamType::FLOAT, 0.0f)
+        .addAlias("a", {"num1", "factor1", "x", "multiplicand"})
+        .addAlias("b", {"num2", "factor2", "y", "multiplier"})
+        .setHandler([](const std::vector<std::any> &args) -> std::any
+                    {
+                if (args.size() >= 2) {
+                    float a = 0.0f, b = 0.0f;
+                    try {
+                        if (args[0].type() == typeid(float)) a = std::any_cast<float>(args[0]);
+                        else if (args[0].type() == typeid(int)) a = static_cast<float>(std::any_cast<int>(args[0]));
+                        if (args[1].type() == typeid(float)) b = std::any_cast<float>(args[1]);
+                        else if (args[1].type() == typeid(int)) b = static_cast<float>(std::any_cast<int>(args[1]));
+                    } catch (...) {
+                        // 使用默认值
+                    }
+                    return multiply(a, b);
+                }
+                return 0.0f; });
+    registry.registerFunction(mul_func);
+
+    // 4. divide 函数
+    auto div_func = std::make_shared<FunctionInfo>("divide", "除法运算");
+    div_func->addParam("a", ParamType::FLOAT, 0.0f)
+        .addParam("b", ParamType::FLOAT, 1.0f)
+        .addAlias("a", {"numerator", "dividend", "x"})
+        .addAlias("b", {"denominator", "divisor", "y"})
+        .setHandler([](const std::vector<std::any> &args) -> std::any
+                    {
+                if (args.size() >= 2) {
+                    float a = 0.0f, b = 1.0f;
+                    try {
+                        if (args[0].type() == typeid(float)) a = std::any_cast<float>(args[0]);
+                        else if (args[0].type() == typeid(int)) a = static_cast<float>(std::any_cast<int>(args[0]));
+                        if (args[1].type() == typeid(float)) b = std::any_cast<float>(args[1]);
+                        else if (args[1].type() == typeid(int)) b = static_cast<float>(std::any_cast<int>(args[1]));
+                    } catch (...) {
+                        // 使用默认值
+                    }
+                    if (b == 0.0f) {
+                        throw std::runtime_error("除数不能为零");
+                    }
+                    return divide(a, b);
+                }
+                return 0.0f; });
+    registry.registerFunction(div_func);
+
+    // 5. fibonacci 函数
+    auto fib_func = std::make_shared<FunctionInfo>("fibonacci", "斐波那契数列");
+    fib_func->addParam("n", ParamType::INT, 0)
+        .addAlias("n", {"number", "index", "term", "position"})
+        .setHandler([](const std::vector<std::any> &args) -> std::any
+                    {
+                if (!args.empty()) {
+                    int n = 0;
+                    try {
+                        n = std::any_cast<int>(args[0]);
+                    } catch (...) {
+                        if (args[0].type() == typeid(float)) n = static_cast<int>(std::any_cast<float>(args[0]));
+                    }
+                    if (n < 0) n = 0;
+                    if (n > 40) n = 40; // 限制大小避免性能问题
+                    return fibonacci(n);
+                }
+                return 0; });
+    registry.registerFunction(fib_func);
+
+    // 6. create_buffer 函数
+    auto create_buf_func = std::make_shared<FunctionInfo>("create_buffer", "创建缓冲区");
+    create_buf_func->addParam("size", ParamType::INT, 1024)
+        .addAlias("size", {"length", "capacity", "bytes"})
+        .setHandler([](const std::vector<std::any> &args) -> std::any
+                    {
+                    if (!args.empty()) {
+                        int size = 1024;
+                        try {
+                            size = std::any_cast<int>(args[0]);
+                        } catch (...) {
+                            if (args[0].type() == typeid(float)) size = static_cast<int>(std::any_cast<float>(args[0]));
+                        }
+                        if (size <= 0) size = 1024;
+                        if (size > 1024 * 1024) size = 1024 * 1024; // 限制1MB
+                        void* buffer = create_buffer(size);
+                        std::stringstream ss;
+                        ss << "0x" << std::hex << reinterpret_cast<uintptr_t>(buffer);
+                        return ss.str();
+                    }
+                    return "null"; });
+    registry.registerFunction(create_buf_func);
+
+    // 7. compute_sum 函数
+    auto sum_func = std::make_shared<FunctionInfo>("compute_sum", "数组求和");
+    sum_func->addParam("array", ParamType::INT_ARRAY, std::vector<int>())
+        .addAlias("array", {"arr", "list", "values", "numbers"})
+        .setHandler([](const std::vector<std::any> &args) -> std::any
+                    {
+                if (!args.empty()) {
+                    try {
+                        auto arr = std::any_cast<std::vector<int>>(args[0]);
+                        if (arr.empty()) return 0;
+                        return compute_sum(arr.data(), static_cast<int>(arr.size()));
+                    } catch (...) {
+                        return 0;
+                    }
+                }
+                return 0; });
+    registry.registerFunction(sum_func);
+
+    // 8. get_greeting 函数
+    auto greet_func = std::make_shared<FunctionInfo>("get_greeting", "获取问候语");
+    greet_func->setHandler([](const std::vector<std::any> &args) -> std::any
+                           {
+        const char* greeting = get_greeting();
+        return std::string(greeting); });
+    registry.registerFunction(greet_func);
+}
+
+// ========== 主处理函数 ==========
+SmartResponse SmartJsonProcessor::processRequest(const std::string &json_request)
+{
+    auto start_time = std::chrono::high_resolution_clock::now();
+
+    try
+    {
+        // 1. 解析基础信息
+        std::string req_code = "REQ_" + std::to_string(std::chrono::system_clock::now().time_since_epoch().count());
+        std::string req_cmd = "unknown";
+
+        // 简化解析req_cmd
+        std::regex cmd_regex("\"req_cmd\"\\s*:\\s*\"([^\"]+)\"");
+        std::smatch match;
+        if (std::regex_search(json_request, match, cmd_regex))
+        {
+            req_cmd = match[1].str();
+        }
+
+        // 2. 获取函数信息
+        auto &registry = FunctionRegistry::instance();
+        auto func_info = registry.getFunction(req_cmd);
+
+        if (!func_info)
+        {
+            return SmartResponse::createError(req_code, req_cmd, 1001,
+                                              "未找到函数: " + req_cmd);
+        }
+
+        // 3. 解析参数
+        auto input_params = parseJsonParams(json_request);
+
+        // 4. 智能参数匹配
+        auto matched_params = registry.smartMatchParams(func_info, input_params);
+
+        // 5. 验证参数
+        std::string error_msg;
+        if (!func_info->validateParams(matched_params, error_msg))
+        {
+            return SmartResponse::createError(req_code, req_cmd, 1002, error_msg);
+        }
+
+        // 6. 准备调用参数
+        std::vector<std::any> call_args;
+        for (const auto &param : func_info->params)
+        {
+            auto it = matched_params.find(param.name);
+            if (it != matched_params.end())
+            {
+                call_args.push_back(it->second);
+            }
+            else if (param.default_value.has_value())
+            {
+                call_args.push_back(param.default_value);
+            }
+            else
+            {
+                call_args.push_back(std::any()); // 空值
+            }
+        }
+
+        // 7. 执行函数
+        std::any result;
+        try
+        {
+            result = func_info->handler(call_args);
+        }
+        catch (const std::exception &e)
+        {
+            return SmartResponse::createError(req_code, req_cmd, 3001,
+                                              std::string("执行错误: ") + e.what());
+        }
+
+        // 8. 构建响应
+        auto response = SmartResponse::createSuccess(req_code, req_cmd, result);
+
+        // 9. 添加执行信息
+        response.execution_info["matched_params"] = std::to_string(matched_params.size());
+        response.execution_info["function"] = func_info->name;
+
+        // 10. 计算执行时间
+        auto end_time = std::chrono::high_resolution_clock::now();
+        auto duration = std::chrono::duration_cast<std::chrono::microseconds>(
+            end_time - start_time);
+        response.execution_time = duration.count() / 1000.0;
+
+        return response;
+    }
+    catch (const std::exception &e)
+    {
+        return SmartResponse::createError("ERROR", "unknown", 5001,
+                                          std::string("处理异常: ") + e.what());
+    }
+}
+
+std::map<std::string, std::shared_ptr<FunctionInfo>>
+SmartJsonProcessor::getAvailableFunctions() const
+{
+    auto &registry = FunctionRegistry::instance();
+    return registry.getAllFunctions();
+}
+
+std::string SmartJsonProcessor::getFunctionInfo(const std::string &func_name) const
+{
+    auto &registry = FunctionRegistry::instance();
+    auto func_info = registry.getFunction(func_name);
+
+    if (!func_info)
+    {
+        return "函数未找到: " + func_name;
+    }
+
+    std::stringstream ss;
+    ss << "函数: " << func_info->name << "\n";
+    ss << "描述: " << func_info->description << "\n";
+    ss << "参数:\n";
+
+    for (const auto &param : func_info->params)
+    {
+        ss << "  - " << param.name << " (" << param.getTypeName() << ")";
+        if (param.is_optional)
+            ss << " [可选]";
+        if (param.default_value.has_value())
+        {
+            ss << " [默认: ";
+            try
+            {
+                if (param.type == ParamType::INT)
+                    ss << param.convert<int>(param.default_value);
+                else if (param.type == ParamType::FLOAT)
+                    ss << param.convert<float>(param.default_value);
+                else if (param.type == ParamType::STRING)
+                    ss << param.convert<std::string>(param.default_value);
+            }
+            catch (...)
+            {
+                ss << "unknown";
+            }
+            ss << "]";
+        }
+        ss << "\n";
+    }
+
+    return ss.str();
+}
+
+// ========== WASM 导出函数 ==========
+static SmartJsonProcessor *g_smart_processor = nullptr;
+// 信号处理
+static KinematicsWebAPI *global_server = nullptr;
+
+EMSCRIPTEN_KEEPALIVE
+const char *init_func()
+{
+    try
+    {
+        if (!global_server)
+        {
+            global_server = new KinematicsWebAPI();
+        }
+        if (!global_server)
+        {
+            auto error = SmartResponse::createError("ERROR", "unknown", 1001, "init_func 请求数据为空");
+            return createWasmString(error.toJson());
+        }
+
+        auto response = SmartResponse::createSuccess("req_code", "init_func");
+
+        return createWasmString(response.toJson());
+    }
+    catch (const std::exception &e)
+    {
+        auto error = SmartResponse::createError("ERROR", "unknown", 5001,
+                                                std::string("处理异常: ") + e.what());
+        return createWasmString(error.toJson());
+    }
+}
+
+EMSCRIPTEN_KEEPALIVE
+const char *func(const char *json_request)
+{
+    if (!global_server)
+    {
+        // 注意：这里需要动态分配，否则作用域结束会被销毁
+        global_server = new KinematicsWebAPI();
+    }
+
+    try
+    {
+        if (!json_request)
+        {
+            auto error = SmartResponse::createError("ERROR", "unknown", 1001, "请求数据为空");
+            return createWasmString(error.toJson());
+        }
+
+        std::string request_str(json_request);
+
+        std::string response_str = global_server->func(request_str);
+
+        return createWasmString(response_str);
+    }
+    catch (const std::exception &e)
+    {
+        auto error = SmartResponse::createError("ERROR", "unknown", 5001,
+                                                std::string("处理异常: ") + e.what());
+        return createWasmString(error.toJson());
+    }
+}
+
+EMSCRIPTEN_KEEPALIVE
+const char *smart_process_json(const char *json_request)
+{
+    if (!g_smart_processor)
+    {
+        g_smart_processor = new SmartJsonProcessor();
+    }
+
+    try
+    {
+        if (!json_request)
+        {
+            auto error = SmartResponse::createError("ERROR", "unknown", 1001, "请求数据为空");
+            return createWasmString(error.toJson());
+        }
+
+        std::string request_str(json_request);
+        auto response = g_smart_processor->processRequest(request_str);
+
+        return createWasmString(response.toJson());
+    }
+    catch (const std::exception &e)
+    {
+        auto error = SmartResponse::createError("ERROR", "unknown", 5001,
+                                                std::string("处理异常: ") + e.what());
+        return createWasmString(error.toJson());
+    }
+}
+
+EMSCRIPTEN_KEEPALIVE
+const char *smart_get_function_list()
+{
+    if (!g_smart_processor)
+    {
+        g_smart_processor = new SmartJsonProcessor();
+    }
+
+    try
+    {
+        auto functions = g_smart_processor->getAvailableFunctions();
+
+        std::stringstream ss;
+        ss << "[";
+        bool first = true;
+        for (const auto &[name, func_info] : functions)
+        {
+            if (!first)
+                ss << ",";
+            ss << "{";
+            ss << "\"name\":\"" << name << "\",";
+            ss << "\"description\":\"" << func_info->description << "\",";
+            ss << "\"params\":[";
+            bool first_param = true;
+            for (const auto &param : func_info->params)
+            {
+                if (!first_param)
+                    ss << ",";
+                ss << "{";
+                ss << "\"name\":\"" << param.name << "\",";
+                ss << "\"type\":\"" << param.getTypeName() << "\",";
+                ss << "\"optional\":" << (param.is_optional ? "true" : "false");
+                ss << "}";
+                first_param = false;
+            }
+            ss << "]";
+            ss << "}";
+            first = false;
+        }
+        ss << "]";
+
+        std::string result_str = ss.str();
+        return createWasmString(result_str);
+    }
+    catch (const std::exception &e)
+    {
+        std::string error = "{\"error\":\"" + std::string(e.what()) + "\"}";
+        return createWasmString(error);
+    }
+}
+
+EMSCRIPTEN_KEEPALIVE
+const char *smart_get_function_info(const char *func_name)
+{
+    if (!g_smart_processor)
+    {
+        g_smart_processor = new SmartJsonProcessor();
+    }
+
+    try
+    {
+        if (!func_name)
+        {
+            std::string error = "{\"error\":\"函数名不能为空\"}";
+            return createWasmString(error);
+        }
+
+        std::string info = g_smart_processor->getFunctionInfo(func_name);
+        return createWasmString(info);
+    }
+    catch (const std::exception &e)
+    {
+        std::string error = "{\"error\":\"" + std::string(e.what()) + "\"}";
+        return createWasmString(error);
+    }
+}
+
+EMSCRIPTEN_KEEPALIVE
+const char *smart_test_match(const char *json_request)
+{
+    if (!g_smart_processor)
+    {
+        g_smart_processor = new SmartJsonProcessor();
+    }
+
+    try
+    {
+        if (!json_request)
+        {
+            auto error = SmartResponse::createError("TEST", "test", 1001, "请求数据为空");
+            return createWasmString(error.toJson());
+        }
+
+        // 演示智能匹配过程
+        std::string request_str(json_request);
+
+        // 解析请求
+        std::regex cmd_regex("\"req_cmd\"\\s*:\\s*\"([^\"]+)\"");
+        std::smatch match;
+        std::string req_cmd = "unknown";
+        if (std::regex_search(request_str, match, cmd_regex))
+        {
+            req_cmd = match[1].str();
+        }
+
+        auto &registry = FunctionRegistry::instance();
+        auto func_info = registry.getFunction(req_cmd);
+
+        if (!func_info)
+        {
+            auto error = SmartResponse::createError("TEST", req_cmd, 1001,
+                                                    "未找到函数: " + req_cmd);
+            error.execution_info["test_type"] = "function_not_found";
+            return createWasmString(error.toJson());
+        }
+
+        // 解析参数
+        SmartJsonProcessor processor;
+        auto input_params = processor.parseJsonParams(request_str);
+
+        // 智能匹配
+        auto matched_params = registry.smartMatchParams(func_info, input_params);
+
+        // 显示匹配结果
+        std::stringstream ss;
+        ss << "{\"test\":\"smart_match\",";
+        ss << "\"function\":\"" << func_info->name << "\",";
+        ss << "\"description\":\"" << func_info->description << "\",";
+
+        ss << "\"input_params\":[";
+        bool first = true;
+        for (const auto &[key, value] : input_params)
+        {
+            if (!first)
+                ss << ",";
+            ss << "{\"key\":\"" << key << "\",";
+            ss << "\"type\":\"" << static_cast<int>(registry.deduceParamType(value)) << "\"}";
+            first = false;
+        }
+        ss << "],";
+
+        ss << "\"matched_params\":[";
+        first = true;
+        for (const auto &[key, value] : matched_params)
+        {
+            if (!first)
+                ss << ",";
+            ss << "{\"key\":\"" << key << "\",";
+            ss << "\"type\":\"" << static_cast<int>(registry.deduceParamType(value)) << "\"}";
+            first = false;
+        }
+        ss << "]";
+
+        ss << "}";
+
+        std::string result_str = ss.str();
+        return createWasmString(result_str);
+    }
+    catch (const std::exception &e)
+    {
+        std::string error = "{\"error\":\"" + std::string(e.what()) + "\"}";
+        return createWasmString(error);
+    }
+}
+
+EMSCRIPTEN_KEEPALIVE
+const char *smart_get_version()
+{
+    std::string version = "{\"version\":\"1.0.0\",\"name\":\"Smart WASM JSON API\"}";
+    return createWasmString(version);
+}
+
+EMSCRIPTEN_KEEPALIVE
+void smart_free_string(const char *str)
+{
+    freeWasmString(str);
+}
+
+// 导出额外的辅助函数
+EMSCRIPTEN_KEEPALIVE
+void *wasm_malloc(size_t size)
+{
+    return malloc(size);
+}
+
+EMSCRIPTEN_KEEPALIVE
+void wasm_free(void *ptr)
+{
+    if (ptr)
+    {
+        free(ptr);
+    }
+}

src/smart_json_wrapper.h

@@ -0,0 +1,100 @@
+#ifndef SMART_JSON_WRAPPER_H
+#define SMART_JSON_WRAPPER_H
+
+#include "function_metadata.h"
+#include "math_utils.h"
+#include <string>
+#include <map>
+#include <any>
+#include <chrono>
+#include <sstream>
+#include <regex>
+#include <cstring>
+
+// JSON 响应结构
+struct SmartResponse
+{
+    bool success = true;
+    int code = 0;
+    std::string msg;
+    std::string req_code;
+    std::string req_cmd;
+    std::any res_data;
+    double execution_time = 0.0;
+    std::map<std::string, std::string> execution_info;
+
+    // 转换为 JSON 字符串
+    std::string toJson() const;
+
+    // 创建成功响应
+    static SmartResponse createSuccess(const std::string &req_code,
+                                       const std::string &req_cmd,
+                                       const std::any &data = {});
+
+    // 创建错误响应
+    static SmartResponse createError(const std::string &req_code,
+                                     const std::string &req_cmd,
+                                     int code,
+                                     const std::string &msg);
+};
+
+// 智能 JSON 处理器
+class SmartJsonProcessor
+{
+private:
+    // 注册所有数学函数
+    void registerMathFunctions();
+
+    // 智能类型推断
+    std::any inferJsonValue(const std::string &key, const std::string &value_str);
+
+    // 解析数组
+    std::vector<int> parseIntArray(const std::string &array_str);
+    std::vector<float> parseFloatArray(const std::string &array_str);
+
+    // 字符串处理
+    static std::string trim(const std::string &str);
+    static std::vector<std::string> split(const std::string &str, char delimiter);
+
+public:
+    SmartJsonProcessor();
+    ~SmartJsonProcessor();
+
+    // JSON 解析
+    std::map<std::string, std::any> parseJsonParams(const std::string &json_str);
+    // 处理 JSON 请求
+    SmartResponse processRequest(const std::string &json_request);
+
+    // 获取函数列表
+    std::map<std::string, std::shared_ptr<FunctionInfo>> getAvailableFunctions() const;
+
+    // 获取函数信息
+    std::string getFunctionInfo(const std::string &func_name) const;
+};
+
+// WASM 导出函数
+extern "C"
+{
+    const char *init_func();
+    // 智能处理 JSON 请求
+    const char *func(const char *json_request);
+    // 智能处理 JSON 请求
+    const char *smart_process_json(const char *json_request);
+
+    // 获取可用函数列表
+    const char *smart_get_function_list();
+
+    // 获取函数详情
+    const char *smart_get_function_info(const char *func_name);
+
+    // 测试智能匹配
+    const char *smart_test_match(const char *json_request);
+
+    // 释放字符串
+    void smart_free_string(const char *str);
+
+    // 获取版本信息
+    const char *smart_get_version();
+}
+
+#endif // SMART_JSON_WRAPPER_H

src/utils.cpp

@@ -0,0 +1,461 @@
+#include "utils.h"
+#include <iostream>
+#include <iomanip>
+#include <sstream>
+#include <chrono>
+#include <ctime>
+#include <fstream>
+#include <algorithm>
+#include <stdexcept>
+
+#include <stdexcept>
+
+// 实现 splitByDelimiter 方法
+std::pair<std::string, std::string> StringUtils::splitByDelimiter(
+    const std::string &str,
+    char delimiter)
+{
+    size_t pos = str.find(delimiter);
+
+    if (pos != std::string::npos)
+    {
+        // 找到分隔符，拆分字符串
+        std::string first = str.substr(0, pos);
+        std::string second = str.substr(pos + 1);
+        return std::make_pair(first, second);
+    }
+
+    // 未找到分隔符，返回原始字符串和空字符串
+    return std::make_pair(str, "");
+}
+
+// 实现 splitA_B 方法
+std::pair<std::string, std::string> StringUtils::splitA_B(const std::string &str)
+{
+    return splitByDelimiter(str, '_');
+}
+
+// 实现 splitToTwoParts 方法
+bool StringUtils::splitToTwoParts(
+    const std::string &str,
+    std::string &part1,
+    std::string &part2,
+    char delimiter)
+{
+    size_t pos = str.find(delimiter);
+
+    if (pos != std::string::npos)
+    {
+        part1 = str.substr(0, pos);
+        part2 = str.substr(pos + 1);
+        return true; // 成功拆分
+    }
+
+    // 未找到分隔符
+    part1 = str;
+    part2 = "";
+    return false; // 拆分失败
+}
+
+// 实现 splitStrict 方法
+std::pair<std::string, std::string> StringUtils::splitStrict(
+    const std::string &str,
+    char delimiter)
+{
+    size_t pos = str.find(delimiter);
+
+    // 检查是否找到分隔符
+    if (pos == std::string::npos)
+    {
+        throw std::invalid_argument("Delimiter '" + std::string(1, delimiter) +
+                                    "' not found in string: \"" + str + "\"");
+    }
+
+    // 检查是否有多于一个分隔符
+    size_t nextPos = str.find(delimiter, pos + 1);
+    if (nextPos != std::string::npos)
+    {
+        throw std::invalid_argument("Multiple delimiters '" + std::string(1, delimiter) +
+                                    "' found in string: \"" + str + "\"");
+    }
+
+    std::string first = str.substr(0, pos);
+    std::string second = str.substr(pos + 1);
+
+    return std::make_pair(first, second);
+}
+
+namespace utils
+{
+
+    // Base64<36>ַ<EFBFBD><D6B7><EFBFBD>
+    const std::string base64_chars =
+        "ABCDEFGHIJKLMNOPQRSTUVWXYZ"
+        "abcdefghijklmnopqrstuvwxyz"
+        "0123456789+/";
+
+    // <20><><EFBFBD><EFBFBD>Ƿ<EFBFBD>Ϊ<EFBFBD><CEAA>Ч<EFBFBD><D0A7>base64<36>ַ<EFBFBD>
+    static bool is_base64(unsigned char c)
+    {
+        return (isalnum(c) || (c == '+') || (c == '/'));
+    }
+
+    std::string get_current_time()
+    {
+        auto now = std::chrono::system_clock::now();
+        auto in_time_t = std::chrono::system_clock::to_time_t(now);
+        auto ms = std::chrono::duration_cast<std::chrono::milliseconds>(
+                      now.time_since_epoch()) %
+                  1000;
+
+        std::stringstream ss;
+        ss << std::put_time(std::gmtime(&in_time_t), "%Y-%m-%dT%H:%M:%S");
+        ss << "." << std::setfill('0') << std::setw(3) << ms.count() << "Z";
+        return ss.str();
+    }
+
+    std::string get_current_timestamp()
+    {
+        auto now = std::chrono::system_clock::now();
+        auto in_time_t = std::chrono::system_clock::to_time_t(now);
+        auto milliseconds = std::chrono::duration_cast<std::chrono::milliseconds>(
+                                now.time_since_epoch()) %
+                            1000;
+
+        std::stringstream ss;
+        ss << std::put_time(std::localtime(&in_time_t), "%Y-%m-%d %H:%M:%S");
+        ss << '.' << std::setfill('0') << std::setw(3) << milliseconds.count();
+        return ss.str();
+    }
+
+    // void setup_cors_headers(httplib::Response &res)
+    // {
+    //     res.set_header("Access-Control-Allow-Origin", "*");
+    //     res.set_header("Access-Control-Allow-Methods", "GET, POST, PUT, DELETE, OPTIONS, PATCH");
+    //     res.set_header("Access-Control-Allow-Headers", "Content-Type, Authorization, X-Requested-With, X-API-Key, Accept, Origin");
+    //     res.set_header("Access-Control-Expose-Headers", "Content-Length, Content-Type, X-Request-Id");
+    //     res.set_header("Access-Control-Max-Age", "86400"); // 24Сʱ
+    //     res.set_header("Vary", "Origin");
+    // }
+
+    json create_error_response(int code, const std::string &message, const std::string &details)
+    {
+        json response = {
+            {"success", false},
+            {"code", code},
+            {"message", message},
+            {"timestamp", get_current_time()}};
+
+        if (!details.empty())
+        {
+            response["details"] = details;
+        }
+
+        return response;
+    }
+
+    json create_success_response(const json &data, const std::string &message)
+    {
+        return {
+            {"success", true},
+            {"code", 0},
+            {"message", message},
+            {"data", data},
+            {"timestamp", get_current_time()}};
+    }
+
+    bool is_valid_utf8(const std::string &str)
+    {
+        for (size_t i = 0; i < str.size(); ++i)
+        {
+            unsigned char c = str[i];
+            if (c <= 0x7F)
+            {
+                continue; // ASCII<49>ַ<EFBFBD>
+            }
+            else if ((c & 0xE0) == 0xC0)
+            {
+                // 2<>ֽ<EFBFBD>UTF-8
+                if (i + 1 >= str.size() || (str[i + 1] & 0xC0) != 0x80)
+                {
+                    return false;
+                }
+                i += 1;
+            }
+            else if ((c & 0xF0) == 0xE0)
+            {
+                // 3<>ֽ<EFBFBD>UTF-8
+                if (i + 2 >= str.size() || (str[i + 1] & 0xC0) != 0x80 || (str[i + 2] & 0xC0) != 0x80)
+                {
+                    return false;
+                }
+                i += 2;
+            }
+            else if ((c & 0xF8) == 0xF0)
+            {
+                // 4<>ֽ<EFBFBD>UTF-8
+                if (i + 3 >= str.size() || (str[i + 1] & 0xC0) != 0x80 ||
+                    (str[i + 2] & 0xC0) != 0x80 || (str[i + 3] & 0xC0) != 0x80)
+                {
+                    return false;
+                }
+                i += 3;
+            }
+            else
+            {
+                return false; // <20><>Ч<EFBFBD><D0A7>UTF-8<>ֽ<EFBFBD>
+            }
+        }
+        return true;
+    }
+
+    std::string sanitize_utf8(const std::string &str)
+    {
+        std::string result;
+        result.reserve(str.size());
+
+        for (size_t i = 0; i < str.size(); ++i)
+        {
+            unsigned char c = str[i];
+            if (c <= 0x7F)
+            {
+                result += c; // ASCII<49>ַ<EFBFBD>
+            }
+            else if ((c & 0xE0) == 0xC0)
+            {
+                // 2<>ֽ<EFBFBD>UTF-8
+                if (i + 1 < str.size() && (str[i + 1] & 0xC0) == 0x80)
+                {
+                    result += c;
+                    result += str[i + 1];
+                    i += 1;
+                }
+            }
+            else if ((c & 0xF0) == 0xE0)
+            {
+                // 3<>ֽ<EFBFBD>UTF-8
+                if (i + 2 < str.size() && (str[i + 1] & 0xC0) == 0x80 && (str[i + 2] & 0xC0) == 0x80)
+                {
+                    result += c;
+                    result += str[i + 1];
+                    result += str[i + 2];
+                    i += 2;
+                }
+            }
+            else if ((c & 0xF8) == 0xF0)
+            {
+                // 4<>ֽ<EFBFBD>UTF-8
+                if (i + 3 < str.size() && (str[i + 1] & 0xC0) == 0x80 &&
+                    (str[i + 2] & 0xC0) == 0x80 && (str[i + 3] & 0xC0) == 0x80)
+                {
+                    result += c;
+                    result += str[i + 1];
+                    result += str[i + 2];
+                    result += str[i + 3];
+                    i += 3;
+                }
+            }
+            // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ч<EFBFBD><D0A7>UTF-8<>ֽ<EFBFBD>
+        }
+        return result;
+    }
+
+    json create_api_response(bool success, int code, const std::string &msg,
+                             const std::string &req_code, const std::string &req_from,
+                             const std::string &req_cmd, const json &res_data)
+    {
+        json response;
+        response["success"] = success;
+        response["code"] = code;
+        response["msg"] = msg;
+        response["req_code"] = req_code;
+        response["req_from"] = req_from;
+        response["req_cmd"] = req_cmd;
+        response["timestamp"] = std::time(nullptr);
+
+        if (!res_data.is_null())
+        {
+            response["res_data"] = res_data;
+        }
+        else
+        {
+            response["res_data"] = json::object();
+        }
+
+        return response;
+    }
+
+    // Base64<36><34><EFBFBD>뺯<EFBFBD><EBBAAF>
+    std::string base64_decode(const std::string &encoded_string)
+    {
+        int in_len = encoded_string.size();
+        int i = 0;
+        int j = 0;
+        int in_ = 0;
+        unsigned char char_array_4[4], char_array_3[3];
+        std::string ret;
+
+        // <20>Ƴ<EFBFBD><C6B3><EFBFBD><EFBFBD>ܵĻ<DCB5><C4BB>з<EFBFBD><D0B7>Ϳո<CDBF>
+        std::string clean_encoded;
+        for (char c : encoded_string)
+        {
+            if (c != '\n' && c != '\r' && c != ' ')
+            {
+                clean_encoded += c;
+            }
+        }
+        in_len = clean_encoded.size();
+
+        while (in_len-- && (clean_encoded[in_] != '=') && is_base64(clean_encoded[in_]))
+        {
+            char_array_4[i++] = clean_encoded[in_];
+            in_++;
+            if (i == 4)
+            {
+                for (i = 0; i < 4; i++)
+                {
+                    size_t pos = base64_chars.find(char_array_4[i]);
+                    if (pos == std::string::npos)
+                    {
+                        throw std::runtime_error("Invalid base64 character");
+                    }
+                    char_array_4[i] = pos;
+                }
+
+                char_array_3[0] = (char_array_4[0] << 2) + ((char_array_4[1] & 0x30) >> 4);
+                char_array_3[1] = ((char_array_4[1] & 0xf) << 4) + ((char_array_4[2] & 0x3c) >> 2);
+                char_array_3[2] = ((char_array_4[2] & 0x3) << 6) + char_array_4[3];
+
+                for (i = 0; i < 3; i++)
+                {
+                    ret += char_array_3[i];
+                }
+                i = 0;
+            }
+        }
+
+        if (i)
+        {
+            for (j = i; j < 4; j++)
+            {
+                char_array_4[j] = 0;
+            }
+
+            for (j = 0; j < 4; j++)
+            {
+                size_t pos = base64_chars.find(char_array_4[j]);
+                if (pos == std::string::npos && j >= i)
+                {
+                    // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ַ<EFBFBD>λ<EFBFBD><CEBB>Ϊ0
+                    char_array_4[j] = 0;
+                }
+                else if (pos != std::string::npos)
+                {
+                    char_array_4[j] = pos;
+                }
+                else
+                {
+                    throw std::runtime_error("Invalid base64 character");
+                }
+            }
+
+            char_array_3[0] = (char_array_4[0] << 2) + ((char_array_4[1] & 0x30) >> 4);
+            char_array_3[1] = ((char_array_4[1] & 0xf) << 4) + ((char_array_4[2] & 0x3c) >> 2);
+            char_array_3[2] = ((char_array_4[2] & 0x3) << 6) + char_array_4[3];
+
+            for (j = 0; j < i - 1; j++)
+            {
+                ret += char_array_3[j];
+            }
+        }
+
+        return ret;
+    }
+
+    // <20><>base64<36><34><EFBFBD><EFBFBD><EFBFBD>URDFת<46><D7AA>Ϊ<EFBFBD>ַ<EFBFBD><D6B7><EFBFBD>
+    std::string base64_to_urdf(const std::string &base64_urdf)
+    {
+        try
+        {
+            std::string urdf_content = base64_decode(base64_urdf);
+
+            // <20><>֤<EFBFBD><D6A4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD><EFBFBD><EFBFBD>Ч<EFBFBD><D0A7>URDF/XML
+            if (urdf_content.find("<?xml") != std::string::npos ||
+                urdf_content.find("<robot") != std::string::npos)
+            {
+                return urdf_content;
+            }
+            else
+            {
+                throw std::runtime_error("Decoded content does not appear to be a valid URDF file");
+            }
+        }
+        catch (const std::exception &e)
+        {
+            throw std::runtime_error(std::string("Failed to decode URDF: ") + e.what());
+        }
+    }
+
+    // <20><>base64<36><34><EFBFBD><EFBFBD><EFBFBD>URDF<44><46><EFBFBD>浽<EFBFBD>ļ<EFBFBD>
+    bool save_base64_urdf_to_file(const std::string &base64_urdf, const std::string &filename)
+    {
+        try
+        {
+            std::string urdf_content = base64_to_urdf(base64_urdf);
+            return save_urdf_string_to_file(urdf_content, filename);
+        }
+        catch (const std::exception &e)
+        {
+            std::cerr << "Error saving base64 URDF to file: " << e.what() << std::endl;
+            return false;
+        }
+    }
+
+    // <20><>URDF<44>ַ<EFBFBD><D6B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD>浽<EFBFBD>ļ<EFBFBD>
+    bool save_urdf_string_to_file(const std::string &urdf_content, const std::string &filename)
+    {
+        try
+        {
+            std::ofstream out_file(filename);
+            if (!out_file)
+            {
+                throw std::runtime_error("Cannot create output file: " + filename);
+            }
+
+            out_file << urdf_content;
+            out_file.close();
+
+            std::cout << "URDF file successfully saved to: " << filename << std::endl;
+            std::cout << "File size: " << urdf_content.size() << " bytes" << std::endl;
+
+            return true;
+        }
+        catch (const std::exception &e)
+        {
+            std::cerr << "Error saving URDF to file: " << e.what() << std::endl;
+            return false;
+        }
+    }
+
+    // <20><>֤base64<36>ַ<EFBFBD><D6B7><EFBFBD><EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD><EFBFBD><EFBFBD>Ч<EFBFBD><D0A7>URDF
+    bool validate_urdf_base64(const std::string &base64_urdf)
+    {
+        try
+        {
+            std::string urdf_content = base64_to_urdf(base64_urdf);
+
+            // <20>򵥵<EFBFBD>URDF<44><46>֤
+            bool has_xml_decl = urdf_content.find("<?xml") != std::string::npos;
+            bool has_robot_tag = urdf_content.find("<robot") != std::string::npos;
+            bool has_link_tag = urdf_content.find("<link") != std::string::npos;
+            bool has_joint_tag = urdf_content.find("<joint") != std::string::npos;
+
+            return has_robot_tag && (has_link_tag || has_joint_tag);
+        }
+        catch (...)
+        {
+            return false;
+        }
+    }
+
+} // namespace utils