按推荐建议,继续执行

结论:继续按 LinuxCNC 源码直接复用路线推进,新增 tripod 运动学源码的 vendored 同步、native 探针、source probe 覆盖和复用文档,并通过 native 验证。
This commit is contained in:
2026-06-07 20:05:51 +08:00
parent 01495ee26b
commit d9296eb5e0
6 changed files with 515 additions and 3 deletions

View File

@@ -0,0 +1,78 @@
#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_xyz(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);
}
void print_pose(const char *prefix, const EmcPose &pose)
{
std::cout << prefix << "_xyz="
<< pose.tran.x << ","
<< pose.tran.y << ","
<< pose.tran.z << "\n";
}
void print_joints(const char *prefix, const double *joints)
{
std::cout << prefix << "_abc="
<< joints[0] << ","
<< joints[1] << ","
<< joints[2] << "\n";
}
} // namespace
int main()
{
const int init_rc = rtapi_app_main();
std::cout << "tripod_init=" << init_rc << "\n";
std::cout << "tripod_type=" << kinematicsType() << "\n";
std::cout << "tripod_switchable=" << kinematicsSwitchable() << "\n";
EmcPose pose{};
pose.tran.x = 0.25;
pose.tran.y = 0.25;
pose.tran.z = 0.5;
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 << "tripod_inverse=" << inverse_rc << "\n";
std::cout << "tripod_inverse_flags=" << fflags << "\n";
print_joints("tripod_inverse", inverse_joints);
EmcPose forward_pose{};
const int forward_rc = kinematicsForward(inverse_joints, &forward_pose, &fflags, &iflags);
std::cout << "tripod_forward=" << forward_rc << "\n";
print_pose("tripod_forward", forward_pose);
std::cout << "tripod_roundtrip_xyz=" << near_xyz(forward_pose, pose) << "\n";
pose.tran.z = -0.5;
const int inverse_below_rc = kinematicsInverse(&pose, inverse_joints, &iflags, &fflags);
std::cout << "tripod_inverse_below=" << inverse_below_rc << "\n";
std::cout << "tripod_inverse_below_flags=" << fflags << "\n";
const int forward_below_rc = kinematicsForward(inverse_joints, &forward_pose, &fflags, &iflags);
std::cout << "tripod_forward_below=" << forward_below_rc << "\n";
print_pose("tripod_forward_below", forward_pose);
std::cout << "tripod_roundtrip_below_xyz=" << near_xyz(forward_pose, pose) << "\n";
rtapi_app_exit();
return 0;
}