按推荐建议,继续执行
结论:继续按 LinuxCNC 源码直接复用路线推进,新增 tripod 运动学源码的 vendored 同步、native 探针、source probe 覆盖和复用文档,并通过 native 验证。
This commit is contained in:
@@ -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` |
|
| 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` |
|
| 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` |
|
| 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` | 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` |
|
| 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` |
|
||||||
| 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` |
|
| 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 |
|
| 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 |
|
| 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,8 @@ Current validation is intentionally mechanical:
|
|||||||
## Known Gaps
|
## Known Gaps
|
||||||
|
|
||||||
- Additional non-trivial kinematics implementation files, including serial,
|
- Additional non-trivial kinematics implementation files, including serial,
|
||||||
SCARA, hexapod, tripod, puma, and other machine-specific modules, are not
|
SCARA, hexapod, puma, and other machine-specific modules, are not yet
|
||||||
yet extracted.
|
extracted.
|
||||||
- Browser/WASM C ABI and JS SDK layers are not yet built for the full
|
- Browser/WASM C ABI and JS SDK layers are not yet built for the full
|
||||||
interpreter/planner core.
|
interpreter/planner core.
|
||||||
- OPFS persistence is not yet connected to INI, tool table, parameter file, or
|
- OPFS persistence is not yet connected to INI, tool table, parameter file, or
|
||||||
|
|||||||
@@ -0,0 +1,78 @@
|
|||||||
|
#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_xyz(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);
|
||||||
|
}
|
||||||
|
|
||||||
|
void print_pose(const char *prefix, const EmcPose &pose)
|
||||||
|
{
|
||||||
|
std::cout << prefix << "_xyz="
|
||||||
|
<< pose.tran.x << ","
|
||||||
|
<< pose.tran.y << ","
|
||||||
|
<< pose.tran.z << "\n";
|
||||||
|
}
|
||||||
|
|
||||||
|
void print_joints(const char *prefix, const double *joints)
|
||||||
|
{
|
||||||
|
std::cout << prefix << "_abc="
|
||||||
|
<< joints[0] << ","
|
||||||
|
<< joints[1] << ","
|
||||||
|
<< joints[2] << "\n";
|
||||||
|
}
|
||||||
|
|
||||||
|
} // namespace
|
||||||
|
|
||||||
|
int main()
|
||||||
|
{
|
||||||
|
const int init_rc = rtapi_app_main();
|
||||||
|
std::cout << "tripod_init=" << init_rc << "\n";
|
||||||
|
std::cout << "tripod_type=" << kinematicsType() << "\n";
|
||||||
|
std::cout << "tripod_switchable=" << kinematicsSwitchable() << "\n";
|
||||||
|
|
||||||
|
EmcPose pose{};
|
||||||
|
pose.tran.x = 0.25;
|
||||||
|
pose.tran.y = 0.25;
|
||||||
|
pose.tran.z = 0.5;
|
||||||
|
|
||||||
|
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 << "tripod_inverse=" << inverse_rc << "\n";
|
||||||
|
std::cout << "tripod_inverse_flags=" << fflags << "\n";
|
||||||
|
print_joints("tripod_inverse", inverse_joints);
|
||||||
|
|
||||||
|
EmcPose forward_pose{};
|
||||||
|
const int forward_rc = kinematicsForward(inverse_joints, &forward_pose, &fflags, &iflags);
|
||||||
|
std::cout << "tripod_forward=" << forward_rc << "\n";
|
||||||
|
print_pose("tripod_forward", forward_pose);
|
||||||
|
std::cout << "tripod_roundtrip_xyz=" << near_xyz(forward_pose, pose) << "\n";
|
||||||
|
|
||||||
|
pose.tran.z = -0.5;
|
||||||
|
const int inverse_below_rc = kinematicsInverse(&pose, inverse_joints, &iflags, &fflags);
|
||||||
|
std::cout << "tripod_inverse_below=" << inverse_below_rc << "\n";
|
||||||
|
std::cout << "tripod_inverse_below_flags=" << fflags << "\n";
|
||||||
|
const int forward_below_rc = kinematicsForward(inverse_joints, &forward_pose, &fflags, &iflags);
|
||||||
|
std::cout << "tripod_forward_below=" << forward_below_rc << "\n";
|
||||||
|
print_pose("tripod_forward_below", forward_pose);
|
||||||
|
std::cout << "tripod_roundtrip_below_xyz=" << near_xyz(forward_pose, pose) << "\n";
|
||||||
|
|
||||||
|
rtapi_app_exit();
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
@@ -101,6 +101,8 @@ check_exitcode linuxcnc_rotarydelta_kinematics_probe
|
|||||||
check_exitcode linuxcnc_rotarydelta_kinematics_probe.run
|
check_exitcode linuxcnc_rotarydelta_kinematics_probe.run
|
||||||
check_exitcode linuxcnc_scorbot_kinematics_probe
|
check_exitcode linuxcnc_scorbot_kinematics_probe
|
||||||
check_exitcode linuxcnc_scorbot_kinematics_probe.run
|
check_exitcode linuxcnc_scorbot_kinematics_probe.run
|
||||||
|
check_exitcode linuxcnc_tripod_kinematics_probe
|
||||||
|
check_exitcode linuxcnc_tripod_kinematics_probe.run
|
||||||
for name in \
|
for name in \
|
||||||
linuxcnc_tp_tp_source_probe \
|
linuxcnc_tp_tp_source_probe \
|
||||||
linuxcnc_tp_tc_source_probe \
|
linuxcnc_tp_tc_source_probe \
|
||||||
@@ -156,6 +158,7 @@ check_exitcode linuxcnc_maxkins_source_probe
|
|||||||
check_exitcode linuxcnc_lineardeltakins_source_probe
|
check_exitcode linuxcnc_lineardeltakins_source_probe
|
||||||
check_exitcode linuxcnc_rotarydeltakins_source_probe
|
check_exitcode linuxcnc_rotarydeltakins_source_probe
|
||||||
check_exitcode linuxcnc_scorbot_kins_source_probe
|
check_exitcode linuxcnc_scorbot_kins_source_probe
|
||||||
|
check_exitcode linuxcnc_tripodkins_source_probe
|
||||||
check_exitcode linuxcnc_interp_convert_source_probe
|
check_exitcode linuxcnc_interp_convert_source_probe
|
||||||
check_exitcode linuxcnc_interp_read_source_probe
|
check_exitcode linuxcnc_interp_read_source_probe
|
||||||
check_exitcode linuxcnc_interp_check_source_probe
|
check_exitcode linuxcnc_interp_check_source_probe
|
||||||
@@ -341,6 +344,19 @@ grep -Fq "scorbot_inverse=0" "$SCORBOT_KINEMATICS_STDOUT"
|
|||||||
grep -Fq "scorbot_roundtrip_forward=0" "$SCORBOT_KINEMATICS_STDOUT"
|
grep -Fq "scorbot_roundtrip_forward=0" "$SCORBOT_KINEMATICS_STDOUT"
|
||||||
grep -Fq "scorbot_roundtrip_pose=1" "$SCORBOT_KINEMATICS_STDOUT"
|
grep -Fq "scorbot_roundtrip_pose=1" "$SCORBOT_KINEMATICS_STDOUT"
|
||||||
|
|
||||||
|
TRIPOD_KINEMATICS_STDOUT="$BUILD_DIR/linuxcnc_tripod_kinematics_probe.run.stdout.log"
|
||||||
|
grep -Fq "tripod_init=0" "$TRIPOD_KINEMATICS_STDOUT"
|
||||||
|
grep -Fq "tripod_type=4" "$TRIPOD_KINEMATICS_STDOUT"
|
||||||
|
grep -Fq "tripod_switchable=0" "$TRIPOD_KINEMATICS_STDOUT"
|
||||||
|
grep -Fq "tripod_inverse=0" "$TRIPOD_KINEMATICS_STDOUT"
|
||||||
|
grep -Fq "tripod_inverse_flags=0" "$TRIPOD_KINEMATICS_STDOUT"
|
||||||
|
grep -Fq "tripod_forward=0" "$TRIPOD_KINEMATICS_STDOUT"
|
||||||
|
grep -Fq "tripod_roundtrip_xyz=1" "$TRIPOD_KINEMATICS_STDOUT"
|
||||||
|
grep -Fq "tripod_inverse_below=0" "$TRIPOD_KINEMATICS_STDOUT"
|
||||||
|
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"
|
||||||
|
|
||||||
check_fixture_output() {
|
check_fixture_output() {
|
||||||
local fixture="$1"
|
local fixture="$1"
|
||||||
local expected="$2"
|
local expected="$2"
|
||||||
|
|||||||
@@ -424,6 +424,12 @@ SCORBOT_KINEMATICS_PROBE_SOURCES=(
|
|||||||
"$WRAP_DIR/linuxcnc_scorbot_kinematics_probe.cpp"
|
"$WRAP_DIR/linuxcnc_scorbot_kinematics_probe.cpp"
|
||||||
)
|
)
|
||||||
|
|
||||||
|
TRIPOD_KINEMATICS_PROBE_SOURCES=(
|
||||||
|
"$VENDOR_DIR/src/emc/kinematics/tripodkins.c"
|
||||||
|
"$WRAP_DIR/linuxcnc_hal_adapter.cpp"
|
||||||
|
"$WRAP_DIR/linuxcnc_tripod_kinematics_probe.cpp"
|
||||||
|
)
|
||||||
|
|
||||||
TP_CORE_SOURCES=(
|
TP_CORE_SOURCES=(
|
||||||
"$VENDOR_DIR/src/emc/tp/tp.c"
|
"$VENDOR_DIR/src/emc/tp/tp.c"
|
||||||
"$VENDOR_DIR/src/emc/tp/tc.c"
|
"$VENDOR_DIR/src/emc/tp/tc.c"
|
||||||
@@ -674,6 +680,13 @@ build_binary_target \
|
|||||||
SCORBOT_KINEMATICS_PROBE_SOURCES \
|
SCORBOT_KINEMATICS_PROBE_SOURCES \
|
||||||
NO_LINK_FLAGS
|
NO_LINK_FLAGS
|
||||||
|
|
||||||
|
build_binary_target \
|
||||||
|
linuxcnc_tripod_kinematics_probe \
|
||||||
|
"$BUILD_DIR/linuxcnc_tripod_kinematics_probe" \
|
||||||
|
TP_FLAGS \
|
||||||
|
TRIPOD_KINEMATICS_PROBE_SOURCES \
|
||||||
|
NO_LINK_FLAGS
|
||||||
|
|
||||||
if [[ "$(tr -d '[:space:]' < "$BUILD_DIR/linuxcnc_kinematics_probe.exitcode")" == "0" ]]; then
|
if [[ "$(tr -d '[:space:]' < "$BUILD_DIR/linuxcnc_kinematics_probe.exitcode")" == "0" ]]; then
|
||||||
set +e
|
set +e
|
||||||
"$BUILD_DIR/linuxcnc_kinematics_probe" \
|
"$BUILD_DIR/linuxcnc_kinematics_probe" \
|
||||||
@@ -839,6 +852,21 @@ else
|
|||||||
"$BUILD_DIR/linuxcnc_scorbot_kinematics_probe.run.stderr.log"
|
"$BUILD_DIR/linuxcnc_scorbot_kinematics_probe.run.stderr.log"
|
||||||
fi
|
fi
|
||||||
|
|
||||||
|
if [[ "$(tr -d '[:space:]' < "$BUILD_DIR/linuxcnc_tripod_kinematics_probe.exitcode")" == "0" ]]; then
|
||||||
|
set +e
|
||||||
|
"$BUILD_DIR/linuxcnc_tripod_kinematics_probe" \
|
||||||
|
>"$BUILD_DIR/linuxcnc_tripod_kinematics_probe.run.stdout.log" \
|
||||||
|
2>"$BUILD_DIR/linuxcnc_tripod_kinematics_probe.run.stderr.log"
|
||||||
|
TRIPOD_KINEMATICS_RUN_RC=$?
|
||||||
|
set -e
|
||||||
|
echo "$TRIPOD_KINEMATICS_RUN_RC" > "$BUILD_DIR/linuxcnc_tripod_kinematics_probe.run.exitcode"
|
||||||
|
else
|
||||||
|
rm -f \
|
||||||
|
"$BUILD_DIR/linuxcnc_tripod_kinematics_probe.run.exitcode" \
|
||||||
|
"$BUILD_DIR/linuxcnc_tripod_kinematics_probe.run.stdout.log" \
|
||||||
|
"$BUILD_DIR/linuxcnc_tripod_kinematics_probe.run.stderr.log"
|
||||||
|
fi
|
||||||
|
|
||||||
if [[ "$(tr -d '[:space:]' < "$BUILD_DIR/linuxcnc_tp_api_probe.exitcode")" == "0" ]]; then
|
if [[ "$(tr -d '[:space:]' < "$BUILD_DIR/linuxcnc_tp_api_probe.exitcode")" == "0" ]]; then
|
||||||
set +e
|
set +e
|
||||||
"$BUILD_DIR/linuxcnc_tp_api_probe" \
|
"$BUILD_DIR/linuxcnc_tp_api_probe" \
|
||||||
@@ -1027,6 +1055,12 @@ build_object_target \
|
|||||||
"$VENDOR_DIR/src/emc/kinematics/scorbot-kins.c" \
|
"$VENDOR_DIR/src/emc/kinematics/scorbot-kins.c" \
|
||||||
TP_FLAGS
|
TP_FLAGS
|
||||||
|
|
||||||
|
build_object_target \
|
||||||
|
linuxcnc_tripodkins_source_probe \
|
||||||
|
"$BUILD_DIR/linuxcnc_tripodkins_source_probe.o" \
|
||||||
|
"$VENDOR_DIR/src/emc/kinematics/tripodkins.c" \
|
||||||
|
TP_FLAGS
|
||||||
|
|
||||||
build_object_target \
|
build_object_target \
|
||||||
linuxcnc_interp_convert_source_probe \
|
linuxcnc_interp_convert_source_probe \
|
||||||
"$BUILD_DIR/linuxcnc_interp_convert_source_probe.o" \
|
"$BUILD_DIR/linuxcnc_interp_convert_source_probe.o" \
|
||||||
|
|||||||
@@ -49,6 +49,7 @@ src/emc/kinematics/lineardeltakins.c
|
|||||||
src/emc/kinematics/rotarydeltakins-common.h
|
src/emc/kinematics/rotarydeltakins-common.h
|
||||||
src/emc/kinematics/rotarydeltakins.c
|
src/emc/kinematics/rotarydeltakins.c
|
||||||
src/emc/kinematics/scorbot-kins.c
|
src/emc/kinematics/scorbot-kins.c
|
||||||
|
src/emc/kinematics/tripodkins.c
|
||||||
src/emc/tp/tp.h
|
src/emc/tp/tp.h
|
||||||
src/emc/tp/tp_types.h
|
src/emc/tp/tp_types.h
|
||||||
src/emc/tp/tc.h
|
src/emc/tp/tc.h
|
||||||
|
|||||||
383
wasm-port/vendor/linuxcnc/src/emc/kinematics/tripodkins.c
vendored
Normal file
383
wasm-port/vendor/linuxcnc/src/emc/kinematics/tripodkins.c
vendored
Normal file
@@ -0,0 +1,383 @@
|
|||||||
|
/********************************************************************
|
||||||
|
* Description: tripodkins.c
|
||||||
|
* Kinematics for 3 axis Tripod machine
|
||||||
|
*
|
||||||
|
* Derived from a work by Fred Proctor
|
||||||
|
*
|
||||||
|
* Author:
|
||||||
|
* License: GPL Version 2
|
||||||
|
* System: Linux
|
||||||
|
*
|
||||||
|
* Copyright (c) 2004 All rights reserved.
|
||||||
|
*
|
||||||
|
* Last change:
|
||||||
|
********************************************************************/
|
||||||
|
|
||||||
|
/*
|
||||||
|
These kinematics are for a tripod with point vertices.
|
||||||
|
|
||||||
|
Vertices A, B, and C are the base, and vertex D is the controlled point.
|
||||||
|
Three tripod strut lengths AD, BD, and CD are the joints that move
|
||||||
|
point D around.
|
||||||
|
|
||||||
|
Point A is the origin, with coordinates (0, 0, 0). Point B lies on the
|
||||||
|
x axis, with coordinates (Bx, 0, 0). Point C lies in the xy plane, with
|
||||||
|
coordinates (Cx, Cy, 0). Point D has coordinates (Dx, Dy, Dz).
|
||||||
|
|
||||||
|
The controlled Cartesian values are Dx, Dy, and Dz. A frame attached to
|
||||||
|
D, say with x parallel to AD and y in the plane ABD, would change its
|
||||||
|
orientation as the strut lengths changed. The orientation of this frame
|
||||||
|
relative to the world frame is not computed.
|
||||||
|
|
||||||
|
With respect to the kinematics functions,
|
||||||
|
|
||||||
|
pos->tran.x = Dx
|
||||||
|
pos->tran.y = Dy
|
||||||
|
pos->tran.z = Dz
|
||||||
|
pos->a,b,c = 0
|
||||||
|
|
||||||
|
joints[0] = AD
|
||||||
|
joints[1] = BD
|
||||||
|
joints[2] = CD
|
||||||
|
|
||||||
|
The inverse kinematics have no singularities. Any values for Dx, Dy, and
|
||||||
|
Dz will yield numerical results. Of course, these may be beyond the
|
||||||
|
strut length limits, but there are no singular effects like infinite speed.
|
||||||
|
|
||||||
|
The forward kinematics has a singularity due to the triangle inequalities
|
||||||
|
for triangles ABD, BCD, and CAD. When any of these approach the limit,
|
||||||
|
Dz is zero and D lies in the base plane.
|
||||||
|
|
||||||
|
The forward kinematics flags, referred to in kinematicsForward and
|
||||||
|
set in kinematicsInverse, let the forward kinematics select between
|
||||||
|
the positive and negative values of Dz for given strut values.
|
||||||
|
Dz > 0 is "above", Dz < 0 is "below". Dz = 0 is the singularity.
|
||||||
|
|
||||||
|
fflags == 0 selects Dz > 0,
|
||||||
|
fflags != 0 selects Dz < 0.
|
||||||
|
|
||||||
|
The inverse kinematics flags let the inverse kinematics select between
|
||||||
|
multiple valid solutions of strut lengths for given Cartesian values
|
||||||
|
for D. There are no multiple solutions: D constrains the strut lengths
|
||||||
|
completely. So, the inverse flags are ignored.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <rtapi.h> /* RTAPI realtime OS API */
|
||||||
|
#include <rtapi_app.h> /* RTAPI realtime module decls */
|
||||||
|
#include <rtapi_math.h>
|
||||||
|
#include <hal.h>
|
||||||
|
#include <kinematics.h> /* these decls */
|
||||||
|
|
||||||
|
/* ident tag */
|
||||||
|
#ifndef __GNUC__
|
||||||
|
#ifndef __attribute__
|
||||||
|
#define __attribute__(x)
|
||||||
|
#endif
|
||||||
|
#endif
|
||||||
|
|
||||||
|
struct haldata {
|
||||||
|
hal_float_t *bx, *cx, *cy;
|
||||||
|
} *haldata = 0;
|
||||||
|
|
||||||
|
#define Bx (*(haldata->bx))
|
||||||
|
#define Cx (*(haldata->cx))
|
||||||
|
#define Cy (*(haldata->cy))
|
||||||
|
|
||||||
|
#define sq(x) ((x)*(x))
|
||||||
|
|
||||||
|
/*
|
||||||
|
forward kinematics takes three strut lengths and computes Dx, Dy, and Dz
|
||||||
|
pos->tran.x,y,z, respectively. The forward flag is used to resolve
|
||||||
|
D above/below the xy plane. The inverse flags are not set since there
|
||||||
|
are no ambiguities going from world to joint coordinates.
|
||||||
|
|
||||||
|
The forward kins are derived as follows:
|
||||||
|
|
||||||
|
1. Let x, y, z be Dx, Dy, Dz to save pixels. Cartesian displacement from
|
||||||
|
D to A, B, and C gives
|
||||||
|
|
||||||
|
AD^2 = x^2 + y^2 + z^2
|
||||||
|
BD^2 = (x - Bx)^2 + y^2 + z^2
|
||||||
|
CD^2 = (x - Cx)^2 + (y - Cy)^2 + z^2
|
||||||
|
|
||||||
|
This yields
|
||||||
|
|
||||||
|
I. P = x^2 + y^2 + z^2
|
||||||
|
II. Q = x^2 + y^2 + z^2 + sx
|
||||||
|
III. R = x^2 + y^2 + z^2 + tx + uy
|
||||||
|
|
||||||
|
Where
|
||||||
|
|
||||||
|
P = AD^2,
|
||||||
|
Q = BD^2 - Bx^2
|
||||||
|
R = CD^2 - Cx^2 - Cy^2
|
||||||
|
s = -2Bx
|
||||||
|
t = -2Cx
|
||||||
|
u = -2Cy
|
||||||
|
|
||||||
|
II - I gives Q - P = sx, so x = (Q - P)/s, s != 0. The constraint on s
|
||||||
|
means that Bx != 0, or points A and B can't be the same.
|
||||||
|
|
||||||
|
III - II gives R - Q = (t - s)x + uy, so y = (R - Q - (t - s)x)/u, u != 0.
|
||||||
|
The constraint on u means that Cy != 0, or points A B C can't be collinear.
|
||||||
|
|
||||||
|
Substituting x, y into I gives z = sqrt(P - x^2 - y^2), which has two
|
||||||
|
solutions. Positive means the tripod is above the xy plane, negative
|
||||||
|
means below.
|
||||||
|
*/
|
||||||
|
int kinematicsForward(const double * joints,
|
||||||
|
EmcPose * pos,
|
||||||
|
const KINEMATICS_FORWARD_FLAGS * fflags,
|
||||||
|
KINEMATICS_INVERSE_FLAGS * iflags)
|
||||||
|
{
|
||||||
|
(void)iflags;
|
||||||
|
#define AD (joints[0])
|
||||||
|
#define BD (joints[1])
|
||||||
|
#define CD (joints[2])
|
||||||
|
#define Dx (pos->tran.x)
|
||||||
|
#define Dy (pos->tran.y)
|
||||||
|
#define Dz (pos->tran.z)
|
||||||
|
double P, Q, R;
|
||||||
|
double s, t, u;
|
||||||
|
|
||||||
|
P = sq(AD);
|
||||||
|
Q = sq(BD) - sq(Bx);
|
||||||
|
R = sq(CD) - sq(Cx) - sq(Cy);
|
||||||
|
s = -2.0 * Bx;
|
||||||
|
t = -2.0 * Cx;
|
||||||
|
u = -2.0 * Cy;
|
||||||
|
|
||||||
|
if (s == 0.0) {
|
||||||
|
/* points A and B coincident. Fix Bx, #defined up top. */
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
Dx = (Q - P) / s;
|
||||||
|
|
||||||
|
if (u == 0.0) {
|
||||||
|
/* points A B C are colinear. Fix Cy, #defined up top. */
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
Dy = (R - Q - (t - s) * Dx) / u;
|
||||||
|
Dz = P - sq(Dx) - sq(Dy);
|
||||||
|
if (Dz < 0.0) {
|
||||||
|
/* triangle inequality violated */
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
Dz = sqrt(Dz);
|
||||||
|
if (*fflags) {
|
||||||
|
Dz = -Dz;
|
||||||
|
}
|
||||||
|
|
||||||
|
pos->a = 0.0;
|
||||||
|
pos->b = 0.0;
|
||||||
|
pos->c = 0.0;
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
|
||||||
|
#undef AD
|
||||||
|
#undef BD
|
||||||
|
#undef CD
|
||||||
|
#undef Dx
|
||||||
|
#undef Dy
|
||||||
|
#undef Dz
|
||||||
|
}
|
||||||
|
|
||||||
|
int kinematicsInverse(const EmcPose * pos,
|
||||||
|
double * joints,
|
||||||
|
const KINEMATICS_INVERSE_FLAGS * iflags,
|
||||||
|
KINEMATICS_FORWARD_FLAGS * fflags)
|
||||||
|
{
|
||||||
|
(void)iflags;
|
||||||
|
#define AD (joints[0])
|
||||||
|
#define BD (joints[1])
|
||||||
|
#define CD (joints[2])
|
||||||
|
#define Dx (pos->tran.x)
|
||||||
|
#define Dy (pos->tran.y)
|
||||||
|
#define Dz (pos->tran.z)
|
||||||
|
|
||||||
|
AD = sqrt(sq(Dx) + sq(Dy) + sq(Dz));
|
||||||
|
BD = sqrt(sq(Dx - Bx) + sq(Dy) + sq(Dz));
|
||||||
|
CD = sqrt(sq(Dx - Cx) + sq(Dy - Cy) + sq(Dz));
|
||||||
|
|
||||||
|
*fflags = 0;
|
||||||
|
if (Dz < 0.0) {
|
||||||
|
*fflags = 1;
|
||||||
|
}
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
|
||||||
|
#undef AD
|
||||||
|
#undef BD
|
||||||
|
#undef CD
|
||||||
|
#undef Dx
|
||||||
|
#undef Dy
|
||||||
|
#undef Dz
|
||||||
|
}
|
||||||
|
|
||||||
|
KINEMATICS_TYPE kinematicsType()
|
||||||
|
{
|
||||||
|
return KINEMATICS_BOTH;
|
||||||
|
}
|
||||||
|
|
||||||
|
#ifdef MAIN
|
||||||
|
|
||||||
|
#include <stdio.h>
|
||||||
|
#include <string.h>
|
||||||
|
|
||||||
|
/*
|
||||||
|
Interactive testing of kins.
|
||||||
|
|
||||||
|
Syntax: a.out <Bx> <Cx> <Cy>
|
||||||
|
*/
|
||||||
|
int main(int argc, char *argv[])
|
||||||
|
{
|
||||||
|
#ifndef BUFFERLEN
|
||||||
|
#define BUFFERLEN 256
|
||||||
|
#endif
|
||||||
|
char buffer[BUFFERLEN];
|
||||||
|
char cmd[BUFFERLEN];
|
||||||
|
EmcPose pos, vel;
|
||||||
|
double joints[3]={0.0,0.0,0.0}, jointvels[3]={0.0,0.0,0.0};
|
||||||
|
char inverse;
|
||||||
|
char flags;
|
||||||
|
KINEMATICS_FORWARD_FLAGS fflags;
|
||||||
|
|
||||||
|
inverse = 0; /* forwards, by default */
|
||||||
|
flags = 0; /* didn't provide flags */
|
||||||
|
fflags = 0; /* above xy plane, by default */
|
||||||
|
if (argc != 4 ||
|
||||||
|
1 != sscanf(argv[1], "%lf", &Bx) ||
|
||||||
|
1 != sscanf(argv[2], "%lf", &Cx) ||
|
||||||
|
1 != sscanf(argv[3], "%lf", &Cy)) {
|
||||||
|
fprintf(stderr, "syntax: %s Bx Cx Cy\n", argv[0]);
|
||||||
|
return 1;
|
||||||
|
}
|
||||||
|
|
||||||
|
while (! feof(stdin)) {
|
||||||
|
if (inverse) {
|
||||||
|
printf("inv> ");
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
printf("fwd> ");
|
||||||
|
}
|
||||||
|
fflush(stdout);
|
||||||
|
|
||||||
|
if (NULL == fgets(buffer, BUFFERLEN, stdin)) {
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
if (1 != sscanf(buffer, "%255s", cmd)) {
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (! strcmp(cmd, "quit")) {
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
if (! strcmp(cmd, "i")) {
|
||||||
|
inverse = 1;
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
if (! strcmp(cmd, "f")) {
|
||||||
|
inverse = 0;
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
if (! strcmp(cmd, "ff")) {
|
||||||
|
if (1 != sscanf(buffer, "%*s %lu", &fflags)) {
|
||||||
|
printf("need forward flag\n");
|
||||||
|
}
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (inverse) { /* inverse kins */
|
||||||
|
if (3 != sscanf(buffer, "%lf %lf %lf",
|
||||||
|
&pos.tran.x,
|
||||||
|
&pos.tran.y,
|
||||||
|
&pos.tran.z)) {
|
||||||
|
printf("need X Y Z\n");
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
if (0 != kinematicsInverse(&pos, joints, NULL, &fflags)) {
|
||||||
|
printf("inverse kin error\n");
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
printf("%f\t%f\t%f\n", joints[0], joints[1], joints[2]);
|
||||||
|
if (0 != kinematicsForward(joints, &pos, &fflags, NULL)) {
|
||||||
|
printf("forward kin error\n");
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
printf("%f\t%f\t%f\n", pos.tran.x, pos.tran.y, pos.tran.z);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else { /* forward kins */
|
||||||
|
if (flags) {
|
||||||
|
if (4 != sscanf(buffer, "%lf %lf %lf %lu",
|
||||||
|
&joints[0],
|
||||||
|
&joints[1],
|
||||||
|
&joints[2],
|
||||||
|
&fflags)) {
|
||||||
|
printf("need 3 strut values and flag\n");
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
if (3 != sscanf(buffer, "%lf %lf %lf",
|
||||||
|
&joints[0],
|
||||||
|
&joints[1],
|
||||||
|
&joints[2])) {
|
||||||
|
printf("need 3 strut values\n");
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if (0 != kinematicsForward(joints, &pos, &fflags, NULL)) {
|
||||||
|
printf("forward kin error\n");
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
printf("%f\t%f\t%f\n", pos.tran.x, pos.tran.y, pos.tran.z);
|
||||||
|
if (0 != kinematicsInverse(&pos, joints, NULL, &fflags)) {
|
||||||
|
printf("inverse kin error\n");
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
printf("%f\t%f\t%f\n", joints[0], joints[1], joints[2]);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
} /* end while (! feof(stdin)) */
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif /* MAIN */
|
||||||
|
|
||||||
|
KINS_NOT_SWITCHABLE
|
||||||
|
EXPORT_SYMBOL(kinematicsType);
|
||||||
|
EXPORT_SYMBOL(kinematicsForward);
|
||||||
|
EXPORT_SYMBOL(kinematicsInverse);
|
||||||
|
|
||||||
|
MODULE_LICENSE("GPL");
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
int comp_id;
|
||||||
|
int rtapi_app_main(void) {
|
||||||
|
int res = 0;
|
||||||
|
|
||||||
|
comp_id = hal_init("tripodkins");
|
||||||
|
if(comp_id < 0) return comp_id;
|
||||||
|
|
||||||
|
haldata = hal_malloc(sizeof(struct haldata));
|
||||||
|
if(!haldata) goto error;
|
||||||
|
|
||||||
|
if((res = hal_pin_float_new("tripodkins.Bx", HAL_IO, &(haldata->bx), comp_id)) < 0) goto error;
|
||||||
|
if((res = hal_pin_float_new("tripodkins.Cx", HAL_IO, &(haldata->cx), comp_id)) < 0) goto error;
|
||||||
|
if((res = hal_pin_float_new("tripodkins.Cy", HAL_IO, &(haldata->cy), comp_id)) < 0) goto error;
|
||||||
|
|
||||||
|
Bx = Cx = Cy = 1.0;
|
||||||
|
hal_ready(comp_id);
|
||||||
|
return 0;
|
||||||
|
|
||||||
|
error:
|
||||||
|
hal_exit(comp_id);
|
||||||
|
return res;
|
||||||
|
}
|
||||||
|
|
||||||
|
void rtapi_app_exit(void) { hal_exit(comp_id); }
|
||||||
Reference in New Issue
Block a user