按推荐建议，继续执行
结论：已将 LinuxCNC 5axiskins 及 switchkins 相关源码纳入 vendored manifest、native probe 和文档验证链，完整 native 验证通过。
This commit is contained in:
@@ -53,6 +53,7 @@ The validation fails if:
|
||||
| `linuxcnc_parameter_file_harness` | Validates LinuxCNC parameter file restore/save behavior and required/read-only parameter handling. |
|
||||
| `linuxcnc_tp_api_probe` | Validates vendored LinuxCNC trajectory planner calls for linear, arc, and queued motion paths. |
|
||||
| `linuxcnc_kinematics_probe` | Validates vendored LinuxCNC `trivkins.c` plus `kins_util.c` initialize and perform identity forward/inverse mapping through the standalone HAL/RTAPI boundary. |
|
||||
| `linuxcnc_5axis_kinematics_probe` | Validates vendored LinuxCNC `5axiskins.c` through `switchkins.c`, including 5-axis forward/inverse round-trip behavior and switching to identity kinematics. |
|
||||
|
||||
## Fixture Coverage
|
||||
|
||||
@@ -95,5 +96,7 @@ The current fixture expectations validate standalone behavior against the
|
||||
vendored LinuxCNC source path. They do not yet run a side-by-side native
|
||||
LinuxCNC executable comparison for each fixture.
|
||||
|
||||
The kinematics probe currently covers LinuxCNC identity/trivial kinematics
|
||||
only. Non-trivial and 5-axis kinematics still need dedicated machine baselines.
|
||||
The kinematics probes currently cover LinuxCNC identity/trivial kinematics and
|
||||
the switchable `5axiskins` XYZBCW bridge-mill model. TRT `xyzac`/`xyzbc`,
|
||||
serial, delta, SCARA, and other machine-specific kinematics still need
|
||||
dedicated baselines.
|
||||
|
||||
@@ -29,7 +29,7 @@ semantic rewrites:
|
||||
| Python/remap | Python/remap hooks are stubbed at the runtime edge. |
|
||||
| Dynamic interpreter path | `interp_base.cc` probe uses standalone `EMC2_HOME` compile-time path boundary. |
|
||||
| Realtime scheduler | TP probes seed deterministic status/config data instead of running LinuxCNC realtime process topology. |
|
||||
| Kinematics component lifecycle | `trivkins.c` is initialized through its LinuxCNC module entry point while HAL component init/ready/exit and RTAPI module metadata are handled by standalone shims. |
|
||||
| Kinematics component lifecycle | `trivkins.c` and `5axiskins.c` are initialized through LinuxCNC module entry points while HAL component init/ready/exit, HAL pin allocation, and RTAPI module metadata are handled by standalone shims. |
|
||||
| Browser storage | OPFS remains outside the native core and is not yet connected. |
|
||||
|
||||
## Enforced Non-Drift Rules
|
||||
@@ -47,10 +47,11 @@ semantic rewrites:
|
||||
- No browser/WASM parity tests yet.
|
||||
- No JS SDK validation yet.
|
||||
- No OPFS persistence validation yet.
|
||||
- Identity/trivial kinematics now have native source-probe coverage; non-trivial
|
||||
and 5-axis kinematics implementation files are not yet extracted.
|
||||
- Full machine baselines beyond the identity/trivial kinematics probe are not
|
||||
established.
|
||||
- Identity/trivial kinematics and LinuxCNC `5axiskins` now have native
|
||||
source-probe coverage; TRT `xyzac`/`xyzbc`, serial, delta, SCARA, and other
|
||||
machine-specific kinematics are not yet extracted.
|
||||
- Full machine baselines beyond the current identity/trivial and `5axiskins`
|
||||
probes are not established.
|
||||
- Fixture expectations are currently checked against the standalone vendored
|
||||
source path, not by running a native LinuxCNC binary for every fixture.
|
||||
|
||||
|
||||
@@ -60,5 +60,6 @@ Negative fixture coverage includes:
|
||||
- missing tool length offset
|
||||
|
||||
Machine baseline is still limited to standalone interpreter, trajectory
|
||||
planner, and LinuxCNC identity/trivial kinematics probes. Non-trivial
|
||||
kinematics and 5-axis machine baselines remain future work.
|
||||
planner, LinuxCNC identity/trivial kinematics, and the LinuxCNC `5axiskins`
|
||||
XYZBCW bridge-mill probe. Additional non-trivial kinematics and fuller machine
|
||||
baselines remain future work.
|
||||
|
||||
@@ -36,6 +36,7 @@ Current validation is intentionally mechanical:
|
||||
| Canon/NML-facing interpreter types | `src/emc/nml_intf/canon*.hh`, `emctool.h`, `interp_return.hh`, `motion_types.h`, `emcpose.*`, `emcpos.h`, `debugflags.h`, `src/emc/linuxcnc.h` | Copy unchanged | NML transport is not ported; wrappers provide only the status/type edges needed by standalone probes | Vendor byte sync, dependent source probes, `linuxcnc_tp_api_probe`, interpreter harnesses |
|
||||
| Motion state headers | `src/emc/motion/state_tag.h`, `emcmotcfg.h`, `simple_tp.h`, `motion.h`, `mot_priv.h`, `axis.h` | Copy unchanged | Realtime motion process is not ported; standalone probes seed the small motion status/config state required by TP calls | Vendor byte sync, `linuxcnc_tp_api_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` |
|
||||
| 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 |
|
||||
| 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 |
|
||||
@@ -59,7 +60,9 @@ Current validation is intentionally mechanical:
|
||||
|
||||
## Known Gaps
|
||||
|
||||
- Full non-trivial and 5-axis kinematics implementation files are not yet extracted.
|
||||
- Additional non-trivial kinematics implementation files, including TRT
|
||||
`xyzac`/`xyzbc`, serial, delta, SCARA, and other machine-specific modules,
|
||||
are not yet extracted.
|
||||
- Browser/WASM C ABI and JS SDK layers are not yet built for the full
|
||||
interpreter/planner core.
|
||||
- OPFS persistence is not yet connected to INI, tool table, parameter file, or
|
||||
|
||||
@@ -0,0 +1,108 @@
|
||||
#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_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.b, expected.b) &&
|
||||
near(actual.c, expected.c) &&
|
||||
near(actual.w, expected.w);
|
||||
}
|
||||
|
||||
int near_joints(const double *actual, const double *expected)
|
||||
{
|
||||
for (int index = 0; index < 6; ++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 << "_bcw="
|
||||
<< pose.b << ","
|
||||
<< pose.c << ","
|
||||
<< pose.w << "\n";
|
||||
}
|
||||
|
||||
void print_joints(const char *prefix, const double *joints)
|
||||
{
|
||||
std::cout << prefix << "_xyzbcw="
|
||||
<< 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 << "fiveaxis_init=" << init_rc << "\n";
|
||||
std::cout << "fiveaxis_type=" << kinematicsType() << "\n";
|
||||
std::cout << "fiveaxis_switchable=" << kinematicsSwitchable() << "\n";
|
||||
|
||||
double joints[EMCMOT_MAX_JOINTS]{};
|
||||
joints[0] = 10.0;
|
||||
joints[1] = 20.0;
|
||||
joints[2] = 30.0;
|
||||
joints[3] = 45.0;
|
||||
joints[4] = 30.0;
|
||||
joints[5] = 5.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 << "fiveaxis_forward=" << forward_rc << "\n";
|
||||
print_pose("fiveaxis_forward", forward_pose);
|
||||
|
||||
double inverse_joints[EMCMOT_MAX_JOINTS]{};
|
||||
const int inverse_rc = kinematicsInverse(&forward_pose, inverse_joints, &iflags, &fflags);
|
||||
std::cout << "fiveaxis_inverse=" << inverse_rc << "\n";
|
||||
print_joints("fiveaxis_inverse", inverse_joints);
|
||||
std::cout << "fiveaxis_roundtrip_joints=" << near_joints(inverse_joints, joints) << "\n";
|
||||
|
||||
const int switch_rc = kinematicsSwitch(1);
|
||||
std::cout << "fiveaxis_switch_identity=" << switch_rc << "\n";
|
||||
EmcPose identity_pose{};
|
||||
const int identity_forward_rc = kinematicsForward(joints, &identity_pose, &fflags, &iflags);
|
||||
std::cout << "fiveaxis_identity_forward=" << identity_forward_rc << "\n";
|
||||
print_pose("fiveaxis_identity", identity_pose);
|
||||
|
||||
EmcPose expected_identity{};
|
||||
expected_identity.tran.x = joints[0];
|
||||
expected_identity.tran.y = joints[1];
|
||||
expected_identity.tran.z = joints[2];
|
||||
expected_identity.b = joints[3];
|
||||
expected_identity.c = joints[4];
|
||||
expected_identity.w = joints[5];
|
||||
std::cout << "fiveaxis_identity_near=" << near_pose(identity_pose, expected_identity) << "\n";
|
||||
|
||||
rtapi_app_exit();
|
||||
return 0;
|
||||
}
|
||||
@@ -1,7 +1,11 @@
|
||||
#include "linuxcnc_hal_adapter.hh"
|
||||
|
||||
#include <cstdarg>
|
||||
#include <cstdio>
|
||||
#include <new>
|
||||
#include <string>
|
||||
#include <unordered_map>
|
||||
#include <vector>
|
||||
|
||||
namespace {
|
||||
|
||||
@@ -29,6 +33,12 @@ std::unordered_map<std::string, HalEntry> ¶ms()
|
||||
return values;
|
||||
}
|
||||
|
||||
std::vector<void *> &hal_allocations()
|
||||
{
|
||||
static std::vector<void *> values;
|
||||
return values;
|
||||
}
|
||||
|
||||
std::unordered_map<std::string, HalEntry> &values_for(standalone::HalValueKind kind)
|
||||
{
|
||||
switch (kind) {
|
||||
@@ -96,6 +106,150 @@ int hal_exit(int)
|
||||
return 0;
|
||||
}
|
||||
|
||||
void *hal_malloc(long int size)
|
||||
{
|
||||
if (size <= 0) {
|
||||
return nullptr;
|
||||
}
|
||||
void *ptr = ::operator new(static_cast<std::size_t>(size), std::nothrow);
|
||||
if (ptr) {
|
||||
hal_allocations().push_back(ptr);
|
||||
}
|
||||
return ptr;
|
||||
}
|
||||
|
||||
int hal_pin_bit_new(const char *, hal_pin_dir_t, hal_bit_t **data_ptr_addr, int)
|
||||
{
|
||||
if (!data_ptr_addr) {
|
||||
return -1;
|
||||
}
|
||||
*data_ptr_addr = new hal_bit_t{};
|
||||
return 0;
|
||||
}
|
||||
|
||||
int hal_pin_float_new(const char *, hal_pin_dir_t, hal_float_t **data_ptr_addr, int)
|
||||
{
|
||||
if (!data_ptr_addr) {
|
||||
return -1;
|
||||
}
|
||||
*data_ptr_addr = new hal_float_t{};
|
||||
return 0;
|
||||
}
|
||||
|
||||
int hal_pin_u32_new(const char *, hal_pin_dir_t, hal_u32_t **data_ptr_addr, int)
|
||||
{
|
||||
if (!data_ptr_addr) {
|
||||
return -1;
|
||||
}
|
||||
*data_ptr_addr = new hal_u32_t{};
|
||||
return 0;
|
||||
}
|
||||
|
||||
int hal_pin_s32_new(const char *, hal_pin_dir_t, hal_s32_t **data_ptr_addr, int)
|
||||
{
|
||||
if (!data_ptr_addr) {
|
||||
return -1;
|
||||
}
|
||||
*data_ptr_addr = new hal_s32_t{};
|
||||
return 0;
|
||||
}
|
||||
|
||||
int hal_pin_u64_new(const char *, hal_pin_dir_t, hal_u64_t **data_ptr_addr, int)
|
||||
{
|
||||
if (!data_ptr_addr) {
|
||||
return -1;
|
||||
}
|
||||
*data_ptr_addr = new hal_u64_t{};
|
||||
return 0;
|
||||
}
|
||||
|
||||
int hal_pin_s64_new(const char *, hal_pin_dir_t, hal_s64_t **data_ptr_addr, int)
|
||||
{
|
||||
if (!data_ptr_addr) {
|
||||
return -1;
|
||||
}
|
||||
*data_ptr_addr = new hal_s64_t{};
|
||||
return 0;
|
||||
}
|
||||
|
||||
template <typename T, int (*Create)(const char *, hal_pin_dir_t, T **, int)>
|
||||
int hal_pin_newf_impl(hal_pin_dir_t dir, T **data_ptr_addr, int comp_id, const char *fmt,
|
||||
va_list ap)
|
||||
{
|
||||
if (!fmt) {
|
||||
return -1;
|
||||
}
|
||||
char name[HAL_NAME_LEN * 2]{};
|
||||
vsnprintf(name, sizeof(name), fmt, ap);
|
||||
return Create(name, dir, data_ptr_addr, comp_id);
|
||||
}
|
||||
|
||||
int hal_pin_bit_newf(hal_pin_dir_t dir, hal_bit_t **data_ptr_addr, int comp_id,
|
||||
const char *fmt, ...)
|
||||
{
|
||||
va_list ap;
|
||||
va_start(ap, fmt);
|
||||
const int rc = hal_pin_newf_impl<hal_bit_t, hal_pin_bit_new>(
|
||||
dir, data_ptr_addr, comp_id, fmt, ap);
|
||||
va_end(ap);
|
||||
return rc;
|
||||
}
|
||||
|
||||
int hal_pin_float_newf(hal_pin_dir_t dir, hal_float_t **data_ptr_addr, int comp_id,
|
||||
const char *fmt, ...)
|
||||
{
|
||||
va_list ap;
|
||||
va_start(ap, fmt);
|
||||
const int rc = hal_pin_newf_impl<hal_float_t, hal_pin_float_new>(
|
||||
dir, data_ptr_addr, comp_id, fmt, ap);
|
||||
va_end(ap);
|
||||
return rc;
|
||||
}
|
||||
|
||||
int hal_pin_u32_newf(hal_pin_dir_t dir, hal_u32_t **data_ptr_addr, int comp_id,
|
||||
const char *fmt, ...)
|
||||
{
|
||||
va_list ap;
|
||||
va_start(ap, fmt);
|
||||
const int rc = hal_pin_newf_impl<hal_u32_t, hal_pin_u32_new>(
|
||||
dir, data_ptr_addr, comp_id, fmt, ap);
|
||||
va_end(ap);
|
||||
return rc;
|
||||
}
|
||||
|
||||
int hal_pin_s32_newf(hal_pin_dir_t dir, hal_s32_t **data_ptr_addr, int comp_id,
|
||||
const char *fmt, ...)
|
||||
{
|
||||
va_list ap;
|
||||
va_start(ap, fmt);
|
||||
const int rc = hal_pin_newf_impl<hal_s32_t, hal_pin_s32_new>(
|
||||
dir, data_ptr_addr, comp_id, fmt, ap);
|
||||
va_end(ap);
|
||||
return rc;
|
||||
}
|
||||
|
||||
int hal_pin_u64_newf(hal_pin_dir_t dir, hal_u64_t **data_ptr_addr, int comp_id,
|
||||
const char *fmt, ...)
|
||||
{
|
||||
va_list ap;
|
||||
va_start(ap, fmt);
|
||||
const int rc = hal_pin_newf_impl<hal_u64_t, hal_pin_u64_new>(
|
||||
dir, data_ptr_addr, comp_id, fmt, ap);
|
||||
va_end(ap);
|
||||
return rc;
|
||||
}
|
||||
|
||||
int hal_pin_s64_newf(hal_pin_dir_t dir, hal_s64_t **data_ptr_addr, int comp_id,
|
||||
const char *fmt, ...)
|
||||
{
|
||||
va_list ap;
|
||||
va_start(ap, fmt);
|
||||
const int rc = hal_pin_newf_impl<hal_s64_t, hal_pin_s64_new>(
|
||||
dir, data_ptr_addr, comp_id, fmt, ap);
|
||||
va_end(ap);
|
||||
return rc;
|
||||
}
|
||||
|
||||
int hal_get_pin_value_by_name(const char *name, hal_type_t *type, hal_data_u **ptr,
|
||||
bool *connected)
|
||||
{
|
||||
|
||||
@@ -2,6 +2,8 @@
|
||||
|
||||
#include <stdbool.h>
|
||||
|
||||
#include "rtapi.h"
|
||||
|
||||
#define HAL_NAME_LEN 64
|
||||
|
||||
typedef enum {
|
||||
@@ -14,6 +16,13 @@ typedef enum {
|
||||
HAL_U64
|
||||
} hal_type_t;
|
||||
|
||||
typedef enum {
|
||||
HAL_DIR_UNSPECIFIED = -1,
|
||||
HAL_IN = 16,
|
||||
HAL_OUT = 32,
|
||||
HAL_IO = (HAL_IN | HAL_OUT),
|
||||
} hal_pin_dir_t;
|
||||
|
||||
typedef union {
|
||||
bool b;
|
||||
double f;
|
||||
@@ -33,6 +42,19 @@ typedef unsigned long long hal_u64_t;
|
||||
int hal_init(const char *);
|
||||
int hal_ready(int);
|
||||
int hal_exit(int);
|
||||
void *hal_malloc(long int);
|
||||
int hal_pin_bit_new(const char *, hal_pin_dir_t, hal_bit_t **, int);
|
||||
int hal_pin_float_new(const char *, hal_pin_dir_t, hal_float_t **, int);
|
||||
int hal_pin_u32_new(const char *, hal_pin_dir_t, hal_u32_t **, int);
|
||||
int hal_pin_s32_new(const char *, hal_pin_dir_t, hal_s32_t **, int);
|
||||
int hal_pin_u64_new(const char *, hal_pin_dir_t, hal_u64_t **, int);
|
||||
int hal_pin_s64_new(const char *, hal_pin_dir_t, hal_s64_t **, int);
|
||||
int hal_pin_bit_newf(hal_pin_dir_t, hal_bit_t **, int, const char *, ...);
|
||||
int hal_pin_float_newf(hal_pin_dir_t, hal_float_t **, int, const char *, ...);
|
||||
int hal_pin_u32_newf(hal_pin_dir_t, hal_u32_t **, int, const char *, ...);
|
||||
int hal_pin_s32_newf(hal_pin_dir_t, hal_s32_t **, int, const char *, ...);
|
||||
int hal_pin_u64_newf(hal_pin_dir_t, hal_u64_t **, int, const char *, ...);
|
||||
int hal_pin_s64_newf(hal_pin_dir_t, hal_s64_t **, int, const char *, ...);
|
||||
int hal_get_pin_value_by_name(const char *, hal_type_t *, hal_data_u **, bool *);
|
||||
int hal_get_signal_value_by_name(const char *, hal_type_t *, hal_data_u **, bool *);
|
||||
int hal_get_param_value_by_name(const char *, hal_type_t *, hal_data_u **);
|
||||
|
||||
@@ -81,6 +81,8 @@ check_exitcode linuxcnc_tp_api_probe
|
||||
check_exitcode linuxcnc_tp_api_probe.run
|
||||
check_exitcode linuxcnc_kinematics_probe
|
||||
check_exitcode linuxcnc_kinematics_probe.run
|
||||
check_exitcode linuxcnc_5axis_kinematics_probe
|
||||
check_exitcode linuxcnc_5axis_kinematics_probe.run
|
||||
for name in \
|
||||
linuxcnc_tp_tp_source_probe \
|
||||
linuxcnc_tp_tc_source_probe \
|
||||
@@ -123,6 +125,9 @@ check_exitcode linuxcnc_parameter_file_harness.run
|
||||
check_exitcode linuxcnc_rs274_compile_probe
|
||||
check_exitcode linuxcnc_kins_util_source_probe
|
||||
check_exitcode linuxcnc_trivkins_source_probe
|
||||
check_exitcode linuxcnc_switchkins_source_probe
|
||||
check_exitcode linuxcnc_userkfuncs_source_probe
|
||||
check_exitcode linuxcnc_5axiskins_source_probe
|
||||
check_exitcode linuxcnc_interp_convert_source_probe
|
||||
check_exitcode linuxcnc_interp_read_source_probe
|
||||
check_exitcode linuxcnc_interp_check_source_probe
|
||||
@@ -218,6 +223,17 @@ grep -Fq "kinematics_inverse=0" "$KINEMATICS_STDOUT"
|
||||
grep -Fq "kinematics_inverse_xyz=10,20,30" "$KINEMATICS_STDOUT"
|
||||
grep -Fq "kinematics_inverse_abcuvw=40,50,60,70,80,90" "$KINEMATICS_STDOUT"
|
||||
|
||||
FIVEAXIS_KINEMATICS_STDOUT="$BUILD_DIR/linuxcnc_5axis_kinematics_probe.run.stdout.log"
|
||||
grep -Fq "fiveaxis_init=0" "$FIVEAXIS_KINEMATICS_STDOUT"
|
||||
grep -Fq "fiveaxis_type=4" "$FIVEAXIS_KINEMATICS_STDOUT"
|
||||
grep -Fq "fiveaxis_switchable=1" "$FIVEAXIS_KINEMATICS_STDOUT"
|
||||
grep -Fq "fiveaxis_forward=0" "$FIVEAXIS_KINEMATICS_STDOUT"
|
||||
grep -Fq "fiveaxis_inverse=0" "$FIVEAXIS_KINEMATICS_STDOUT"
|
||||
grep -Fq "fiveaxis_roundtrip_joints=1" "$FIVEAXIS_KINEMATICS_STDOUT"
|
||||
grep -Fq "fiveaxis_switch_identity=0" "$FIVEAXIS_KINEMATICS_STDOUT"
|
||||
grep -Fq "fiveaxis_identity_forward=0" "$FIVEAXIS_KINEMATICS_STDOUT"
|
||||
grep -Fq "fiveaxis_identity_near=1" "$FIVEAXIS_KINEMATICS_STDOUT"
|
||||
|
||||
check_fixture_output() {
|
||||
local fixture="$1"
|
||||
local expected="$2"
|
||||
|
||||
@@ -353,6 +353,15 @@ KINEMATICS_PROBE_SOURCES=(
|
||||
"$WRAP_DIR/linuxcnc_kinematics_probe.cpp"
|
||||
)
|
||||
|
||||
FIVEAXIS_KINEMATICS_PROBE_SOURCES=(
|
||||
"$VENDOR_DIR/src/emc/kinematics/kins_util.c"
|
||||
"$VENDOR_DIR/src/emc/kinematics/switchkins.c"
|
||||
"$VENDOR_DIR/src/emc/kinematics/userkfuncs.c"
|
||||
"$VENDOR_DIR/src/emc/kinematics/5axiskins.c"
|
||||
"$WRAP_DIR/linuxcnc_hal_adapter.cpp"
|
||||
"$WRAP_DIR/linuxcnc_5axis_kinematics_probe.cpp"
|
||||
)
|
||||
|
||||
TP_CORE_SOURCES=(
|
||||
"$VENDOR_DIR/src/emc/tp/tp.c"
|
||||
"$VENDOR_DIR/src/emc/tp/tc.c"
|
||||
@@ -533,6 +542,13 @@ build_binary_target \
|
||||
KINEMATICS_PROBE_SOURCES \
|
||||
NO_LINK_FLAGS
|
||||
|
||||
build_binary_target \
|
||||
linuxcnc_5axis_kinematics_probe \
|
||||
"$BUILD_DIR/linuxcnc_5axis_kinematics_probe" \
|
||||
TP_FLAGS \
|
||||
FIVEAXIS_KINEMATICS_PROBE_SOURCES \
|
||||
NO_LINK_FLAGS
|
||||
|
||||
if [[ "$(tr -d '[:space:]' < "$BUILD_DIR/linuxcnc_kinematics_probe.exitcode")" == "0" ]]; then
|
||||
set +e
|
||||
"$BUILD_DIR/linuxcnc_kinematics_probe" \
|
||||
@@ -548,6 +564,21 @@ else
|
||||
"$BUILD_DIR/linuxcnc_kinematics_probe.run.stderr.log"
|
||||
fi
|
||||
|
||||
if [[ "$(tr -d '[:space:]' < "$BUILD_DIR/linuxcnc_5axis_kinematics_probe.exitcode")" == "0" ]]; then
|
||||
set +e
|
||||
"$BUILD_DIR/linuxcnc_5axis_kinematics_probe" \
|
||||
>"$BUILD_DIR/linuxcnc_5axis_kinematics_probe.run.stdout.log" \
|
||||
2>"$BUILD_DIR/linuxcnc_5axis_kinematics_probe.run.stderr.log"
|
||||
FIVEAXIS_KINEMATICS_RUN_RC=$?
|
||||
set -e
|
||||
echo "$FIVEAXIS_KINEMATICS_RUN_RC" > "$BUILD_DIR/linuxcnc_5axis_kinematics_probe.run.exitcode"
|
||||
else
|
||||
rm -f \
|
||||
"$BUILD_DIR/linuxcnc_5axis_kinematics_probe.run.exitcode" \
|
||||
"$BUILD_DIR/linuxcnc_5axis_kinematics_probe.run.stdout.log" \
|
||||
"$BUILD_DIR/linuxcnc_5axis_kinematics_probe.run.stderr.log"
|
||||
fi
|
||||
|
||||
if [[ "$(tr -d '[:space:]' < "$BUILD_DIR/linuxcnc_tp_api_probe.exitcode")" == "0" ]]; then
|
||||
set +e
|
||||
"$BUILD_DIR/linuxcnc_tp_api_probe" \
|
||||
@@ -658,6 +689,24 @@ build_object_target \
|
||||
"$VENDOR_DIR/src/emc/kinematics/trivkins.c" \
|
||||
TP_FLAGS
|
||||
|
||||
build_object_target \
|
||||
linuxcnc_switchkins_source_probe \
|
||||
"$BUILD_DIR/linuxcnc_switchkins_source_probe.o" \
|
||||
"$VENDOR_DIR/src/emc/kinematics/switchkins.c" \
|
||||
TP_FLAGS
|
||||
|
||||
build_object_target \
|
||||
linuxcnc_userkfuncs_source_probe \
|
||||
"$BUILD_DIR/linuxcnc_userkfuncs_source_probe.o" \
|
||||
"$VENDOR_DIR/src/emc/kinematics/userkfuncs.c" \
|
||||
TP_FLAGS
|
||||
|
||||
build_object_target \
|
||||
linuxcnc_5axiskins_source_probe \
|
||||
"$BUILD_DIR/linuxcnc_5axiskins_source_probe.o" \
|
||||
"$VENDOR_DIR/src/emc/kinematics/5axiskins.c" \
|
||||
TP_FLAGS
|
||||
|
||||
build_object_target \
|
||||
linuxcnc_interp_convert_source_probe \
|
||||
"$BUILD_DIR/linuxcnc_interp_convert_source_probe.o" \
|
||||
|
||||
@@ -11,6 +11,7 @@ src/rtapi/rtapi_gfp.h
|
||||
src/rtapi/rtapi_math.h
|
||||
src/rtapi/rtapi_byteorder.h
|
||||
src/rtapi/rtapi_app.h
|
||||
src/rtapi/rtapi_ctype.h
|
||||
src/emc/nml_intf/emcpos.h
|
||||
src/emc/nml_intf/emcpose.h
|
||||
src/emc/nml_intf/emcpose.c
|
||||
@@ -32,6 +33,10 @@ src/emc/kinematics/kinematics.h
|
||||
src/emc/kinematics/cubic.h
|
||||
src/emc/kinematics/kins_util.c
|
||||
src/emc/kinematics/trivkins.c
|
||||
src/emc/kinematics/switchkins.h
|
||||
src/emc/kinematics/switchkins.c
|
||||
src/emc/kinematics/userkfuncs.c
|
||||
src/emc/kinematics/5axiskins.c
|
||||
src/emc/tp/tp.h
|
||||
src/emc/tp/tp_types.h
|
||||
src/emc/tp/tc.h
|
||||
|
||||
272
wasm-port/vendor/linuxcnc/src/emc/kinematics/5axiskins.c
vendored
Normal file
272
wasm-port/vendor/linuxcnc/src/emc/kinematics/5axiskins.c
vendored
Normal file
@@ -0,0 +1,272 @@
|
||||
/********************************************************************
|
||||
* Description: 5axiskins.c
|
||||
* kinematics for XYZBC 5 axis bridge mill
|
||||
*
|
||||
* Derived from a work by Fred Proctor & Will Shackleford
|
||||
*
|
||||
* Author:
|
||||
* License: GPL Version 2
|
||||
* System: Linux
|
||||
*
|
||||
* Copyright (c) 2007 Chris Radek
|
||||
*
|
||||
* Notes:
|
||||
* 1) pivot_length hal pin must agree with mechanical
|
||||
* design (including vismach simulation) and augmented
|
||||
* with current tool z offset
|
||||
* (typ: mechanical_pivot_length + motion.tooloffset.z)
|
||||
* 2) C axis: spherical coordinates aziumthal angle (t or theta)
|
||||
* projection of radius to xy plane
|
||||
* 3) B axis: spherical coordinates polar angle (p or phi)
|
||||
* wrt z axis
|
||||
* 4) W axis: tool motion. Negative values increase tool radial
|
||||
* motion example: drilling into body at b,c angles
|
||||
* 5) W axis motion is incorporated into the motion of the
|
||||
* joints used for X,Y,Z positioning and no motor or
|
||||
* hal pin connections are required for the joint specified
|
||||
* as JW. However, a joint must be configured for W to
|
||||
* support display of the W axis letter value for
|
||||
* complicated reasons. (motion/control.c computes joint
|
||||
* positions only for the number of configured kinematic
|
||||
* joints (NO_OF_KINS_JOINTS) and the joint positions
|
||||
* are needed to display axis letters via inverse
|
||||
* kinematics.
|
||||
* 6) If no coordinates module parameter is supplied, kins
|
||||
* will use the required coordinates XYZBCW mapped
|
||||
* to joints 0..5 in sequence.
|
||||
* 7) Multiple joints may be assigned to an axis letter
|
||||
* with the module coordinates parameter
|
||||
* 8) If a coordinates module parameter is supplied,
|
||||
* the kins will map coordinate letters in sequence
|
||||
* to joint numbers beginning with joint 0.
|
||||
* 9) Coordinates XYZBCW are required, AUV may be used
|
||||
* if specified with the coordinates parameter and will
|
||||
* be mapped one-to-one with the assigned joint.
|
||||
* 10) The direction of the tilt axis is the opposite of the
|
||||
* conventional axis direction. See
|
||||
* https://linuxcnc.org/docs/html/gcode/machining-center.html
|
||||
********************************************************************/
|
||||
|
||||
// non-required coordinates (A,U,V) can be set by using
|
||||
// the module coordinates parameter
|
||||
#define REQUIRED_COORDINATES "XYZBCW"
|
||||
|
||||
#define DEFAULT_PIVOT_LENGTH 250
|
||||
|
||||
#include <rtapi.h>
|
||||
#include <rtapi_math.h>
|
||||
#include <rtapi_string.h>
|
||||
#include <rtapi_ctype.h>
|
||||
#include <hal.h>
|
||||
#include <emcmotcfg.h>
|
||||
#include <kinematics.h>
|
||||
|
||||
#include "switchkins.h"
|
||||
|
||||
struct haldata {
|
||||
hal_float_t *pivot_length;
|
||||
} *haldata;
|
||||
static int fiveaxis_max_joints;
|
||||
|
||||
static PmCartesian s2r(double r, double t, double p) {
|
||||
// s2r: spherical coordinates to cartesian coordinates
|
||||
// r = length of vector
|
||||
// p=phi = angle of vector wrt z axis
|
||||
// t=theta = angle of vector projected onto xy plane
|
||||
// (projection length in xy plane is r*sin(p)
|
||||
PmCartesian c;
|
||||
t = TO_RAD*t; p = TO_RAD*p; // degrees to radians
|
||||
|
||||
c.x = r * sin(p) * cos(t);
|
||||
c.y = r * sin(p) * sin(t);
|
||||
c.z = r * cos(p);
|
||||
|
||||
return c;
|
||||
} //s2r()
|
||||
|
||||
// assignments of principal joints to axis letters:
|
||||
// (-1 means not defined (yet))
|
||||
static int JX = -1;
|
||||
static int JY = -1;
|
||||
static int JZ = -1;
|
||||
static int JA = -1;
|
||||
static int JB = -1;
|
||||
static int JC = -1;
|
||||
static int JU = -1;
|
||||
static int JV = -1;
|
||||
static int JW = -1;
|
||||
|
||||
static int fiveaxis_KinematicsForward(const double *joints,
|
||||
EmcPose * pos,
|
||||
const KINEMATICS_FORWARD_FLAGS * fflags,
|
||||
KINEMATICS_INVERSE_FLAGS * iflags)
|
||||
{
|
||||
(void)fflags;
|
||||
(void)iflags;
|
||||
PmCartesian r = s2r(*(haldata->pivot_length) + joints[JW],
|
||||
joints[JC],
|
||||
180.0 - joints[JB]);
|
||||
|
||||
// Note: 'principal' joints are used
|
||||
pos->tran.x = joints[JX] + r.x;
|
||||
pos->tran.y = joints[JY] + r.y;
|
||||
pos->tran.z = joints[JZ] + *(haldata->pivot_length) + r.z;
|
||||
pos->b = joints[JB];
|
||||
pos->c = joints[JC];
|
||||
pos->w = joints[JW];
|
||||
|
||||
// optional letters (specify with coordinates module parameter)
|
||||
pos->a = (JA != -1)? joints[JA] : 0;
|
||||
pos->u = (JU != -1)? joints[JU] : 0;
|
||||
pos->v = (JV != -1)? joints[JV] : 0;
|
||||
|
||||
return 0;
|
||||
} //fiveaxis_KinematicsForward()
|
||||
|
||||
static int fiveaxis_KinematicsInverse(const EmcPose * pos,
|
||||
double *joints,
|
||||
const KINEMATICS_INVERSE_FLAGS * iflags,
|
||||
KINEMATICS_FORWARD_FLAGS * fflags)
|
||||
{
|
||||
(void)iflags;
|
||||
(void)fflags;
|
||||
PmCartesian r = s2r(*(haldata->pivot_length) + pos->w,
|
||||
pos->c,
|
||||
180.0 - pos->b);
|
||||
|
||||
EmcPose P; // computed position
|
||||
P.tran.x = pos->tran.x - r.x;
|
||||
P.tran.y = pos->tran.y - r.y;
|
||||
P.tran.z = pos->tran.z - *(haldata->pivot_length) - r.z;
|
||||
|
||||
P.b = pos->b;
|
||||
P.c = pos->c;
|
||||
P.w = pos->w;
|
||||
|
||||
// optional letters (specify with coordinates module parameter)
|
||||
P.a = (JA != -1)? pos->a : 0;
|
||||
P.u = (JU != -1)? pos->u : 0;
|
||||
P.v = (JV != -1)? pos->v : 0;
|
||||
|
||||
// update joints with support for
|
||||
// multiple-joints per-coordinate letter:
|
||||
// based on computed position
|
||||
position_to_mapped_joints(fiveaxis_max_joints,
|
||||
&P,
|
||||
joints);
|
||||
return 0;
|
||||
} // fiveaxis_kinematicsInverse()
|
||||
|
||||
int fiveaxis_KinematicsSetup(const int comp_id,
|
||||
const char* coordinates,
|
||||
kparms* kp)
|
||||
{
|
||||
int result=0;
|
||||
int i,jno;
|
||||
int axis_idx_for_jno[EMCMOT_MAX_JOINTS];
|
||||
int minjoints = strlen(kp->required_coordinates);
|
||||
fiveaxis_max_joints = strlen(coordinates); // allow for dup coords
|
||||
|
||||
if (fiveaxis_max_joints > kp->max_joints) {
|
||||
rtapi_print_msg(RTAPI_MSG_ERR,
|
||||
"ERROR %s: coordinates=%s requires %d joints, max joints=%d\n",
|
||||
kp->kinsname,
|
||||
coordinates,
|
||||
fiveaxis_max_joints,
|
||||
kp->max_joints);
|
||||
goto error;
|
||||
}
|
||||
|
||||
if (map_coordinates_to_jnumbers(coordinates,
|
||||
kp->max_joints,
|
||||
kp->allow_duplicates,
|
||||
axis_idx_for_jno)) {
|
||||
goto error;
|
||||
}
|
||||
// require all chars in reqd_coordinates (order doesn't matter)
|
||||
for (i=0; i < minjoints; i++) {
|
||||
char reqd_char;
|
||||
reqd_char = *(kp->required_coordinates + i);
|
||||
if ( !strchr(coordinates,toupper(reqd_char))
|
||||
&& !strchr(coordinates,tolower(reqd_char)) ) {
|
||||
rtapi_print_msg(RTAPI_MSG_ERR,
|
||||
"ERROR %s:\nrequired coordinates:%s\n"
|
||||
"specified coordinates:%s\n",
|
||||
kp->kinsname, kp->required_coordinates, coordinates);
|
||||
goto error;
|
||||
}
|
||||
}
|
||||
// assign principal joint numbers (first found in coordinates map)
|
||||
// duplicates are handled by position_to_mapped_joints()
|
||||
for (jno=0; jno<EMCMOT_MAX_JOINTS; jno++) {
|
||||
if (axis_idx_for_jno[jno] == 0) {if (JX == -1) JX=jno;}
|
||||
if (axis_idx_for_jno[jno] == 1) {if (JY == -1) JY=jno;}
|
||||
if (axis_idx_for_jno[jno] == 2) {if (JZ == -1) JZ=jno;}
|
||||
if (axis_idx_for_jno[jno] == 3) {if (JA == -1) JA=jno;}
|
||||
if (axis_idx_for_jno[jno] == 4) {if (JB == -1) JB=jno;}
|
||||
if (axis_idx_for_jno[jno] == 5) {if (JC == -1) JC=jno;}
|
||||
if (axis_idx_for_jno[jno] == 6) {if (JU == -1) JU=jno;}
|
||||
if (axis_idx_for_jno[jno] == 7) {if (JV == -1) JV=jno;}
|
||||
if (axis_idx_for_jno[jno] == 8) {if (JW == -1) JW=jno;}
|
||||
}
|
||||
|
||||
haldata = hal_malloc(sizeof(struct haldata));
|
||||
|
||||
result = hal_pin_float_newf(HAL_IN,&(haldata->pivot_length),comp_id,
|
||||
"%s.pivot-length",kp->halprefix);
|
||||
if(result < 0) goto error;
|
||||
|
||||
*haldata->pivot_length = DEFAULT_PIVOT_LENGTH;
|
||||
|
||||
rtapi_print("Kinematics Module %s\n",__FILE__);
|
||||
rtapi_print(" module name = %s\n"
|
||||
" coordinates = %s Requires: [KINS]JOINTS>=%d\n"
|
||||
" sparm = %s\n",
|
||||
kp->kinsname,
|
||||
coordinates,fiveaxis_max_joints,
|
||||
kp->sparm?kp->sparm:"NOTSPECIFIED");
|
||||
rtapi_print(" default pivot-length = %.3f\n",*haldata->pivot_length);
|
||||
|
||||
return 0;
|
||||
|
||||
error:
|
||||
return -1;
|
||||
} // fiveaxis_KinematicsSetup()
|
||||
|
||||
int switchkinsSetup(kparms* kp,
|
||||
KS* kset0, KS* kset1, KS* kset2,
|
||||
KF* kfwd0, KF* kfwd1, KF* kfwd2,
|
||||
KI* kinv0, KI* kinv1, KI* kinv2
|
||||
)
|
||||
{
|
||||
kp->kinsname = "5axiskins"; // !!! must agree with filename
|
||||
kp->halprefix = "5axiskins"; // hal pin names
|
||||
kp->required_coordinates = REQUIRED_COORDINATES;
|
||||
kp->allow_duplicates = 1;
|
||||
kp->max_joints = EMCMOT_MAX_JOINTS;
|
||||
|
||||
if (kp->sparm && strstr(kp->sparm,"identityfirst")) {
|
||||
rtapi_print("\n!!! switchkins-type 0 is IDENTITY\n");
|
||||
*kset0 = identityKinematicsSetup;
|
||||
*kfwd0 = identityKinematicsForward;
|
||||
*kinv0 = identityKinematicsInverse;
|
||||
|
||||
*kset1 = fiveaxis_KinematicsSetup;
|
||||
*kfwd1 = fiveaxis_KinematicsForward;
|
||||
*kinv1 = fiveaxis_KinematicsInverse;
|
||||
} else {
|
||||
rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname);
|
||||
*kset0 = fiveaxis_KinematicsSetup;
|
||||
*kfwd0 = fiveaxis_KinematicsForward;
|
||||
*kinv0 = fiveaxis_KinematicsInverse;
|
||||
|
||||
*kset1 = identityKinematicsSetup;
|
||||
*kfwd1 = identityKinematicsForward;
|
||||
*kinv1 = identityKinematicsInverse;
|
||||
}
|
||||
*kset2 = userkKinematicsSetup;
|
||||
*kfwd2 = userkKinematicsForward;
|
||||
*kinv2 = userkKinematicsInverse;
|
||||
|
||||
return 0;
|
||||
} // switchkinsSetup()
|
||||
342
wasm-port/vendor/linuxcnc/src/emc/kinematics/switchkins.c
vendored
Normal file
342
wasm-port/vendor/linuxcnc/src/emc/kinematics/switchkins.c
vendored
Normal file
@@ -0,0 +1,342 @@
|
||||
/*
|
||||
Copyright 2019 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.
|
||||
*/
|
||||
|
||||
/* switchkins.c provide functions for switchable kins modules:
|
||||
* rtapi_app()
|
||||
* rtapi_exit()
|
||||
* kinematicsType()
|
||||
* kinematicsForward()
|
||||
* kinematicsInverse()
|
||||
* kinematicsSwitch()
|
||||
* kinematicsSwitchable()
|
||||
* Using modules must supply function: switchkinsSetup()
|
||||
*/
|
||||
#include <rtapi.h>
|
||||
#include <rtapi_app.h>
|
||||
#include <hal.h>
|
||||
#include <emcmotcfg.h>
|
||||
#include <kinematics.h>
|
||||
|
||||
#include "switchkins.h"
|
||||
|
||||
//*********************************************************************
|
||||
// kinematic functions (default=0 for err detection):
|
||||
static kparms kp; // kinematics parms (common all types)
|
||||
|
||||
static KF kfwd0 = 0; // 0==switchkins_type kinematics forward
|
||||
static KF kfwd1 = 0; // 1
|
||||
static KF kfwd2 = 0; // 2
|
||||
|
||||
static KI kinv0 = 0; // 0==switchkins_type kinematics inverse
|
||||
static KI kinv1 = 0; // 1
|
||||
static KI kinv2 = 0; // 2
|
||||
|
||||
static hal_u32_t switchkins_type;
|
||||
static struct swdata {
|
||||
hal_bit_t *kinstype_is_0;
|
||||
hal_bit_t *kinstype_is_1;
|
||||
hal_bit_t *kinstype_is_2;
|
||||
|
||||
hal_float_t *gui_x;
|
||||
hal_float_t *gui_y;
|
||||
hal_float_t *gui_z;
|
||||
hal_float_t *gui_a;
|
||||
hal_float_t *gui_b;
|
||||
hal_float_t *gui_c;
|
||||
} *swdata;
|
||||
|
||||
// Note: parallel kinematics (like genhexkins) often
|
||||
// use iterative method for Forward algorithm
|
||||
// and require an initial EmcPose.
|
||||
// If fwd_iterates_mask is set
|
||||
// then save/use the lastpose
|
||||
static int fwd_iterates[SWITCHKINS_MAX_TYPES] = {0};
|
||||
static bool use_lastpose[SWITCHKINS_MAX_TYPES] = {0};
|
||||
static EmcPose lastpose[SWITCHKINS_MAX_TYPES];
|
||||
|
||||
static void save_lastpose(int ktype, EmcPose* pos)
|
||||
{
|
||||
lastpose[ktype].tran.x = pos->tran.x;
|
||||
lastpose[ktype].tran.y = pos->tran.y;
|
||||
lastpose[ktype].tran.z = pos->tran.z;
|
||||
lastpose[ktype].a = pos->a;
|
||||
lastpose[ktype].b = pos->b;
|
||||
lastpose[ktype].c = pos->c;
|
||||
lastpose[ktype].u = pos->u;
|
||||
lastpose[ktype].v = pos->v;
|
||||
lastpose[ktype].w = pos->w;
|
||||
} // save_lastpose()
|
||||
|
||||
static void get_lastpose(int ktype, EmcPose* pos)
|
||||
{
|
||||
pos->tran.x = lastpose[ktype].tran.x;
|
||||
pos->tran.y = lastpose[ktype].tran.y;
|
||||
pos->tran.z = lastpose[ktype].tran.z;
|
||||
pos->a = lastpose[ktype].a;
|
||||
pos->b = lastpose[ktype].b;
|
||||
pos->c = lastpose[ktype].c;
|
||||
pos->u = lastpose[ktype].u;
|
||||
pos->v = lastpose[ktype].v;
|
||||
pos->w = lastpose[ktype].w;
|
||||
} // get_lastpose()
|
||||
|
||||
static int gui_forward_kins(const double *joints)
|
||||
{
|
||||
// the hexapod vismach gui uses these hal pins to
|
||||
// display platform position/orientation in both
|
||||
// genhexkins and identity kinematic types
|
||||
// (similar needs for many parallel kinemtic machines)
|
||||
int res;
|
||||
KINEMATICS_FORWARD_FLAGS fflags = 0;
|
||||
KINEMATICS_INVERSE_FLAGS iflags;
|
||||
switch (kp.gui_kinstype) {
|
||||
case 0: res = kfwd0(joints, &lastpose[0], &fflags, &iflags);break;
|
||||
case 1: res = kfwd1(joints, &lastpose[1], &fflags, &iflags);break;
|
||||
case 2: res = kfwd2(joints, &lastpose[2], &fflags, &iflags);break;
|
||||
default: rtapi_print_msg(RTAPI_MSG_ERR,
|
||||
"gui_forward_kins BAD gui_kinstype <%d>\n",
|
||||
kp.gui_kinstype);
|
||||
return -1;
|
||||
}
|
||||
*swdata->gui_x = lastpose[kp.gui_kinstype].tran.x;
|
||||
*swdata->gui_y = lastpose[kp.gui_kinstype].tran.y;
|
||||
*swdata->gui_z = lastpose[kp.gui_kinstype].tran.z;
|
||||
*swdata->gui_a = lastpose[kp.gui_kinstype].a;
|
||||
*swdata->gui_b = lastpose[kp.gui_kinstype].b;
|
||||
*swdata->gui_c = lastpose[kp.gui_kinstype].c;
|
||||
return res;
|
||||
} // gui_forward_kins
|
||||
|
||||
//*********************************************************************
|
||||
int kinematicsSwitchable() {return 1;}
|
||||
|
||||
int kinematicsSwitch(int new_switchkins_type)
|
||||
{
|
||||
int k;
|
||||
for (k=0; k< SWITCHKINS_MAX_TYPES; k++) { use_lastpose[k] = 0;}
|
||||
|
||||
switchkins_type = new_switchkins_type;
|
||||
switch (switchkins_type) {
|
||||
case 0: rtapi_print_msg(RTAPI_MSG_INFO,
|
||||
"kinematicsSwitch:TYPE0\n");
|
||||
*swdata->kinstype_is_0 = 1;
|
||||
*swdata->kinstype_is_1 = 0;
|
||||
*swdata->kinstype_is_2 = 0;
|
||||
break;
|
||||
case 1: rtapi_print_msg(RTAPI_MSG_INFO,
|
||||
"kinematicsSwitch:TYPE1\n");
|
||||
*swdata->kinstype_is_0 = 0;
|
||||
*swdata->kinstype_is_1 = 1;
|
||||
*swdata->kinstype_is_2 = 0;
|
||||
break;
|
||||
case 2: rtapi_print_msg(RTAPI_MSG_INFO,
|
||||
"kinematicsSwitch:TYPE2\n");
|
||||
*swdata->kinstype_is_0 = 0;
|
||||
*swdata->kinstype_is_1 = 0;
|
||||
*swdata->kinstype_is_2 = 1;
|
||||
break;
|
||||
default: rtapi_print_msg(RTAPI_MSG_ERR,
|
||||
"kinematicsSwitch:BAD VALUE <%d>\n",
|
||||
switchkins_type);
|
||||
*swdata->kinstype_is_1 = 0;
|
||||
*swdata->kinstype_is_0 = 0;
|
||||
*swdata->kinstype_is_2 = 0;
|
||||
return -1; // FAIL
|
||||
}
|
||||
if (fwd_iterates[switchkins_type]) {
|
||||
use_lastpose[switchkins_type] = 1; // restarting a kins types
|
||||
}
|
||||
return 0; // 0==> no error
|
||||
} // kinematicsSwitch()
|
||||
|
||||
int kinematicsForward(const double *joint,
|
||||
EmcPose * pos,
|
||||
const KINEMATICS_FORWARD_FLAGS * fflags,
|
||||
KINEMATICS_INVERSE_FLAGS * iflags)
|
||||
{
|
||||
int r;
|
||||
|
||||
if (fwd_iterates[switchkins_type] && use_lastpose[switchkins_type]) {
|
||||
// initialize iterative forward kins (ok for identity too)
|
||||
get_lastpose(switchkins_type,pos);
|
||||
use_lastpose[switchkins_type] = 0;
|
||||
}
|
||||
|
||||
switch (switchkins_type) {
|
||||
case 0: r = kfwd0(joint, pos, fflags, iflags); break;
|
||||
case 1: r = kfwd1(joint, pos, fflags, iflags); break;
|
||||
case 2: r = kfwd2(joint, pos, fflags, iflags); break;
|
||||
default: rtapi_print_msg(RTAPI_MSG_ERR,
|
||||
"switchkins: Forward BAD switchkins_type </%d>\n",
|
||||
switchkins_type);
|
||||
return -1;
|
||||
}
|
||||
if (fwd_iterates[switchkins_type]) {save_lastpose(switchkins_type,pos);}
|
||||
if (r) return r;
|
||||
|
||||
// gui.* pins created only if gui_kinstype>=0
|
||||
// consider alternate implementations for gui_forward_kins():
|
||||
// a) always call and use -1 to select default 0 type
|
||||
if (kp.gui_kinstype >=0) {
|
||||
// create gui pins for a vismach gui using the
|
||||
// kins type specified by kp.gui_kinstype;
|
||||
// currently the skgui pins are only needed for
|
||||
// the hexagui vismach program (as it needs
|
||||
// world coords for switchkin-types
|
||||
r = gui_forward_kins(joint);
|
||||
}
|
||||
|
||||
return r;
|
||||
} // kinematicsForward()
|
||||
|
||||
int kinematicsInverse(const EmcPose * pos,
|
||||
double *joint,
|
||||
const KINEMATICS_INVERSE_FLAGS * iflags,
|
||||
KINEMATICS_FORWARD_FLAGS * fflags)
|
||||
{
|
||||
int r;
|
||||
|
||||
switch (switchkins_type) {
|
||||
case 0: r = kinv0(pos, joint, iflags, fflags); break;
|
||||
case 1: r = kinv1(pos, joint, iflags, fflags); break;
|
||||
case 2: r = kinv2(pos, joint, iflags, fflags); break;
|
||||
default: rtapi_print_msg(RTAPI_MSG_ERR,
|
||||
"switchkins: Inverse BAD switchkins_type </%d>\n",
|
||||
switchkins_type);
|
||||
return -1;
|
||||
}
|
||||
return r;
|
||||
} // kinematicsInverse()
|
||||
|
||||
KINEMATICS_TYPE kinematicsType()
|
||||
{
|
||||
return KINEMATICS_BOTH;
|
||||
}
|
||||
|
||||
//*********************************************************************
|
||||
static char *coordinates;
|
||||
RTAPI_MP_STRING(coordinates, "Axes-to-joints-ordering");
|
||||
static char *sparm;
|
||||
RTAPI_MP_STRING(sparm, "switchkins module-specific parameter");
|
||||
|
||||
EXPORT_SYMBOL(kinematicsSwitchable);
|
||||
EXPORT_SYMBOL(kinematicsSwitch);
|
||||
EXPORT_SYMBOL(kinematicsType);
|
||||
EXPORT_SYMBOL(kinematicsForward);
|
||||
EXPORT_SYMBOL(kinematicsInverse);
|
||||
MODULE_LICENSE("GPL");
|
||||
|
||||
static int comp_id;
|
||||
//*********************************************************************
|
||||
int rtapi_app_main(void)
|
||||
{
|
||||
int i,res;
|
||||
char* emsg="other";
|
||||
|
||||
// defaults prior to switchkinsSetup() call
|
||||
kp.kinsname = NULL;
|
||||
kp.halprefix = NULL;
|
||||
kp.required_coordinates = "";
|
||||
kp.max_joints = 0; // Setup must supply
|
||||
kp.allow_duplicates = 0;
|
||||
kp.fwd_iterates_mask = 0;
|
||||
kp.gui_kinstype = -1; // negative means: not used
|
||||
|
||||
kp.sparm = sparm; // module parm passed to kins
|
||||
|
||||
KS ksetup0 = 0;
|
||||
KS ksetup1 = 0;
|
||||
KS ksetup2 = 0;
|
||||
|
||||
res = switchkinsSetup(&kp,
|
||||
&ksetup0, &ksetup1, &ksetup2,
|
||||
&kfwd0, &kfwd1, &kfwd2,
|
||||
&kinv0, &kinv1, &kinv2);
|
||||
if (res) {emsg="switchkinsSetp FAIL"; goto error;}
|
||||
|
||||
for (i=0; i < SWITCHKINS_MAX_TYPES; i++) {
|
||||
if (kp.fwd_iterates_mask & (1<<i)) {
|
||||
fwd_iterates[i] = 1;
|
||||
rtapi_print("switchkins-type %d: fwd_iterates\n",i);
|
||||
}
|
||||
}
|
||||
|
||||
if (!kp.kinsname) { emsg = "Missing kinsname"; goto error; }
|
||||
|
||||
if (!kp.halprefix) {
|
||||
kp.halprefix = kp.kinsname;
|
||||
rtapi_print("Missing halprefix, using \"%s\"\n",kp.halprefix);
|
||||
}
|
||||
|
||||
if (kp.max_joints <= 0 || kp.max_joints > EMCMOT_MAX_JOINTS) {
|
||||
emsg = "bogus max_joints"; goto error;
|
||||
}
|
||||
if (kp.gui_kinstype >= SWITCHKINS_MAX_TYPES) {
|
||||
emsg = "bogus gui_kinstype"; goto error;
|
||||
}
|
||||
|
||||
if (!ksetup0 || !ksetup1 || !ksetup2) {
|
||||
emsg = "Missing setup function"; goto error;
|
||||
}
|
||||
if (!kfwd0 || !kfwd1 || !kfwd2) {
|
||||
emsg = "Missing fwd functionn"; goto error;
|
||||
}
|
||||
if (!kinv0 || !kinv1 || !kinv2) {
|
||||
emsg = "Missing inv function"; goto error;
|
||||
}
|
||||
|
||||
comp_id = hal_init(kp.kinsname);
|
||||
if(comp_id < 0) goto error;
|
||||
|
||||
swdata = hal_malloc(sizeof(struct swdata));
|
||||
if (!swdata) goto error;
|
||||
|
||||
res += hal_pin_bit_new("kinstype.is-0", HAL_OUT, &(swdata->kinstype_is_0), comp_id);
|
||||
res += hal_pin_bit_new("kinstype.is-1", HAL_OUT, &(swdata->kinstype_is_1), comp_id);
|
||||
res += hal_pin_bit_new("kinstype.is-2", HAL_OUT, &(swdata->kinstype_is_2), comp_id);
|
||||
|
||||
if (kp.gui_kinstype >=0) {
|
||||
res += hal_pin_float_newf(HAL_IN, &swdata->gui_x, comp_id, "skgui.x");
|
||||
res += hal_pin_float_newf(HAL_IN, &swdata->gui_y, comp_id, "skgui.y");
|
||||
res += hal_pin_float_newf(HAL_IN, &swdata->gui_z, comp_id, "skgui.z");
|
||||
res += hal_pin_float_newf(HAL_IN, &swdata->gui_a, comp_id, "skgui.a");
|
||||
res += hal_pin_float_newf(HAL_IN, &swdata->gui_b, comp_id, "skgui.b");
|
||||
res += hal_pin_float_newf(HAL_IN, &swdata->gui_c, comp_id, "skgui.c");
|
||||
if (res) {emsg = "hal pin create fail";goto error;}
|
||||
}
|
||||
|
||||
switchkins_type = 0; // startup with default type
|
||||
kinematicsSwitch(switchkins_type);
|
||||
|
||||
if (!coordinates) {coordinates = kp.required_coordinates;}
|
||||
|
||||
ksetup0(comp_id,coordinates,&kp);
|
||||
ksetup1(comp_id,coordinates,&kp);
|
||||
ksetup2(comp_id,coordinates,&kp);
|
||||
|
||||
hal_ready(comp_id);
|
||||
return 0;
|
||||
|
||||
error:
|
||||
rtapi_print_msg(RTAPI_MSG_ERR,
|
||||
"\nSwitchkins FAIL %s:<%s>\n",kp.kinsname,emsg);
|
||||
hal_exit(comp_id);
|
||||
return -1;
|
||||
} // rtapi_app_main()
|
||||
|
||||
void rtapi_app_exit(void) { hal_exit(comp_id); }
|
||||
36
wasm-port/vendor/linuxcnc/src/emc/kinematics/switchkins.h
vendored
Normal file
36
wasm-port/vendor/linuxcnc/src/emc/kinematics/switchkins.h
vendored
Normal file
@@ -0,0 +1,36 @@
|
||||
/*
|
||||
** License GPL Version 2
|
||||
*/
|
||||
#ifndef SWITCHKINS_H // {
|
||||
#define SWITCHKINS_H
|
||||
|
||||
#include <kinematics.h>
|
||||
|
||||
//hardcoded number of switchkins types (KS,KF,KI):
|
||||
#define SWITCHKINS_MAX_TYPES 3
|
||||
|
||||
// KinematicsFORWARD functions
|
||||
typedef int (*KF)(const double *joint,
|
||||
EmcPose * pos,
|
||||
const KINEMATICS_FORWARD_FLAGS * fflags,
|
||||
KINEMATICS_INVERSE_FLAGS * iflags);
|
||||
|
||||
// KinematicsINVERSE functions
|
||||
typedef int (*KI)(const struct EmcPose * world,
|
||||
double *joint,
|
||||
const KINEMATICS_INVERSE_FLAGS * iflags,
|
||||
KINEMATICS_FORWARD_FLAGS * fflags);
|
||||
|
||||
// KinematicsSETUP functions
|
||||
typedef int (*KS)(const int comp_id, // halpins
|
||||
const char* coordinates, // module parameter
|
||||
kparms* ksetup_parms //
|
||||
);
|
||||
|
||||
//*********************************************************************
|
||||
extern int switchkinsSetup(kparms* ksetup_parms,
|
||||
KS* kset0, KS* kset1, KS* kset2,
|
||||
KF* kfwd0, KF* kfwd1, KF* kfwd2,
|
||||
KI* kinv0, KI* kinv1, KI* kinv2
|
||||
);
|
||||
#endif // }
|
||||
83
wasm-port/vendor/linuxcnc/src/emc/kinematics/userkfuncs.c
vendored
Normal file
83
wasm-port/vendor/linuxcnc/src/emc/kinematics/userkfuncs.c
vendored
Normal file
@@ -0,0 +1,83 @@
|
||||
/* userkfuncs.c: template file for a user set of
|
||||
** switchable kinematics functions.
|
||||
** License GPL Version 2
|
||||
**
|
||||
** Example Usage (for customizing the genser-switchkins module):
|
||||
** (works with rtpreempt only rtai --> Makefile needs work)
|
||||
**
|
||||
** LDIR is LinuxCNC git root directory
|
||||
** UDIR is user directory (not in LinuxCNC git tree)
|
||||
** 1) $ cp LDIR/src/emc/kinematics/userkfuncs.c UDIR/my_userk.c
|
||||
** 2) $ edit UDIR/my_userk.c as required
|
||||
** 3) $ source LDIR/scripts/rip-environment
|
||||
** 4) For genser-switchkins module use make command line option:
|
||||
** $ cd LDIR/src
|
||||
** $ userkfuncs=UDIR/my_userk.c make && sudo make setuid
|
||||
*/
|
||||
|
||||
// typical includes:
|
||||
//#include <rtapi_math.h> // if reqd
|
||||
#include <hal.h>
|
||||
#include <kinematics.h>
|
||||
|
||||
// Add for kins based on genserkins:
|
||||
// #include "genserkins.h" //includes gomath,hal
|
||||
|
||||
//**********************************************************************
|
||||
// static local variables and functions go here
|
||||
|
||||
static int userk_inited = 0;
|
||||
static struct udata {
|
||||
hal_s32_t *fct;
|
||||
hal_s32_t *ict;
|
||||
} *udata;
|
||||
|
||||
//**********************************************************************
|
||||
int userkKinematicsSetup(const int comp_id,
|
||||
const char* coordinates,
|
||||
kparms* kp)
|
||||
{
|
||||
int res=0;
|
||||
rtapi_print("\nuserkKinematicsSetup:\n"
|
||||
" %s <%s> max_joints=%d allow_duplicates=%d\n\n",
|
||||
__FILE__,coordinates,
|
||||
kp->max_joints,kp->allow_duplicates);
|
||||
|
||||
|
||||
udata = hal_malloc(sizeof(struct udata));
|
||||
if (!udata) goto error;
|
||||
|
||||
// HAL_IO used to allow resetting demo pins:
|
||||
res += hal_pin_s32_new("userk.fct", HAL_IO, &(udata->fct), comp_id);
|
||||
res += hal_pin_s32_new("userk.ict", HAL_IO, &(udata->ict), comp_id);
|
||||
if (res) goto error;
|
||||
|
||||
userk_inited = 1;
|
||||
return 0; // 0 ==> OK
|
||||
|
||||
error:
|
||||
return -1;
|
||||
}
|
||||
|
||||
int userkKinematicsForward(const double *joint,
|
||||
struct EmcPose * world,
|
||||
const KINEMATICS_FORWARD_FLAGS * fflags,
|
||||
KINEMATICS_INVERSE_FLAGS * iflags)
|
||||
{
|
||||
if (!userk_inited) {
|
||||
rtapi_print_msg(RTAPI_MSG_ERR,
|
||||
"userkKinematics: not initialized\n");
|
||||
return -1;
|
||||
}
|
||||
(*udata->fct)++;
|
||||
return identityKinematicsForward(joint,world,fflags,iflags);
|
||||
}
|
||||
|
||||
int userkKinematicsInverse(const EmcPose * pos,
|
||||
double *joint,
|
||||
const KINEMATICS_INVERSE_FLAGS * iflags,
|
||||
KINEMATICS_FORWARD_FLAGS * fflags)
|
||||
{
|
||||
(*udata->ict)++;
|
||||
return identityKinematicsInverse(pos,joint,iflags,fflags);
|
||||
}
|
||||
24
wasm-port/vendor/linuxcnc/src/rtapi/rtapi_ctype.h
vendored
Normal file
24
wasm-port/vendor/linuxcnc/src/rtapi/rtapi_ctype.h
vendored
Normal file
@@ -0,0 +1,24 @@
|
||||
#ifndef __LINUXCNC_RTAPI_CTYPE_H
|
||||
#define __LINUXCNC_RTAPI_CTYPE_H
|
||||
// Copyright 2006 Jeff Epler
|
||||
//
|
||||
// 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.
|
||||
#ifdef MODULE
|
||||
#include <linux/ctype.h>
|
||||
#else
|
||||
#include <ctype.h>
|
||||
#endif
|
||||
|
||||
#endif
|
||||
Reference in New Issue
Block a user