按推荐建议，继续执行

结论：继续按 LinuxCNC 源码直接复用路线推进，新增 PUMA 运动学源码的 vendored 同步、switchkins native 探针、source probe 覆盖和复用文档，并通过 native 验证。

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

wasm-port/runtime/core/linuxcnc_wrap/linuxcnc_puma_kinematics_probe.cpp

@@ -0,0 +1,109 @@
+#include <cmath>
+#include <iostream>
+
+#include "emc/kinematics/kinematics.h"
+#include "emc/motion/emcmotcfg.h"
+
+int rtapi_app_main(void);
+void rtapi_app_exit(void);
+
+namespace {
+
+int near(double actual, double expected)
+{
+    return std::fabs(actual - expected) < 1e-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;
+}

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"

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" \

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

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 <rtapi.h>
+#include <rtapi_math.h>
+#include <rtapi_string.h>
+#include <hal.h>
+#include <kinematics.h>
+
+#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()

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 */