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

按推荐建议，继续执行

Browse Source 
结论：已补齐剩余 LinuxCNC kinematics 源文件的 vendoring 和 native source-probe 覆盖，完整 native 验证通过。
This commit is contained in:
wangdequan
2026-06-07 20:41:20 +08:00
parent 578c159802
commit 1665cccb02
16 changed files with 3142 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
wasm-port/docs/drift-report.md
View File

@@ -47,11 +47,12 @@ semantic rewrites:
- No browser/WASM parity tests yet.
- No JS SDK validation yet.
- No OPFS persistence validation yet.
- Identity/trivial kinematics, LinuxCNC `5axiskins`, and TRT `xyzac`/`xyzbc`
now have native source-probe coverage; serial, delta, SCARA, and other
machine-specific kinematics are not yet extracted.
- Full machine baselines beyond the current identity/trivial, `5axiskins`, and
TRT probes are not established.
- Identity/trivial, `5axiskins`, TRT `xyzac`/`xyzbc`, delta, SCARA, PUMA,
serial, hexapod, pentapod, and related kinematics sources now have native
source-probe coverage.
- Full machine baselines beyond the current identity/trivial, `5axiskins`,
TRT, delta, SCARA, PUMA, and other existing runtime probes are not
established.
- Fixture expectations are currently checked against the standalone vendored
source path, not by running a native LinuxCNC binary for every fixture.

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

@@ -38,7 +38,7 @@ Current validation is intentionally mechanical:
| Identity/trivial kinematics | `src/emc/kinematics/kinematics.h`, `cubic.h`, `kins_util.c`, `trivkins.c` | Copy unchanged | HAL component lifecycle and RTAPI module metadata are replaced by standalone shims; forward/inverse mapping behavior remains LinuxCNC source | Vendor byte sync, per-file source probes, `linuxcnc_kinematics_probe` |
| Switchable 5-axis bridge kinematics | `src/emc/kinematics/5axiskins.c`, `switchkins.c`, `switchkins.h`, `userkfuncs.c`, plus `src/rtapi/rtapi_ctype.h` | Copy unchanged | HAL pin allocation, HAL component lifecycle, and RTAPI module metadata are standalone runtime edges; switchable 5-axis forward/inverse behavior remains LinuxCNC source | Vendor byte sync, per-file source probes, `linuxcnc_5axis_kinematics_probe` |
| TRT table-rotary kinematics | `src/emc/kinematics/trtfuncs.c`, `xyzac-trt-kins.c`, `xyzbc-trt-kins.c` | Copy unchanged | HAL pin allocation and switchkins lifecycle stay runtime boundaries; XYZAC/XYZBC TRT forward/inverse behavior remains LinuxCNC source | Vendor byte sync, per-file source probes, `linuxcnc_xyzac_trt_kinematics_probe`, `linuxcnc_xyzbc_trt_kinematics_probe` |
| Additional non-switchable kinematics | `src/emc/kinematics/corexykins.c`, `rotatekins.c`, `rosekins.c`, `maxkins.c`, `lineardeltakins.c`, `lineardeltakins-common.h`, `rotarydeltakins.c`, `rotarydeltakins-common.h`, `scorbot-kins.c`, `tripodkins.c`, `scarakins.c`, `pumakins.c`, `pumakins.h` | 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`, `linuxcnc_puma_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`, `pumakins.c`, `pumakins.h`, `genhexkins.c`, `genhexkins.h`, `genserfuncs.c`, `genserkins.c`, `genserkins.h`, `ugenserkins.c`, `pentakins.c`, `pentakins.h`, `cubic.c` | Copy unchanged | HAL pin allocation, HAL parameter allocation, HAL component lifecycle, RTAPI module metadata, and userspace test-program process entry remain standalone runtime edges; forward/inverse behavior remains LinuxCNC source where the module exposes it | 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`, `linuxcnc_puma_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 |
@@ -62,8 +62,9 @@ Current validation is intentionally mechanical:
## Known Gaps
- Additional non-trivial kinematics implementation files, including serial,
hexapod, and other machine-specific modules, are not yet extracted.
- Additional non-trivial kinematics implementation files are now extracted at
source-probe level. Full runtime machine baselines for serial, hexapod,
pentapod, and userspace genser flows are not yet established.
- 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

9
wasm-port/runtime/core/linuxcnc_wrap/linuxcnc_hal_adapter.cpp
View File

@@ -250,6 +250,15 @@ int hal_pin_s64_newf(hal_pin_dir_t dir, hal_s64_t **data_ptr_addr, int comp_id,
return rc;
}
int hal_param_float_newf(hal_pin_dir_t, hal_float_t *data_addr, int, const char *, ...)
{
if (!data_addr) {
return -1;
}
*data_addr = 0.0;
return 0;
}
int hal_get_pin_value_by_name(const char *name, hal_type_t *type, hal_data_u **ptr,
bool *connected)
{

2
wasm-port/runtime/core/shims/hal.h
View File

@@ -21,6 +21,7 @@ typedef enum {
HAL_IN = 16,
HAL_OUT = 32,
HAL_IO = (HAL_IN | HAL_OUT),
HAL_RW = HAL_IO,
} hal_pin_dir_t;
typedef union {
@@ -56,6 +57,7 @@ int hal_pin_u32_newf(hal_pin_dir_t, hal_u32_t **, int, const char *, ...);
int hal_pin_s32_newf(hal_pin_dir_t, hal_s32_t **, int, const char *, ...);
int hal_pin_u64_newf(hal_pin_dir_t, hal_u64_t **, int, const char *, ...);
int hal_pin_s64_newf(hal_pin_dir_t, hal_s64_t **, int, const char *, ...);
int hal_param_float_newf(hal_pin_dir_t, hal_float_t *, int, const char *, ...);
int hal_get_pin_value_by_name(const char *, hal_type_t *, hal_data_u **, bool *);
int hal_get_signal_value_by_name(const char *, hal_type_t *, hal_data_u **, bool *);
int hal_get_param_value_by_name(const char *, hal_type_t *, hal_data_u **);

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

@@ -148,6 +148,7 @@ check_exitcode linuxcnc_parameter_file_harness
check_exitcode linuxcnc_parameter_file_harness.run
check_exitcode linuxcnc_rs274_compile_probe
check_exitcode linuxcnc_kins_util_source_probe
check_exitcode linuxcnc_cubic_source_probe
check_exitcode linuxcnc_trivkins_source_probe
check_exitcode linuxcnc_switchkins_source_probe
check_exitcode linuxcnc_userkfuncs_source_probe
@@ -165,6 +166,11 @@ check_exitcode linuxcnc_scorbot_kins_source_probe
check_exitcode linuxcnc_tripodkins_source_probe
check_exitcode linuxcnc_scarakins_source_probe
check_exitcode linuxcnc_pumakins_source_probe
check_exitcode linuxcnc_genhexkins_source_probe
check_exitcode linuxcnc_genserfuncs_source_probe
check_exitcode linuxcnc_genserkins_source_probe
check_exitcode linuxcnc_ugenserkins_source_probe
check_exitcode linuxcnc_pentakins_source_probe
check_exitcode linuxcnc_interp_convert_source_probe
check_exitcode linuxcnc_interp_read_source_probe
check_exitcode linuxcnc_interp_check_source_probe

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

@@ -1036,6 +1036,12 @@ build_object_target \
"$VENDOR_DIR/src/emc/kinematics/kins_util.c" \
TP_FLAGS
build_object_target \
linuxcnc_cubic_source_probe \
"$BUILD_DIR/linuxcnc_cubic_source_probe.o" \
"$VENDOR_DIR/src/emc/kinematics/cubic.c" \
TP_FLAGS
build_object_target \
linuxcnc_trivkins_source_probe \
"$BUILD_DIR/linuxcnc_trivkins_source_probe.o" \
@@ -1138,6 +1144,36 @@ build_object_target \
"$VENDOR_DIR/src/emc/kinematics/pumakins.c" \
TP_FLAGS
build_object_target \
linuxcnc_genhexkins_source_probe \
"$BUILD_DIR/linuxcnc_genhexkins_source_probe.o" \
"$VENDOR_DIR/src/emc/kinematics/genhexkins.c" \
TP_FLAGS
build_object_target \
linuxcnc_genserfuncs_source_probe \
"$BUILD_DIR/linuxcnc_genserfuncs_source_probe.o" \
"$VENDOR_DIR/src/emc/kinematics/genserfuncs.c" \
TP_FLAGS
build_object_target \
linuxcnc_genserkins_source_probe \
"$BUILD_DIR/linuxcnc_genserkins_source_probe.o" \
"$VENDOR_DIR/src/emc/kinematics/genserkins.c" \
TP_FLAGS
build_object_target \
linuxcnc_ugenserkins_source_probe \
"$BUILD_DIR/linuxcnc_ugenserkins_source_probe.o" \
"$VENDOR_DIR/src/emc/kinematics/ugenserkins.c" \
TP_FLAGS
build_object_target \
linuxcnc_pentakins_source_probe \
"$BUILD_DIR/linuxcnc_pentakins_source_probe.o" \
"$VENDOR_DIR/src/emc/kinematics/pentakins.c" \
TP_FLAGS
build_object_target \
linuxcnc_interp_convert_source_probe \
"$BUILD_DIR/linuxcnc_interp_convert_source_probe.o" \

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

@@ -30,6 +30,7 @@ src/emc/motion/motion.h
src/emc/motion/mot_priv.h
src/emc/motion/axis.h
src/emc/kinematics/kinematics.h
src/emc/kinematics/cubic.c
src/emc/kinematics/cubic.h
src/emc/kinematics/kins_util.c
src/emc/kinematics/trivkins.c
@@ -53,6 +54,14 @@ src/emc/kinematics/tripodkins.c
src/emc/kinematics/scarakins.c
src/emc/kinematics/pumakins.h
src/emc/kinematics/pumakins.c
src/emc/kinematics/genhexkins.h
src/emc/kinematics/genhexkins.c
src/emc/kinematics/genserkins.h
src/emc/kinematics/genserfuncs.c
src/emc/kinematics/genserkins.c
src/emc/kinematics/ugenserkins.c
src/emc/kinematics/pentakins.h
src/emc/kinematics/pentakins.c
src/emc/tp/tp.h
src/emc/tp/tp_types.h
src/emc/tp/tc.h

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

@@ -0,0 +1,422 @@
/********************************************************************
* Description: cubic.c
* Cubic spline interpolation code
* Analysis taken in part from Curtis S. Wilson, "How Close Do You
* Have to Specify Points In a Contouring Application?", Delta Tau
* Data Systems (unpublished).
*
* Derived from a work by Fred Proctor & Will Shackleford
*
* Author:
* License: GPL Version 2
* System: Linux
*
* Copyright (c) 2004 All rights reserved.
********************************************************************/
#include <rtapi_math.h>
#include "cubic.h"
#define SEGMENT_TIME_SET 0x01
#define INTERPOLATION_RATE_SET 0x02
#define ALL_SET (SEGMENT_TIME_SET | INTERPOLATION_RATE_SET)
/*
cubicCoeff calculates the coefficients of the cubic spline
fit to the values of x and v at 0 and t.
deltaT is the length of the interval.
The return value is a CUBIC_COEFF.
Derivation:
Solve
a t^3 + b t^2 + c t + d = x,
3a t^2 + 2b t + c = v
for a, b, c, and d, given
x(t=0), v(t=0),
x(t=deltaT), v(t=deltaT)
*/
static CUBIC_COEFF cubicCoeff(double x0, double v0,
double xn, double vn, double deltaT)
{
CUBIC_COEFF retval;
/* first the easy ones */
retval.d = x0;
retval.c = v0;
/* now the hard ones */
retval.b = 3 * (xn - x0) / (deltaT * deltaT) - (2 * v0 + vn) / deltaT;
retval.a = (vn - v0) / (3.0 * (deltaT * deltaT)) -
(2.0 * retval.b) / (3.0 * deltaT);
return retval;
}
/*
Interpolate points along a cubic, given t and cubic params
*/
static double interpolateCubic(CUBIC_COEFF coeff, double t)
{
return coeff.a * (t * t * t) + coeff.b * (t * t) + coeff.c * t +
coeff.d;
}
/*
Interpolate velocity given same cubic params as above, using
differentiation of cubic coeffs
*/
static double interpolateVel(CUBIC_COEFF coeff, double t)
{
return 3.0 * coeff.a * (t * t) + 2.0 * coeff.b * t + coeff.c;
}
/*
Interpolate acceleration given same cubic params as above, using
differentiation twice of cubic coeffs
*/
static double interpolateAccel(CUBIC_COEFF coeff, double t)
{
return 6.0 * coeff.a * t + 2.0 * coeff.b;
}
/*
Interpolate jerk given same cubic params as above, using
triple differentiation of cubic coeffs
*/
static double interpolateJerk(CUBIC_COEFF coeff, double t)
{
(void)t;
return 6.0 * coeff.a;
}
/*
Calculate the cubic spline way point, given a point and its
previous and successive neighbors
*/
static double wayPoint(double xMinus1, double x, double xPlus1)
{
return (xMinus1 + 4.0 * x + xPlus1) / 6.0;
}
/*
Calculate the cubic spline velocity value, given a point and its
previous and successive neighbors
*/
static double velPoint(double xMinus1, double xPlus1, double deltaT)
{
if (deltaT <= 0.0) {
return 0.0;
} else {
return (xPlus1 - xMinus1) / (2.0 * deltaT);
}
}
int cubicInit(CUBIC_STRUCT * ci)
{
if (0 == ci) {
return -1;
}
ci->configured = 0;
ci->segmentTime = 0.0;
ci->interpolationRate = 0;
ci->interpolationIncrement = 0.0;
cubicDrain(ci);
return 0;
}
int cubicSetSegmentTime(CUBIC_STRUCT * ci, double time)
{
if (0 == ci || time <= 0.0) {
return -1;
}
ci->segmentTime = time;
ci->configured |= SEGMENT_TIME_SET;
if (ci->configured == ALL_SET) {
ci->interpolationIncrement =
ci->segmentTime / ci->interpolationRate;
}
return 0;
}
double cubicGetSegmentTime(CUBIC_STRUCT * ci)
{
if (0 == ci || !(ci->configured & SEGMENT_TIME_SET)) {
return 0.0;
}
return ci->segmentTime;
}
int cubicSetInterpolationRate(CUBIC_STRUCT * ci, int rate)
{
if (0 == ci || rate <= 0) {
return -1;
}
ci->interpolationRate = rate;
ci->configured |= INTERPOLATION_RATE_SET;
if (ci->configured == ALL_SET) {
ci->interpolationIncrement =
ci->segmentTime / ci->interpolationRate;
}
return 0;
}
int cubicGetInterpolationRate(CUBIC_STRUCT * ci)
{
if (0 == ci || !(ci->configured & INTERPOLATION_RATE_SET)) {
return 0;
}
return ci->interpolationRate;
}
double cubicGetInterpolationIncrement(CUBIC_STRUCT * ci)
{
if (0 == ci || ci->configured != ALL_SET) {
return 0;
}
return ci->interpolationIncrement;
}
CUBIC_COEFF cubicGetCubicCoeff(CUBIC_STRUCT * ci)
{
CUBIC_COEFF errorReturn;
if (0 == ci || !ci->filled) {
errorReturn.a = 0.0;
errorReturn.b = 0.0;
errorReturn.c = 0.0;
errorReturn.d = 0.0;
return errorReturn;
}
return ci->coeff;
}
/*
cubicAddPoint(double point)
Add a point to the end of the cubic interpolator.
Can only be called to initially fill up the four-point queue required
for interpolation, or when interpolate() has been called for the full
segment and the needNextPoint() flag is non-zero.
The first point added fills the first two slots, since interpolation
is done between the second and third input point, and this filling
is required so that the output interpolation matches with the input
points.
*/
int cubicAddPoint(CUBIC_STRUCT * ci, double point)
{
if (0 == ci || !(ci->configured == ALL_SET)) {
return -1;
}
if (!ci->needNextPoint) {
return -1;
}
if (!ci->filled) {
ci->x0 = point;
ci->x1 = point;
ci->x2 = point;
ci->x3 = point;
ci->filled = 1;
} else {
ci->x0 = ci->x1;
ci->x1 = ci->x2;
ci->x2 = ci->x3;
ci->x3 = point;
}
/* calculate way points and coeff */
ci->wp0 = wayPoint(ci->x0, ci->x1, ci->x2);
ci->wp1 = wayPoint(ci->x1, ci->x2, ci->x3);
ci->velp0 = velPoint(ci->x0, ci->x2, ci->segmentTime);
ci->velp1 = velPoint(ci->x1, ci->x3, ci->segmentTime);
ci->coeff = cubicCoeff(ci->wp0, ci->velp0, ci->wp1,
ci->velp1, ci->segmentTime);
ci->interpolationTime = 0.0;
ci->needNextPoint = 0;
return 0;
}
/*
cubicOffset(CUBIC_STRUCT * ci, double offset)
Set the interpolator so that the points inside are offset by the given
value, as if the original points all had this offset added to them prior
to their addition to the interpolator. This is used when offsetting
trajectory points, to keep the interpolators consistent without draining
them.
*/
int cubicOffset(CUBIC_STRUCT * ci, double offset)
{
if (0 == ci || !(ci->configured == ALL_SET)) {
return -1;
}
ci->x0 += offset;
ci->x1 += offset;
ci->x2 += offset;
ci->x3 += offset;
ci->wp0 += offset;
ci->wp1 += offset;
/* leave velp0, velp1 alone, since these are velocity points and are
unaffected by position offsets */
/* only the D coeff is affected, so we can change this directly */
ci->coeff.d += offset;
return 0;
}
int cubicFilled(CUBIC_STRUCT * ci)
{
if (0 == ci) {
return 0;
}
return ci->filled;
}
double cubicInterpolate(CUBIC_STRUCT * ci,
double *x, double *v, double *a, double *j)
{
double retval;
if (0 == ci || !(ci->configured == ALL_SET)) {
return 0.0;
}
if (ci->needNextPoint) {
/* queue ran out-- fill right with last point */
cubicAddPoint(ci, ci->x3);
}
retval = interpolateCubic(ci->coeff, ci->interpolationTime);
/* do optional ones */
if (x != 0) {
*x = retval;
}
if (v != 0) {
*v = interpolateVel(ci->coeff, ci->interpolationTime);
}
if (a != 0) {
*a = interpolateAccel(ci->coeff, ci->interpolationTime);
}
if (j != 0) {
*j = interpolateJerk(ci->coeff, ci->interpolationTime);
}
ci->interpolationTime += ci->interpolationIncrement;
/* check to see if the next point is at (close to) the segment end */
if (fabs(ci->segmentTime - ci->interpolationTime)
< 0.5 * ci->interpolationIncrement) {
/* just computed last point-- flag that we need a new one */
ci->needNextPoint = 1;
}
return retval;
}
int cubicNeedNextPoint(CUBIC_STRUCT * ci)
{
return ci->needNextPoint;
}
int cubicDrain(CUBIC_STRUCT * ci)
{
ci->x0 = ci->x1 = ci->x2 = ci->x3 = 0.0;
ci->wp0 = ci->wp1 = 0.0;
ci->velp0 = ci->velp1 = 0.0;
ci->filled = 0;
ci->needNextPoint = 1;
ci->coeff.a = 0.0;
ci->coeff.b = 0.0;
ci->coeff.c = 0.0;
ci->coeff.d = 0.0;
return 0;
}
#ifdef MAIN
#include <stdio.h>
/*
syntax: testcubic <segment time> <interpolation rate>
*/
int main(int argc, char *argv[])
{
CUBIC_STRUCT cubic;
double segmentTime;
int interpolationRate;
double xin;
double xout;
double time = 0.0;
if (argc != 3) {
fprintf(stderr, "syntax: %s <segment time> <interpolation rate>\n",
argv[0]);
return 1;
}
if (1 != sscanf(argv[1], "%lf", &segmentTime) || segmentTime <= 0.0) {
fprintf(stderr, "invalid segment time %s\n", argv[1]);
return 1;
}
if (1 != sscanf(argv[2], "%d", &interpolationRate) ||
interpolationRate <= 0) {
fprintf(stderr, "invalid interpolation rate %s\n", argv[2]);
return 1;
}
if (0 != cubicInit(&cubic)) {
fprintf(stderr, "can't initialize interpolator\n");
return 1;
}
if (0 != cubicSetSegmentTime(&cubic, segmentTime)) {
fprintf(stderr, "can't set segment time\n");
return 1;
}
if (0 != cubicSetInterpolationRate(&cubic, interpolationRate)) {
fprintf(stderr, "can't set interpolation rate\n");
return 1;
}
while (!feof(stdin)) {
if (cubicNeedNextPoint(&cubic)) {
if (1 != scanf("%lf", &xin)) {
break;
} else {
cubicAddPoint(&cubic, xin);
}
}
xout = cubicInterpolate(&cubic, 0, 0, 0, 0);
printf("%f %f\n", time, xout);
time += segmentTime;
}
}
#endif

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

@@ -0,0 +1,739 @@
/********************************************************************
* Description: genhexkins.c
*
* Kinematics for a generalised hexapod machine
*
* Derived from a work by R. Brian Register
*
* Adapting Author: Andrew Kyrychenko
* License: GPL Version 2
* System: Linux
*
* Copyright (c) 2004 All rights reserved.
*********************************************************************
These are the forward and inverse kinematic functions for a class of
machines referred to as "Stewart Platforms".
The functions are general enough to be configured for any platform
configuration. In the functions "genhexKinematicsForward" and
"genhexKinematicsInverse" are arrays "a[i]" and "b[i]". The values stored
in these arrays correspond to the positions of the ends of the i'th
strut. The value stored in a[i] is the position of the end of the i'th
strut attached to the platform, in platform coordinates. The value
stored in b[i] is the position of the end of the i'th strut attached
to the base, in base (world) coordinates.
The default values for base and platform joints positions are defined
in the header file genhexkins.h. The actual values for a particular
machine can be adjusted by hal parameters:
genhexkins.base.N.x
genhexkins.base.N.y
genhexkins.base.N.z - base joint coordinates.
genhexkins.platform.N.x
genhexkins.platform.N.y
genhexkins.platform.N.z - platform joint coordinates.
genhexkins.spindle-offset - added to Z coordinates of all joints to
change the machine origin. Facilitates
adjusting spindle position.
genhexkins.tool-offset - tool length offset (TCP offset along Z),
implements RTCP function when connected to
motion.tooloffset.Z.
To avoid joints jump change tool offset (G43, G49) only when the
platform is not tilted (A = B = 0).
Some hexapods use non-captive screw actuators and universal (cardanic)
joints, thus the strut lengths depend on orientation of joints axes.
Strut length correction is implemented to compensate for this.
The calculations use orientation (unit vectors) of base and platform
joint axes and the lead of actuator screws:
genhexkins.base-n.N.x
genhexkins.base-n.N.y
genhexkins.base-n.N.z - unit vectors of base joint axes;
genhexkins.platform-n.N.x
genhexkins.platform-n.N.y
genhexkins.platform-n.N.z - unit vectors of platform joint axes
in platform CS.
genhexkins.screw-lead - lead of strut actuator screw, positive for
right-hand thread. Default is 0 (strut length
correction disabled).
genhexkins.correction.N - pins showing current values of strut length
correction.
The genhexKinematicsInverse function solves the inverse kinematics using
a closed form algorithm. The inverse kinematics problem is given
the pose of the platform and returns the strut lengths. For this
problem there is only one solution that is always returned correctly.
The genhexKinematicsForward function solves the forward kinematics using
an iterative algorithm. Due to the iterative nature of this algorithm
the genhexKinematicsForward function requires an initial value to begin the
iterative routine and then converges to the "nearest" solution. The
forward kinematics problem is given the strut lengths and returns the
pose of the platform. For this problem there arein multiple
solutions. The genhexKinematicsForward function will return only one of
these solutions which will be the solution nearest to the initial
value given. It is possible that there are no solutions "near" the
given initial value and the iteration will not converge and no
solution will be returned. Assuming there is a solution "near" the
initial value, the function will always return one correct solution
out of the multiple possible solutions.
Hal pins to control and observe forward kinematics iterations:
genhexkins.convergence-criterion - minimum error value that ends
iterations with converged solution;
genhexkins.limit-iterations - limit of iterations, if exceeded
iterations stop with no convergence;
genhexkins.max-error - maximum error value, if exceeded iterations
stop with no convergence;
genhexkins.last-iterations - number of iterations spent for the
last forward kinematics solution;
genhexkins.max-iterations - maximum number of iterations spent for
a converged solution during current session.
----------------------------------------------------------------------------*/
#include <rtapi.h>
#include <rtapi_math.h>
#include <rtapi_string.h>
#include <hal.h>
#include <emcmotcfg.h>
#include <kinematics.h> /* these decls, KINEMATICS_FORWARD_FLAGS */
#include "genhexkins.h"
#include "switchkins.h"
static struct haldata {
hal_float_t *basex[NUM_STRUTS];
hal_float_t *basey[NUM_STRUTS];
hal_float_t *basez[NUM_STRUTS];
hal_float_t *platformx[NUM_STRUTS];
hal_float_t *platformy[NUM_STRUTS];
hal_float_t *platformz[NUM_STRUTS];
hal_float_t *basenx[NUM_STRUTS];
hal_float_t *baseny[NUM_STRUTS];
hal_float_t *basenz[NUM_STRUTS];
hal_float_t *platformnx[NUM_STRUTS];
hal_float_t *platformny[NUM_STRUTS];
hal_float_t *platformnz[NUM_STRUTS];
hal_float_t *correction[NUM_STRUTS];
hal_float_t *screw_lead;
hal_u32_t *last_iter;
hal_u32_t *max_iter;
hal_u32_t *iter_limit;
hal_float_t *max_error;
hal_float_t *conv_criterion;
hal_float_t *tool_offset;
hal_float_t *spindle_offset;
hal_bit_t *fwd_kins_fail;
hal_float_t *gui_x;
hal_float_t *gui_y;
hal_float_t *gui_z;
hal_float_t *gui_a;
hal_float_t *gui_b;
hal_float_t *gui_c;
} *haldata;
static int genhex_gui_forward_kins(EmcPose *pos)
{
*haldata->gui_x = pos->tran.x;
*haldata->gui_y = pos->tran.y;
*haldata->gui_z = pos->tran.z;
*haldata->gui_a = pos->a;
*haldata->gui_b = pos->b;
*haldata->gui_c = pos->c;
return 0;
} // genhex_gui_forward_kins
/******************************* MatInvert() ***************************/
/*-----------------------------------------------------------------------------
This is a function that inverts a 6x6 matrix.
-----------------------------------------------------------------------------*/
static int MatInvert(double J[][NUM_STRUTS], double InvJ[][NUM_STRUTS])
{
double JAug[NUM_STRUTS][12], m, temp;
int j, k, n;
/* This function determines the inverse of a 6x6 matrix using
Gauss-Jordan elimination */
/* Augment the Identity matrix to the Jacobian matrix */
for (j=0; j<=5; ++j){
for (k=0; k<=5; ++k){ /* Assign J matrix to first 6 columns of AugJ */
JAug[j][k] = J[j][k];
}
for(k=6; k<=11; ++k){ /* Assign I matrix to last six columns of AugJ */
if (k-6 == j){
JAug[j][k]=1;
}
else{
JAug[j][k]=0;
}
}
}
/* Perform Gauss elimination */
for (k=0; k<=4; ++k){ /* Pivot */
if ((JAug[k][k]< 0.01) && (JAug[k][k] > -0.01)){
for (j=k+1;j<=5; ++j){
if ((JAug[j][k]>0.01) || (JAug[j][k]<-0.01)){
for (n=0; n<=11;++n){
temp = JAug[k][n];
JAug[k][n] = JAug[j][n];
JAug[j][n] = temp;
}
break;
}
}
}
for (j=k+1; j<=5; ++j){ /* Pivot */
m = -JAug[j][k] / JAug[k][k];
for (n=0; n<=11; ++n){
JAug[j][n]=JAug[j][n] + m*JAug[k][n]; /* (Row j) + m * (Row k) */
if ((JAug[j][n] < 0.000001) && (JAug[j][n] > -0.000001)){
JAug[j][n] = 0;
}
}
}
}
/* Normalization of Diagonal Terms */
for (j=0; j<=5; ++j){
m=1/JAug[j][j];
for(k=0; k<=11; ++k){
JAug[j][k] = m * JAug[j][k];
}
}
/* Perform Gauss Jordan Steps */
for (k=5; k>=0; --k){
for(j=k-1; j>=0; --j){
m = -JAug[j][k]/JAug[k][k];
for (n=0; n<=11; ++n){
JAug[j][n] = JAug[j][n] + m * JAug[k][n];
}
}
}
/* Assign last 6 columns of JAug to InvJ */
for (j=0; j<=5; ++j){
for (k=0; k<=5; ++k){
InvJ[j][k] = JAug[j][k+6];
}
}
return 0; /* FIXME-- check divisors for 0 above */
} // MatInvert()
/******************************** MatMult() *********************************/
/*---------------------------------------------------------------------------
This function simply multiplies a 6x6 matrix by a 1x6 vector
---------------------------------------------------------------------------*/
static void MatMult(double J[][6], const double x[], double Ans[])
{
int j, k;
for (j=0; j<=5; ++j){
Ans[j] = 0;
for (k=0; k<=5; ++k){
Ans[j] = J[j][k]*x[k]+Ans[j];
}
}
} // MatMult()
/* declare arrays for base and platform coordinates */
static PmCartesian b[NUM_STRUTS];
static PmCartesian a[NUM_STRUTS];
/* declare base and platform joint axes vectors */
static PmCartesian nb1[NUM_STRUTS];
static PmCartesian na0[NUM_STRUTS];
/************************genhex_read_hal_pins**************************/
static int genhex_read_hal_pins(void) {
int t;
/* set the base and platform coordinates from hal pin values */
for (t = 0; t < NUM_STRUTS; t++) {
b[t].x = *haldata->basex[t];
b[t].y = *haldata->basey[t];
b[t].z = *haldata->basez[t] + *haldata->spindle_offset + *haldata->tool_offset;
a[t].x = *haldata->platformx[t];
a[t].y = *haldata->platformy[t];
a[t].z = *haldata->platformz[t] + *haldata->spindle_offset + *haldata->tool_offset;
nb1[t].x = *haldata->basenx[t];
nb1[t].y = *haldata->baseny[t];
nb1[t].z = *haldata->basenz[t];
na0[t].x = *haldata->platformnx[t];
na0[t].y = *haldata->platformny[t];
na0[t].z = *haldata->platformnz[t];
}
return 0;
} // genhex_read_hal_pins()
/***************************StrutLengthCorrection***************************/
static int StrutLengthCorrection(const PmCartesian * StrutVectUnit,
const PmRotationMatrix * RMatrix,
const int strut_number,
double * correction)
{
PmCartesian nb2, nb3, na1, na2;
double dotprod;
/* define base joints axis vectors */
pmCartCartCross(&nb1[strut_number], StrutVectUnit, &nb2);
pmCartCartCross(StrutVectUnit, &nb2, &nb3);
pmCartUnitEq(&nb3);
/* define platform joints axis vectors */
pmMatCartMult(RMatrix, &na0[strut_number], &na1);
pmCartCartCross(&na1, StrutVectUnit, &na2);
pmCartUnitEq(&na2);
/* define dot product */
pmCartCartDot(&nb3, &na2, &dotprod);
*correction = *haldata->screw_lead * asin(dotprod) / PM_2_PI;
return 0;
} // StrutLengthCorrection()
/**************** genhexKinematicsForward() *****************/
static int genhexKinematicsForward(const double * joints,
EmcPose * pos,
const KINEMATICS_FORWARD_FLAGS * fflags,
KINEMATICS_INVERSE_FLAGS * iflags)
{
(void)fflags;
(void)iflags;
PmCartesian aw;
PmCartesian InvKinStrutVect,InvKinStrutVectUnit;
PmCartesian q_trans, RMatrix_a, RMatrix_a_cross_Strut;
double Jacobian[NUM_STRUTS][NUM_STRUTS];
double InverseJacobian[NUM_STRUTS][NUM_STRUTS];
double InvKinStrutLength, StrutLengthDiff[NUM_STRUTS];
double delta[NUM_STRUTS];
double conv_err = 1.0;
double corr;
PmRotationMatrix RMatrix;
PmRpy q_RPY;
int iterate = 1;
int i;
unsigned iteration = 0;
genhex_read_hal_pins();
/* abort on obvious problems, like joints <= 0 */
/* FIXME-- should check against triangle inequality, so that joints
are never too short to span shared base and platform sides */
if (joints[0] <= 0.0 ||
joints[1] <= 0.0 ||
joints[2] <= 0.0 ||
joints[3] <= 0.0 ||
joints[4] <= 0.0 ||
joints[5] <= 0.0) {
return -1;
}
/* assign a,b,c to roll, pitch, yaw angles */
q_RPY.r = pos->a * PM_PI / 180.0;
q_RPY.p = pos->b * PM_PI / 180.0;
q_RPY.y = pos->c * PM_PI / 180.0;
/* Assign translation values in pos to q_trans */
q_trans.x = pos->tran.x;
q_trans.y = pos->tran.y;
q_trans.z = pos->tran.z;
/* Enter Newton-Raphson iterative method */
while (iterate) {
/* check for large error and return error flag if no convergence */
if ((conv_err > +(*haldata->max_error)) ||
(conv_err < -(*haldata->max_error))) {
/* we can't converge */
*haldata->fwd_kins_fail = 1;
return -2;
};
iteration++;
/* check iteration to see if the kinematics can reach the
convergence criterion and return error flag if it can't */
if (iteration > *haldata->iter_limit) {
/* we can't converge */
*haldata->fwd_kins_fail = 1;
return -5;
}
/* Convert q_RPY to Rotation Matrix */
pmRpyMatConvert(&q_RPY, &RMatrix);
/* compute StrutLengthDiff[] by running inverse kins on Cartesian
estimate to get joint estimate, subtract joints to get joint deltas,
and compute inv J while we're at it */
for (i = 0; i < NUM_STRUTS; i++) {
pmMatCartMult(&RMatrix, &a[i], &RMatrix_a);
pmCartCartAdd(&q_trans, &RMatrix_a, &aw);
pmCartCartSub(&aw, &b[i], &InvKinStrutVect);
if (0 != pmCartUnit(&InvKinStrutVect, &InvKinStrutVectUnit)) {
*haldata->fwd_kins_fail = 1;
return -1;
}
pmCartMag(&InvKinStrutVect, &InvKinStrutLength);
if (*haldata->screw_lead != 0.0) {
/* enable strut length correction */
StrutLengthCorrection(&InvKinStrutVectUnit, &RMatrix, i, &corr);
/* define corrected joint lengths */
InvKinStrutLength += corr;
}
StrutLengthDiff[i] = InvKinStrutLength - joints[i];
/* Determine RMatrix_a_cross_strut */
pmCartCartCross(&RMatrix_a, &InvKinStrutVectUnit, &RMatrix_a_cross_Strut);
/* Build Inverse Jacobian Matrix */
InverseJacobian[i][0] = InvKinStrutVectUnit.x;
InverseJacobian[i][1] = InvKinStrutVectUnit.y;
InverseJacobian[i][2] = InvKinStrutVectUnit.z;
InverseJacobian[i][3] = RMatrix_a_cross_Strut.x;
InverseJacobian[i][4] = RMatrix_a_cross_Strut.y;
InverseJacobian[i][5] = RMatrix_a_cross_Strut.z;
}
/* invert Inverse Jacobian */
MatInvert(InverseJacobian, Jacobian);
/* multiply Jacobian by LegLengthDiff */
MatMult(Jacobian, StrutLengthDiff, delta);
/* subtract delta from last iterations pos values */
q_trans.x -= delta[0];
q_trans.y -= delta[1];
q_trans.z -= delta[2];
q_RPY.r -= delta[3];
q_RPY.p -= delta[4];
q_RPY.y -= delta[5];
/* determine value of conv_error (used to determine if no convergence) */
conv_err = 0.0;
for (i = 0; i < NUM_STRUTS; i++) {
conv_err += fabs(StrutLengthDiff[i]);
}
/* enter loop to determine if a strut needs another iteration */
iterate = 0; /*assume iteration is done */
for (i = 0; i < NUM_STRUTS; i++) {
if (fabs(StrutLengthDiff[i]) > *haldata->conv_criterion) {
iterate = 1;
}
}
} /* exit Newton-Raphson Iterative loop */
/* assign r,p,y to a,b,c */
pos->a = q_RPY.r * 180.0 / PM_PI;
pos->b = q_RPY.p * 180.0 / PM_PI;
pos->c = q_RPY.y * 180.0 / PM_PI;
/* assign q_trans to pos */
pos->tran.x = q_trans.x;
pos->tran.y = q_trans.y;
pos->tran.z = q_trans.z;
*haldata->last_iter = iteration;
if (iteration > *haldata->max_iter){
*haldata->max_iter = iteration;
}
*haldata->fwd_kins_fail = 0;
genhex_gui_forward_kins(pos);
return 0;
} // genhexKinematicsForward()
/************************ genhexKinematicsInverse() ************************/
/* the inverse kinematics take world coordinates and determine joint values,
given the inverse kinematics flags to resolve any ambiguities. The forward
flags are set to indicate their value appropriate to the world coordinates
passed in. */
static int genhexKinematicsInverse(const EmcPose * pos,
double * joints,
const KINEMATICS_INVERSE_FLAGS * iflags,
KINEMATICS_FORWARD_FLAGS * fflags)
{
(void)iflags;
(void)fflags;
PmCartesian aw, temp;
PmCartesian InvKinStrutVect, InvKinStrutVectUnit;
PmRotationMatrix RMatrix;
PmRpy rpy;
int i;
double InvKinStrutLength, corr;
genhex_read_hal_pins();
/* define Rotation Matrix */
rpy.r = pos->a * PM_PI / 180.0;
rpy.p = pos->b * PM_PI / 180.0;
rpy.y = pos->c * PM_PI / 180.0;
pmRpyMatConvert(&rpy, &RMatrix);
/* enter for loop to calculate joints (strut lengths) */
for (i = 0; i < NUM_STRUTS; i++) {
/* convert location of platform strut end from platform
to world coordinates */
pmMatCartMult(&RMatrix, &a[i], &temp);
pmCartCartAdd(&pos->tran, &temp, &aw);
/* define strut lengths */
pmCartCartSub(&aw, &b[i], &InvKinStrutVect);
pmCartMag(&InvKinStrutVect, &InvKinStrutLength);
if (*haldata->screw_lead != 0.0) {
/* enable strut length correction */
/* define unit strut vector */
if (0 != pmCartUnit(&InvKinStrutVect, &InvKinStrutVectUnit)) {
return -1;
}
/* define correction value and corrected joint lengths */
StrutLengthCorrection(&InvKinStrutVectUnit, &RMatrix, i, &corr);
*haldata->correction[i] = corr;
InvKinStrutLength += corr;
}
joints[i] = InvKinStrutLength;
}
return 0;
} //genhexKinematicsInverse()
static
int genhexKinematicsSetup(const int comp_id,
const char* coordinates,
kparms* kp)
{
(void)coordinates;
int i,res=0;
haldata = hal_malloc(sizeof(struct haldata));
if (!haldata) {
rtapi_print_msg(RTAPI_MSG_ERR,"genhexKinematicsSetup: hal_malloc fail\n");
return -1;
}
for (i = 0; i < kp->max_joints; i++) {
res += hal_pin_float_newf(HAL_IN, &(haldata->basex[i]), comp_id,
"%s.base.%d.x", kp->halprefix, i);
res += hal_pin_float_newf(HAL_IN, &haldata->basey[i], comp_id,
"%s.base.%d.y", kp->halprefix, i);
res += hal_pin_float_newf(HAL_IN, &haldata->basez[i], comp_id,
"%s.base.%d.z", kp->halprefix, i);
res += hal_pin_float_newf(HAL_IN, &haldata->platformx[i], comp_id,
"%s.platform.%d.x", kp->halprefix, i);
res += hal_pin_float_newf(HAL_IN, &haldata->platformy[i], comp_id,
"%s.platform.%d.y", kp->halprefix, i);
res += hal_pin_float_newf(HAL_IN, &haldata->platformz[i], comp_id,
"%s.platform.%d.z", kp->halprefix, i);
res += hal_pin_float_newf(HAL_IN, &haldata->basenx[i], comp_id,
"%s.base-n.%d.x", kp->halprefix, i);
res += hal_pin_float_newf(HAL_IN, &haldata->baseny[i], comp_id,
"%s.base-n.%d.y", kp->halprefix, i);
res += hal_pin_float_newf(HAL_IN, &haldata->basenz[i], comp_id,
"%s.base-n.%d.z", kp->halprefix, i);
res += hal_pin_float_newf(HAL_IN, &haldata->platformnx[i], comp_id,
"%s.platform-n.%d.x", kp->halprefix, i);
res += hal_pin_float_newf(HAL_IN, &haldata->platformny[i], comp_id,
"%s.platform-n.%d.y", kp->halprefix, i);
res += hal_pin_float_newf(HAL_IN, &haldata->platformnz[i], comp_id,
"%s.platform-n.%d.z", kp->halprefix, i);
res += hal_pin_float_newf(HAL_OUT, &haldata->correction[i], comp_id,
"%s.correction.%d", kp->halprefix, i);
if (res) {goto error;}
*haldata->correction[i] = 0.0;
}
res += hal_pin_u32_newf(HAL_OUT, &haldata->last_iter, comp_id,
"genhexkins.last-iterations");
*haldata->last_iter = 0;
res += hal_pin_u32_newf(HAL_OUT, &haldata->max_iter, comp_id,
"genhexkins.max-iterations");
*haldata->max_iter = 0;
res += hal_pin_float_newf(HAL_IN, &haldata->max_error, comp_id,
"genhexkins.max-error");
*haldata->max_error = 500.0;
res += hal_pin_float_newf(HAL_IN, &haldata->conv_criterion, comp_id,
"genhexkins.convergence-criterion");
*haldata->conv_criterion = 1e-9;
res += hal_pin_u32_newf(HAL_IN, &haldata->iter_limit, comp_id,
"genhexkins.limit-iterations");
*haldata->iter_limit = 120;
res += hal_pin_float_newf(HAL_IN, &haldata->tool_offset, comp_id,
"genhexkins.tool-offset");
*haldata->tool_offset = 0.0;
res += hal_pin_float_newf(HAL_IN, &haldata->spindle_offset, comp_id,
"genhexkins.spindle-offset");
*haldata->spindle_offset = 0.0;
res += hal_pin_float_newf(HAL_IN, &haldata->screw_lead, comp_id,
"genhexkins.screw-lead");
*haldata->screw_lead = DEFAULT_SCREW_LEAD;
if (res) {goto error;}
*haldata->basex[0] = DEFAULT_BASE_0_X;
*haldata->basey[0] = DEFAULT_BASE_0_Y;
*haldata->basez[0] = DEFAULT_BASE_0_Z;
*haldata->basex[1] = DEFAULT_BASE_1_X;
*haldata->basey[1] = DEFAULT_BASE_1_Y;
*haldata->basez[1] = DEFAULT_BASE_1_Z;
*haldata->basex[2] = DEFAULT_BASE_2_X;
*haldata->basey[2] = DEFAULT_BASE_2_Y;
*haldata->basez[2] = DEFAULT_BASE_2_Z;
*haldata->basex[3] = DEFAULT_BASE_3_X;
*haldata->basey[3] = DEFAULT_BASE_3_Y;
*haldata->basez[3] = DEFAULT_BASE_3_Z;
*haldata->basex[4] = DEFAULT_BASE_4_X;
*haldata->basey[4] = DEFAULT_BASE_4_Y;
*haldata->basez[4] = DEFAULT_BASE_4_Z;
*haldata->basex[5] = DEFAULT_BASE_5_X;
*haldata->basey[5] = DEFAULT_BASE_5_Y;
*haldata->basez[5] = DEFAULT_BASE_5_Z;
*haldata->platformx[0] = DEFAULT_PLATFORM_0_X;
*haldata->platformy[0] = DEFAULT_PLATFORM_0_Y;
*haldata->platformz[0] = DEFAULT_PLATFORM_0_Z;
*haldata->platformx[1] = DEFAULT_PLATFORM_1_X;
*haldata->platformy[1] = DEFAULT_PLATFORM_1_Y;
*haldata->platformz[1] = DEFAULT_PLATFORM_1_Z;
*haldata->platformx[2] = DEFAULT_PLATFORM_2_X;
*haldata->platformy[2] = DEFAULT_PLATFORM_2_Y;
*haldata->platformz[2] = DEFAULT_PLATFORM_2_Z;
*haldata->platformx[3] = DEFAULT_PLATFORM_3_X;
*haldata->platformy[3] = DEFAULT_PLATFORM_3_Y;
*haldata->platformz[3] = DEFAULT_PLATFORM_3_Z;
*haldata->platformx[4] = DEFAULT_PLATFORM_4_X;
*haldata->platformy[4] = DEFAULT_PLATFORM_4_Y;
*haldata->platformz[4] = DEFAULT_PLATFORM_4_Z;
*haldata->platformx[5] = DEFAULT_PLATFORM_5_X;
*haldata->platformy[5] = DEFAULT_PLATFORM_5_Y;
*haldata->platformz[5] = DEFAULT_PLATFORM_5_Z;
*haldata->basenx[0] = DEFAULT_BASE_0_NX;
*haldata->baseny[0] = DEFAULT_BASE_0_NY;
*haldata->basenz[0] = DEFAULT_BASE_0_NZ;
*haldata->basenx[1] = DEFAULT_BASE_1_NX;
*haldata->baseny[1] = DEFAULT_BASE_1_NY;
*haldata->basenz[1] = DEFAULT_BASE_1_NZ;
*haldata->basenx[2] = DEFAULT_BASE_2_NX;
*haldata->baseny[2] = DEFAULT_BASE_2_NY;
*haldata->basenz[2] = DEFAULT_BASE_2_NZ;
*haldata->basenx[3] = DEFAULT_BASE_3_NX;
*haldata->baseny[3] = DEFAULT_BASE_3_NY;
*haldata->basenz[3] = DEFAULT_BASE_3_NZ;
*haldata->basenx[4] = DEFAULT_BASE_4_NX;
*haldata->baseny[4] = DEFAULT_BASE_4_NY;
*haldata->basenz[4] = DEFAULT_BASE_4_NZ;
*haldata->basenx[5] = DEFAULT_BASE_5_NX;
*haldata->baseny[5] = DEFAULT_BASE_5_NY;
*haldata->basenz[5] = DEFAULT_BASE_5_NZ;
*haldata->platformnx[0] = DEFAULT_PLATFORM_0_NX;
*haldata->platformny[0] = DEFAULT_PLATFORM_0_NY;
*haldata->platformnz[0] = DEFAULT_PLATFORM_0_NZ;
*haldata->platformnx[1] = DEFAULT_PLATFORM_1_NX;
*haldata->platformny[1] = DEFAULT_PLATFORM_1_NY;
*haldata->platformnz[1] = DEFAULT_PLATFORM_1_NZ;
*haldata->platformnx[2] = DEFAULT_PLATFORM_2_NX;
*haldata->platformny[2] = DEFAULT_PLATFORM_2_NY;
*haldata->platformnz[2] = DEFAULT_PLATFORM_2_NZ;
*haldata->platformnx[3] = DEFAULT_PLATFORM_3_NX;
*haldata->platformny[3] = DEFAULT_PLATFORM_3_NY;
*haldata->platformnz[3] = DEFAULT_PLATFORM_3_NZ;
*haldata->platformnx[4] = DEFAULT_PLATFORM_4_NX;
*haldata->platformny[4] = DEFAULT_PLATFORM_4_NY;
*haldata->platformnz[4] = DEFAULT_PLATFORM_4_NZ;
*haldata->platformnx[5] = DEFAULT_PLATFORM_5_NX;
*haldata->platformny[5] = DEFAULT_PLATFORM_5_NY;
*haldata->platformnz[5] = DEFAULT_PLATFORM_5_NZ;
//note: switchkins does not uses these as it provides gui.x, gui.y, etc.
res += hal_pin_float_newf(HAL_IN, &haldata->gui_x, comp_id, "genhexkins.x");
res += hal_pin_float_newf(HAL_IN, &haldata->gui_y, comp_id, "genhexkins.y");
res += hal_pin_float_newf(HAL_IN, &haldata->gui_z, comp_id, "genhexkins.z");
res += hal_pin_float_newf(HAL_IN, &haldata->gui_a, comp_id, "genhexkins.a");
res += hal_pin_float_newf(HAL_IN, &haldata->gui_b, comp_id, "genhexkins.b");
res += hal_pin_float_newf(HAL_IN, &haldata->gui_c, comp_id, "genhexkins.c");
res += hal_pin_bit_newf(HAL_OUT, &haldata->fwd_kins_fail, comp_id,
"genhexkins.fwd-kins-fail");
if (res) goto error;
return 0;
error:
return res;
} // genhexKinematicsSetup()
int switchkinsSetup(kparms* kp,
KS* kset0, KS* kset1, KS* kset2,
KF* kfwd0, KF* kfwd1, KF* kfwd2,
KI* kinv0, KI* kinv1, KI* kinv2
)
{
kp->kinsname = "genhexkins"; // !!! must agree with filename
kp->halprefix = "genhexkins"; // hal pin names
kp->required_coordinates = "xyzabc";
kp->max_joints = strlen(kp->required_coordinates);
kp->allow_duplicates = 0;
kp->fwd_iterates_mask = 0x1; //genhexkins switchkins_type==0
kp->gui_kinstype = 0; //vismach gui for switchkins_type==0
// switchkins_type==0 is startup default
// kins with iterative forward algorithm should be switchkins_type==0
*kset0 = genhexKinematicsSetup;
*kfwd0 = genhexKinematicsForward;
*kinv0 = genhexKinematicsInverse;
*kset1 = identityKinematicsSetup;
*kfwd1 = identityKinematicsForward;
*kinv1 = identityKinematicsInverse;
*kset2 = userkKinematicsSetup;
*kfwd2 = userkKinematicsForward;
*kinv2 = userkKinematicsInverse;
return 0;
} //switchkinsSetup()

131
wasm-port/vendor/linuxcnc/src/emc/kinematics/genhexkins.h vendored Normal file
View File

@@ -0,0 +1,131 @@
/*******************************************************************
* Description: genhexkins.h
*
* Kinematics for a generalised hexapod machine
*
* Derived from a work by R. Brian Register
*
* Adapting Author: Andrew Kyrychenko
* License: GPL Version 2
* System: Linux
*
* Copyright (c) 2004 All rights reserved.
********************************************************************
This is the header file to accompany genhexkins.c. This header file
is used to configure genhexkins.c to solve the kinematics for a default
Stewart Platform configuration.
*/
#ifndef GENHEXKINS_H
#define GENHEXKINS_H
#include <kinematics.h>
#include <hal.h>
#define GENHEX_MAX_JOINTS 6
#define NUM_STRUTS 6 // number of struts, only 6 supported for now
/* Default position of base strut ends in base (world) coordinates */
#define DEFAULT_BASE_0_X -22.950
#define DEFAULT_BASE_1_X 22.950
#define DEFAULT_BASE_2_X 22.950
#define DEFAULT_BASE_3_X 0.000
#define DEFAULT_BASE_4_X 0.000
#define DEFAULT_BASE_5_X -22.950
#define DEFAULT_BASE_0_Y 13.250
#define DEFAULT_BASE_1_Y 13.250
#define DEFAULT_BASE_2_Y 13.250
#define DEFAULT_BASE_3_Y -26.500
#define DEFAULT_BASE_4_Y -26.500
#define DEFAULT_BASE_5_Y 13.250
#define DEFAULT_BASE_0_Z 0.000
#define DEFAULT_BASE_1_Z 0.000
#define DEFAULT_BASE_2_Z 0.000
#define DEFAULT_BASE_3_Z 0.000
#define DEFAULT_BASE_4_Z 0.000
#define DEFAULT_BASE_5_Z 0.000
/* Default position of platform strut end in platform coordinate system */
#define DEFAULT_PLATFORM_0_X -1.000
#define DEFAULT_PLATFORM_1_X 1.000
#define DEFAULT_PLATFORM_2_X 10.459
#define DEFAULT_PLATFORM_3_X 9.459
#define DEFAULT_PLATFORM_4_X -9.459
#define DEFAULT_PLATFORM_5_X -10.459
#define DEFAULT_PLATFORM_0_Y 11.500
#define DEFAULT_PLATFORM_1_Y 11.500
#define DEFAULT_PLATFORM_2_Y -4.884
#define DEFAULT_PLATFORM_3_Y -6.616
#define DEFAULT_PLATFORM_4_Y -6.616
#define DEFAULT_PLATFORM_5_Y -4.884
#define DEFAULT_PLATFORM_0_Z 0.000
#define DEFAULT_PLATFORM_1_Z 0.000
#define DEFAULT_PLATFORM_2_Z 0.000
#define DEFAULT_PLATFORM_3_Z 0.000
#define DEFAULT_PLATFORM_4_Z 0.000
#define DEFAULT_PLATFORM_5_Z 0.000
/* Default unit vectors of base joints axes in world coordinates */
#define DEFAULT_BASE_0_NX 0.707107
#define DEFAULT_BASE_0_NY 0.0
#define DEFAULT_BASE_0_NZ 0.707107
#define DEFAULT_BASE_1_NX 0.0
#define DEFAULT_BASE_1_NY -0.707107
#define DEFAULT_BASE_1_NZ 0.707107
#define DEFAULT_BASE_2_NX -0.707107
#define DEFAULT_BASE_2_NY 0.0
#define DEFAULT_BASE_2_NZ 0.707107
#define DEFAULT_BASE_3_NX -0.707107
#define DEFAULT_BASE_3_NY 0.0
#define DEFAULT_BASE_3_NZ 0.707107
#define DEFAULT_BASE_4_NX 0.0
#define DEFAULT_BASE_4_NY 0.707107
#define DEFAULT_BASE_4_NZ 0.707107
#define DEFAULT_BASE_5_NX 0.707107
#define DEFAULT_BASE_5_NY 0.0
#define DEFAULT_BASE_5_NZ 0.707107
/* Default unit vectors of platform joints axes in platform coordinates */
#define DEFAULT_PLATFORM_0_NX -1.0
#define DEFAULT_PLATFORM_0_NY 0.0
#define DEFAULT_PLATFORM_0_NZ 0.0
#define DEFAULT_PLATFORM_1_NX 0.866025
#define DEFAULT_PLATFORM_1_NY 0.5
#define DEFAULT_PLATFORM_1_NZ 0.0
#define DEFAULT_PLATFORM_2_NX 0.866025
#define DEFAULT_PLATFORM_2_NY 0.5
#define DEFAULT_PLATFORM_2_NZ 0.0
#define DEFAULT_PLATFORM_3_NX 0.866025
#define DEFAULT_PLATFORM_3_NY -0.5
#define DEFAULT_PLATFORM_3_NZ 0.0
#define DEFAULT_PLATFORM_4_NX 0.866025
#define DEFAULT_PLATFORM_4_NY -0.5
#define DEFAULT_PLATFORM_4_NZ 0.0
#define DEFAULT_PLATFORM_5_NX -1.0
#define DEFAULT_PLATFORM_5_NY 0.0
#define DEFAULT_PLATFORM_5_NZ 0.0
/* Default lead of strut actuator screw */
#define DEFAULT_SCREW_LEAD 0.0
#endif

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

@@ -0,0 +1,668 @@
/********************************************************************
* Description: genserfuncs.c
* (originally part of genserkins.c)
* Kinematics for a generalised serial kinematics machine
*
* Derived from a work by Fred Proctor,
* changed to work with emc2 and HAL
*
* Adapting Author: Alex Joni
* License: GPL Version 2
* System: Linux
*
* Users:
* 1) genserkins.c -- kinematics modules
* 2) ugenserkins.c -- usermode test program (needs work)
*******************************************************************
These are the forward and inverse kinematic functions for a general
serial-link manipulator. Thanks to Herman Bruyninckx and John
Hallam at http://www.roble.info/ for this.
The functions are general enough to be configured for any serial
configuration.
The kinematics use Denavit-Hartenberg definition for the joint and
links. The DH definitions are the ones used by John J Craig in
"Introduction to Robotics: Mechanics and Control"
The parameters for the manipulator are defined by hal pins.
Currently the type of the joints is hardcoded to ANGULAR, although
the kins support both ANGULAR and LINEAR axes.
TODO:
* make number of joints a loadtime parameter
* add HAL pins for all settable parameters, including joint type: ANGULAR / LINEAR
*/
#ifdef RTAPI
#include <rtapi.h>
#endif
#include <rtapi_math.h>
#include <hal.h>
#include <gotypes.h> /* go_result, go_integer */
#include <gomath.h> /* go_pose */
#include <kinematics.h>
#include "genserkins.h" /* these decls */
// Only gcc/g++ supports the #pragma
#if __GNUC__ && !defined(__clang__)
// The matrix and vector storage is just big.
// genser_kin_jac_inv() is 2112
// genserKinematicsInverse() is 2576
#pragma GCC diagnostic warning "-Wframe-larger-than=2600"
#endif
static struct haldata {
hal_u32_t *max_iterations;
hal_u32_t *last_iterations;
hal_float_t *a[GENSER_MAX_JOINTS];
hal_float_t *alpha[GENSER_MAX_JOINTS];
hal_float_t *d[GENSER_MAX_JOINTS];
hal_s32_t *unrotate[GENSER_MAX_JOINTS];
genser_struct *kins;
go_pose *pos; // used in various functions, we malloc it
// only once in genserKinematicsSetup()
} *haldata = 0;
static int total_joints;
double j[GENSER_MAX_JOINTS];
#define A(i) (*(haldata->a[i]))
#define ALPHA(i) (*(haldata->alpha[i]))
#define D(i) (*(haldata->d[i]))
#define KINS_PTR (haldata->kins)
#if GENSER_MAX_JOINTS < 6
#error GENSER_MAX_JOINTS must be at least 6; fix genserkins.h
#endif
static int genser_hal_inited = 0;
int genser_kin_init(void) {
genser_struct *genser = KINS_PTR;
int t;
static volatile double tst=0;tst=sqrt(tst); // ensure -lm used
/* init them all and make them revolute joints */
/* FIXME: should allow LINEAR joints based on HAL param too */
for (t = 0; t < GENSER_MAX_JOINTS; t++) {
genser->links[t].u.dh.a = A(t);
genser->links[t].u.dh.alpha = ALPHA(t);
genser->links[t].u.dh.d = D(t);
genser->links[t].u.dh.theta = 0;
genser->links[t].type = GO_LINK_DH;
genser->links[t].quantity = GO_QUANTITY_ANGLE;
}
/* set a select few to make it PUMA-like */
// FIXME-AJ: make a hal pin, also set number of joints based on it
genser->link_num = 6;
return GO_RESULT_OK;
}
/* compute the forward jacobian function:
the jacobian is a linear approximation of the kinematics function.
It is calculated using derivation of the position transformation matrix,
and usually used for feeding velocities through it.
It is analytically possible to calculate the inverse of the jacobian
(sometimes only the pseudoinverse) and to use that for the inverse kinematics.
*/
int compute_jfwd(go_link * link_params,
int link_number,
go_matrix * Jfwd,
go_pose * T_L_0)
{
GO_MATRIX_DECLARE(Jv, Jvstg, 3, GENSER_MAX_JOINTS);
GO_MATRIX_DECLARE(Jw, Jwstg, 3, GENSER_MAX_JOINTS);
GO_MATRIX_DECLARE(R_i_ip1, R_i_ip1stg, 3, 3);
GO_MATRIX_DECLARE(scratch, scratchstg, 3, GENSER_MAX_JOINTS);
GO_MATRIX_DECLARE(R_inv, R_invstg, 3, 3);
go_pose pose;
go_quat quat;
go_vector P_ip1_i[3];
int row, col;
/* init matrices to possibly smaller size */
go_matrix_init(Jv, Jvstg, 3, link_number);
go_matrix_init(Jw, Jwstg, 3, link_number);
go_matrix_init(R_i_ip1, R_i_ip1stg, 3, 3);
go_matrix_init(scratch, scratchstg, 3, link_number);
go_matrix_init(R_inv, R_invstg, 3, 3);
Jv.el[0][0] = 0, Jv.el[1][0] = 0, Jv.el[2][0] = (GO_QUANTITY_LENGTH == link_params[0].quantity ? 1 : 0);
Jw.el[0][0] = 0, Jw.el[1][0] = 0, Jw.el[2][0] = (GO_QUANTITY_ANGLE == link_params[0].quantity ? 1 : 0);
/* initialize inverse rotational transform */
if (GO_LINK_DH == link_params[0].type) {
go_dh_pose_convert(&link_params[0].u.dh, &pose);
} else if (GO_LINK_PP == link_params[0].type) {
pose = link_params[0].u.pp.pose;
} else {
return GO_RESULT_IMPL_ERROR;
}
*T_L_0 = pose;
for (col = 1; col < link_number; col++) {
/* T_ip1_i */
if (GO_LINK_DH == link_params[col].type) {
go_dh_pose_convert(&link_params[col].u.dh, &pose);
} else if (GO_LINK_PP == link_params[col].type) {
pose = link_params[col].u.pp.pose;
} else {
return GO_RESULT_IMPL_ERROR;
}
go_cart_vector_convert(&pose.tran, P_ip1_i);
go_quat_inv(&pose.rot, &quat);
go_quat_matrix_convert(&quat, &R_i_ip1);
/* Jv */
go_matrix_vector_cross(&Jw, P_ip1_i, &scratch);
go_matrix_matrix_add(&Jv, &scratch, &scratch);
go_matrix_matrix_mult(&R_i_ip1, &scratch, &Jv);
Jv.el[0][col] = 0, Jv.el[1][col] = 0, Jv.el[2][col] = (GO_QUANTITY_LENGTH == link_params[col].quantity ? 1 : 0);
/* Jw */
go_matrix_matrix_mult(&R_i_ip1, &Jw, &Jw);
Jw.el[0][col] = 0, Jw.el[1][col] = 0, Jw.el[2][col] = (GO_QUANTITY_ANGLE == link_params[col].quantity ? 1 : 0);
if (GO_LINK_DH == link_params[col].type) {
go_dh_pose_convert(&link_params[col].u.dh, &pose);
} else if (GO_LINK_PP == link_params[col].type) {
pose = link_params[col].u.pp.pose;
} else {
return GO_RESULT_IMPL_ERROR;
}
go_pose_pose_mult(T_L_0, &pose, T_L_0);
}
/* rotate back into {0} frame */
go_quat_matrix_convert(&T_L_0->rot, &R_inv);
go_matrix_matrix_mult(&R_inv, &Jv, &Jv);
go_matrix_matrix_mult(&R_inv, &Jw, &Jw);
/* put Jv atop Jw in J */
for (row = 0; row < 6; row++) {
for (col = 0; col < link_number; col++) {
if (row < 3) {
Jfwd->el[row][col] = Jv.el[row][col];
} else {
Jfwd->el[row][col] = Jw.el[row - 3][col];
}
}
}
return GO_RESULT_OK;
}
/* compute the inverse of the jacobian matrix */
int compute_jinv(go_matrix * Jfwd, go_matrix * Jinv)
{
int retval;
GO_MATRIX_DECLARE(JT, JTstg, GENSER_MAX_JOINTS, 6);
/* compute inverse, or pseudo-inverse */
if (Jfwd->rows == Jfwd->cols) {
retval = go_matrix_inv(Jfwd, Jinv);
if (GO_RESULT_OK != retval)
return retval;
} else if (Jfwd->rows < Jfwd->cols) {
/* underdetermined, optimize on smallest sum of square of speeds */
/* JT(JJT)inv */
GO_MATRIX_DECLARE(JJT, JJTstg, 6, 6);
go_matrix_init(JT, JTstg, Jfwd->cols, Jfwd->rows);
go_matrix_init(JJT, JJTstg, Jfwd->rows, Jfwd->rows);
go_matrix_transpose(Jfwd, &JT);
go_matrix_matrix_mult(Jfwd, &JT, &JJT);
retval = go_matrix_inv(&JJT, &JJT);
if (GO_RESULT_OK != retval)
return retval;
go_matrix_matrix_mult(&JT, &JJT, Jinv);
} else {
/* overdetermined, do least-squares best fit */
/* (JTJ)invJT */
GO_MATRIX_DECLARE(JTJ, JTJstg, GENSER_MAX_JOINTS, GENSER_MAX_JOINTS);
go_matrix_init(JT, JTstg, Jfwd->cols, Jfwd->rows);
go_matrix_init(JTJ, JTJstg, Jfwd->cols, Jfwd->cols);
go_matrix_transpose(Jfwd, &JT);
go_matrix_matrix_mult(&JT, Jfwd, &JTJ);
retval = go_matrix_inv(&JTJ, &JTJ);
if (GO_RESULT_OK != retval)
return retval;
go_matrix_matrix_mult(&JTJ, &JT, Jinv);
}
return GO_RESULT_OK;
}
int genser_kin_jac_inv(void *kins,
const go_pose * pos,
const go_screw * vel, const go_real * joints, go_real * jointvels)
{
(void)pos;
genser_struct *genser = (genser_struct *) kins;
GO_MATRIX_DECLARE(Jfwd, Jfwd_stg, 6, GENSER_MAX_JOINTS);
GO_MATRIX_DECLARE(Jinv, Jinv_stg, GENSER_MAX_JOINTS, 6);
go_pose T_L_0;
go_link linkout[GENSER_MAX_JOINTS] = {};
go_real vw[6];
int link;
int retval;
go_matrix_init(Jfwd, Jfwd_stg, 6, genser->link_num);
go_matrix_init(Jinv, Jinv_stg, GENSER_MAX_JOINTS, 6);
for (link = 0; link < genser->link_num; link++) {
retval =
go_link_joint_set(&genser->links[link], joints[link],
&linkout[link]);
if (GO_RESULT_OK != retval)
return retval;
}
retval = compute_jfwd(linkout, genser->link_num, &Jfwd, &T_L_0);
if (GO_RESULT_OK != retval)
return retval;
retval = compute_jinv(&Jfwd, &Jinv);
if (GO_RESULT_OK != retval)
return retval;
vw[0] = vel->v.x;
vw[1] = vel->v.y;
vw[2] = vel->v.z;
vw[3] = vel->w.x;
vw[4] = vel->w.y;
vw[5] = vel->w.z;
return go_matrix_vector_mult(&Jinv, vw, jointvels);
}
int genser_kin_jac_fwd(void *kins,
const go_real * joints,
const go_real * jointvels, const go_pose * pos, go_screw * vel)
{
(void)pos;
genser_struct *genser = (genser_struct *) kins;
GO_MATRIX_DECLARE(Jfwd, Jfwd_stg, 6, GENSER_MAX_JOINTS);
go_pose T_L_0;
go_link linkout[GENSER_MAX_JOINTS] = {};
go_real vw[6];
int link;
int retval;
go_matrix_init(Jfwd, Jfwd_stg, 6, genser->link_num);
for (link = 0; link < genser->link_num; link++) {
retval =
go_link_joint_set(&genser->links[link], joints[link],
&linkout[link]);
if (GO_RESULT_OK != retval)
return retval;
}
retval = compute_jfwd(linkout, genser->link_num, &Jfwd, &T_L_0);
if (GO_RESULT_OK != retval)
return retval;
go_matrix_vector_mult(&Jfwd, jointvels, vw);
vel->v.x = vw[0];
vel->v.y = vw[1];
vel->v.z = vw[2];
vel->w.x = vw[3];
vel->w.y = vw[4];
vel->w.z = vw[5];
return GO_RESULT_OK;
}
/* main function called by emc2 for forward Kins */
int genserKinematicsForward(const double *joint,
EmcPose * world,
const KINEMATICS_FORWARD_FLAGS * fflags,
KINEMATICS_INVERSE_FLAGS * iflags) {
(void)fflags;
(void)iflags;
go_pose *pos;
go_rpy rpy;
go_real jcopy[GENSER_MAX_JOINTS]; // will hold the radian conversion of joints
int ret = 0;
int i, changed=0;
if (!genser_hal_inited) {
rtapi_print_msg(RTAPI_MSG_ERR,
"genserKinematicsForward: not initialized\n");
return -1;
}
for (i=0; i< 6; i++) {
// FIXME - debug hack
if (!GO_ROT_CLOSE(j[i],joint[i])) changed = 1;
// convert to radians to pass to genser_kin_fwd
jcopy[i] = joint[i] * PM_PI / 180;
if ((i) && *(haldata->unrotate[i]))
jcopy[i] -= *(haldata->unrotate[i])*jcopy[i-1];
}
if (changed) {
for (i=0; i< 6; i++)
j[i] = joint[i];
// rtapi_print("genserKinematicsForward(joints: %f %f %f %f %f %f)\n",
//joint[0],joint[1],joint[2],joint[3],joint[4],joint[5]);
}
// AJ: convert from emc2 coords (XYZABC - which are actually rpy euler
// angles)
// to go angles (quaternions)
pos = haldata->pos;
rpy.y = world->c * PM_PI / 180;
rpy.p = world->b * PM_PI / 180;
rpy.r = world->a * PM_PI / 180;
go_rpy_quat_convert(&rpy, &pos->rot);
pos->tran.x = world->tran.x;
pos->tran.y = world->tran.y;
pos->tran.z = world->tran.z;
//pass through unused 678 as uvw
if (total_joints > 6) world->u = joint[6];
if (total_joints > 7) world->v = joint[7];
if (total_joints > 8) world->w = joint[8];
// pos will be the world location
// jcopy: joitn position in radians
ret = genser_kin_fwd(KINS_PTR, jcopy, pos);
if (ret < 0)
return ret;
// AJ: convert back to emc2 coords
ret = go_quat_rpy_convert(&pos->rot, &rpy);
if (ret < 0)
return ret;
world->tran.x = pos->tran.x;
world->tran.y = pos->tran.y;
world->tran.z = pos->tran.z;
world->a = rpy.r * 180 / PM_PI;
world->b = rpy.p * 180 / PM_PI;
world->c = rpy.y * 180 / PM_PI;
if (changed) {
// rtapi_print("genserKinematicsForward(world: %f %f %f %f %f %f)\n", world->tran.x, world->tran.y, world->tran.z, world->a, world->b, world->c);
}
return 0;
}
int genser_kin_fwd(void *kins, const go_real * joints, go_pose * pos)
{
genser_struct *genser = kins;
go_link linkout[GENSER_MAX_JOINTS] = {};
int link;
int retval;
genser_kin_init();
for (link = 0; link < genser->link_num; link++) {
retval = go_link_joint_set(&genser->links[link], joints[link], &linkout[link]);
if (GO_RESULT_OK != retval)
return retval;
}
retval = go_link_pose_build(linkout, genser->link_num, pos);
if (GO_RESULT_OK != retval)
return retval;
return GO_RESULT_OK;
}
int genserKinematicsInverse(const EmcPose * world,
double *joints,
const KINEMATICS_INVERSE_FLAGS * iflags,
KINEMATICS_FORWARD_FLAGS * fflags)
{
(void)iflags;
(void)fflags;
genser_struct *genser = KINS_PTR;
GO_MATRIX_DECLARE(Jfwd, Jfwd_stg, 6, GENSER_MAX_JOINTS);
GO_MATRIX_DECLARE(Jinv, Jinv_stg, GENSER_MAX_JOINTS, 6);
go_pose T_L_0;
go_real dvw[6];
go_real jest[GENSER_MAX_JOINTS];
go_real dj[GENSER_MAX_JOINTS];
go_pose pest, pestinv, Tdelta; // pos = converted pose from EmcPose
go_rpy rpy;
go_rvec rvec;
go_cart cart;
go_link linkout[GENSER_MAX_JOINTS];
int link;
int smalls;
int retval;
// rtapi_print("kineInverse(joints: %f %f %f %f %f %f)\n",
// joints[0],joints[1],joints[2],joints[3],joints[4],joints[5]);
// rtapi_print("kineInverse(world: %f %f %f %f %f %f)\n",
// world->tran.x, world->tran.y, world->tran.z, world->a, world->b, world->c);
#ifndef ULAPI
genser_kin_init();
if (!genser_hal_inited) {
rtapi_print_msg(RTAPI_MSG_ERR,
"genserKinematicsInverse: not initialized\n");
return -1;
}
#endif
// FIXME-AJ: rpy or zyx ?
rpy.y = world->c * PM_PI / 180;
rpy.p = world->b * PM_PI / 180;
rpy.r = world->a * PM_PI / 180;
go_rpy_quat_convert(&rpy, &haldata->pos->rot);
haldata->pos->tran.x = world->tran.x;
haldata->pos->tran.y = world->tran.y;
haldata->pos->tran.z = world->tran.z;
go_matrix_init(Jfwd, Jfwd_stg, 6, genser->link_num);
go_matrix_init(Jinv, Jinv_stg, genser->link_num, 6);
/* jest[] is a copy of joints[], which is the joint estimate */
for (link = 0; link < genser->link_num; link++) {
// jest, and the rest of joint related calcs are in radians
jest[link] = joints[link] * (PM_PI / 180);
}
for (genser->iterations = 0;
genser->iterations < *haldata->max_iterations;
genser->iterations++) {
*(haldata->last_iterations) = genser->iterations;
/* update the Jacobians */
for (link = 0; link < genser->link_num; link++) {
go_link_joint_set(&genser->links[link], jest[link], &linkout[link]);
}
retval = compute_jfwd(linkout, genser->link_num, &Jfwd, &T_L_0);
if (GO_RESULT_OK != retval) {
rtapi_print("ERR kI - compute_jfwd (joints: %f %f %f %f %f %f), (iterations=%d)\n",
joints[0],joints[1],joints[2],joints[3],joints[4],joints[5], genser->iterations);
return retval;
}
retval = compute_jinv(&Jfwd, &Jinv);
if (GO_RESULT_OK != retval) {
rtapi_print("ERR kI - compute_jinv (joints: %f %f %f %f %f %f), (iterations=%d)\n",
joints[0],joints[1],joints[2],joints[3],joints[4],joints[5], genser->iterations);
return retval;
}
/* pest is the resulting pose estimate given joint estimate */
genser_kin_fwd(KINS_PTR, jest, &pest);
//printf("jest: %f %f %f %f %f %f\n",jest[0],jest[1],jest[2],jest[3],jest[4],jest[5]);
/* pestinv is its inverse */
go_pose_inv(&pest, &pestinv);
/*
Tdelta is the incremental pose from pest to pos, such that
0 L 0
. pest * Tdelta = pos, or
L L L
L L 0
.Tdelta = pestinv * pos
L 0 L
*/
go_pose_pose_mult(&pestinv, haldata->pos, &Tdelta);
/*
We need Tdelta in 0 frame, not pest frame, so rotate it
back. Since it's effectively a velocity, we just rotate it, and
don't translate it.
*/
/* first rotate the translation differential */
go_quat_cart_mult(&pest.rot, &Tdelta.tran, &cart);
dvw[0] = cart.x;
dvw[1] = cart.y;
dvw[2] = cart.z;
/* to rotate the rotation differential, convert it to a
velocity screw and rotate that */
go_quat_rvec_convert(&Tdelta.rot, &rvec);
cart.x = rvec.x;
cart.y = rvec.y;
cart.z = rvec.z;
go_quat_cart_mult(&pest.rot, &cart, &cart);
dvw[3] = cart.x;
dvw[4] = cart.y;
dvw[5] = cart.z;
/* push the Cartesian velocity vector through the inverse Jacobian */
go_matrix_vector_mult(&Jinv, dvw, dj);
//pass through 678 as uvw
if (total_joints > 6) joints[6] = world->u;
if (total_joints > 7) joints[7] = world->v;
if (total_joints > 8) joints[8] = world->w;
/* check for small joint increments, if so we're done */
for (link = 0, smalls = 0; link < genser->link_num; link++) {
if (GO_QUANTITY_LENGTH == linkout[link].quantity) {
if (GO_TRAN_SMALL(dj[link]))
smalls++;
} else {
if (GO_ROT_SMALL(dj[link]))
smalls++;
}
}
if (smalls == genser->link_num) {
/* converged, copy jest[] out */
for (link = 0; link < genser->link_num; link++) {
// convert from radians back to angles
joints[link] = jest[link] * 180 / PM_PI;
if ((link) && *(haldata->unrotate[link]))
joints[link] += *(haldata->unrotate[link]) * joints[link-1];
}
//rtapi_print("DONEkineInverse(joints: %f %f %f %f %f %f), (iterations=%d)\n",
// joints[0],joints[1],joints[2],joints[3],joints[4],joints[5], genser->iterations);
//rtapi_print("OKkineInverse: %.2f %.2f %.2f %.2f %.2f %.2f)\n",
// world->tran.x, world->tran.y, world->tran.z, world->a, world->b, world->c);
return GO_RESULT_OK;
}
/* else keep iterating */
for (link = 0; link < genser->link_num; link++) {
jest[link] += dj[link]; //still in radians
}
} /* for (iterations) */
rtapi_print("ERRkineInverse(joints: %f %f %f %f %f %f), (iterations=%d)\n",
joints[0],joints[1],joints[2],joints[3],joints[4],joints[5], genser->iterations);
return GO_RESULT_ERROR;
}
/*
Extras, not callable using go_kin_ wrapper but if you know you have
linked in these kinematics, go ahead and call these for your ad hoc
purposes.
*/
int genser_kin_inv_iterations(genser_struct * genser)
{
return genser->iterations;
}
int genser_kin_inv_set_max_iterations(int i)
{
if (i <= 0) return GO_RESULT_ERROR;
*haldata->max_iterations = i;
return GO_RESULT_OK;
}
int genser_kin_inv_get_max_iterations()
{
return *haldata->max_iterations;
}
int genserKinematicsSetup(const int comp_id,
const char* coordinates,
kparms* kp)
{
(void)coordinates;
int i,res=0;
haldata = hal_malloc(sizeof(struct haldata));
if (!haldata) {goto error;}
// allow for pass through joints 6,7,8 u,v,w
total_joints = kp->max_joints;
// only the first 6 joints have A,ALPHA,D,unrotate pins
for (i = 0; i < 6; i++) {
res += hal_pin_float_newf(HAL_IN, &(haldata->a[i]), comp_id,
"%s.A-%d", kp->halprefix, i);
*(haldata->a[i])=0;
res += hal_pin_float_newf(HAL_IN, &(haldata->alpha[i]), comp_id,
"%s.ALPHA-%d", kp->halprefix, i);
*(haldata->alpha[i])=0;
res += hal_pin_float_newf(HAL_IN, &(haldata->d[i]), comp_id,
"%s.D-%d", kp->halprefix, i);
*(haldata->d[i])=0;
res += hal_pin_s32_newf(HAL_IN, &(haldata->unrotate[i]), comp_id,
"%s.unrotate-%d", kp->halprefix, i);
*haldata->unrotate[i]=0;
}
res += hal_pin_u32_newf(HAL_OUT, &(haldata->last_iterations), comp_id,
"%s.last-iterations",kp->halprefix);
KINS_PTR = hal_malloc(sizeof(genser_struct));
haldata->pos = (go_pose *) hal_malloc(sizeof(go_pose));
if (KINS_PTR == NULL) {goto error;}
if (haldata->pos == NULL) {goto error;}
res += hal_pin_u32_newf(HAL_IN, &haldata->max_iterations, comp_id,
"%s.max-iterations",kp->halprefix);
if (res) {goto error;}
*haldata->max_iterations = GENSER_DEFAULT_MAX_ITERATIONS;
A(0) = DEFAULT_A1;
A(1) = DEFAULT_A2;
A(2) = DEFAULT_A3;
A(3) = DEFAULT_A4;
A(4) = DEFAULT_A5;
A(5) = DEFAULT_A6;
ALPHA(0) = DEFAULT_ALPHA1;
ALPHA(1) = DEFAULT_ALPHA2;
ALPHA(2) = DEFAULT_ALPHA3;
ALPHA(3) = DEFAULT_ALPHA4;
ALPHA(4) = DEFAULT_ALPHA5;
ALPHA(5) = DEFAULT_ALPHA6;
D(0) = DEFAULT_D1;
D(1) = DEFAULT_D2;
D(2) = DEFAULT_D3;
D(3) = DEFAULT_D4;
D(4) = DEFAULT_D5;
D(5) = DEFAULT_D6;
genser_hal_inited = 1;
return 0;
error:
return -1;
} // genserKinematicsSetup()

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

@@ -0,0 +1,79 @@
/********************************************************************
* genserkins.c employing switchkins.[ch]
* License: GPL Version 2
*
* NOTEs:
* 1) specify all kparms items
* 2) specify 3 KS,KF,KI functions (setup,forward,inverse)
*/
/********************************************************************
TEST: switchable kinematics: identity or genserkins
1) genser kinematics provided by genserfuncs.c (shared with genserkins.c
2) uses same pin names as genserkins
3) tesing mm configs: increase GO_REAL_EPSILON from 1e-7 to 1e-6
NOTE:
a) requires *exactly* 6 joints
b) identity assignments can use any of xyzabcuvw
but should agree with [TRAJ]COORDINATES
and may be confusing
www refs:
frame-larger-than:
https://www.mail-archive.com/emc-developers@lists.sourceforge.net/msg03790.html
angles:
https://www.mail-archive.com/emc-developers@lists.sourceforge.net/msg15285.html
*/
//----------------------------------------------------------------------
// Only gcc/g++ supports the #pragma
#if __GNUC__ && !defined(__clang__)
// genserKinematicsInverse() is 5104 with buster amd64 gcc 8.3.0-6
//#pragma GCC diagnostic error "-Wframe-larger-than=6000"
#pragma GCC diagnostic warning "-Wframe-larger-than=6000"
#endif
#include <rtapi.h>
#include <rtapi_string.h>
#include <emcmotcfg.h>
#include "genserkins.h"
#include "switchkins.h"
//-7 is system defined -3 ok, -4 ok, -5 ok,-6 ok (mm system)
#undef GO_REAL_EPSILON
#define GO_REAL_EPSILON (1e-6)
//*********************************************************************
int switchkinsSetup(kparms* kp,
KS* kset0, KS* kset1, KS* kset2,
KF* kfwd0, KF* kfwd1, KF* kfwd2,
KI* kinv0, KI* kinv1, KI* kinv2
)
{
kp->kinsname = "genserkins"; // !!! must agree with filename
kp->halprefix = "genserkins"; // hal pin names
kp->required_coordinates = "xyzabcuvw"; // u,v,w are joints 6,7,8
kp->max_joints = strlen(kp->required_coordinates);
kp->allow_duplicates = 0;
*kset0 = genserKinematicsSetup;
*kfwd0 = genserKinematicsForward;
*kinv0 = genserKinematicsInverse;
*kset1 = identityKinematicsSetup;
*kfwd1 = identityKinematicsForward;
*kinv1 = identityKinematicsInverse;
*kset2 = userkKinematicsSetup;
*kfwd2 = userkKinematicsForward;
*kinv2 = userkKinematicsInverse;
return 0;
}

159
wasm-port/vendor/linuxcnc/src/emc/kinematics/genserkins.h vendored Normal file
View File

@@ -0,0 +1,159 @@
/********************************************************************
* Description: genserkins.h
* Kinematics for a generalised serial kinematics machine
*
* Derived from a work by Fred Proctor,
* changed to work with emc2 and HAL
*
* Adapting Author: Alex Joni
* License: GPL Version 2
* System: Linux
*
*******************************************************************
These are the forward and inverse kinematic functions for a general
serial-link manipulator. Thanks to Herman Bruyninckx and John
Hallam at http://www.roble.info/ for this.
The functions are general enough to be configured for any serial
configuration.
The kinematics use Denavit-Hartenberg definition for the joint and
links. The DH definitions are the ones used by John J Craig in
"Introduction to Robotics: Mechanics and Control"
The parameters for the manipulator are defined by hal pins.
Currently the type of the joints is hardcoded to ANGULAR, although
the kins support both ANGULAR and LINEAR axes.
*/
/*
genserkins.h
*/
#ifndef GENSERKINS_H
#define GENSERKINS_H
#include <hal.h> /* HAL data types */
#include <gotypes.h> /* go_result, go_integer */
#include <gomath.h> /* go_pose */
#include <kinematics.h>
/*!
The maximum number of joints supported by the general serial
kinematics. Make this at least 6; a device can have fewer than these.
*/
#define GENSER_MAX_JOINTS 6
#define GENSER_DEFAULT_MAX_ITERATIONS 100
#define PI_2 GO_PI_2
/* default DH parameters, these should be ok for a puma - at least according to Craig */
#define DEFAULT_A1 0
#define DEFAULT_ALPHA1 0
#define DEFAULT_D1 0
#define DEFAULT_A2 0
#define DEFAULT_ALPHA2 -PI_2
#define DEFAULT_D2 0
#define DEFAULT_A3 300
#define DEFAULT_ALPHA3 0
#define DEFAULT_D3 70
#define DEFAULT_A4 50
#define DEFAULT_ALPHA4 -PI_2
#define DEFAULT_D4 400
#define DEFAULT_A5 0
#define DEFAULT_ALPHA5 PI_2
#define DEFAULT_D5 0
#define DEFAULT_A6 0
#define DEFAULT_ALPHA6 -PI_2
#define DEFAULT_D6 0
typedef struct {
go_link links[GENSER_MAX_JOINTS]; /*!< The link description of the device. */
int link_num; /*!< How many are actually present. */
hal_u32_t iterations; /*!< How many iterations were actually used to compute the inverse kinematics. */
} genser_struct;
extern int genser_kin_size(void);
extern int genser_kin_init(void);
extern const char * genser_kin_get_name(void);
extern int genser_kin_num_joints(void * kins);
extern int genser_kin_fwd(void * kins,
const go_real *joint,
go_pose * world);
extern int genser_kin_inv(void * kins,
const go_pose * world,
go_real *joint);
extern int genser_kin_set_parameters(void * kins, go_link * params, int num);
extern int genser_kin_get_parameters(void * kins, go_link * params, int num);
extern int genser_kin_jac_inv(void * kins,
const go_pose * pos,
const go_screw * vel,
const go_real * joints,
go_real * jointvels);
extern int genser_kin_jac_fwd(void * kins,
const go_real * joints,
const go_real * jointvels,
const go_pose * pos,
go_screw * vel);
extern int genser_kin_fwd_interations(genser_struct * genser);
/*
Extras, not callable using go_kin_ wrapper but if you know you have
linked in these kinematics, go ahead and call these for your ad hoc
purposes.
*/
/*! Returns the number of iterations used during the last call to the
inverse kinematics functions */
extern int genser_kin_inv_iterations(genser_struct * genser);
/*! Sets the maximum number of iterations to use in future calls to
the inverse kinematics functions, after which an error will be
reported */
extern int genser_kin_inv_set_max_iterations(int i);
/*! Returns the maximum number of iterations that will be used to
compute inverse kinematics functions */
extern int genser_kin_inv_get_max_iterations(void);
extern int compute_jfwd(go_link * link_params,
int link_number,
go_matrix * Jfwd,
go_pose * T_L_0);
extern int compute_jinv(go_matrix * Jfwd,
go_matrix * Jinv);
extern int genserKinematicsForward(const double *joint,
EmcPose * world,
const KINEMATICS_FORWARD_FLAGS * fflags,
KINEMATICS_INVERSE_FLAGS * iflags);
extern int genserKinematicsInverse(const EmcPose * world,
double *joints,
const KINEMATICS_INVERSE_FLAGS * iflags,
KINEMATICS_FORWARD_FLAGS * fflags);
extern int genserKinematicsSetup(const int comp_id,
const char* coordinates,
kparms* ksetup_parms);
#endif

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

@@ -0,0 +1,519 @@
/********************************************************************
* Description: pentakins.c
*
* Kinematics for a pentapod machine
*
* Derived from genhexkins.c
*
* Author: Andrew Kyrychenko
* License: GPL Version 2
* System: Linux
*
* Copyright (c) 2016 All rights reserved.
*********************************************************************
These are the forward and inverse kinematic functions for a pentapod
parallel kinematics machine.
The default values for base and effector joints positions are defined
in the header file pentakins.h. The actual values for a particular
machine can be adjusted by hal parameters:
pentakins.base.N.x
pentakins.base.N.y
pentakins.base.N.z
pentakins.effector.N.r
pentakins.effector.N.z
Hal pins:
pentakins.convergence-criterion - minimum error value that ends
iterations with converged solution;
pentakins.limit-iterations - limit of iterations, if exceeded
iterations stop with no convergence;
pentakins.max-error - maximum error value, if exceeded iterations
stop with no convergence;
pentakins.last-iterations - number of iterations spent for the
last forward kinematics solution;
pentakins.max-iterations - maximum number of iterations spent for
a converged solution during current session.
pentakins.tool-offset - tool length from the origin along z axis,
changes the effector pivot point.
----------------------------------------------------------------------------*/
#include <rtapi.h>
#include <rtapi_app.h>
#include <rtapi_math.h>
#include <hal.h>
#include <kinematics.h> /* these decls, KINEMATICS_FORWARD_FLAGS */
#include "pentakins.h"
struct haldata {
hal_float_t basex[NUM_STRUTS];
hal_float_t basey[NUM_STRUTS];
hal_float_t basez[NUM_STRUTS];
hal_float_t effectorr[NUM_STRUTS];
hal_float_t effectorz[NUM_STRUTS];
hal_u32_t *last_iter;
hal_u32_t *max_iter;
hal_u32_t *iter_limit;
hal_float_t *max_error;
hal_float_t *conv_criterion;
hal_float_t *tool_offset;
} *haldata;
/******************************* MatInvert5() ***************************/
/*-----------------------------------------------------------------------------
This is a function that inverts a 5x5 matrix.
-----------------------------------------------------------------------------*/
static int MatInvert5(double J[][NUM_STRUTS], double InvJ[][NUM_STRUTS])
{
double JAug[NUM_STRUTS][10], m, temp;
int j, k, n;
/* This function determines the inverse of a 6x6 matrix using
Gauss-Jordan elimination */
/* Augment the Identity matrix to the Jacobian matrix */
for (j=0; j<=4; ++j){
for (k=0; k<=4; ++k){ /* Assign J matrix to first 6 columns of AugJ */
JAug[j][k] = J[j][k];
}
for(k=5; k<=9; ++k){ /* Assign I matrix to last six columns of AugJ */
if (k-5 == j){
JAug[j][k]=1;
}
else{
JAug[j][k]=0;
}
}
}
/* Perform Gauss elimination */
for (k=0; k<=3; ++k){ /* Pivot */
if ((JAug[k][k]< 0.01) && (JAug[k][k] > -0.01)){
for (j=k+1;j<=4; ++j){
if ((JAug[j][k]>0.01) || (JAug[j][k]<-0.01)){
for (n=0; n<=9;++n){
temp = JAug[k][n];
JAug[k][n] = JAug[j][n];
JAug[j][n] = temp;
}
break;
}
}
}
for (j=k+1; j<=4; ++j){ /* Pivot */
m = -JAug[j][k] / JAug[k][k];
for (n=0; n<=9; ++n){
JAug[j][n]=JAug[j][n] + m*JAug[k][n]; /* (Row j) + m * (Row k) */
if ((JAug[j][n] < 0.000001) && (JAug[j][n] > -0.000001)){
JAug[j][n] = 0;
}
}
}
}
/* Normalization of Diagonal Terms */
for (j=0; j<=4; ++j){
m=1/JAug[j][j];
for(k=0; k<=9; ++k){
JAug[j][k] = m * JAug[j][k];
}
}
/* Perform Gauss Jordan Steps */
for (k=4; k>=0; --k){
for(j=k-1; j>=0; --j){
m = -JAug[j][k]/JAug[k][k];
for (n=0; n<=9; ++n){
JAug[j][n] = JAug[j][n] + m * JAug[k][n];
}
}
}
/* Assign last 4 columns of JAug to InvJ */
for (j=0; j<=4; ++j){
for (k=0; k<=4; ++k){
InvJ[j][k] = JAug[j][k+5];
}
}
return 0; /* FIXME-- check divisors for 0 above */
}
/******************************** MatMult() *********************************/
/*---------------------------------------------------------------------------
This function simply multiplies a 6x6 matrix by a 1x6 vector
---------------------------------------------------------------------------*/
static void MatMult5(double J[][5], const double x[], double Ans[])
{
int j, k;
for (j=0; j<=4; ++j){
Ans[j] = 0;
for (k=0; k<=4; ++k){
Ans[j] = J[j][k]*x[k]+Ans[j];
}
}
}
/*--------------
------square-----*/
static double sqr(double x)
{
return (x)*(x);
}
/* declare arrays for base and effector coordinates */
static PmCartesian b[NUM_STRUTS];
static double za[NUM_STRUTS], ra[NUM_STRUTS];
/************************pentakins_read_hal_pins**************************/
int pentakins_read_hal_pins(void) {
int t;
/* set the base and effector coordinates from hal pin values */
for (t = 0; t < NUM_STRUTS; t++) {
b[t].x = haldata->basex[t];
b[t].y = haldata->basey[t];
b[t].z = haldata->basez[t] + *haldata->tool_offset;
ra[t] = haldata->effectorr[t];
za[t] = haldata->effectorz[t] + *haldata->tool_offset;
}
return 0;
}
/************************ InvKins() ********************************/
int InvKins(const double * coord,
double * struts)
{
PmCartesian xyz, pmcoord, temp;
PmRotationMatrix RMatrix, InvRMatrix;
PmRpy rpy;
int i;
// pentakins_read_hal_pins();
/* define Rotation Matrix */
pmcoord.x = coord[0];
pmcoord.y = coord[1];
pmcoord.z = coord[2];
rpy.r = coord[3];
rpy.p = coord[4];
rpy.y = 0;
pmRpyMatConvert(&rpy, &RMatrix);
/* enter for loop to calculate joints (strut lengths) */
for (i = 0; i < NUM_STRUTS; i++) {
/* convert location of effector strut end from effector
to world coordinates */
pmCartCartSub(&b[i], &pmcoord, &temp);
pmMatInv(&RMatrix, &InvRMatrix);
pmMatCartMult(&InvRMatrix, &temp, &xyz);
/* define strut lengths */
struts[i] = sqrt( sqr(xyz.z - za[i]) + sqr( sqrt(sqr(xyz.x) + sqr(xyz.y)) - ra[i]) );
}
return 0;
}
/**************************** kinematicsForward() ***************************/
int kinematicsForward(const double * joints,
EmcPose * pos,
const KINEMATICS_FORWARD_FLAGS * fflags,
KINEMATICS_INVERSE_FLAGS * iflags)
{
(void)fflags;
(void)iflags;
// PmCartesian aw;
// PmCartesian InvKinStrutVect,InvKinStrutVectUnit;
// PmCartesian q_trans, RMatrix_a, RMatrix_a_cross_Strut;
double Jacobian[NUM_STRUTS][NUM_STRUTS];
double InverseJacobian[NUM_STRUTS][NUM_STRUTS];
double InvKinStrutLength[NUM_STRUTS], StrutLengthDiff[NUM_STRUTS];
double delta[NUM_STRUTS];
double jointdelta[NUM_STRUTS];
double coord[NUM_STRUTS];
double conv_err = 1.0;
// PmRotationMatrix RMatrix;
// PmRpy q_RPY;
int iterate = 1;
int i, j;
unsigned iteration = 0;
pentakins_read_hal_pins();
/* abort on obvious problems, like joints <= 0 */
if (joints[0] <= 0.0 ||
joints[1] <= 0.0 ||
joints[2] <= 0.0 ||
joints[3] <= 0.0 ||
joints[4] <= 0.0 ) {
return -1;
}
/* assign a,b,c to roll, pitch, yaw angles */
coord[0] = pos->tran.x;
coord[1] = pos->tran.y;
coord[2] = pos->tran.z;
coord[3] = pos->a * PM_PI / 180.0;
coord[4] = pos->b * PM_PI / 180.0;
/* Enter Newton-Raphson iterative method */
while (iterate) {
/* check for large error and return error flag if no convergence */
if ((conv_err > +(*haldata->max_error)) ||
(conv_err < -(*haldata->max_error))) {
/* we can't converge */
return -2;
};
iteration++;
/* check iteration to see if the kinematics can reach the
convergence criterion and return error flag if it can't */
if (iteration > *haldata->iter_limit) {
/* we can't converge */
return -5;
}
/* compute StrutLengthDiff[] by running inverse kins on Cartesian
estimate to get joint estimate, subtract joints to get joint deltas,
and compute inv J while we're at it */
InvKins(coord, InvKinStrutLength);
for (i = 0; i < NUM_STRUTS; i++) {
StrutLengthDiff[i] = InvKinStrutLength[i] - joints[i];
/* Build Inverse Jacobian Matrix */
coord[i] += 1e-4;
InvKins(coord, jointdelta);
coord[i] -= 1e-4;
for (j = 0; j < NUM_STRUTS; j++) {
InverseJacobian[j][i] = (jointdelta[j] - InvKinStrutLength[j]) * 1e4;
}
}
/* invert Inverse Jacobian */
MatInvert5(InverseJacobian, Jacobian);
/* multiply Jacobian by LegLengthDiff */
MatMult5(Jacobian, StrutLengthDiff, delta);
/* subtract delta from last iterations pos values */
coord[0] -= delta[0];
coord[1] -= delta[1];
coord[2] -= delta[2];
coord[3] -= delta[3];
coord[4] -= delta[4];
/* determine value of conv_error (used to determine if no convergence) */
conv_err = 0.0;
for (i = 0; i < NUM_STRUTS; i++) {
conv_err += fabs(StrutLengthDiff[i]);
}
/* enter loop to determine if a strut needs another iteration */
iterate = 0; /*assume iteration is done */
for (i = 0; i < NUM_STRUTS; i++) {
if (fabs(StrutLengthDiff[i]) > *haldata->conv_criterion) {
iterate = 1;
}
}
} /* exit Newton-Raphson Iterative loop */
/* assign coord to pos */
pos->tran.x = coord[0];
pos->tran.y = coord[1];
pos->tran.z = coord[2];
pos->a = coord[3] * 180.0 / PM_PI;
pos->b = coord[4] * 180.0 / PM_PI;
*haldata->last_iter = iteration;
if (iteration > *haldata->max_iter){
*haldata->max_iter = iteration;
}
return 0;
}
/************************ kinematicsInverse() ********************************/
/* the inverse kinematics take world coordinates and determine joint values,
given the inverse kinematics flags to resolve any ambiguities. The forward
flags are set to indicate their value appropriate to the world coordinates
passed in. */
/************************ kinematicsInverse() ********************************/
int kinematicsInverse(const EmcPose * pos,
double * joints,
const KINEMATICS_INVERSE_FLAGS * iflags,
KINEMATICS_FORWARD_FLAGS * fflags)
{
(void)iflags;
(void)fflags;
double coord[NUM_STRUTS];
pentakins_read_hal_pins();
coord[0] = pos->tran.x;
coord[1] = pos->tran.y;
coord[2] = pos->tran.z;
coord[3] = pos->a * PM_PI / 180.0;
coord[4] = pos->b * PM_PI / 180.0;
if (0 != InvKins(coord,joints)) {
return -1;
}
return 0;
}
KINEMATICS_TYPE kinematicsType()
{
return KINEMATICS_BOTH;
}
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, i;
comp_id = hal_init("pentakins");
if (comp_id < 0)
return comp_id;
haldata = hal_malloc(sizeof(struct haldata));
if (!haldata)
goto error;
for (i = 0; i < 6; i++) {
if ((res = hal_param_float_newf(HAL_RW, &(haldata->basex[i]), comp_id,
"pentakins.base.%d.x", i)) < 0)
goto error;
if ((res = hal_param_float_newf(HAL_RW, &haldata->basey[i], comp_id,
"pentakins.base.%d.y", i)) < 0)
goto error;
if ((res = hal_param_float_newf(HAL_RW, &haldata->basez[i], comp_id,
"pentakins.base.%d.z", i)) < 0)
goto error;
if ((res = hal_param_float_newf(HAL_RW, &haldata->effectorr[i], comp_id,
"pentakins.effector.%d.r", i)) < 0)
goto error;
if ((res = hal_param_float_newf(HAL_RW, &haldata->effectorz[i], comp_id,
"pentakins.effector.%d.z", i)) < 0)
goto error;
}
if ((res = hal_pin_u32_newf(HAL_OUT, &haldata->last_iter, comp_id,
"pentakins.last-iterations")) < 0)
goto error;
*haldata->last_iter = 0;
if ((res = hal_pin_u32_newf(HAL_OUT, &haldata->max_iter, comp_id,
"pentakins.max-iterations")) < 0)
goto error;
*haldata->max_iter = 0;
if ((res = hal_pin_float_newf(HAL_IO, &haldata->max_error, comp_id,
"pentakins.max-error")) < 0)
goto error;
*haldata->max_error = 100;
if ((res = hal_pin_float_newf(HAL_IO, &haldata->conv_criterion, comp_id,
"pentakins.convergence-criterion")) < 0)
goto error;
*haldata->conv_criterion = 1e-9;
if ((res = hal_pin_u32_newf(HAL_IO, &haldata->iter_limit, comp_id,
"pentakins.limit-iterations")) < 0)
goto error;
*haldata->iter_limit = 120;
if ((res = hal_pin_float_newf(HAL_IN, &haldata->tool_offset, comp_id,
"pentakins.tool-offset")) < 0)
goto error;
*haldata->tool_offset = 0.0;
haldata->basex[0] = DEFAULT_BASE_0_X;
haldata->basey[0] = DEFAULT_BASE_0_Y;
haldata->basez[0] = DEFAULT_BASE_0_Z;
haldata->basex[1] = DEFAULT_BASE_1_X;
haldata->basey[1] = DEFAULT_BASE_1_Y;
haldata->basez[1] = DEFAULT_BASE_1_Z;
haldata->basex[2] = DEFAULT_BASE_2_X;
haldata->basey[2] = DEFAULT_BASE_2_Y;
haldata->basez[2] = DEFAULT_BASE_2_Z;
haldata->basex[3] = DEFAULT_BASE_3_X;
haldata->basey[3] = DEFAULT_BASE_3_Y;
haldata->basez[3] = DEFAULT_BASE_3_Z;
haldata->basex[4] = DEFAULT_BASE_4_X;
haldata->basey[4] = DEFAULT_BASE_4_Y;
haldata->basez[4] = DEFAULT_BASE_4_Z;
haldata->effectorz[0] = DEFAULT_EFFECTOR_0_Z;
haldata->effectorz[1] = DEFAULT_EFFECTOR_1_Z;
haldata->effectorz[2] = DEFAULT_EFFECTOR_2_Z;
haldata->effectorz[3] = DEFAULT_EFFECTOR_3_Z;
haldata->effectorz[4] = DEFAULT_EFFECTOR_4_Z;
haldata->effectorr[0] = DEFAULT_EFFECTOR_0_R;
haldata->effectorr[1] = DEFAULT_EFFECTOR_1_R;
haldata->effectorr[2] = DEFAULT_EFFECTOR_2_R;
haldata->effectorr[3] = DEFAULT_EFFECTOR_3_R;
haldata->effectorr[4] = DEFAULT_EFFECTOR_4_R;
hal_ready(comp_id);
return 0;
error:
hal_exit(comp_id);
return res;
}
void rtapi_app_exit(void)
{
hal_exit(comp_id);
}

56
wasm-port/vendor/linuxcnc/src/emc/kinematics/pentakins.h vendored Normal file
View File

@@ -0,0 +1,56 @@
/*******************************************************************
* Description: pentakins.h
*
* Kinematics for a pentapod machine
*
* Derived from genhexkins
*
* Author: Andrew Kyrychenko
* License: GPL Version 2
* System: Linux
*
* Copyright (c) 2016 All rights reserved.
********************************************************************
This is the header file containing joints coordinates.
*/
#define NUM_STRUTS 5 // number of struts
/* Default position of base strut ends in base (world) coordinates */
#define DEFAULT_BASE_0_X -418.03
#define DEFAULT_BASE_1_X 417.96
#define DEFAULT_BASE_2_X -418.03
#define DEFAULT_BASE_3_X 417.96
#define DEFAULT_BASE_4_X -0.06
#define DEFAULT_BASE_0_Y 324.56
#define DEFAULT_BASE_1_Y 324.56
#define DEFAULT_BASE_2_Y -325.44
#define DEFAULT_BASE_3_Y -325.44
#define DEFAULT_BASE_4_Y -492.96
#define DEFAULT_BASE_0_Z 895.56
#define DEFAULT_BASE_1_Z 895.56
#define DEFAULT_BASE_2_Z 895.56
#define DEFAULT_BASE_3_Z 895.56
#define DEFAULT_BASE_4_Z 895.56
/* Default radius of effector strut end in platform coordinate system */
#define DEFAULT_EFFECTOR_0_R 80.32
#define DEFAULT_EFFECTOR_1_R 80.32
#define DEFAULT_EFFECTOR_2_R 80.32
#define DEFAULT_EFFECTOR_3_R 80.32
#define DEFAULT_EFFECTOR_4_R 80.32
/* Default position of effector joints along the spindle axis in
effector coordinate system */
#define DEFAULT_EFFECTOR_0_Z -185.50
#define DEFAULT_EFFECTOR_1_Z -159.50
#define DEFAULT_EFFECTOR_2_Z -67.50
#define DEFAULT_EFFECTOR_3_Z -41.50
#define DEFAULT_EFFECTOR_4_Z -14.00

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

@@ -0,0 +1,297 @@
/********************************************************************
* !!! Declining usage
* !!! This userspace program was been separated from the original
* !!! genserkins.c as part of refactoring kinematics for
* !!! switchkins support. The userspace program has not been
* !!! updated since 2009 (commit 1939285758) and correct
* !!! functioning in its current state is doubtful.
*
* Description: ugenserkins.c
* Test program (originally part of genserkins.c)
* License GPL Version 2
********************************************************************/
#include <stdio.h> /* ulapi */
#include <sys/time.h> /* struct timeval */
#include "genserkins.h"
static double timestamp()
{
struct timeval tp;
if (0 != gettimeofday(&tp, NULL)) {
return 0.0;
}
return ((double) tp.tv_sec) + ((double) tp.tv_usec) / 1000000.0;
}
int main(int argc, char *argv[])
{
#define BUFFERLEN 256
char buffer[BUFFERLEN];
int inverse = 1;
int jacobian = 0;
EmcPose pos = { {0.0, 0.0, 0.0}, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
EmcPose vel = { {0.0, 0.0, 0.0}, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; // will need this for
// jacobian
double joints[6] = { 0.0 };
double jointvels[6] = { 0.0 };
KINEMATICS_INVERSE_FLAGS iflags = 0;
KINEMATICS_FORWARD_FLAGS fflags = 0;
int t;
int retval = 0;
double start, end;
int comp_id;
kparms kp;
kp.max_joints = GENSER_MAX_JOINTS;
kp.allow_duplicates = 0;
comp_id = hal_init("usergenserkins");
if (genserKinematicsSetup(comp_id,"XYZABC",&kp)) printf("unexpected\n");
genser_kin_init();
/* syntax is a.out {i|f # # # # # #} */
if (argc == 8) {
if (argv[1][0] == 'f') {
/* joints passed, so do iterations on forward kins for timing */
for (t = 0; t < 6; t++) {
if (1 != sscanf(argv[t + 2], "%lf", &joints[t])) {
fprintf(stderr, "bad value: %s\n", argv[t + 2]);
return 1;
}
}
inverse = 0;
} else if (argv[1][0] == 'i') {
/* world coords passed, so do iterations on inverse kins for
timing */
if (1 != sscanf(argv[2], "%lf", &pos.tran.x)) {
fprintf(stderr, "bad value: %s\n", argv[2]);
return 1;
}
if (1 != sscanf(argv[3], "%lf", &pos.tran.y)) {
fprintf(stderr, "bad value: %s\n", argv[3]);
return 1;
}
if (1 != sscanf(argv[4], "%lf", &pos.tran.z)) {
fprintf(stderr, "bad value: %s\n", argv[4]);
return 1;
}
if (1 != sscanf(argv[5], "%lf", &pos.a)) {
fprintf(stderr, "bad value: %s\n", argv[5]);
return 1;
}
if (1 != sscanf(argv[6], "%lf", &pos.b)) {
fprintf(stderr, "bad value: %s\n", argv[6]);
return 1;
}
if (1 != sscanf(argv[7], "%lf", &pos.c)) {
fprintf(stderr, "bad value: %s\n", argv[7]);
return 1;
}
inverse = 1;
} else {
fprintf(stderr, "syntax: %s {i|f # # # # # #}\n", argv[0]);
hal_exit(comp_id);
return 1;
}
/* need an initial estimate for the forward kins, so ask for it */
if (inverse == 0) {
do {
printf("initial estimate for Cartesian position, xyzrpw: ");
fflush(stdout);
if (NULL == fgets(buffer, BUFFERLEN, stdin)) {
hal_exit(comp_id);
return 0;
}
} while (6 != sscanf(buffer, "%lf %lf %lf %lf %lf %lf",
&pos.tran.x, &pos.tran.y, &pos.tran.z, &pos.a, &pos.b, &pos.c));
}
start = timestamp();
if (inverse) {
fprintf(stderr,"%stest pos.b\n",__FILE__);
joints[4]=90;
pos.tran.x= 19.2;
pos.tran.y= 5.5;
pos.tran.z= 9.4;
pos.a = 92.88;
pos.b = -90;
pos.c = 87.12;
fprintf(stderr,"gki0:P %6.2f %6.2f %6.2f %6.2f %6.2f %6.2f\n",
pos.tran.x,pos.tran.y,pos.tran.z,pos.a,pos.b,pos.c);
fprintf(stderr,"gki1:J %6.2f %6.2f %6.2f %6.2f %6.2f %6.2f\n",
joints[0],joints[1],joints[2],joints[3],joints[4],joints[5]);
retval = genserKinematicsInverse(&pos, joints, &iflags, &fflags);
fprintf(stderr,"gki2:J %6.2f %6.2f %6.2f %6.2f %6.2f %6.2f\n",
joints[0],joints[1],joints[2],joints[3],joints[4],joints[5]);
if (0 != retval) {
printf("inv kins error %d <%s>\n", retval,go_result_to_string(retval));
}
} else {
retval = genserKinematicsForward(joints, &pos, &fflags, &iflags);
if (0 != retval) {
printf("fwd kins error %d\n", retval);
}
}
end = timestamp();
printf("calculation time: %f secs\n", (end - start));
printf("Joints: 0=%8.3f 1=%8.3f 2=%8.3f 3=%8.3f 4=%8.3f 5=%8.3f\n",
joints[0],joints[1],joints[2], joints[3],joints[4],joints[5]);
printf("Inverse: x=%8.3f y=%8.3f z=%8.3f a=%8.3f b=%8.3f z=%8.3f\n",
pos.tran.x,pos.tran.y,pos.tran.z,pos.a,pos.b,pos.c);
hal_exit(comp_id);
return 0;
}
/* end of if args for timestamping */
/* else we're interactive, terminate with 'quit'|'exit'|CTRL-D */
while (!feof(stdin)) {
if (inverse) {
if (jacobian) {
printf("jinv> ");
} else {
printf("inv> ");
}
} else {
if (jacobian) {
printf("jfwd> ");
} else {
printf("fwd> ");
}
}
fflush(stdout);
if (NULL == fgets(buffer, BUFFERLEN, stdin)) {
break;
}
if (buffer[0] == 'i') {
inverse = 1;
continue;
} else if (buffer[0] == 'f') {
inverse = 0;
continue;
} else if (buffer[0] == 'j') {
jacobian = !jacobian;
continue;
} else if (buffer[0] == 'q') { //quit
break;
} else if (buffer[0] == 'e') { //exit
break;
}
if (inverse) {
if (jacobian) {
if (12 != sscanf(buffer,
"%lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf",
&pos.tran.x, &pos.tran.y, &pos.tran.z, &pos.a, &pos.b,
&pos.c, &vel.tran.x, &vel.tran.y, &vel.tran.z, &vel.a,
&vel.b, &vel.c)) {
printf("?\n");
} else {
//FIXME-AJ
//disabled for now retval = jacobianInverse(&pos, &vel, joints, jointvels);
printf("%f %f %f %f %f %f\n",
jointvels[0],
jointvels[1],
jointvels[2],
jointvels[3], jointvels[4], jointvels[5]);
if (0 != retval) {
printf("inv Jacobian error %d\n", retval);
} else {
//FIXME-AJ
//disabled for now retval = jacobianForward(joints, jointvels, &pos, &vel);
printf("%f %f %f %f %f %f\n",
vel.tran.x,
vel.tran.y, vel.tran.z, vel.a, vel.b, vel.c);
if (0 != retval) {
printf("fwd kins error %d\n", retval);
}
}
}
} else {
if (6 != sscanf(buffer, "%lf %lf %lf %lf %lf %lf",
&pos.tran.x,
&pos.tran.y, &pos.tran.z, &pos.a, &pos.b, &pos.c)) {
printf("?\n");
} else {
fprintf(stderr,"gki1:\n");
retval =
genserKinematicsInverse(&pos, joints, &iflags, &fflags);
printf("%f %f %f %f %f %f\n", joints[0], joints[1],
joints[2], joints[3], joints[4], joints[5]);
if (0 != retval) {
printf("inv kins error %d <%s>\n", retval,go_result_to_string(retval));
} else {
retval =
genserKinematicsForward(joints, &pos, &fflags, &iflags);
printf("%f %f %f %f %f %f\n", pos.tran.x, pos.tran.y,
pos.tran.z, pos.a, pos.b, pos.c);
if (0 != retval) {
printf("fwd kins error %d\n", retval);
}
}
}
}
} else {
if (jacobian) {
if (12 != sscanf(buffer,
"%lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf",
&joints[0], &joints[1], &joints[2], &joints[3],
&joints[4], &joints[5], &jointvels[0], &jointvels[1],
&jointvels[2], &jointvels[3], &jointvels[4],
&jointvels[5])) {
printf("?\n");
} else {
//FIXME-AJ
//disabled for now retval = jacobianForward(joints, jointvels, &pos, &vel);
printf("%f %f %f %f %f %f\n",
vel.tran.x,
vel.tran.y, vel.tran.z, vel.a, vel.b, vel.c);
if (0 != retval) {
printf("fwd kins error %d\n", retval);
} else {
//FIXME-AJ
//disabled for now retval = jacobianInverse(&pos, &vel, joints, jointvels);
printf("%f %f %f %f %f %f\n",
jointvels[0],
jointvels[1],
jointvels[2],
jointvels[3], jointvels[4], jointvels[5]);
if (0 != retval) {
printf("inv kins error %d <%s>\n", retval,go_result_to_string(retval));
}
}
}
} else {
if (6 != sscanf(buffer, "%lf %lf %lf %lf %lf %lf",
&joints[0], &joints[1], &joints[2], &joints[3], &joints[4], &joints[5])) {
printf("?\n");
} else {
retval = genserKinematicsForward(joints, &pos, &fflags, &iflags);
printf("xyzabc: %f %f %f %f %f %f\n",
pos.tran.x, pos.tran.y, pos.tran.z, pos.a, pos.b, pos.c);
if (0 != retval) {
printf("fwd kins error %d\n", retval);
} else {
retval = genserKinematicsInverse(&pos, joints, &iflags, &fflags);
printf("j0--j5: %f %f %f %f %f %f\n",
joints[0], joints[1], joints[2], joints[3], joints[4], joints[5]);
if (0 != retval) {
printf("inv kins error %d <%s>\n", retval,go_result_to_string(retval));
}
}
}
}
}
}
hal_exit(comp_id);
return 0;
#undef ITERATIONS
#undef BUFFERLEN
}