From d9296eb5e0ab9d5e35818d5b4fc80fbd4602e974 Mon Sep 17 00:00:00 2001 From: wangdequan Date: Sun, 7 Jun 2026 20:05:51 +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 源码直接复用路线推进，新增 tripod 运动学源码的 vendored 同步、native 探针、source probe 覆盖和复用文档，并通过 native 验证。 --- wasm-port/docs/source-reuse-map.md | 6 +- .../linuxcnc_tripod_kinematics_probe.cpp | 78 ++++ .../tests/native/verify_native_probes.sh | 16 + wasm-port/tools/build_native_probes.sh | 34 ++ wasm-port/tools/source-manifest.txt | 1 + .../linuxcnc/src/emc/kinematics/tripodkins.c | 383 ++++++++++++++++++ 6 files changed, 515 insertions(+), 3 deletions(-) create mode 100644 wasm-port/runtime/core/linuxcnc_wrap/linuxcnc_tripod_kinematics_probe.cpp create mode 100644 wasm-port/vendor/linuxcnc/src/emc/kinematics/tripodkins.c diff --git a/wasm-port/docs/source-reuse-map.md b/wasm-port/docs/source-reuse-map.md index e8bb98e..3d71c11 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`, `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` | +| 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`, `tripodkins.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`, `linuxcnc_tripod_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,8 +63,8 @@ Current validation is intentionally mechanical: ## Known Gaps - Additional non-trivial kinematics implementation files, including serial, - SCARA, hexapod, tripod, puma, and other machine-specific modules, are not - yet extracted. + SCARA, hexapod, 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. - OPFS persistence is not yet connected to INI, tool table, parameter file, or diff --git a/wasm-port/runtime/core/linuxcnc_wrap/linuxcnc_tripod_kinematics_probe.cpp b/wasm-port/runtime/core/linuxcnc_wrap/linuxcnc_tripod_kinematics_probe.cpp new file mode 100644 index 0000000..c91092b --- /dev/null +++ b/wasm-port/runtime/core/linuxcnc_wrap/linuxcnc_tripod_kinematics_probe.cpp @@ -0,0 +1,78 @@ +#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_xyz(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); +} + +void print_pose(const char *prefix, const EmcPose &pose) +{ + std::cout << prefix << "_xyz=" + << pose.tran.x << "," + << pose.tran.y << "," + << pose.tran.z << "\n"; +} + +void print_joints(const char *prefix, const double *joints) +{ + std::cout << prefix << "_abc=" + << joints[0] << "," + << joints[1] << "," + << joints[2] << "\n"; +} + +} // namespace + +int main() +{ + const int init_rc = rtapi_app_main(); + std::cout << "tripod_init=" << init_rc << "\n"; + std::cout << "tripod_type=" << kinematicsType() << "\n"; + std::cout << "tripod_switchable=" << kinematicsSwitchable() << "\n"; + + EmcPose pose{}; + pose.tran.x = 0.25; + pose.tran.y = 0.25; + pose.tran.z = 0.5; + + 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 << "tripod_inverse=" << inverse_rc << "\n"; + std::cout << "tripod_inverse_flags=" << fflags << "\n"; + print_joints("tripod_inverse", inverse_joints); + + EmcPose forward_pose{}; + const int forward_rc = kinematicsForward(inverse_joints, &forward_pose, &fflags, &iflags); + std::cout << "tripod_forward=" << forward_rc << "\n"; + print_pose("tripod_forward", forward_pose); + std::cout << "tripod_roundtrip_xyz=" << near_xyz(forward_pose, pose) << "\n"; + + pose.tran.z = -0.5; + const int inverse_below_rc = kinematicsInverse(&pose, inverse_joints, &iflags, &fflags); + std::cout << "tripod_inverse_below=" << inverse_below_rc << "\n"; + std::cout << "tripod_inverse_below_flags=" << fflags << "\n"; + const int forward_below_rc = kinematicsForward(inverse_joints, &forward_pose, &fflags, &iflags); + std::cout << "tripod_forward_below=" << forward_below_rc << "\n"; + print_pose("tripod_forward_below", forward_pose); + std::cout << "tripod_roundtrip_below_xyz=" << near_xyz(forward_pose, 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 910bf77..5f50155 100755 --- a/wasm-port/tests/native/verify_native_probes.sh +++ b/wasm-port/tests/native/verify_native_probes.sh @@ -101,6 +101,8 @@ 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 +check_exitcode linuxcnc_tripod_kinematics_probe +check_exitcode linuxcnc_tripod_kinematics_probe.run for name in \ linuxcnc_tp_tp_source_probe \ linuxcnc_tp_tc_source_probe \ @@ -156,6 +158,7 @@ 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_tripodkins_source_probe check_exitcode linuxcnc_interp_convert_source_probe check_exitcode linuxcnc_interp_read_source_probe check_exitcode linuxcnc_interp_check_source_probe @@ -341,6 +344,19 @@ 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" +TRIPOD_KINEMATICS_STDOUT="$BUILD_DIR/linuxcnc_tripod_kinematics_probe.run.stdout.log" +grep -Fq "tripod_init=0" "$TRIPOD_KINEMATICS_STDOUT" +grep -Fq "tripod_type=4" "$TRIPOD_KINEMATICS_STDOUT" +grep -Fq "tripod_switchable=0" "$TRIPOD_KINEMATICS_STDOUT" +grep -Fq "tripod_inverse=0" "$TRIPOD_KINEMATICS_STDOUT" +grep -Fq "tripod_inverse_flags=0" "$TRIPOD_KINEMATICS_STDOUT" +grep -Fq "tripod_forward=0" "$TRIPOD_KINEMATICS_STDOUT" +grep -Fq "tripod_roundtrip_xyz=1" "$TRIPOD_KINEMATICS_STDOUT" +grep -Fq "tripod_inverse_below=0" "$TRIPOD_KINEMATICS_STDOUT" +grep -Fq "tripod_inverse_below_flags=1" "$TRIPOD_KINEMATICS_STDOUT" +grep -Fq "tripod_forward_below=0" "$TRIPOD_KINEMATICS_STDOUT" +grep -Fq "tripod_roundtrip_below_xyz=1" "$TRIPOD_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 ffe51ab..b220130 100755 --- a/wasm-port/tools/build_native_probes.sh +++ b/wasm-port/tools/build_native_probes.sh @@ -424,6 +424,12 @@ SCORBOT_KINEMATICS_PROBE_SOURCES=( "$WRAP_DIR/linuxcnc_scorbot_kinematics_probe.cpp" ) +TRIPOD_KINEMATICS_PROBE_SOURCES=( + "$VENDOR_DIR/src/emc/kinematics/tripodkins.c" + "$WRAP_DIR/linuxcnc_hal_adapter.cpp" + "$WRAP_DIR/linuxcnc_tripod_kinematics_probe.cpp" +) + TP_CORE_SOURCES=( "$VENDOR_DIR/src/emc/tp/tp.c" "$VENDOR_DIR/src/emc/tp/tc.c" @@ -674,6 +680,13 @@ build_binary_target \ SCORBOT_KINEMATICS_PROBE_SOURCES \ NO_LINK_FLAGS +build_binary_target \ + linuxcnc_tripod_kinematics_probe \ + "$BUILD_DIR/linuxcnc_tripod_kinematics_probe" \ + TP_FLAGS \ + TRIPOD_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" \ @@ -839,6 +852,21 @@ else "$BUILD_DIR/linuxcnc_scorbot_kinematics_probe.run.stderr.log" fi +if [[ "$(tr -d '[:space:]' < "$BUILD_DIR/linuxcnc_tripod_kinematics_probe.exitcode")" == "0" ]]; then + set +e + "$BUILD_DIR/linuxcnc_tripod_kinematics_probe" \ + >"$BUILD_DIR/linuxcnc_tripod_kinematics_probe.run.stdout.log" \ + 2>"$BUILD_DIR/linuxcnc_tripod_kinematics_probe.run.stderr.log" + TRIPOD_KINEMATICS_RUN_RC=$? + set -e + echo "$TRIPOD_KINEMATICS_RUN_RC" > "$BUILD_DIR/linuxcnc_tripod_kinematics_probe.run.exitcode" +else + rm -f \ + "$BUILD_DIR/linuxcnc_tripod_kinematics_probe.run.exitcode" \ + "$BUILD_DIR/linuxcnc_tripod_kinematics_probe.run.stdout.log" \ + "$BUILD_DIR/linuxcnc_tripod_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" \ @@ -1027,6 +1055,12 @@ build_object_target \ "$VENDOR_DIR/src/emc/kinematics/scorbot-kins.c" \ TP_FLAGS +build_object_target \ + linuxcnc_tripodkins_source_probe \ + "$BUILD_DIR/linuxcnc_tripodkins_source_probe.o" \ + "$VENDOR_DIR/src/emc/kinematics/tripodkins.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 4049c16..849e188 100644 --- a/wasm-port/tools/source-manifest.txt +++ b/wasm-port/tools/source-manifest.txt @@ -49,6 +49,7 @@ src/emc/kinematics/lineardeltakins.c src/emc/kinematics/rotarydeltakins-common.h src/emc/kinematics/rotarydeltakins.c src/emc/kinematics/scorbot-kins.c +src/emc/kinematics/tripodkins.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/tripodkins.c b/wasm-port/vendor/linuxcnc/src/emc/kinematics/tripodkins.c new file mode 100644 index 0000000..d0a12dc --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/emc/kinematics/tripodkins.c @@ -0,0 +1,383 @@ +/******************************************************************** +* Description: tripodkins.c +* Kinematics for 3 axis Tripod machine +* +* Derived from a work by Fred Proctor +* +* Author: +* License: GPL Version 2 +* System: Linux +* +* Copyright (c) 2004 All rights reserved. +* +* Last change: +********************************************************************/ + +/* + These kinematics are for a tripod with point vertices. + + Vertices A, B, and C are the base, and vertex D is the controlled point. + Three tripod strut lengths AD, BD, and CD are the joints that move + point D around. + + Point A is the origin, with coordinates (0, 0, 0). Point B lies on the + x axis, with coordinates (Bx, 0, 0). Point C lies in the xy plane, with + coordinates (Cx, Cy, 0). Point D has coordinates (Dx, Dy, Dz). + + The controlled Cartesian values are Dx, Dy, and Dz. A frame attached to + D, say with x parallel to AD and y in the plane ABD, would change its + orientation as the strut lengths changed. The orientation of this frame + relative to the world frame is not computed. + + With respect to the kinematics functions, + + pos->tran.x = Dx + pos->tran.y = Dy + pos->tran.z = Dz + pos->a,b,c = 0 + + joints[0] = AD + joints[1] = BD + joints[2] = CD + + The inverse kinematics have no singularities. Any values for Dx, Dy, and + Dz will yield numerical results. Of course, these may be beyond the + strut length limits, but there are no singular effects like infinite speed. + + The forward kinematics has a singularity due to the triangle inequalities + for triangles ABD, BCD, and CAD. When any of these approach the limit, + Dz is zero and D lies in the base plane. + + The forward kinematics flags, referred to in kinematicsForward and + set in kinematicsInverse, let the forward kinematics select between + the positive and negative values of Dz for given strut values. + Dz > 0 is "above", Dz < 0 is "below". Dz = 0 is the singularity. + + fflags == 0 selects Dz > 0, + fflags != 0 selects Dz < 0. + + The inverse kinematics flags let the inverse kinematics select between + multiple valid solutions of strut lengths for given Cartesian values + for D. There are no multiple solutions: D constrains the strut lengths + completely. So, the inverse flags are ignored. + */ + +#include /* RTAPI realtime OS API */ +#include /* RTAPI realtime module decls */ +#include +#include +#include /* these decls */ + +/* ident tag */ +#ifndef __GNUC__ +#ifndef __attribute__ +#define __attribute__(x) +#endif +#endif + +struct haldata { + hal_float_t *bx, *cx, *cy; +} *haldata = 0; + +#define Bx (*(haldata->bx)) +#define Cx (*(haldata->cx)) +#define Cy (*(haldata->cy)) + +#define sq(x) ((x)*(x)) + +/* + forward kinematics takes three strut lengths and computes Dx, Dy, and Dz + pos->tran.x,y,z, respectively. The forward flag is used to resolve + D above/below the xy plane. The inverse flags are not set since there + are no ambiguities going from world to joint coordinates. + + The forward kins are derived as follows: + + 1. Let x, y, z be Dx, Dy, Dz to save pixels. Cartesian displacement from + D to A, B, and C gives + + AD^2 = x^2 + y^2 + z^2 + BD^2 = (x - Bx)^2 + y^2 + z^2 + CD^2 = (x - Cx)^2 + (y - Cy)^2 + z^2 + + This yields + + I. P = x^2 + y^2 + z^2 + II. Q = x^2 + y^2 + z^2 + sx + III. R = x^2 + y^2 + z^2 + tx + uy + + Where + + P = AD^2, + Q = BD^2 - Bx^2 + R = CD^2 - Cx^2 - Cy^2 + s = -2Bx + t = -2Cx + u = -2Cy + + II - I gives Q - P = sx, so x = (Q - P)/s, s != 0. The constraint on s + means that Bx != 0, or points A and B can't be the same. + + III - II gives R - Q = (t - s)x + uy, so y = (R - Q - (t - s)x)/u, u != 0. + The constraint on u means that Cy != 0, or points A B C can't be collinear. + + Substituting x, y into I gives z = sqrt(P - x^2 - y^2), which has two + solutions. Positive means the tripod is above the xy plane, negative + means below. +*/ +int kinematicsForward(const double * joints, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) +{ + (void)iflags; +#define AD (joints[0]) +#define BD (joints[1]) +#define CD (joints[2]) +#define Dx (pos->tran.x) +#define Dy (pos->tran.y) +#define Dz (pos->tran.z) + double P, Q, R; + double s, t, u; + + P = sq(AD); + Q = sq(BD) - sq(Bx); + R = sq(CD) - sq(Cx) - sq(Cy); + s = -2.0 * Bx; + t = -2.0 * Cx; + u = -2.0 * Cy; + + if (s == 0.0) { + /* points A and B coincident. Fix Bx, #defined up top. */ + return -1; + } + Dx = (Q - P) / s; + + if (u == 0.0) { + /* points A B C are colinear. Fix Cy, #defined up top. */ + return -1; + } + Dy = (R - Q - (t - s) * Dx) / u; + Dz = P - sq(Dx) - sq(Dy); + if (Dz < 0.0) { + /* triangle inequality violated */ + return -1; + } + Dz = sqrt(Dz); + if (*fflags) { + Dz = -Dz; + } + + pos->a = 0.0; + pos->b = 0.0; + pos->c = 0.0; + + return 0; + +#undef AD +#undef BD +#undef CD +#undef Dx +#undef Dy +#undef Dz +} + +int kinematicsInverse(const EmcPose * pos, + double * joints, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) +{ + (void)iflags; +#define AD (joints[0]) +#define BD (joints[1]) +#define CD (joints[2]) +#define Dx (pos->tran.x) +#define Dy (pos->tran.y) +#define Dz (pos->tran.z) + + AD = sqrt(sq(Dx) + sq(Dy) + sq(Dz)); + BD = sqrt(sq(Dx - Bx) + sq(Dy) + sq(Dz)); + CD = sqrt(sq(Dx - Cx) + sq(Dy - Cy) + sq(Dz)); + + *fflags = 0; + if (Dz < 0.0) { + *fflags = 1; + } + + return 0; + +#undef AD +#undef BD +#undef CD +#undef Dx +#undef Dy +#undef Dz +} + +KINEMATICS_TYPE kinematicsType() +{ + return KINEMATICS_BOTH; +} + +#ifdef MAIN + +#include +#include + +/* + Interactive testing of kins. + + Syntax: a.out +*/ +int main(int argc, char *argv[]) +{ +#ifndef BUFFERLEN +#define BUFFERLEN 256 +#endif + char buffer[BUFFERLEN]; + char cmd[BUFFERLEN]; + EmcPose pos, vel; + double joints[3]={0.0,0.0,0.0}, jointvels[3]={0.0,0.0,0.0}; + char inverse; + char flags; + KINEMATICS_FORWARD_FLAGS fflags; + + inverse = 0; /* forwards, by default */ + flags = 0; /* didn't provide flags */ + fflags = 0; /* above xy plane, by default */ + if (argc != 4 || + 1 != sscanf(argv[1], "%lf", &Bx) || + 1 != sscanf(argv[2], "%lf", &Cx) || + 1 != sscanf(argv[3], "%lf", &Cy)) { + fprintf(stderr, "syntax: %s Bx Cx Cy\n", argv[0]); + return 1; + } + + while (! feof(stdin)) { + if (inverse) { + printf("inv> "); + } + else { + printf("fwd> "); + } + fflush(stdout); + + if (NULL == fgets(buffer, BUFFERLEN, stdin)) { + break; + } + if (1 != sscanf(buffer, "%255s", cmd)) { + continue; + } + + if (! strcmp(cmd, "quit")) { + break; + } + if (! strcmp(cmd, "i")) { + inverse = 1; + continue; + } + if (! strcmp(cmd, "f")) { + inverse = 0; + continue; + } + if (! strcmp(cmd, "ff")) { + if (1 != sscanf(buffer, "%*s %lu", &fflags)) { + printf("need forward flag\n"); + } + continue; + } + + if (inverse) { /* inverse kins */ + if (3 != sscanf(buffer, "%lf %lf %lf", + &pos.tran.x, + &pos.tran.y, + &pos.tran.z)) { + printf("need X Y Z\n"); + continue; + } + if (0 != kinematicsInverse(&pos, joints, NULL, &fflags)) { + printf("inverse kin error\n"); + } + else { + printf("%f\t%f\t%f\n", joints[0], joints[1], joints[2]); + if (0 != kinematicsForward(joints, &pos, &fflags, NULL)) { + printf("forward kin error\n"); + } + else { + printf("%f\t%f\t%f\n", pos.tran.x, pos.tran.y, pos.tran.z); + } + } + } + else { /* forward kins */ + if (flags) { + if (4 != sscanf(buffer, "%lf %lf %lf %lu", + &joints[0], + &joints[1], + &joints[2], + &fflags)) { + printf("need 3 strut values and flag\n"); + continue; + } + } + else { + if (3 != sscanf(buffer, "%lf %lf %lf", + &joints[0], + &joints[1], + &joints[2])) { + printf("need 3 strut values\n"); + continue; + } + } + if (0 != kinematicsForward(joints, &pos, &fflags, NULL)) { + printf("forward kin error\n"); + } + else { + printf("%f\t%f\t%f\n", pos.tran.x, pos.tran.y, pos.tran.z); + if (0 != kinematicsInverse(&pos, joints, NULL, &fflags)) { + printf("inverse kin error\n"); + } + else { + printf("%f\t%f\t%f\n", joints[0], joints[1], joints[2]); + } + } + } + } /* end while (! feof(stdin)) */ + + return 0; +} + +#endif /* MAIN */ + +KINS_NOT_SWITCHABLE +EXPORT_SYMBOL(kinematicsType); +EXPORT_SYMBOL(kinematicsForward); +EXPORT_SYMBOL(kinematicsInverse); + +MODULE_LICENSE("GPL"); + + + +int comp_id; +int rtapi_app_main(void) { + int res = 0; + + comp_id = hal_init("tripodkins"); + if(comp_id < 0) return comp_id; + + haldata = hal_malloc(sizeof(struct haldata)); + if(!haldata) goto error; + + if((res = hal_pin_float_new("tripodkins.Bx", HAL_IO, &(haldata->bx), comp_id)) < 0) goto error; + if((res = hal_pin_float_new("tripodkins.Cx", HAL_IO, &(haldata->cx), comp_id)) < 0) goto error; + if((res = hal_pin_float_new("tripodkins.Cy", HAL_IO, &(haldata->cy), comp_id)) < 0) goto error; + + Bx = Cx = Cy = 1.0; + hal_ready(comp_id); + return 0; + +error: + hal_exit(comp_id); + return res; +} + +void rtapi_app_exit(void) { hal_exit(comp_id); }