From 01495ee26b63fc11cdec82e5b091204c1d9097bf Mon Sep 17 00:00:00 2001 From: wangdequan Date: Sun, 7 Jun 2026 19:55:43 +0800 Subject: [PATCH] =?UTF-8?q?=E6=8C=89=E6=8E=A8=E8=8D=90=E5=BB=BA=E8=AE=AE?= =?UTF-8?q?=EF=BC=8C=E7=BB=A7=E7=BB=AD=E6=89=A7=E8=A1=8C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 结论：继续按 LinuxCNC 源码直接复用路线推进，新增 linear delta、rotary delta、scorbot 三类运动学源码的 vendored 同步、native 探针、source probe 覆盖和复用文档，并通过 native 验证。 --- wasm-port/docs/source-reuse-map.md | 4 +- .../linuxcnc_lineardelta_kinematics_probe.cpp | 93 +++++ .../linuxcnc_rotarydelta_kinematics_probe.cpp | 93 +++++ .../linuxcnc_scorbot_kinematics_probe.cpp | 82 +++++ .../tests/native/verify_native_probes.sh | 34 ++ wasm-port/tools/build_native_probes.sh | 102 ++++++ wasm-port/tools/source-manifest.txt | 5 + .../emc/kinematics/lineardeltakins-common.h | 140 ++++++++ .../src/emc/kinematics/lineardeltakins.c | 98 ++++++ .../emc/kinematics/rotarydeltakins-common.h | 195 +++++++++++ .../src/emc/kinematics/rotarydeltakins.c | 110 ++++++ .../src/emc/kinematics/scorbot-kins.c | 322 ++++++++++++++++++ 12 files changed, 1276 insertions(+), 2 deletions(-) create mode 100644 wasm-port/runtime/core/linuxcnc_wrap/linuxcnc_lineardelta_kinematics_probe.cpp create mode 100644 wasm-port/runtime/core/linuxcnc_wrap/linuxcnc_rotarydelta_kinematics_probe.cpp create mode 100644 wasm-port/runtime/core/linuxcnc_wrap/linuxcnc_scorbot_kinematics_probe.cpp create mode 100644 wasm-port/vendor/linuxcnc/src/emc/kinematics/lineardeltakins-common.h create mode 100644 wasm-port/vendor/linuxcnc/src/emc/kinematics/lineardeltakins.c create mode 100644 wasm-port/vendor/linuxcnc/src/emc/kinematics/rotarydeltakins-common.h create mode 100644 wasm-port/vendor/linuxcnc/src/emc/kinematics/rotarydeltakins.c create mode 100644 wasm-port/vendor/linuxcnc/src/emc/kinematics/scorbot-kins.c diff --git a/wasm-port/docs/source-reuse-map.md b/wasm-port/docs/source-reuse-map.md index 5a73827..e8bb98e 100644 --- a/wasm-port/docs/source-reuse-map.md +++ b/wasm-port/docs/source-reuse-map.md @@ -38,7 +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` | | 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` | -| 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` | +| Additional non-switchable kinematics | `src/emc/kinematics/corexykins.c`, `rotatekins.c`, `rosekins.c`, `maxkins.c`, `lineardeltakins.c`, `lineardeltakins-common.h`, `rotarydeltakins.c`, `rotarydeltakins-common.h`, `scorbot-kins.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`, `linuxcnc_lineardelta_kinematics_probe`, `linuxcnc_rotarydelta_kinematics_probe`, `linuxcnc_scorbot_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 | @@ -63,7 +63,7 @@ Current validation is intentionally mechanical: ## Known Gaps - Additional non-trivial kinematics implementation files, including serial, - delta, SCARA, hexapod, tripod, and other machine-specific modules, are not + SCARA, hexapod, tripod, puma, 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. diff --git a/wasm-port/runtime/core/linuxcnc_wrap/linuxcnc_lineardelta_kinematics_probe.cpp b/wasm-port/runtime/core/linuxcnc_wrap/linuxcnc_lineardelta_kinematics_probe.cpp new file mode 100644 index 0000000..49ae096 --- /dev/null +++ b/wasm-port/runtime/core/linuxcnc_wrap/linuxcnc_lineardelta_kinematics_probe.cpp @@ -0,0 +1,93 @@ +#include +#include + +#include "emc/kinematics/kinematics.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.a, expected.a) && + near(actual.b, expected.b) && + near(actual.c, expected.c) && + near(actual.u, expected.u) && + near(actual.v, expected.v) && + near(actual.w, expected.w); +} + +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 << "lineardelta_init=" << init_rc << "\n"; + std::cout << "lineardelta_type=" << kinematicsType() << "\n"; + std::cout << "lineardelta_switchable=" << kinematicsSwitchable() << "\n"; + + EmcPose pose{}; + pose.tran.x = 10.0; + pose.tran.y = 20.0; + pose.tran.z = -30.0; + pose.a = 1.0; + pose.b = 2.0; + pose.c = 3.0; + pose.u = 4.0; + pose.v = 5.0; + pose.w = 6.0; + + KINEMATICS_FORWARD_FLAGS fflags = 0; + KINEMATICS_INVERSE_FLAGS iflags = 0; + double inverse_joints[9]{}; + const int inverse_rc = kinematicsInverse(&pose, inverse_joints, &iflags, &fflags); + std::cout << "lineardelta_inverse=" << inverse_rc << "\n"; + print_joints("lineardelta_inverse", inverse_joints); + + EmcPose forward_pose{}; + const int forward_rc = kinematicsForward(inverse_joints, &forward_pose, &fflags, &iflags); + std::cout << "lineardelta_forward=" << forward_rc << "\n"; + print_pose("lineardelta_forward", forward_pose); + std::cout << "lineardelta_roundtrip_pose=" << near_pose(forward_pose, pose) << "\n"; + + rtapi_app_exit(); + return 0; +} diff --git a/wasm-port/runtime/core/linuxcnc_wrap/linuxcnc_rotarydelta_kinematics_probe.cpp b/wasm-port/runtime/core/linuxcnc_wrap/linuxcnc_rotarydelta_kinematics_probe.cpp new file mode 100644 index 0000000..da92e92 --- /dev/null +++ b/wasm-port/runtime/core/linuxcnc_wrap/linuxcnc_rotarydelta_kinematics_probe.cpp @@ -0,0 +1,93 @@ +#include +#include + +#include "emc/kinematics/kinematics.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.a, expected.a) && + near(actual.b, expected.b) && + near(actual.c, expected.c) && + near(actual.u, expected.u) && + near(actual.v, expected.v) && + near(actual.w, expected.w); +} + +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 << "rotarydelta_init=" << init_rc << "\n"; + std::cout << "rotarydelta_type=" << kinematicsType() << "\n"; + std::cout << "rotarydelta_switchable=" << kinematicsSwitchable() << "\n"; + + EmcPose pose{}; + pose.tran.x = 0.0; + pose.tran.y = 0.0; + pose.tran.z = -12.0; + pose.a = 1.0; + pose.b = 2.0; + pose.c = 3.0; + pose.u = 4.0; + pose.v = 5.0; + pose.w = 6.0; + + KINEMATICS_FORWARD_FLAGS fflags = 0; + KINEMATICS_INVERSE_FLAGS iflags = 0; + double inverse_joints[9]{}; + const int inverse_rc = kinematicsInverse(&pose, inverse_joints, &iflags, &fflags); + std::cout << "rotarydelta_inverse=" << inverse_rc << "\n"; + print_joints("rotarydelta_inverse", inverse_joints); + + EmcPose forward_pose{}; + const int forward_rc = kinematicsForward(inverse_joints, &forward_pose, &fflags, &iflags); + std::cout << "rotarydelta_forward=" << forward_rc << "\n"; + print_pose("rotarydelta_forward", forward_pose); + std::cout << "rotarydelta_roundtrip_pose=" << near_pose(forward_pose, pose) << "\n"; + + rtapi_app_exit(); + return 0; +} diff --git a/wasm-port/runtime/core/linuxcnc_wrap/linuxcnc_scorbot_kinematics_probe.cpp b/wasm-port/runtime/core/linuxcnc_wrap/linuxcnc_scorbot_kinematics_probe.cpp new file mode 100644 index 0000000..82260de --- /dev/null +++ b/wasm-port/runtime/core/linuxcnc_wrap/linuxcnc_scorbot_kinematics_probe.cpp @@ -0,0 +1,82 @@ +#include +#include + +#include "emc/kinematics/kinematics.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.a, expected.a) && + near(actual.b, expected.b); +} + +void print_pose(const char *prefix, const EmcPose &pose) +{ + std::cout << prefix << "_xyz=" + << pose.tran.x << "," + << pose.tran.y << "," + << pose.tran.z << "\n"; + std::cout << prefix << "_ab=" + << pose.a << "," + << pose.b << "\n"; +} + +void print_joints(const char *prefix, const double *joints) +{ + std::cout << prefix << "_xyzab=" + << joints[0] << "," + << joints[1] << "," + << joints[2] << "," + << joints[3] << "," + << joints[4] << "\n"; +} + +} // namespace + +int main() +{ + const int init_rc = rtapi_app_main(); + std::cout << "scorbot_init=" << init_rc << "\n"; + std::cout << "scorbot_type=" << kinematicsType() << "\n"; + std::cout << "scorbot_switchable=" << kinematicsSwitchable() << "\n"; + + double joints[9]{}; + joints[0] = 25.0; + joints[1] = 20.0; + joints[2] = 10.0; + joints[3] = 4.0; + joints[4] = 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 << "scorbot_forward=" << forward_rc << "\n"; + print_pose("scorbot_forward", forward_pose); + + double inverse_joints[9]{}; + const int inverse_rc = kinematicsInverse(&forward_pose, inverse_joints, &iflags, &fflags); + std::cout << "scorbot_inverse=" << inverse_rc << "\n"; + print_joints("scorbot_inverse", inverse_joints); + + EmcPose roundtrip_pose{}; + const int roundtrip_rc = kinematicsForward(inverse_joints, &roundtrip_pose, &fflags, &iflags); + std::cout << "scorbot_roundtrip_forward=" << roundtrip_rc << "\n"; + print_pose("scorbot_roundtrip", roundtrip_pose); + std::cout << "scorbot_roundtrip_pose=" << near_pose(roundtrip_pose, forward_pose) << "\n"; + + rtapi_app_exit(); + return 0; +} diff --git a/wasm-port/tests/native/verify_native_probes.sh b/wasm-port/tests/native/verify_native_probes.sh index f8eee75..910bf77 100755 --- a/wasm-port/tests/native/verify_native_probes.sh +++ b/wasm-port/tests/native/verify_native_probes.sh @@ -95,6 +95,12 @@ 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 +check_exitcode linuxcnc_lineardelta_kinematics_probe +check_exitcode linuxcnc_lineardelta_kinematics_probe.run +check_exitcode linuxcnc_rotarydelta_kinematics_probe +check_exitcode linuxcnc_rotarydelta_kinematics_probe.run +check_exitcode linuxcnc_scorbot_kinematics_probe +check_exitcode linuxcnc_scorbot_kinematics_probe.run for name in \ linuxcnc_tp_tp_source_probe \ linuxcnc_tp_tc_source_probe \ @@ -147,6 +153,9 @@ 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_lineardeltakins_source_probe +check_exitcode linuxcnc_rotarydeltakins_source_probe +check_exitcode linuxcnc_scorbot_kins_source_probe check_exitcode linuxcnc_interp_convert_source_probe check_exitcode linuxcnc_interp_read_source_probe check_exitcode linuxcnc_interp_check_source_probe @@ -307,6 +316,31 @@ 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" +LINEARDELTA_KINEMATICS_STDOUT="$BUILD_DIR/linuxcnc_lineardelta_kinematics_probe.run.stdout.log" +grep -Fq "lineardelta_init=0" "$LINEARDELTA_KINEMATICS_STDOUT" +grep -Fq "lineardelta_type=4" "$LINEARDELTA_KINEMATICS_STDOUT" +grep -Fq "lineardelta_switchable=0" "$LINEARDELTA_KINEMATICS_STDOUT" +grep -Fq "lineardelta_inverse=0" "$LINEARDELTA_KINEMATICS_STDOUT" +grep -Fq "lineardelta_forward=0" "$LINEARDELTA_KINEMATICS_STDOUT" +grep -Fq "lineardelta_roundtrip_pose=1" "$LINEARDELTA_KINEMATICS_STDOUT" + +ROTARYDELTA_KINEMATICS_STDOUT="$BUILD_DIR/linuxcnc_rotarydelta_kinematics_probe.run.stdout.log" +grep -Fq "rotarydelta_init=0" "$ROTARYDELTA_KINEMATICS_STDOUT" +grep -Fq "rotarydelta_type=4" "$ROTARYDELTA_KINEMATICS_STDOUT" +grep -Fq "rotarydelta_switchable=0" "$ROTARYDELTA_KINEMATICS_STDOUT" +grep -Fq "rotarydelta_inverse=0" "$ROTARYDELTA_KINEMATICS_STDOUT" +grep -Fq "rotarydelta_forward=0" "$ROTARYDELTA_KINEMATICS_STDOUT" +grep -Fq "rotarydelta_roundtrip_pose=1" "$ROTARYDELTA_KINEMATICS_STDOUT" + +SCORBOT_KINEMATICS_STDOUT="$BUILD_DIR/linuxcnc_scorbot_kinematics_probe.run.stdout.log" +grep -Fq "scorbot_init=0" "$SCORBOT_KINEMATICS_STDOUT" +grep -Fq "scorbot_type=4" "$SCORBOT_KINEMATICS_STDOUT" +grep -Fq "scorbot_switchable=0" "$SCORBOT_KINEMATICS_STDOUT" +grep -Fq "scorbot_forward=0" "$SCORBOT_KINEMATICS_STDOUT" +grep -Fq "scorbot_inverse=0" "$SCORBOT_KINEMATICS_STDOUT" +grep -Fq "scorbot_roundtrip_forward=0" "$SCORBOT_KINEMATICS_STDOUT" +grep -Fq "scorbot_roundtrip_pose=1" "$SCORBOT_KINEMATICS_STDOUT" + check_fixture_output() { local fixture="$1" local expected="$2" diff --git a/wasm-port/tools/build_native_probes.sh b/wasm-port/tools/build_native_probes.sh index 795bd45..ffe51ab 100755 --- a/wasm-port/tools/build_native_probes.sh +++ b/wasm-port/tools/build_native_probes.sh @@ -406,6 +406,24 @@ MAX_KINEMATICS_PROBE_SOURCES=( "$WRAP_DIR/linuxcnc_max_kinematics_probe.cpp" ) +LINEARDELTA_KINEMATICS_PROBE_SOURCES=( + "$VENDOR_DIR/src/emc/kinematics/lineardeltakins.c" + "$WRAP_DIR/linuxcnc_hal_adapter.cpp" + "$WRAP_DIR/linuxcnc_lineardelta_kinematics_probe.cpp" +) + +ROTARYDELTA_KINEMATICS_PROBE_SOURCES=( + "$VENDOR_DIR/src/emc/kinematics/rotarydeltakins.c" + "$WRAP_DIR/linuxcnc_hal_adapter.cpp" + "$WRAP_DIR/linuxcnc_rotarydelta_kinematics_probe.cpp" +) + +SCORBOT_KINEMATICS_PROBE_SOURCES=( + "$VENDOR_DIR/src/emc/kinematics/scorbot-kins.c" + "$WRAP_DIR/linuxcnc_hal_adapter.cpp" + "$WRAP_DIR/linuxcnc_scorbot_kinematics_probe.cpp" +) + TP_CORE_SOURCES=( "$VENDOR_DIR/src/emc/tp/tp.c" "$VENDOR_DIR/src/emc/tp/tc.c" @@ -635,6 +653,27 @@ build_binary_target \ MAX_KINEMATICS_PROBE_SOURCES \ NO_LINK_FLAGS +build_binary_target \ + linuxcnc_lineardelta_kinematics_probe \ + "$BUILD_DIR/linuxcnc_lineardelta_kinematics_probe" \ + TP_FLAGS \ + LINEARDELTA_KINEMATICS_PROBE_SOURCES \ + NO_LINK_FLAGS + +build_binary_target \ + linuxcnc_rotarydelta_kinematics_probe \ + "$BUILD_DIR/linuxcnc_rotarydelta_kinematics_probe" \ + TP_FLAGS \ + ROTARYDELTA_KINEMATICS_PROBE_SOURCES \ + NO_LINK_FLAGS + +build_binary_target \ + linuxcnc_scorbot_kinematics_probe \ + "$BUILD_DIR/linuxcnc_scorbot_kinematics_probe" \ + TP_FLAGS \ + SCORBOT_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" \ @@ -755,6 +794,51 @@ else "$BUILD_DIR/linuxcnc_max_kinematics_probe.run.stderr.log" fi +if [[ "$(tr -d '[:space:]' < "$BUILD_DIR/linuxcnc_lineardelta_kinematics_probe.exitcode")" == "0" ]]; then + set +e + "$BUILD_DIR/linuxcnc_lineardelta_kinematics_probe" \ + >"$BUILD_DIR/linuxcnc_lineardelta_kinematics_probe.run.stdout.log" \ + 2>"$BUILD_DIR/linuxcnc_lineardelta_kinematics_probe.run.stderr.log" + LINEARDELTA_KINEMATICS_RUN_RC=$? + set -e + echo "$LINEARDELTA_KINEMATICS_RUN_RC" > "$BUILD_DIR/linuxcnc_lineardelta_kinematics_probe.run.exitcode" +else + rm -f \ + "$BUILD_DIR/linuxcnc_lineardelta_kinematics_probe.run.exitcode" \ + "$BUILD_DIR/linuxcnc_lineardelta_kinematics_probe.run.stdout.log" \ + "$BUILD_DIR/linuxcnc_lineardelta_kinematics_probe.run.stderr.log" +fi + +if [[ "$(tr -d '[:space:]' < "$BUILD_DIR/linuxcnc_rotarydelta_kinematics_probe.exitcode")" == "0" ]]; then + set +e + "$BUILD_DIR/linuxcnc_rotarydelta_kinematics_probe" \ + >"$BUILD_DIR/linuxcnc_rotarydelta_kinematics_probe.run.stdout.log" \ + 2>"$BUILD_DIR/linuxcnc_rotarydelta_kinematics_probe.run.stderr.log" + ROTARYDELTA_KINEMATICS_RUN_RC=$? + set -e + echo "$ROTARYDELTA_KINEMATICS_RUN_RC" > "$BUILD_DIR/linuxcnc_rotarydelta_kinematics_probe.run.exitcode" +else + rm -f \ + "$BUILD_DIR/linuxcnc_rotarydelta_kinematics_probe.run.exitcode" \ + "$BUILD_DIR/linuxcnc_rotarydelta_kinematics_probe.run.stdout.log" \ + "$BUILD_DIR/linuxcnc_rotarydelta_kinematics_probe.run.stderr.log" +fi + +if [[ "$(tr -d '[:space:]' < "$BUILD_DIR/linuxcnc_scorbot_kinematics_probe.exitcode")" == "0" ]]; then + set +e + "$BUILD_DIR/linuxcnc_scorbot_kinematics_probe" \ + >"$BUILD_DIR/linuxcnc_scorbot_kinematics_probe.run.stdout.log" \ + 2>"$BUILD_DIR/linuxcnc_scorbot_kinematics_probe.run.stderr.log" + SCORBOT_KINEMATICS_RUN_RC=$? + set -e + echo "$SCORBOT_KINEMATICS_RUN_RC" > "$BUILD_DIR/linuxcnc_scorbot_kinematics_probe.run.exitcode" +else + rm -f \ + "$BUILD_DIR/linuxcnc_scorbot_kinematics_probe.run.exitcode" \ + "$BUILD_DIR/linuxcnc_scorbot_kinematics_probe.run.stdout.log" \ + "$BUILD_DIR/linuxcnc_scorbot_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" \ @@ -925,6 +1009,24 @@ build_object_target \ "$VENDOR_DIR/src/emc/kinematics/maxkins.c" \ TP_FLAGS +build_object_target \ + linuxcnc_lineardeltakins_source_probe \ + "$BUILD_DIR/linuxcnc_lineardeltakins_source_probe.o" \ + "$VENDOR_DIR/src/emc/kinematics/lineardeltakins.c" \ + TP_FLAGS + +build_object_target \ + linuxcnc_rotarydeltakins_source_probe \ + "$BUILD_DIR/linuxcnc_rotarydeltakins_source_probe.o" \ + "$VENDOR_DIR/src/emc/kinematics/rotarydeltakins.c" \ + TP_FLAGS + +build_object_target \ + linuxcnc_scorbot_kins_source_probe \ + "$BUILD_DIR/linuxcnc_scorbot_kins_source_probe.o" \ + "$VENDOR_DIR/src/emc/kinematics/scorbot-kins.c" \ + TP_FLAGS + build_object_target \ linuxcnc_interp_convert_source_probe \ "$BUILD_DIR/linuxcnc_interp_convert_source_probe.o" \ diff --git a/wasm-port/tools/source-manifest.txt b/wasm-port/tools/source-manifest.txt index 1b844b8..4049c16 100644 --- a/wasm-port/tools/source-manifest.txt +++ b/wasm-port/tools/source-manifest.txt @@ -44,6 +44,11 @@ src/emc/kinematics/corexykins.c src/emc/kinematics/rotatekins.c src/emc/kinematics/rosekins.c src/emc/kinematics/maxkins.c +src/emc/kinematics/lineardeltakins-common.h +src/emc/kinematics/lineardeltakins.c +src/emc/kinematics/rotarydeltakins-common.h +src/emc/kinematics/rotarydeltakins.c +src/emc/kinematics/scorbot-kins.c src/emc/tp/tp.h src/emc/tp/tp_types.h src/emc/tp/tc.h diff --git a/wasm-port/vendor/linuxcnc/src/emc/kinematics/lineardeltakins-common.h b/wasm-port/vendor/linuxcnc/src/emc/kinematics/lineardeltakins-common.h new file mode 100644 index 0000000..6e0b003 --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/emc/kinematics/lineardeltakins-common.h @@ -0,0 +1,140 @@ +#ifndef LINUXCNCLINEARDELTAKINS_COMMON_H +#define LINUXCNCLINEARDELTAKINS_COMMON_H +// Copyright 2013 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. +/* + * Kinematics for a rostock-style delta robot + * + * Towers 0, 1 and 2 are spaced at 120 degrees around the origin + * at distance R. A rod of length L (L > R) connects each tower to the + * platform. Tower 0 is at (0,R). (note: this is not at zero radians!) + * + * ABCUVW coordinates are passed through in joints[3..8]. + * + * L is like DELTA_DIAGONAL_ROD and R is like DELTA_RADIUS in + * Marlin---remember to account for the effector and carriage offsets + * when changing from the default. + */ + +// common routines used by the userspace kinematics and the realtime kinematics +// user must include a math.h-type header first +// Inspired by Marlin delta firmware and https://gist.github.com/kastner/5279172 +#include + +static double L, R; +static double Ax, Ay, Bx, By, Cx, Cy, L2; + +#define SQ3 (sqrt(3)) + +#define SIN_60 (SQ3/2) +#define COS_60 (.5) + +static double sq(double x) { return x*x; } + +static void set_geometry(double r_, double l_) +{ + if(L == l_ && R == r_) return; + + L = l_; + R = r_; + + L2 = sq(L); + + Ax = 0.0; + Ay = R; + + Bx = -SIN_60 * R; + By = -COS_60 * R; + + Cx = SIN_60 * R; + Cy = -COS_60 * R; +} + +static int kinematics_inverse(const EmcPose *pos, double *joints) +{ + double x = pos->tran.x, y = pos->tran.y, z = pos->tran.z; + joints[0] = z + sqrt(L2 - sq(Ax-x) - sq(Ay-y)); + joints[1] = z + sqrt(L2 - sq(Bx-x) - sq(By-y)); + joints[2] = z + sqrt(L2 - sq(Cx-x) - sq(Cy-y)); + 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 isnan(joints[0]) || isnan(joints[1]) || isnan(joints[2]) + ? -1 : 0; +} + +static int kinematics_forward(const double *joints, EmcPose *pos) +{ + double q1 = joints[0]; + double q2 = joints[1]; + double q3 = joints[2]; + + double den = (By-Ay)*Cx-(Cy-Ay)*Bx; + + double w1 = Ay*Ay + q1*q1; // n.b. assumption that Ax is 0 all through here + double w2 = Bx*Bx + By*By + q2*q2; + double w3 = Cx*Cx + Cy*Cy + q3*q3; + + double a1 = (q2-q1)*(Cy-Ay)-(q3-q1)*(By-Ay); + double b1 = -((w2-w1)*(Cy-Ay)-(w3-w1)*(By-Ay))/2.0; + + double a2 = -(q2-q1)*Cx+(q3-q1)*Bx; + double b2 = ((w2-w1)*Cx - (w3-w1)*Bx)/2.0; + + // a*z^2 + b*z + c = 0 + double a = a1*a1 + a2*a2 + den*den; + double b = 2*(a1*b1 + a2*(b2-Ay*den) - q1*den*den); + double c = (b2-Ay*den)*(b2-Ay*den) + b1*b1 + den*den*(q1*q1 - L*L); + + double discr = b*b - 4.0*a*c; + if (discr < 0) return -1; // non-existing point + + double z = -0.5*(b+sqrt(discr))/a; + pos->tran.z = z; + pos->tran.x = (a1*z + b1)/den; + pos->tran.y = (a2*z + b2)/den; + 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; +} + +// Default values which may correspond to someone's linear delta robot. To +// change these, use halcmd setp rather than rebuilding the software. + +// Center-to-center distance of the holes in the diagonal push rods. +#define DELTA_DIAGONAL_ROD 269.0 // mm + +// Horizontal offset from middle of printer to smooth rod center. +#define DELTA_SMOOTH_ROD_OFFSET 198.25 // mm + +// Horizontal offset of the universal joints on the end effector. +#define DELTA_EFFECTOR_OFFSET 33.0 // mm + +// Horizontal offset of the universal joints on the carriages. +#define DELTA_CARRIAGE_OFFSET 35.0 // mm + +// Effective horizontal distance bridged by diagonal push rods. +#define DELTA_RADIUS (DELTA_SMOOTH_ROD_OFFSET-DELTA_EFFECTOR_OFFSET-DELTA_CARRIAGE_OFFSET) +#endif diff --git a/wasm-port/vendor/linuxcnc/src/emc/kinematics/lineardeltakins.c b/wasm-port/vendor/linuxcnc/src/emc/kinematics/lineardeltakins.c new file mode 100644 index 0000000..156338f --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/emc/kinematics/lineardeltakins.c @@ -0,0 +1,98 @@ +// Copyright 2013 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. + +#include +#include +#include +#include + +#include "lineardeltakins-common.h" + +struct haldata +{ + hal_float_t *r, *l; +} *haldata; + +int comp_id; + +int kinematicsForward(const double * joints, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)fflags; + (void)iflags; + set_geometry(*haldata->r, *haldata->l); + return kinematics_forward(joints, pos); +} + +int kinematicsInverse(const EmcPose *pos, double *joints, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags) { + (void)iflags; + (void)fflags; + set_geometry(*haldata->r, *haldata->l); + return kinematics_inverse(pos, joints); +} + +KINEMATICS_TYPE kinematicsType() +{ + return KINEMATICS_BOTH; +} + +int rtapi_app_main(void) +{ + int retval = 0; + + comp_id = hal_init("lineardeltakins"); + if(comp_id < 0) retval = comp_id; + + if(retval == 0) + { + haldata = hal_malloc(sizeof(struct haldata)); + retval = !haldata; + } + + if(retval == 0) + retval = hal_pin_float_newf(HAL_IN, &haldata->r, comp_id, + "lineardeltakins.R"); + if(retval == 0) + retval = hal_pin_float_newf(HAL_IN, &haldata->l, comp_id, + "lineardeltakins.L"); + + if(retval == 0) + { + *haldata->r = DELTA_RADIUS; + *haldata->l = DELTA_DIAGONAL_ROD; + } + + if(retval == 0) + { + hal_ready(comp_id); + } + + return retval; +} + +void rtapi_app_exit(void) +{ + hal_exit(comp_id); +} + +KINS_NOT_SWITCHABLE +EXPORT_SYMBOL(kinematicsType); +EXPORT_SYMBOL(kinematicsForward); +EXPORT_SYMBOL(kinematicsInverse); +MODULE_LICENSE("GPL"); diff --git a/wasm-port/vendor/linuxcnc/src/emc/kinematics/rotarydeltakins-common.h b/wasm-port/vendor/linuxcnc/src/emc/kinematics/rotarydeltakins-common.h new file mode 100644 index 0000000..59cc872 --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/emc/kinematics/rotarydeltakins-common.h @@ -0,0 +1,195 @@ + +// Copyright 2013 Chris Radek +// +// 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. + + +/* + Based on work by "mzavatsky" at: + http://forums.trossenrobotics.com/tutorials/introduction-129/delta-robot-kinematics-3276/ + + "You can freely use this code in your applications." + + which was based on: + + Descriptive Geometric Kinematic Analysis of Clavel's "Delta" Robot + P.J. Zsombor-Murray, McGill University + + "... the purpose of this article: to provide a clear kinematic + analysis useful to those who may wish to program and employ nice + little three legged robots ..." + + + The platform is on "top", the origin is in the center of the plane + containing the three hip joints. Z points upward, so Z coordinates + are always negative. Thighs always point outward, straight out + (knee at Z=0) is considered zero degrees for the angular hip joint. + Positive rotation is knee-downward, so if you rotate all knees + positive, the Z coordinate will get more negative. + + Joint zero is the one whose thigh swings in the YZ plane. +*/ + +#ifndef LINUXCNCROTARYDELTAKINS_COMMON_H +#define LINUXCNCROTARYDELTAKINS_COMMON_H + +#include + +// distance from origin to a hip joint +static double platformradius; + +// thigh connects the hip to the knee +static double thighlength; + +// shin (the parallelogram) connects the knee to the foot +static double shinlength; + +// distance from center of foot (controlled point) to an ankle joint +static double footradius; + +#ifndef sq +#define sq(a) ((a)*(a)) +#endif +#ifndef D2R +#define D2R(d) ((d)*M_PI/180.) +#endif + +static void set_geometry(double pfr, double tl, double sl, double fr) { + platformradius = pfr; + thighlength = tl; + shinlength = sl; + footradius = fr; +} + +// Given three hip joint angles, find the controlled point +static int kinematics_forward(const double *joints, EmcPose *pos) { + double + j0 = joints[0], + j1 = joints[1], + j2 = joints[2], + y1, z1, // x1 is 0 + x2, y2, z2, x3, y3, z3, + a1, b1, a2, b2, + w1, w2, w3, + denom, + a, b, c, d; + + j0 = D2R(j0); + j1 = D2R(j1); + j2 = D2R(j2); + + y1 = -(platformradius - footradius + thighlength * cos(j0)); + z1 = -thighlength * sin(j0); + + y2 = (platformradius - footradius + thighlength * cos(j1)) * 0.5; + x2 = y2 * sqrt(3); + z2 = -thighlength * sin(j1); + + y3 = (platformradius - footradius + thighlength * cos(j2)) * 0.5; + x3 = -y3 * sqrt(3); + z3 = -thighlength * sin(j2); + + denom = x3 * (y2 - y1) - x2 * (y3 - y1); + + w1 = sq(y1) + sq(z1); + w2 = sq(x2) + sq(y2) + sq(z2); + w3 = sq(x3) + sq(y3) + sq(z3); + + a1 = (z2-z1) * (y3-y1) - (z3-z1) * (y2-y1); + b1 = -((w2-w1) * (y3-y1) - (w3-w1) * (y2-y1)) / 2.0; + + a2 = -(z2 - z1) * x3 + (z3 - z1) * x2; + b2 = ((w2 - w1) * x3 - (w3 - w1) * x2) / 2.0; + + // a*z^2 + b*z + c = 0 + a = sq(a1) + sq(a2) + sq(denom); + b = 2 * (a1 * b1 + a2 * (b2 - y1 * denom) - z1 * sq(denom)); + c = (b2 - y1 * denom) * (b2 - y1 * denom) + + sq(b1) + sq(denom) * (sq(z1) - sq(shinlength)); + + d = sq(b) - 4 * a * c; + if (d < 0) return -1; + + pos->tran.z = (-b - sqrt(d)) / (2 * a); + pos->tran.x = (a1 * pos->tran.z + b1) / denom; + pos->tran.y = (a2 * pos->tran.z + b2) / denom; + + 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; +} + + +// Given controlled point, find joint zero's angle +// (J0 is the easy one in the ZY plane) +static int inverse_j0(double x, double y, double z, double *theta) { + double a, b, d, knee_y, knee_z; + + a = 0.5 * (sq(x) + sq(y - footradius) + sq(z) + sq(thighlength) - + sq(shinlength) - sq(platformradius)) / z; + b = (footradius - platformradius - y) / z; + + d = sq(thighlength) * (sq(b) + 1) - sq(a - b * platformradius); + if (d < 0) return -1; + + knee_y = (platformradius + a*b + sqrt(d)) / (sq(b) + 1); + knee_z = b * knee_y - a; + + *theta = atan2(knee_z, knee_y - platformradius); + *theta *= 180.0/M_PI; + return 0; +} + +static void rotate(double *x, double *y, double theta) { + double xx, yy; + xx = *x, yy = *y; + *x = xx * cos(theta) - yy * sin(theta); + *y = xx * sin(theta) + yy * cos(theta); +} + +static int kinematics_inverse(const EmcPose *pos, double *joints) { + double xr, yr; + if(inverse_j0(pos->tran.x, pos->tran.y, pos->tran.z, &joints[0])) return -1; + + // now use symmetry property to get the other two just as easily... + xr = pos->tran.x; yr = pos->tran.y; + rotate(&xr, &yr, -2*M_PI/3); + if(inverse_j0(xr, yr, pos->tran.z, &joints[1])) return -1; + + xr = pos->tran.x; yr = pos->tran.y; + rotate(&xr, &yr, 2*M_PI/3); + if(inverse_j0(xr, yr, pos->tran.z, &joints[2])) return -1; + + 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; +} + +#define RDELTA_PFR 10.0 +#define RDELTA_TL 10.0 +#define RDELTA_SL 14.0 +#define RDELTA_FR 6.0 + +#endif diff --git a/wasm-port/vendor/linuxcnc/src/emc/kinematics/rotarydeltakins.c b/wasm-port/vendor/linuxcnc/src/emc/kinematics/rotarydeltakins.c new file mode 100644 index 0000000..099d18f --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/emc/kinematics/rotarydeltakins.c @@ -0,0 +1,110 @@ + +// Copyright 2013 Chris Radek +// +// 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 +#include +#include +#include + +#include "rotarydeltakins-common.h" + +struct haldata +{ + hal_float_t *pfr; + hal_float_t *tl; + hal_float_t *sl; + hal_float_t *fr; +} *haldata; + +int comp_id; + +int kinematicsForward(const double * joints, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)fflags; + (void)iflags; + set_geometry(*haldata->pfr, *haldata->tl, *haldata->sl, *haldata->fr); + return kinematics_forward(joints, pos); +} + +int kinematicsInverse(const EmcPose *pos, double *joints, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags) { + (void)iflags; + (void)fflags; + set_geometry(*haldata->pfr, *haldata->tl, *haldata->sl, *haldata->fr); + return kinematics_inverse(pos, joints); +} + +KINEMATICS_TYPE kinematicsType() +{ + return KINEMATICS_BOTH; +} + +int rtapi_app_main(void) +{ + int retval = 0; + + comp_id = hal_init("rotarydeltakins"); + if(comp_id < 0) retval = comp_id; + + if(retval == 0) + { + haldata = hal_malloc(sizeof(struct haldata)); + retval = !haldata; + } + + if(retval == 0) + retval = hal_pin_float_newf(HAL_IN, &haldata->pfr, comp_id, + "rotarydeltakins.platformradius"); + if(retval == 0) + retval = hal_pin_float_newf(HAL_IN, &haldata->tl, comp_id, + "rotarydeltakins.thighlength"); + if(retval == 0) + retval = hal_pin_float_newf(HAL_IN, &haldata->sl, comp_id, + "rotarydeltakins.shinlength"); + if(retval == 0) + retval = hal_pin_float_newf(HAL_IN, &haldata->fr, comp_id, + "rotarydeltakins.footradius"); + + if(retval == 0) + { + *haldata->pfr = RDELTA_PFR; + *haldata->tl = RDELTA_TL; + *haldata->sl = RDELTA_SL; + *haldata->fr = RDELTA_FR; + } + + if(retval == 0) + { + hal_ready(comp_id); + } + + return retval; +} + +void rtapi_app_exit(void) +{ + hal_exit(comp_id); +} + +KINS_NOT_SWITCHABLE +EXPORT_SYMBOL(kinematicsType); +EXPORT_SYMBOL(kinematicsForward); +EXPORT_SYMBOL(kinematicsInverse); +MODULE_LICENSE("GPL"); diff --git a/wasm-port/vendor/linuxcnc/src/emc/kinematics/scorbot-kins.c b/wasm-port/vendor/linuxcnc/src/emc/kinematics/scorbot-kins.c new file mode 100644 index 0000000..2f4eeec --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/emc/kinematics/scorbot-kins.c @@ -0,0 +1,322 @@ + +// +// This is a kinematics module for the Scorbot ER 3. +// +// Copyright (C) 2015-2016 Sebastian Kuzminsky +// +// 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 St, Fifth Floor, Boston, MA 02110-1301 USA +// + +// +// The origin of the G53 coordinate system is at the center of rotation of +// joint J0, and at the bottom of the base plate. +// +// FIXME: The origin should probably be at the bottom of the base (part 5 +// in the parts diagram on page 7-11 of the SCORBOT-ER III User's Manual). +// +// Joint 0 is rotation around the Z axis. It chooses the plane that +// the rest of the arm moves in. +// +// Joint 1 is the shoulder. +// +// Joint 2 is the elbow. +// +// Joint 3 is pitch of the wrist, joint 4 is roll of the wrist. These are +// converted to motor actuations by an external differential comp in HAL. +// + + +#include +#include +#include +#include +#include +#include + + +// +// linkage constants, in mm & degrees +// + +// Link 0 connects the origin to J1 (shoulder) +// These dimensions come off a drawing I got from Intelitek. +#define L0_HORIZONTAL_DISTANCE 16 +#define L0_VERTICAL_DISTANCE 140 + +#define L1_LENGTH 221 // Link 1 connects J1 (shoulder) to J2 (elbow) +#define L2_LENGTH 221 // Link 2 connects J2 (shoulder) to the wrist + + +// Compute the cartesian coordinates of J1, given the J0 angle (and the +// fixed, known link L0 between J0 and J1). +static void compute_j1_cartesian_location(double j0, EmcPose *j1_cart) { + j1_cart->tran.x = L0_HORIZONTAL_DISTANCE * cos(TO_RAD * j0); + j1_cart->tran.y = L0_HORIZONTAL_DISTANCE * sin(TO_RAD * j0); + j1_cart->tran.z = L0_VERTICAL_DISTANCE; + j1_cart->a = 0; + j1_cart->b = 0; + j1_cart->c = 0; + j1_cart->u = 0; + j1_cart->v = 0; + j1_cart->w = 0; +} + + +// Forward kinematics takes the joint positions and computes the cartesian +// coordinates of the controlled point. +int kinematicsForward( + const double *joints, + EmcPose *pose, + const KINEMATICS_FORWARD_FLAGS *fflags, + KINEMATICS_INVERSE_FLAGS *iflags +) { + (void)fflags; + (void)iflags; + EmcPose j1_vector; // the vector from j0 ("base") to joint 1 ("shoulder", end of link 0) + EmcPose j2_vector; // the vector from j1 ("shoulder") to joint 2 ("elbow", end of link 1) + EmcPose j3_vector; // the vector from j2 ("elbow") to joint 3 ("wrist", end of link 2) + + double r; + + // rtapi_print("fwd: j0=%f, j1=%f, j2=%f\n", joints[0], joints[1], joints[2]); + compute_j1_cartesian_location(joints[0], &j1_vector); + // rtapi_print("fwd: j1=(%f, %f, %f)\n", j1_vector.tran.x, j1_vector.tran.y, j1_vector.tran.z); + + // Link 1 connects j1 (shoulder) to j2 (elbow). + r = L1_LENGTH * cos(TO_RAD * joints[1]); + j2_vector.tran.x = r * cos(TO_RAD * joints[0]); + j2_vector.tran.y = r * sin(TO_RAD * joints[0]); + j2_vector.tran.z = L1_LENGTH * sin(TO_RAD * joints[1]); + // rtapi_print("fwd: j2=(%f, %f, %f)\n", j2_vector.tran.x, j2_vector.tran.y, j2_vector.tran.z); + + // Link 2 connects j2 (elbow) to j3 (wrist). + // J3 is the controlled point. + r = L2_LENGTH * cos(TO_RAD * joints[2]); + j3_vector.tran.x = r * cos(TO_RAD * joints[0]); + j3_vector.tran.y = r * sin(TO_RAD * joints[0]); + j3_vector.tran.z = L2_LENGTH * sin(TO_RAD * joints[2]); + // rtapi_print("fwd: j3=(%f, %f, %f)\n", j3_vector.tran.x, j3_vector.tran.y, j3_vector.tran.z); + + // The end-effector location is the sum of the linkage vectors. + pose->tran.x = j1_vector.tran.x + j2_vector.tran.x + j3_vector.tran.x; + pose->tran.y = j1_vector.tran.y + j2_vector.tran.y + j3_vector.tran.y; + pose->tran.z = j1_vector.tran.z + j2_vector.tran.z + j3_vector.tran.z; + // rtapi_print("fwd: pose=(%f, %f, %f)\n", pose->tran.x, pose->tran.y, pose->tran.z); + + // A and B are wrist roll and pitch, handled in hal by external kinematics + pose->a = joints[3]; + pose->b = joints[4]; + + return 0; +} + + +// +// Inverse kinematics takes the cartesian coordinates of the controlled +// point and computes corresponding the joint positions. +// +// Joint 0 rotates the arm around the base. The rest of the joints are +// confined to the vertical plane containing J0, and rotated around the +// vertical at J0. This kinematics code calls this plane the "RZ" plane. +// The Z coordinate in this plane is the same as the Z coordinate in the +// "cartesian" coordinates of LinuxCNC's world space. The R coordinate +// is the horizontal distance (ie, in the XY plane) of the controlled +// point from J0. +// +int kinematicsInverse( + const EmcPose *pose, + double *joints, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags +) { + (void)iflags; + (void)fflags; + // EmcPose j1_cart; + double distance_to_cp, distance_to_center; + double r_j1, z_j1; // (r_j1, z_j1) is the location of J1 in the RZ plane + double r_cp, z_cp; // (r_cp, z_cp) is the location of the controlled point in the RZ plane + double angle_to_cp; + double j1_angle; + + // the location of J2, this is what we're trying to find + double z_j2; + + // rtapi_print("inv: x=%f, y=%f, z=%f\n", pose->tran.x, pose->tran.y, pose->tran.z); + + // J0 is easy. Project the (X, Y, Z) of the pose onto the Z=0 plane. + // J0 points at the projected (X, Y) point. tan(J0) = Y/X + // J0 then defines the plane that the rest of the arm operates in. + joints[0] = TO_DEG * atan2(pose->tran.y, pose->tran.x); + // rtapi_print("inv: j0=%f\n", joints[0]); + + // compute_j1_cartesian_location(joints[0], &j1_cart); + // rtapi_print("inv: j1=(X=%f, Y=%f, Z=%f)\n", j1_cart.tran.x, j1_cart.tran.y, j1_cart.tran.z); + + // FIXME: Until i figure the wrist differential out, the controlled + // point will be the location of the wrist joint, J3/J4. + + // The location of J1 (computed above) and the location of the + // controlled point are separated by J1, L1, J2, and L2. L1 and L2 are + // known, but J1 and J2 are not. + + // (r_j1, z_j1) is the location of J1 in the RZ plane (the vertical + // plane defined by the angle of J0, with the origin at the location + // of J0. This is just a known, static vector. + r_j1 = L0_HORIZONTAL_DISTANCE; + z_j1 = L0_VERTICAL_DISTANCE; + // rtapi_print("inv: r_j1=%f, z_j1=%f\n", r_j1, z_j1); + + // (r_cp, z_cp) is the location of J3 (the controlled point), again in + // the plane defined by the angle of J0, with the origin of the + // machine. + r_cp = sqrt(pow(pose->tran.x, 2) + pow(pose->tran.y, 2)); + z_cp = pose->tran.z; + // rtapi_print("inv: r_cp=%f, z_cp=%f (controlled point)\n", r_cp, z_cp); + + // translate so (r_j1, z_j1) is the origin of the coordinate system + r_cp -= r_j1; + z_cp -= z_j1; + // rtapi_print("inv: r_cp=%f, z_cp=%f (translated controlled point)\n", r_cp, z_cp); + + // + // Now the origin (aka J1), J2, and CP define a triangle in the RZ plane. + // The triangle is isosceles, because from the origin to J2 is L1, and + // from J2 to CP is L2, and L1 and L2 are the same length. + // + // Bisect the base of that triangle, and call the center point of the + // base "Center". + // + // Draw a line between J2 and Center. This defines two right + // triangles: (J1, J2, Center) and (CP, J2, Center). + // + // The length of the (J1, Center) and (CP, Center) lines are equal, and + // are half the distance from the origin to CP. + // + + distance_to_cp = sqrt(pow(r_cp, 2) + pow(z_cp, 2)); + distance_to_center = distance_to_cp / 2; + // rtapi_print("inv: distance to cp: %f\n", distance_to_cp); + + // find the angle of the vector from the origin to the CP + angle_to_cp = TO_DEG * acos(r_cp / distance_to_cp); + if (z_cp < 0) { + angle_to_cp *= -1; + } + // rtapi_print("inv: angle to cp: %f\n", angle_to_cp); + + // find the angle (Center, J1, J2) + j1_angle = TO_DEG * acos(distance_to_center / L1_LENGTH); + // rtapi_print("inv: j1 angle: %f\n", j1_angle); + + joints[1] = angle_to_cp + j1_angle; + // rtapi_print("inv: j1: %f\n", joints[1]); + + // now we can compute the location of J2 + z_j2 = L1_LENGTH * sin(TO_RAD * joints[1]); + // rtapi_print("inv: r_j2=%f, z_j2=%f (translated j2)\n", r_j2, z_j2); + + joints[2] = -1.0 * TO_DEG * asin((z_j2 - z_cp) / L2_LENGTH); + + +#if 0 + // Distance between controlled point and the location of j1. These two + // points are separated by link 1, joint 1, and link 2. + distance_between_centers = sqrt(pow((r2 - r1), 2) + pow((z2 - z1), 2)); + + if (distance_between_centers > (L1_LENGTH + L2_LENGTH)) { + // trying to reach too far + return GO_RESULT_RANGE_ERROR; + } + + if (distance_between_centers < fabs(L1_LENGTH - L2_LENGTH)) { + // trying to reach too far into armpit + return GO_RESULT_RANGE_ERROR; + } + + delta = (1.0 / 4.0) * sqrt((distance_between_centers + L1_LENGTH + L2_LENGTH) * (distance_between_centers + L1_LENGTH - L2_LENGTH) * (distance_between_centers - L1_LENGTH + L2_LENGTH) * (L1_LENGTH + L2_LENGTH - distance_between_centers)); + + ir1 = ((r1 + r2) / 2) + (((r2 - r1) * (pow(L1_LENGTH, 2) - pow(L2_LENGTH, 2)))/(2 * pow(distance_between_centers, 2))) + ((2 * (z1 - z2) * delta) / pow(distance_between_centers, 2)); + ir2 = ((r1 + r2) / 2) + (((r2 - r1) * (pow(L1_LENGTH, 2) - pow(L2_LENGTH, 2)))/(2 * pow(distance_between_centers, 2))) - ((2 * (z1 - z2) * delta) / pow(distance_between_centers, 2)); + + iz1 = ((z1 + z2) / 2) + (((z2 - z1) * (pow(L1_LENGTH, 2) - pow(L2_LENGTH, 2)))/(2 * pow(distance_between_centers, 2))) - ((2 * (r1 - r2) * delta) / pow(distance_between_centers, 2)); + iz2 = ((z1 + z2) / 2) + (((z2 - z1) * (pow(L1_LENGTH, 2) - pow(L2_LENGTH, 2)))/(2 * pow(distance_between_centers, 2))) + ((2 * (r1 - r2) * delta) / pow(distance_between_centers, 2)); + + + // (ir1, iz1) is one intersection point, (ir2, iz2) is the other. + // These are the possible locations of the J2 joint. + // FIXME: For now we arbitrarily pick the one with the bigger Z. + + if (iz1 > iz2) { + j2_r = ir1; + j2_z = iz1; + } else { + j2_r = ir2; + j2_z = iz2; + } + // rtapi_print("inv: j2_r=%f, j2_z=%f (J2, intersection point)\n", j2_r, j2_z); + + // Make J1 point at J2 (j2_r, j2_z). + { + double l1_r = j2_r - r1; + joints[1] = TO_DEG * acos(l1_r / L1_LENGTH); + // rtapi_print("inv: l1_r=%f, j1=%f\n", l1_r, joints[1]); + } + + // Make J2 point at the controlled point. + { + double l2_r = r2 - j2_r; + double j2; + j2 = TO_DEG * acos(l2_r / L2_LENGTH); + if (j2_z > pose->tran.z) { + j2 *= -1; + } + joints[2] = j2; + // rtapi_print("inv: l2_r=%f, j2=%f\n", l2_r, joints[2]); + } +#endif + + // A and B are wrist roll and pitch, handled in hal by external kinematics + joints[3] = pose->a; + joints[4] = pose->b; + + return 0; +} + + +KINEMATICS_TYPE kinematicsType(void) { + 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("scorbot-kins"); + if (comp_id < 0) { + return comp_id; + } + hal_ready(comp_id); + return 0; +} + +void rtapi_app_exit(void) { + hal_exit(comp_id); +} +