按推荐建议,继续执行
结论:已继续补齐 LinuxCNC 源码派生的 genhexkins 与 pentakins 原生 runtime baseline,构建、vendor 同步、语义守卫和 native probe 验证均通过。
This commit is contained in:
@@ -0,0 +1,110 @@
|
||||
#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-6;
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
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.b, joints[4]) &&
|
||||
near(actual.c, joints[5]);
|
||||
}
|
||||
|
||||
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 << "_abc="
|
||||
<< pose.a << ","
|
||||
<< pose.b << ","
|
||||
<< pose.c << "\n";
|
||||
}
|
||||
|
||||
void print_joints(const char *prefix, const double *joints)
|
||||
{
|
||||
std::cout << prefix << "_struts="
|
||||
<< joints[0] << ","
|
||||
<< joints[1] << ","
|
||||
<< joints[2] << ","
|
||||
<< joints[3] << ","
|
||||
<< joints[4] << ","
|
||||
<< joints[5] << "\n";
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
int main()
|
||||
{
|
||||
const int init_rc = rtapi_app_main();
|
||||
std::cout << "genhex_init=" << init_rc << "\n";
|
||||
std::cout << "genhex_type=" << kinematicsType() << "\n";
|
||||
std::cout << "genhex_switchable=" << kinematicsSwitchable() << "\n";
|
||||
|
||||
EmcPose pose{};
|
||||
pose.tran.x = 1.25;
|
||||
pose.tran.y = -2.5;
|
||||
pose.tran.z = 3.75;
|
||||
pose.a = 1.0;
|
||||
pose.b = -2.0;
|
||||
pose.c = 3.0;
|
||||
|
||||
KINEMATICS_FORWARD_FLAGS fflags = 0;
|
||||
KINEMATICS_INVERSE_FLAGS iflags = 0;
|
||||
double inverse_joints[EMCMOT_MAX_JOINTS]{};
|
||||
const int inverse_rc = kinematicsInverse(&pose, inverse_joints, &iflags, &fflags);
|
||||
std::cout << "genhex_inverse=" << inverse_rc << "\n";
|
||||
print_joints("genhex_inverse", inverse_joints);
|
||||
|
||||
EmcPose warmup_pose = pose;
|
||||
const int warmup_rc = kinematicsForward(inverse_joints, &warmup_pose, &fflags, &iflags);
|
||||
std::cout << "genhex_forward_warmup=" << warmup_rc << "\n";
|
||||
|
||||
EmcPose forward_pose = pose;
|
||||
const int forward_rc = kinematicsForward(inverse_joints, &forward_pose, &fflags, &iflags);
|
||||
std::cout << "genhex_forward=" << forward_rc << "\n";
|
||||
print_pose("genhex_forward", forward_pose);
|
||||
std::cout << "genhex_roundtrip_pose=" << near_pose(forward_pose, pose) << "\n";
|
||||
|
||||
const int switch_rc = kinematicsSwitch(1);
|
||||
std::cout << "genhex_switch_identity=" << switch_rc << "\n";
|
||||
double identity_joints[EMCMOT_MAX_JOINTS]{};
|
||||
for (int index = 0; index < 6; ++index) {
|
||||
identity_joints[index] = inverse_joints[index];
|
||||
}
|
||||
EmcPose identity_pose{};
|
||||
const int identity_forward_rc = kinematicsForward(
|
||||
identity_joints, &identity_pose, &fflags, &iflags);
|
||||
std::cout << "genhex_identity_forward=" << identity_forward_rc << "\n";
|
||||
print_pose("genhex_identity", identity_pose);
|
||||
std::cout << "genhex_identity_near="
|
||||
<< near_identity_pose(identity_pose, identity_joints) << "\n";
|
||||
|
||||
rtapi_app_exit();
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,89 @@
|
||||
#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-6;
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
int positive_struts(const double *joints)
|
||||
{
|
||||
for (int index = 0; index < 5; ++index) {
|
||||
if (joints[index] <= 0.0) {
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
return 1;
|
||||
}
|
||||
|
||||
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 << "_struts="
|
||||
<< joints[0] << ","
|
||||
<< joints[1] << ","
|
||||
<< joints[2] << ","
|
||||
<< joints[3] << ","
|
||||
<< joints[4] << "\n";
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
int main()
|
||||
{
|
||||
const int init_rc = rtapi_app_main();
|
||||
std::cout << "pentakins_init=" << init_rc << "\n";
|
||||
std::cout << "pentakins_type=" << kinematicsType() << "\n";
|
||||
std::cout << "pentakins_switchable=" << kinematicsSwitchable() << "\n";
|
||||
|
||||
EmcPose pose{};
|
||||
pose.tran.x = 12.5;
|
||||
pose.tran.y = -8.25;
|
||||
pose.tran.z = 25.0;
|
||||
pose.a = 2.0;
|
||||
pose.b = -1.5;
|
||||
|
||||
KINEMATICS_FORWARD_FLAGS fflags = 0;
|
||||
KINEMATICS_INVERSE_FLAGS iflags = 0;
|
||||
double inverse_joints[EMCMOT_MAX_JOINTS]{};
|
||||
const int inverse_rc = kinematicsInverse(&pose, inverse_joints, &iflags, &fflags);
|
||||
std::cout << "pentakins_inverse=" << inverse_rc << "\n";
|
||||
print_joints("pentakins_inverse", inverse_joints);
|
||||
std::cout << "pentakins_positive_struts=" << positive_struts(inverse_joints) << "\n";
|
||||
|
||||
EmcPose forward_pose = pose;
|
||||
const int forward_rc = kinematicsForward(inverse_joints, &forward_pose, &fflags, &iflags);
|
||||
std::cout << "pentakins_forward=" << forward_rc << "\n";
|
||||
print_pose("pentakins_forward", forward_pose);
|
||||
std::cout << "pentakins_roundtrip_pose=" << near_pose(forward_pose, pose) << "\n";
|
||||
|
||||
rtapi_app_exit();
|
||||
return 0;
|
||||
}
|
||||
Reference in New Issue
Block a user