按推荐建议,继续执行

结论:已将 LinuxCNC TRT xyzac/xyzbc 运动学源码纳入 vendored manifest、native probe 和文档验证链,完整 native 验证通过。
This commit is contained in:
2026-06-07 19:21:54 +08:00
parent 1b2f95b3d1
commit e634adfa56
13 changed files with 804 additions and 15 deletions

View File

@@ -0,0 +1,96 @@
#include <cmath>
#include <iostream>
#include "emc/kinematics/kinematics.h"
#include "emc/motion/emcmotcfg.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_joints(const double *actual, const double *expected)
{
for (int index = 0; index < 5; ++index) {
if (!near(actual[index], expected[index])) {
return 0;
}
}
return 1;
}
int near_identity_pose(const EmcPose &actual, const double *joints)
{
return near(actual.tran.x, joints[0]) &&
near(actual.tran.y, joints[1]) &&
near(actual.tran.z, joints[2]) &&
near(actual.a, joints[3]) &&
near(actual.c, joints[4]);
}
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 << "_ac="
<< pose.a << ","
<< pose.c << "\n";
}
void print_joints(const char *prefix, const double *joints)
{
std::cout << prefix << "_xyzac="
<< joints[0] << ","
<< joints[1] << ","
<< joints[2] << ","
<< joints[3] << ","
<< joints[4] << "\n";
}
} // namespace
int main()
{
const int init_rc = rtapi_app_main();
std::cout << "xyzac_trt_init=" << init_rc << "\n";
std::cout << "xyzac_trt_type=" << kinematicsType() << "\n";
std::cout << "xyzac_trt_switchable=" << kinematicsSwitchable() << "\n";
double joints[EMCMOT_MAX_JOINTS]{};
joints[0] = 10.0;
joints[1] = 20.0;
joints[2] = 30.0;
joints[3] = 25.0;
joints[4] = 40.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 << "xyzac_trt_forward=" << forward_rc << "\n";
print_pose("xyzac_trt_forward", forward_pose);
double inverse_joints[EMCMOT_MAX_JOINTS]{};
const int inverse_rc = kinematicsInverse(&forward_pose, inverse_joints, &iflags, &fflags);
std::cout << "xyzac_trt_inverse=" << inverse_rc << "\n";
print_joints("xyzac_trt_inverse", inverse_joints);
std::cout << "xyzac_trt_roundtrip_joints=" << near_joints(inverse_joints, joints) << "\n";
const int switch_rc = kinematicsSwitch(1);
std::cout << "xyzac_trt_switch_identity=" << switch_rc << "\n";
EmcPose identity_pose{};
const int identity_forward_rc = kinematicsForward(joints, &identity_pose, &fflags, &iflags);
std::cout << "xyzac_trt_identity_forward=" << identity_forward_rc << "\n";
print_pose("xyzac_trt_identity", identity_pose);
std::cout << "xyzac_trt_identity_near=" << near_identity_pose(identity_pose, joints) << "\n";
rtapi_app_exit();
return 0;
}

View File

@@ -0,0 +1,96 @@
#include <cmath>
#include <iostream>
#include "emc/kinematics/kinematics.h"
#include "emc/motion/emcmotcfg.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_joints(const double *actual, const double *expected)
{
for (int index = 0; index < 5; ++index) {
if (!near(actual[index], expected[index])) {
return 0;
}
}
return 1;
}
int near_identity_pose(const EmcPose &actual, const double *joints)
{
return near(actual.tran.x, joints[0]) &&
near(actual.tran.y, joints[1]) &&
near(actual.tran.z, joints[2]) &&
near(actual.b, joints[3]) &&
near(actual.c, joints[4]);
}
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 << "_bc="
<< pose.b << ","
<< pose.c << "\n";
}
void print_joints(const char *prefix, const double *joints)
{
std::cout << prefix << "_xyzbc="
<< joints[0] << ","
<< joints[1] << ","
<< joints[2] << ","
<< joints[3] << ","
<< joints[4] << "\n";
}
} // namespace
int main()
{
const int init_rc = rtapi_app_main();
std::cout << "xyzbc_trt_init=" << init_rc << "\n";
std::cout << "xyzbc_trt_type=" << kinematicsType() << "\n";
std::cout << "xyzbc_trt_switchable=" << kinematicsSwitchable() << "\n";
double joints[EMCMOT_MAX_JOINTS]{};
joints[0] = 10.0;
joints[1] = 20.0;
joints[2] = 30.0;
joints[3] = 35.0;
joints[4] = 40.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 << "xyzbc_trt_forward=" << forward_rc << "\n";
print_pose("xyzbc_trt_forward", forward_pose);
double inverse_joints[EMCMOT_MAX_JOINTS]{};
const int inverse_rc = kinematicsInverse(&forward_pose, inverse_joints, &iflags, &fflags);
std::cout << "xyzbc_trt_inverse=" << inverse_rc << "\n";
print_joints("xyzbc_trt_inverse", inverse_joints);
std::cout << "xyzbc_trt_roundtrip_joints=" << near_joints(inverse_joints, joints) << "\n";
const int switch_rc = kinematicsSwitch(1);
std::cout << "xyzbc_trt_switch_identity=" << switch_rc << "\n";
EmcPose identity_pose{};
const int identity_forward_rc = kinematicsForward(joints, &identity_pose, &fflags, &iflags);
std::cout << "xyzbc_trt_identity_forward=" << identity_forward_rc << "\n";
print_pose("xyzbc_trt_identity", identity_pose);
std::cout << "xyzbc_trt_identity_near=" << near_identity_pose(identity_pose, joints) << "\n";
rtapi_app_exit();
return 0;
}

View File

@@ -33,6 +33,7 @@ typedef union {
} hal_data_u;
typedef bool hal_bit_t;
typedef double real_t;
typedef double hal_float_t;
typedef int hal_s32_t;
typedef unsigned int hal_u32_t;