wangdequan/cnc_wams
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

按推荐建议，继续执行

Browse Source 
结论：继续按 LinuxCNC 源码直接复用路线推进，新增 linear delta、rotary delta、scorbot 三类运动学源码的 vendored 同步、native 探针、source probe 覆盖和复用文档，并通过 native 验证。
This commit is contained in:
wangdequan
2026-06-07 19:55:43 +08:00
parent d936e9f10c
commit 01495ee26b
12 changed files with 1276 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
wasm-port/docs/source-reuse-map.md
View File

@@ -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` | 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` |
| Additional non-switchable kinematics | `src/emc/kinematics/corexykins.c`, `rotatekins.c`, `rosekins.c`, `maxkins.c`, `lineardeltakins.c`, `lineardeltakins-common.h`, `rotarydeltakins.c`, `rotarydeltakins-common.h`, `scorbot-kins.c` | Copy unchanged | HAL pin allocation, HAL component lifecycle, and RTAPI module metadata are standalone runtime edges; forward/inverse behavior remains LinuxCNC source | Vendor byte sync, per-file source probes, `linuxcnc_corexy_kinematics_probe`, `linuxcnc_rotate_kinematics_probe`, `linuxcnc_rose_kinematics_probe`, `linuxcnc_max_kinematics_probe`, `linuxcnc_lineardelta_kinematics_probe`, `linuxcnc_rotarydelta_kinematics_probe`, `linuxcnc_scorbot_kinematics_probe` |
| 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,
delta, SCARA, hexapod, tripod, and other machine-specific modules, are not
SCARA, hexapod, tripod, 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.

93
wasm-port/runtime/core/linuxcnc_wrap/linuxcnc_lineardelta_kinematics_probe.cpp Normal file
View File

@@ -0,0 +1,93 @@
#include <cmath>
#include <iostream>
#include "emc/kinematics/kinematics.h"
int rtapi_app_main(void);
void rtapi_app_exit(void);
namespace {
int near(double actual, double expected)
{
return std::fabs(actual - expected) < 1e-9;
}
int near_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) &&
near(actual.u, expected.u) &&
near(actual.v, expected.v) &&
near(actual.w, expected.w);
}
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 << "_abcuvw="
<< pose.a << ","
<< pose.b << ","
<< pose.c << ","
<< pose.u << ","
<< pose.v << ","
<< pose.w << "\n";
}
void print_joints(const char *prefix, const double *joints)
{
std::cout << prefix << "_xyzabcuvw="
<< joints[0] << ","
<< joints[1] << ","
<< joints[2] << ","
<< joints[3] << ","
<< joints[4] << ","
<< joints[5] << ","
<< joints[6] << ","
<< joints[7] << ","
<< joints[8] << "\n";
}
} // namespace
int main()
{
const int init_rc = rtapi_app_main();
std::cout << "lineardelta_init=" << init_rc << "\n";
std::cout << "lineardelta_type=" << kinematicsType() << "\n";
std::cout << "lineardelta_switchable=" << kinematicsSwitchable() << "\n";
EmcPose pose{};
pose.tran.x = 10.0;
pose.tran.y = 20.0;
pose.tran.z = -30.0;
pose.a = 1.0;
pose.b = 2.0;
pose.c = 3.0;
pose.u = 4.0;
pose.v = 5.0;
pose.w = 6.0;
KINEMATICS_FORWARD_FLAGS fflags = 0;
KINEMATICS_INVERSE_FLAGS iflags = 0;
double inverse_joints[9]{};
const int inverse_rc = kinematicsInverse(&pose, inverse_joints, &iflags, &fflags);
std::cout << "lineardelta_inverse=" << inverse_rc << "\n";
print_joints("lineardelta_inverse", inverse_joints);
EmcPose forward_pose{};
const int forward_rc = kinematicsForward(inverse_joints, &forward_pose, &fflags, &iflags);
std::cout << "lineardelta_forward=" << forward_rc << "\n";
print_pose("lineardelta_forward", forward_pose);
std::cout << "lineardelta_roundtrip_pose=" << near_pose(forward_pose, pose) << "\n";
rtapi_app_exit();
return 0;
}

93
wasm-port/runtime/core/linuxcnc_wrap/linuxcnc_rotarydelta_kinematics_probe.cpp Normal file
View File

@@ -0,0 +1,93 @@
#include <cmath>
#include <iostream>
#include "emc/kinematics/kinematics.h"
int rtapi_app_main(void);
void rtapi_app_exit(void);
namespace {
int near(double actual, double expected)
{
return std::fabs(actual - expected) < 1e-9;
}
int near_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) &&
near(actual.u, expected.u) &&
near(actual.v, expected.v) &&
near(actual.w, expected.w);
}
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 << "_abcuvw="
<< pose.a << ","
<< pose.b << ","
<< pose.c << ","
<< pose.u << ","
<< pose.v << ","
<< pose.w << "\n";
}
void print_joints(const char *prefix, const double *joints)
{
std::cout << prefix << "_xyzabcuvw="
<< joints[0] << ","
<< joints[1] << ","
<< joints[2] << ","
<< joints[3] << ","
<< joints[4] << ","
<< joints[5] << ","
<< joints[6] << ","
<< joints[7] << ","
<< joints[8] << "\n";
}
} // namespace
int main()
{
const int init_rc = rtapi_app_main();
std::cout << "rotarydelta_init=" << init_rc << "\n";
std::cout << "rotarydelta_type=" << kinematicsType() << "\n";
std::cout << "rotarydelta_switchable=" << kinematicsSwitchable() << "\n";
EmcPose pose{};
pose.tran.x = 0.0;
pose.tran.y = 0.0;
pose.tran.z = -12.0;
pose.a = 1.0;
pose.b = 2.0;
pose.c = 3.0;
pose.u = 4.0;
pose.v = 5.0;
pose.w = 6.0;
KINEMATICS_FORWARD_FLAGS fflags = 0;
KINEMATICS_INVERSE_FLAGS iflags = 0;
double inverse_joints[9]{};
const int inverse_rc = kinematicsInverse(&pose, inverse_joints, &iflags, &fflags);
std::cout << "rotarydelta_inverse=" << inverse_rc << "\n";
print_joints("rotarydelta_inverse", inverse_joints);
EmcPose forward_pose{};
const int forward_rc = kinematicsForward(inverse_joints, &forward_pose, &fflags, &iflags);
std::cout << "rotarydelta_forward=" << forward_rc << "\n";
print_pose("rotarydelta_forward", forward_pose);
std::cout << "rotarydelta_roundtrip_pose=" << near_pose(forward_pose, pose) << "\n";
rtapi_app_exit();
return 0;
}

82
wasm-port/runtime/core/linuxcnc_wrap/linuxcnc_scorbot_kinematics_probe.cpp Normal file
View File

@@ -0,0 +1,82 @@
#include <cmath>
#include <iostream>
#include "emc/kinematics/kinematics.h"
int rtapi_app_main(void);
void rtapi_app_exit(void);
namespace {
int near(double actual, double expected)
{
return std::fabs(actual - expected) < 1e-9;
}
int near_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);
}
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 << "_ab="
<< pose.a << ","
<< pose.b << "\n";
}
void print_joints(const char *prefix, const double *joints)
{
std::cout << prefix << "_xyzab="
<< joints[0] << ","
<< joints[1] << ","
<< joints[2] << ","
<< joints[3] << ","
<< joints[4] << "\n";
}
} // namespace
int main()
{
const int init_rc = rtapi_app_main();
std::cout << "scorbot_init=" << init_rc << "\n";
std::cout << "scorbot_type=" << kinematicsType() << "\n";
std::cout << "scorbot_switchable=" << kinematicsSwitchable() << "\n";
double joints[9]{};
joints[0] = 25.0;
joints[1] = 20.0;
joints[2] = 10.0;
joints[3] = 4.0;
joints[4] = 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 << "scorbot_forward=" << forward_rc << "\n";
print_pose("scorbot_forward", forward_pose);
double inverse_joints[9]{};
const int inverse_rc = kinematicsInverse(&forward_pose, inverse_joints, &iflags, &fflags);
std::cout << "scorbot_inverse=" << inverse_rc << "\n";
print_joints("scorbot_inverse", inverse_joints);
EmcPose roundtrip_pose{};
const int roundtrip_rc = kinematicsForward(inverse_joints, &roundtrip_pose, &fflags, &iflags);
std::cout << "scorbot_roundtrip_forward=" << roundtrip_rc << "\n";
print_pose("scorbot_roundtrip", roundtrip_pose);
std::cout << "scorbot_roundtrip_pose=" << near_pose(roundtrip_pose, forward_pose) << "\n";
rtapi_app_exit();
return 0;
}

34
wasm-port/tests/native/verify_native_probes.sh
View File

@@ -95,6 +95,12 @@ check_exitcode linuxcnc_rose_kinematics_probe
check_exitcode linuxcnc_rose_kinematics_probe.run
check_exitcode linuxcnc_max_kinematics_probe
check_exitcode linuxcnc_max_kinematics_probe.run
check_exitcode linuxcnc_lineardelta_kinematics_probe
check_exitcode linuxcnc_lineardelta_kinematics_probe.run
check_exitcode linuxcnc_rotarydelta_kinematics_probe
check_exitcode linuxcnc_rotarydelta_kinematics_probe.run
check_exitcode linuxcnc_scorbot_kinematics_probe
check_exitcode linuxcnc_scorbot_kinematics_probe.run
for name in \
linuxcnc_tp_tp_source_probe \
linuxcnc_tp_tc_source_probe \
@@ -147,6 +153,9 @@ check_exitcode linuxcnc_corexykins_source_probe
check_exitcode linuxcnc_rotatekins_source_probe
check_exitcode linuxcnc_rosekins_source_probe
check_exitcode linuxcnc_maxkins_source_probe
check_exitcode linuxcnc_lineardeltakins_source_probe
check_exitcode linuxcnc_rotarydeltakins_source_probe
check_exitcode linuxcnc_scorbot_kins_source_probe
check_exitcode linuxcnc_interp_convert_source_probe
check_exitcode linuxcnc_interp_read_source_probe
check_exitcode linuxcnc_interp_check_source_probe
@@ -307,6 +316,31 @@ grep -Fq "max_forward=0" "$MAX_KINEMATICS_STDOUT"
grep -Fq "max_inverse=0" "$MAX_KINEMATICS_STDOUT"
grep -Fq "max_roundtrip_joints=1" "$MAX_KINEMATICS_STDOUT"
LINEARDELTA_KINEMATICS_STDOUT="$BUILD_DIR/linuxcnc_lineardelta_kinematics_probe.run.stdout.log"
grep -Fq "lineardelta_init=0" "$LINEARDELTA_KINEMATICS_STDOUT"
grep -Fq "lineardelta_type=4" "$LINEARDELTA_KINEMATICS_STDOUT"
grep -Fq "lineardelta_switchable=0" "$LINEARDELTA_KINEMATICS_STDOUT"
grep -Fq "lineardelta_inverse=0" "$LINEARDELTA_KINEMATICS_STDOUT"
grep -Fq "lineardelta_forward=0" "$LINEARDELTA_KINEMATICS_STDOUT"
grep -Fq "lineardelta_roundtrip_pose=1" "$LINEARDELTA_KINEMATICS_STDOUT"
ROTARYDELTA_KINEMATICS_STDOUT="$BUILD_DIR/linuxcnc_rotarydelta_kinematics_probe.run.stdout.log"
grep -Fq "rotarydelta_init=0" "$ROTARYDELTA_KINEMATICS_STDOUT"
grep -Fq "rotarydelta_type=4" "$ROTARYDELTA_KINEMATICS_STDOUT"
grep -Fq "rotarydelta_switchable=0" "$ROTARYDELTA_KINEMATICS_STDOUT"
grep -Fq "rotarydelta_inverse=0" "$ROTARYDELTA_KINEMATICS_STDOUT"
grep -Fq "rotarydelta_forward=0" "$ROTARYDELTA_KINEMATICS_STDOUT"
grep -Fq "rotarydelta_roundtrip_pose=1" "$ROTARYDELTA_KINEMATICS_STDOUT"
SCORBOT_KINEMATICS_STDOUT="$BUILD_DIR/linuxcnc_scorbot_kinematics_probe.run.stdout.log"
grep -Fq "scorbot_init=0" "$SCORBOT_KINEMATICS_STDOUT"
grep -Fq "scorbot_type=4" "$SCORBOT_KINEMATICS_STDOUT"
grep -Fq "scorbot_switchable=0" "$SCORBOT_KINEMATICS_STDOUT"
grep -Fq "scorbot_forward=0" "$SCORBOT_KINEMATICS_STDOUT"
grep -Fq "scorbot_inverse=0" "$SCORBOT_KINEMATICS_STDOUT"
grep -Fq "scorbot_roundtrip_forward=0" "$SCORBOT_KINEMATICS_STDOUT"
grep -Fq "scorbot_roundtrip_pose=1" "$SCORBOT_KINEMATICS_STDOUT"
check_fixture_output() {
local fixture="$1"
local expected="$2"

102
wasm-port/tools/build_native_probes.sh
View File

@@ -406,6 +406,24 @@ MAX_KINEMATICS_PROBE_SOURCES=(
"$WRAP_DIR/linuxcnc_max_kinematics_probe.cpp"
)
LINEARDELTA_KINEMATICS_PROBE_SOURCES=(
"$VENDOR_DIR/src/emc/kinematics/lineardeltakins.c"
"$WRAP_DIR/linuxcnc_hal_adapter.cpp"
"$WRAP_DIR/linuxcnc_lineardelta_kinematics_probe.cpp"
)
ROTARYDELTA_KINEMATICS_PROBE_SOURCES=(
"$VENDOR_DIR/src/emc/kinematics/rotarydeltakins.c"
"$WRAP_DIR/linuxcnc_hal_adapter.cpp"
"$WRAP_DIR/linuxcnc_rotarydelta_kinematics_probe.cpp"
)
SCORBOT_KINEMATICS_PROBE_SOURCES=(
"$VENDOR_DIR/src/emc/kinematics/scorbot-kins.c"
"$WRAP_DIR/linuxcnc_hal_adapter.cpp"
"$WRAP_DIR/linuxcnc_scorbot_kinematics_probe.cpp"
)
TP_CORE_SOURCES=(
"$VENDOR_DIR/src/emc/tp/tp.c"
"$VENDOR_DIR/src/emc/tp/tc.c"
@@ -635,6 +653,27 @@ build_binary_target \
MAX_KINEMATICS_PROBE_SOURCES \
NO_LINK_FLAGS
build_binary_target \
linuxcnc_lineardelta_kinematics_probe \
"$BUILD_DIR/linuxcnc_lineardelta_kinematics_probe" \
TP_FLAGS \
LINEARDELTA_KINEMATICS_PROBE_SOURCES \
NO_LINK_FLAGS
build_binary_target \
linuxcnc_rotarydelta_kinematics_probe \
"$BUILD_DIR/linuxcnc_rotarydelta_kinematics_probe" \
TP_FLAGS \
ROTARYDELTA_KINEMATICS_PROBE_SOURCES \
NO_LINK_FLAGS
build_binary_target \
linuxcnc_scorbot_kinematics_probe \
"$BUILD_DIR/linuxcnc_scorbot_kinematics_probe" \
TP_FLAGS \
SCORBOT_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" \
@@ -755,6 +794,51 @@ else
"$BUILD_DIR/linuxcnc_max_kinematics_probe.run.stderr.log"
fi
if [[ "$(tr -d '[:space:]' < "$BUILD_DIR/linuxcnc_lineardelta_kinematics_probe.exitcode")" == "0" ]]; then
set +e
"$BUILD_DIR/linuxcnc_lineardelta_kinematics_probe" \
>"$BUILD_DIR/linuxcnc_lineardelta_kinematics_probe.run.stdout.log" \
2>"$BUILD_DIR/linuxcnc_lineardelta_kinematics_probe.run.stderr.log"
LINEARDELTA_KINEMATICS_RUN_RC=$?
set -e
echo "$LINEARDELTA_KINEMATICS_RUN_RC" > "$BUILD_DIR/linuxcnc_lineardelta_kinematics_probe.run.exitcode"
else
rm -f \
"$BUILD_DIR/linuxcnc_lineardelta_kinematics_probe.run.exitcode" \
"$BUILD_DIR/linuxcnc_lineardelta_kinematics_probe.run.stdout.log" \
"$BUILD_DIR/linuxcnc_lineardelta_kinematics_probe.run.stderr.log"
fi
if [[ "$(tr -d '[:space:]' < "$BUILD_DIR/linuxcnc_rotarydelta_kinematics_probe.exitcode")" == "0" ]]; then
set +e
"$BUILD_DIR/linuxcnc_rotarydelta_kinematics_probe" \
>"$BUILD_DIR/linuxcnc_rotarydelta_kinematics_probe.run.stdout.log" \
2>"$BUILD_DIR/linuxcnc_rotarydelta_kinematics_probe.run.stderr.log"
ROTARYDELTA_KINEMATICS_RUN_RC=$?
set -e
echo "$ROTARYDELTA_KINEMATICS_RUN_RC" > "$BUILD_DIR/linuxcnc_rotarydelta_kinematics_probe.run.exitcode"
else
rm -f \
"$BUILD_DIR/linuxcnc_rotarydelta_kinematics_probe.run.exitcode" \
"$BUILD_DIR/linuxcnc_rotarydelta_kinematics_probe.run.stdout.log" \
"$BUILD_DIR/linuxcnc_rotarydelta_kinematics_probe.run.stderr.log"
fi
if [[ "$(tr -d '[:space:]' < "$BUILD_DIR/linuxcnc_scorbot_kinematics_probe.exitcode")" == "0" ]]; then
set +e
"$BUILD_DIR/linuxcnc_scorbot_kinematics_probe" \
>"$BUILD_DIR/linuxcnc_scorbot_kinematics_probe.run.stdout.log" \
2>"$BUILD_DIR/linuxcnc_scorbot_kinematics_probe.run.stderr.log"
SCORBOT_KINEMATICS_RUN_RC=$?
set -e
echo "$SCORBOT_KINEMATICS_RUN_RC" > "$BUILD_DIR/linuxcnc_scorbot_kinematics_probe.run.exitcode"
else
rm -f \
"$BUILD_DIR/linuxcnc_scorbot_kinematics_probe.run.exitcode" \
"$BUILD_DIR/linuxcnc_scorbot_kinematics_probe.run.stdout.log" \
"$BUILD_DIR/linuxcnc_scorbot_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" \
@@ -925,6 +1009,24 @@ build_object_target \
"$VENDOR_DIR/src/emc/kinematics/maxkins.c" \
TP_FLAGS
build_object_target \
linuxcnc_lineardeltakins_source_probe \
"$BUILD_DIR/linuxcnc_lineardeltakins_source_probe.o" \
"$VENDOR_DIR/src/emc/kinematics/lineardeltakins.c" \
TP_FLAGS
build_object_target \
linuxcnc_rotarydeltakins_source_probe \
"$BUILD_DIR/linuxcnc_rotarydeltakins_source_probe.o" \
"$VENDOR_DIR/src/emc/kinematics/rotarydeltakins.c" \
TP_FLAGS
build_object_target \
linuxcnc_scorbot_kins_source_probe \
"$BUILD_DIR/linuxcnc_scorbot_kins_source_probe.o" \
"$VENDOR_DIR/src/emc/kinematics/scorbot-kins.c" \
TP_FLAGS
build_object_target \
linuxcnc_interp_convert_source_probe \
"$BUILD_DIR/linuxcnc_interp_convert_source_probe.o" \

5
wasm-port/tools/source-manifest.txt
View File

@@ -44,6 +44,11 @@ src/emc/kinematics/corexykins.c
src/emc/kinematics/rotatekins.c
src/emc/kinematics/rosekins.c
src/emc/kinematics/maxkins.c
src/emc/kinematics/lineardeltakins-common.h
src/emc/kinematics/lineardeltakins.c
src/emc/kinematics/rotarydeltakins-common.h
src/emc/kinematics/rotarydeltakins.c
src/emc/kinematics/scorbot-kins.c
src/emc/tp/tp.h
src/emc/tp/tp_types.h
src/emc/tp/tc.h

140
wasm-port/vendor/linuxcnc/src/emc/kinematics/lineardeltakins-common.h vendored Normal file
View File

@@ -0,0 +1,140 @@
#ifndef LINUXCNCLINEARDELTAKINS_COMMON_H
#define LINUXCNCLINEARDELTAKINS_COMMON_H
// Copyright 2013 Jeff Epler <jepler@unpythonic.net>
//
// This program is free software; you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation; either version 2 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program; if not, write to the Free Software
// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
/*
* Kinematics for a rostock-style delta robot
*
* Towers 0, 1 and 2 are spaced at 120 degrees around the origin
* at distance R. A rod of length L (L > R) connects each tower to the
* platform. Tower 0 is at (0,R). (note: this is not at zero radians!)
*
* ABCUVW coordinates are passed through in joints[3..8].
*
* L is like DELTA_DIAGONAL_ROD and R is like DELTA_RADIUS in
* Marlin---remember to account for the effector and carriage offsets
* when changing from the default.
*/
// common routines used by the userspace kinematics and the realtime kinematics
// user must include a math.h-type header first
// Inspired by Marlin delta firmware and https://gist.github.com/kastner/5279172
#include <emcpos.h>
static double L, R;
static double Ax, Ay, Bx, By, Cx, Cy, L2;
#define SQ3 (sqrt(3))
#define SIN_60 (SQ3/2)
#define COS_60 (.5)
static double sq(double x) { return x*x; }
static void set_geometry(double r_, double l_)
{
if(L == l_ && R == r_) return;
L = l_;
R = r_;
L2 = sq(L);
Ax = 0.0;
Ay = R;
Bx = -SIN_60 * R;
By = -COS_60 * R;
Cx = SIN_60 * R;
Cy = -COS_60 * R;
}
static int kinematics_inverse(const EmcPose *pos, double *joints)
{
double x = pos->tran.x, y = pos->tran.y, z = pos->tran.z;
joints[0] = z + sqrt(L2 - sq(Ax-x) - sq(Ay-y));
joints[1] = z + sqrt(L2 - sq(Bx-x) - sq(By-y));
joints[2] = z + sqrt(L2 - sq(Cx-x) - sq(Cy-y));
joints[3] = pos->a;
joints[4] = pos->b;
joints[5] = pos->c;
joints[6] = pos->u;
joints[7] = pos->v;
joints[8] = pos->w;
return isnan(joints[0]) || isnan(joints[1]) || isnan(joints[2])
? -1 : 0;
}
static int kinematics_forward(const double *joints, EmcPose *pos)
{
double q1 = joints[0];
double q2 = joints[1];
double q3 = joints[2];
double den = (By-Ay)*Cx-(Cy-Ay)*Bx;
double w1 = Ay*Ay + q1*q1; // n.b. assumption that Ax is 0 all through here
double w2 = Bx*Bx + By*By + q2*q2;
double w3 = Cx*Cx + Cy*Cy + q3*q3;
double a1 = (q2-q1)*(Cy-Ay)-(q3-q1)*(By-Ay);
double b1 = -((w2-w1)*(Cy-Ay)-(w3-w1)*(By-Ay))/2.0;
double a2 = -(q2-q1)*Cx+(q3-q1)*Bx;
double b2 = ((w2-w1)*Cx - (w3-w1)*Bx)/2.0;
// a*z^2 + b*z + c = 0
double a = a1*a1 + a2*a2 + den*den;
double b = 2*(a1*b1 + a2*(b2-Ay*den) - q1*den*den);
double c = (b2-Ay*den)*(b2-Ay*den) + b1*b1 + den*den*(q1*q1 - L*L);
double discr = b*b - 4.0*a*c;
if (discr < 0) return -1; // non-existing point
double z = -0.5*(b+sqrt(discr))/a;
pos->tran.z = z;
pos->tran.x = (a1*z + b1)/den;
pos->tran.y = (a2*z + b2)/den;
pos->a = joints[3];
pos->b = joints[4];
pos->c = joints[5];
pos->u = joints[6];
pos->v = joints[7];
pos->w = joints[8];
return 0;
}
// Default values which may correspond to someone's linear delta robot. To
// change these, use halcmd setp rather than rebuilding the software.
// Center-to-center distance of the holes in the diagonal push rods.
#define DELTA_DIAGONAL_ROD 269.0 // mm
// Horizontal offset from middle of printer to smooth rod center.
#define DELTA_SMOOTH_ROD_OFFSET 198.25 // mm
// Horizontal offset of the universal joints on the end effector.
#define DELTA_EFFECTOR_OFFSET 33.0 // mm
// Horizontal offset of the universal joints on the carriages.
#define DELTA_CARRIAGE_OFFSET 35.0 // mm
// Effective horizontal distance bridged by diagonal push rods.
#define DELTA_RADIUS (DELTA_SMOOTH_ROD_OFFSET-DELTA_EFFECTOR_OFFSET-DELTA_CARRIAGE_OFFSET)
#endif

98
wasm-port/vendor/linuxcnc/src/emc/kinematics/lineardeltakins.c vendored Normal file
View File

@@ -0,0 +1,98 @@
// Copyright 2013 Jeff Epler <jepler@unpythonic.net>
//
// This program is free software; you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation; either version 2 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program; if not, write to the Free Software
// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
#include <rtapi_math.h>
#include <rtapi_app.h>
#include <hal.h>
#include <kinematics.h>
#include "lineardeltakins-common.h"
struct haldata
{
hal_float_t *r, *l;
} *haldata;
int comp_id;
int kinematicsForward(const double * joints,
EmcPose * pos,
const KINEMATICS_FORWARD_FLAGS * fflags,
KINEMATICS_INVERSE_FLAGS * iflags) {
(void)fflags;
(void)iflags;
set_geometry(*haldata->r, *haldata->l);
return kinematics_forward(joints, pos);
}
int kinematicsInverse(const EmcPose *pos, double *joints,
const KINEMATICS_INVERSE_FLAGS *iflags,
KINEMATICS_FORWARD_FLAGS *fflags) {
(void)iflags;
(void)fflags;
set_geometry(*haldata->r, *haldata->l);
return kinematics_inverse(pos, joints);
}
KINEMATICS_TYPE kinematicsType()
{
return KINEMATICS_BOTH;
}
int rtapi_app_main(void)
{
int retval = 0;
comp_id = hal_init("lineardeltakins");
if(comp_id < 0) retval = comp_id;
if(retval == 0)
{
haldata = hal_malloc(sizeof(struct haldata));
retval = !haldata;
}
if(retval == 0)
retval = hal_pin_float_newf(HAL_IN, &haldata->r, comp_id,
"lineardeltakins.R");
if(retval == 0)
retval = hal_pin_float_newf(HAL_IN, &haldata->l, comp_id,
"lineardeltakins.L");
if(retval == 0)
{
*haldata->r = DELTA_RADIUS;
*haldata->l = DELTA_DIAGONAL_ROD;
}
if(retval == 0)
{
hal_ready(comp_id);
}
return retval;
}
void rtapi_app_exit(void)
{
hal_exit(comp_id);
}
KINS_NOT_SWITCHABLE
EXPORT_SYMBOL(kinematicsType);
EXPORT_SYMBOL(kinematicsForward);
EXPORT_SYMBOL(kinematicsInverse);
MODULE_LICENSE("GPL");

195
wasm-port/vendor/linuxcnc/src/emc/kinematics/rotarydeltakins-common.h vendored Normal file
View File

@@ -0,0 +1,195 @@
// Copyright 2013 Chris Radek <chris@timeguy.com>
//
// This program is free software; you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation; either version 2 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program; if not, write to the Free Software
// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
/*
Based on work by "mzavatsky" at:
http://forums.trossenrobotics.com/tutorials/introduction-129/delta-robot-kinematics-3276/
"You can freely use this code in your applications."
which was based on:
Descriptive Geometric Kinematic Analysis of Clavel's "Delta" Robot
P.J. Zsombor-Murray, McGill University
"... the purpose of this article: to provide a clear kinematic
analysis useful to those who may wish to program and employ nice
little three legged robots ..."
The platform is on "top", the origin is in the center of the plane
containing the three hip joints. Z points upward, so Z coordinates
are always negative. Thighs always point outward, straight out
(knee at Z=0) is considered zero degrees for the angular hip joint.
Positive rotation is knee-downward, so if you rotate all knees
positive, the Z coordinate will get more negative.
Joint zero is the one whose thigh swings in the YZ plane.
*/
#ifndef LINUXCNCROTARYDELTAKINS_COMMON_H
#define LINUXCNCROTARYDELTAKINS_COMMON_H
#include <emcpos.h>
// distance from origin to a hip joint
static double platformradius;
// thigh connects the hip to the knee
static double thighlength;
// shin (the parallelogram) connects the knee to the foot
static double shinlength;
// distance from center of foot (controlled point) to an ankle joint
static double footradius;
#ifndef sq
#define sq(a) ((a)*(a))
#endif
#ifndef D2R
#define D2R(d) ((d)*M_PI/180.)
#endif
static void set_geometry(double pfr, double tl, double sl, double fr) {
platformradius = pfr;
thighlength = tl;
shinlength = sl;
footradius = fr;
}
// Given three hip joint angles, find the controlled point
static int kinematics_forward(const double *joints, EmcPose *pos) {
double
j0 = joints[0],
j1 = joints[1],
j2 = joints[2],
y1, z1, // x1 is 0
x2, y2, z2, x3, y3, z3,
a1, b1, a2, b2,
w1, w2, w3,
denom,
a, b, c, d;
j0 = D2R(j0);
j1 = D2R(j1);
j2 = D2R(j2);
y1 = -(platformradius - footradius + thighlength * cos(j0));
z1 = -thighlength * sin(j0);
y2 = (platformradius - footradius + thighlength * cos(j1)) * 0.5;
x2 = y2 * sqrt(3);
z2 = -thighlength * sin(j1);
y3 = (platformradius - footradius + thighlength * cos(j2)) * 0.5;
x3 = -y3 * sqrt(3);
z3 = -thighlength * sin(j2);
denom = x3 * (y2 - y1) - x2 * (y3 - y1);
w1 = sq(y1) + sq(z1);
w2 = sq(x2) + sq(y2) + sq(z2);
w3 = sq(x3) + sq(y3) + sq(z3);
a1 = (z2-z1) * (y3-y1) - (z3-z1) * (y2-y1);
b1 = -((w2-w1) * (y3-y1) - (w3-w1) * (y2-y1)) / 2.0;
a2 = -(z2 - z1) * x3 + (z3 - z1) * x2;
b2 = ((w2 - w1) * x3 - (w3 - w1) * x2) / 2.0;
// a*z^2 + b*z + c = 0
a = sq(a1) + sq(a2) + sq(denom);
b = 2 * (a1 * b1 + a2 * (b2 - y1 * denom) - z1 * sq(denom));
c = (b2 - y1 * denom) * (b2 - y1 * denom) +
sq(b1) + sq(denom) * (sq(z1) - sq(shinlength));
d = sq(b) - 4 * a * c;
if (d < 0) return -1;
pos->tran.z = (-b - sqrt(d)) / (2 * a);
pos->tran.x = (a1 * pos->tran.z + b1) / denom;
pos->tran.y = (a2 * pos->tran.z + b2) / denom;
pos->a = joints[3];
pos->b = joints[4];
pos->c = joints[5];
pos->u = joints[6];
pos->v = joints[7];
pos->w = joints[8];
return 0;
}
// Given controlled point, find joint zero's angle
// (J0 is the easy one in the ZY plane)
static int inverse_j0(double x, double y, double z, double *theta) {
double a, b, d, knee_y, knee_z;
a = 0.5 * (sq(x) + sq(y - footradius) + sq(z) + sq(thighlength) -
sq(shinlength) - sq(platformradius)) / z;
b = (footradius - platformradius - y) / z;
d = sq(thighlength) * (sq(b) + 1) - sq(a - b * platformradius);
if (d < 0) return -1;
knee_y = (platformradius + a*b + sqrt(d)) / (sq(b) + 1);
knee_z = b * knee_y - a;
*theta = atan2(knee_z, knee_y - platformradius);
*theta *= 180.0/M_PI;
return 0;
}
static void rotate(double *x, double *y, double theta) {
double xx, yy;
xx = *x, yy = *y;
*x = xx * cos(theta) - yy * sin(theta);
*y = xx * sin(theta) + yy * cos(theta);
}
static int kinematics_inverse(const EmcPose *pos, double *joints) {
double xr, yr;
if(inverse_j0(pos->tran.x, pos->tran.y, pos->tran.z, &joints[0])) return -1;
// now use symmetry property to get the other two just as easily...
xr = pos->tran.x; yr = pos->tran.y;
rotate(&xr, &yr, -2*M_PI/3);
if(inverse_j0(xr, yr, pos->tran.z, &joints[1])) return -1;
xr = pos->tran.x; yr = pos->tran.y;
rotate(&xr, &yr, 2*M_PI/3);
if(inverse_j0(xr, yr, pos->tran.z, &joints[2])) return -1;
joints[3] = pos->a;
joints[4] = pos->b;
joints[5] = pos->c;
joints[6] = pos->u;
joints[7] = pos->v;
joints[8] = pos->w;
return 0;
}
#define RDELTA_PFR 10.0
#define RDELTA_TL 10.0
#define RDELTA_SL 14.0
#define RDELTA_FR 6.0
#endif

110
wasm-port/vendor/linuxcnc/src/emc/kinematics/rotarydeltakins.c vendored Normal file
View File

@@ -0,0 +1,110 @@
// Copyright 2013 Chris Radek <chris@timeguy.com>
//
// This program is free software; you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation; either version 2 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program; if not, write to the Free Software
// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
#include <rtapi_math.h>
#include <rtapi_app.h>
#include <hal.h>
#include <kinematics.h>
#include "rotarydeltakins-common.h"
struct haldata
{
hal_float_t *pfr;
hal_float_t *tl;
hal_float_t *sl;
hal_float_t *fr;
} *haldata;
int comp_id;
int kinematicsForward(const double * joints,
EmcPose * pos,
const KINEMATICS_FORWARD_FLAGS * fflags,
KINEMATICS_INVERSE_FLAGS * iflags) {
(void)fflags;
(void)iflags;
set_geometry(*haldata->pfr, *haldata->tl, *haldata->sl, *haldata->fr);
return kinematics_forward(joints, pos);
}
int kinematicsInverse(const EmcPose *pos, double *joints,
const KINEMATICS_INVERSE_FLAGS *iflags,
KINEMATICS_FORWARD_FLAGS *fflags) {
(void)iflags;
(void)fflags;
set_geometry(*haldata->pfr, *haldata->tl, *haldata->sl, *haldata->fr);
return kinematics_inverse(pos, joints);
}
KINEMATICS_TYPE kinematicsType()
{
return KINEMATICS_BOTH;
}
int rtapi_app_main(void)
{
int retval = 0;
comp_id = hal_init("rotarydeltakins");
if(comp_id < 0) retval = comp_id;
if(retval == 0)
{
haldata = hal_malloc(sizeof(struct haldata));
retval = !haldata;
}
if(retval == 0)
retval = hal_pin_float_newf(HAL_IN, &haldata->pfr, comp_id,
"rotarydeltakins.platformradius");
if(retval == 0)
retval = hal_pin_float_newf(HAL_IN, &haldata->tl, comp_id,
"rotarydeltakins.thighlength");
if(retval == 0)
retval = hal_pin_float_newf(HAL_IN, &haldata->sl, comp_id,
"rotarydeltakins.shinlength");
if(retval == 0)
retval = hal_pin_float_newf(HAL_IN, &haldata->fr, comp_id,
"rotarydeltakins.footradius");
if(retval == 0)
{
*haldata->pfr = RDELTA_PFR;
*haldata->tl = RDELTA_TL;
*haldata->sl = RDELTA_SL;
*haldata->fr = RDELTA_FR;
}
if(retval == 0)
{
hal_ready(comp_id);
}
return retval;
}
void rtapi_app_exit(void)
{
hal_exit(comp_id);
}
KINS_NOT_SWITCHABLE
EXPORT_SYMBOL(kinematicsType);
EXPORT_SYMBOL(kinematicsForward);
EXPORT_SYMBOL(kinematicsInverse);
MODULE_LICENSE("GPL");

322
wasm-port/vendor/linuxcnc/src/emc/kinematics/scorbot-kins.c vendored Normal file
View File

@@ -0,0 +1,322 @@
//
// This is a kinematics module for the Scorbot ER 3.
//
// Copyright (C) 2015-2016 Sebastian Kuzminsky <seb@highlab.com>
//
// This program is free software; you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation; either version 2 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
//
//
// The origin of the G53 coordinate system is at the center of rotation of
// joint J0, and at the bottom of the base plate.
//
// FIXME: The origin should probably be at the bottom of the base (part 5
// in the parts diagram on page 7-11 of the SCORBOT-ER III User's Manual).
//
// Joint 0 is rotation around the Z axis. It chooses the plane that
// the rest of the arm moves in.
//
// Joint 1 is the shoulder.
//
// Joint 2 is the elbow.
//
// Joint 3 is pitch of the wrist, joint 4 is roll of the wrist. These are
// converted to motor actuations by an external differential comp in HAL.
//
#include <rtapi.h>
#include <rtapi_app.h>
#include <rtapi_math.h>
#include <hal.h>
#include <gotypes.h>
#include <kinematics.h>
//
// linkage constants, in mm & degrees
//
// Link 0 connects the origin to J1 (shoulder)
// These dimensions come off a drawing I got from Intelitek.
#define L0_HORIZONTAL_DISTANCE 16
#define L0_VERTICAL_DISTANCE 140
#define L1_LENGTH 221 // Link 1 connects J1 (shoulder) to J2 (elbow)
#define L2_LENGTH 221 // Link 2 connects J2 (shoulder) to the wrist
// Compute the cartesian coordinates of J1, given the J0 angle (and the
// fixed, known link L0 between J0 and J1).
static void compute_j1_cartesian_location(double j0, EmcPose *j1_cart) {
j1_cart->tran.x = L0_HORIZONTAL_DISTANCE * cos(TO_RAD * j0);
j1_cart->tran.y = L0_HORIZONTAL_DISTANCE * sin(TO_RAD * j0);
j1_cart->tran.z = L0_VERTICAL_DISTANCE;
j1_cart->a = 0;
j1_cart->b = 0;
j1_cart->c = 0;
j1_cart->u = 0;
j1_cart->v = 0;
j1_cart->w = 0;
}
// Forward kinematics takes the joint positions and computes the cartesian
// coordinates of the controlled point.
int kinematicsForward(
const double *joints,
EmcPose *pose,
const KINEMATICS_FORWARD_FLAGS *fflags,
KINEMATICS_INVERSE_FLAGS *iflags
) {
(void)fflags;
(void)iflags;
EmcPose j1_vector; // the vector from j0 ("base") to joint 1 ("shoulder", end of link 0)
EmcPose j2_vector; // the vector from j1 ("shoulder") to joint 2 ("elbow", end of link 1)
EmcPose j3_vector; // the vector from j2 ("elbow") to joint 3 ("wrist", end of link 2)
double r;
// rtapi_print("fwd: j0=%f, j1=%f, j2=%f\n", joints[0], joints[1], joints[2]);
compute_j1_cartesian_location(joints[0], &j1_vector);
// rtapi_print("fwd: j1=(%f, %f, %f)\n", j1_vector.tran.x, j1_vector.tran.y, j1_vector.tran.z);
// Link 1 connects j1 (shoulder) to j2 (elbow).
r = L1_LENGTH * cos(TO_RAD * joints[1]);
j2_vector.tran.x = r * cos(TO_RAD * joints[0]);
j2_vector.tran.y = r * sin(TO_RAD * joints[0]);
j2_vector.tran.z = L1_LENGTH * sin(TO_RAD * joints[1]);
// rtapi_print("fwd: j2=(%f, %f, %f)\n", j2_vector.tran.x, j2_vector.tran.y, j2_vector.tran.z);
// Link 2 connects j2 (elbow) to j3 (wrist).
// J3 is the controlled point.
r = L2_LENGTH * cos(TO_RAD * joints[2]);
j3_vector.tran.x = r * cos(TO_RAD * joints[0]);
j3_vector.tran.y = r * sin(TO_RAD * joints[0]);
j3_vector.tran.z = L2_LENGTH * sin(TO_RAD * joints[2]);
// rtapi_print("fwd: j3=(%f, %f, %f)\n", j3_vector.tran.x, j3_vector.tran.y, j3_vector.tran.z);
// The end-effector location is the sum of the linkage vectors.
pose->tran.x = j1_vector.tran.x + j2_vector.tran.x + j3_vector.tran.x;
pose->tran.y = j1_vector.tran.y + j2_vector.tran.y + j3_vector.tran.y;
pose->tran.z = j1_vector.tran.z + j2_vector.tran.z + j3_vector.tran.z;
// rtapi_print("fwd: pose=(%f, %f, %f)\n", pose->tran.x, pose->tran.y, pose->tran.z);
// A and B are wrist roll and pitch, handled in hal by external kinematics
pose->a = joints[3];
pose->b = joints[4];
return 0;
}
//
// Inverse kinematics takes the cartesian coordinates of the controlled
// point and computes corresponding the joint positions.
//
// Joint 0 rotates the arm around the base. The rest of the joints are
// confined to the vertical plane containing J0, and rotated around the
// vertical at J0. This kinematics code calls this plane the "RZ" plane.
// The Z coordinate in this plane is the same as the Z coordinate in the
// "cartesian" coordinates of LinuxCNC's world space. The R coordinate
// is the horizontal distance (ie, in the XY plane) of the controlled
// point from J0.
//
int kinematicsInverse(
const EmcPose *pose,
double *joints,
const KINEMATICS_INVERSE_FLAGS *iflags,
KINEMATICS_FORWARD_FLAGS *fflags
) {
(void)iflags;
(void)fflags;
// EmcPose j1_cart;
double distance_to_cp, distance_to_center;
double r_j1, z_j1; // (r_j1, z_j1) is the location of J1 in the RZ plane
double r_cp, z_cp; // (r_cp, z_cp) is the location of the controlled point in the RZ plane
double angle_to_cp;
double j1_angle;
// the location of J2, this is what we're trying to find
double z_j2;
// rtapi_print("inv: x=%f, y=%f, z=%f\n", pose->tran.x, pose->tran.y, pose->tran.z);
// J0 is easy. Project the (X, Y, Z) of the pose onto the Z=0 plane.
// J0 points at the projected (X, Y) point. tan(J0) = Y/X
// J0 then defines the plane that the rest of the arm operates in.
joints[0] = TO_DEG * atan2(pose->tran.y, pose->tran.x);
// rtapi_print("inv: j0=%f\n", joints[0]);
// compute_j1_cartesian_location(joints[0], &j1_cart);
// rtapi_print("inv: j1=(X=%f, Y=%f, Z=%f)\n", j1_cart.tran.x, j1_cart.tran.y, j1_cart.tran.z);
// FIXME: Until i figure the wrist differential out, the controlled
// point will be the location of the wrist joint, J3/J4.
// The location of J1 (computed above) and the location of the
// controlled point are separated by J1, L1, J2, and L2. L1 and L2 are
// known, but J1 and J2 are not.
// (r_j1, z_j1) is the location of J1 in the RZ plane (the vertical
// plane defined by the angle of J0, with the origin at the location
// of J0. This is just a known, static vector.
r_j1 = L0_HORIZONTAL_DISTANCE;
z_j1 = L0_VERTICAL_DISTANCE;
// rtapi_print("inv: r_j1=%f, z_j1=%f\n", r_j1, z_j1);
// (r_cp, z_cp) is the location of J3 (the controlled point), again in
// the plane defined by the angle of J0, with the origin of the
// machine.
r_cp = sqrt(pow(pose->tran.x, 2) + pow(pose->tran.y, 2));
z_cp = pose->tran.z;
// rtapi_print("inv: r_cp=%f, z_cp=%f (controlled point)\n", r_cp, z_cp);
// translate so (r_j1, z_j1) is the origin of the coordinate system
r_cp -= r_j1;
z_cp -= z_j1;
// rtapi_print("inv: r_cp=%f, z_cp=%f (translated controlled point)\n", r_cp, z_cp);
//
// Now the origin (aka J1), J2, and CP define a triangle in the RZ plane.
// The triangle is isosceles, because from the origin to J2 is L1, and
// from J2 to CP is L2, and L1 and L2 are the same length.
//
// Bisect the base of that triangle, and call the center point of the
// base "Center".
//
// Draw a line between J2 and Center. This defines two right
// triangles: (J1, J2, Center) and (CP, J2, Center).
//
// The length of the (J1, Center) and (CP, Center) lines are equal, and
// are half the distance from the origin to CP.
//
distance_to_cp = sqrt(pow(r_cp, 2) + pow(z_cp, 2));
distance_to_center = distance_to_cp / 2;
// rtapi_print("inv: distance to cp: %f\n", distance_to_cp);
// find the angle of the vector from the origin to the CP
angle_to_cp = TO_DEG * acos(r_cp / distance_to_cp);
if (z_cp < 0) {
angle_to_cp *= -1;
}
// rtapi_print("inv: angle to cp: %f\n", angle_to_cp);
// find the angle (Center, J1, J2)
j1_angle = TO_DEG * acos(distance_to_center / L1_LENGTH);
// rtapi_print("inv: j1 angle: %f\n", j1_angle);
joints[1] = angle_to_cp + j1_angle;
// rtapi_print("inv: j1: %f\n", joints[1]);
// now we can compute the location of J2
z_j2 = L1_LENGTH * sin(TO_RAD * joints[1]);
// rtapi_print("inv: r_j2=%f, z_j2=%f (translated j2)\n", r_j2, z_j2);
joints[2] = -1.0 * TO_DEG * asin((z_j2 - z_cp) / L2_LENGTH);
#if 0
// Distance between controlled point and the location of j1. These two
// points are separated by link 1, joint 1, and link 2.
distance_between_centers = sqrt(pow((r2 - r1), 2) + pow((z2 - z1), 2));
if (distance_between_centers > (L1_LENGTH + L2_LENGTH)) {
// trying to reach too far
return GO_RESULT_RANGE_ERROR;
}
if (distance_between_centers < fabs(L1_LENGTH - L2_LENGTH)) {
// trying to reach too far into armpit
return GO_RESULT_RANGE_ERROR;
}
delta = (1.0 / 4.0) * sqrt((distance_between_centers + L1_LENGTH + L2_LENGTH) * (distance_between_centers + L1_LENGTH - L2_LENGTH) * (distance_between_centers - L1_LENGTH + L2_LENGTH) * (L1_LENGTH + L2_LENGTH - distance_between_centers));
ir1 = ((r1 + r2) / 2) + (((r2 - r1) * (pow(L1_LENGTH, 2) - pow(L2_LENGTH, 2)))/(2 * pow(distance_between_centers, 2))) + ((2 * (z1 - z2) * delta) / pow(distance_between_centers, 2));
ir2 = ((r1 + r2) / 2) + (((r2 - r1) * (pow(L1_LENGTH, 2) - pow(L2_LENGTH, 2)))/(2 * pow(distance_between_centers, 2))) - ((2 * (z1 - z2) * delta) / pow(distance_between_centers, 2));
iz1 = ((z1 + z2) / 2) + (((z2 - z1) * (pow(L1_LENGTH, 2) - pow(L2_LENGTH, 2)))/(2 * pow(distance_between_centers, 2))) - ((2 * (r1 - r2) * delta) / pow(distance_between_centers, 2));
iz2 = ((z1 + z2) / 2) + (((z2 - z1) * (pow(L1_LENGTH, 2) - pow(L2_LENGTH, 2)))/(2 * pow(distance_between_centers, 2))) + ((2 * (r1 - r2) * delta) / pow(distance_between_centers, 2));
// (ir1, iz1) is one intersection point, (ir2, iz2) is the other.
// These are the possible locations of the J2 joint.
// FIXME: For now we arbitrarily pick the one with the bigger Z.
if (iz1 > iz2) {
j2_r = ir1;
j2_z = iz1;
} else {
j2_r = ir2;
j2_z = iz2;
}
// rtapi_print("inv: j2_r=%f, j2_z=%f (J2, intersection point)\n", j2_r, j2_z);
// Make J1 point at J2 (j2_r, j2_z).
{
double l1_r = j2_r - r1;
joints[1] = TO_DEG * acos(l1_r / L1_LENGTH);
// rtapi_print("inv: l1_r=%f, j1=%f\n", l1_r, joints[1]);
}
// Make J2 point at the controlled point.
{
double l2_r = r2 - j2_r;
double j2;
j2 = TO_DEG * acos(l2_r / L2_LENGTH);
if (j2_z > pose->tran.z) {
j2 *= -1;
}
joints[2] = j2;
// rtapi_print("inv: l2_r=%f, j2=%f\n", l2_r, joints[2]);
}
#endif
// A and B are wrist roll and pitch, handled in hal by external kinematics
joints[3] = pose->a;
joints[4] = pose->b;
return 0;
}
KINEMATICS_TYPE kinematicsType(void) {
return KINEMATICS_BOTH;
}
KINS_NOT_SWITCHABLE
EXPORT_SYMBOL(kinematicsType);
EXPORT_SYMBOL(kinematicsForward);
EXPORT_SYMBOL(kinematicsInverse);
MODULE_LICENSE("GPL");
static int comp_id;
int rtapi_app_main(void) {
comp_id = hal_init("scorbot-kins");
if (comp_id < 0) {
return comp_id;
}
hal_ready(comp_id);
return 0;
}
void rtapi_app_exit(void) {
hal_exit(comp_id);
}