按推荐建议,继续执行
结论:继续按 LinuxCNC 源码直接复用路线推进,新增 corexy、rotate、rose、max 四个运动学源码的 vendored 同步、native 探针、source probe 覆盖和复用文档,并通过 native 验证。
This commit is contained in:
@@ -38,6 +38,7 @@ Current validation is intentionally mechanical:
|
|||||||
| Identity/trivial kinematics | `src/emc/kinematics/kinematics.h`, `cubic.h`, `kins_util.c`, `trivkins.c` | Copy unchanged | HAL component lifecycle and RTAPI module metadata are replaced by standalone shims; forward/inverse mapping behavior remains LinuxCNC source | Vendor byte sync, per-file source probes, `linuxcnc_kinematics_probe` |
|
| Identity/trivial kinematics | `src/emc/kinematics/kinematics.h`, `cubic.h`, `kins_util.c`, `trivkins.c` | Copy unchanged | HAL component lifecycle and RTAPI module metadata are replaced by standalone shims; forward/inverse mapping behavior remains LinuxCNC source | Vendor byte sync, per-file source probes, `linuxcnc_kinematics_probe` |
|
||||||
| Switchable 5-axis bridge kinematics | `src/emc/kinematics/5axiskins.c`, `switchkins.c`, `switchkins.h`, `userkfuncs.c`, plus `src/rtapi/rtapi_ctype.h` | Copy unchanged | HAL pin allocation, HAL component lifecycle, and RTAPI module metadata are standalone runtime edges; switchable 5-axis forward/inverse behavior remains LinuxCNC source | Vendor byte sync, per-file source probes, `linuxcnc_5axis_kinematics_probe` |
|
| Switchable 5-axis bridge kinematics | `src/emc/kinematics/5axiskins.c`, `switchkins.c`, `switchkins.h`, `userkfuncs.c`, plus `src/rtapi/rtapi_ctype.h` | Copy unchanged | HAL pin allocation, HAL component lifecycle, and RTAPI module metadata are standalone runtime edges; switchable 5-axis forward/inverse behavior remains LinuxCNC source | Vendor byte sync, per-file source probes, `linuxcnc_5axis_kinematics_probe` |
|
||||||
| TRT table-rotary kinematics | `src/emc/kinematics/trtfuncs.c`, `xyzac-trt-kins.c`, `xyzbc-trt-kins.c` | Copy unchanged | HAL pin allocation and switchkins lifecycle stay runtime boundaries; XYZAC/XYZBC TRT forward/inverse behavior remains LinuxCNC source | Vendor byte sync, per-file source probes, `linuxcnc_xyzac_trt_kinematics_probe`, `linuxcnc_xyzbc_trt_kinematics_probe` |
|
| TRT table-rotary kinematics | `src/emc/kinematics/trtfuncs.c`, `xyzac-trt-kins.c`, `xyzbc-trt-kins.c` | Copy unchanged | HAL pin allocation and switchkins lifecycle stay runtime boundaries; XYZAC/XYZBC TRT forward/inverse behavior remains LinuxCNC source | Vendor byte sync, per-file source probes, `linuxcnc_xyzac_trt_kinematics_probe`, `linuxcnc_xyzbc_trt_kinematics_probe` |
|
||||||
|
| Additional non-switchable kinematics | `src/emc/kinematics/corexykins.c`, `rotatekins.c`, `rosekins.c`, `maxkins.c` | Copy unchanged | HAL pin allocation, HAL component lifecycle, and RTAPI module metadata are standalone runtime edges; forward/inverse behavior remains LinuxCNC source | Vendor byte sync, per-file source probes, `linuxcnc_corexy_kinematics_probe`, `linuxcnc_rotate_kinematics_probe`, `linuxcnc_rose_kinematics_probe`, `linuxcnc_max_kinematics_probe` |
|
||||||
| Trajectory planner | `src/emc/tp/tp.c`, `tc.c`, `tcq.c`, `spherical_arc.c`, `blendmath.c`, `sp_scurve.c`, `ruckig_wrapper.c`, plus matching `*.h` files | Copy unchanged | Native realtime scheduling and motion process state are replaced by standalone probe setup | Vendor byte sync, per-file source probes, `linuxcnc_tp_api_probe` |
|
| Trajectory planner | `src/emc/tp/tp.c`, `tc.c`, `tcq.c`, `spherical_arc.c`, `blendmath.c`, `sp_scurve.c`, `ruckig_wrapper.c`, plus matching `*.h` files | Copy unchanged | Native realtime scheduling and motion process state are replaced by standalone probe setup | Vendor byte sync, per-file source probes, `linuxcnc_tp_api_probe` |
|
||||||
| Ruckig C planner support | Selected `src/emc/tp/cruckig/*.c` and `*.h` files in the manifest | Copy unchanged | Used as LinuxCNC planner support code through vendored TP sources | Vendor byte sync, per-file source probes |
|
| Ruckig C planner support | Selected `src/emc/tp/cruckig/*.c` and `*.h` files in the manifest | Copy unchanged | Used as LinuxCNC planner support code through vendored TP sources | Vendor byte sync, per-file source probes |
|
||||||
| Posemath | `src/libnml/posemath/posemath.cc`, `_posemath.c`, `gomath.c`, `sincos.c`, and matching headers | Copy unchanged | `gomath.c` is compiled as C; `rtapi.h` shim is C/C++ compatible for this boundary | Vendor byte sync, per-file source probes, TP probe |
|
| Posemath | `src/libnml/posemath/posemath.cc`, `_posemath.c`, `gomath.c`, `sincos.c`, and matching headers | Copy unchanged | `gomath.c` is compiled as C; `rtapi.h` shim is C/C++ compatible for this boundary | Vendor byte sync, per-file source probes, TP probe |
|
||||||
@@ -62,7 +63,8 @@ Current validation is intentionally mechanical:
|
|||||||
## Known Gaps
|
## Known Gaps
|
||||||
|
|
||||||
- Additional non-trivial kinematics implementation files, including serial,
|
- Additional non-trivial kinematics implementation files, including serial,
|
||||||
delta, SCARA, and other machine-specific modules, are not yet extracted.
|
delta, SCARA, hexapod, tripod, and other machine-specific modules, are not
|
||||||
|
yet extracted.
|
||||||
- Browser/WASM C ABI and JS SDK layers are not yet built for the full
|
- Browser/WASM C ABI and JS SDK layers are not yet built for the full
|
||||||
interpreter/planner core.
|
interpreter/planner core.
|
||||||
- OPFS persistence is not yet connected to INI, tool table, parameter file, or
|
- OPFS persistence is not yet connected to INI, tool table, parameter file, or
|
||||||
|
|||||||
@@ -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;
|
||||||
|
}
|
||||||
@@ -87,6 +87,14 @@ check_exitcode linuxcnc_xyzac_trt_kinematics_probe
|
|||||||
check_exitcode linuxcnc_xyzac_trt_kinematics_probe.run
|
check_exitcode linuxcnc_xyzac_trt_kinematics_probe.run
|
||||||
check_exitcode linuxcnc_xyzbc_trt_kinematics_probe
|
check_exitcode linuxcnc_xyzbc_trt_kinematics_probe
|
||||||
check_exitcode linuxcnc_xyzbc_trt_kinematics_probe.run
|
check_exitcode linuxcnc_xyzbc_trt_kinematics_probe.run
|
||||||
|
check_exitcode linuxcnc_corexy_kinematics_probe
|
||||||
|
check_exitcode linuxcnc_corexy_kinematics_probe.run
|
||||||
|
check_exitcode linuxcnc_rotate_kinematics_probe
|
||||||
|
check_exitcode linuxcnc_rotate_kinematics_probe.run
|
||||||
|
check_exitcode linuxcnc_rose_kinematics_probe
|
||||||
|
check_exitcode linuxcnc_rose_kinematics_probe.run
|
||||||
|
check_exitcode linuxcnc_max_kinematics_probe
|
||||||
|
check_exitcode linuxcnc_max_kinematics_probe.run
|
||||||
for name in \
|
for name in \
|
||||||
linuxcnc_tp_tp_source_probe \
|
linuxcnc_tp_tp_source_probe \
|
||||||
linuxcnc_tp_tc_source_probe \
|
linuxcnc_tp_tc_source_probe \
|
||||||
@@ -135,6 +143,10 @@ check_exitcode linuxcnc_5axiskins_source_probe
|
|||||||
check_exitcode linuxcnc_trtfuncs_source_probe
|
check_exitcode linuxcnc_trtfuncs_source_probe
|
||||||
check_exitcode linuxcnc_xyzac_trt_kins_source_probe
|
check_exitcode linuxcnc_xyzac_trt_kins_source_probe
|
||||||
check_exitcode linuxcnc_xyzbc_trt_kins_source_probe
|
check_exitcode linuxcnc_xyzbc_trt_kins_source_probe
|
||||||
|
check_exitcode linuxcnc_corexykins_source_probe
|
||||||
|
check_exitcode linuxcnc_rotatekins_source_probe
|
||||||
|
check_exitcode linuxcnc_rosekins_source_probe
|
||||||
|
check_exitcode linuxcnc_maxkins_source_probe
|
||||||
check_exitcode linuxcnc_interp_convert_source_probe
|
check_exitcode linuxcnc_interp_convert_source_probe
|
||||||
check_exitcode linuxcnc_interp_read_source_probe
|
check_exitcode linuxcnc_interp_read_source_probe
|
||||||
check_exitcode linuxcnc_interp_check_source_probe
|
check_exitcode linuxcnc_interp_check_source_probe
|
||||||
@@ -263,6 +275,38 @@ grep -Fq "xyzbc_trt_switch_identity=0" "$XYZBC_TRT_KINEMATICS_STDOUT"
|
|||||||
grep -Fq "xyzbc_trt_identity_forward=0" "$XYZBC_TRT_KINEMATICS_STDOUT"
|
grep -Fq "xyzbc_trt_identity_forward=0" "$XYZBC_TRT_KINEMATICS_STDOUT"
|
||||||
grep -Fq "xyzbc_trt_identity_near=1" "$XYZBC_TRT_KINEMATICS_STDOUT"
|
grep -Fq "xyzbc_trt_identity_near=1" "$XYZBC_TRT_KINEMATICS_STDOUT"
|
||||||
|
|
||||||
|
COREXY_KINEMATICS_STDOUT="$BUILD_DIR/linuxcnc_corexy_kinematics_probe.run.stdout.log"
|
||||||
|
grep -Fq "corexy_init=0" "$COREXY_KINEMATICS_STDOUT"
|
||||||
|
grep -Fq "corexy_type=4" "$COREXY_KINEMATICS_STDOUT"
|
||||||
|
grep -Fq "corexy_switchable=0" "$COREXY_KINEMATICS_STDOUT"
|
||||||
|
grep -Fq "corexy_forward=0" "$COREXY_KINEMATICS_STDOUT"
|
||||||
|
grep -Fq "corexy_inverse=0" "$COREXY_KINEMATICS_STDOUT"
|
||||||
|
grep -Fq "corexy_roundtrip_joints=1" "$COREXY_KINEMATICS_STDOUT"
|
||||||
|
|
||||||
|
ROTATE_KINEMATICS_STDOUT="$BUILD_DIR/linuxcnc_rotate_kinematics_probe.run.stdout.log"
|
||||||
|
grep -Fq "rotate_init=0" "$ROTATE_KINEMATICS_STDOUT"
|
||||||
|
grep -Fq "rotate_type=4" "$ROTATE_KINEMATICS_STDOUT"
|
||||||
|
grep -Fq "rotate_switchable=0" "$ROTATE_KINEMATICS_STDOUT"
|
||||||
|
grep -Fq "rotate_forward=0" "$ROTATE_KINEMATICS_STDOUT"
|
||||||
|
grep -Fq "rotate_inverse=0" "$ROTATE_KINEMATICS_STDOUT"
|
||||||
|
grep -Fq "rotate_roundtrip_joints=1" "$ROTATE_KINEMATICS_STDOUT"
|
||||||
|
|
||||||
|
ROSE_KINEMATICS_STDOUT="$BUILD_DIR/linuxcnc_rose_kinematics_probe.run.stdout.log"
|
||||||
|
grep -Fq "rose_init=0" "$ROSE_KINEMATICS_STDOUT"
|
||||||
|
grep -Fq "rose_type=4" "$ROSE_KINEMATICS_STDOUT"
|
||||||
|
grep -Fq "rose_switchable=0" "$ROSE_KINEMATICS_STDOUT"
|
||||||
|
grep -Fq "rose_forward=0" "$ROSE_KINEMATICS_STDOUT"
|
||||||
|
grep -Fq "rose_inverse=0" "$ROSE_KINEMATICS_STDOUT"
|
||||||
|
grep -Fq "rose_roundtrip_joints=1" "$ROSE_KINEMATICS_STDOUT"
|
||||||
|
|
||||||
|
MAX_KINEMATICS_STDOUT="$BUILD_DIR/linuxcnc_max_kinematics_probe.run.stdout.log"
|
||||||
|
grep -Fq "max_init=0" "$MAX_KINEMATICS_STDOUT"
|
||||||
|
grep -Fq "max_type=4" "$MAX_KINEMATICS_STDOUT"
|
||||||
|
grep -Fq "max_switchable=0" "$MAX_KINEMATICS_STDOUT"
|
||||||
|
grep -Fq "max_forward=0" "$MAX_KINEMATICS_STDOUT"
|
||||||
|
grep -Fq "max_inverse=0" "$MAX_KINEMATICS_STDOUT"
|
||||||
|
grep -Fq "max_roundtrip_joints=1" "$MAX_KINEMATICS_STDOUT"
|
||||||
|
|
||||||
check_fixture_output() {
|
check_fixture_output() {
|
||||||
local fixture="$1"
|
local fixture="$1"
|
||||||
local expected="$2"
|
local expected="$2"
|
||||||
|
|||||||
@@ -382,6 +382,30 @@ XYZBC_TRT_KINEMATICS_PROBE_SOURCES=(
|
|||||||
"$WRAP_DIR/linuxcnc_xyzbc_trt_kinematics_probe.cpp"
|
"$WRAP_DIR/linuxcnc_xyzbc_trt_kinematics_probe.cpp"
|
||||||
)
|
)
|
||||||
|
|
||||||
|
COREXY_KINEMATICS_PROBE_SOURCES=(
|
||||||
|
"$VENDOR_DIR/src/emc/kinematics/corexykins.c"
|
||||||
|
"$WRAP_DIR/linuxcnc_hal_adapter.cpp"
|
||||||
|
"$WRAP_DIR/linuxcnc_corexy_kinematics_probe.cpp"
|
||||||
|
)
|
||||||
|
|
||||||
|
ROTATE_KINEMATICS_PROBE_SOURCES=(
|
||||||
|
"$VENDOR_DIR/src/emc/kinematics/rotatekins.c"
|
||||||
|
"$WRAP_DIR/linuxcnc_hal_adapter.cpp"
|
||||||
|
"$WRAP_DIR/linuxcnc_rotate_kinematics_probe.cpp"
|
||||||
|
)
|
||||||
|
|
||||||
|
ROSE_KINEMATICS_PROBE_SOURCES=(
|
||||||
|
"$VENDOR_DIR/src/emc/kinematics/rosekins.c"
|
||||||
|
"$WRAP_DIR/linuxcnc_hal_adapter.cpp"
|
||||||
|
"$WRAP_DIR/linuxcnc_rose_kinematics_probe.cpp"
|
||||||
|
)
|
||||||
|
|
||||||
|
MAX_KINEMATICS_PROBE_SOURCES=(
|
||||||
|
"$VENDOR_DIR/src/emc/kinematics/maxkins.c"
|
||||||
|
"$WRAP_DIR/linuxcnc_hal_adapter.cpp"
|
||||||
|
"$WRAP_DIR/linuxcnc_max_kinematics_probe.cpp"
|
||||||
|
)
|
||||||
|
|
||||||
TP_CORE_SOURCES=(
|
TP_CORE_SOURCES=(
|
||||||
"$VENDOR_DIR/src/emc/tp/tp.c"
|
"$VENDOR_DIR/src/emc/tp/tp.c"
|
||||||
"$VENDOR_DIR/src/emc/tp/tc.c"
|
"$VENDOR_DIR/src/emc/tp/tc.c"
|
||||||
@@ -583,6 +607,34 @@ build_binary_target \
|
|||||||
XYZBC_TRT_KINEMATICS_PROBE_SOURCES \
|
XYZBC_TRT_KINEMATICS_PROBE_SOURCES \
|
||||||
NO_LINK_FLAGS
|
NO_LINK_FLAGS
|
||||||
|
|
||||||
|
build_binary_target \
|
||||||
|
linuxcnc_corexy_kinematics_probe \
|
||||||
|
"$BUILD_DIR/linuxcnc_corexy_kinematics_probe" \
|
||||||
|
TP_FLAGS \
|
||||||
|
COREXY_KINEMATICS_PROBE_SOURCES \
|
||||||
|
NO_LINK_FLAGS
|
||||||
|
|
||||||
|
build_binary_target \
|
||||||
|
linuxcnc_rotate_kinematics_probe \
|
||||||
|
"$BUILD_DIR/linuxcnc_rotate_kinematics_probe" \
|
||||||
|
TP_FLAGS \
|
||||||
|
ROTATE_KINEMATICS_PROBE_SOURCES \
|
||||||
|
NO_LINK_FLAGS
|
||||||
|
|
||||||
|
build_binary_target \
|
||||||
|
linuxcnc_rose_kinematics_probe \
|
||||||
|
"$BUILD_DIR/linuxcnc_rose_kinematics_probe" \
|
||||||
|
TP_FLAGS \
|
||||||
|
ROSE_KINEMATICS_PROBE_SOURCES \
|
||||||
|
NO_LINK_FLAGS
|
||||||
|
|
||||||
|
build_binary_target \
|
||||||
|
linuxcnc_max_kinematics_probe \
|
||||||
|
"$BUILD_DIR/linuxcnc_max_kinematics_probe" \
|
||||||
|
TP_FLAGS \
|
||||||
|
MAX_KINEMATICS_PROBE_SOURCES \
|
||||||
|
NO_LINK_FLAGS
|
||||||
|
|
||||||
if [[ "$(tr -d '[:space:]' < "$BUILD_DIR/linuxcnc_kinematics_probe.exitcode")" == "0" ]]; then
|
if [[ "$(tr -d '[:space:]' < "$BUILD_DIR/linuxcnc_kinematics_probe.exitcode")" == "0" ]]; then
|
||||||
set +e
|
set +e
|
||||||
"$BUILD_DIR/linuxcnc_kinematics_probe" \
|
"$BUILD_DIR/linuxcnc_kinematics_probe" \
|
||||||
@@ -643,6 +695,66 @@ else
|
|||||||
"$BUILD_DIR/linuxcnc_xyzbc_trt_kinematics_probe.run.stderr.log"
|
"$BUILD_DIR/linuxcnc_xyzbc_trt_kinematics_probe.run.stderr.log"
|
||||||
fi
|
fi
|
||||||
|
|
||||||
|
if [[ "$(tr -d '[:space:]' < "$BUILD_DIR/linuxcnc_corexy_kinematics_probe.exitcode")" == "0" ]]; then
|
||||||
|
set +e
|
||||||
|
"$BUILD_DIR/linuxcnc_corexy_kinematics_probe" \
|
||||||
|
>"$BUILD_DIR/linuxcnc_corexy_kinematics_probe.run.stdout.log" \
|
||||||
|
2>"$BUILD_DIR/linuxcnc_corexy_kinematics_probe.run.stderr.log"
|
||||||
|
COREXY_KINEMATICS_RUN_RC=$?
|
||||||
|
set -e
|
||||||
|
echo "$COREXY_KINEMATICS_RUN_RC" > "$BUILD_DIR/linuxcnc_corexy_kinematics_probe.run.exitcode"
|
||||||
|
else
|
||||||
|
rm -f \
|
||||||
|
"$BUILD_DIR/linuxcnc_corexy_kinematics_probe.run.exitcode" \
|
||||||
|
"$BUILD_DIR/linuxcnc_corexy_kinematics_probe.run.stdout.log" \
|
||||||
|
"$BUILD_DIR/linuxcnc_corexy_kinematics_probe.run.stderr.log"
|
||||||
|
fi
|
||||||
|
|
||||||
|
if [[ "$(tr -d '[:space:]' < "$BUILD_DIR/linuxcnc_rotate_kinematics_probe.exitcode")" == "0" ]]; then
|
||||||
|
set +e
|
||||||
|
"$BUILD_DIR/linuxcnc_rotate_kinematics_probe" \
|
||||||
|
>"$BUILD_DIR/linuxcnc_rotate_kinematics_probe.run.stdout.log" \
|
||||||
|
2>"$BUILD_DIR/linuxcnc_rotate_kinematics_probe.run.stderr.log"
|
||||||
|
ROTATE_KINEMATICS_RUN_RC=$?
|
||||||
|
set -e
|
||||||
|
echo "$ROTATE_KINEMATICS_RUN_RC" > "$BUILD_DIR/linuxcnc_rotate_kinematics_probe.run.exitcode"
|
||||||
|
else
|
||||||
|
rm -f \
|
||||||
|
"$BUILD_DIR/linuxcnc_rotate_kinematics_probe.run.exitcode" \
|
||||||
|
"$BUILD_DIR/linuxcnc_rotate_kinematics_probe.run.stdout.log" \
|
||||||
|
"$BUILD_DIR/linuxcnc_rotate_kinematics_probe.run.stderr.log"
|
||||||
|
fi
|
||||||
|
|
||||||
|
if [[ "$(tr -d '[:space:]' < "$BUILD_DIR/linuxcnc_rose_kinematics_probe.exitcode")" == "0" ]]; then
|
||||||
|
set +e
|
||||||
|
"$BUILD_DIR/linuxcnc_rose_kinematics_probe" \
|
||||||
|
>"$BUILD_DIR/linuxcnc_rose_kinematics_probe.run.stdout.log" \
|
||||||
|
2>"$BUILD_DIR/linuxcnc_rose_kinematics_probe.run.stderr.log"
|
||||||
|
ROSE_KINEMATICS_RUN_RC=$?
|
||||||
|
set -e
|
||||||
|
echo "$ROSE_KINEMATICS_RUN_RC" > "$BUILD_DIR/linuxcnc_rose_kinematics_probe.run.exitcode"
|
||||||
|
else
|
||||||
|
rm -f \
|
||||||
|
"$BUILD_DIR/linuxcnc_rose_kinematics_probe.run.exitcode" \
|
||||||
|
"$BUILD_DIR/linuxcnc_rose_kinematics_probe.run.stdout.log" \
|
||||||
|
"$BUILD_DIR/linuxcnc_rose_kinematics_probe.run.stderr.log"
|
||||||
|
fi
|
||||||
|
|
||||||
|
if [[ "$(tr -d '[:space:]' < "$BUILD_DIR/linuxcnc_max_kinematics_probe.exitcode")" == "0" ]]; then
|
||||||
|
set +e
|
||||||
|
"$BUILD_DIR/linuxcnc_max_kinematics_probe" \
|
||||||
|
>"$BUILD_DIR/linuxcnc_max_kinematics_probe.run.stdout.log" \
|
||||||
|
2>"$BUILD_DIR/linuxcnc_max_kinematics_probe.run.stderr.log"
|
||||||
|
MAX_KINEMATICS_RUN_RC=$?
|
||||||
|
set -e
|
||||||
|
echo "$MAX_KINEMATICS_RUN_RC" > "$BUILD_DIR/linuxcnc_max_kinematics_probe.run.exitcode"
|
||||||
|
else
|
||||||
|
rm -f \
|
||||||
|
"$BUILD_DIR/linuxcnc_max_kinematics_probe.run.exitcode" \
|
||||||
|
"$BUILD_DIR/linuxcnc_max_kinematics_probe.run.stdout.log" \
|
||||||
|
"$BUILD_DIR/linuxcnc_max_kinematics_probe.run.stderr.log"
|
||||||
|
fi
|
||||||
|
|
||||||
if [[ "$(tr -d '[:space:]' < "$BUILD_DIR/linuxcnc_tp_api_probe.exitcode")" == "0" ]]; then
|
if [[ "$(tr -d '[:space:]' < "$BUILD_DIR/linuxcnc_tp_api_probe.exitcode")" == "0" ]]; then
|
||||||
set +e
|
set +e
|
||||||
"$BUILD_DIR/linuxcnc_tp_api_probe" \
|
"$BUILD_DIR/linuxcnc_tp_api_probe" \
|
||||||
@@ -789,6 +901,30 @@ build_object_target \
|
|||||||
"$VENDOR_DIR/src/emc/kinematics/xyzbc-trt-kins.c" \
|
"$VENDOR_DIR/src/emc/kinematics/xyzbc-trt-kins.c" \
|
||||||
TP_FLAGS
|
TP_FLAGS
|
||||||
|
|
||||||
|
build_object_target \
|
||||||
|
linuxcnc_corexykins_source_probe \
|
||||||
|
"$BUILD_DIR/linuxcnc_corexykins_source_probe.o" \
|
||||||
|
"$VENDOR_DIR/src/emc/kinematics/corexykins.c" \
|
||||||
|
TP_FLAGS
|
||||||
|
|
||||||
|
build_object_target \
|
||||||
|
linuxcnc_rotatekins_source_probe \
|
||||||
|
"$BUILD_DIR/linuxcnc_rotatekins_source_probe.o" \
|
||||||
|
"$VENDOR_DIR/src/emc/kinematics/rotatekins.c" \
|
||||||
|
TP_FLAGS
|
||||||
|
|
||||||
|
build_object_target \
|
||||||
|
linuxcnc_rosekins_source_probe \
|
||||||
|
"$BUILD_DIR/linuxcnc_rosekins_source_probe.o" \
|
||||||
|
"$VENDOR_DIR/src/emc/kinematics/rosekins.c" \
|
||||||
|
TP_FLAGS
|
||||||
|
|
||||||
|
build_object_target \
|
||||||
|
linuxcnc_maxkins_source_probe \
|
||||||
|
"$BUILD_DIR/linuxcnc_maxkins_source_probe.o" \
|
||||||
|
"$VENDOR_DIR/src/emc/kinematics/maxkins.c" \
|
||||||
|
TP_FLAGS
|
||||||
|
|
||||||
build_object_target \
|
build_object_target \
|
||||||
linuxcnc_interp_convert_source_probe \
|
linuxcnc_interp_convert_source_probe \
|
||||||
"$BUILD_DIR/linuxcnc_interp_convert_source_probe.o" \
|
"$BUILD_DIR/linuxcnc_interp_convert_source_probe.o" \
|
||||||
|
|||||||
@@ -40,6 +40,10 @@ src/emc/kinematics/5axiskins.c
|
|||||||
src/emc/kinematics/trtfuncs.c
|
src/emc/kinematics/trtfuncs.c
|
||||||
src/emc/kinematics/xyzac-trt-kins.c
|
src/emc/kinematics/xyzac-trt-kins.c
|
||||||
src/emc/kinematics/xyzbc-trt-kins.c
|
src/emc/kinematics/xyzbc-trt-kins.c
|
||||||
|
src/emc/kinematics/corexykins.c
|
||||||
|
src/emc/kinematics/rotatekins.c
|
||||||
|
src/emc/kinematics/rosekins.c
|
||||||
|
src/emc/kinematics/maxkins.c
|
||||||
src/emc/tp/tp.h
|
src/emc/tp/tp.h
|
||||||
src/emc/tp/tp_types.h
|
src/emc/tp/tp_types.h
|
||||||
src/emc/tp/tc.h
|
src/emc/tp/tc.h
|
||||||
|
|||||||
89
wasm-port/vendor/linuxcnc/src/emc/kinematics/corexykins.c
vendored
Normal file
89
wasm-port/vendor/linuxcnc/src/emc/kinematics/corexykins.c
vendored
Normal file
@@ -0,0 +1,89 @@
|
|||||||
|
/********************************************************************
|
||||||
|
* Description: kinematics for corexy
|
||||||
|
* Adapted from trivkins.c
|
||||||
|
* ref: http://corexy.com/theory.html
|
||||||
|
********************************************************************/
|
||||||
|
|
||||||
|
#include <rtapi.h>
|
||||||
|
#include <rtapi.h>
|
||||||
|
#include <rtapi_app.h>
|
||||||
|
#include <rtapi_math.h>
|
||||||
|
#include <rtapi_string.h>
|
||||||
|
#include <hal.h>
|
||||||
|
#include <emcmotcfg.h>
|
||||||
|
#include <kinematics.h>
|
||||||
|
|
||||||
|
static struct data {
|
||||||
|
hal_s32_t joints[EMCMOT_MAX_JOINTS];
|
||||||
|
} *data;
|
||||||
|
|
||||||
|
int kinematicsForward(const double *joints
|
||||||
|
,EmcPose *pos
|
||||||
|
,const KINEMATICS_FORWARD_FLAGS *fflags
|
||||||
|
,KINEMATICS_INVERSE_FLAGS *iflags
|
||||||
|
) {
|
||||||
|
(void)fflags;
|
||||||
|
(void)iflags;
|
||||||
|
pos->tran.x = 0.5 * (joints[0] + joints[1]);
|
||||||
|
pos->tran.y = 0.5 * (joints[0] - joints[1]);
|
||||||
|
pos->tran.z = joints[2];
|
||||||
|
pos->a = joints[3];
|
||||||
|
pos->b = joints[4];
|
||||||
|
pos->c = joints[5];
|
||||||
|
pos->u = joints[6];
|
||||||
|
pos->v = joints[7];
|
||||||
|
pos->w = joints[8];
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
int kinematicsInverse(const EmcPose *pos
|
||||||
|
,double *joints
|
||||||
|
,const KINEMATICS_INVERSE_FLAGS *iflags
|
||||||
|
,KINEMATICS_FORWARD_FLAGS *fflags
|
||||||
|
) {
|
||||||
|
(void)iflags;
|
||||||
|
(void)fflags;
|
||||||
|
joints[0] = pos->tran.x + pos->tran.y;
|
||||||
|
joints[1] = pos->tran.x - pos->tran.y;
|
||||||
|
joints[2] = pos->tran.z;
|
||||||
|
joints[3] = pos->a;
|
||||||
|
joints[4] = pos->b;
|
||||||
|
joints[5] = pos->c;
|
||||||
|
joints[6] = pos->u;
|
||||||
|
joints[7] = pos->v;
|
||||||
|
joints[8] = pos->w;
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
int kinematicsHome(EmcPose *world
|
||||||
|
,double *joint
|
||||||
|
,KINEMATICS_FORWARD_FLAGS *fflags
|
||||||
|
,KINEMATICS_INVERSE_FLAGS *iflags
|
||||||
|
) {
|
||||||
|
*fflags = 0;
|
||||||
|
*iflags = 0;
|
||||||
|
return kinematicsForward(joint, world, fflags, iflags);
|
||||||
|
}
|
||||||
|
|
||||||
|
KINEMATICS_TYPE kinematicsType() { return KINEMATICS_BOTH; }
|
||||||
|
|
||||||
|
KINS_NOT_SWITCHABLE
|
||||||
|
EXPORT_SYMBOL(kinematicsType);
|
||||||
|
EXPORT_SYMBOL(kinematicsForward);
|
||||||
|
EXPORT_SYMBOL(kinematicsInverse);
|
||||||
|
MODULE_LICENSE("GPL");
|
||||||
|
|
||||||
|
static int comp_id;
|
||||||
|
int rtapi_app_main(void) {
|
||||||
|
comp_id = hal_init("corexykins");
|
||||||
|
if(comp_id < 0) return comp_id;
|
||||||
|
|
||||||
|
data = hal_malloc(sizeof(struct data));
|
||||||
|
|
||||||
|
hal_ready(comp_id);
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
void rtapi_app_exit(void) { hal_exit(comp_id); }
|
||||||
148
wasm-port/vendor/linuxcnc/src/emc/kinematics/maxkins.c
vendored
Normal file
148
wasm-port/vendor/linuxcnc/src/emc/kinematics/maxkins.c
vendored
Normal file
@@ -0,0 +1,148 @@
|
|||||||
|
/********************************************************************
|
||||||
|
* Description: maxkins.c
|
||||||
|
* Kinematics for Chris Radek's tabletop 5 axis mill named 'max'.
|
||||||
|
* This mill has a tilting head (B axis) and horizontal rotary
|
||||||
|
* mounted to the table (C axis).
|
||||||
|
*
|
||||||
|
* Author: Chris Radek
|
||||||
|
* License: GPL Version 2
|
||||||
|
*
|
||||||
|
* Copyright (c) 2007 Chris Radek
|
||||||
|
********************************************************************/
|
||||||
|
|
||||||
|
/********************************************************************
|
||||||
|
* Note: The direction of the B axis is the opposite of the
|
||||||
|
* conventional axis direction. See
|
||||||
|
* https://linuxcnc.org/docs/html/gcode/machining-center.html
|
||||||
|
********************************************************************/
|
||||||
|
|
||||||
|
#include <rtapi.h>
|
||||||
|
#include <rtapi_app.h>
|
||||||
|
#include <rtapi_math.h>
|
||||||
|
#include <hal.h>
|
||||||
|
#include <kinematics.h> /* these decls */
|
||||||
|
|
||||||
|
#define d2r(d) ((d)*PM_PI/180.0)
|
||||||
|
#define r2d(r) ((r)*180.0/PM_PI)
|
||||||
|
|
||||||
|
#ifndef hypot
|
||||||
|
#define hypot(a,b) (sqrt((a)*(a)+(b)*(b)))
|
||||||
|
#endif
|
||||||
|
|
||||||
|
struct haldata {
|
||||||
|
hal_float_t *pivot_length;
|
||||||
|
hal_bit_t *conventional_directions; //default is false
|
||||||
|
} *haldata;
|
||||||
|
|
||||||
|
int kinematicsForward(const double *joints,
|
||||||
|
EmcPose * pos,
|
||||||
|
const KINEMATICS_FORWARD_FLAGS * fflags,
|
||||||
|
KINEMATICS_INVERSE_FLAGS * iflags)
|
||||||
|
{
|
||||||
|
(void)fflags;
|
||||||
|
(void)iflags;
|
||||||
|
|
||||||
|
const real_t con = *(haldata->conventional_directions) ? 1.0 : -1.0;
|
||||||
|
|
||||||
|
// B correction
|
||||||
|
const double zb = (*(haldata->pivot_length) + joints[8]) * cos(d2r(joints[4]));
|
||||||
|
const double xb = (*(haldata->pivot_length) + joints[8]) * sin(d2r(joints[4]));
|
||||||
|
|
||||||
|
// C correction
|
||||||
|
const double xyr = hypot(joints[0], joints[1]);
|
||||||
|
const double xytheta = atan2(joints[1], joints[0]) + d2r(joints[5]);
|
||||||
|
|
||||||
|
// U correction
|
||||||
|
const double zv = joints[6] * sin(d2r(joints[4]));
|
||||||
|
const double xv = joints[6] * cos(d2r(joints[4]));
|
||||||
|
|
||||||
|
// V correction is always in joint 1 only
|
||||||
|
|
||||||
|
pos->tran.x = xyr * cos(xytheta) - (con * xb) - xv;
|
||||||
|
pos->tran.y = xyr * sin(xytheta) - joints[7];
|
||||||
|
pos->tran.z = joints[2] - zb - (con * zv) + *(haldata->pivot_length);
|
||||||
|
|
||||||
|
pos->a = joints[3];
|
||||||
|
pos->b = joints[4];
|
||||||
|
pos->c = joints[5];
|
||||||
|
pos->u = joints[6];
|
||||||
|
pos->v = joints[7];
|
||||||
|
pos->w = joints[8];
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
int kinematicsInverse(const EmcPose * pos,
|
||||||
|
double *joints,
|
||||||
|
const KINEMATICS_INVERSE_FLAGS * iflags,
|
||||||
|
KINEMATICS_FORWARD_FLAGS * fflags)
|
||||||
|
{
|
||||||
|
(void)iflags;
|
||||||
|
(void)fflags;
|
||||||
|
|
||||||
|
const real_t con = *(haldata->conventional_directions) ? 1.0 : -1.0;
|
||||||
|
|
||||||
|
// B correction
|
||||||
|
const double zb = (*(haldata->pivot_length) + pos->w) * cos(d2r(pos->b));
|
||||||
|
const double xb = (*(haldata->pivot_length) + pos->w) * sin(d2r(pos->b));
|
||||||
|
|
||||||
|
// C correction
|
||||||
|
const double xyr = hypot(pos->tran.x, pos->tran.y);
|
||||||
|
const double xytheta = atan2(pos->tran.y, pos->tran.x) - d2r(pos->c);
|
||||||
|
|
||||||
|
// U correction
|
||||||
|
const double zv = pos->u * sin(d2r(pos->b));
|
||||||
|
const double xv = pos->u * cos(d2r(pos->b));
|
||||||
|
|
||||||
|
// V correction is always in joint 1 only
|
||||||
|
|
||||||
|
joints[0] = xyr * cos(xytheta) + (con * xb) + xv;
|
||||||
|
joints[1] = xyr * sin(xytheta) + pos->v;
|
||||||
|
joints[2] = pos->tran.z + zb - (con * zv) - *(haldata->pivot_length);
|
||||||
|
|
||||||
|
joints[3] = pos->a;
|
||||||
|
joints[4] = pos->b;
|
||||||
|
joints[5] = pos->c;
|
||||||
|
joints[6] = pos->u;
|
||||||
|
joints[7] = pos->v;
|
||||||
|
joints[8] = pos->w;
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
KINEMATICS_TYPE kinematicsType()
|
||||||
|
{
|
||||||
|
return KINEMATICS_BOTH;
|
||||||
|
}
|
||||||
|
|
||||||
|
KINS_NOT_SWITCHABLE
|
||||||
|
EXPORT_SYMBOL(kinematicsType);
|
||||||
|
EXPORT_SYMBOL(kinematicsInverse);
|
||||||
|
EXPORT_SYMBOL(kinematicsForward);
|
||||||
|
MODULE_LICENSE("GPL");
|
||||||
|
|
||||||
|
int comp_id;
|
||||||
|
int rtapi_app_main(void) {
|
||||||
|
int result;
|
||||||
|
comp_id = hal_init("maxkins");
|
||||||
|
if(comp_id < 0) return comp_id;
|
||||||
|
|
||||||
|
haldata = hal_malloc(sizeof(struct haldata));
|
||||||
|
|
||||||
|
result = hal_pin_float_new("maxkins.pivot-length", HAL_IO, &(haldata->pivot_length), comp_id);
|
||||||
|
|
||||||
|
result += hal_pin_bit_new("maxkins.conventional-directions", HAL_IN, &(haldata->conventional_directions), comp_id);
|
||||||
|
|
||||||
|
if(result < 0) goto error;
|
||||||
|
|
||||||
|
*(haldata->pivot_length) = 0.666;
|
||||||
|
*(haldata->conventional_directions) = 0; // default is unconventional
|
||||||
|
hal_ready(comp_id);
|
||||||
|
return 0;
|
||||||
|
|
||||||
|
error:
|
||||||
|
hal_exit(comp_id);
|
||||||
|
return result;
|
||||||
|
}
|
||||||
|
|
||||||
|
void rtapi_app_exit(void) { hal_exit(comp_id); }
|
||||||
143
wasm-port/vendor/linuxcnc/src/emc/kinematics/rosekins.c
vendored
Normal file
143
wasm-port/vendor/linuxcnc/src/emc/kinematics/rosekins.c
vendored
Normal file
@@ -0,0 +1,143 @@
|
|||||||
|
/*
|
||||||
|
Copyright 2016 Dewey Garrett <dgarrett@panix.com>
|
||||||
|
|
||||||
|
This program is free software; you can redistribute it and/or modify
|
||||||
|
it under the terms of the GNU General Public License as published by
|
||||||
|
the Free Software Foundation; either version 2 of the License, or
|
||||||
|
(at your option) any later version.
|
||||||
|
|
||||||
|
This program is distributed in the hope that it will be useful,
|
||||||
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||||
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||||
|
GNU General Public License for more details.
|
||||||
|
|
||||||
|
You should have received a copy of the GNU General Public License
|
||||||
|
along with this program; if not, write to the Free Software
|
||||||
|
Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <rtapi.h>
|
||||||
|
#include <rtapi_math.h>
|
||||||
|
#include <rtapi_app.h>
|
||||||
|
#include <hal.h>
|
||||||
|
#include <kinematics.h>
|
||||||
|
|
||||||
|
KINS_NOT_SWITCHABLE
|
||||||
|
EXPORT_SYMBOL(kinematicsType);
|
||||||
|
EXPORT_SYMBOL(kinematicsInverse);
|
||||||
|
EXPORT_SYMBOL(kinematicsForward);
|
||||||
|
MODULE_LICENSE("GPL");
|
||||||
|
|
||||||
|
#ifndef hypot
|
||||||
|
#define hypot(a,b) (sqrt((a)*(a)+(b)*(b)))
|
||||||
|
#endif
|
||||||
|
|
||||||
|
struct haldata {
|
||||||
|
hal_float_t *revolutions;
|
||||||
|
hal_float_t *theta_degrees;
|
||||||
|
hal_float_t *bigtheta_degrees;
|
||||||
|
} *haldata;
|
||||||
|
|
||||||
|
int kinematicsForward(const double *joints,
|
||||||
|
EmcPose * pos,
|
||||||
|
const KINEMATICS_FORWARD_FLAGS * fflags,
|
||||||
|
KINEMATICS_INVERSE_FLAGS * iflags)
|
||||||
|
{
|
||||||
|
(void)fflags;
|
||||||
|
(void)iflags;
|
||||||
|
double radius,z,theta;
|
||||||
|
|
||||||
|
radius = joints[0];
|
||||||
|
z = joints[1];
|
||||||
|
theta = TO_RAD * joints[2];
|
||||||
|
|
||||||
|
pos->tran.x = radius * cos(theta);
|
||||||
|
pos->tran.y = radius * sin(theta);
|
||||||
|
pos->tran.z = z;
|
||||||
|
pos->a = 0;
|
||||||
|
pos->b = 0;
|
||||||
|
pos->c = 0;
|
||||||
|
pos->u = 0;
|
||||||
|
pos->v = 0;
|
||||||
|
pos->w = 0;
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
int kinematicsInverse(const EmcPose * pos,
|
||||||
|
double *joints,
|
||||||
|
const KINEMATICS_INVERSE_FLAGS * iflags,
|
||||||
|
KINEMATICS_FORWARD_FLAGS * fflags)
|
||||||
|
{
|
||||||
|
(void)iflags;
|
||||||
|
(void)fflags;
|
||||||
|
// There is a potential problem when accumulating bigtheta -- loss of
|
||||||
|
// precision based on size of mantissa -- but in practice, it is probably ok
|
||||||
|
|
||||||
|
static int oldquad;
|
||||||
|
static int revolutions;
|
||||||
|
|
||||||
|
double theta,bigtheta;
|
||||||
|
int nowquad = 0;
|
||||||
|
double x = pos->tran.x;
|
||||||
|
double y = pos->tran.y;
|
||||||
|
double z = pos->tran.z;
|
||||||
|
|
||||||
|
if (x >= 0 && y >= 0) nowquad = 1;
|
||||||
|
else if (x < 0 && y >= 0) nowquad = 2;
|
||||||
|
else if (x < 0 && y < 0) nowquad = 3;
|
||||||
|
else if (x >= 0 && y < 0) nowquad = 4;
|
||||||
|
|
||||||
|
if (oldquad == 2 && nowquad == 3) {revolutions += 1;}
|
||||||
|
if (oldquad == 3 && nowquad == 2) {revolutions -= 1;}
|
||||||
|
|
||||||
|
theta = atan2(y,x);
|
||||||
|
bigtheta = theta + PM_2_PI * revolutions;
|
||||||
|
|
||||||
|
*(haldata->revolutions) = revolutions;
|
||||||
|
*(haldata->theta_degrees) = theta * TO_DEG;
|
||||||
|
*(haldata->bigtheta_degrees) = bigtheta * TO_DEG;
|
||||||
|
|
||||||
|
joints[0] = hypot(x,y);
|
||||||
|
joints[1] = z;
|
||||||
|
joints[2] = TO_DEG * bigtheta;
|
||||||
|
joints[3] = 0;
|
||||||
|
joints[4] = 0;
|
||||||
|
joints[5] = 0;
|
||||||
|
joints[6] = 0;
|
||||||
|
joints[7] = 0;
|
||||||
|
joints[8] = 0;
|
||||||
|
|
||||||
|
oldquad = nowquad;
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
KINEMATICS_TYPE kinematicsType()
|
||||||
|
{
|
||||||
|
return KINEMATICS_BOTH;
|
||||||
|
}
|
||||||
|
|
||||||
|
static int comp_id;
|
||||||
|
|
||||||
|
void rtapi_app_exit(void) { hal_exit(comp_id); }
|
||||||
|
|
||||||
|
int rtapi_app_main(void) {
|
||||||
|
int ans;
|
||||||
|
comp_id = hal_init("rosekins");
|
||||||
|
if(comp_id < 0) return comp_id;
|
||||||
|
|
||||||
|
haldata = hal_malloc(sizeof(struct haldata));
|
||||||
|
|
||||||
|
if((ans = hal_pin_float_new("rosekins.revolutions",
|
||||||
|
HAL_OUT, &(haldata->revolutions), comp_id)) < 0) goto error;
|
||||||
|
if((ans = hal_pin_float_new("rosekins.theta_degrees",
|
||||||
|
HAL_OUT, &(haldata->theta_degrees), comp_id)) < 0) goto error;
|
||||||
|
if((ans = hal_pin_float_new("rosekins.bigtheta_degrees",
|
||||||
|
HAL_OUT, &(haldata->bigtheta_degrees), comp_id)) < 0) goto error;
|
||||||
|
|
||||||
|
hal_ready(comp_id);
|
||||||
|
return 0;
|
||||||
|
|
||||||
|
error:
|
||||||
|
return ans;
|
||||||
|
}
|
||||||
96
wasm-port/vendor/linuxcnc/src/emc/kinematics/rotatekins.c
vendored
Normal file
96
wasm-port/vendor/linuxcnc/src/emc/kinematics/rotatekins.c
vendored
Normal file
@@ -0,0 +1,96 @@
|
|||||||
|
/********************************************************************
|
||||||
|
* Description: rotatekins.c
|
||||||
|
* Simple example kinematics for a rotary table in software
|
||||||
|
*
|
||||||
|
* Derived from a work by Fred Proctor & Will Shackleford
|
||||||
|
*
|
||||||
|
* Author: Chris Radek
|
||||||
|
* License: GPL Version 2
|
||||||
|
* System: Linux
|
||||||
|
*
|
||||||
|
* Copyright (c) 2006 All rights reserved.
|
||||||
|
*
|
||||||
|
********************************************************************/
|
||||||
|
|
||||||
|
#include <rtapi.h>
|
||||||
|
#include <rtapi_app.h>
|
||||||
|
#include <rtapi_math.h>
|
||||||
|
#include <hal.h>
|
||||||
|
#include <kinematics.h> /* these decls */
|
||||||
|
|
||||||
|
int kinematicsForward(const double *joints,
|
||||||
|
EmcPose * pos,
|
||||||
|
const KINEMATICS_FORWARD_FLAGS * fflags,
|
||||||
|
KINEMATICS_INVERSE_FLAGS * iflags)
|
||||||
|
{
|
||||||
|
(void)fflags;
|
||||||
|
(void)iflags;
|
||||||
|
double c_rad = -joints[5]*M_PI/180;
|
||||||
|
pos->tran.x = joints[0] * cos(c_rad) - joints[1] * sin(c_rad);
|
||||||
|
pos->tran.y = joints[0] * sin(c_rad) + joints[1] * cos(c_rad);
|
||||||
|
pos->tran.z = joints[2];
|
||||||
|
pos->a = joints[3];
|
||||||
|
pos->b = joints[4];
|
||||||
|
pos->c = joints[5];
|
||||||
|
pos->u = joints[6];
|
||||||
|
pos->v = joints[7];
|
||||||
|
pos->w = joints[8];
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
int kinematicsInverse(const EmcPose * pos,
|
||||||
|
double *joints,
|
||||||
|
const KINEMATICS_INVERSE_FLAGS * iflags,
|
||||||
|
KINEMATICS_FORWARD_FLAGS * fflags)
|
||||||
|
{
|
||||||
|
(void)iflags;
|
||||||
|
(void)fflags;
|
||||||
|
double c_rad = pos->c*M_PI/180;
|
||||||
|
joints[0] = pos->tran.x * cos(c_rad) - pos->tran.y * sin(c_rad);
|
||||||
|
joints[1] = pos->tran.x * sin(c_rad) + pos->tran.y * cos(c_rad);
|
||||||
|
joints[2] = pos->tran.z;
|
||||||
|
joints[3] = pos->a;
|
||||||
|
joints[4] = pos->b;
|
||||||
|
joints[5] = pos->c;
|
||||||
|
joints[6] = pos->u;
|
||||||
|
joints[7] = pos->v;
|
||||||
|
joints[8] = pos->w;
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* implemented for these kinematics as giving joints preference */
|
||||||
|
int kinematicsHome(EmcPose * world,
|
||||||
|
double *joint,
|
||||||
|
KINEMATICS_FORWARD_FLAGS * fflags,
|
||||||
|
KINEMATICS_INVERSE_FLAGS * iflags)
|
||||||
|
{
|
||||||
|
*fflags = 0;
|
||||||
|
*iflags = 0;
|
||||||
|
|
||||||
|
return kinematicsForward(joint, world, fflags, iflags);
|
||||||
|
}
|
||||||
|
|
||||||
|
KINEMATICS_TYPE kinematicsType()
|
||||||
|
{
|
||||||
|
return KINEMATICS_BOTH;
|
||||||
|
}
|
||||||
|
|
||||||
|
KINS_NOT_SWITCHABLE
|
||||||
|
EXPORT_SYMBOL(kinematicsType);
|
||||||
|
EXPORT_SYMBOL(kinematicsForward);
|
||||||
|
EXPORT_SYMBOL(kinematicsInverse);
|
||||||
|
MODULE_LICENSE("GPL");
|
||||||
|
|
||||||
|
int comp_id;
|
||||||
|
int rtapi_app_main(void) {
|
||||||
|
comp_id = hal_init("rotatekins");
|
||||||
|
if(comp_id > 0) {
|
||||||
|
hal_ready(comp_id);
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
return comp_id;
|
||||||
|
}
|
||||||
|
|
||||||
|
void rtapi_app_exit(void) { hal_exit(comp_id); }
|
||||||
Reference in New Issue
Block a user