diff --git a/wasm-port/docs/drift-report.md b/wasm-port/docs/drift-report.md index eebdf8f..daf8982 100644 --- a/wasm-port/docs/drift-report.md +++ b/wasm-port/docs/drift-report.md @@ -47,11 +47,12 @@ semantic rewrites: - No browser/WASM parity tests yet. - No JS SDK validation yet. - No OPFS persistence validation yet. -- Identity/trivial kinematics, LinuxCNC `5axiskins`, and TRT `xyzac`/`xyzbc` - now have native source-probe coverage; serial, delta, SCARA, and other - machine-specific kinematics are not yet extracted. -- Full machine baselines beyond the current identity/trivial, `5axiskins`, and - TRT probes are not established. +- Identity/trivial, `5axiskins`, TRT `xyzac`/`xyzbc`, delta, SCARA, PUMA, + serial, hexapod, pentapod, and related kinematics sources now have native + source-probe coverage. +- Full machine baselines beyond the current identity/trivial, `5axiskins`, + TRT, delta, SCARA, PUMA, and other existing runtime 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. diff --git a/wasm-port/docs/source-reuse-map.md b/wasm-port/docs/source-reuse-map.md index 2514fa7..cce0990 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`, `tripodkins.c`, `scarakins.c`, `pumakins.c`, `pumakins.h` | 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`, `linuxcnc_scara_kinematics_probe`, `linuxcnc_puma_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`, `scarakins.c`, `pumakins.c`, `pumakins.h`, `genhexkins.c`, `genhexkins.h`, `genserfuncs.c`, `genserkins.c`, `genserkins.h`, `ugenserkins.c`, `pentakins.c`, `pentakins.h`, `cubic.c` | Copy unchanged | HAL pin allocation, HAL parameter allocation, HAL component lifecycle, RTAPI module metadata, and userspace test-program process entry remain standalone runtime edges; forward/inverse behavior remains LinuxCNC source where the module exposes it | 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`, `linuxcnc_scara_kinematics_probe`, `linuxcnc_puma_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 | @@ -62,8 +62,9 @@ Current validation is intentionally mechanical: ## Known Gaps -- Additional non-trivial kinematics implementation files, including serial, - hexapod, and other machine-specific modules, are not yet extracted. +- Additional non-trivial kinematics implementation files are now extracted at + source-probe level. Full runtime machine baselines for serial, hexapod, + pentapod, and userspace genser flows are not yet established. - 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_hal_adapter.cpp b/wasm-port/runtime/core/linuxcnc_wrap/linuxcnc_hal_adapter.cpp index 3875bd9..3a8d053 100644 --- a/wasm-port/runtime/core/linuxcnc_wrap/linuxcnc_hal_adapter.cpp +++ b/wasm-port/runtime/core/linuxcnc_wrap/linuxcnc_hal_adapter.cpp @@ -250,6 +250,15 @@ int hal_pin_s64_newf(hal_pin_dir_t dir, hal_s64_t **data_ptr_addr, int comp_id, return rc; } +int hal_param_float_newf(hal_pin_dir_t, hal_float_t *data_addr, int, const char *, ...) +{ + if (!data_addr) { + return -1; + } + *data_addr = 0.0; + return 0; +} + int hal_get_pin_value_by_name(const char *name, hal_type_t *type, hal_data_u **ptr, bool *connected) { diff --git a/wasm-port/runtime/core/shims/hal.h b/wasm-port/runtime/core/shims/hal.h index ea34972..a211939 100644 --- a/wasm-port/runtime/core/shims/hal.h +++ b/wasm-port/runtime/core/shims/hal.h @@ -21,6 +21,7 @@ typedef enum { HAL_IN = 16, HAL_OUT = 32, HAL_IO = (HAL_IN | HAL_OUT), + HAL_RW = HAL_IO, } hal_pin_dir_t; typedef union { @@ -56,6 +57,7 @@ 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_param_float_newf(hal_pin_dir_t, hal_float_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 **); diff --git a/wasm-port/tests/native/verify_native_probes.sh b/wasm-port/tests/native/verify_native_probes.sh index 4f60673..2e0fdb9 100755 --- a/wasm-port/tests/native/verify_native_probes.sh +++ b/wasm-port/tests/native/verify_native_probes.sh @@ -148,6 +148,7 @@ check_exitcode linuxcnc_parameter_file_harness check_exitcode linuxcnc_parameter_file_harness.run check_exitcode linuxcnc_rs274_compile_probe check_exitcode linuxcnc_kins_util_source_probe +check_exitcode linuxcnc_cubic_source_probe check_exitcode linuxcnc_trivkins_source_probe check_exitcode linuxcnc_switchkins_source_probe check_exitcode linuxcnc_userkfuncs_source_probe @@ -165,6 +166,11 @@ check_exitcode linuxcnc_scorbot_kins_source_probe check_exitcode linuxcnc_tripodkins_source_probe check_exitcode linuxcnc_scarakins_source_probe check_exitcode linuxcnc_pumakins_source_probe +check_exitcode linuxcnc_genhexkins_source_probe +check_exitcode linuxcnc_genserfuncs_source_probe +check_exitcode linuxcnc_genserkins_source_probe +check_exitcode linuxcnc_ugenserkins_source_probe +check_exitcode linuxcnc_pentakins_source_probe check_exitcode linuxcnc_interp_convert_source_probe check_exitcode linuxcnc_interp_read_source_probe check_exitcode linuxcnc_interp_check_source_probe diff --git a/wasm-port/tools/build_native_probes.sh b/wasm-port/tools/build_native_probes.sh index 217389d..011ea96 100755 --- a/wasm-port/tools/build_native_probes.sh +++ b/wasm-port/tools/build_native_probes.sh @@ -1036,6 +1036,12 @@ build_object_target \ "$VENDOR_DIR/src/emc/kinematics/kins_util.c" \ TP_FLAGS +build_object_target \ + linuxcnc_cubic_source_probe \ + "$BUILD_DIR/linuxcnc_cubic_source_probe.o" \ + "$VENDOR_DIR/src/emc/kinematics/cubic.c" \ + TP_FLAGS + build_object_target \ linuxcnc_trivkins_source_probe \ "$BUILD_DIR/linuxcnc_trivkins_source_probe.o" \ @@ -1138,6 +1144,36 @@ build_object_target \ "$VENDOR_DIR/src/emc/kinematics/pumakins.c" \ TP_FLAGS +build_object_target \ + linuxcnc_genhexkins_source_probe \ + "$BUILD_DIR/linuxcnc_genhexkins_source_probe.o" \ + "$VENDOR_DIR/src/emc/kinematics/genhexkins.c" \ + TP_FLAGS + +build_object_target \ + linuxcnc_genserfuncs_source_probe \ + "$BUILD_DIR/linuxcnc_genserfuncs_source_probe.o" \ + "$VENDOR_DIR/src/emc/kinematics/genserfuncs.c" \ + TP_FLAGS + +build_object_target \ + linuxcnc_genserkins_source_probe \ + "$BUILD_DIR/linuxcnc_genserkins_source_probe.o" \ + "$VENDOR_DIR/src/emc/kinematics/genserkins.c" \ + TP_FLAGS + +build_object_target \ + linuxcnc_ugenserkins_source_probe \ + "$BUILD_DIR/linuxcnc_ugenserkins_source_probe.o" \ + "$VENDOR_DIR/src/emc/kinematics/ugenserkins.c" \ + TP_FLAGS + +build_object_target \ + linuxcnc_pentakins_source_probe \ + "$BUILD_DIR/linuxcnc_pentakins_source_probe.o" \ + "$VENDOR_DIR/src/emc/kinematics/pentakins.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 c062294..54c62d3 100644 --- a/wasm-port/tools/source-manifest.txt +++ b/wasm-port/tools/source-manifest.txt @@ -30,6 +30,7 @@ src/emc/motion/motion.h src/emc/motion/mot_priv.h src/emc/motion/axis.h src/emc/kinematics/kinematics.h +src/emc/kinematics/cubic.c src/emc/kinematics/cubic.h src/emc/kinematics/kins_util.c src/emc/kinematics/trivkins.c @@ -53,6 +54,14 @@ src/emc/kinematics/tripodkins.c src/emc/kinematics/scarakins.c src/emc/kinematics/pumakins.h src/emc/kinematics/pumakins.c +src/emc/kinematics/genhexkins.h +src/emc/kinematics/genhexkins.c +src/emc/kinematics/genserkins.h +src/emc/kinematics/genserfuncs.c +src/emc/kinematics/genserkins.c +src/emc/kinematics/ugenserkins.c +src/emc/kinematics/pentakins.h +src/emc/kinematics/pentakins.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/cubic.c b/wasm-port/vendor/linuxcnc/src/emc/kinematics/cubic.c new file mode 100644 index 0000000..6a0fbe8 --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/emc/kinematics/cubic.c @@ -0,0 +1,422 @@ +/******************************************************************** +* Description: cubic.c +* Cubic spline interpolation code +* Analysis taken in part from Curtis S. Wilson, "How Close Do You +* Have to Specify Points In a Contouring Application?", Delta Tau +* Data Systems (unpublished). +* +* Derived from a work by Fred Proctor & Will Shackleford +* +* Author: +* License: GPL Version 2 +* System: Linux +* +* Copyright (c) 2004 All rights reserved. +********************************************************************/ +#include + +#include "cubic.h" + +#define SEGMENT_TIME_SET 0x01 +#define INTERPOLATION_RATE_SET 0x02 +#define ALL_SET (SEGMENT_TIME_SET | INTERPOLATION_RATE_SET) + +/* + cubicCoeff calculates the coefficients of the cubic spline + fit to the values of x and v at 0 and t. + deltaT is the length of the interval. + The return value is a CUBIC_COEFF. + + Derivation: + + Solve + a t^3 + b t^2 + c t + d = x, + 3a t^2 + 2b t + c = v + + for a, b, c, and d, given + + x(t=0), v(t=0), + x(t=deltaT), v(t=deltaT) +*/ +static CUBIC_COEFF cubicCoeff(double x0, double v0, + double xn, double vn, double deltaT) +{ + CUBIC_COEFF retval; + + /* first the easy ones */ + retval.d = x0; + retval.c = v0; + + /* now the hard ones */ + retval.b = 3 * (xn - x0) / (deltaT * deltaT) - (2 * v0 + vn) / deltaT; + retval.a = (vn - v0) / (3.0 * (deltaT * deltaT)) - + (2.0 * retval.b) / (3.0 * deltaT); + + return retval; +} + +/* + Interpolate points along a cubic, given t and cubic params +*/ +static double interpolateCubic(CUBIC_COEFF coeff, double t) +{ + return coeff.a * (t * t * t) + coeff.b * (t * t) + coeff.c * t + + coeff.d; +} + +/* + Interpolate velocity given same cubic params as above, using + differentiation of cubic coeffs +*/ +static double interpolateVel(CUBIC_COEFF coeff, double t) +{ + return 3.0 * coeff.a * (t * t) + 2.0 * coeff.b * t + coeff.c; +} + +/* + Interpolate acceleration given same cubic params as above, using + differentiation twice of cubic coeffs +*/ +static double interpolateAccel(CUBIC_COEFF coeff, double t) +{ + return 6.0 * coeff.a * t + 2.0 * coeff.b; +} + +/* + Interpolate jerk given same cubic params as above, using + triple differentiation of cubic coeffs +*/ +static double interpolateJerk(CUBIC_COEFF coeff, double t) +{ + (void)t; + return 6.0 * coeff.a; +} + +/* + Calculate the cubic spline way point, given a point and its + previous and successive neighbors +*/ +static double wayPoint(double xMinus1, double x, double xPlus1) +{ + return (xMinus1 + 4.0 * x + xPlus1) / 6.0; +} + +/* + Calculate the cubic spline velocity value, given a point and its + previous and successive neighbors +*/ +static double velPoint(double xMinus1, double xPlus1, double deltaT) +{ + if (deltaT <= 0.0) { + return 0.0; + } else { + return (xPlus1 - xMinus1) / (2.0 * deltaT); + } +} + +int cubicInit(CUBIC_STRUCT * ci) +{ + if (0 == ci) { + return -1; + } + + ci->configured = 0; + ci->segmentTime = 0.0; + ci->interpolationRate = 0; + ci->interpolationIncrement = 0.0; + cubicDrain(ci); + + return 0; +} + +int cubicSetSegmentTime(CUBIC_STRUCT * ci, double time) +{ + if (0 == ci || time <= 0.0) { + return -1; + } + + ci->segmentTime = time; + ci->configured |= SEGMENT_TIME_SET; + if (ci->configured == ALL_SET) { + ci->interpolationIncrement = + ci->segmentTime / ci->interpolationRate; + } + + return 0; +} + +double cubicGetSegmentTime(CUBIC_STRUCT * ci) +{ + if (0 == ci || !(ci->configured & SEGMENT_TIME_SET)) { + return 0.0; + } + + return ci->segmentTime; +} + +int cubicSetInterpolationRate(CUBIC_STRUCT * ci, int rate) +{ + if (0 == ci || rate <= 0) { + return -1; + } + + ci->interpolationRate = rate; + ci->configured |= INTERPOLATION_RATE_SET; + if (ci->configured == ALL_SET) { + ci->interpolationIncrement = + ci->segmentTime / ci->interpolationRate; + } + + return 0; +} + +int cubicGetInterpolationRate(CUBIC_STRUCT * ci) +{ + if (0 == ci || !(ci->configured & INTERPOLATION_RATE_SET)) { + return 0; + } + + return ci->interpolationRate; +} + +double cubicGetInterpolationIncrement(CUBIC_STRUCT * ci) +{ + if (0 == ci || ci->configured != ALL_SET) { + return 0; + } + + return ci->interpolationIncrement; +} + +CUBIC_COEFF cubicGetCubicCoeff(CUBIC_STRUCT * ci) +{ + CUBIC_COEFF errorReturn; + + if (0 == ci || !ci->filled) { + errorReturn.a = 0.0; + errorReturn.b = 0.0; + errorReturn.c = 0.0; + errorReturn.d = 0.0; + + return errorReturn; + } + + return ci->coeff; +} + +/* + cubicAddPoint(double point) + + Add a point to the end of the cubic interpolator. + + Can only be called to initially fill up the four-point queue required + for interpolation, or when interpolate() has been called for the full + segment and the needNextPoint() flag is non-zero. + + The first point added fills the first two slots, since interpolation + is done between the second and third input point, and this filling + is required so that the output interpolation matches with the input + points. +*/ +int cubicAddPoint(CUBIC_STRUCT * ci, double point) +{ + if (0 == ci || !(ci->configured == ALL_SET)) { + return -1; + } + + if (!ci->needNextPoint) { + return -1; + } + + if (!ci->filled) { + ci->x0 = point; + ci->x1 = point; + ci->x2 = point; + ci->x3 = point; + ci->filled = 1; + } else { + ci->x0 = ci->x1; + ci->x1 = ci->x2; + ci->x2 = ci->x3; + ci->x3 = point; + } + + /* calculate way points and coeff */ + ci->wp0 = wayPoint(ci->x0, ci->x1, ci->x2); + ci->wp1 = wayPoint(ci->x1, ci->x2, ci->x3); + ci->velp0 = velPoint(ci->x0, ci->x2, ci->segmentTime); + ci->velp1 = velPoint(ci->x1, ci->x3, ci->segmentTime); + ci->coeff = cubicCoeff(ci->wp0, ci->velp0, ci->wp1, + ci->velp1, ci->segmentTime); + ci->interpolationTime = 0.0; + ci->needNextPoint = 0; + + return 0; +} + +/* + cubicOffset(CUBIC_STRUCT * ci, double offset) + + Set the interpolator so that the points inside are offset by the given + value, as if the original points all had this offset added to them prior + to their addition to the interpolator. This is used when offsetting + trajectory points, to keep the interpolators consistent without draining + them. +*/ +int cubicOffset(CUBIC_STRUCT * ci, double offset) +{ + if (0 == ci || !(ci->configured == ALL_SET)) { + return -1; + } + + ci->x0 += offset; + ci->x1 += offset; + ci->x2 += offset; + ci->x3 += offset; + ci->wp0 += offset; + ci->wp1 += offset; + + /* leave velp0, velp1 alone, since these are velocity points and are + unaffected by position offsets */ + + /* only the D coeff is affected, so we can change this directly */ + ci->coeff.d += offset; + + return 0; +} + +int cubicFilled(CUBIC_STRUCT * ci) +{ + if (0 == ci) { + return 0; + } + + return ci->filled; +} + +double cubicInterpolate(CUBIC_STRUCT * ci, + double *x, double *v, double *a, double *j) +{ + double retval; + + if (0 == ci || !(ci->configured == ALL_SET)) { + return 0.0; + } + + if (ci->needNextPoint) { + /* queue ran out-- fill right with last point */ + cubicAddPoint(ci, ci->x3); + } + + retval = interpolateCubic(ci->coeff, ci->interpolationTime); + + /* do optional ones */ + if (x != 0) { + *x = retval; + } + if (v != 0) { + *v = interpolateVel(ci->coeff, ci->interpolationTime); + } + if (a != 0) { + *a = interpolateAccel(ci->coeff, ci->interpolationTime); + } + if (j != 0) { + *j = interpolateJerk(ci->coeff, ci->interpolationTime); + } + + ci->interpolationTime += ci->interpolationIncrement; + + /* check to see if the next point is at (close to) the segment end */ + if (fabs(ci->segmentTime - ci->interpolationTime) + < 0.5 * ci->interpolationIncrement) { + /* just computed last point-- flag that we need a new one */ + ci->needNextPoint = 1; + } + + return retval; +} + +int cubicNeedNextPoint(CUBIC_STRUCT * ci) +{ + return ci->needNextPoint; +} + +int cubicDrain(CUBIC_STRUCT * ci) +{ + ci->x0 = ci->x1 = ci->x2 = ci->x3 = 0.0; + ci->wp0 = ci->wp1 = 0.0; + ci->velp0 = ci->velp1 = 0.0; + ci->filled = 0; + ci->needNextPoint = 1; + ci->coeff.a = 0.0; + ci->coeff.b = 0.0; + ci->coeff.c = 0.0; + ci->coeff.d = 0.0; + + return 0; +} + +#ifdef MAIN + +#include + +/* + syntax: testcubic +*/ +int main(int argc, char *argv[]) +{ + CUBIC_STRUCT cubic; + double segmentTime; + int interpolationRate; + double xin; + double xout; + double time = 0.0; + + if (argc != 3) { + fprintf(stderr, "syntax: %s \n", + argv[0]); + return 1; + } + + if (1 != sscanf(argv[1], "%lf", &segmentTime) || segmentTime <= 0.0) { + fprintf(stderr, "invalid segment time %s\n", argv[1]); + return 1; + } + + if (1 != sscanf(argv[2], "%d", &interpolationRate) || + interpolationRate <= 0) { + fprintf(stderr, "invalid interpolation rate %s\n", argv[2]); + return 1; + } + + if (0 != cubicInit(&cubic)) { + fprintf(stderr, "can't initialize interpolator\n"); + return 1; + } + + if (0 != cubicSetSegmentTime(&cubic, segmentTime)) { + fprintf(stderr, "can't set segment time\n"); + return 1; + } + + if (0 != cubicSetInterpolationRate(&cubic, interpolationRate)) { + fprintf(stderr, "can't set interpolation rate\n"); + return 1; + } + + while (!feof(stdin)) { + if (cubicNeedNextPoint(&cubic)) { + if (1 != scanf("%lf", &xin)) { + break; + } else { + cubicAddPoint(&cubic, xin); + } + } + + xout = cubicInterpolate(&cubic, 0, 0, 0, 0); + printf("%f %f\n", time, xout); + time += segmentTime; + } +} + +#endif diff --git a/wasm-port/vendor/linuxcnc/src/emc/kinematics/genhexkins.c b/wasm-port/vendor/linuxcnc/src/emc/kinematics/genhexkins.c new file mode 100644 index 0000000..5a858c2 --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/emc/kinematics/genhexkins.c @@ -0,0 +1,739 @@ +/******************************************************************** +* Description: genhexkins.c +* +* Kinematics for a generalised hexapod machine +* +* Derived from a work by R. Brian Register +* +* Adapting Author: Andrew Kyrychenko +* License: GPL Version 2 +* System: Linux +* +* Copyright (c) 2004 All rights reserved. +********************************************************************* + + These are the forward and inverse kinematic functions for a class of + machines referred to as "Stewart Platforms". + + The functions are general enough to be configured for any platform + configuration. In the functions "genhexKinematicsForward" and + "genhexKinematicsInverse" are arrays "a[i]" and "b[i]". The values stored + in these arrays correspond to the positions of the ends of the i'th + strut. The value stored in a[i] is the position of the end of the i'th + strut attached to the platform, in platform coordinates. The value + stored in b[i] is the position of the end of the i'th strut attached + to the base, in base (world) coordinates. + + The default values for base and platform joints positions are defined + in the header file genhexkins.h. The actual values for a particular + machine can be adjusted by hal parameters: + + genhexkins.base.N.x + genhexkins.base.N.y + genhexkins.base.N.z - base joint coordinates. + + genhexkins.platform.N.x + genhexkins.platform.N.y + genhexkins.platform.N.z - platform joint coordinates. + + genhexkins.spindle-offset - added to Z coordinates of all joints to + change the machine origin. Facilitates + adjusting spindle position. + + genhexkins.tool-offset - tool length offset (TCP offset along Z), + implements RTCP function when connected to + motion.tooloffset.Z. + + To avoid joints jump change tool offset (G43, G49) only when the + platform is not tilted (A = B = 0). + + Some hexapods use non-captive screw actuators and universal (cardanic) + joints, thus the strut lengths depend on orientation of joints axes. + Strut length correction is implemented to compensate for this. + The calculations use orientation (unit vectors) of base and platform + joint axes and the lead of actuator screws: + + genhexkins.base-n.N.x + genhexkins.base-n.N.y + genhexkins.base-n.N.z - unit vectors of base joint axes; + + genhexkins.platform-n.N.x + genhexkins.platform-n.N.y + genhexkins.platform-n.N.z - unit vectors of platform joint axes + in platform CS. + genhexkins.screw-lead - lead of strut actuator screw, positive for + right-hand thread. Default is 0 (strut length + correction disabled). + genhexkins.correction.N - pins showing current values of strut length + correction. + + The genhexKinematicsInverse function solves the inverse kinematics using + a closed form algorithm. The inverse kinematics problem is given + the pose of the platform and returns the strut lengths. For this + problem there is only one solution that is always returned correctly. + + The genhexKinematicsForward function solves the forward kinematics using + an iterative algorithm. Due to the iterative nature of this algorithm + the genhexKinematicsForward function requires an initial value to begin the + iterative routine and then converges to the "nearest" solution. The + forward kinematics problem is given the strut lengths and returns the + pose of the platform. For this problem there arein multiple + solutions. The genhexKinematicsForward function will return only one of + these solutions which will be the solution nearest to the initial + value given. It is possible that there are no solutions "near" the + given initial value and the iteration will not converge and no + solution will be returned. Assuming there is a solution "near" the + initial value, the function will always return one correct solution + out of the multiple possible solutions. + + Hal pins to control and observe forward kinematics iterations: + + genhexkins.convergence-criterion - minimum error value that ends + iterations with converged solution; + + genhexkins.limit-iterations - limit of iterations, if exceeded + iterations stop with no convergence; + + genhexkins.max-error - maximum error value, if exceeded iterations + stop with no convergence; + + genhexkins.last-iterations - number of iterations spent for the + last forward kinematics solution; + + genhexkins.max-iterations - maximum number of iterations spent for + a converged solution during current session. + + ----------------------------------------------------------------------------*/ + +#include +#include +#include +#include +#include +#include /* these decls, KINEMATICS_FORWARD_FLAGS */ + +#include "genhexkins.h" +#include "switchkins.h" + +static struct haldata { + hal_float_t *basex[NUM_STRUTS]; + hal_float_t *basey[NUM_STRUTS]; + hal_float_t *basez[NUM_STRUTS]; + hal_float_t *platformx[NUM_STRUTS]; + hal_float_t *platformy[NUM_STRUTS]; + hal_float_t *platformz[NUM_STRUTS]; + hal_float_t *basenx[NUM_STRUTS]; + hal_float_t *baseny[NUM_STRUTS]; + hal_float_t *basenz[NUM_STRUTS]; + hal_float_t *platformnx[NUM_STRUTS]; + hal_float_t *platformny[NUM_STRUTS]; + hal_float_t *platformnz[NUM_STRUTS]; + hal_float_t *correction[NUM_STRUTS]; + hal_float_t *screw_lead; + hal_u32_t *last_iter; + hal_u32_t *max_iter; + hal_u32_t *iter_limit; + hal_float_t *max_error; + hal_float_t *conv_criterion; + hal_float_t *tool_offset; + hal_float_t *spindle_offset; + hal_bit_t *fwd_kins_fail; + + 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; + +} *haldata; + +static int genhex_gui_forward_kins(EmcPose *pos) +{ + *haldata->gui_x = pos->tran.x; + *haldata->gui_y = pos->tran.y; + *haldata->gui_z = pos->tran.z; + *haldata->gui_a = pos->a; + *haldata->gui_b = pos->b; + *haldata->gui_c = pos->c; + return 0; +} // genhex_gui_forward_kins + +/******************************* MatInvert() ***************************/ + +/*----------------------------------------------------------------------------- + This is a function that inverts a 6x6 matrix. +-----------------------------------------------------------------------------*/ + +static int MatInvert(double J[][NUM_STRUTS], double InvJ[][NUM_STRUTS]) +{ + double JAug[NUM_STRUTS][12], m, temp; + int j, k, n; + + /* This function determines the inverse of a 6x6 matrix using + Gauss-Jordan elimination */ + + /* Augment the Identity matrix to the Jacobian matrix */ + + for (j=0; j<=5; ++j){ + for (k=0; k<=5; ++k){ /* Assign J matrix to first 6 columns of AugJ */ + JAug[j][k] = J[j][k]; + } + for(k=6; k<=11; ++k){ /* Assign I matrix to last six columns of AugJ */ + if (k-6 == j){ + JAug[j][k]=1; + } + else{ + JAug[j][k]=0; + } + } + } + + /* Perform Gauss elimination */ + for (k=0; k<=4; ++k){ /* Pivot */ + if ((JAug[k][k]< 0.01) && (JAug[k][k] > -0.01)){ + for (j=k+1;j<=5; ++j){ + if ((JAug[j][k]>0.01) || (JAug[j][k]<-0.01)){ + for (n=0; n<=11;++n){ + temp = JAug[k][n]; + JAug[k][n] = JAug[j][n]; + JAug[j][n] = temp; + } + break; + } + } + } + for (j=k+1; j<=5; ++j){ /* Pivot */ + m = -JAug[j][k] / JAug[k][k]; + for (n=0; n<=11; ++n){ + JAug[j][n]=JAug[j][n] + m*JAug[k][n]; /* (Row j) + m * (Row k) */ + if ((JAug[j][n] < 0.000001) && (JAug[j][n] > -0.000001)){ + JAug[j][n] = 0; + } + } + } + } + + /* Normalization of Diagonal Terms */ + for (j=0; j<=5; ++j){ + m=1/JAug[j][j]; + for(k=0; k<=11; ++k){ + JAug[j][k] = m * JAug[j][k]; + } + } + + /* Perform Gauss Jordan Steps */ + for (k=5; k>=0; --k){ + for(j=k-1; j>=0; --j){ + m = -JAug[j][k]/JAug[k][k]; + for (n=0; n<=11; ++n){ + JAug[j][n] = JAug[j][n] + m * JAug[k][n]; + } + } + } + + /* Assign last 6 columns of JAug to InvJ */ + for (j=0; j<=5; ++j){ + for (k=0; k<=5; ++k){ + InvJ[j][k] = JAug[j][k+6]; + + } + } + + return 0; /* FIXME-- check divisors for 0 above */ +} // MatInvert() + +/******************************** MatMult() *********************************/ + +/*--------------------------------------------------------------------------- + This function simply multiplies a 6x6 matrix by a 1x6 vector + ---------------------------------------------------------------------------*/ + +static void MatMult(double J[][6], const double x[], double Ans[]) +{ + int j, k; + for (j=0; j<=5; ++j){ + Ans[j] = 0; + for (k=0; k<=5; ++k){ + Ans[j] = J[j][k]*x[k]+Ans[j]; + } + } +} // MatMult() + +/* declare arrays for base and platform coordinates */ +static PmCartesian b[NUM_STRUTS]; +static PmCartesian a[NUM_STRUTS]; + +/* declare base and platform joint axes vectors */ + +static PmCartesian nb1[NUM_STRUTS]; +static PmCartesian na0[NUM_STRUTS]; + +/************************genhex_read_hal_pins**************************/ + +static int genhex_read_hal_pins(void) { + int t; + + /* set the base and platform coordinates from hal pin values */ + for (t = 0; t < NUM_STRUTS; t++) { + b[t].x = *haldata->basex[t]; + b[t].y = *haldata->basey[t]; + b[t].z = *haldata->basez[t] + *haldata->spindle_offset + *haldata->tool_offset; + a[t].x = *haldata->platformx[t]; + a[t].y = *haldata->platformy[t]; + a[t].z = *haldata->platformz[t] + *haldata->spindle_offset + *haldata->tool_offset; + + nb1[t].x = *haldata->basenx[t]; + nb1[t].y = *haldata->baseny[t]; + nb1[t].z = *haldata->basenz[t]; + na0[t].x = *haldata->platformnx[t]; + na0[t].y = *haldata->platformny[t]; + na0[t].z = *haldata->platformnz[t]; + + } + return 0; +} // genhex_read_hal_pins() + +/***************************StrutLengthCorrection***************************/ + +static int StrutLengthCorrection(const PmCartesian * StrutVectUnit, + const PmRotationMatrix * RMatrix, + const int strut_number, + double * correction) +{ + PmCartesian nb2, nb3, na1, na2; + double dotprod; + + /* define base joints axis vectors */ + pmCartCartCross(&nb1[strut_number], StrutVectUnit, &nb2); + pmCartCartCross(StrutVectUnit, &nb2, &nb3); + pmCartUnitEq(&nb3); + + /* define platform joints axis vectors */ + pmMatCartMult(RMatrix, &na0[strut_number], &na1); + pmCartCartCross(&na1, StrutVectUnit, &na2); + pmCartUnitEq(&na2); + + /* define dot product */ + pmCartCartDot(&nb3, &na2, &dotprod); + + *correction = *haldata->screw_lead * asin(dotprod) / PM_2_PI; + + return 0; +} // StrutLengthCorrection() + + +/**************** genhexKinematicsForward() *****************/ +static int genhexKinematicsForward(const double * joints, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) +{ + (void)fflags; + (void)iflags; + PmCartesian aw; + PmCartesian InvKinStrutVect,InvKinStrutVectUnit; + PmCartesian q_trans, RMatrix_a, RMatrix_a_cross_Strut; + + double Jacobian[NUM_STRUTS][NUM_STRUTS]; + double InverseJacobian[NUM_STRUTS][NUM_STRUTS]; + double InvKinStrutLength, StrutLengthDiff[NUM_STRUTS]; + double delta[NUM_STRUTS]; + double conv_err = 1.0; + double corr; + + PmRotationMatrix RMatrix; + PmRpy q_RPY; + + int iterate = 1; + int i; + unsigned iteration = 0; + + genhex_read_hal_pins(); + + /* abort on obvious problems, like joints <= 0 */ + /* FIXME-- should check against triangle inequality, so that joints + are never too short to span shared base and platform sides */ + if (joints[0] <= 0.0 || + joints[1] <= 0.0 || + joints[2] <= 0.0 || + joints[3] <= 0.0 || + joints[4] <= 0.0 || + joints[5] <= 0.0) { + return -1; + } + + /* assign a,b,c to roll, pitch, yaw angles */ + q_RPY.r = pos->a * PM_PI / 180.0; + q_RPY.p = pos->b * PM_PI / 180.0; + q_RPY.y = pos->c * PM_PI / 180.0; + + /* Assign translation values in pos to q_trans */ + q_trans.x = pos->tran.x; + q_trans.y = pos->tran.y; + q_trans.z = pos->tran.z; + + /* Enter Newton-Raphson iterative method */ + while (iterate) { + /* check for large error and return error flag if no convergence */ + if ((conv_err > +(*haldata->max_error)) || + (conv_err < -(*haldata->max_error))) { + /* we can't converge */ + *haldata->fwd_kins_fail = 1; + return -2; + }; + + iteration++; + + /* check iteration to see if the kinematics can reach the + convergence criterion and return error flag if it can't */ + if (iteration > *haldata->iter_limit) { + /* we can't converge */ + *haldata->fwd_kins_fail = 1; + return -5; + } + + /* Convert q_RPY to Rotation Matrix */ + pmRpyMatConvert(&q_RPY, &RMatrix); + + /* compute StrutLengthDiff[] by running inverse kins on Cartesian + estimate to get joint estimate, subtract joints to get joint deltas, + and compute inv J while we're at it */ + for (i = 0; i < NUM_STRUTS; i++) { + pmMatCartMult(&RMatrix, &a[i], &RMatrix_a); + pmCartCartAdd(&q_trans, &RMatrix_a, &aw); + pmCartCartSub(&aw, &b[i], &InvKinStrutVect); + if (0 != pmCartUnit(&InvKinStrutVect, &InvKinStrutVectUnit)) { + *haldata->fwd_kins_fail = 1; + return -1; + } + pmCartMag(&InvKinStrutVect, &InvKinStrutLength); + + if (*haldata->screw_lead != 0.0) { + /* enable strut length correction */ + StrutLengthCorrection(&InvKinStrutVectUnit, &RMatrix, i, &corr); + /* define corrected joint lengths */ + InvKinStrutLength += corr; + } + + StrutLengthDiff[i] = InvKinStrutLength - joints[i]; + + /* Determine RMatrix_a_cross_strut */ + pmCartCartCross(&RMatrix_a, &InvKinStrutVectUnit, &RMatrix_a_cross_Strut); + + /* Build Inverse Jacobian Matrix */ + InverseJacobian[i][0] = InvKinStrutVectUnit.x; + InverseJacobian[i][1] = InvKinStrutVectUnit.y; + InverseJacobian[i][2] = InvKinStrutVectUnit.z; + InverseJacobian[i][3] = RMatrix_a_cross_Strut.x; + InverseJacobian[i][4] = RMatrix_a_cross_Strut.y; + InverseJacobian[i][5] = RMatrix_a_cross_Strut.z; + } + + /* invert Inverse Jacobian */ + MatInvert(InverseJacobian, Jacobian); + + /* multiply Jacobian by LegLengthDiff */ + MatMult(Jacobian, StrutLengthDiff, delta); + + /* subtract delta from last iterations pos values */ + q_trans.x -= delta[0]; + q_trans.y -= delta[1]; + q_trans.z -= delta[2]; + q_RPY.r -= delta[3]; + q_RPY.p -= delta[4]; + q_RPY.y -= delta[5]; + + /* determine value of conv_error (used to determine if no convergence) */ + conv_err = 0.0; + for (i = 0; i < NUM_STRUTS; i++) { + conv_err += fabs(StrutLengthDiff[i]); + } + + /* enter loop to determine if a strut needs another iteration */ + iterate = 0; /*assume iteration is done */ + for (i = 0; i < NUM_STRUTS; i++) { + if (fabs(StrutLengthDiff[i]) > *haldata->conv_criterion) { + iterate = 1; + } + } + } /* exit Newton-Raphson Iterative loop */ + + /* assign r,p,y to a,b,c */ + pos->a = q_RPY.r * 180.0 / PM_PI; + pos->b = q_RPY.p * 180.0 / PM_PI; + pos->c = q_RPY.y * 180.0 / PM_PI; + + /* assign q_trans to pos */ + pos->tran.x = q_trans.x; + pos->tran.y = q_trans.y; + pos->tran.z = q_trans.z; + + *haldata->last_iter = iteration; + + if (iteration > *haldata->max_iter){ + *haldata->max_iter = iteration; + } + *haldata->fwd_kins_fail = 0; + + genhex_gui_forward_kins(pos); + + return 0; +} // genhexKinematicsForward() + + +/************************ genhexKinematicsInverse() ************************/ +/* the inverse kinematics take world coordinates and determine joint values, + given the inverse kinematics flags to resolve any ambiguities. The forward + flags are set to indicate their value appropriate to the world coordinates + passed in. */ + +static int genhexKinematicsInverse(const EmcPose * pos, + double * joints, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) +{ + (void)iflags; + (void)fflags; + + PmCartesian aw, temp; + PmCartesian InvKinStrutVect, InvKinStrutVectUnit; + PmRotationMatrix RMatrix; + PmRpy rpy; + int i; + double InvKinStrutLength, corr; + + genhex_read_hal_pins(); + + /* define Rotation Matrix */ + rpy.r = pos->a * PM_PI / 180.0; + rpy.p = pos->b * PM_PI / 180.0; + rpy.y = pos->c * PM_PI / 180.0; + pmRpyMatConvert(&rpy, &RMatrix); + + /* enter for loop to calculate joints (strut lengths) */ + for (i = 0; i < NUM_STRUTS; i++) { + /* convert location of platform strut end from platform + to world coordinates */ + pmMatCartMult(&RMatrix, &a[i], &temp); + pmCartCartAdd(&pos->tran, &temp, &aw); + + /* define strut lengths */ + pmCartCartSub(&aw, &b[i], &InvKinStrutVect); + pmCartMag(&InvKinStrutVect, &InvKinStrutLength); + + if (*haldata->screw_lead != 0.0) { + /* enable strut length correction */ + /* define unit strut vector */ + if (0 != pmCartUnit(&InvKinStrutVect, &InvKinStrutVectUnit)) { + return -1; + } + /* define correction value and corrected joint lengths */ + StrutLengthCorrection(&InvKinStrutVectUnit, &RMatrix, i, &corr); + *haldata->correction[i] = corr; + InvKinStrutLength += corr; + } + + joints[i] = InvKinStrutLength; + } + + return 0; +} //genhexKinematicsInverse() + +static +int genhexKinematicsSetup(const int comp_id, + const char* coordinates, + kparms* kp) +{ + (void)coordinates; + int i,res=0; + + haldata = hal_malloc(sizeof(struct haldata)); + if (!haldata) { + rtapi_print_msg(RTAPI_MSG_ERR,"genhexKinematicsSetup: hal_malloc fail\n"); + return -1; + } + + for (i = 0; i < kp->max_joints; i++) { + res += hal_pin_float_newf(HAL_IN, &(haldata->basex[i]), comp_id, + "%s.base.%d.x", kp->halprefix, i); + res += hal_pin_float_newf(HAL_IN, &haldata->basey[i], comp_id, + "%s.base.%d.y", kp->halprefix, i); + res += hal_pin_float_newf(HAL_IN, &haldata->basez[i], comp_id, + "%s.base.%d.z", kp->halprefix, i); + res += hal_pin_float_newf(HAL_IN, &haldata->platformx[i], comp_id, + "%s.platform.%d.x", kp->halprefix, i); + res += hal_pin_float_newf(HAL_IN, &haldata->platformy[i], comp_id, + "%s.platform.%d.y", kp->halprefix, i); + res += hal_pin_float_newf(HAL_IN, &haldata->platformz[i], comp_id, + "%s.platform.%d.z", kp->halprefix, i); + res += hal_pin_float_newf(HAL_IN, &haldata->basenx[i], comp_id, + "%s.base-n.%d.x", kp->halprefix, i); + res += hal_pin_float_newf(HAL_IN, &haldata->baseny[i], comp_id, + "%s.base-n.%d.y", kp->halprefix, i); + res += hal_pin_float_newf(HAL_IN, &haldata->basenz[i], comp_id, + "%s.base-n.%d.z", kp->halprefix, i); + res += hal_pin_float_newf(HAL_IN, &haldata->platformnx[i], comp_id, + "%s.platform-n.%d.x", kp->halprefix, i); + res += hal_pin_float_newf(HAL_IN, &haldata->platformny[i], comp_id, + "%s.platform-n.%d.y", kp->halprefix, i); + res += hal_pin_float_newf(HAL_IN, &haldata->platformnz[i], comp_id, + "%s.platform-n.%d.z", kp->halprefix, i); + res += hal_pin_float_newf(HAL_OUT, &haldata->correction[i], comp_id, + "%s.correction.%d", kp->halprefix, i); + if (res) {goto error;} + *haldata->correction[i] = 0.0; + + } + + res += hal_pin_u32_newf(HAL_OUT, &haldata->last_iter, comp_id, + "genhexkins.last-iterations"); + *haldata->last_iter = 0; + res += hal_pin_u32_newf(HAL_OUT, &haldata->max_iter, comp_id, + "genhexkins.max-iterations"); + *haldata->max_iter = 0; + res += hal_pin_float_newf(HAL_IN, &haldata->max_error, comp_id, + "genhexkins.max-error"); + *haldata->max_error = 500.0; + res += hal_pin_float_newf(HAL_IN, &haldata->conv_criterion, comp_id, + "genhexkins.convergence-criterion"); + *haldata->conv_criterion = 1e-9; + res += hal_pin_u32_newf(HAL_IN, &haldata->iter_limit, comp_id, + "genhexkins.limit-iterations"); + *haldata->iter_limit = 120; + res += hal_pin_float_newf(HAL_IN, &haldata->tool_offset, comp_id, + "genhexkins.tool-offset"); + *haldata->tool_offset = 0.0; + res += hal_pin_float_newf(HAL_IN, &haldata->spindle_offset, comp_id, + "genhexkins.spindle-offset"); + *haldata->spindle_offset = 0.0; + res += hal_pin_float_newf(HAL_IN, &haldata->screw_lead, comp_id, + "genhexkins.screw-lead"); + *haldata->screw_lead = DEFAULT_SCREW_LEAD; + + if (res) {goto error;} + + *haldata->basex[0] = DEFAULT_BASE_0_X; + *haldata->basey[0] = DEFAULT_BASE_0_Y; + *haldata->basez[0] = DEFAULT_BASE_0_Z; + *haldata->basex[1] = DEFAULT_BASE_1_X; + *haldata->basey[1] = DEFAULT_BASE_1_Y; + *haldata->basez[1] = DEFAULT_BASE_1_Z; + *haldata->basex[2] = DEFAULT_BASE_2_X; + *haldata->basey[2] = DEFAULT_BASE_2_Y; + *haldata->basez[2] = DEFAULT_BASE_2_Z; + *haldata->basex[3] = DEFAULT_BASE_3_X; + *haldata->basey[3] = DEFAULT_BASE_3_Y; + *haldata->basez[3] = DEFAULT_BASE_3_Z; + *haldata->basex[4] = DEFAULT_BASE_4_X; + *haldata->basey[4] = DEFAULT_BASE_4_Y; + *haldata->basez[4] = DEFAULT_BASE_4_Z; + *haldata->basex[5] = DEFAULT_BASE_5_X; + *haldata->basey[5] = DEFAULT_BASE_5_Y; + *haldata->basez[5] = DEFAULT_BASE_5_Z; + + *haldata->platformx[0] = DEFAULT_PLATFORM_0_X; + *haldata->platformy[0] = DEFAULT_PLATFORM_0_Y; + *haldata->platformz[0] = DEFAULT_PLATFORM_0_Z; + *haldata->platformx[1] = DEFAULT_PLATFORM_1_X; + *haldata->platformy[1] = DEFAULT_PLATFORM_1_Y; + *haldata->platformz[1] = DEFAULT_PLATFORM_1_Z; + *haldata->platformx[2] = DEFAULT_PLATFORM_2_X; + *haldata->platformy[2] = DEFAULT_PLATFORM_2_Y; + *haldata->platformz[2] = DEFAULT_PLATFORM_2_Z; + *haldata->platformx[3] = DEFAULT_PLATFORM_3_X; + *haldata->platformy[3] = DEFAULT_PLATFORM_3_Y; + *haldata->platformz[3] = DEFAULT_PLATFORM_3_Z; + *haldata->platformx[4] = DEFAULT_PLATFORM_4_X; + *haldata->platformy[4] = DEFAULT_PLATFORM_4_Y; + *haldata->platformz[4] = DEFAULT_PLATFORM_4_Z; + *haldata->platformx[5] = DEFAULT_PLATFORM_5_X; + *haldata->platformy[5] = DEFAULT_PLATFORM_5_Y; + *haldata->platformz[5] = DEFAULT_PLATFORM_5_Z; + + *haldata->basenx[0] = DEFAULT_BASE_0_NX; + *haldata->baseny[0] = DEFAULT_BASE_0_NY; + *haldata->basenz[0] = DEFAULT_BASE_0_NZ; + *haldata->basenx[1] = DEFAULT_BASE_1_NX; + *haldata->baseny[1] = DEFAULT_BASE_1_NY; + *haldata->basenz[1] = DEFAULT_BASE_1_NZ; + *haldata->basenx[2] = DEFAULT_BASE_2_NX; + *haldata->baseny[2] = DEFAULT_BASE_2_NY; + *haldata->basenz[2] = DEFAULT_BASE_2_NZ; + *haldata->basenx[3] = DEFAULT_BASE_3_NX; + *haldata->baseny[3] = DEFAULT_BASE_3_NY; + *haldata->basenz[3] = DEFAULT_BASE_3_NZ; + *haldata->basenx[4] = DEFAULT_BASE_4_NX; + *haldata->baseny[4] = DEFAULT_BASE_4_NY; + *haldata->basenz[4] = DEFAULT_BASE_4_NZ; + *haldata->basenx[5] = DEFAULT_BASE_5_NX; + *haldata->baseny[5] = DEFAULT_BASE_5_NY; + *haldata->basenz[5] = DEFAULT_BASE_5_NZ; + + *haldata->platformnx[0] = DEFAULT_PLATFORM_0_NX; + *haldata->platformny[0] = DEFAULT_PLATFORM_0_NY; + *haldata->platformnz[0] = DEFAULT_PLATFORM_0_NZ; + *haldata->platformnx[1] = DEFAULT_PLATFORM_1_NX; + *haldata->platformny[1] = DEFAULT_PLATFORM_1_NY; + *haldata->platformnz[1] = DEFAULT_PLATFORM_1_NZ; + *haldata->platformnx[2] = DEFAULT_PLATFORM_2_NX; + *haldata->platformny[2] = DEFAULT_PLATFORM_2_NY; + *haldata->platformnz[2] = DEFAULT_PLATFORM_2_NZ; + *haldata->platformnx[3] = DEFAULT_PLATFORM_3_NX; + *haldata->platformny[3] = DEFAULT_PLATFORM_3_NY; + *haldata->platformnz[3] = DEFAULT_PLATFORM_3_NZ; + *haldata->platformnx[4] = DEFAULT_PLATFORM_4_NX; + *haldata->platformny[4] = DEFAULT_PLATFORM_4_NY; + *haldata->platformnz[4] = DEFAULT_PLATFORM_4_NZ; + *haldata->platformnx[5] = DEFAULT_PLATFORM_5_NX; + *haldata->platformny[5] = DEFAULT_PLATFORM_5_NY; + *haldata->platformnz[5] = DEFAULT_PLATFORM_5_NZ; + + //note: switchkins does not uses these as it provides gui.x, gui.y, etc. + res += hal_pin_float_newf(HAL_IN, &haldata->gui_x, comp_id, "genhexkins.x"); + res += hal_pin_float_newf(HAL_IN, &haldata->gui_y, comp_id, "genhexkins.y"); + res += hal_pin_float_newf(HAL_IN, &haldata->gui_z, comp_id, "genhexkins.z"); + res += hal_pin_float_newf(HAL_IN, &haldata->gui_a, comp_id, "genhexkins.a"); + res += hal_pin_float_newf(HAL_IN, &haldata->gui_b, comp_id, "genhexkins.b"); + res += hal_pin_float_newf(HAL_IN, &haldata->gui_c, comp_id, "genhexkins.c"); + + res += hal_pin_bit_newf(HAL_OUT, &haldata->fwd_kins_fail, comp_id, + "genhexkins.fwd-kins-fail"); + + if (res) goto error; + return 0; + +error: + return res; +} // genhexKinematicsSetup() + +int switchkinsSetup(kparms* kp, + KS* kset0, KS* kset1, KS* kset2, + KF* kfwd0, KF* kfwd1, KF* kfwd2, + KI* kinv0, KI* kinv1, KI* kinv2 + ) +{ + kp->kinsname = "genhexkins"; // !!! must agree with filename + kp->halprefix = "genhexkins"; // hal pin names + kp->required_coordinates = "xyzabc"; + kp->max_joints = strlen(kp->required_coordinates); + kp->allow_duplicates = 0; + kp->fwd_iterates_mask = 0x1; //genhexkins switchkins_type==0 + kp->gui_kinstype = 0; //vismach gui for switchkins_type==0 + + // switchkins_type==0 is startup default + // kins with iterative forward algorithm should be switchkins_type==0 + *kset0 = genhexKinematicsSetup; + *kfwd0 = genhexKinematicsForward; + *kinv0 = genhexKinematicsInverse; + + *kset1 = identityKinematicsSetup; + *kfwd1 = identityKinematicsForward; + *kinv1 = identityKinematicsInverse; + + *kset2 = userkKinematicsSetup; + *kfwd2 = userkKinematicsForward; + *kinv2 = userkKinematicsInverse; + + return 0; +} //switchkinsSetup() diff --git a/wasm-port/vendor/linuxcnc/src/emc/kinematics/genhexkins.h b/wasm-port/vendor/linuxcnc/src/emc/kinematics/genhexkins.h new file mode 100644 index 0000000..671419e --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/emc/kinematics/genhexkins.h @@ -0,0 +1,131 @@ +/******************************************************************* +* Description: genhexkins.h +* +* Kinematics for a generalised hexapod machine +* +* Derived from a work by R. Brian Register +* +* Adapting Author: Andrew Kyrychenko +* License: GPL Version 2 +* System: Linux +* +* Copyright (c) 2004 All rights reserved. +******************************************************************** + + This is the header file to accompany genhexkins.c. This header file + is used to configure genhexkins.c to solve the kinematics for a default + Stewart Platform configuration. + + */ +#ifndef GENHEXKINS_H +#define GENHEXKINS_H + +#include +#include + +#define GENHEX_MAX_JOINTS 6 +#define NUM_STRUTS 6 // number of struts, only 6 supported for now + +/* Default position of base strut ends in base (world) coordinates */ + +#define DEFAULT_BASE_0_X -22.950 +#define DEFAULT_BASE_1_X 22.950 +#define DEFAULT_BASE_2_X 22.950 +#define DEFAULT_BASE_3_X 0.000 +#define DEFAULT_BASE_4_X 0.000 +#define DEFAULT_BASE_5_X -22.950 + +#define DEFAULT_BASE_0_Y 13.250 +#define DEFAULT_BASE_1_Y 13.250 +#define DEFAULT_BASE_2_Y 13.250 +#define DEFAULT_BASE_3_Y -26.500 +#define DEFAULT_BASE_4_Y -26.500 +#define DEFAULT_BASE_5_Y 13.250 + +#define DEFAULT_BASE_0_Z 0.000 +#define DEFAULT_BASE_1_Z 0.000 +#define DEFAULT_BASE_2_Z 0.000 +#define DEFAULT_BASE_3_Z 0.000 +#define DEFAULT_BASE_4_Z 0.000 +#define DEFAULT_BASE_5_Z 0.000 + +/* Default position of platform strut end in platform coordinate system */ + +#define DEFAULT_PLATFORM_0_X -1.000 +#define DEFAULT_PLATFORM_1_X 1.000 +#define DEFAULT_PLATFORM_2_X 10.459 +#define DEFAULT_PLATFORM_3_X 9.459 +#define DEFAULT_PLATFORM_4_X -9.459 +#define DEFAULT_PLATFORM_5_X -10.459 + +#define DEFAULT_PLATFORM_0_Y 11.500 +#define DEFAULT_PLATFORM_1_Y 11.500 +#define DEFAULT_PLATFORM_2_Y -4.884 +#define DEFAULT_PLATFORM_3_Y -6.616 +#define DEFAULT_PLATFORM_4_Y -6.616 +#define DEFAULT_PLATFORM_5_Y -4.884 + +#define DEFAULT_PLATFORM_0_Z 0.000 +#define DEFAULT_PLATFORM_1_Z 0.000 +#define DEFAULT_PLATFORM_2_Z 0.000 +#define DEFAULT_PLATFORM_3_Z 0.000 +#define DEFAULT_PLATFORM_4_Z 0.000 +#define DEFAULT_PLATFORM_5_Z 0.000 + +/* Default unit vectors of base joints axes in world coordinates */ + +#define DEFAULT_BASE_0_NX 0.707107 +#define DEFAULT_BASE_0_NY 0.0 +#define DEFAULT_BASE_0_NZ 0.707107 + +#define DEFAULT_BASE_1_NX 0.0 +#define DEFAULT_BASE_1_NY -0.707107 +#define DEFAULT_BASE_1_NZ 0.707107 + +#define DEFAULT_BASE_2_NX -0.707107 +#define DEFAULT_BASE_2_NY 0.0 +#define DEFAULT_BASE_2_NZ 0.707107 + +#define DEFAULT_BASE_3_NX -0.707107 +#define DEFAULT_BASE_3_NY 0.0 +#define DEFAULT_BASE_3_NZ 0.707107 + +#define DEFAULT_BASE_4_NX 0.0 +#define DEFAULT_BASE_4_NY 0.707107 +#define DEFAULT_BASE_4_NZ 0.707107 + +#define DEFAULT_BASE_5_NX 0.707107 +#define DEFAULT_BASE_5_NY 0.0 +#define DEFAULT_BASE_5_NZ 0.707107 + +/* Default unit vectors of platform joints axes in platform coordinates */ + +#define DEFAULT_PLATFORM_0_NX -1.0 +#define DEFAULT_PLATFORM_0_NY 0.0 +#define DEFAULT_PLATFORM_0_NZ 0.0 + +#define DEFAULT_PLATFORM_1_NX 0.866025 +#define DEFAULT_PLATFORM_1_NY 0.5 +#define DEFAULT_PLATFORM_1_NZ 0.0 + +#define DEFAULT_PLATFORM_2_NX 0.866025 +#define DEFAULT_PLATFORM_2_NY 0.5 +#define DEFAULT_PLATFORM_2_NZ 0.0 + +#define DEFAULT_PLATFORM_3_NX 0.866025 +#define DEFAULT_PLATFORM_3_NY -0.5 +#define DEFAULT_PLATFORM_3_NZ 0.0 + +#define DEFAULT_PLATFORM_4_NX 0.866025 +#define DEFAULT_PLATFORM_4_NY -0.5 +#define DEFAULT_PLATFORM_4_NZ 0.0 + +#define DEFAULT_PLATFORM_5_NX -1.0 +#define DEFAULT_PLATFORM_5_NY 0.0 +#define DEFAULT_PLATFORM_5_NZ 0.0 + +/* Default lead of strut actuator screw */ + +#define DEFAULT_SCREW_LEAD 0.0 + +#endif diff --git a/wasm-port/vendor/linuxcnc/src/emc/kinematics/genserfuncs.c b/wasm-port/vendor/linuxcnc/src/emc/kinematics/genserfuncs.c new file mode 100644 index 0000000..9512a64 --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/emc/kinematics/genserfuncs.c @@ -0,0 +1,668 @@ +/******************************************************************** +* Description: genserfuncs.c +* (originally part of genserkins.c) +* Kinematics for a generalised serial kinematics machine +* +* Derived from a work by Fred Proctor, +* changed to work with emc2 and HAL +* +* Adapting Author: Alex Joni +* License: GPL Version 2 +* System: Linux +* +* Users: +* 1) genserkins.c -- kinematics modules +* 2) ugenserkins.c -- usermode test program (needs work) +******************************************************************* + + These are the forward and inverse kinematic functions for a general + serial-link manipulator. Thanks to Herman Bruyninckx and John + Hallam at http://www.roble.info/ for this. + + The functions are general enough to be configured for any serial + configuration. + The kinematics use Denavit-Hartenberg definition for the joint and + links. The DH definitions are the ones used by John J Craig in + "Introduction to Robotics: Mechanics and Control" + The parameters for the manipulator are defined by hal pins. + Currently the type of the joints is hardcoded to ANGULAR, although + the kins support both ANGULAR and LINEAR axes. + + TODO: + * make number of joints a loadtime parameter + * add HAL pins for all settable parameters, including joint type: ANGULAR / LINEAR +*/ + +#ifdef RTAPI +#include +#endif +#include +#include +#include /* go_result, go_integer */ +#include /* go_pose */ +#include + +#include "genserkins.h" /* these decls */ + +// Only gcc/g++ supports the #pragma +#if __GNUC__ && !defined(__clang__) +// The matrix and vector storage is just big. +// genser_kin_jac_inv() is 2112 +// genserKinematicsInverse() is 2576 + #pragma GCC diagnostic warning "-Wframe-larger-than=2600" +#endif + +static struct haldata { + hal_u32_t *max_iterations; + hal_u32_t *last_iterations; + hal_float_t *a[GENSER_MAX_JOINTS]; + hal_float_t *alpha[GENSER_MAX_JOINTS]; + hal_float_t *d[GENSER_MAX_JOINTS]; + hal_s32_t *unrotate[GENSER_MAX_JOINTS]; + genser_struct *kins; + go_pose *pos; // used in various functions, we malloc it + // only once in genserKinematicsSetup() +} *haldata = 0; + +static int total_joints; +double j[GENSER_MAX_JOINTS]; + +#define A(i) (*(haldata->a[i])) +#define ALPHA(i) (*(haldata->alpha[i])) +#define D(i) (*(haldata->d[i])) + +#define KINS_PTR (haldata->kins) + +#if GENSER_MAX_JOINTS < 6 +#error GENSER_MAX_JOINTS must be at least 6; fix genserkins.h +#endif + +static int genser_hal_inited = 0; + +int genser_kin_init(void) { + genser_struct *genser = KINS_PTR; + int t; + + static volatile double tst=0;tst=sqrt(tst); // ensure -lm used + /* init them all and make them revolute joints */ + /* FIXME: should allow LINEAR joints based on HAL param too */ + for (t = 0; t < GENSER_MAX_JOINTS; t++) { + genser->links[t].u.dh.a = A(t); + genser->links[t].u.dh.alpha = ALPHA(t); + genser->links[t].u.dh.d = D(t); + genser->links[t].u.dh.theta = 0; + genser->links[t].type = GO_LINK_DH; + genser->links[t].quantity = GO_QUANTITY_ANGLE; + } + + /* set a select few to make it PUMA-like */ + // FIXME-AJ: make a hal pin, also set number of joints based on it + genser->link_num = 6; + + return GO_RESULT_OK; +} + +/* compute the forward jacobian function: + the jacobian is a linear approximation of the kinematics function. + It is calculated using derivation of the position transformation matrix, + and usually used for feeding velocities through it. + It is analytically possible to calculate the inverse of the jacobian + (sometimes only the pseudoinverse) and to use that for the inverse kinematics. +*/ +int compute_jfwd(go_link * link_params, + int link_number, + go_matrix * Jfwd, + go_pose * T_L_0) +{ + GO_MATRIX_DECLARE(Jv, Jvstg, 3, GENSER_MAX_JOINTS); + GO_MATRIX_DECLARE(Jw, Jwstg, 3, GENSER_MAX_JOINTS); + GO_MATRIX_DECLARE(R_i_ip1, R_i_ip1stg, 3, 3); + GO_MATRIX_DECLARE(scratch, scratchstg, 3, GENSER_MAX_JOINTS); + GO_MATRIX_DECLARE(R_inv, R_invstg, 3, 3); + go_pose pose; + go_quat quat; + go_vector P_ip1_i[3]; + int row, col; + + /* init matrices to possibly smaller size */ + go_matrix_init(Jv, Jvstg, 3, link_number); + go_matrix_init(Jw, Jwstg, 3, link_number); + go_matrix_init(R_i_ip1, R_i_ip1stg, 3, 3); + go_matrix_init(scratch, scratchstg, 3, link_number); + go_matrix_init(R_inv, R_invstg, 3, 3); + + Jv.el[0][0] = 0, Jv.el[1][0] = 0, Jv.el[2][0] = (GO_QUANTITY_LENGTH == link_params[0].quantity ? 1 : 0); + Jw.el[0][0] = 0, Jw.el[1][0] = 0, Jw.el[2][0] = (GO_QUANTITY_ANGLE == link_params[0].quantity ? 1 : 0); + + /* initialize inverse rotational transform */ + if (GO_LINK_DH == link_params[0].type) { + go_dh_pose_convert(&link_params[0].u.dh, &pose); + } else if (GO_LINK_PP == link_params[0].type) { + pose = link_params[0].u.pp.pose; + } else { + return GO_RESULT_IMPL_ERROR; + } + + *T_L_0 = pose; + + for (col = 1; col < link_number; col++) { + /* T_ip1_i */ + if (GO_LINK_DH == link_params[col].type) { + go_dh_pose_convert(&link_params[col].u.dh, &pose); + } else if (GO_LINK_PP == link_params[col].type) { + pose = link_params[col].u.pp.pose; + } else { + return GO_RESULT_IMPL_ERROR; + } + + go_cart_vector_convert(&pose.tran, P_ip1_i); + go_quat_inv(&pose.rot, &quat); + go_quat_matrix_convert(&quat, &R_i_ip1); + + /* Jv */ + go_matrix_vector_cross(&Jw, P_ip1_i, &scratch); + go_matrix_matrix_add(&Jv, &scratch, &scratch); + go_matrix_matrix_mult(&R_i_ip1, &scratch, &Jv); + Jv.el[0][col] = 0, Jv.el[1][col] = 0, Jv.el[2][col] = (GO_QUANTITY_LENGTH == link_params[col].quantity ? 1 : 0); + /* Jw */ + go_matrix_matrix_mult(&R_i_ip1, &Jw, &Jw); + Jw.el[0][col] = 0, Jw.el[1][col] = 0, Jw.el[2][col] = (GO_QUANTITY_ANGLE == link_params[col].quantity ? 1 : 0); + if (GO_LINK_DH == link_params[col].type) { + go_dh_pose_convert(&link_params[col].u.dh, &pose); + } else if (GO_LINK_PP == link_params[col].type) { + pose = link_params[col].u.pp.pose; + } else { + return GO_RESULT_IMPL_ERROR; + } + go_pose_pose_mult(T_L_0, &pose, T_L_0); + } + + /* rotate back into {0} frame */ + go_quat_matrix_convert(&T_L_0->rot, &R_inv); + go_matrix_matrix_mult(&R_inv, &Jv, &Jv); + go_matrix_matrix_mult(&R_inv, &Jw, &Jw); + + /* put Jv atop Jw in J */ + for (row = 0; row < 6; row++) { + for (col = 0; col < link_number; col++) { + if (row < 3) { + Jfwd->el[row][col] = Jv.el[row][col]; + } else { + Jfwd->el[row][col] = Jw.el[row - 3][col]; + } + } + } + + return GO_RESULT_OK; +} + +/* compute the inverse of the jacobian matrix */ +int compute_jinv(go_matrix * Jfwd, go_matrix * Jinv) +{ + int retval; + GO_MATRIX_DECLARE(JT, JTstg, GENSER_MAX_JOINTS, 6); + + /* compute inverse, or pseudo-inverse */ + if (Jfwd->rows == Jfwd->cols) { + retval = go_matrix_inv(Jfwd, Jinv); + if (GO_RESULT_OK != retval) + return retval; + } else if (Jfwd->rows < Jfwd->cols) { + /* underdetermined, optimize on smallest sum of square of speeds */ + /* JT(JJT)inv */ + GO_MATRIX_DECLARE(JJT, JJTstg, 6, 6); + + go_matrix_init(JT, JTstg, Jfwd->cols, Jfwd->rows); + go_matrix_init(JJT, JJTstg, Jfwd->rows, Jfwd->rows); + go_matrix_transpose(Jfwd, &JT); + go_matrix_matrix_mult(Jfwd, &JT, &JJT); + retval = go_matrix_inv(&JJT, &JJT); + if (GO_RESULT_OK != retval) + return retval; + go_matrix_matrix_mult(&JT, &JJT, Jinv); + } else { + /* overdetermined, do least-squares best fit */ + /* (JTJ)invJT */ + GO_MATRIX_DECLARE(JTJ, JTJstg, GENSER_MAX_JOINTS, GENSER_MAX_JOINTS); + + go_matrix_init(JT, JTstg, Jfwd->cols, Jfwd->rows); + go_matrix_init(JTJ, JTJstg, Jfwd->cols, Jfwd->cols); + go_matrix_transpose(Jfwd, &JT); + go_matrix_matrix_mult(&JT, Jfwd, &JTJ); + retval = go_matrix_inv(&JTJ, &JTJ); + if (GO_RESULT_OK != retval) + return retval; + go_matrix_matrix_mult(&JTJ, &JT, Jinv); + } + + return GO_RESULT_OK; +} + +int genser_kin_jac_inv(void *kins, + const go_pose * pos, + const go_screw * vel, const go_real * joints, go_real * jointvels) +{ + (void)pos; + genser_struct *genser = (genser_struct *) kins; + GO_MATRIX_DECLARE(Jfwd, Jfwd_stg, 6, GENSER_MAX_JOINTS); + GO_MATRIX_DECLARE(Jinv, Jinv_stg, GENSER_MAX_JOINTS, 6); + go_pose T_L_0; + go_link linkout[GENSER_MAX_JOINTS] = {}; + go_real vw[6]; + int link; + int retval; + + go_matrix_init(Jfwd, Jfwd_stg, 6, genser->link_num); + go_matrix_init(Jinv, Jinv_stg, GENSER_MAX_JOINTS, 6); + + for (link = 0; link < genser->link_num; link++) { + retval = + go_link_joint_set(&genser->links[link], joints[link], + &linkout[link]); + if (GO_RESULT_OK != retval) + return retval; + } + retval = compute_jfwd(linkout, genser->link_num, &Jfwd, &T_L_0); + if (GO_RESULT_OK != retval) + return retval; + retval = compute_jinv(&Jfwd, &Jinv); + if (GO_RESULT_OK != retval) + return retval; + + vw[0] = vel->v.x; + vw[1] = vel->v.y; + vw[2] = vel->v.z; + vw[3] = vel->w.x; + vw[4] = vel->w.y; + vw[5] = vel->w.z; + + return go_matrix_vector_mult(&Jinv, vw, jointvels); +} + +int genser_kin_jac_fwd(void *kins, + const go_real * joints, + const go_real * jointvels, const go_pose * pos, go_screw * vel) +{ + (void)pos; + genser_struct *genser = (genser_struct *) kins; + GO_MATRIX_DECLARE(Jfwd, Jfwd_stg, 6, GENSER_MAX_JOINTS); + go_pose T_L_0; + go_link linkout[GENSER_MAX_JOINTS] = {}; + go_real vw[6]; + int link; + int retval; + + go_matrix_init(Jfwd, Jfwd_stg, 6, genser->link_num); + + for (link = 0; link < genser->link_num; link++) { + retval = + go_link_joint_set(&genser->links[link], joints[link], + &linkout[link]); + if (GO_RESULT_OK != retval) + return retval; + } + + retval = compute_jfwd(linkout, genser->link_num, &Jfwd, &T_L_0); + if (GO_RESULT_OK != retval) + return retval; + + go_matrix_vector_mult(&Jfwd, jointvels, vw); + vel->v.x = vw[0]; + vel->v.y = vw[1]; + vel->v.z = vw[2]; + vel->w.x = vw[3]; + vel->w.y = vw[4]; + vel->w.z = vw[5]; + + return GO_RESULT_OK; +} + +/* main function called by emc2 for forward Kins */ +int genserKinematicsForward(const double *joint, + EmcPose * world, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)fflags; + (void)iflags; + + go_pose *pos; + go_rpy rpy; + go_real jcopy[GENSER_MAX_JOINTS]; // will hold the radian conversion of joints + int ret = 0; + int i, changed=0; + if (!genser_hal_inited) { + rtapi_print_msg(RTAPI_MSG_ERR, + "genserKinematicsForward: not initialized\n"); + return -1; + } + + for (i=0; i< 6; i++) { + // FIXME - debug hack + if (!GO_ROT_CLOSE(j[i],joint[i])) changed = 1; + // convert to radians to pass to genser_kin_fwd + jcopy[i] = joint[i] * PM_PI / 180; + if ((i) && *(haldata->unrotate[i])) + jcopy[i] -= *(haldata->unrotate[i])*jcopy[i-1]; + } + + if (changed) { + for (i=0; i< 6; i++) + j[i] = joint[i]; + // rtapi_print("genserKinematicsForward(joints: %f %f %f %f %f %f)\n", + //joint[0],joint[1],joint[2],joint[3],joint[4],joint[5]); + } + // AJ: convert from emc2 coords (XYZABC - which are actually rpy euler + // angles) + // to go angles (quaternions) + pos = haldata->pos; + rpy.y = world->c * PM_PI / 180; + rpy.p = world->b * PM_PI / 180; + rpy.r = world->a * PM_PI / 180; + + go_rpy_quat_convert(&rpy, &pos->rot); + pos->tran.x = world->tran.x; + pos->tran.y = world->tran.y; + pos->tran.z = world->tran.z; + + //pass through unused 678 as uvw + if (total_joints > 6) world->u = joint[6]; + if (total_joints > 7) world->v = joint[7]; + if (total_joints > 8) world->w = joint[8]; + + // pos will be the world location + // jcopy: joitn position in radians + ret = genser_kin_fwd(KINS_PTR, jcopy, pos); + if (ret < 0) + return ret; + + // AJ: convert back to emc2 coords + ret = go_quat_rpy_convert(&pos->rot, &rpy); + if (ret < 0) + return ret; + world->tran.x = pos->tran.x; + world->tran.y = pos->tran.y; + world->tran.z = pos->tran.z; + world->a = rpy.r * 180 / PM_PI; + world->b = rpy.p * 180 / PM_PI; + world->c = rpy.y * 180 / PM_PI; + + if (changed) { +// rtapi_print("genserKinematicsForward(world: %f %f %f %f %f %f)\n", world->tran.x, world->tran.y, world->tran.z, world->a, world->b, world->c); + } + return 0; +} + +int genser_kin_fwd(void *kins, const go_real * joints, go_pose * pos) +{ + genser_struct *genser = kins; + go_link linkout[GENSER_MAX_JOINTS] = {}; + + int link; + int retval; + + genser_kin_init(); + + for (link = 0; link < genser->link_num; link++) { + retval = go_link_joint_set(&genser->links[link], joints[link], &linkout[link]); + if (GO_RESULT_OK != retval) + return retval; + } + + retval = go_link_pose_build(linkout, genser->link_num, pos); + if (GO_RESULT_OK != retval) + return retval; + + return GO_RESULT_OK; +} + +int genserKinematicsInverse(const EmcPose * world, + double *joints, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) +{ + (void)iflags; + (void)fflags; + + genser_struct *genser = KINS_PTR; + GO_MATRIX_DECLARE(Jfwd, Jfwd_stg, 6, GENSER_MAX_JOINTS); + GO_MATRIX_DECLARE(Jinv, Jinv_stg, GENSER_MAX_JOINTS, 6); + go_pose T_L_0; + go_real dvw[6]; + go_real jest[GENSER_MAX_JOINTS]; + go_real dj[GENSER_MAX_JOINTS]; + go_pose pest, pestinv, Tdelta; // pos = converted pose from EmcPose + go_rpy rpy; + go_rvec rvec; + go_cart cart; + go_link linkout[GENSER_MAX_JOINTS]; + int link; + int smalls; + int retval; + + // rtapi_print("kineInverse(joints: %f %f %f %f %f %f)\n", + // joints[0],joints[1],joints[2],joints[3],joints[4],joints[5]); + // rtapi_print("kineInverse(world: %f %f %f %f %f %f)\n", + // world->tran.x, world->tran.y, world->tran.z, world->a, world->b, world->c); + +#ifndef ULAPI + genser_kin_init(); + if (!genser_hal_inited) { + rtapi_print_msg(RTAPI_MSG_ERR, + "genserKinematicsInverse: not initialized\n"); + return -1; + } +#endif + + // FIXME-AJ: rpy or zyx ? + rpy.y = world->c * PM_PI / 180; + rpy.p = world->b * PM_PI / 180; + rpy.r = world->a * PM_PI / 180; + + go_rpy_quat_convert(&rpy, &haldata->pos->rot); + haldata->pos->tran.x = world->tran.x; + haldata->pos->tran.y = world->tran.y; + haldata->pos->tran.z = world->tran.z; + + go_matrix_init(Jfwd, Jfwd_stg, 6, genser->link_num); + go_matrix_init(Jinv, Jinv_stg, genser->link_num, 6); + + /* jest[] is a copy of joints[], which is the joint estimate */ + for (link = 0; link < genser->link_num; link++) { + // jest, and the rest of joint related calcs are in radians + jest[link] = joints[link] * (PM_PI / 180); + } + + for (genser->iterations = 0; + genser->iterations < *haldata->max_iterations; + genser->iterations++) { + *(haldata->last_iterations) = genser->iterations; + /* update the Jacobians */ + for (link = 0; link < genser->link_num; link++) { + go_link_joint_set(&genser->links[link], jest[link], &linkout[link]); + } + retval = compute_jfwd(linkout, genser->link_num, &Jfwd, &T_L_0); + if (GO_RESULT_OK != retval) { + rtapi_print("ERR kI - compute_jfwd (joints: %f %f %f %f %f %f), (iterations=%d)\n", + joints[0],joints[1],joints[2],joints[3],joints[4],joints[5], genser->iterations); + return retval; + } + retval = compute_jinv(&Jfwd, &Jinv); + if (GO_RESULT_OK != retval) { + rtapi_print("ERR kI - compute_jinv (joints: %f %f %f %f %f %f), (iterations=%d)\n", + joints[0],joints[1],joints[2],joints[3],joints[4],joints[5], genser->iterations); + return retval; + } + + /* pest is the resulting pose estimate given joint estimate */ + genser_kin_fwd(KINS_PTR, jest, &pest); + //printf("jest: %f %f %f %f %f %f\n",jest[0],jest[1],jest[2],jest[3],jest[4],jest[5]); + /* pestinv is its inverse */ + go_pose_inv(&pest, &pestinv); + /* + Tdelta is the incremental pose from pest to pos, such that + + 0 L 0 + . pest * Tdelta = pos, or + L L L + + L L 0 + .Tdelta = pestinv * pos + L 0 L + */ + go_pose_pose_mult(&pestinv, haldata->pos, &Tdelta); + + /* + We need Tdelta in 0 frame, not pest frame, so rotate it + back. Since it's effectively a velocity, we just rotate it, and + don't translate it. + */ + + /* first rotate the translation differential */ + go_quat_cart_mult(&pest.rot, &Tdelta.tran, &cart); + dvw[0] = cart.x; + dvw[1] = cart.y; + dvw[2] = cart.z; + + /* to rotate the rotation differential, convert it to a + velocity screw and rotate that */ + go_quat_rvec_convert(&Tdelta.rot, &rvec); + cart.x = rvec.x; + cart.y = rvec.y; + cart.z = rvec.z; + go_quat_cart_mult(&pest.rot, &cart, &cart); + dvw[3] = cart.x; + dvw[4] = cart.y; + dvw[5] = cart.z; + + /* push the Cartesian velocity vector through the inverse Jacobian */ + go_matrix_vector_mult(&Jinv, dvw, dj); + + //pass through 678 as uvw + if (total_joints > 6) joints[6] = world->u; + if (total_joints > 7) joints[7] = world->v; + if (total_joints > 8) joints[8] = world->w; + + /* check for small joint increments, if so we're done */ + for (link = 0, smalls = 0; link < genser->link_num; link++) { + if (GO_QUANTITY_LENGTH == linkout[link].quantity) { + if (GO_TRAN_SMALL(dj[link])) + smalls++; + } else { + if (GO_ROT_SMALL(dj[link])) + smalls++; + } + } + if (smalls == genser->link_num) { + /* converged, copy jest[] out */ + for (link = 0; link < genser->link_num; link++) { + // convert from radians back to angles + joints[link] = jest[link] * 180 / PM_PI; + if ((link) && *(haldata->unrotate[link])) + joints[link] += *(haldata->unrotate[link]) * joints[link-1]; + } + //rtapi_print("DONEkineInverse(joints: %f %f %f %f %f %f), (iterations=%d)\n", + // joints[0],joints[1],joints[2],joints[3],joints[4],joints[5], genser->iterations); + //rtapi_print("OKkineInverse: %.2f %.2f %.2f %.2f %.2f %.2f)\n", + // world->tran.x, world->tran.y, world->tran.z, world->a, world->b, world->c); + return GO_RESULT_OK; + } + /* else keep iterating */ + for (link = 0; link < genser->link_num; link++) { + jest[link] += dj[link]; //still in radians + } + } /* for (iterations) */ + + rtapi_print("ERRkineInverse(joints: %f %f %f %f %f %f), (iterations=%d)\n", + joints[0],joints[1],joints[2],joints[3],joints[4],joints[5], genser->iterations); + return GO_RESULT_ERROR; +} + +/* + Extras, not callable using go_kin_ wrapper but if you know you have + linked in these kinematics, go ahead and call these for your ad hoc + purposes. +*/ + +int genser_kin_inv_iterations(genser_struct * genser) +{ + return genser->iterations; +} + +int genser_kin_inv_set_max_iterations(int i) +{ + if (i <= 0) return GO_RESULT_ERROR; + *haldata->max_iterations = i; + return GO_RESULT_OK; +} + +int genser_kin_inv_get_max_iterations() +{ + return *haldata->max_iterations; +} + +int genserKinematicsSetup(const int comp_id, + const char* coordinates, + kparms* kp) +{ + (void)coordinates; + int i,res=0; + haldata = hal_malloc(sizeof(struct haldata)); + if (!haldata) {goto error;} + + // allow for pass through joints 6,7,8 u,v,w + total_joints = kp->max_joints; + + // only the first 6 joints have A,ALPHA,D,unrotate pins + for (i = 0; i < 6; i++) { + res += hal_pin_float_newf(HAL_IN, &(haldata->a[i]), comp_id, + "%s.A-%d", kp->halprefix, i); + *(haldata->a[i])=0; + res += hal_pin_float_newf(HAL_IN, &(haldata->alpha[i]), comp_id, + "%s.ALPHA-%d", kp->halprefix, i); + *(haldata->alpha[i])=0; + res += hal_pin_float_newf(HAL_IN, &(haldata->d[i]), comp_id, + "%s.D-%d", kp->halprefix, i); + *(haldata->d[i])=0; + res += hal_pin_s32_newf(HAL_IN, &(haldata->unrotate[i]), comp_id, + "%s.unrotate-%d", kp->halprefix, i); + *haldata->unrotate[i]=0; + } + res += hal_pin_u32_newf(HAL_OUT, &(haldata->last_iterations), comp_id, + "%s.last-iterations",kp->halprefix); + + KINS_PTR = hal_malloc(sizeof(genser_struct)); + haldata->pos = (go_pose *) hal_malloc(sizeof(go_pose)); + if (KINS_PTR == NULL) {goto error;} + if (haldata->pos == NULL) {goto error;} + res += hal_pin_u32_newf(HAL_IN, &haldata->max_iterations, comp_id, + "%s.max-iterations",kp->halprefix); + + if (res) {goto error;} + + *haldata->max_iterations = GENSER_DEFAULT_MAX_ITERATIONS; + + A(0) = DEFAULT_A1; + A(1) = DEFAULT_A2; + A(2) = DEFAULT_A3; + A(3) = DEFAULT_A4; + A(4) = DEFAULT_A5; + A(5) = DEFAULT_A6; + ALPHA(0) = DEFAULT_ALPHA1; + ALPHA(1) = DEFAULT_ALPHA2; + ALPHA(2) = DEFAULT_ALPHA3; + ALPHA(3) = DEFAULT_ALPHA4; + ALPHA(4) = DEFAULT_ALPHA5; + ALPHA(5) = DEFAULT_ALPHA6; + D(0) = DEFAULT_D1; + D(1) = DEFAULT_D2; + D(2) = DEFAULT_D3; + D(3) = DEFAULT_D4; + D(4) = DEFAULT_D5; + D(5) = DEFAULT_D6; + + genser_hal_inited = 1; + return 0; + +error: + return -1; +} // genserKinematicsSetup() diff --git a/wasm-port/vendor/linuxcnc/src/emc/kinematics/genserkins.c b/wasm-port/vendor/linuxcnc/src/emc/kinematics/genserkins.c new file mode 100644 index 0000000..64fe559 --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/emc/kinematics/genserkins.c @@ -0,0 +1,79 @@ +/******************************************************************** +* genserkins.c employing switchkins.[ch] +* License: GPL Version 2 +* +* NOTEs: +* 1) specify all kparms items +* 2) specify 3 KS,KF,KI functions (setup,forward,inverse) +*/ + +/******************************************************************** +TEST: switchable kinematics: identity or genserkins + +1) genser kinematics provided by genserfuncs.c (shared with genserkins.c +2) uses same pin names as genserkins +3) tesing mm configs: increase GO_REAL_EPSILON from 1e-7 to 1e-6 + +NOTE: +a) requires *exactly* 6 joints +b) identity assignments can use any of xyzabcuvw + but should agree with [TRAJ]COORDINATES + and may be confusing + +www refs: + +frame-larger-than: +https://www.mail-archive.com/emc-developers@lists.sourceforge.net/msg03790.html + +angles: +https://www.mail-archive.com/emc-developers@lists.sourceforge.net/msg15285.html +*/ + +//---------------------------------------------------------------------- +// Only gcc/g++ supports the #pragma +#if __GNUC__ && !defined(__clang__) +// genserKinematicsInverse() is 5104 with buster amd64 gcc 8.3.0-6 +//#pragma GCC diagnostic error "-Wframe-larger-than=6000" + #pragma GCC diagnostic warning "-Wframe-larger-than=6000" +#endif + +#include +#include +#include + +#include "genserkins.h" +#include "switchkins.h" + +//-7 is system defined -3 ok, -4 ok, -5 ok,-6 ok (mm system) +#undef GO_REAL_EPSILON +#define GO_REAL_EPSILON (1e-6) + +//********************************************************************* + + +int switchkinsSetup(kparms* kp, + KS* kset0, KS* kset1, KS* kset2, + KF* kfwd0, KF* kfwd1, KF* kfwd2, + KI* kinv0, KI* kinv1, KI* kinv2 + ) +{ + kp->kinsname = "genserkins"; // !!! must agree with filename + kp->halprefix = "genserkins"; // hal pin names + kp->required_coordinates = "xyzabcuvw"; // u,v,w are joints 6,7,8 + kp->max_joints = strlen(kp->required_coordinates); + kp->allow_duplicates = 0; + + *kset0 = genserKinematicsSetup; + *kfwd0 = genserKinematicsForward; + *kinv0 = genserKinematicsInverse; + + *kset1 = identityKinematicsSetup; + *kfwd1 = identityKinematicsForward; + *kinv1 = identityKinematicsInverse; + + *kset2 = userkKinematicsSetup; + *kfwd2 = userkKinematicsForward; + *kinv2 = userkKinematicsInverse; + + return 0; +} diff --git a/wasm-port/vendor/linuxcnc/src/emc/kinematics/genserkins.h b/wasm-port/vendor/linuxcnc/src/emc/kinematics/genserkins.h new file mode 100644 index 0000000..eda6f90 --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/emc/kinematics/genserkins.h @@ -0,0 +1,159 @@ +/******************************************************************** +* Description: genserkins.h +* Kinematics for a generalised serial kinematics machine +* +* Derived from a work by Fred Proctor, +* changed to work with emc2 and HAL +* +* Adapting Author: Alex Joni +* License: GPL Version 2 +* System: Linux +* +******************************************************************* + + These are the forward and inverse kinematic functions for a general + serial-link manipulator. Thanks to Herman Bruyninckx and John + Hallam at http://www.roble.info/ for this. + + The functions are general enough to be configured for any serial + configuration. + The kinematics use Denavit-Hartenberg definition for the joint and + links. The DH definitions are the ones used by John J Craig in + "Introduction to Robotics: Mechanics and Control" + The parameters for the manipulator are defined by hal pins. + Currently the type of the joints is hardcoded to ANGULAR, although + the kins support both ANGULAR and LINEAR axes. + +*/ + +/* + genserkins.h +*/ + +#ifndef GENSERKINS_H +#define GENSERKINS_H + +#include /* HAL data types */ +#include /* go_result, go_integer */ +#include /* go_pose */ +#include + +/*! + The maximum number of joints supported by the general serial + kinematics. Make this at least 6; a device can have fewer than these. +*/ +#define GENSER_MAX_JOINTS 6 + +#define GENSER_DEFAULT_MAX_ITERATIONS 100 + +#define PI_2 GO_PI_2 + +/* default DH parameters, these should be ok for a puma - at least according to Craig */ +#define DEFAULT_A1 0 +#define DEFAULT_ALPHA1 0 +#define DEFAULT_D1 0 + +#define DEFAULT_A2 0 +#define DEFAULT_ALPHA2 -PI_2 +#define DEFAULT_D2 0 + +#define DEFAULT_A3 300 +#define DEFAULT_ALPHA3 0 +#define DEFAULT_D3 70 + +#define DEFAULT_A4 50 +#define DEFAULT_ALPHA4 -PI_2 +#define DEFAULT_D4 400 + +#define DEFAULT_A5 0 +#define DEFAULT_ALPHA5 PI_2 +#define DEFAULT_D5 0 + +#define DEFAULT_A6 0 +#define DEFAULT_ALPHA6 -PI_2 +#define DEFAULT_D6 0 + +typedef struct { + go_link links[GENSER_MAX_JOINTS]; /*!< The link description of the device. */ + int link_num; /*!< How many are actually present. */ + hal_u32_t iterations; /*!< How many iterations were actually used to compute the inverse kinematics. */ +} genser_struct; + +extern int genser_kin_size(void); + +extern int genser_kin_init(void); + +extern const char * genser_kin_get_name(void); + +extern int genser_kin_num_joints(void * kins); + +extern int genser_kin_fwd(void * kins, + const go_real *joint, + go_pose * world); + +extern int genser_kin_inv(void * kins, + const go_pose * world, + go_real *joint); + +extern int genser_kin_set_parameters(void * kins, go_link * params, int num); + +extern int genser_kin_get_parameters(void * kins, go_link * params, int num); + +extern int genser_kin_jac_inv(void * kins, + const go_pose * pos, + const go_screw * vel, + const go_real * joints, + go_real * jointvels); + + +extern int genser_kin_jac_fwd(void * kins, + const go_real * joints, + const go_real * jointvels, + const go_pose * pos, + go_screw * vel); + +extern int genser_kin_fwd_interations(genser_struct * genser); + + +/* + Extras, not callable using go_kin_ wrapper but if you know you have + linked in these kinematics, go ahead and call these for your ad hoc + purposes. +*/ + +/*! Returns the number of iterations used during the last call to the + inverse kinematics functions */ +extern int genser_kin_inv_iterations(genser_struct * genser); + +/*! Sets the maximum number of iterations to use in future calls to + the inverse kinematics functions, after which an error will be + reported */ +extern int genser_kin_inv_set_max_iterations(int i); + +/*! Returns the maximum number of iterations that will be used to + compute inverse kinematics functions */ +extern int genser_kin_inv_get_max_iterations(void); + +extern int compute_jfwd(go_link * link_params, + int link_number, + go_matrix * Jfwd, + go_pose * T_L_0); + +extern int compute_jinv(go_matrix * Jfwd, + go_matrix * Jinv); + +extern int genserKinematicsForward(const double *joint, + EmcPose * world, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags); + +extern int genserKinematicsInverse(const EmcPose * world, + double *joints, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags); + +extern int genserKinematicsSetup(const int comp_id, + const char* coordinates, + kparms* ksetup_parms); + +#endif diff --git a/wasm-port/vendor/linuxcnc/src/emc/kinematics/pentakins.c b/wasm-port/vendor/linuxcnc/src/emc/kinematics/pentakins.c new file mode 100644 index 0000000..bf9c1bc --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/emc/kinematics/pentakins.c @@ -0,0 +1,519 @@ +/******************************************************************** +* Description: pentakins.c +* +* Kinematics for a pentapod machine +* +* Derived from genhexkins.c +* +* Author: Andrew Kyrychenko +* License: GPL Version 2 +* System: Linux +* +* Copyright (c) 2016 All rights reserved. +********************************************************************* + + These are the forward and inverse kinematic functions for a pentapod + parallel kinematics machine. + + The default values for base and effector joints positions are defined + in the header file pentakins.h. The actual values for a particular + machine can be adjusted by hal parameters: + + pentakins.base.N.x + pentakins.base.N.y + pentakins.base.N.z + pentakins.effector.N.r + pentakins.effector.N.z + + Hal pins: + + pentakins.convergence-criterion - minimum error value that ends + iterations with converged solution; + + pentakins.limit-iterations - limit of iterations, if exceeded + iterations stop with no convergence; + + pentakins.max-error - maximum error value, if exceeded iterations + stop with no convergence; + + pentakins.last-iterations - number of iterations spent for the + last forward kinematics solution; + + pentakins.max-iterations - maximum number of iterations spent for + a converged solution during current session. + + pentakins.tool-offset - tool length from the origin along z axis, + changes the effector pivot point. + + ----------------------------------------------------------------------------*/ + +#include +#include +#include +#include +#include /* these decls, KINEMATICS_FORWARD_FLAGS */ + +#include "pentakins.h" + +struct haldata { + hal_float_t basex[NUM_STRUTS]; + hal_float_t basey[NUM_STRUTS]; + hal_float_t basez[NUM_STRUTS]; + hal_float_t effectorr[NUM_STRUTS]; + hal_float_t effectorz[NUM_STRUTS]; + hal_u32_t *last_iter; + hal_u32_t *max_iter; + hal_u32_t *iter_limit; + hal_float_t *max_error; + hal_float_t *conv_criterion; + hal_float_t *tool_offset; +} *haldata; + + +/******************************* MatInvert5() ***************************/ + +/*----------------------------------------------------------------------------- + This is a function that inverts a 5x5 matrix. +-----------------------------------------------------------------------------*/ + +static int MatInvert5(double J[][NUM_STRUTS], double InvJ[][NUM_STRUTS]) +{ + double JAug[NUM_STRUTS][10], m, temp; + int j, k, n; + + /* This function determines the inverse of a 6x6 matrix using + Gauss-Jordan elimination */ + + /* Augment the Identity matrix to the Jacobian matrix */ + + for (j=0; j<=4; ++j){ + for (k=0; k<=4; ++k){ /* Assign J matrix to first 6 columns of AugJ */ + JAug[j][k] = J[j][k]; + } + for(k=5; k<=9; ++k){ /* Assign I matrix to last six columns of AugJ */ + if (k-5 == j){ + JAug[j][k]=1; + } + else{ + JAug[j][k]=0; + } + } + } + + /* Perform Gauss elimination */ + for (k=0; k<=3; ++k){ /* Pivot */ + if ((JAug[k][k]< 0.01) && (JAug[k][k] > -0.01)){ + for (j=k+1;j<=4; ++j){ + if ((JAug[j][k]>0.01) || (JAug[j][k]<-0.01)){ + for (n=0; n<=9;++n){ + temp = JAug[k][n]; + JAug[k][n] = JAug[j][n]; + JAug[j][n] = temp; + } + break; + } + } + } + for (j=k+1; j<=4; ++j){ /* Pivot */ + m = -JAug[j][k] / JAug[k][k]; + for (n=0; n<=9; ++n){ + JAug[j][n]=JAug[j][n] + m*JAug[k][n]; /* (Row j) + m * (Row k) */ + if ((JAug[j][n] < 0.000001) && (JAug[j][n] > -0.000001)){ + JAug[j][n] = 0; + } + } + } + } + + /* Normalization of Diagonal Terms */ + for (j=0; j<=4; ++j){ + m=1/JAug[j][j]; + for(k=0; k<=9; ++k){ + JAug[j][k] = m * JAug[j][k]; + } + } + + /* Perform Gauss Jordan Steps */ + for (k=4; k>=0; --k){ + for(j=k-1; j>=0; --j){ + m = -JAug[j][k]/JAug[k][k]; + for (n=0; n<=9; ++n){ + JAug[j][n] = JAug[j][n] + m * JAug[k][n]; + } + } + } + + /* Assign last 4 columns of JAug to InvJ */ + for (j=0; j<=4; ++j){ + for (k=0; k<=4; ++k){ + InvJ[j][k] = JAug[j][k+5]; + + } + } + + return 0; /* FIXME-- check divisors for 0 above */ +} + +/******************************** MatMult() *********************************/ + +/*--------------------------------------------------------------------------- + This function simply multiplies a 6x6 matrix by a 1x6 vector + ---------------------------------------------------------------------------*/ + +static void MatMult5(double J[][5], const double x[], double Ans[]) +{ + int j, k; + for (j=0; j<=4; ++j){ + Ans[j] = 0; + for (k=0; k<=4; ++k){ + Ans[j] = J[j][k]*x[k]+Ans[j]; + } + } +} + +/*-------------- +------square-----*/ + +static double sqr(double x) +{ + return (x)*(x); +} + +/* declare arrays for base and effector coordinates */ +static PmCartesian b[NUM_STRUTS]; +static double za[NUM_STRUTS], ra[NUM_STRUTS]; + +/************************pentakins_read_hal_pins**************************/ + +int pentakins_read_hal_pins(void) { + int t; + + /* set the base and effector coordinates from hal pin values */ + for (t = 0; t < NUM_STRUTS; t++) { + b[t].x = haldata->basex[t]; + b[t].y = haldata->basey[t]; + b[t].z = haldata->basez[t] + *haldata->tool_offset; + ra[t] = haldata->effectorr[t]; + za[t] = haldata->effectorz[t] + *haldata->tool_offset; + } + return 0; +} + +/************************ InvKins() ********************************/ + +int InvKins(const double * coord, + double * struts) +{ + + PmCartesian xyz, pmcoord, temp; + PmRotationMatrix RMatrix, InvRMatrix; + PmRpy rpy; + int i; + +// pentakins_read_hal_pins(); + + /* define Rotation Matrix */ + pmcoord.x = coord[0]; + pmcoord.y = coord[1]; + pmcoord.z = coord[2]; + rpy.r = coord[3]; + rpy.p = coord[4]; + rpy.y = 0; + pmRpyMatConvert(&rpy, &RMatrix); + + /* enter for loop to calculate joints (strut lengths) */ + for (i = 0; i < NUM_STRUTS; i++) { + /* convert location of effector strut end from effector + to world coordinates */ + pmCartCartSub(&b[i], &pmcoord, &temp); + pmMatInv(&RMatrix, &InvRMatrix); + pmMatCartMult(&InvRMatrix, &temp, &xyz); + + /* define strut lengths */ + struts[i] = sqrt( sqr(xyz.z - za[i]) + sqr( sqrt(sqr(xyz.x) + sqr(xyz.y)) - ra[i]) ); + } + + return 0; +} + + +/**************************** kinematicsForward() ***************************/ + +int kinematicsForward(const double * joints, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) +{ + (void)fflags; + (void)iflags; + +// PmCartesian aw; +// PmCartesian InvKinStrutVect,InvKinStrutVectUnit; +// PmCartesian q_trans, RMatrix_a, RMatrix_a_cross_Strut; + + double Jacobian[NUM_STRUTS][NUM_STRUTS]; + double InverseJacobian[NUM_STRUTS][NUM_STRUTS]; + double InvKinStrutLength[NUM_STRUTS], StrutLengthDiff[NUM_STRUTS]; + double delta[NUM_STRUTS]; + double jointdelta[NUM_STRUTS]; + double coord[NUM_STRUTS]; + double conv_err = 1.0; + +// PmRotationMatrix RMatrix; +// PmRpy q_RPY; + + int iterate = 1; + int i, j; + unsigned iteration = 0; + + pentakins_read_hal_pins(); + + /* abort on obvious problems, like joints <= 0 */ + if (joints[0] <= 0.0 || + joints[1] <= 0.0 || + joints[2] <= 0.0 || + joints[3] <= 0.0 || + joints[4] <= 0.0 ) { + return -1; + } + + /* assign a,b,c to roll, pitch, yaw angles */ + coord[0] = pos->tran.x; + coord[1] = pos->tran.y; + coord[2] = pos->tran.z; + coord[3] = pos->a * PM_PI / 180.0; + coord[4] = pos->b * PM_PI / 180.0; + + /* Enter Newton-Raphson iterative method */ + while (iterate) { + /* check for large error and return error flag if no convergence */ + if ((conv_err > +(*haldata->max_error)) || + (conv_err < -(*haldata->max_error))) { + /* we can't converge */ + return -2; + }; + + iteration++; + + /* check iteration to see if the kinematics can reach the + convergence criterion and return error flag if it can't */ + if (iteration > *haldata->iter_limit) { + /* we can't converge */ + return -5; + } + + /* compute StrutLengthDiff[] by running inverse kins on Cartesian + estimate to get joint estimate, subtract joints to get joint deltas, + and compute inv J while we're at it */ + InvKins(coord, InvKinStrutLength); + + for (i = 0; i < NUM_STRUTS; i++) { + StrutLengthDiff[i] = InvKinStrutLength[i] - joints[i]; + + /* Build Inverse Jacobian Matrix */ + coord[i] += 1e-4; + InvKins(coord, jointdelta); + coord[i] -= 1e-4; + for (j = 0; j < NUM_STRUTS; j++) { + InverseJacobian[j][i] = (jointdelta[j] - InvKinStrutLength[j]) * 1e4; + } + } + + /* invert Inverse Jacobian */ + MatInvert5(InverseJacobian, Jacobian); + + /* multiply Jacobian by LegLengthDiff */ + MatMult5(Jacobian, StrutLengthDiff, delta); + + /* subtract delta from last iterations pos values */ + coord[0] -= delta[0]; + coord[1] -= delta[1]; + coord[2] -= delta[2]; + coord[3] -= delta[3]; + coord[4] -= delta[4]; + + /* determine value of conv_error (used to determine if no convergence) */ + conv_err = 0.0; + for (i = 0; i < NUM_STRUTS; i++) { + conv_err += fabs(StrutLengthDiff[i]); + } + + /* enter loop to determine if a strut needs another iteration */ + iterate = 0; /*assume iteration is done */ + for (i = 0; i < NUM_STRUTS; i++) { + if (fabs(StrutLengthDiff[i]) > *haldata->conv_criterion) { + iterate = 1; + } + } + } /* exit Newton-Raphson Iterative loop */ + + /* assign coord to pos */ + pos->tran.x = coord[0]; + pos->tran.y = coord[1]; + pos->tran.z = coord[2]; + pos->a = coord[3] * 180.0 / PM_PI; + pos->b = coord[4] * 180.0 / PM_PI; + + *haldata->last_iter = iteration; + + if (iteration > *haldata->max_iter){ + *haldata->max_iter = iteration; + } + return 0; +} + + +/************************ kinematicsInverse() ********************************/ +/* the inverse kinematics take world coordinates and determine joint values, + given the inverse kinematics flags to resolve any ambiguities. The forward + flags are set to indicate their value appropriate to the world coordinates + passed in. */ + +/************************ kinematicsInverse() ********************************/ + +int kinematicsInverse(const EmcPose * pos, + double * joints, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) +{ + (void)iflags; + (void)fflags; + + double coord[NUM_STRUTS]; + + pentakins_read_hal_pins(); + + coord[0] = pos->tran.x; + coord[1] = pos->tran.y; + coord[2] = pos->tran.z; + coord[3] = pos->a * PM_PI / 180.0; + coord[4] = pos->b * PM_PI / 180.0; + + if (0 != InvKins(coord,joints)) { + return -1; + } + + return 0; +} + +KINEMATICS_TYPE kinematicsType() +{ + return KINEMATICS_BOTH; +} + +KINS_NOT_SWITCHABLE +EXPORT_SYMBOL(kinematicsType); +EXPORT_SYMBOL(kinematicsForward); +EXPORT_SYMBOL(kinematicsInverse); + +MODULE_LICENSE("GPL"); + +int comp_id; + +int rtapi_app_main(void) +{ + int res = 0, i; + + comp_id = hal_init("pentakins"); + if (comp_id < 0) + return comp_id; + + haldata = hal_malloc(sizeof(struct haldata)); + if (!haldata) + goto error; + + + for (i = 0; i < 6; i++) { + + if ((res = hal_param_float_newf(HAL_RW, &(haldata->basex[i]), comp_id, + "pentakins.base.%d.x", i)) < 0) + goto error; + + if ((res = hal_param_float_newf(HAL_RW, &haldata->basey[i], comp_id, + "pentakins.base.%d.y", i)) < 0) + goto error; + + if ((res = hal_param_float_newf(HAL_RW, &haldata->basez[i], comp_id, + "pentakins.base.%d.z", i)) < 0) + goto error; + + if ((res = hal_param_float_newf(HAL_RW, &haldata->effectorr[i], comp_id, + "pentakins.effector.%d.r", i)) < 0) + goto error; + + if ((res = hal_param_float_newf(HAL_RW, &haldata->effectorz[i], comp_id, + "pentakins.effector.%d.z", i)) < 0) + goto error; + } + + if ((res = hal_pin_u32_newf(HAL_OUT, &haldata->last_iter, comp_id, + "pentakins.last-iterations")) < 0) + goto error; + *haldata->last_iter = 0; + + if ((res = hal_pin_u32_newf(HAL_OUT, &haldata->max_iter, comp_id, + "pentakins.max-iterations")) < 0) + goto error; + *haldata->max_iter = 0; + + if ((res = hal_pin_float_newf(HAL_IO, &haldata->max_error, comp_id, + "pentakins.max-error")) < 0) + goto error; + *haldata->max_error = 100; + + if ((res = hal_pin_float_newf(HAL_IO, &haldata->conv_criterion, comp_id, + "pentakins.convergence-criterion")) < 0) + goto error; + *haldata->conv_criterion = 1e-9; + + if ((res = hal_pin_u32_newf(HAL_IO, &haldata->iter_limit, comp_id, + "pentakins.limit-iterations")) < 0) + goto error; + *haldata->iter_limit = 120; + + if ((res = hal_pin_float_newf(HAL_IN, &haldata->tool_offset, comp_id, + "pentakins.tool-offset")) < 0) + goto error; + *haldata->tool_offset = 0.0; + + haldata->basex[0] = DEFAULT_BASE_0_X; + haldata->basey[0] = DEFAULT_BASE_0_Y; + haldata->basez[0] = DEFAULT_BASE_0_Z; + haldata->basex[1] = DEFAULT_BASE_1_X; + haldata->basey[1] = DEFAULT_BASE_1_Y; + haldata->basez[1] = DEFAULT_BASE_1_Z; + haldata->basex[2] = DEFAULT_BASE_2_X; + haldata->basey[2] = DEFAULT_BASE_2_Y; + haldata->basez[2] = DEFAULT_BASE_2_Z; + haldata->basex[3] = DEFAULT_BASE_3_X; + haldata->basey[3] = DEFAULT_BASE_3_Y; + haldata->basez[3] = DEFAULT_BASE_3_Z; + haldata->basex[4] = DEFAULT_BASE_4_X; + haldata->basey[4] = DEFAULT_BASE_4_Y; + haldata->basez[4] = DEFAULT_BASE_4_Z; + + haldata->effectorz[0] = DEFAULT_EFFECTOR_0_Z; + haldata->effectorz[1] = DEFAULT_EFFECTOR_1_Z; + haldata->effectorz[2] = DEFAULT_EFFECTOR_2_Z; + haldata->effectorz[3] = DEFAULT_EFFECTOR_3_Z; + haldata->effectorz[4] = DEFAULT_EFFECTOR_4_Z; + + haldata->effectorr[0] = DEFAULT_EFFECTOR_0_R; + haldata->effectorr[1] = DEFAULT_EFFECTOR_1_R; + haldata->effectorr[2] = DEFAULT_EFFECTOR_2_R; + haldata->effectorr[3] = DEFAULT_EFFECTOR_3_R; + haldata->effectorr[4] = DEFAULT_EFFECTOR_4_R; + + hal_ready(comp_id); + return 0; + +error: + hal_exit(comp_id); + return res; +} + + +void rtapi_app_exit(void) +{ + hal_exit(comp_id); +} diff --git a/wasm-port/vendor/linuxcnc/src/emc/kinematics/pentakins.h b/wasm-port/vendor/linuxcnc/src/emc/kinematics/pentakins.h new file mode 100644 index 0000000..cca9e18 --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/emc/kinematics/pentakins.h @@ -0,0 +1,56 @@ +/******************************************************************* +* Description: pentakins.h +* +* Kinematics for a pentapod machine +* +* Derived from genhexkins +* +* Author: Andrew Kyrychenko +* License: GPL Version 2 +* System: Linux +* +* Copyright (c) 2016 All rights reserved. +******************************************************************** + + This is the header file containing joints coordinates. + + */ + +#define NUM_STRUTS 5 // number of struts + +/* Default position of base strut ends in base (world) coordinates */ + +#define DEFAULT_BASE_0_X -418.03 +#define DEFAULT_BASE_1_X 417.96 +#define DEFAULT_BASE_2_X -418.03 +#define DEFAULT_BASE_3_X 417.96 +#define DEFAULT_BASE_4_X -0.06 + +#define DEFAULT_BASE_0_Y 324.56 +#define DEFAULT_BASE_1_Y 324.56 +#define DEFAULT_BASE_2_Y -325.44 +#define DEFAULT_BASE_3_Y -325.44 +#define DEFAULT_BASE_4_Y -492.96 + +#define DEFAULT_BASE_0_Z 895.56 +#define DEFAULT_BASE_1_Z 895.56 +#define DEFAULT_BASE_2_Z 895.56 +#define DEFAULT_BASE_3_Z 895.56 +#define DEFAULT_BASE_4_Z 895.56 + +/* Default radius of effector strut end in platform coordinate system */ + +#define DEFAULT_EFFECTOR_0_R 80.32 +#define DEFAULT_EFFECTOR_1_R 80.32 +#define DEFAULT_EFFECTOR_2_R 80.32 +#define DEFAULT_EFFECTOR_3_R 80.32 +#define DEFAULT_EFFECTOR_4_R 80.32 + +/* Default position of effector joints along the spindle axis in + effector coordinate system */ + +#define DEFAULT_EFFECTOR_0_Z -185.50 +#define DEFAULT_EFFECTOR_1_Z -159.50 +#define DEFAULT_EFFECTOR_2_Z -67.50 +#define DEFAULT_EFFECTOR_3_Z -41.50 +#define DEFAULT_EFFECTOR_4_Z -14.00 diff --git a/wasm-port/vendor/linuxcnc/src/emc/kinematics/ugenserkins.c b/wasm-port/vendor/linuxcnc/src/emc/kinematics/ugenserkins.c new file mode 100644 index 0000000..1d80e5a --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/emc/kinematics/ugenserkins.c @@ -0,0 +1,297 @@ +/******************************************************************** +* !!! Declining usage +* !!! This userspace program was been separated from the original +* !!! genserkins.c as part of refactoring kinematics for +* !!! switchkins support. The userspace program has not been +* !!! updated since 2009 (commit 1939285758) and correct +* !!! functioning in its current state is doubtful. +* +* Description: ugenserkins.c +* Test program (originally part of genserkins.c) +* License GPL Version 2 +********************************************************************/ + + +#include /* ulapi */ +#include /* struct timeval */ +#include "genserkins.h" + +static double timestamp() +{ + struct timeval tp; + + if (0 != gettimeofday(&tp, NULL)) { + return 0.0; + } + return ((double) tp.tv_sec) + ((double) tp.tv_usec) / 1000000.0; +} + +int main(int argc, char *argv[]) +{ +#define BUFFERLEN 256 + char buffer[BUFFERLEN]; + int inverse = 1; + int jacobian = 0; + EmcPose pos = { {0.0, 0.0, 0.0}, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; + EmcPose vel = { {0.0, 0.0, 0.0}, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; // will need this for + // jacobian + double joints[6] = { 0.0 }; + double jointvels[6] = { 0.0 }; + KINEMATICS_INVERSE_FLAGS iflags = 0; + KINEMATICS_FORWARD_FLAGS fflags = 0; + int t; + int retval = 0; + double start, end; + int comp_id; + kparms kp; + kp.max_joints = GENSER_MAX_JOINTS; + kp.allow_duplicates = 0; + + comp_id = hal_init("usergenserkins"); + if (genserKinematicsSetup(comp_id,"XYZABC",&kp)) printf("unexpected\n"); + + genser_kin_init(); + + /* syntax is a.out {i|f # # # # # #} */ + if (argc == 8) { + if (argv[1][0] == 'f') { + /* joints passed, so do iterations on forward kins for timing */ + for (t = 0; t < 6; t++) { + if (1 != sscanf(argv[t + 2], "%lf", &joints[t])) { + fprintf(stderr, "bad value: %s\n", argv[t + 2]); + return 1; + } + } + inverse = 0; + } else if (argv[1][0] == 'i') { + /* world coords passed, so do iterations on inverse kins for + timing */ + if (1 != sscanf(argv[2], "%lf", &pos.tran.x)) { + fprintf(stderr, "bad value: %s\n", argv[2]); + return 1; + } + if (1 != sscanf(argv[3], "%lf", &pos.tran.y)) { + fprintf(stderr, "bad value: %s\n", argv[3]); + return 1; + } + if (1 != sscanf(argv[4], "%lf", &pos.tran.z)) { + fprintf(stderr, "bad value: %s\n", argv[4]); + return 1; + } + if (1 != sscanf(argv[5], "%lf", &pos.a)) { + fprintf(stderr, "bad value: %s\n", argv[5]); + return 1; + } + if (1 != sscanf(argv[6], "%lf", &pos.b)) { + fprintf(stderr, "bad value: %s\n", argv[6]); + return 1; + } + if (1 != sscanf(argv[7], "%lf", &pos.c)) { + fprintf(stderr, "bad value: %s\n", argv[7]); + return 1; + } + inverse = 1; + } else { + fprintf(stderr, "syntax: %s {i|f # # # # # #}\n", argv[0]); + hal_exit(comp_id); + return 1; + } + /* need an initial estimate for the forward kins, so ask for it */ + if (inverse == 0) { + do { + printf("initial estimate for Cartesian position, xyzrpw: "); + fflush(stdout); + if (NULL == fgets(buffer, BUFFERLEN, stdin)) { + hal_exit(comp_id); + return 0; + } + } while (6 != sscanf(buffer, "%lf %lf %lf %lf %lf %lf", + &pos.tran.x, &pos.tran.y, &pos.tran.z, &pos.a, &pos.b, &pos.c)); + } + + start = timestamp(); + if (inverse) { +fprintf(stderr,"%stest pos.b\n",__FILE__); +joints[4]=90; +pos.tran.x= 19.2; +pos.tran.y= 5.5; +pos.tran.z= 9.4; +pos.a = 92.88; +pos.b = -90; +pos.c = 87.12; +fprintf(stderr,"gki0:P %6.2f %6.2f %6.2f %6.2f %6.2f %6.2f\n", +pos.tran.x,pos.tran.y,pos.tran.z,pos.a,pos.b,pos.c); +fprintf(stderr,"gki1:J %6.2f %6.2f %6.2f %6.2f %6.2f %6.2f\n", +joints[0],joints[1],joints[2],joints[3],joints[4],joints[5]); + retval = genserKinematicsInverse(&pos, joints, &iflags, &fflags); +fprintf(stderr,"gki2:J %6.2f %6.2f %6.2f %6.2f %6.2f %6.2f\n", +joints[0],joints[1],joints[2],joints[3],joints[4],joints[5]); + if (0 != retval) { + printf("inv kins error %d <%s>\n", retval,go_result_to_string(retval)); + } + } else { + retval = genserKinematicsForward(joints, &pos, &fflags, &iflags); + if (0 != retval) { + printf("fwd kins error %d\n", retval); + } + } + end = timestamp(); + + printf("calculation time: %f secs\n", (end - start)); + printf("Joints: 0=%8.3f 1=%8.3f 2=%8.3f 3=%8.3f 4=%8.3f 5=%8.3f\n", + joints[0],joints[1],joints[2], joints[3],joints[4],joints[5]); + printf("Inverse: x=%8.3f y=%8.3f z=%8.3f a=%8.3f b=%8.3f z=%8.3f\n", + pos.tran.x,pos.tran.y,pos.tran.z,pos.a,pos.b,pos.c); + hal_exit(comp_id); + return 0; + } + + /* end of if args for timestamping */ + /* else we're interactive, terminate with 'quit'|'exit'|CTRL-D */ + while (!feof(stdin)) { + if (inverse) { + if (jacobian) { + printf("jinv> "); + } else { + printf("inv> "); + } + } else { + if (jacobian) { + printf("jfwd> "); + } else { + printf("fwd> "); + } + } + fflush(stdout); + + if (NULL == fgets(buffer, BUFFERLEN, stdin)) { + break; + } + + if (buffer[0] == 'i') { + inverse = 1; + continue; + } else if (buffer[0] == 'f') { + inverse = 0; + continue; + } else if (buffer[0] == 'j') { + jacobian = !jacobian; + continue; + } else if (buffer[0] == 'q') { //quit + break; + } else if (buffer[0] == 'e') { //exit + break; + } + + if (inverse) { + if (jacobian) { + if (12 != sscanf(buffer, + "%lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf", + &pos.tran.x, &pos.tran.y, &pos.tran.z, &pos.a, &pos.b, + &pos.c, &vel.tran.x, &vel.tran.y, &vel.tran.z, &vel.a, + &vel.b, &vel.c)) { + printf("?\n"); + } else { +//FIXME-AJ +//disabled for now retval = jacobianInverse(&pos, &vel, joints, jointvels); + printf("%f %f %f %f %f %f\n", + jointvels[0], + jointvels[1], + jointvels[2], + jointvels[3], jointvels[4], jointvels[5]); + if (0 != retval) { + printf("inv Jacobian error %d\n", retval); + } else { +//FIXME-AJ +//disabled for now retval = jacobianForward(joints, jointvels, &pos, &vel); + printf("%f %f %f %f %f %f\n", + vel.tran.x, + vel.tran.y, vel.tran.z, vel.a, vel.b, vel.c); + if (0 != retval) { + printf("fwd kins error %d\n", retval); + } + } + } + } else { + if (6 != sscanf(buffer, "%lf %lf %lf %lf %lf %lf", + &pos.tran.x, + &pos.tran.y, &pos.tran.z, &pos.a, &pos.b, &pos.c)) { + printf("?\n"); + } else { +fprintf(stderr,"gki1:\n"); + retval = + genserKinematicsInverse(&pos, joints, &iflags, &fflags); + printf("%f %f %f %f %f %f\n", joints[0], joints[1], + joints[2], joints[3], joints[4], joints[5]); + if (0 != retval) { + printf("inv kins error %d <%s>\n", retval,go_result_to_string(retval)); + } else { + retval = + genserKinematicsForward(joints, &pos, &fflags, &iflags); + printf("%f %f %f %f %f %f\n", pos.tran.x, pos.tran.y, + pos.tran.z, pos.a, pos.b, pos.c); + if (0 != retval) { + printf("fwd kins error %d\n", retval); + } + } + } + } + } else { + if (jacobian) { + if (12 != sscanf(buffer, + "%lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf", + &joints[0], &joints[1], &joints[2], &joints[3], + &joints[4], &joints[5], &jointvels[0], &jointvels[1], + &jointvels[2], &jointvels[3], &jointvels[4], + &jointvels[5])) { + printf("?\n"); + } else { +//FIXME-AJ +//disabled for now retval = jacobianForward(joints, jointvels, &pos, &vel); + printf("%f %f %f %f %f %f\n", + vel.tran.x, + vel.tran.y, vel.tran.z, vel.a, vel.b, vel.c); + if (0 != retval) { + printf("fwd kins error %d\n", retval); + } else { +//FIXME-AJ +//disabled for now retval = jacobianInverse(&pos, &vel, joints, jointvels); + printf("%f %f %f %f %f %f\n", + jointvels[0], + jointvels[1], + jointvels[2], + jointvels[3], jointvels[4], jointvels[5]); + if (0 != retval) { + printf("inv kins error %d <%s>\n", retval,go_result_to_string(retval)); + } + } + } + } else { + if (6 != sscanf(buffer, "%lf %lf %lf %lf %lf %lf", + &joints[0], &joints[1], &joints[2], &joints[3], &joints[4], &joints[5])) { + printf("?\n"); + } else { + retval = genserKinematicsForward(joints, &pos, &fflags, &iflags); + printf("xyzabc: %f %f %f %f %f %f\n", + pos.tran.x, pos.tran.y, pos.tran.z, pos.a, pos.b, pos.c); + if (0 != retval) { + printf("fwd kins error %d\n", retval); + } else { + retval = genserKinematicsInverse(&pos, joints, &iflags, &fflags); + printf("j0--j5: %f %f %f %f %f %f\n", + joints[0], joints[1], joints[2], joints[3], joints[4], joints[5]); + if (0 != retval) { + printf("inv kins error %d <%s>\n", retval,go_result_to_string(retval)); + } + } + } + } + } + } + hal_exit(comp_id); + + return 0; + +#undef ITERATIONS +#undef BUFFERLEN +}