diff --git a/wasm-port/docs/source-reuse-map.md b/wasm-port/docs/source-reuse-map.md index dd2231d..2514fa7 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` | 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` | +| 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` | | Trajectory planner | `src/emc/tp/tp.c`, `tc.c`, `tcq.c`, `spherical_arc.c`, `blendmath.c`, `sp_scurve.c`, `ruckig_wrapper.c`, plus matching `*.h` files | Copy unchanged | Native realtime scheduling and motion process state are replaced by standalone probe setup | Vendor byte sync, per-file source probes, `linuxcnc_tp_api_probe` | | Ruckig C planner support | Selected `src/emc/tp/cruckig/*.c` and `*.h` files in the manifest | Copy unchanged | Used as LinuxCNC planner support code through vendored TP sources | Vendor byte sync, per-file source probes | | Posemath | `src/libnml/posemath/posemath.cc`, `_posemath.c`, `gomath.c`, `sincos.c`, and matching headers | Copy unchanged | `gomath.c` is compiled as C; `rtapi.h` shim is C/C++ compatible for this boundary | Vendor byte sync, per-file source probes, TP probe | @@ -63,7 +63,7 @@ Current validation is intentionally mechanical: ## Known Gaps - Additional non-trivial kinematics implementation files, including serial, - hexapod, puma, and other machine-specific modules, are not yet extracted. + hexapod, and other machine-specific modules, are not yet extracted. - Browser/WASM C ABI and JS SDK layers are not yet built for the full interpreter/planner core. - OPFS persistence is not yet connected to INI, tool table, parameter file, or diff --git a/wasm-port/runtime/core/linuxcnc_wrap/linuxcnc_puma_kinematics_probe.cpp b/wasm-port/runtime/core/linuxcnc_wrap/linuxcnc_puma_kinematics_probe.cpp new file mode 100644 index 0000000..97378b5 --- /dev/null +++ b/wasm-port/runtime/core/linuxcnc_wrap/linuxcnc_puma_kinematics_probe.cpp @@ -0,0 +1,109 @@ +#include +#include + +#include "emc/kinematics/kinematics.h" +#include "emc/motion/emcmotcfg.h" + +int rtapi_app_main(void); +void rtapi_app_exit(void); + +namespace { + +int near(double actual, double expected) +{ + return std::fabs(actual - expected) < 1e-7; +} + +int near_pose(const EmcPose &actual, const EmcPose &expected) +{ + return near(actual.tran.x, expected.tran.x) && + near(actual.tran.y, expected.tran.y) && + near(actual.tran.z, expected.tran.z) && + near(actual.a, expected.a) && + near(actual.b, expected.b) && + near(actual.c, expected.c); +} + +int near_identity_pose(const EmcPose &actual, const double *joints) +{ + return near(actual.tran.x, joints[0]) && + near(actual.tran.y, joints[1]) && + near(actual.tran.z, joints[2]) && + near(actual.a, joints[3]) && + near(actual.b, joints[4]) && + near(actual.c, joints[5]); +} + +void print_pose(const char *prefix, const EmcPose &pose) +{ + std::cout << prefix << "_xyz=" + << pose.tran.x << "," + << pose.tran.y << "," + << pose.tran.z << "\n"; + std::cout << prefix << "_abc=" + << pose.a << "," + << pose.b << "," + << pose.c << "\n"; +} + +void print_joints(const char *prefix, const double *joints) +{ + std::cout << prefix << "_xyzabc=" + << joints[0] << "," + << joints[1] << "," + << joints[2] << "," + << joints[3] << "," + << joints[4] << "," + << joints[5] << "\n"; +} + +} // namespace + +int main() +{ + const int init_rc = rtapi_app_main(); + std::cout << "puma_init=" << init_rc << "\n"; + std::cout << "puma_type=" << kinematicsType() << "\n"; + std::cout << "puma_switchable=" << kinematicsSwitchable() << "\n"; + + double joints[EMCMOT_MAX_JOINTS]{}; + joints[0] = 20.0; + joints[1] = -30.0; + joints[2] = 40.0; + joints[3] = 15.0; + joints[4] = 35.0; + joints[5] = -25.0; + + KINEMATICS_FORWARD_FLAGS fflags = 0; + KINEMATICS_INVERSE_FLAGS iflags = 0; + EmcPose forward_pose{}; + const int forward_rc = kinematicsForward(joints, &forward_pose, &fflags, &iflags); + std::cout << "puma_forward=" << forward_rc << "\n"; + std::cout << "puma_forward_iflags=" << iflags << "\n"; + print_pose("puma_forward", forward_pose); + + double inverse_joints[EMCMOT_MAX_JOINTS]{}; + const int inverse_rc = kinematicsInverse(&forward_pose, inverse_joints, &iflags, &fflags); + std::cout << "puma_inverse=" << inverse_rc << "\n"; + std::cout << "puma_inverse_fflags=" << fflags << "\n"; + print_joints("puma_inverse", inverse_joints); + + KINEMATICS_INVERSE_FLAGS roundtrip_iflags = 0; + EmcPose roundtrip_pose{}; + const int roundtrip_rc = kinematicsForward( + inverse_joints, &roundtrip_pose, &fflags, &roundtrip_iflags); + std::cout << "puma_roundtrip_forward=" << roundtrip_rc << "\n"; + print_pose("puma_roundtrip", roundtrip_pose); + std::cout << "puma_roundtrip_pose=" << near_pose(roundtrip_pose, forward_pose) << "\n"; + + const int switch_rc = kinematicsSwitch(1); + std::cout << "puma_switch_identity=" << switch_rc << "\n"; + EmcPose identity_pose{}; + const int identity_forward_rc = kinematicsForward(joints, &identity_pose, &fflags, &iflags); + std::cout << "puma_identity_forward=" << identity_forward_rc << "\n"; + print_pose("puma_identity", identity_pose); + std::cout << "puma_identity_near=" << near_identity_pose(identity_pose, joints) << "\n"; + + rtapi_app_exit(); + return 0; +} diff --git a/wasm-port/tests/native/verify_native_probes.sh b/wasm-port/tests/native/verify_native_probes.sh index dadf1db..4f60673 100755 --- a/wasm-port/tests/native/verify_native_probes.sh +++ b/wasm-port/tests/native/verify_native_probes.sh @@ -105,6 +105,8 @@ check_exitcode linuxcnc_tripod_kinematics_probe check_exitcode linuxcnc_tripod_kinematics_probe.run check_exitcode linuxcnc_scara_kinematics_probe check_exitcode linuxcnc_scara_kinematics_probe.run +check_exitcode linuxcnc_puma_kinematics_probe +check_exitcode linuxcnc_puma_kinematics_probe.run for name in \ linuxcnc_tp_tp_source_probe \ linuxcnc_tp_tc_source_probe \ @@ -162,6 +164,7 @@ check_exitcode linuxcnc_rotarydeltakins_source_probe 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_interp_convert_source_probe check_exitcode linuxcnc_interp_read_source_probe check_exitcode linuxcnc_interp_check_source_probe @@ -372,6 +375,18 @@ grep -Fq "scara_switch_identity=0" "$SCARA_KINEMATICS_STDOUT" grep -Fq "scara_identity_forward=0" "$SCARA_KINEMATICS_STDOUT" grep -Fq "scara_identity_near=1" "$SCARA_KINEMATICS_STDOUT" +PUMA_KINEMATICS_STDOUT="$BUILD_DIR/linuxcnc_puma_kinematics_probe.run.stdout.log" +grep -Fq "puma_init=0" "$PUMA_KINEMATICS_STDOUT" +grep -Fq "puma_type=4" "$PUMA_KINEMATICS_STDOUT" +grep -Fq "puma_switchable=1" "$PUMA_KINEMATICS_STDOUT" +grep -Fq "puma_forward=0" "$PUMA_KINEMATICS_STDOUT" +grep -Fq "puma_inverse=0" "$PUMA_KINEMATICS_STDOUT" +grep -Fq "puma_roundtrip_forward=0" "$PUMA_KINEMATICS_STDOUT" +grep -Fq "puma_roundtrip_pose=1" "$PUMA_KINEMATICS_STDOUT" +grep -Fq "puma_switch_identity=0" "$PUMA_KINEMATICS_STDOUT" +grep -Fq "puma_identity_forward=0" "$PUMA_KINEMATICS_STDOUT" +grep -Fq "puma_identity_near=1" "$PUMA_KINEMATICS_STDOUT" + check_fixture_output() { local fixture="$1" local expected="$2" diff --git a/wasm-port/tools/build_native_probes.sh b/wasm-port/tools/build_native_probes.sh index 99dc829..217389d 100755 --- a/wasm-port/tools/build_native_probes.sh +++ b/wasm-port/tools/build_native_probes.sh @@ -439,6 +439,18 @@ SCARA_KINEMATICS_PROBE_SOURCES=( "$WRAP_DIR/linuxcnc_scara_kinematics_probe.cpp" ) +PUMA_KINEMATICS_PROBE_SOURCES=( + "$VENDOR_DIR/src/emc/kinematics/kins_util.c" + "$VENDOR_DIR/src/emc/kinematics/switchkins.c" + "$VENDOR_DIR/src/emc/kinematics/userkfuncs.c" + "$VENDOR_DIR/src/emc/kinematics/pumakins.c" + "$VENDOR_DIR/src/libnml/posemath/posemath.cc" + "$VENDOR_DIR/src/libnml/posemath/_posemath.c" + "$VENDOR_DIR/src/libnml/posemath/sincos.c" + "$WRAP_DIR/linuxcnc_hal_adapter.cpp" + "$WRAP_DIR/linuxcnc_puma_kinematics_probe.cpp" +) + TP_CORE_SOURCES=( "$VENDOR_DIR/src/emc/tp/tp.c" "$VENDOR_DIR/src/emc/tp/tc.c" @@ -703,6 +715,13 @@ build_binary_target \ SCARA_KINEMATICS_PROBE_SOURCES \ NO_LINK_FLAGS +build_binary_target \ + linuxcnc_puma_kinematics_probe \ + "$BUILD_DIR/linuxcnc_puma_kinematics_probe" \ + TP_FLAGS \ + PUMA_KINEMATICS_PROBE_SOURCES \ + NO_LINK_FLAGS + if [[ "$(tr -d '[:space:]' < "$BUILD_DIR/linuxcnc_kinematics_probe.exitcode")" == "0" ]]; then set +e "$BUILD_DIR/linuxcnc_kinematics_probe" \ @@ -898,6 +917,21 @@ else "$BUILD_DIR/linuxcnc_scara_kinematics_probe.run.stderr.log" fi +if [[ "$(tr -d '[:space:]' < "$BUILD_DIR/linuxcnc_puma_kinematics_probe.exitcode")" == "0" ]]; then + set +e + "$BUILD_DIR/linuxcnc_puma_kinematics_probe" \ + >"$BUILD_DIR/linuxcnc_puma_kinematics_probe.run.stdout.log" \ + 2>"$BUILD_DIR/linuxcnc_puma_kinematics_probe.run.stderr.log" + PUMA_KINEMATICS_RUN_RC=$? + set -e + echo "$PUMA_KINEMATICS_RUN_RC" > "$BUILD_DIR/linuxcnc_puma_kinematics_probe.run.exitcode" +else + rm -f \ + "$BUILD_DIR/linuxcnc_puma_kinematics_probe.run.exitcode" \ + "$BUILD_DIR/linuxcnc_puma_kinematics_probe.run.stdout.log" \ + "$BUILD_DIR/linuxcnc_puma_kinematics_probe.run.stderr.log" +fi + if [[ "$(tr -d '[:space:]' < "$BUILD_DIR/linuxcnc_tp_api_probe.exitcode")" == "0" ]]; then set +e "$BUILD_DIR/linuxcnc_tp_api_probe" \ @@ -1098,6 +1132,12 @@ build_object_target \ "$VENDOR_DIR/src/emc/kinematics/scarakins.c" \ TP_FLAGS +build_object_target \ + linuxcnc_pumakins_source_probe \ + "$BUILD_DIR/linuxcnc_pumakins_source_probe.o" \ + "$VENDOR_DIR/src/emc/kinematics/pumakins.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 477a5f2..c062294 100644 --- a/wasm-port/tools/source-manifest.txt +++ b/wasm-port/tools/source-manifest.txt @@ -51,6 +51,8 @@ src/emc/kinematics/rotarydeltakins.c src/emc/kinematics/scorbot-kins.c src/emc/kinematics/tripodkins.c src/emc/kinematics/scarakins.c +src/emc/kinematics/pumakins.h +src/emc/kinematics/pumakins.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/pumakins.c b/wasm-port/vendor/linuxcnc/src/emc/kinematics/pumakins.c new file mode 100644 index 0000000..ab718a6 --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/emc/kinematics/pumakins.c @@ -0,0 +1,384 @@ +/***************************************************************** +* Description: pumakins.c +* Kinematics for puma typed robots +* Set the params using HAL to fit your robot +* +* Derived from a work by Fred Proctor +* +* modified by rdp to add effect of D6 parameter (see pumagui) +* +* Author: +* License: GPL Version 2 +* System: Linux +* +* Copyright (c) 2004 All rights reserved. +* +* Last change: +******************************************************************* +*/ +#include +#include +#include +#include +#include + +#include "pumakins.h" +#include "switchkins.h" + +struct haldata { + hal_float_t *a2, *a3, *d3, *d4, *d6; +} *haldata = 0; + +#define PUMA_A2 (*(haldata->a2)) +#define PUMA_A3 (*(haldata->a3)) +#define PUMA_D3 (*(haldata->d3)) +#define PUMA_D4 (*(haldata->d4)) +#define PUMA_D6 (*(haldata->d6)) + +static int pumaKinematicsForward(const double * joint, + EmcPose * world, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) +{ + (void)fflags; + double s1, s2, s3, s4, s5, s6; + double c1, c2, c3, c4, c5, c6; + double s23; + double c23; + double t1, t2, t3, t4, t5; + double sumSq, k; + PmHomogeneous hom; + PmPose worldPose; + PmRpy rpy; + + /* Calculate sin of joints for future use */ + s1 = sin(joint[0]*PM_PI/180); + s2 = sin(joint[1]*PM_PI/180); + s3 = sin(joint[2]*PM_PI/180); + s4 = sin(joint[3]*PM_PI/180); + s5 = sin(joint[4]*PM_PI/180); + s6 = sin(joint[5]*PM_PI/180); + + /* Calculate cos of joints for future use */ + c1 = cos(joint[0]*PM_PI/180); + c2 = cos(joint[1]*PM_PI/180); + c3 = cos(joint[2]*PM_PI/180); + c4 = cos(joint[3]*PM_PI/180); + c5 = cos(joint[4]*PM_PI/180); + c6 = cos(joint[5]*PM_PI/180); + + s23 = c2 * s3 + s2 * c3; + c23 = c2 * c3 - s2 * s3; + + /* Calculate terms to be used in definition of... */ + /* first column of rotation matrix. */ + t1 = c4 * c5 * c6 - s4 * s6; + t2 = s23 * s5 * c6; + t3 = s4 * c5 * c6 + c4 * s6; + t4 = c23 * t1 - t2; + t5 = c23 * s5 * c6; + + /* Define first column of rotation matrix */ + hom.rot.x.x = c1 * t4 + s1 * t3; + hom.rot.x.y = s1 * t4 - c1 * t3; + hom.rot.x.z = -s23 * t1 - t5; + + /* Calculate terms to be used in definition of... */ + /* second column of rotation matrix. */ + t1 = -c4 * c5 * s6 - s4 * c6; + t2 = s23 * s5 * s6; + t3 = c4 * c6 - s4 * c5 * s6; + t4 = c23 * t1 + t2; + t5 = c23 * s5 * s6; + + /* Define second column of rotation matrix */ + hom.rot.y.x = c1 * t4 + s1 * t3; + hom.rot.y.y = s1 * t4 - c1 * t3; + hom.rot.y.z = -s23 * t1 + t5; + + /* Calculate term to be used in definition of... */ + /* third column of rotation matrix. */ + t1 = c23 * c4 * s5 + s23 * c5; + + /* Define third column of rotation matrix */ + hom.rot.z.x = -c1 * t1 - s1 * s4 * s5; + hom.rot.z.y = -s1 * t1 + c1 * s4 * s5; + hom.rot.z.z = s23 * c4 * s5 - c23 * c5; + + /* Calculate term to be used in definition of... */ + /* position vector. */ + t1 = PUMA_A2 * c2 + PUMA_A3 * c23 - PUMA_D4 * s23; + + /* Define position vector */ + hom.tran.x = c1 * t1 - PUMA_D3 * s1; + hom.tran.y = s1 * t1 + PUMA_D3 * c1; + hom.tran.z = -PUMA_A3 * s23 - PUMA_A2 * s2 - PUMA_D4 * c23; + + /* Calculate terms to be used to... */ + /* determine flags. */ + sumSq = hom.tran.x * hom.tran.x + hom.tran.y * hom.tran.y - + PUMA_D3 * PUMA_D3; + k = (sumSq + hom.tran.z * hom.tran.z - PUMA_A2 * PUMA_A2 - + PUMA_A3 * PUMA_A3 - PUMA_D4 * PUMA_D4) / + (2.0 * PUMA_A2); + + /* reset flags */ + *iflags = 0; + + /* Set shoulder-up flag if necessary */ + if (fabs(joint[0]*PM_PI/180 - atan2(hom.tran.y, hom.tran.x) + + atan2(PUMA_D3, -sqrt(sumSq))) < FLAG_FUZZ) + { + *iflags |= PUMA_SHOULDER_RIGHT; + } + + /* Set elbow down flag if necessary */ + if (fabs(joint[2]*PM_PI/180 - atan2(PUMA_A3, PUMA_D4) + + atan2(k, -sqrt(PUMA_A3 * PUMA_A3 + + PUMA_D4 * PUMA_D4 - k * k))) < FLAG_FUZZ) + { + *iflags |= PUMA_ELBOW_DOWN; + } + + /* set singular flag if necessary */ + t1 = -hom.rot.z.x * s1 + hom.rot.z.y * c1; + t2 = -hom.rot.z.x * c1 * c23 - hom.rot.z.y * s1 * c23 + + hom.rot.z.z * s23; + if (fabs(t1) < SINGULAR_FUZZ && fabs(t2) < SINGULAR_FUZZ) + { + *iflags |= PUMA_SINGULAR; + } + + /* if not singular set wrist flip flag if necessary */ + else{ + if (! (fabs(joint[3]*PM_PI/180 - atan2(t1, t2)) < FLAG_FUZZ)) + { + *iflags |= PUMA_WRIST_FLIP; + } + } + /* add effect of d6 parameter */ + hom.tran.x = hom.tran.x + hom.rot.z.x*PUMA_D6; + hom.tran.y = hom.tran.y + hom.rot.z.y*PUMA_D6; + hom.tran.z = hom.tran.z + hom.rot.z.z*PUMA_D6; + + /* convert hom.rot to world->quat */ + pmHomPoseConvert(&hom, &worldPose); + pmQuatRpyConvert(&worldPose.rot,&rpy); + world->tran = worldPose.tran; + world->a = rpy.r * 180.0/PM_PI; + world->b = rpy.p * 180.0/PM_PI; + world->c = rpy.y * 180.0/PM_PI; + + + /* return 0 and exit */ + return 0; +} + +static int pumaKinematicsInverse(const EmcPose * world, + double * joint, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) +{ + PmHomogeneous hom; + PmPose worldPose; + PmRpy rpy; + + double t1, t2, t3; + double k; + double sumSq; + + double th1; + double th3; + double th23; + double th2; + double th4; + double th5; + double th6; + + double s1, c1; + double s3, c3; + double s23, c23; + double s4, c4; + double s5, c5; + double s6, c6; + double px, py, pz; + + /* reset flags */ + *fflags = 0; + + /* convert pose to hom */ + worldPose.tran = world->tran; + rpy.r = world->a*PM_PI/180.0; + rpy.p = world->b*PM_PI/180.0; + rpy.y = world->c*PM_PI/180.0; + pmRpyQuatConvert(&rpy,&worldPose.rot); + pmPoseHomConvert(&worldPose, &hom); + + /* remove effect of d6 parameter */ + px = hom.tran.x - PUMA_D6*hom.rot.z.x; + py = hom.tran.y - PUMA_D6*hom.rot.z.y; + pz = hom.tran.z - PUMA_D6*hom.rot.z.z; + + /* Joint 1 (2 independent solutions) */ + + /* save sum of squares for this and subsequent calcs */ + sumSq = px * px + py * py - + PUMA_D3 * PUMA_D3; + + /* FIXME-- is use of + sqrt shoulder right or left? */ + if (*iflags & PUMA_SHOULDER_RIGHT){ + th1 = atan2(py, px) - atan2(PUMA_D3, -sqrt(sumSq)); + } + else{ + th1 = atan2(py, px) - atan2(PUMA_D3, sqrt(sumSq)); + } + + /* save sin, cos for later calcs */ + s1 = sin(th1); + c1 = cos(th1); + + /* Joint 3 (2 independent solutions) */ + + k = (sumSq + pz * pz - PUMA_A2 * PUMA_A2 - + PUMA_A3 * PUMA_A3 - PUMA_D4 * PUMA_D4) / (2.0 * PUMA_A2); + + /* FIXME-- is use of + sqrt elbow up or down? */ + if (*iflags & PUMA_ELBOW_DOWN){ + th3 = atan2(PUMA_A3, PUMA_D4) - atan2(k, -sqrt(PUMA_A3 * PUMA_A3 + PUMA_D4 * PUMA_D4 - k * k)); + } + else{ + th3 = atan2(PUMA_A3, PUMA_D4) - + atan2(k, sqrt(PUMA_A3 * PUMA_A3 + PUMA_D4 * PUMA_D4 - k * k)); + } + + /* compute sin, cos for later calcs */ + s3 = sin(th3); + c3 = cos(th3); + + /* Joint 2 */ + + t1 = (-PUMA_A3 - PUMA_A2 * c3) * pz + + (c1 * px + s1 * py) * (PUMA_A2 * s3 - PUMA_D4); + t2 = (PUMA_A2 * s3 - PUMA_D4) * pz + + (PUMA_A3 + PUMA_A2 * c3) * (c1 * px + s1 * py); + t3 = pz * pz + (c1 * px + s1 * py) * + (c1 * px + s1 * py); + + th23 = atan2(t1, t2); + th2 = th23 - th3; + + /* compute sin, cos for later calcs */ + s23 = t1 / t3; + c23 = t2 / t3; + + /* Joint 4 */ + + t1 = -hom.rot.z.x * s1 + hom.rot.z.y * c1; + t2 = -hom.rot.z.x * c1 * c23 - hom.rot.z.y * s1 * c23 + hom.rot.z.z * s23; + if (fabs(t1) < SINGULAR_FUZZ && fabs(t2) < SINGULAR_FUZZ){ + *fflags |= PUMA_REACH; + th4 = joint[3]*PM_PI/180; /* use current value */ + } + else{ + th4 = atan2(t1, t2); + } + + /* compute sin, cos for later calcs */ + s4 = sin(th4); + c4 = cos(th4); + + /* Joint 5 */ + + s5 = hom.rot.z.z * (s23 * c4) - + hom.rot.z.x * (c1 * c23 * c4 + s1 * s4) - + hom.rot.z.y * (s1 * c23 * c4 - c1 * s4); + c5 =-hom.rot.z.x * (c1 * s23) - hom.rot.z.y * + (s1 * s23) - hom.rot.z.z * c23; + th5 = atan2(s5, c5); + + /* Joint 6 */ + + s6 = hom.rot.x.z * (s23 * s4) - hom.rot.x.x * + (c1 * c23 * s4 - s1 * c4) - hom.rot.x.y * + (s1 * c23 * s4 + c1 * c4); + c6 = hom.rot.x.x * ((c1 * c23 * c4 + s1 * s4) * + c5 - c1 * s23 * s5) + hom.rot.x.y * + ((s1 * c23 * c4 - c1 * s4) * c5 - s1 * s23 * s5) - + hom.rot.x.z * (s23 * c4 * c5 + c23 * s5); + th6 = atan2(s6, c6); + + /* FIXME-- is wrist flip the normal or offset results? */ + if (*iflags & PUMA_WRIST_FLIP){ + th4 = th4 + PM_PI; + th5 = -th5; + th6 = th6 + PM_PI; + } + + /* copy out */ + joint[0] = th1*180/PM_PI; + joint[1] = th2*180/PM_PI; + joint[2] = th3*180/PM_PI; + joint[3] = th4*180/PM_PI; + joint[4] = th5*180/PM_PI; + joint[5] = th6*180/PM_PI; + + return 0; +} + +int pumaKinematicsSetup(const int comp_id, + const char* coordinates, + kparms* kp) +{ + (void)coordinates; + int res=0; + + haldata = hal_malloc(sizeof(*haldata)); + if (!haldata) goto error; + + + res += hal_pin_float_newf(HAL_IN, &(haldata->a2), comp_id,"%s.A2",kp->halprefix); + res += hal_pin_float_newf(HAL_IN, &(haldata->a3), comp_id,"%s.A3",kp->halprefix); + res += hal_pin_float_newf(HAL_IN, &(haldata->d3), comp_id,"%s.D3",kp->halprefix); + res += hal_pin_float_newf(HAL_IN, &(haldata->d4), comp_id,"%s.D4",kp->halprefix); + res += hal_pin_float_newf(HAL_IN, &(haldata->d6), comp_id,"%s.D6",kp->halprefix); + if (res) { goto error; } + + PUMA_A2 = DEFAULT_PUMA560_A2; + PUMA_A3 = DEFAULT_PUMA560_A3; + PUMA_D3 = DEFAULT_PUMA560_D3; + PUMA_D4 = DEFAULT_PUMA560_D4; + PUMA_D6 = DEFAULT_PUMA560_D6; + + return 0; + +error: + return -1; +} // pumaKinematicsSetup() + +int switchkinsSetup(kparms* kp, + KS* kset0, KS* kset1, KS* kset2, + KF* kfwd0, KF* kfwd1, KF* kfwd2, + KI* kinv0, KI* kinv1, KI* kinv2 + ) +{ + kp->kinsname = "pumakins"; // !!! must agree with filename + kp->halprefix = "pumakins"; // hal pin names + kp->required_coordinates = "xyzabc"; + kp->allow_duplicates = 0; + kp->max_joints = strlen(kp->required_coordinates); + + rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname); + *kset0 = pumaKinematicsSetup; + *kfwd0 = pumaKinematicsForward; + *kinv0 = pumaKinematicsInverse; + + *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/pumakins.h b/wasm-port/vendor/linuxcnc/src/emc/kinematics/pumakins.h new file mode 100644 index 0000000..6858010 --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/emc/kinematics/pumakins.h @@ -0,0 +1,43 @@ +/***************************************************************** +* Description: pumakins.h +* Kinematics for a puma typed robot +* +* Derived from a work by Fred Proctor +* +* rdp added PUMA560_D6 (left the old values which may be used in puma560kins?) +* +* Author: +* License: GPL Version 2 +* System: Linux +* +* Copyright (c) 2004 All rights reserved. +* +* Last change: +******************************************************************* +* This is the header file to accompany pumakins.c. +******************************************************************* +*/ +#ifndef PUMA_H +#define PUMA_H + + +/* the default values for a PUMA 560 type robot, these can be changed as HAL parameters */ +#define DEFAULT_PUMA560_A2 300.0 +#define DEFAULT_PUMA560_A3 50.0 +#define DEFAULT_PUMA560_D3 70.0 +#define DEFAULT_PUMA560_D4 400.0 +#define DEFAULT_PUMA560_D6 70.0 + +#define SINGULAR_FUZZ 0.000001 +#define FLAG_FUZZ 0.000001 + +/* flags for inverse kinematics */ +#define PUMA_SHOULDER_RIGHT 0x01 +#define PUMA_ELBOW_DOWN 0x02 +#define PUMA_WRIST_FLIP 0x04 +#define PUMA_SINGULAR 0x08 /* joints at a singularity */ + +/* flags for forward kinematics */ +#define PUMA_REACH 0x01 /* pose out of reach */ + +#endif /* PUMA_H */