按推荐建议,继续执行
结论:继续按 LinuxCNC 源码直接复用路线推进,新增 SCARA 运动学源码的 vendored 同步、switchkins 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`, `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` |
|
| 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,7 @@ 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, puma, and other machine-specific modules, are not yet
|
hexapod, puma, and other machine-specific modules, are not 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,101 @@
|
|||||||
|
#include <cmath>
|
||||||
|
#include <iostream>
|
||||||
|
|
||||||
|
#include "emc/kinematics/kinematics.h"
|
||||||
|
#include "emc/motion/emcmotcfg.h"
|
||||||
|
|
||||||
|
int rtapi_app_main(void);
|
||||||
|
void rtapi_app_exit(void);
|
||||||
|
|
||||||
|
namespace {
|
||||||
|
|
||||||
|
int near(double actual, double expected)
|
||||||
|
{
|
||||||
|
return std::fabs(actual - expected) < 1e-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;
|
||||||
|
}
|
||||||
@@ -103,6 +103,8 @@ 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
|
||||||
check_exitcode linuxcnc_tripod_kinematics_probe.run
|
check_exitcode linuxcnc_tripod_kinematics_probe.run
|
||||||
|
check_exitcode linuxcnc_scara_kinematics_probe
|
||||||
|
check_exitcode linuxcnc_scara_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 \
|
||||||
@@ -159,6 +161,7 @@ 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_tripodkins_source_probe
|
||||||
|
check_exitcode linuxcnc_scarakins_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
|
||||||
@@ -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_forward_below=0" "$TRIPOD_KINEMATICS_STDOUT"
|
||||||
grep -Fq "tripod_roundtrip_below_xyz=1" "$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() {
|
check_fixture_output() {
|
||||||
local fixture="$1"
|
local fixture="$1"
|
||||||
local expected="$2"
|
local expected="$2"
|
||||||
|
|||||||
@@ -430,6 +430,15 @@ TRIPOD_KINEMATICS_PROBE_SOURCES=(
|
|||||||
"$WRAP_DIR/linuxcnc_tripod_kinematics_probe.cpp"
|
"$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=(
|
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"
|
||||||
@@ -687,6 +696,13 @@ build_binary_target \
|
|||||||
TRIPOD_KINEMATICS_PROBE_SOURCES \
|
TRIPOD_KINEMATICS_PROBE_SOURCES \
|
||||||
NO_LINK_FLAGS
|
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
|
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" \
|
||||||
@@ -867,6 +883,21 @@ else
|
|||||||
"$BUILD_DIR/linuxcnc_tripod_kinematics_probe.run.stderr.log"
|
"$BUILD_DIR/linuxcnc_tripod_kinematics_probe.run.stderr.log"
|
||||||
fi
|
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
|
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" \
|
||||||
@@ -1061,6 +1092,12 @@ build_object_target \
|
|||||||
"$VENDOR_DIR/src/emc/kinematics/tripodkins.c" \
|
"$VENDOR_DIR/src/emc/kinematics/tripodkins.c" \
|
||||||
TP_FLAGS
|
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 \
|
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" \
|
||||||
|
|||||||
@@ -50,6 +50,7 @@ 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/kinematics/tripodkins.c
|
||||||
|
src/emc/kinematics/scarakins.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
|
||||||
|
|||||||
238
wasm-port/vendor/linuxcnc/src/emc/kinematics/scarakins.c
vendored
Normal file
238
wasm-port/vendor/linuxcnc/src/emc/kinematics/scarakins.c
vendored
Normal file
@@ -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 <rtapi.h>
|
||||||
|
#include <rtapi_math.h>
|
||||||
|
#include <rtapi_string.h>
|
||||||
|
#include <hal.h>
|
||||||
|
#include <kinematics.h>
|
||||||
|
|
||||||
|
#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()
|
||||||
Reference in New Issue
Block a user