按推荐建议,继续执行

结论:继续按 LinuxCNC 源码直接复用路线推进,新增 SCARA 运动学源码的 vendored 同步、switchkins native 探针、source probe 覆盖和复用文档,并通过 native 验证。
This commit is contained in:
2026-06-07 20:16:32 +08:00
parent d9296eb5e0
commit 42f1acd34a
6 changed files with 394 additions and 3 deletions

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` | | 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

View File

@@ -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;
}

View File

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

View File

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

View File

@@ -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

View 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()