按推荐建议,继续执行
结论:继续按 LinuxCNC 源码直接复用路线推进,新增 corexy、rotate、rose、max 四个运动学源码的 vendored 同步、native 探针、source probe 覆盖和复用文档,并通过 native 验证。
This commit is contained in:
@@ -0,0 +1,91 @@
|
||||
#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 < 9; ++index) {
|
||||
if (!near(actual[index], expected[index])) {
|
||||
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 << "_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 << "corexy_init=" << init_rc << "\n";
|
||||
std::cout << "corexy_type=" << kinematicsType() << "\n";
|
||||
std::cout << "corexy_switchable=" << kinematicsSwitchable() << "\n";
|
||||
|
||||
double joints[EMCMOT_MAX_JOINTS]{};
|
||||
joints[0] = 12.0;
|
||||
joints[1] = 4.0;
|
||||
joints[2] = 3.0;
|
||||
joints[3] = 4.0;
|
||||
joints[4] = 5.0;
|
||||
joints[5] = 6.0;
|
||||
joints[6] = 7.0;
|
||||
joints[7] = 8.0;
|
||||
joints[8] = 9.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 << "corexy_forward=" << forward_rc << "\n";
|
||||
print_pose("corexy_forward", forward_pose);
|
||||
|
||||
double inverse_joints[EMCMOT_MAX_JOINTS]{};
|
||||
const int inverse_rc = kinematicsInverse(&forward_pose, inverse_joints, &iflags, &fflags);
|
||||
std::cout << "corexy_inverse=" << inverse_rc << "\n";
|
||||
print_joints("corexy_inverse", inverse_joints);
|
||||
std::cout << "corexy_roundtrip_joints=" << near_joints(inverse_joints, joints) << "\n";
|
||||
|
||||
rtapi_app_exit();
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,91 @@
|
||||
#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 < 9; ++index) {
|
||||
if (!near(actual[index], expected[index])) {
|
||||
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 << "_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 << "max_init=" << init_rc << "\n";
|
||||
std::cout << "max_type=" << kinematicsType() << "\n";
|
||||
std::cout << "max_switchable=" << kinematicsSwitchable() << "\n";
|
||||
|
||||
double joints[EMCMOT_MAX_JOINTS]{};
|
||||
joints[0] = 10.0;
|
||||
joints[1] = 20.0;
|
||||
joints[2] = 30.0;
|
||||
joints[3] = 4.0;
|
||||
joints[4] = 0.0;
|
||||
joints[5] = 25.0;
|
||||
joints[6] = 0.0;
|
||||
joints[7] = 0.0;
|
||||
joints[8] = 3.5;
|
||||
|
||||
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 << "max_forward=" << forward_rc << "\n";
|
||||
print_pose("max_forward", forward_pose);
|
||||
|
||||
double inverse_joints[EMCMOT_MAX_JOINTS]{};
|
||||
const int inverse_rc = kinematicsInverse(&forward_pose, inverse_joints, &iflags, &fflags);
|
||||
std::cout << "max_inverse=" << inverse_rc << "\n";
|
||||
print_joints("max_inverse", inverse_joints);
|
||||
std::cout << "max_roundtrip_joints=" << near_joints(inverse_joints, joints) << "\n";
|
||||
|
||||
rtapi_app_exit();
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,72 @@
|
||||
#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 < 3; ++index) {
|
||||
if (!near(actual[index], expected[index])) {
|
||||
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";
|
||||
}
|
||||
|
||||
void print_joints(const char *prefix, const double *joints)
|
||||
{
|
||||
std::cout << prefix << "_rzt="
|
||||
<< joints[0] << ","
|
||||
<< joints[1] << ","
|
||||
<< joints[2] << "\n";
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
int main()
|
||||
{
|
||||
const int init_rc = rtapi_app_main();
|
||||
std::cout << "rose_init=" << init_rc << "\n";
|
||||
std::cout << "rose_type=" << kinematicsType() << "\n";
|
||||
std::cout << "rose_switchable=" << kinematicsSwitchable() << "\n";
|
||||
|
||||
double joints[EMCMOT_MAX_JOINTS]{};
|
||||
joints[0] = 12.0;
|
||||
joints[1] = 5.0;
|
||||
joints[2] = 30.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 << "rose_forward=" << forward_rc << "\n";
|
||||
print_pose("rose_forward", forward_pose);
|
||||
|
||||
double inverse_joints[EMCMOT_MAX_JOINTS]{};
|
||||
const int inverse_rc = kinematicsInverse(&forward_pose, inverse_joints, &iflags, &fflags);
|
||||
std::cout << "rose_inverse=" << inverse_rc << "\n";
|
||||
print_joints("rose_inverse", inverse_joints);
|
||||
std::cout << "rose_roundtrip_joints=" << near_joints(inverse_joints, joints) << "\n";
|
||||
|
||||
rtapi_app_exit();
|
||||
return 0;
|
||||
}
|
||||
@@ -0,0 +1,91 @@
|
||||
#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 < 9; ++index) {
|
||||
if (!near(actual[index], expected[index])) {
|
||||
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 << "_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 << "rotate_init=" << init_rc << "\n";
|
||||
std::cout << "rotate_type=" << kinematicsType() << "\n";
|
||||
std::cout << "rotate_switchable=" << kinematicsSwitchable() << "\n";
|
||||
|
||||
double joints[EMCMOT_MAX_JOINTS]{};
|
||||
joints[0] = 10.0;
|
||||
joints[1] = 20.0;
|
||||
joints[2] = 30.0;
|
||||
joints[3] = 4.0;
|
||||
joints[4] = 5.0;
|
||||
joints[5] = 30.0;
|
||||
joints[6] = 7.0;
|
||||
joints[7] = 8.0;
|
||||
joints[8] = 9.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 << "rotate_forward=" << forward_rc << "\n";
|
||||
print_pose("rotate_forward", forward_pose);
|
||||
|
||||
double inverse_joints[EMCMOT_MAX_JOINTS]{};
|
||||
const int inverse_rc = kinematicsInverse(&forward_pose, inverse_joints, &iflags, &fflags);
|
||||
std::cout << "rotate_inverse=" << inverse_rc << "\n";
|
||||
print_joints("rotate_inverse", inverse_joints);
|
||||
std::cout << "rotate_roundtrip_joints=" << near_joints(inverse_joints, joints) << "\n";
|
||||
|
||||
rtapi_app_exit();
|
||||
return 0;
|
||||
}
|
||||
Reference in New Issue
Block a user