按推荐建议,继续执行
结论:继续按 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` |
|
||||
| Switchable 5-axis bridge kinematics | `src/emc/kinematics/5axiskins.c`, `switchkins.c`, `switchkins.h`, `userkfuncs.c`, plus `src/rtapi/rtapi_ctype.h` | Copy unchanged | HAL pin allocation, HAL component lifecycle, and RTAPI module metadata are standalone runtime edges; switchable 5-axis forward/inverse behavior remains LinuxCNC source | Vendor byte sync, per-file source probes, `linuxcnc_5axis_kinematics_probe` |
|
||||
| TRT table-rotary kinematics | `src/emc/kinematics/trtfuncs.c`, `xyzac-trt-kins.c`, `xyzbc-trt-kins.c` | Copy unchanged | HAL pin allocation and switchkins lifecycle stay runtime boundaries; XYZAC/XYZBC TRT forward/inverse behavior remains LinuxCNC source | Vendor byte sync, per-file source probes, `linuxcnc_xyzac_trt_kinematics_probe`, `linuxcnc_xyzbc_trt_kinematics_probe` |
|
||||
| Additional non-switchable kinematics | `src/emc/kinematics/corexykins.c`, `rotatekins.c`, `rosekins.c`, `maxkins.c`, `lineardeltakins.c`, `lineardeltakins-common.h`, `rotarydeltakins.c`, `rotarydeltakins-common.h`, `scorbot-kins.c` | 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` |
|
||||
| Ruckig C planner support | Selected `src/emc/tp/cruckig/*.c` and `*.h` files in the manifest | Copy unchanged | Used as LinuxCNC planner support code through vendored TP sources | Vendor byte sync, per-file source probes |
|
||||
| Posemath | `src/libnml/posemath/posemath.cc`, `_posemath.c`, `gomath.c`, `sincos.c`, and matching headers | Copy unchanged | `gomath.c` is compiled as C; `rtapi.h` shim is C/C++ compatible for this boundary | Vendor byte sync, per-file source probes, TP probe |
|
||||
@@ -63,8 +63,8 @@ Current validation is intentionally mechanical:
|
||||
## Known Gaps
|
||||
|
||||
- Additional non-trivial kinematics implementation files, including serial,
|
||||
SCARA, hexapod, tripod, puma, and other machine-specific modules, are not
|
||||
yet extracted.
|
||||
SCARA, hexapod, puma, and other machine-specific modules, are not yet
|
||||
extracted.
|
||||
- Browser/WASM C ABI and JS SDK layers are not yet built for the full
|
||||
interpreter/planner core.
|
||||
- OPFS persistence is not yet connected to INI, tool table, parameter file, or
|
||||
|
||||
@@ -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_scorbot_kinematics_probe
|
||||
check_exitcode linuxcnc_scorbot_kinematics_probe.run
|
||||
check_exitcode linuxcnc_tripod_kinematics_probe
|
||||
check_exitcode linuxcnc_tripod_kinematics_probe.run
|
||||
for name in \
|
||||
linuxcnc_tp_tp_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_rotarydeltakins_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_read_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_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() {
|
||||
local fixture="$1"
|
||||
local expected="$2"
|
||||
|
||||
@@ -424,6 +424,12 @@ SCORBOT_KINEMATICS_PROBE_SOURCES=(
|
||||
"$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=(
|
||||
"$VENDOR_DIR/src/emc/tp/tp.c"
|
||||
"$VENDOR_DIR/src/emc/tp/tc.c"
|
||||
@@ -674,6 +680,13 @@ build_binary_target \
|
||||
SCORBOT_KINEMATICS_PROBE_SOURCES \
|
||||
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
|
||||
set +e
|
||||
"$BUILD_DIR/linuxcnc_kinematics_probe" \
|
||||
@@ -839,6 +852,21 @@ else
|
||||
"$BUILD_DIR/linuxcnc_scorbot_kinematics_probe.run.stderr.log"
|
||||
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
|
||||
set +e
|
||||
"$BUILD_DIR/linuxcnc_tp_api_probe" \
|
||||
@@ -1027,6 +1055,12 @@ build_object_target \
|
||||
"$VENDOR_DIR/src/emc/kinematics/scorbot-kins.c" \
|
||||
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 \
|
||||
linuxcnc_interp_convert_source_probe \
|
||||
"$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.c
|
||||
src/emc/kinematics/scorbot-kins.c
|
||||
src/emc/kinematics/tripodkins.c
|
||||
src/emc/tp/tp.h
|
||||
src/emc/tp/tp_types.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