按推荐建议,继续执行

结论:继续按 LinuxCNC 源码直接复用路线推进,新增 linear delta、rotary delta、scorbot 三类运动学源码的 vendored 同步、native 探针、source probe 覆盖和复用文档,并通过 native 验证。
This commit is contained in:
2026-06-07 19:55:43 +08:00
parent d936e9f10c
commit 01495ee26b
12 changed files with 1276 additions and 2 deletions

View File

@@ -0,0 +1,93 @@
#include <cmath>
#include <iostream>
#include "emc/kinematics/kinematics.h"
int rtapi_app_main(void);
void rtapi_app_exit(void);
namespace {
int near(double actual, double expected)
{
return std::fabs(actual - expected) < 1e-9;
}
int near_pose(const EmcPose &actual, const EmcPose &expected)
{
return near(actual.tran.x, expected.tran.x) &&
near(actual.tran.y, expected.tran.y) &&
near(actual.tran.z, expected.tran.z) &&
near(actual.a, expected.a) &&
near(actual.b, expected.b) &&
near(actual.c, expected.c) &&
near(actual.u, expected.u) &&
near(actual.v, expected.v) &&
near(actual.w, expected.w);
}
void print_pose(const char *prefix, const EmcPose &pose)
{
std::cout << prefix << "_xyz="
<< pose.tran.x << ","
<< pose.tran.y << ","
<< pose.tran.z << "\n";
std::cout << prefix << "_abcuvw="
<< pose.a << ","
<< pose.b << ","
<< pose.c << ","
<< pose.u << ","
<< pose.v << ","
<< pose.w << "\n";
}
void print_joints(const char *prefix, const double *joints)
{
std::cout << prefix << "_xyzabcuvw="
<< joints[0] << ","
<< joints[1] << ","
<< joints[2] << ","
<< joints[3] << ","
<< joints[4] << ","
<< joints[5] << ","
<< joints[6] << ","
<< joints[7] << ","
<< joints[8] << "\n";
}
} // namespace
int main()
{
const int init_rc = rtapi_app_main();
std::cout << "lineardelta_init=" << init_rc << "\n";
std::cout << "lineardelta_type=" << kinematicsType() << "\n";
std::cout << "lineardelta_switchable=" << kinematicsSwitchable() << "\n";
EmcPose pose{};
pose.tran.x = 10.0;
pose.tran.y = 20.0;
pose.tran.z = -30.0;
pose.a = 1.0;
pose.b = 2.0;
pose.c = 3.0;
pose.u = 4.0;
pose.v = 5.0;
pose.w = 6.0;
KINEMATICS_FORWARD_FLAGS fflags = 0;
KINEMATICS_INVERSE_FLAGS iflags = 0;
double inverse_joints[9]{};
const int inverse_rc = kinematicsInverse(&pose, inverse_joints, &iflags, &fflags);
std::cout << "lineardelta_inverse=" << inverse_rc << "\n";
print_joints("lineardelta_inverse", inverse_joints);
EmcPose forward_pose{};
const int forward_rc = kinematicsForward(inverse_joints, &forward_pose, &fflags, &iflags);
std::cout << "lineardelta_forward=" << forward_rc << "\n";
print_pose("lineardelta_forward", forward_pose);
std::cout << "lineardelta_roundtrip_pose=" << near_pose(forward_pose, pose) << "\n";
rtapi_app_exit();
return 0;
}

View File

@@ -0,0 +1,93 @@
#include <cmath>
#include <iostream>
#include "emc/kinematics/kinematics.h"
int rtapi_app_main(void);
void rtapi_app_exit(void);
namespace {
int near(double actual, double expected)
{
return std::fabs(actual - expected) < 1e-9;
}
int near_pose(const EmcPose &actual, const EmcPose &expected)
{
return near(actual.tran.x, expected.tran.x) &&
near(actual.tran.y, expected.tran.y) &&
near(actual.tran.z, expected.tran.z) &&
near(actual.a, expected.a) &&
near(actual.b, expected.b) &&
near(actual.c, expected.c) &&
near(actual.u, expected.u) &&
near(actual.v, expected.v) &&
near(actual.w, expected.w);
}
void print_pose(const char *prefix, const EmcPose &pose)
{
std::cout << prefix << "_xyz="
<< pose.tran.x << ","
<< pose.tran.y << ","
<< pose.tran.z << "\n";
std::cout << prefix << "_abcuvw="
<< pose.a << ","
<< pose.b << ","
<< pose.c << ","
<< pose.u << ","
<< pose.v << ","
<< pose.w << "\n";
}
void print_joints(const char *prefix, const double *joints)
{
std::cout << prefix << "_xyzabcuvw="
<< joints[0] << ","
<< joints[1] << ","
<< joints[2] << ","
<< joints[3] << ","
<< joints[4] << ","
<< joints[5] << ","
<< joints[6] << ","
<< joints[7] << ","
<< joints[8] << "\n";
}
} // namespace
int main()
{
const int init_rc = rtapi_app_main();
std::cout << "rotarydelta_init=" << init_rc << "\n";
std::cout << "rotarydelta_type=" << kinematicsType() << "\n";
std::cout << "rotarydelta_switchable=" << kinematicsSwitchable() << "\n";
EmcPose pose{};
pose.tran.x = 0.0;
pose.tran.y = 0.0;
pose.tran.z = -12.0;
pose.a = 1.0;
pose.b = 2.0;
pose.c = 3.0;
pose.u = 4.0;
pose.v = 5.0;
pose.w = 6.0;
KINEMATICS_FORWARD_FLAGS fflags = 0;
KINEMATICS_INVERSE_FLAGS iflags = 0;
double inverse_joints[9]{};
const int inverse_rc = kinematicsInverse(&pose, inverse_joints, &iflags, &fflags);
std::cout << "rotarydelta_inverse=" << inverse_rc << "\n";
print_joints("rotarydelta_inverse", inverse_joints);
EmcPose forward_pose{};
const int forward_rc = kinematicsForward(inverse_joints, &forward_pose, &fflags, &iflags);
std::cout << "rotarydelta_forward=" << forward_rc << "\n";
print_pose("rotarydelta_forward", forward_pose);
std::cout << "rotarydelta_roundtrip_pose=" << near_pose(forward_pose, pose) << "\n";
rtapi_app_exit();
return 0;
}

View File

@@ -0,0 +1,82 @@
#include <cmath>
#include <iostream>
#include "emc/kinematics/kinematics.h"
int rtapi_app_main(void);
void rtapi_app_exit(void);
namespace {
int near(double actual, double expected)
{
return std::fabs(actual - expected) < 1e-9;
}
int near_pose(const EmcPose &actual, const EmcPose &expected)
{
return near(actual.tran.x, expected.tran.x) &&
near(actual.tran.y, expected.tran.y) &&
near(actual.tran.z, expected.tran.z) &&
near(actual.a, expected.a) &&
near(actual.b, expected.b);
}
void print_pose(const char *prefix, const EmcPose &pose)
{
std::cout << prefix << "_xyz="
<< pose.tran.x << ","
<< pose.tran.y << ","
<< pose.tran.z << "\n";
std::cout << prefix << "_ab="
<< pose.a << ","
<< pose.b << "\n";
}
void print_joints(const char *prefix, const double *joints)
{
std::cout << prefix << "_xyzab="
<< joints[0] << ","
<< joints[1] << ","
<< joints[2] << ","
<< joints[3] << ","
<< joints[4] << "\n";
}
} // namespace
int main()
{
const int init_rc = rtapi_app_main();
std::cout << "scorbot_init=" << init_rc << "\n";
std::cout << "scorbot_type=" << kinematicsType() << "\n";
std::cout << "scorbot_switchable=" << kinematicsSwitchable() << "\n";
double joints[9]{};
joints[0] = 25.0;
joints[1] = 20.0;
joints[2] = 10.0;
joints[3] = 4.0;
joints[4] = 5.0;
KINEMATICS_FORWARD_FLAGS fflags = 0;
KINEMATICS_INVERSE_FLAGS iflags = 0;
EmcPose forward_pose{};
const int forward_rc = kinematicsForward(joints, &forward_pose, &fflags, &iflags);
std::cout << "scorbot_forward=" << forward_rc << "\n";
print_pose("scorbot_forward", forward_pose);
double inverse_joints[9]{};
const int inverse_rc = kinematicsInverse(&forward_pose, inverse_joints, &iflags, &fflags);
std::cout << "scorbot_inverse=" << inverse_rc << "\n";
print_joints("scorbot_inverse", inverse_joints);
EmcPose roundtrip_pose{};
const int roundtrip_rc = kinematicsForward(inverse_joints, &roundtrip_pose, &fflags, &iflags);
std::cout << "scorbot_roundtrip_forward=" << roundtrip_rc << "\n";
print_pose("scorbot_roundtrip", roundtrip_pose);
std::cout << "scorbot_roundtrip_pose=" << near_pose(roundtrip_pose, forward_pose) << "\n";
rtapi_app_exit();
return 0;
}