From 42f1acd34ac086c2e512ff12dd08c86a169337ec Mon Sep 17 00:00:00 2001 From: wangdequan Date: Sun, 7 Jun 2026 20:16:32 +0800 Subject: [PATCH] =?UTF-8?q?=E6=8C=89=E6=8E=A8=E8=8D=90=E5=BB=BA=E8=AE=AE?= =?UTF-8?q?=EF=BC=8C=E7=BB=A7=E7=BB=AD=E6=89=A7=E8=A1=8C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 结论：继续按 LinuxCNC 源码直接复用路线推进，新增 SCARA 运动学源码的 vendored 同步、switchkins native 探针、source probe 覆盖和复用文档，并通过 native 验证。 --- wasm-port/docs/source-reuse-map.md | 5 +- .../linuxcnc_scara_kinematics_probe.cpp | 101 ++++++++ .../tests/native/verify_native_probes.sh | 15 ++ wasm-port/tools/build_native_probes.sh | 37 +++ wasm-port/tools/source-manifest.txt | 1 + .../linuxcnc/src/emc/kinematics/scarakins.c | 238 ++++++++++++++++++ 6 files changed, 394 insertions(+), 3 deletions(-) create mode 100644 wasm-port/runtime/core/linuxcnc_wrap/linuxcnc_scara_kinematics_probe.cpp create mode 100644 wasm-port/vendor/linuxcnc/src/emc/kinematics/scarakins.c diff --git a/wasm-port/docs/source-reuse-map.md b/wasm-port/docs/source-reuse-map.md index 3d71c11..dd2231d 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` | 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` | +| 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` | | Trajectory planner | `src/emc/tp/tp.c`, `tc.c`, `tcq.c`, `spherical_arc.c`, `blendmath.c`, `sp_scurve.c`, `ruckig_wrapper.c`, plus matching `*.h` files | Copy unchanged | Native realtime scheduling and motion process state are replaced by standalone probe setup | Vendor byte sync, per-file source probes, `linuxcnc_tp_api_probe` | | Ruckig C planner support | Selected `src/emc/tp/cruckig/*.c` and `*.h` files in the manifest | Copy unchanged | Used as LinuxCNC planner support code through vendored TP sources | Vendor byte sync, per-file source probes | | Posemath | `src/libnml/posemath/posemath.cc`, `_posemath.c`, `gomath.c`, `sincos.c`, and matching headers | Copy unchanged | `gomath.c` is compiled as C; `rtapi.h` shim is C/C++ compatible for this boundary | Vendor byte sync, per-file source probes, TP probe | @@ -63,8 +63,7 @@ Current validation is intentionally mechanical: ## Known Gaps - Additional non-trivial kinematics implementation files, including serial, - SCARA, hexapod, puma, and other machine-specific modules, are not yet - extracted. + hexapod, puma, and other machine-specific modules, are not yet extracted. - Browser/WASM C ABI and JS SDK layers are not yet built for the full interpreter/planner core. - OPFS persistence is not yet connected to INI, tool table, parameter file, or diff --git a/wasm-port/runtime/core/linuxcnc_wrap/linuxcnc_scara_kinematics_probe.cpp b/wasm-port/runtime/core/linuxcnc_wrap/linuxcnc_scara_kinematics_probe.cpp new file mode 100644 index 0000000..714c7b1 --- /dev/null +++ b/wasm-port/runtime/core/linuxcnc_wrap/linuxcnc_scara_kinematics_probe.cpp @@ -0,0 +1,101 @@ +#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-9; +} + +int near_joints(const double *actual, const double *expected) +{ + for (int index = 0; index < 6; ++index) { + if (!near(actual[index], expected[index])) { + return 0; + } + } + return 1; +} + +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 << "scara_init=" << init_rc << "\n"; + std::cout << "scara_type=" << kinematicsType() << "\n"; + std::cout << "scara_switchable=" << kinematicsSwitchable() << "\n"; + + double joints[EMCMOT_MAX_JOINTS]{}; + joints[0] = 20.0; + joints[1] = 110.0; + joints[2] = 30.0; + joints[3] = 15.0; + joints[4] = 4.0; + joints[5] = 5.0; + + KINEMATICS_FORWARD_FLAGS fflags = 0; + KINEMATICS_INVERSE_FLAGS iflags = 0; + EmcPose forward_pose{}; + const int forward_rc = kinematicsForward(joints, &forward_pose, &fflags, &iflags); + std::cout << "scara_forward=" << forward_rc << "\n"; + std::cout << "scara_forward_iflags=" << iflags << "\n"; + print_pose("scara_forward", forward_pose); + + double inverse_joints[EMCMOT_MAX_JOINTS]{}; + const int inverse_rc = kinematicsInverse(&forward_pose, inverse_joints, &iflags, &fflags); + std::cout << "scara_inverse=" << inverse_rc << "\n"; + print_joints("scara_inverse", inverse_joints); + std::cout << "scara_roundtrip_joints=" << near_joints(inverse_joints, joints) << "\n"; + + const int switch_rc = kinematicsSwitch(1); + std::cout << "scara_switch_identity=" << switch_rc << "\n"; + EmcPose identity_pose{}; + const int identity_forward_rc = kinematicsForward(joints, &identity_pose, &fflags, &iflags); + std::cout << "scara_identity_forward=" << identity_forward_rc << "\n"; + print_pose("scara_identity", identity_pose); + std::cout << "scara_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 5f50155..dadf1db 100755 --- a/wasm-port/tests/native/verify_native_probes.sh +++ b/wasm-port/tests/native/verify_native_probes.sh @@ -103,6 +103,8 @@ check_exitcode linuxcnc_scorbot_kinematics_probe check_exitcode linuxcnc_scorbot_kinematics_probe.run check_exitcode linuxcnc_tripod_kinematics_probe check_exitcode linuxcnc_tripod_kinematics_probe.run +check_exitcode linuxcnc_scara_kinematics_probe +check_exitcode linuxcnc_scara_kinematics_probe.run for name in \ linuxcnc_tp_tp_source_probe \ linuxcnc_tp_tc_source_probe \ @@ -159,6 +161,7 @@ check_exitcode linuxcnc_lineardeltakins_source_probe check_exitcode linuxcnc_rotarydeltakins_source_probe check_exitcode linuxcnc_scorbot_kins_source_probe check_exitcode linuxcnc_tripodkins_source_probe +check_exitcode linuxcnc_scarakins_source_probe check_exitcode linuxcnc_interp_convert_source_probe check_exitcode linuxcnc_interp_read_source_probe check_exitcode linuxcnc_interp_check_source_probe @@ -357,6 +360,18 @@ grep -Fq "tripod_inverse_below_flags=1" "$TRIPOD_KINEMATICS_STDOUT" grep -Fq "tripod_forward_below=0" "$TRIPOD_KINEMATICS_STDOUT" grep -Fq "tripod_roundtrip_below_xyz=1" "$TRIPOD_KINEMATICS_STDOUT" +SCARA_KINEMATICS_STDOUT="$BUILD_DIR/linuxcnc_scara_kinematics_probe.run.stdout.log" +grep -Fq "scara_init=0" "$SCARA_KINEMATICS_STDOUT" +grep -Fq "scara_type=4" "$SCARA_KINEMATICS_STDOUT" +grep -Fq "scara_switchable=1" "$SCARA_KINEMATICS_STDOUT" +grep -Fq "scara_forward=0" "$SCARA_KINEMATICS_STDOUT" +grep -Fq "scara_forward_iflags=0" "$SCARA_KINEMATICS_STDOUT" +grep -Fq "scara_inverse=0" "$SCARA_KINEMATICS_STDOUT" +grep -Fq "scara_roundtrip_joints=1" "$SCARA_KINEMATICS_STDOUT" +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" + 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 b220130..99dc829 100755 --- a/wasm-port/tools/build_native_probes.sh +++ b/wasm-port/tools/build_native_probes.sh @@ -430,6 +430,15 @@ TRIPOD_KINEMATICS_PROBE_SOURCES=( "$WRAP_DIR/linuxcnc_tripod_kinematics_probe.cpp" ) +SCARA_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/scarakins.c" + "$WRAP_DIR/linuxcnc_hal_adapter.cpp" + "$WRAP_DIR/linuxcnc_scara_kinematics_probe.cpp" +) + TP_CORE_SOURCES=( "$VENDOR_DIR/src/emc/tp/tp.c" "$VENDOR_DIR/src/emc/tp/tc.c" @@ -687,6 +696,13 @@ build_binary_target \ TRIPOD_KINEMATICS_PROBE_SOURCES \ NO_LINK_FLAGS +build_binary_target \ + linuxcnc_scara_kinematics_probe \ + "$BUILD_DIR/linuxcnc_scara_kinematics_probe" \ + TP_FLAGS \ + SCARA_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" \ @@ -867,6 +883,21 @@ else "$BUILD_DIR/linuxcnc_tripod_kinematics_probe.run.stderr.log" fi +if [[ "$(tr -d '[:space:]' < "$BUILD_DIR/linuxcnc_scara_kinematics_probe.exitcode")" == "0" ]]; then + set +e + "$BUILD_DIR/linuxcnc_scara_kinematics_probe" \ + >"$BUILD_DIR/linuxcnc_scara_kinematics_probe.run.stdout.log" \ + 2>"$BUILD_DIR/linuxcnc_scara_kinematics_probe.run.stderr.log" + SCARA_KINEMATICS_RUN_RC=$? + set -e + echo "$SCARA_KINEMATICS_RUN_RC" > "$BUILD_DIR/linuxcnc_scara_kinematics_probe.run.exitcode" +else + rm -f \ + "$BUILD_DIR/linuxcnc_scara_kinematics_probe.run.exitcode" \ + "$BUILD_DIR/linuxcnc_scara_kinematics_probe.run.stdout.log" \ + "$BUILD_DIR/linuxcnc_scara_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" \ @@ -1061,6 +1092,12 @@ build_object_target \ "$VENDOR_DIR/src/emc/kinematics/tripodkins.c" \ TP_FLAGS +build_object_target \ + linuxcnc_scarakins_source_probe \ + "$BUILD_DIR/linuxcnc_scarakins_source_probe.o" \ + "$VENDOR_DIR/src/emc/kinematics/scarakins.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 849e188..477a5f2 100644 --- a/wasm-port/tools/source-manifest.txt +++ b/wasm-port/tools/source-manifest.txt @@ -50,6 +50,7 @@ src/emc/kinematics/rotarydeltakins-common.h src/emc/kinematics/rotarydeltakins.c src/emc/kinematics/scorbot-kins.c src/emc/kinematics/tripodkins.c +src/emc/kinematics/scarakins.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/scarakins.c b/wasm-port/vendor/linuxcnc/src/emc/kinematics/scarakins.c new file mode 100644 index 0000000..235cf6a --- /dev/null +++ b/wasm-port/vendor/linuxcnc/src/emc/kinematics/scarakins.c @@ -0,0 +1,238 @@ +/***************************************************************** +* Description: scarakins.c +* Kinematics for scara typed robots +* Set the params using HAL to fit your robot +* +* Derived from a work by Sagar Behere +* +* Author: Sagar Behere +* License: GPL Version 2 +* System: Linux +* +* Copyright (c) 2003 All rights reserved. +* +* Last change: +******************************************************************* +*/ + +#include +#include +#include +#include +#include + +#include "switchkins.h" + +struct scara_data { + hal_float_t *d1, *d2, *d3, *d4, *d5, *d6; +} *haldata = 0; + +/* key dimensions + + joint[0] = Entire arm rotates around a vertical axis at its inner end + which is attached to the earth. A value of zero means the + inner arm is pointing along the X axis. + D1 = Vertical distance from the ground plane to the center of the inner + arm. + D2 = Horizontal distance between joint[0] axis and joint[1] axis, ie. + the length of the inner arm. + joint[1] = Outer arm rotates around a vertical axis at its inner end + which is attached to the outer end of the inner arm. A + value of zero means the outer arm is parallel to the + inner arm (and extending outward). + D3 = Vertical distance from the center of the inner arm to the center + of the outer arm. May be positive or negative depending + on the structure of the robot. + joint[2] = End effector slides along a vertical axis at the outer end + of the outer arm. A value of zero means the end effector + is at the same height as the center of the outer arm, and + positive values mean downward movement. + D4 = Horizontal distance between joint[1] axis and joint[2] axis, ie. + the length of the outer arm + joint[3] = End effector rotates around the same vertical axis that it + slides along. A value of zero means that the tooltip (if + offset from the axis) is pointing in the same direction + as the centerline of the outer arm. + D5 = Vertical distance from the end effector to the tooltip. Positive + means the tooltip is lower than the end effector, and is + the normal case. + D6 = Horizontal distance from the centerline of the end effector (and + the joints 2 and 3 axis) and the tooltip. Zero means the + tooltip is on the centerline. Non-zero values should be + positive, if negative they introduce a 180 degree offset + on the value of joint[3]. +*/ + +#define D1 (*(haldata->d1)) +#define D2 (*(haldata->d2)) +#define D3 (*(haldata->d3)) +#define D4 (*(haldata->d4)) +#define D5 (*(haldata->d5)) +#define D6 (*(haldata->d6)) + +/* joint[0], joint[1] and joint[3] are in degrees and joint[2] is in length units */ +static +int scaraKinematicsForward(const double * joint, + EmcPose * world, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) +{ + (void)fflags; + double a0, a1, a3; + double x, y, z, c; + +/* convert joint angles to radians for sin() and cos() */ + + a0 = joint[0] * ( PM_PI / 180 ); + a1 = joint[1] * ( PM_PI / 180 ); + a3 = joint[3] * ( PM_PI / 180 ); +/* convert angles into world coords */ + + a1 = a1 + a0; + a3 = a3 + a1; + + x = D2*cos(a0) + D4*cos(a1) + D6*cos(a3); + y = D2*sin(a0) + D4*sin(a1) + D6*sin(a3); + z = D1 + D3 - joint[2] - D5; + c = a3; + + *iflags = 0; + if (joint[1] < 90) + *iflags = 1; + + world->tran.x = x; + world->tran.y = y; + world->tran.z = z; + world->c = c * 180 / PM_PI; + + world->a = joint[4]; + world->b = joint[5]; + + return (0); +} //scaraKinematicsForward() + +static int scaraKinematicsInverse(const EmcPose * world, + double * joint, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) +{ + double a3; + double q0, q1; + double xt, yt, rsq, cc; + double x, y, z, c; + + x = world->tran.x; + y = world->tran.y; + z = world->tran.z; + c = world->c; + + /* convert degrees to radians */ + a3 = c * ( PM_PI / 180 ); + + /* center of end effector (correct for D6) */ + xt = x - D6*cos(a3); + yt = y - D6*sin(a3); + + /* horizontal distance (squared) from end effector centerline + to main column centerline */ + rsq = xt*xt + yt*yt; + /* joint 1 angle needed to make arm length match sqrt(rsq) */ + cc = (rsq - D2*D2 - D4*D4) / (2*D2*D4); + if(cc < -1) cc = -1; + if(cc > 1) cc = 1; + q1 = acos(cc); + + if (*iflags) + q1 = -q1; + + /* angle to end effector */ + q0 = atan2(yt, xt); + + /* end effector coords in inner arm coord system */ + xt = D2 + D4*cos(q1); + yt = D4*sin(q1); + + /* inner arm angle */ + q0 = q0 - atan2(yt, xt); + + /* q0 and q1 are still in radians. convert them to degrees */ + q0 = q0 * (180 / PM_PI); + q1 = q1 * (180 / PM_PI); + + joint[0] = q0; + joint[1] = q1; + joint[2] = D1 + D3 - D5 - z; + joint[3] = c - ( q0 + q1); + joint[4] = world->a; + joint[5] = world->b; + + *fflags = 0; + + return (0); +} // scaraKinematicsInverse() + +#define DEFAULT_D1 490 +#define DEFAULT_D2 340 +#define DEFAULT_D3 50 +#define DEFAULT_D4 250 +#define DEFAULT_D5 50 +#define DEFAULT_D6 50 + +static int scaraKinematicsSetup(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->d1), comp_id,"%s.D1",kp->halprefix); + res += hal_pin_float_newf(HAL_IN, &(haldata->d2), comp_id,"%s.D2",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->d5), comp_id,"%s.D5",kp->halprefix); + res += hal_pin_float_newf(HAL_IN, &(haldata->d6), comp_id,"%s.D6",kp->halprefix); + if (res) { goto error; } + + D1 = DEFAULT_D1; + D2 = DEFAULT_D2; + D3 = DEFAULT_D3; + D4 = DEFAULT_D4; + D5 = DEFAULT_D5; + D6 = DEFAULT_D6; + + return 0; + +error: + return -1; +} // scaraKinematicsSetup() + +int switchkinsSetup(kparms* kp, + KS* kset0, KS* kset1, KS* kset2, + KF* kfwd0, KF* kfwd1, KF* kfwd2, + KI* kinv0, KI* kinv1, KI* kinv2 + ) +{ + kp->kinsname = "scarakins"; // !!! must agree with filename + kp->halprefix = "scarakins"; // hal pin names + kp->required_coordinates = "xyzabc"; // ab are scaragui table tilts + kp->allow_duplicates = 0; + kp->max_joints = strlen(kp->required_coordinates); + + rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname); + *kset0 = scaraKinematicsSetup; + *kfwd0 = scaraKinematicsForward; + *kinv0 = scaraKinematicsInverse; + + *kset1 = identityKinematicsSetup; + *kfwd1 = identityKinematicsForward; + *kinv1 = identityKinematicsInverse; + + *kset2 = userkKinematicsSetup; + *kfwd2 = userkKinematicsForward; + *kinv2 = userkKinematicsInverse; + + return 0; +} // switchkinsSetup()