From 43b84a4149260ac8889017d15572da54709f7cbd Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Mon, 10 Aug 2026 18:05:21 +1000 Subject: [PATCH 01/19] kinematics: evaluate a module outside RT by binding to its live pins A module's haldata is a struct of pin handles, and a handle is a pointer to the value cell hal_get_real() reads, so a second copy of the module outside realtime can point its haldata at cells carrying the values the RT instance reads and its forward and inverse work unmodified, on live values, at any pose. Per module: export nonrt_attach(), which asks a caller-supplied resolver for each input pin by name, runs the same coordinate parse setup runs and returns the forward and inverse; the maths is untouched and no header grows per module. The cells bound are pins of the caller's own component, connected to the signal the RT pin reads or to one made for the purpose, since a reference into another component's pin has that component's lifetime. Name lookup lives in the loader, userspace code linked against liblinuxcnchal; walking the HAL name space from an RT object is what the HAL isolation work removes. Input pins only, or the two copies write each other's state. Verified against the shared-memory snapshot design this replaces: identical caps from kinslimits, and a pin set by setp is seen without a motion thread running. --- debian/linuxcnc.install.in | 1 + include/kinematics_user.h | 199 +++++++++ include/nonrt_kins.h | 95 +++++ src/Makefile | 1 + src/emc/kinematics/5axiskins.c | 63 ++- src/emc/kinematics/kins_util.c | 4 +- src/emc/kinematics/trivkins.c | 15 + src/emc/kinematics_userspace/Submakefile | 1 + .../kinematics_userspace/kinematics_user.c | 379 ++++++++++++++++++ src/emc/motion_planning/Submakefile | 49 +++ src/emc/motion_planning/jacobian.cc | 197 +++++++++ src/emc/motion_planning/jacobian.hh | 99 +++++ src/emc/motion_planning/joint_limits.cc | 358 +++++++++++++++++ src/emc/motion_planning/joint_limits.hh | 234 +++++++++++ src/emc/motion_planning/kinslimits.cc | 269 +++++++++++++ 15 files changed, 1954 insertions(+), 10 deletions(-) create mode 100644 include/kinematics_user.h create mode 100644 include/nonrt_kins.h create mode 100644 src/emc/kinematics_userspace/Submakefile create mode 100644 src/emc/kinematics_userspace/kinematics_user.c create mode 100644 src/emc/motion_planning/Submakefile create mode 100644 src/emc/motion_planning/jacobian.cc create mode 100644 src/emc/motion_planning/jacobian.hh create mode 100644 src/emc/motion_planning/joint_limits.cc create mode 100644 src/emc/motion_planning/joint_limits.hh create mode 100644 src/emc/motion_planning/kinslimits.cc diff --git a/debian/linuxcnc.install.in b/debian/linuxcnc.install.in index d66045b43a5..a71302665a0 100644 --- a/debian/linuxcnc.install.in +++ b/debian/linuxcnc.install.in @@ -36,6 +36,7 @@ usr/bin/hy_vfd usr/bin/image-to-gcode usr/bin/inivalue usr/bin/inivar +usr/bin/kinslimits usr/bin/latency-histogram usr/bin/latency-plot usr/bin/latency-test diff --git a/include/kinematics_user.h b/include/kinematics_user.h new file mode 100644 index 00000000000..8bbabea8727 --- /dev/null +++ b/include/kinematics_user.h @@ -0,0 +1,199 @@ +/******************************************************************** + * Description: kinematics_user.h + * Userspace kinematics interface for trajectory planning + * + * This provides a userspace-compatible kinematics interface that mirrors + * the RT kinematics interface. Used by the 9D planner to compute joint + * positions from world coordinates without requiring RT kernel calls. + * + * The kinematics module is loaded into this process and given input pins + * belonging to the caller's HAL component, connected to the same signals + * the running RT instance reads. Its own forward and inverse then work on + * live values, unmodified. + * + * Author: LinuxCNC + * License: GPL Version 2 + * System: Linux + * + * Copyright (c) 2024 All rights reserved. + ********************************************************************/ +#ifndef KINEMATICS_USER_H +#define KINEMATICS_USER_H + +#include "emcpos.h" /* EmcPose */ +#include "kinematics.h" /* KINEMATICS_TYPE, flags */ +#include "hal.h" /* hal_type_t, HAL_NAME_LEN */ + +#ifdef __cplusplus +#include "axis_kinds.hh" /* AXIS_X to AXIS_W */ +extern "C" { +#else +/* Axis coordinate indices for EmcPose, as axis_kinds.hh has them for C++ */ +enum AxisIndex { + AXIS_X, AXIS_Y, AXIS_Z, + AXIS_A, AXIS_B, AXIS_C, + AXIS_U, AXIS_V, AXIS_W, +}; +#endif + +/* Maximum number of joints supported */ +#define KINEMATICS_USER_MAX_JOINTS 9 + +#define AXIS_COUNT 9 + +/* Opaque context for userspace kinematics */ +typedef struct KinematicsUserContext KinematicsUserContext; + +/** + * Initialize userspace kinematics context + * + * The pins this creates belong to the caller's component, so call this + * after hal_init() and before hal_ready(): HAL refuses new pins once a + * component is ready. + * + * @param kins_type Kinematics module name (e.g., "trivkins", "5axiskins", "maxkins") + * @param num_joints Number of joints in the machine + * @param coordinates Coordinate string (e.g., "XYZABC", "XYZBCW") + * @param comp_id Caller's HAL component, from hal_init() + * @param prefix Its name, which the created pin names start with + * @return Allocated context, or NULL if kinematics type not supported + */ +KinematicsUserContext* kinematicsUserInit(const char* kins_type, + int num_joints, + const char* coordinates, + int comp_id, + const char* prefix); + +/** + * Perform inverse kinematics (world coords -> joint positions) + * + * @param ctx Kinematics context from kinematicsUserInit + * @param world World coordinates (X, Y, Z, A, B, C, U, V, W) + * @param joints Output array of joint positions [KINEMATICS_USER_MAX_JOINTS] + * @return 0 on success, -1 on failure + */ +int kinematicsUserInverse(KinematicsUserContext* ctx, + const EmcPose* world, + double* joints); + +/** + * Perform forward kinematics (joint positions -> world coords) + * + * @param ctx Kinematics context from kinematicsUserInit + * @param joints Array of joint positions [KINEMATICS_USER_MAX_JOINTS] + * @param world Output world coordinates + * @return 0 on success, -1 on failure + */ +int kinematicsUserForward(KinematicsUserContext* ctx, + const double* joints, + EmcPose* world); + +/** + * Check if kinematics type is identity (world coords = joint coords) + * + * @param ctx Kinematics context + * @return 1 if identity, 0 if not + */ +int kinematicsUserIsIdentity(KinematicsUserContext* ctx); + +/** + * Get number of joints + * + * @param ctx Kinematics context + * @return Number of joints + */ +int kinematicsUserGetNumJoints(KinematicsUserContext* ctx); + +/** + * Get KINEMATICS_TYPE (IDENTITY, BOTH, FORWARD_ONLY, INVERSE_ONLY) + * + * @param ctx Kinematics context + * @return KINEMATICS_TYPE enum value + */ +KINEMATICS_TYPE kinematicsUserGetType(KinematicsUserContext* ctx); + +/** + * Get kinematics module name + * + * @param ctx Kinematics context + * @return Module name string (e.g., "5axiskins") + */ +const char* kinematicsUserGetModuleName(KinematicsUserContext* ctx); + +/** + * Refresh kinematics parameters (no-op) + * + * The bound pins read the live values, so there is nothing to fetch. + * This function is kept for API compatibility but does nothing. + * + * @param ctx Kinematics context + * @return 0 always + */ +int kinematicsUserRefreshParams(KinematicsUserContext* ctx); + +/** + * Check if this context is RT-only + * + * An RT-only module exports no nonrt_attach() and so cannot be evaluated + * outside RT. Planner 2 is unavailable for such modules. + * + * @param ctx Kinematics context + * @return 1 if RT-only (planner 2 unavailable), 0 if the module is bound + */ +int kinematicsUserIsRtOnly(KinematicsUserContext* ctx); + +/** + * Free kinematics context + * + * @param ctx Context to free + */ +void kinematicsUserFree(KinematicsUserContext* ctx); + +/** + * Get axis value from EmcPose by index + * + * @param pose Pointer to EmcPose + * @param axis Axis index (AXIS_X through AXIS_W) + * @return Axis value + */ +static inline double emcPoseGetAxis(const EmcPose* pose, int axis) { + switch (axis) { + case AXIS_X: return pose->tran.x; + case AXIS_Y: return pose->tran.y; + case AXIS_Z: return pose->tran.z; + case AXIS_A: return pose->a; + case AXIS_B: return pose->b; + case AXIS_C: return pose->c; + case AXIS_U: return pose->u; + case AXIS_V: return pose->v; + case AXIS_W: return pose->w; + default: return 0.0; + } +} + +/** + * Set axis value in EmcPose by index + * + * @param pose Pointer to EmcPose + * @param axis Axis index (AXIS_X through AXIS_W) + * @param value Value to set + */ +static inline void emcPoseSetAxis(EmcPose* pose, int axis, double value) { + switch (axis) { + case AXIS_X: pose->tran.x = value; break; + case AXIS_Y: pose->tran.y = value; break; + case AXIS_Z: pose->tran.z = value; break; + case AXIS_A: pose->a = value; break; + case AXIS_B: pose->b = value; break; + case AXIS_C: pose->c = value; break; + case AXIS_U: pose->u = value; break; + case AXIS_V: pose->v = value; break; + case AXIS_W: pose->w = value; break; + } +} + +#ifdef __cplusplus +} +#endif + +#endif /* KINEMATICS_USER_H */ diff --git a/include/nonrt_kins.h b/include/nonrt_kins.h new file mode 100644 index 00000000000..48e8ea3d86e --- /dev/null +++ b/include/nonrt_kins.h @@ -0,0 +1,95 @@ +/******************************************************************** + * Description: nonrt_kins.h + * Interface a kinematics module exports so that a non-RT caller can + * evaluate it. + * + * A trajectory planner needs forward and inverse kinematics at poses + * the machine has not reached yet, which means calling them outside + * the servo thread. A module opts in by exporting nonrt_attach(). + * + * The caller dlopens the module and calls nonrt_attach() once with + * the coordinates string and a resolver callback. The module names + * each of the pins it reads, keeps the references the resolver + * returns in its own haldata, and hands back its existing forward + * and inverse. The kinematics code itself does not change. + * + * A reference does not point into the RT instance's pin. The + * resolver creates an input pin on the caller's own component and + * connects it to the signal the RT pin reads, so the reference + * belongs to the caller and rewiring cannot strand it. + * + * Name lookup belongs to the caller, userspace code linked against + * liblinuxcnchal. This file is compiled into an RT module, which + * has no business walking the HAL name space and would risk binding + * against rtlib's copy of the same symbols. + * + * Resolve input pins only. Output pins and scratch storage stay + * private to the non-RT copy, or the two copies write to each + * other's state. + * + * Author: LinuxCNC + * License: GPL Version 2 + * System: Linux + * + * Copyright (c) 2024 All rights reserved. + ********************************************************************/ + +#ifndef NONRT_KINS_H +#define NONRT_KINS_H + +#include + +#include +#include "hal.h" +#include "emcpos.h" +#include "kinematics.h" + +/* Supplied by the caller. Finds 'pin_name' in HAL, checks that it has + type 'type', and writes to 'out' a reference carrying that pin's + value. The reference is to storage the caller owns, not to the named + pin itself. Returns 0 on success. */ +typedef int (*nonrt_resolve_fn)(const char *pin_name, + hal_type_t type, + hal_refs_u *out, + void *arg); + +/* Filled in by nonrt_attach(). A module that reports is_identity has + joints equal to axes and the caller needs no module code at all, so + forward and inverse may be left NULL. */ +typedef struct { + int (*forward)(const double *joints, EmcPose *pos, + const KINEMATICS_FORWARD_FLAGS *fflags, + KINEMATICS_INVERSE_FLAGS *iflags); + int (*inverse)(const EmcPose *pos, double *joints, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags); + int is_identity; +} nonrt_ops_t; + +/* Exported by a participating module: + int nonrt_attach(const char *coordinates, nonrt_ops_t *ops, + nonrt_resolve_fn resolve, void *arg); + Returns 0 on success. */ + +/* Convenience for the common case: resolve one float pin, by printf + style name, into a haldata field. */ +static inline int nonrt_resolve_real(nonrt_resolve_fn resolve, void *arg, + hal_real_t *dst, const char *fmt, ...) +{ + char name[HAL_NAME_LEN + 1]; + hal_refs_u ref; + va_list ap; + + if (!resolve || !dst) return -1; + + va_start(ap, fmt); + rtapi_vsnprintf(name, sizeof(name), fmt, ap); + va_end(ap); + + if (resolve(name, HAL_REAL, &ref, arg) != 0) return -1; + + *dst = ref.r; + return 0; +} + +#endif /* NONRT_KINS_H */ diff --git a/src/Makefile b/src/Makefile index df19b0c3dc7..63f65ba7fad 100644 --- a/src/Makefile +++ b/src/Makefile @@ -193,6 +193,7 @@ SUBDIRS := \ \ $(GUI_SUBDIRS) \ emc/usr_intf/axis emc/usr_intf emc/nml_intf emc/task emc/kinematics emc/canterp \ + emc/motion_planning emc/kinematics_userspace \ emc/ini emc/rs274ngc emc/sai emc/pythonplugin \ emc/motion-logger \ emc/tooldata \ diff --git a/src/emc/kinematics/5axiskins.c b/src/emc/kinematics/5axiskins.c index b2510e094ef..d4798261ff8 100644 --- a/src/emc/kinematics/5axiskins.c +++ b/src/emc/kinematics/5axiskins.c @@ -61,6 +61,7 @@ #include #include +#include static struct haldata { hal_real_t pivot_length; @@ -201,11 +202,20 @@ static int fiveaxis_KinematicsJacobian(const double *joints, jac); } // fiveaxis_KinematicsJacobian() -int fiveaxis_KinematicsSetup(const int comp_id, - const char* coordinates, - kparms* kp) +// module constants, shared by switchkinsSetup() and nonrt_attach() +static void fiveaxis_kparms(kparms* kp) +{ + kp->kinsname = "5axiskins"; // !!! must agree with filename + kp->halprefix = "5axiskins"; // hal pin names + kp->required_coordinates = REQUIRED_COORDINATES; + kp->allow_duplicates = 1; + kp->max_joints = EMCMOT_MAX_JOINTS; +} + +// assign principal joint numbers from the coordinates string. +// No HAL involvement, so the non-RT path can use it too. +static int fiveaxis_map_joints(const char* coordinates, kparms* kp) { - int result=0; int i,jno; int axis_idx_for_jno[EMCMOT_MAX_JOINTS]; int minjoints = strlen(kp->required_coordinates); @@ -254,6 +264,20 @@ int fiveaxis_KinematicsSetup(const int comp_id, if (axis_idx_for_jno[jno] == 8) {if (JW == -1) JW=jno;} } + return 0; + +error: + return -1; +} // fiveaxis_map_joints() + +int fiveaxis_KinematicsSetup(const int comp_id, + const char* coordinates, + kparms* kp) +{ + int result=0; + + if (fiveaxis_map_joints(coordinates, kp)) goto error; + haldata = hal_malloc(sizeof(*haldata)); if(!haldata) goto error; @@ -286,11 +310,7 @@ int switchkinsSetup(kparms* kp, KI* kinv0, KI* kinv1, KI* kinv2 ) { - kp->kinsname = "5axiskins"; // !!! must agree with filename - kp->halprefix = "5axiskins"; // hal pin names - kp->required_coordinates = REQUIRED_COORDINATES; - kp->allow_duplicates = 1; - kp->max_joints = EMCMOT_MAX_JOINTS; + fiveaxis_kparms(kp); if (kp->sparm && strstr(kp->sparm,"identityfirst")) { rtapi_print("\n!!! switchkins-type 0 is IDENTITY\n"); @@ -323,3 +343,28 @@ int switchkinsSetup(kparms* kp, return 0; } // switchkinsSetup() + +// Non-RT entry point: bind this copy of the module to the pins the +// running RT instance owns, then hand back the unmodified kinematics. +int nonrt_attach(const char* coordinates, nonrt_ops_t* ops, + nonrt_resolve_fn resolve, void* arg) +{ + static struct haldata nonrt_haldata; // private to this copy of the module + kparms kp = {0}; + + fiveaxis_kparms(&kp); + + haldata = &nonrt_haldata; + + if (nonrt_resolve_real(resolve, arg, &haldata->pivot_length, + "%s.pivot-length", kp.halprefix)) return -1; + + if (fiveaxis_map_joints(coordinates, &kp)) return -1; + + ops->forward = fiveaxis_KinematicsForward; + ops->inverse = fiveaxis_KinematicsInverse; + ops->is_identity = 0; + return 0; +} // nonrt_attach() + +EXPORT_SYMBOL(nonrt_attach); diff --git a/src/emc/kinematics/kins_util.c b/src/emc/kinematics/kins_util.c index 113abd50867..bf350e3d791 100644 --- a/src/emc/kinematics/kins_util.c +++ b/src/emc/kinematics/kins_util.c @@ -299,7 +299,6 @@ int identityKinematicsAxisJointsBitmap(int axis_num) } return 0; } // identityKinematicsAxisJointsBitmap() -EXPORT_SYMBOL(identityKinematicsAxisJointsBitmap); static int identity_kinematics_initialized = 0; static int identity_max_joints; @@ -1304,9 +1303,11 @@ int identityKinematicsJacobian(const double *joint, jac); } // identityKinematicsJacobian() +#ifdef RTAPI EXPORT_SYMBOL(map_coordinates_to_jnumbers); EXPORT_SYMBOL(mapped_joints_to_position); EXPORT_SYMBOL(position_to_mapped_joints); +EXPORT_SYMBOL(identityKinematicsAxisJointsBitmap); EXPORT_SYMBOL(identityKinematicsSetup); EXPORT_SYMBOL(identityKinematicsForward); EXPORT_SYMBOL(identityKinematicsInverse); @@ -1322,3 +1323,4 @@ EXPORT_SYMBOL(kinsJacobianFromMappedAxes); EXPORT_SYMBOL(kinsJacobianFromDhArm); EXPORT_SYMBOL(TOOL_FRAME_SPINDLE); EXPORT_SYMBOL(TOOL_FRAME_FLANGE); +#endif diff --git a/src/emc/kinematics/trivkins.c b/src/emc/kinematics/trivkins.c index f04d9642622..a170c94d6ad 100644 --- a/src/emc/kinematics/trivkins.c +++ b/src/emc/kinematics/trivkins.c @@ -18,6 +18,7 @@ #include #include #include +#include #define SET(f) pos->f = joints[i] @@ -110,3 +111,17 @@ int rtapi_app_main(void) { } void rtapi_app_exit(void) { hal_exit(comp_id); } + +// Non-RT entry point: joints are axes, so a non-RT caller needs no +// module code at all and reads nothing from HAL. +int nonrt_attach(const char* coordinates, nonrt_ops_t* ops, + nonrt_resolve_fn resolve, void* arg) +{ + (void)coordinates; (void)resolve; (void)arg; + ops->forward = NULL; + ops->inverse = NULL; + ops->is_identity = 1; + return 0; +} + +EXPORT_SYMBOL(nonrt_attach); diff --git a/src/emc/kinematics_userspace/Submakefile b/src/emc/kinematics_userspace/Submakefile new file mode 100644 index 00000000000..f92b17d356e --- /dev/null +++ b/src/emc/kinematics_userspace/Submakefile @@ -0,0 +1 @@ +INCLUDES += emc/kinematics_userspace diff --git a/src/emc/kinematics_userspace/kinematics_user.c b/src/emc/kinematics_userspace/kinematics_user.c new file mode 100644 index 00000000000..66f840a770b --- /dev/null +++ b/src/emc/kinematics_userspace/kinematics_user.c @@ -0,0 +1,379 @@ +/******************************************************************** + * Description: kinematics_user.c + * Non-RT loader for kinematics modules + * + * Loads a kinematics .so with dlopen and calls the nonrt_attach() it + * exports, so this process evaluates the kinematics the machine is + * running, at whatever poses it likes. See nonrt_kins.h. + * + * Identity kinematics needs no module code: the module says so through + * nonrt_ops_t and this file maps joints to axes directly. A module + * exporting no nonrt_attach() is not an error either; the context comes + * back flagged rt_only. + * + * Author: LinuxCNC + * License: GPL Version 2 + * System: Linux + * + * Copyright (c) 2024 All rights reserved. + ********************************************************************/ + +#include +#include +#include +#include +#include +#include +#include + +#include "config.h" /* EMC2_HOME */ + +typedef int (*nonrt_attach_fn)(const char *coordinates, nonrt_ops_t *ops, + nonrt_resolve_fn resolve, void *arg); + +/* One per value a kinematics module reads is a generous bound. */ +#define MAX_MADE_SIGNALS 16 +#define MAX_BOUND_PINS 16 + +struct KinematicsUserContext { + int initialized; + int rt_only; /* 1 if the module exports no nonrt_attach() */ + int is_identity; /* 1 for identity kinematics: no module code needed */ + KINEMATICS_TYPE kins_type; + void *rt_handle; /* dlopen handle */ + nonrt_ops_t ops; + int num_joints; + int joint_to_axis[KINEMATICS_USER_MAX_JOINTS]; /* identity path only */ + char module_name[64]; + int comp_id; /* the caller's component, owns the pins made here */ + const char *prefix; /* its name, which those pin names start with */ + char made_signal[MAX_MADE_SIGNALS][HAL_NAME_LEN + 1]; + int num_made_signals; + hal_refs_u *cell; /* HAL storage those pins are made against */ + int num_cells; +}; + +/* ======================================================================== + * Pin binding + * ======================================================================== */ + +/* + * Give a kinematics module a reference to a value it asked for. + * + * The reference is to a pin of ours rather than into the RT instance's, + * so that its lifetime is ours: see nonrt_kins.h. Ours is connected to + * the signal the RT pin reads, or, when the RT pin has no signal, to one + * made here and removed again in kinematicsUserFree(). + * + * The reference has to live in HAL shared memory, since that is where + * HAL rewrites it on connect and disconnect, so the pins are made + * against hal_malloc() cells and the module gets what a cell holds once + * the connection is in place. + */ +static int make_signal(KinematicsUserContext *ctx, const char *pin_name, + hal_type_t type, char *out, size_t outlen) +{ + if (ctx->num_made_signals >= MAX_MADE_SIGNALS) { + fprintf(stderr, "kinematicsUserInit: too many signals to create\n"); + return -1; + } + if ((size_t)snprintf(out, outlen, "%s-nonrt", pin_name) >= outlen) { + fprintf(stderr, "kinematicsUserInit: signal name for '%s' too long\n", + pin_name); + return -1; + } + if (hal_signal_new(out, type) != 0) return -1; + if (hal_link(pin_name, out) != 0) { + hal_signal_delete(out); + return -1; + } + snprintf(ctx->made_signal[ctx->num_made_signals++], + sizeof(ctx->made_signal[0]), "%s", out); + return 0; +} + +static int new_pin(int comp_id, hal_type_t type, hal_refs_u *out, + const char *name) +{ + switch (type) { + case HAL_BOOL: return hal_pin_new_bool(comp_id, HAL_IN, &out->b, 0, "%s", name); + case HAL_REAL: return hal_pin_new_real(comp_id, HAL_IN, &out->r, 0.0, "%s", name); + case HAL_SINT: return hal_pin_new_sint(comp_id, HAL_IN, &out->s, 0, "%s", name); + case HAL_UINT: return hal_pin_new_uint(comp_id, HAL_IN, &out->u, 0, "%s", name); + default: break; + } + return -1; +} + +static int bind_pin(const char *pin_name, hal_type_t type, + hal_refs_u *out, void *arg) +{ + KinematicsUserContext *ctx = (KinematicsUserContext *)arg; + char signal[HAL_NAME_LEN + 1]; + char mine[HAL_NAME_LEN + 1]; + hal_refs_u *cell; + hal_query_t q; + + if (!ctx || !pin_name || !out) return -1; + + memset(&q, 0, sizeof(q)); + q.name = pin_name; + q.qtype = HAL_QTYPE_PIN; + + if (hal_getref_p(&q) != 0) { + fprintf(stderr, "kinematicsUserInit: no such pin '%s'\n", pin_name); + return -1; + } + if (q.pp.type != type) { + fprintf(stderr, "kinematicsUserInit: pin '%s' has the wrong type\n", + pin_name); + return -1; + } + + if (q.pp.signal) { + snprintf(signal, sizeof(signal), "%s", q.pp.signal); + } else if (make_signal(ctx, pin_name, type, signal, sizeof(signal))) { + fprintf(stderr, "kinematicsUserInit: cannot reach '%s'\n", pin_name); + return -1; + } + + if ((size_t)snprintf(mine, sizeof(mine), "%s.%s", ctx->prefix, pin_name) + >= sizeof(mine)) { + fprintf(stderr, "kinematicsUserInit: pin name for '%s' too long\n", + pin_name); + return -1; + } + if (ctx->num_cells >= MAX_BOUND_PINS) { + fprintf(stderr, "kinematicsUserInit: too many pins to bind\n"); + return -1; + } + cell = &ctx->cell[ctx->num_cells++]; + + if (new_pin(ctx->comp_id, type, cell, mine) != 0) { + fprintf(stderr, "kinematicsUserInit: cannot create pin '%s'\n", mine); + return -1; + } + if (hal_link(mine, signal) != 0) { + fprintf(stderr, "kinematicsUserInit: cannot link '%s' to '%s'\n", + mine, signal); + return -1; + } + + *out = *cell; + return 0; +} + +/* ======================================================================== + * Identity joint mapping + * ======================================================================== */ + +static void fill_identity_joint_map(KinematicsUserContext *ctx, const char *coords) +{ + int i, j = 0; + for (i = 0; i < KINEMATICS_USER_MAX_JOINTS; i++) ctx->joint_to_axis[i] = -1; + if (!coords) return; + for (; *coords && j < ctx->num_joints; coords++) { + int axis; + switch (tolower((unsigned char)*coords)) { + case 'x': axis = 0; break; case 'y': axis = 1; break; + case 'z': axis = 2; break; case 'a': axis = 3; break; + case 'b': axis = 4; break; case 'c': axis = 5; break; + case 'u': axis = 6; break; case 'v': axis = 7; break; + case 'w': axis = 8; break; default: continue; + } + ctx->joint_to_axis[j++] = axis; + } +} + +/* ======================================================================== + * Module loading + * ======================================================================== */ + +static int load_module(KinematicsUserContext *ctx, + const char *module_name, + const char *coordinates) +{ + char module_path[512]; + void *handle; + nonrt_attach_fn attach; + + snprintf(module_path, sizeof(module_path), + "%s/rtlib/%s.so", EMC2_HOME, module_name); + + handle = dlopen(module_path, RTLD_NOW | RTLD_LOCAL); + if (!handle) { + fprintf(stderr, "kinematicsUserInit: dlopen '%s': %s\n", + module_path, dlerror()); + return -1; + } + ctx->rt_handle = handle; + + attach = (nonrt_attach_fn)dlsym(handle, "nonrt_attach"); + if (!attach) { + fprintf(stderr, "kinematicsUserInit: '%s' exports no nonrt_attach\n", + module_name); + dlclose(handle); + ctx->rt_handle = NULL; + ctx->rt_only = 1; + return -1; + } + + if (attach(coordinates, &ctx->ops, bind_pin, ctx) != 0) { + fprintf(stderr, "kinematicsUserInit: nonrt_attach failed for '%s'\n", + module_name); + dlclose(handle); + ctx->rt_handle = NULL; + ctx->rt_only = 1; + return -1; + } + + if (ctx->ops.is_identity) { + ctx->is_identity = 1; + ctx->kins_type = KINEMATICS_IDENTITY; + return 0; + } + + if (!ctx->ops.forward || !ctx->ops.inverse) { + fprintf(stderr, "kinematicsUserInit: '%s' set no fwd/inv\n", module_name); + dlclose(handle); + ctx->rt_handle = NULL; + ctx->rt_only = 1; + return -1; + } + + ctx->kins_type = KINEMATICS_BOTH; + return 0; +} + +/* ======================================================================== + * Public API + * ======================================================================== */ + +KinematicsUserContext* kinematicsUserInit(const char* kins_type, + int num_joints, + const char* coordinates, + int comp_id, + const char* prefix) +{ + KinematicsUserContext *ctx; + + if (!kins_type || num_joints < 1 || num_joints > KINEMATICS_USER_MAX_JOINTS + || comp_id < 0 || !prefix) { + fprintf(stderr, "kinematicsUserInit: invalid arguments\n"); + return NULL; + } + + ctx = (KinematicsUserContext *)calloc(1, sizeof(KinematicsUserContext)); + if (!ctx) return NULL; + + ctx->num_joints = num_joints; + ctx->comp_id = comp_id; + ctx->prefix = prefix; + + ctx->cell = (hal_refs_u *)hal_malloc(MAX_BOUND_PINS * sizeof(hal_refs_u)); + if (!ctx->cell) { + fprintf(stderr, "kinematicsUserInit: out of HAL memory\n"); + free(ctx); + return NULL; + } + strncpy(ctx->module_name, kins_type, sizeof(ctx->module_name) - 1); + + load_module(ctx, kins_type, coordinates); + + if (ctx->is_identity) { + fill_identity_joint_map(ctx, coordinates); + } + + ctx->initialized = 1; + return ctx; +} + +int kinematicsUserInverse(KinematicsUserContext* ctx, + const EmcPose* world, + double* joints) +{ + if (!ctx || !ctx->initialized || !world || !joints) return -1; + + if (ctx->is_identity) { + int i; + for (i = 0; i < ctx->num_joints; i++) { + int ax = ctx->joint_to_axis[i]; + joints[i] = (ax >= 0) ? emcPoseGetAxis(world, ax) : 0.0; + } + return 0; + } + + if (ctx->rt_only) return -1; + return ctx->ops.inverse(world, joints, NULL, NULL); +} + +int kinematicsUserForward(KinematicsUserContext* ctx, + const double* joints, + EmcPose* world) +{ + if (!ctx || !ctx->initialized || !joints || !world) return -1; + + if (ctx->is_identity) { + int i; + memset(world, 0, sizeof(*world)); + for (i = 0; i < ctx->num_joints; i++) { + int ax = ctx->joint_to_axis[i]; + if (ax >= 0) emcPoseSetAxis(world, ax, joints[i]); + } + return 0; + } + + if (ctx->rt_only) return -1; + return ctx->ops.forward(joints, world, NULL, NULL); +} + +int kinematicsUserIsIdentity(KinematicsUserContext* ctx) +{ + if (!ctx || !ctx->initialized) return 0; + return ctx->is_identity; +} + +int kinematicsUserGetNumJoints(KinematicsUserContext* ctx) +{ + if (!ctx || !ctx->initialized) return 0; + return ctx->num_joints; +} + +KINEMATICS_TYPE kinematicsUserGetType(KinematicsUserContext* ctx) +{ + if (!ctx || !ctx->initialized) return KINEMATICS_IDENTITY; + return ctx->kins_type; +} + +const char* kinematicsUserGetModuleName(KinematicsUserContext* ctx) +{ + if (!ctx || !ctx->initialized) return "unknown"; + return ctx->module_name; +} + +int kinematicsUserRefreshParams(KinematicsUserContext* ctx) +{ + (void)ctx; + return 0; /* nothing to refresh: the bound pins are the live values */ +} + +int kinematicsUserIsRtOnly(KinematicsUserContext* ctx) +{ + if (!ctx || !ctx->initialized) return 1; + return ctx->rt_only; +} + +void kinematicsUserFree(KinematicsUserContext* ctx) +{ + int i; + + if (!ctx) return; + + /* Removing one hands its value back to the RT pin, leaving the + machine as it was found. */ + for (i = 0; i < ctx->num_made_signals; i++) { + hal_signal_delete(ctx->made_signal[i]); + } + if (ctx->rt_handle) dlclose(ctx->rt_handle); + free(ctx); +} diff --git a/src/emc/motion_planning/Submakefile b/src/emc/motion_planning/Submakefile new file mode 100644 index 00000000000..e4267fba11e --- /dev/null +++ b/src/emc/motion_planning/Submakefile @@ -0,0 +1,49 @@ +INCLUDES += emc/motion_planning +INCLUDES += emc/kinematics_userspace + +# Jacobian-based world-space limit calculation, plus the non-RT kinematics +# loader it sits on top of. +LIBKINSLIMITS_CXXSRCS := $(addprefix emc/motion_planning/, \ + jacobian.cc \ + joint_limits.cc \ + ) + +LIBKINSLIMITS_CSRCS := $(addprefix emc/kinematics_userspace/, \ + kinematics_user.c \ + ) + +USERSRCS += $(LIBKINSLIMITS_CXXSRCS) $(LIBKINSLIMITS_CSRCS) + +$(call TOOBJSDEPS, $(LIBKINSLIMITS_CXXSRCS)): EXTRAFLAGS = -fPIC +$(call TOOBJSDEPS, $(LIBKINSLIMITS_CSRCS)): EXTRAFLAGS = -fPIC -D_GNU_SOURCE + +# -Bsymbolic: the library carries its own copy of the kinematics helpers, +# and the loader brings switchkins_core and kins_single_core into scope +# with the same names, which must not take over the library's own calls +../lib/libkinslimits.so.0: $(call TOOBJS, $(LIBKINSLIMITS_CXXSRCS) $(LIBKINSLIMITS_CSRCS)) \ + ../lib/libposemath.so.0 ../lib/liblinuxcnchal.so.0 + $(ECHO) Linking $(notdir $@) + @mkdir -p ../lib + $(Q)$(CXX) $(LDFLAGS) -Wl,-soname,$(notdir $@) -Wl,-Bsymbolic -shared -o $@ $^ -ldl + +../lib/libkinslimits.so: ../lib/libkinslimits.so.0 + ln -sf $(notdir $<) $@ + +TARGETS += ../lib/libkinslimits.so ../lib/libkinslimits.so.0 + +# Diagnostic: print the Jacobian and the caps it implies for one move. +KINSLIMITS_SRCS := emc/motion_planning/kinslimits.cc +USERSRCS += $(KINSLIMITS_SRCS) + +../bin/kinslimits: $(call TOOBJS, $(KINSLIMITS_SRCS)) \ + ../lib/libkinslimits.so.0 ../lib/liblinuxcnchal.so.0 ../lib/libposemath.so.0 + $(ECHO) Linking $(notdir $@) + @mkdir -p ../bin + $(Q)$(CXX) $(LDFLAGS) -o $@ $^ + +TARGETS += ../bin/kinslimits + +MOTION_PLANNING_HH := emc/motion_planning/jacobian.hh emc/motion_planning/joint_limits.hh +$(patsubst emc/motion_planning/%,../include/%,$(MOTION_PLANNING_HH)): ../include/%.hh: emc/motion_planning/%.hh + cp $^ $@ +HEADERS += $(patsubst emc/motion_planning/%,../include/%,$(MOTION_PLANNING_HH)) diff --git a/src/emc/motion_planning/jacobian.cc b/src/emc/motion_planning/jacobian.cc new file mode 100644 index 00000000000..a7d5a7661e7 --- /dev/null +++ b/src/emc/motion_planning/jacobian.cc @@ -0,0 +1,197 @@ +/******************************************************************** + * Description: jacobian.cc + * Jacobian calculation implementation for userspace kinematics trajectory planning + * + * Author: LinuxCNC + * License: GPL Version 2 + * System: Linux + * + * Copyright (c) 2024 All rights reserved. + ********************************************************************/ + +#include "jacobian.hh" +#include +#include +#include + +namespace motion_planning { + +JacobianCalculator::JacobianCalculator() + : kins_ctx_(nullptr), + is_identity_(false), + num_joints_(0) { +} + +JacobianCalculator::~JacobianCalculator() { + // kins_ctx_ is owned externally +} + +bool JacobianCalculator::init(KinematicsUserContext* kins_ctx) { + if (!kins_ctx) { + return false; + } + + kins_ctx_ = kins_ctx; + is_identity_ = (kinematicsUserIsIdentity(kins_ctx) != 0); + num_joints_ = kinematicsUserGetNumJoints(kins_ctx); + + return true; +} + +void JacobianCalculator::computeTrivkins(double J[9][9]) { + // Zero the matrix + std::memset(J, 0, sizeof(double) * 9 * 9); + + // For trivkins, the Jacobian is identity (with axis mapping) + // Since trivkins maps: joint[i] = world_axis[mapped_axis[i]] + // The Jacobian is: J[joint][axis] = 1 if axis == mapped_axis[joint], else 0 + + // For a simple XYZ trivkins: + // J[0][AXIS_X] = 1 (joint 0 = X) + // J[1][AXIS_Y] = 1 (joint 1 = Y) + // J[2][AXIS_Z] = 1 (joint 2 = Z) + // etc. + + // We need to query the kinematics context for the mapping. + // Since the context is opaque, we use inverse kinematics to determine + // the mapping. + + // Test each axis: perturb it and see which joint changes + EmcPose zero_pose; + ZERO_EMC_POSE(zero_pose); + double zero_joints[9]; + kinematicsUserInverse(kins_ctx_, &zero_pose, zero_joints); + + for (int axis = 0; axis < AXIS_COUNT; axis++) { + EmcPose test_pose = zero_pose; + emcPoseSetAxis(&test_pose, axis, 1.0); + + double test_joints[9]; + kinematicsUserInverse(kins_ctx_, &test_pose, test_joints); + + for (int joint = 0; joint < num_joints_; joint++) { + double delta = test_joints[joint] - zero_joints[joint]; + if (std::fabs(delta) > 0.5) { + // This axis maps to this joint + J[joint][axis] = 1.0; + } + } + } +} + +bool JacobianCalculator::computeNumerical(const EmcPose& pose, double J[9][9]) { + // Zero the matrix + std::memset(J, 0, sizeof(double) * 9 * 9); + + // Compute joints at nominal pose + double joints_center[9]; + if (kinematicsUserInverse(kins_ctx_, &pose, joints_center) != 0) { + return false; + } + + // Perturb each axis and compute derivatives + for (int axis = 0; axis < AXIS_COUNT; axis++) { + // Choose perturbation size based on axis type + double delta = (axis < 3 || axis >= 6) ? DELTA_LINEAR : DELTA_ROTARY; + + // Positive perturbation + EmcPose pose_plus = pose; + double val_plus = emcPoseGetAxis(&pose_plus, axis) + delta; + emcPoseSetAxis(&pose_plus, axis, val_plus); + + double joints_plus[9]; + if (kinematicsUserInverse(kins_ctx_, &pose_plus, joints_plus) != 0) { + // Kinematics failed - use one-sided difference + for (int joint = 0; joint < num_joints_; joint++) { + J[joint][axis] = (joints_plus[joint] - joints_center[joint]) / delta; + } + continue; + } + + // Negative perturbation + EmcPose pose_minus = pose; + double val_minus = emcPoseGetAxis(&pose_minus, axis) - delta; + emcPoseSetAxis(&pose_minus, axis, val_minus); + + double joints_minus[9]; + if (kinematicsUserInverse(kins_ctx_, &pose_minus, joints_minus) != 0) { + // Use forward difference + for (int joint = 0; joint < num_joints_; joint++) { + J[joint][axis] = (joints_plus[joint] - joints_center[joint]) / delta; + } + continue; + } + + // Central difference (most accurate) + for (int joint = 0; joint < num_joints_; joint++) { + J[joint][axis] = (joints_plus[joint] - joints_minus[joint]) / (2.0 * delta); + } + } + + // Check for NaN/Inf values and replace with safe defaults + bool had_nan = false; + for (int joint = 0; joint < num_joints_; joint++) { + for (int axis = 0; axis < AXIS_COUNT; axis++) { + if (!std::isfinite(J[joint][axis])) { + // Replace NaN/Inf with 0 (assume no coupling) + J[joint][axis] = 0.0; + had_nan = true; + } + } + } + + // If we had NaN values, the Jacobian may be unreliable + // Return true anyway but the condition number check will catch issues + (void)had_nan; // Could log this in debug mode + + return true; +} + +bool JacobianCalculator::compute(const EmcPose& pose, double J[9][9]) { + if (!kins_ctx_) { + return false; + } + + if (is_identity_) { + // For trivkins, use the fast identity computation + computeTrivkins(J); + return true; + } else { + // For non-trivial kinematics, use numerical differentiation + return computeNumerical(pose, J); + } +} + +double JacobianCalculator::conditionNumber(const double J[9][9]) { + if (is_identity_) { + // Identity matrix has condition number 1 + return 1.0; + } + + // We use a simplified condition number estimate: + // Find the ratio of largest to smallest row norms + // This is not the true 2-norm condition number, but gives a rough indication + + double max_row_norm = 0.0; + double min_row_norm = 1e18; + + for (int joint = 0; joint < num_joints_; joint++) { + double row_norm = 0.0; + for (int axis = 0; axis < AXIS_COUNT; axis++) { + row_norm += J[joint][axis] * J[joint][axis]; + } + row_norm = std::sqrt(row_norm); + + if (row_norm > max_row_norm) max_row_norm = row_norm; + if (row_norm > 1e-15 && row_norm < min_row_norm) min_row_norm = row_norm; + } + + if (min_row_norm < 1e-15) { + // Near-singular: a row is almost zero + return 1e18; + } + + return max_row_norm / min_row_norm; +} + +} // namespace motion_planning diff --git a/src/emc/motion_planning/jacobian.hh b/src/emc/motion_planning/jacobian.hh new file mode 100644 index 00000000000..8713e89f180 --- /dev/null +++ b/src/emc/motion_planning/jacobian.hh @@ -0,0 +1,99 @@ +/******************************************************************** + * Description: jacobian.hh + * Jacobian calculation for userspace kinematics trajectory planning + * + * Computes the Jacobian matrix relating world velocities to joint + * velocities. For trivkins this is the identity matrix. + * + * Author: LinuxCNC + * License: GPL Version 2 + * System: Linux + * + * Copyright (c) 2024 All rights reserved. + ********************************************************************/ +#ifndef JACOBIAN_HH +#define JACOBIAN_HH + +#include +#include + +namespace motion_planning { + +/** + * Jacobian calculator class + * + * Computes the Jacobian matrix J where: + * joint_velocities = J × world_velocities + * + * For trivkins, J is the identity matrix (with appropriate axis mapping). + * For non-trivial kinematics, J is computed via numerical differentiation. + */ +class JacobianCalculator { +public: + JacobianCalculator(); + ~JacobianCalculator(); + + /** + * Initialize with kinematics context + * + * @param kins_ctx Userspace kinematics context + * @return true on success + */ + bool init(KinematicsUserContext* kins_ctx); + + /** + * Compute Jacobian at a given pose + * + * The Jacobian J[joint][axis] relates: + * d(joint[j])/dt = sum over axis a of J[j][a] * d(axis[a])/dt + * + * @param pose World pose at which to compute Jacobian + * @param J Output 9×9 Jacobian matrix [joint][axis] + * @return true on success, false on failure + */ + bool compute(const EmcPose& pose, double J[9][9]); + + /** + * Compute condition number of Jacobian + * + * The condition number indicates how close to a singularity the pose is. + * High condition number = near singularity. + * + * For trivkins, always returns 1.0 (no singularities). + * + * @param J Jacobian matrix + * @return Condition number (≥ 1.0), or -1.0 on error + */ + double conditionNumber(const double J[9][9]); + + /** + * Check if current kinematics is identity (trivkins) + */ + bool isIdentity() const { return is_identity_; } + +private: + /** + * Compute Jacobian for trivkins (identity with axis mapping) + */ + void computeTrivkins(double J[9][9]); + + /** + * Compute Jacobian via numerical differentiation + * Uses central differences: J[j][a] = (f(x+h) - f(x-h)) / (2h) + */ + bool computeNumerical(const EmcPose& pose, double J[9][9]); + + KinematicsUserContext* kins_ctx_; + bool is_identity_; + int num_joints_; + + // Perturbation size for numerical differentiation (mm or degrees) + // Must be large enough for kinematics to produce stable results + // but small enough for accurate derivatives + static constexpr double DELTA_LINEAR = 0.1; // 0.1 mm + static constexpr double DELTA_ROTARY = 0.1; // 0.1 degrees +}; + +} // namespace motion_planning + +#endif // JACOBIAN_HH diff --git a/src/emc/motion_planning/joint_limits.cc b/src/emc/motion_planning/joint_limits.cc new file mode 100644 index 00000000000..ee4ee34d06f --- /dev/null +++ b/src/emc/motion_planning/joint_limits.cc @@ -0,0 +1,358 @@ +/******************************************************************** + * Description: joint_limits.cc + * Joint limit calculation implementation for userspace kinematics trajectory planning + * + * Author: LinuxCNC + * License: GPL Version 2 + * System: Linux + * + * Copyright (c) 2024 All rights reserved. + ********************************************************************/ + +#include "joint_limits.hh" +#include +#include +#include + +namespace motion_planning { + +JointLimitCalculator::JointLimitCalculator() + : num_joints_(0), + initialized_(false) { +} + +JointLimitCalculator::~JointLimitCalculator() { +} + +bool JointLimitCalculator::init(int num_joints) { + if (num_joints < 1 || num_joints > KINEMATICS_USER_MAX_JOINTS) { + return false; + } + + num_joints_ = num_joints; + + // Initialize with default (very permissive) limits + for (int i = 0; i < KINEMATICS_USER_MAX_JOINTS; i++) { + limits_[i] = JointLimitConfig(); + } + + initialized_ = true; + return true; +} + +bool JointLimitCalculator::setJointLimits(int joint, const JointLimitConfig& limits) { + if (joint < 0 || joint >= num_joints_) { + return false; + } + limits_[joint] = limits; + return true; +} + +const JointLimitConfig& JointLimitCalculator::getJointLimits(int joint) const { + static JointLimitConfig default_limits; + if (joint < 0 || joint >= num_joints_) { + return default_limits; + } + return limits_[joint]; +} + +double JointLimitCalculator::getJointVelLimit(int joint) const { + if (joint < 0 || joint >= num_joints_) return 1e9; + return limits_[joint].vel_limit; +} + +double JointLimitCalculator::getJointAccLimit(int joint) const { + if (joint < 0 || joint >= num_joints_) return 1e9; + return limits_[joint].acc_limit; +} + +double JointLimitCalculator::getJointJerkLimit(int joint) const { + if (joint < 0 || joint >= num_joints_) return 1e9; + return limits_[joint].jerk_limit; +} + +bool JointLimitCalculator::updateAllLimits(const double* vel_limits, + const double* acc_limits, + const double* min_pos, + const double* max_pos, + const double* jerk_limits) { + if (!initialized_) { + return false; + } + + // Update limits from arrays + // This is used to refresh limits from shared memory (motion status), + // which reflects any runtime changes via HAL pins (ini.N.max_limit, etc.) + for (int j = 0; j < num_joints_; j++) { + if (vel_limits) limits_[j].vel_limit = vel_limits[j]; + if (acc_limits) limits_[j].acc_limit = acc_limits[j]; + if (min_pos) limits_[j].min_pos_limit = min_pos[j]; + if (max_pos) limits_[j].max_pos_limit = max_pos[j]; + if (jerk_limits) limits_[j].jerk_limit = jerk_limits[j]; + } + + return true; +} + +bool JointLimitCalculator::checkPositionLimits(const double joint_pos[9]) { + for (int j = 0; j < num_joints_; j++) { + if (joint_pos[j] > limits_[j].max_pos_limit || + joint_pos[j] < limits_[j].min_pos_limit) { + return false; + } + } + return true; +} + +double JointLimitCalculator::computeMaxVelocity(const double J[9][9], int& limiting_joint) { + // Conservative estimate: assume worst-case direction + // For each joint j, find the maximum Jacobian element magnitude + // max_world_vel = min over j of: vel_limit[j] / max(|J[j][:]|) + + double max_world_vel = 1e18; + limiting_joint = -1; + + for (int j = 0; j < num_joints_; j++) { + // Find maximum absolute value in this row of J + double max_abs_J = 0.0; + for (int a = 0; a < AXIS_COUNT; a++) { + double abs_J = std::fabs(J[j][a]); + if (abs_J > max_abs_J) { + max_abs_J = abs_J; + } + } + + if (max_abs_J > 1e-15) { + // This joint contributes to motion + double vel_limit_world = limits_[j].vel_limit / max_abs_J; + if (vel_limit_world < max_world_vel) { + max_world_vel = vel_limit_world; + limiting_joint = j; + } + } + } + + // Apply sanity bounds + if (max_world_vel > 1e9) max_world_vel = 1e9; + if (max_world_vel < 1e-9) max_world_vel = 1e-9; + + return max_world_vel; +} + +double JointLimitCalculator::computeMaxAcceleration(const double J[9][9], int& limiting_joint) { + // Same approach as velocity + double max_world_acc = 1e18; + limiting_joint = -1; + + for (int j = 0; j < num_joints_; j++) { + double max_abs_J = 0.0; + for (int a = 0; a < AXIS_COUNT; a++) { + double abs_J = std::fabs(J[j][a]); + if (abs_J > max_abs_J) { + max_abs_J = abs_J; + } + } + + if (max_abs_J > 1e-15) { + double acc_limit_world = limits_[j].acc_limit / max_abs_J; + if (acc_limit_world < max_world_acc) { + max_world_acc = acc_limit_world; + limiting_joint = j; + } + } + } + + if (max_world_acc > 1e9) max_world_acc = 1e9; + if (max_world_acc < 1e-9) max_world_acc = 1e-9; + + return max_world_acc; +} + +double JointLimitCalculator::computeMaxJerk(const double J[9][9], int& limiting_joint) { + // Same approach as velocity and acceleration + double max_world_jerk = 1e18; + limiting_joint = -1; + + for (int j = 0; j < num_joints_; j++) { + double max_abs_J = 0.0; + for (int a = 0; a < AXIS_COUNT; a++) { + double abs_J = std::fabs(J[j][a]); + if (abs_J > max_abs_J) { + max_abs_J = abs_J; + } + } + + if (max_abs_J > 1e-15) { + double jerk_limit_world = limits_[j].jerk_limit / max_abs_J; + if (jerk_limit_world < max_world_jerk) { + max_world_jerk = jerk_limit_world; + limiting_joint = j; + } + } + } + + if (max_world_jerk > 1e9) max_world_jerk = 1e9; + if (max_world_jerk < 1e-9) max_world_jerk = 1e-9; + + return max_world_jerk; +} + +double JointLimitCalculator::computeMaxVelocityForTangent(const double J[9][9], const double tangent[9], int& limiting_joint) { + double max_world_vel = 1e18; + limiting_joint = -1; + + for (int j = 0; j < num_joints_; j++) { + // Compute sum(|J[j][a]| * |tangent[a]|) — the actual amplification + // for this joint along the given path direction + double amplification = 0.0; + for (int a = 0; a < AXIS_COUNT; a++) { + amplification += std::fabs(J[j][a]) * std::fabs(tangent[a]); + } + + if (amplification > 1e-15) { + double vel_limit_world = limits_[j].vel_limit / amplification; + if (vel_limit_world < max_world_vel) { + max_world_vel = vel_limit_world; + limiting_joint = j; + } + } + } + + if (max_world_vel > 1e9) max_world_vel = 1e9; + if (max_world_vel < 1e-9) max_world_vel = 1e-9; + return max_world_vel; +} + +double JointLimitCalculator::computeMaxAccelerationForTangent(const double J[9][9], const double tangent[9], int& limiting_joint) { + double max_world_acc = 1e18; + limiting_joint = -1; + + for (int j = 0; j < num_joints_; j++) { + double amplification = 0.0; + for (int a = 0; a < AXIS_COUNT; a++) { + amplification += std::fabs(J[j][a]) * std::fabs(tangent[a]); + } + + if (amplification > 1e-15) { + double acc_limit_world = limits_[j].acc_limit / amplification; + if (acc_limit_world < max_world_acc) { + max_world_acc = acc_limit_world; + limiting_joint = j; + } + } + } + + if (max_world_acc > 1e9) max_world_acc = 1e9; + if (max_world_acc < 1e-9) max_world_acc = 1e-9; + return max_world_acc; +} + +double JointLimitCalculator::computeMaxJerkForTangent(const double J[9][9], const double tangent[9], int& limiting_joint) { + double max_world_jerk = 1e18; + limiting_joint = -1; + + for (int j = 0; j < num_joints_; j++) { + double amplification = 0.0; + for (int a = 0; a < AXIS_COUNT; a++) { + amplification += std::fabs(J[j][a]) * std::fabs(tangent[a]); + } + + if (amplification > 1e-15) { + double jerk_limit_world = limits_[j].jerk_limit / amplification; + if (jerk_limit_world < max_world_jerk) { + max_world_jerk = jerk_limit_world; + limiting_joint = j; + } + } + } + + if (max_world_jerk > 1e9) max_world_jerk = 1e9; + if (max_world_jerk < 1e-9) max_world_jerk = 1e-9; + return max_world_jerk; +} + +bool JointLimitCalculator::computeForTangent(const double J[9][9], + const double joint_pos[9], + const double tangent[9], + JointLimitResult& result, + double singularity_threshold) { + if (!initialized_) { + return false; + } + + result.position_ok = checkPositionLimits(joint_pos); + result.condition_number = computeConditionNumber(J); + + result.max_world_vel = computeMaxVelocityForTangent(J, tangent, result.limiting_joint_vel); + result.max_world_acc = computeMaxAccelerationForTangent(J, tangent, result.limiting_joint_acc); + result.max_world_jerk = computeMaxJerkForTangent(J, tangent, result.limiting_joint_jerk); + + if (result.condition_number > singularity_threshold) { + double slowdown_factor = singularity_threshold / result.condition_number; + result.max_world_vel *= slowdown_factor; + result.max_world_acc *= slowdown_factor; + result.max_world_jerk *= slowdown_factor; + } + + return true; +} + +double JointLimitCalculator::computeConditionNumber(const double J[9][9]) { + // Simplified condition number: ratio of max to min row norms + double max_row_norm = 0.0; + double min_row_norm = 1e18; + + for (int j = 0; j < num_joints_; j++) { + double row_norm = 0.0; + for (int a = 0; a < AXIS_COUNT; a++) { + row_norm += J[j][a] * J[j][a]; + } + row_norm = std::sqrt(row_norm); + + if (row_norm > max_row_norm) max_row_norm = row_norm; + if (row_norm > 1e-15 && row_norm < min_row_norm) min_row_norm = row_norm; + } + + if (min_row_norm < 1e-15) { + return 1e18; // Near-singular + } + + return max_row_norm / min_row_norm; +} + +bool JointLimitCalculator::compute(const double J[9][9], + const double joint_pos[9], + JointLimitResult& result, + double singularity_threshold) { + if (!initialized_) { + return false; + } + + // Check position limits + result.position_ok = checkPositionLimits(joint_pos); + + // Compute condition number + result.condition_number = computeConditionNumber(J); + + // Compute max velocity + result.max_world_vel = computeMaxVelocity(J, result.limiting_joint_vel); + + // Compute max acceleration + result.max_world_acc = computeMaxAcceleration(J, result.limiting_joint_acc); + + // Compute max jerk + result.max_world_jerk = computeMaxJerk(J, result.limiting_joint_jerk); + + // Apply singularity slowdown + // If condition number exceeds threshold, reduce limits proportionally + if (result.condition_number > singularity_threshold) { + double slowdown_factor = singularity_threshold / result.condition_number; + result.max_world_vel *= slowdown_factor; + result.max_world_acc *= slowdown_factor; + result.max_world_jerk *= slowdown_factor; + } + + return true; +} + +} // namespace motion_planning diff --git a/src/emc/motion_planning/joint_limits.hh b/src/emc/motion_planning/joint_limits.hh new file mode 100644 index 00000000000..d8bcbd82b57 --- /dev/null +++ b/src/emc/motion_planning/joint_limits.hh @@ -0,0 +1,234 @@ +/******************************************************************** + * Description: joint_limits.hh + * Joint limit calculation for userspace kinematics trajectory planning + * + * Uses the Jacobian to compute maximum world-space velocity and + * acceleration that respects all joint limits. + * + * Author: LinuxCNC + * License: GPL Version 2 + * System: Linux + * + * Copyright (c) 2024 All rights reserved. + ********************************************************************/ +#ifndef JOINT_LIMITS_HH +#define JOINT_LIMITS_HH + +#include +#include + +namespace motion_planning { + +/** + * Joint limit configuration + * Mirrors emcmot_joint_t limits from motion.h + */ +struct JointLimitConfig { + double max_pos_limit; // Upper soft limit on joint position + double min_pos_limit; // Lower soft limit on joint position + double vel_limit; // Maximum joint velocity + double acc_limit; // Maximum joint acceleration + double jerk_limit; // Maximum joint jerk (for S-curve planning) + + JointLimitConfig() : + max_pos_limit(1e9), + min_pos_limit(-1e9), + vel_limit(1e9), + acc_limit(1e9), + jerk_limit(1e9) {} +}; + +/** + * Result of joint limit calculation + */ +struct JointLimitResult { + double max_world_vel; // Max world velocity respecting joint vel limits + double max_world_acc; // Max world accel respecting joint acc limits + double max_world_jerk; // Max world jerk (for S-curve planning) + bool position_ok; // True if joint positions are within soft limits + int limiting_joint_vel; // Joint index that limits velocity (-1 if none) + int limiting_joint_acc; // Joint index that limits acceleration + int limiting_joint_jerk; // Joint index that limits jerk + double condition_number; // Jacobian condition number (singularity indicator) + + JointLimitResult() : + max_world_vel(1e9), + max_world_acc(1e9), + max_world_jerk(1e9), + position_ok(true), + limiting_joint_vel(-1), + limiting_joint_acc(-1), + limiting_joint_jerk(-1), + condition_number(1.0) {} +}; + +/** + * Joint limit calculator class + * + * Computes maximum world-space velocity/acceleration that respects + * all joint limits, given the Jacobian at a pose. + * + * The relationship is: + * joint_vel = J × world_vel + * |joint_vel[j]| ≤ joint_limit[j].vel_limit for all j + * + * To find max world velocity, we solve: + * max_world_vel = min over all joints j of: + * joint_limit[j].vel_limit / |J[j] · direction| + * + * For a general direction, we use a conservative estimate: + * max_world_vel = min over all joints j of: + * joint_limit[j].vel_limit / max(|J[j][:]|) + */ +class JointLimitCalculator { +public: + JointLimitCalculator(); + ~JointLimitCalculator(); + + /** + * Initialize with number of joints + * + * @param num_joints Number of joints + * @return true on success + */ + bool init(int num_joints); + + /** + * Set limits for a joint + * + * @param joint Joint index (0 to num_joints-1) + * @param limits Limit configuration for this joint + * @return true on success + */ + bool setJointLimits(int joint, const JointLimitConfig& limits); + + /** + * Update limits for all joints at once + * + * This is used to refresh limits from shared memory (motion status structure), + * which reflects any runtime changes via HAL pins (ini.N.max_limit, etc.) + * + * @param vel_limits Array of velocity limits [num_joints] + * @param acc_limits Array of acceleration limits [num_joints] + * @param min_pos Array of min position limits [num_joints] + * @param max_pos Array of max position limits [num_joints] + * @param jerk_limits Array of jerk limits [num_joints] (can be NULL) + * @return true on success + */ + bool updateAllLimits(const double* vel_limits, + const double* acc_limits, + const double* min_pos, + const double* max_pos, + const double* jerk_limits = nullptr); + + /** + * Get limits for a joint + */ + const JointLimitConfig& getJointLimits(int joint) const; + + /** + * Get velocity limit for a specific joint + */ + double getJointVelLimit(int joint) const; + + /** + * Get acceleration limit for a specific joint + */ + double getJointAccLimit(int joint) const; + + /** + * Get jerk limit for a specific joint + */ + double getJointJerkLimit(int joint) const; + + /** + * Compute world-space limits at a pose given the Jacobian + * + * Uses conservative direction-independent bound (max |J[j][:]|). + * + * @param J Jacobian matrix [joint][axis] + * @param joint_pos Current joint positions (for position limit check) + * @param result Output limit result + * @param singularity_threshold Condition number threshold for singularity + * @return true on success + */ + bool compute(const double J[9][9], + const double joint_pos[9], + JointLimitResult& result, + double singularity_threshold = 100.0); + + /** + * Compute world-space limits for a specific path tangent direction + * + * Uses the actual path tangent to compute tight bounds. The tangent + * is in world-axis units per unit of the Ruckig path parameter (which + * may be XYZ arc length). Rotary components can be >> 1.0 when + * rotary axes move much more than linear axes per unit path. + * + * The bound for each joint is: + * limit[j] / sum(|J[j][a]| * |tangent[a]|) + * + * @param J Jacobian matrix [joint][axis] + * @param joint_pos Current joint positions (for position limit check) + * @param tangent Path tangent: d(world_axis)/d(path_param) [9] + * @param result Output limit result + * @param singularity_threshold Condition number threshold for singularity + * @return true on success + */ + bool computeForTangent(const double J[9][9], + const double joint_pos[9], + const double tangent[9], + JointLimitResult& result, + double singularity_threshold = 100.0); + + /** + * Check if joint positions are within soft limits + * + * @param joint_pos Array of joint positions + * @return true if all joints within limits + */ + bool checkPositionLimits(const double joint_pos[9]); + + /** + * Get the number of joints + */ + int getNumJoints() const { return num_joints_; } + +private: + /** + * Compute maximum world velocity from joint velocity limits and Jacobian + * + * Uses conservative estimate: max over all directions + */ + double computeMaxVelocity(const double J[9][9], int& limiting_joint); + + /** + * Compute maximum world acceleration from joint accel limits and Jacobian + */ + double computeMaxAcceleration(const double J[9][9], int& limiting_joint); + + /** + * Compute maximum world jerk from joint jerk limits and Jacobian + */ + double computeMaxJerk(const double J[9][9], int& limiting_joint); + + /** + * Tangent-aware versions: use sum(|J[j][a]| * |tangent[a]|) instead of max(|J[j][a]|) + */ + double computeMaxVelocityForTangent(const double J[9][9], const double tangent[9], int& limiting_joint); + double computeMaxAccelerationForTangent(const double J[9][9], const double tangent[9], int& limiting_joint); + double computeMaxJerkForTangent(const double J[9][9], const double tangent[9], int& limiting_joint); + + /** + * Compute Jacobian condition number (simplified) + */ + double computeConditionNumber(const double J[9][9]); + + int num_joints_; + JointLimitConfig limits_[KINEMATICS_USER_MAX_JOINTS]; + bool initialized_; +}; + +} // namespace motion_planning + +#endif // JOINT_LIMITS_HH diff --git a/src/emc/motion_planning/kinslimits.cc b/src/emc/motion_planning/kinslimits.cc new file mode 100644 index 00000000000..1b750be6776 --- /dev/null +++ b/src/emc/motion_planning/kinslimits.cc @@ -0,0 +1,269 @@ +/******************************************************************** + * Description: kinslimits.cc + * Diagnostic tool: print the Jacobian and the world-space velocity, + * acceleration and jerk caps that a given kinematics module imposes + * on a straight move between two poses. + * + * The tool attaches to a running HAL instance, loads the kinematics + * module through the non-RT interface, samples the move, and reports + * the most restrictive cap found along it. The sampling loop here is + * the same one the trajectory planner uses to cap a segment. + * + * Example (in a terminal with a running config, or under halrun): + * + * halrun -I + * halcmd: loadrt 5axiskins coordinates=XYZBCW + * halcmd: setp 5axiskins.pivot-length 100 + * halcmd: loadusr -w kinslimits --module 5axiskins --joints 6 \ + * --coords XYZBCW --start 0,0,0,0,0,0,0,0,0 \ + * --end 100,0,0,0,90,0,0,0,0 \ + * --vel 100,100,100,30,30,30 --acc 500,500,500,200,200,200 + * + * Author: LinuxCNC + * License: GPL Version 2 + * System: Linux + * + * Copyright (c) 2024 All rights reserved. + ********************************************************************/ + +#include +#include +#include +#include +#include +#include + +#include +#include "jacobian.hh" +#include "joint_limits.hh" + +using namespace motion_planning; + +static const char *AXIS_NAME[9] = {"X","Y","Z","A","B","C","U","V","W"}; + +static std::vector parse_list(const char *s) +{ + std::vector out; + const char *p = s; + while (*p) { + char *endp = nullptr; + double v = strtod(p, &endp); + if (endp == p) break; + out.push_back(v); + p = endp; + while (*p == ',' || *p == ' ') p++; + } + return out; +} + +static void list_to_pose(const std::vector& v, EmcPose *p) +{ + double a[9] = {0,0,0,0,0,0,0,0,0}; + for (size_t i = 0; i < v.size() && i < 9; i++) a[i] = v[i]; + p->tran.x = a[0]; p->tran.y = a[1]; p->tran.z = a[2]; + p->a = a[3]; p->b = a[4]; p->c = a[5]; + p->u = a[6]; p->v = a[7]; p->w = a[8]; +} + +static double pose_axis(const EmcPose& p, int ax) +{ + switch (ax) { + case 0: return p.tran.x; case 1: return p.tran.y; case 2: return p.tran.z; + case 3: return p.a; case 4: return p.b; case 5: return p.c; + case 6: return p.u; case 7: return p.v; default: return p.w; + } +} + +static void set_pose_axis(EmcPose *p, int ax, double val) +{ + switch (ax) { + case 0: p->tran.x = val; break; case 1: p->tran.y = val; break; + case 2: p->tran.z = val; break; case 3: p->a = val; break; + case 4: p->b = val; break; case 5: p->c = val; break; + case 6: p->u = val; break; case 7: p->v = val; break; + default: p->w = val; break; + } +} + +static void usage(const char *argv0) +{ + fprintf(stderr, + "usage: %s --module NAME --joints N --coords LETTERS\n" + " --start x,y,z,a,b,c,u,v,w --end x,y,z,a,b,c,u,v,w\n" + " --vel v0,v1,... --acc a0,a1,... [--jerk j0,j1,...]\n" + " [--samples N] [--singularity COND]\n" + "\n" + "Prints the Jacobian and the world-space caps the joint limits imply\n" + "for a straight move from --start to --end. Requires a running HAL\n" + "instance with the kinematics module loaded.\n", argv0); +} + +int main(int argc, char **argv) +{ + const char *module = nullptr; + const char *coords = nullptr; + int num_joints = 0; + int samples = 11; + double singularity = 100.0; + std::vector start_v, end_v, vel_v, acc_v, jerk_v; + + for (int i = 1; i < argc; i++) { + const char *a = argv[i]; + const char *next = (i + 1 < argc) ? argv[i + 1] : nullptr; + if (!strcmp(a, "--module") && next) { module = next; i++; } + else if (!strcmp(a, "--coords") && next) { coords = next; i++; } + else if (!strcmp(a, "--joints") && next) { num_joints = atoi(next); i++; } + else if (!strcmp(a, "--samples") && next) { samples = atoi(next); i++; } + else if (!strcmp(a, "--singularity") && next){ singularity = atof(next); i++; } + else if (!strcmp(a, "--start") && next) { start_v = parse_list(next); i++; } + else if (!strcmp(a, "--end") && next) { end_v = parse_list(next); i++; } + else if (!strcmp(a, "--vel") && next) { vel_v = parse_list(next); i++; } + else if (!strcmp(a, "--acc") && next) { acc_v = parse_list(next); i++; } + else if (!strcmp(a, "--jerk") && next) { jerk_v = parse_list(next); i++; } + else { usage(argv[0]); return 1; } + } + + if (!module || !coords || num_joints < 1 || + start_v.empty() || end_v.empty() || vel_v.empty() || acc_v.empty()) { + usage(argv[0]); + return 1; + } + if ((int)vel_v.size() < num_joints || (int)acc_v.size() < num_joints) { + fprintf(stderr, "kinslimits: --vel and --acc need %d entries\n", num_joints); + return 1; + } + if (samples < 2) samples = 2; + + int comp_id = hal_init("kinslimits"); + if (comp_id < 0) { + fprintf(stderr, "kinslimits: hal_init failed (is HAL running?)\n"); + return 1; + } + + KinematicsUserContext *ctx = kinematicsUserInit(module, num_joints, coords, + comp_id, "kinslimits"); + if (!ctx) { + fprintf(stderr, "kinslimits: kinematicsUserInit failed for '%s'\n", module); + hal_exit(comp_id); + return 1; + } + if (kinematicsUserIsRtOnly(ctx)) { + fprintf(stderr, "kinslimits: '%s' is RT-only, no non-RT interface\n", module); + kinematicsUserFree(ctx); + hal_exit(comp_id); + return 1; + } + + JacobianCalculator jac; + JointLimitCalculator lim; + if (!jac.init(ctx) || !lim.init(num_joints)) { + fprintf(stderr, "kinslimits: calculator init failed\n"); + kinematicsUserFree(ctx); + hal_exit(comp_id); + return 1; + } + + std::vector minpos(num_joints, -1e9), maxpos(num_joints, 1e9); + if ((int)jerk_v.size() < num_joints) jerk_v.assign(num_joints, 1e9); + lim.updateAllLimits(vel_v.data(), acc_v.data(), + minpos.data(), maxpos.data(), jerk_v.data()); + + EmcPose start, end; + list_to_pose(start_v, &start); + list_to_pose(end_v, &end); + + /* Path parameter: XYZ arc length, falling back to the largest rotary + delta for a pure rotary move, matching what the planner uses. */ + double dx = end.tran.x - start.tran.x; + double dy = end.tran.y - start.tran.y; + double dz = end.tran.z - start.tran.z; + double target = sqrt(dx*dx + dy*dy + dz*dz); + if (target < 1e-12) { + for (int ax = 3; ax < 9; ax++) { + double d = fabs(pose_axis(end, ax) - pose_axis(start, ax)); + if (d > target) target = d; + } + } + if (target < 1e-12) { + fprintf(stderr, "kinslimits: start and end are the same pose\n"); + kinematicsUserFree(ctx); + hal_exit(comp_id); + return 1; + } + + /* tangent[a] = d(world axis a) / d(path parameter) */ + double tangent[9]; + for (int ax = 0; ax < 9; ax++) { + tangent[ax] = (pose_axis(end, ax) - pose_axis(start, ax)) / target; + } + + printf("module : %s (%s, %d joints)%s\n", module, coords, num_joints, + kinematicsUserIsIdentity(ctx) ? " [identity]" : ""); + printf("path length : %.6f (tangent units per path unit)\n", target); + printf("tangent :"); + for (int ax = 0; ax < 9; ax++) { + if (fabs(tangent[ax]) > 1e-12) printf(" %s=%.4f", AXIS_NAME[ax], tangent[ax]); + } + printf("\n\n"); + + double min_vel = 1e9, min_acc = 1e9, min_jerk = 1e9, max_cond = 1.0; + int at_vel = -1, at_acc = -1, at_jerk = -1; + double min_vel_s = 0.0; + + for (int i = 0; i < samples; i++) { + double frac = (double)i / (double)(samples - 1); + EmcPose p; + for (int ax = 0; ax < 9; ax++) { + set_pose_axis(&p, ax, + pose_axis(start, ax) + frac * (pose_axis(end, ax) - pose_axis(start, ax))); + } + + double joints[KINEMATICS_USER_MAX_JOINTS] = {0}; + if (kinematicsUserInverse(ctx, &p, joints) != 0) { + printf("sample %2d: inverse kinematics failed\n", i); + continue; + } + + double J[9][9]; + if (!jac.compute(p, J)) { + printf("sample %2d: Jacobian failed\n", i); + continue; + } + + double jpad[9] = {0}; + for (int j = 0; j < num_joints && j < 9; j++) jpad[j] = joints[j]; + + JointLimitResult r; + if (!lim.computeForTangent(J, jpad, tangent, r, singularity)) { + printf("sample %2d: limit calculation failed\n", i); + continue; + } + + printf("s=%.3f vel<=%10.3f (j%d) acc<=%10.1f (j%d) jerk<=%12.1f (j%d) cond=%.2f\n", + frac, r.max_world_vel, r.limiting_joint_vel, + r.max_world_acc, r.limiting_joint_acc, + r.max_world_jerk, r.limiting_joint_jerk, r.condition_number); + + if (r.max_world_vel < min_vel) { min_vel = r.max_world_vel; at_vel = r.limiting_joint_vel; min_vel_s = frac; } + if (r.max_world_acc < min_acc) { min_acc = r.max_world_acc; at_acc = r.limiting_joint_acc; } + if (r.max_world_jerk < min_jerk) { min_jerk = r.max_world_jerk; at_jerk = r.limiting_joint_jerk; } + if (r.condition_number > max_cond) max_cond = r.condition_number; + + if (i == 0) { + printf(" Jacobian at start (rows = joints, cols = XYZABCUVW):\n"); + for (int j = 0; j < num_joints && j < 9; j++) { + printf(" j%d:", j); + for (int ax = 0; ax < 9; ax++) printf(" %8.4f", J[j][ax]); + printf("\n"); + } + } + } + + printf("\nsegment cap : vel %.3f (joint %d at s=%.3f), acc %.1f (joint %d), jerk %.1f (joint %d)\n", + min_vel, at_vel, min_vel_s, min_acc, at_acc, min_jerk, at_jerk); + printf("worst cond : %.3f\n", max_cond); + + kinematicsUserFree(ctx); + hal_exit(comp_id); + return 0; +} From 33526db379bf02bf857fbabdde0815e07cd38d46 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Fri, 4 Sep 2026 12:44:16 +1000 Subject: [PATCH 02/19] kinematics: add the parameter block form of a module A module reads its geometry from pins it created and keeps its type and iteration scratch in statics, so it can only answer for the machine as it is now, from inside realtime; a planner, a load-time check or a preview had to carry a second copy of the maths. Add the form in which the caller supplies everything: a kins_params block with the type, the joint map, the tool and the geometry, a kins_scratch for what an iterative method carries between calls, and a kins_ops table with the forward, inverse, frames and Jacobian of one type as functions of the two. A module declares its geometry as a table of named entries; in RT the shared code makes one pin per entry under the names configs already use and copies pins in and outputs out around every call, so the maths never touches a pin. kins_single.c supplies the classic entry points for a module with one type, switchkins.c for several, dispatching a type registered with switchkinsRegisterOps() through the block and the older registrations as before, so modules convert one at a time. Every converted module exports kinsDescribe() for callers outside RT; one that does not reports itself RT-only. trivkins, 5axiskins and userkfuncs convert here. The non-RT loader moves onto kinsDescribe() and nonrt_attach() goes; jacobian.cc asks the module for its Jacobian. An iterating ops forward restarts from the pose it last solved and otherwise from the caller's estimate, keeping a result only when the solve succeeded, and the gui forward of a pure type is seeded the same way. kinslimits reports the same caps for 5axiskins to the digit. --- include/kinematics.h | 257 ++++++++- include/kinematics_user.h | 61 ++- include/kins_rt.h | 64 +++ include/nonrt_kins.h | 95 ---- include/switchkins.h | 41 +- src/Makefile | 10 + src/emc/kinematics/5axiskins.c | 280 +++------- src/emc/kinematics/kins_single.c | 156 ++++++ src/emc/kinematics/kins_single_module.c | 23 + src/emc/kinematics/kins_util.c | 498 +++++++++++++++++- src/emc/kinematics/switchkins.c | 366 +++++++++++-- src/emc/kinematics/switchkins_main.c | 42 +- src/emc/kinematics/switchkins_setup.c | 29 + src/emc/kinematics/trivkins.c | 126 ++--- src/emc/kinematics/userkfuncs.c | 34 ++ .../kinematics_userspace/kinematics_user.c | 369 +++++++++---- src/emc/motion_planning/Submakefile | 4 +- src/emc/motion_planning/jacobian.cc | 121 +---- src/emc/motion_planning/jacobian.hh | 24 +- src/hal/components/millturn.comp | 2 +- src/hal/components/xyzab_tdr_kins.comp | 2 +- src/hal/components/xyzacb_trsrn.comp | 2 +- src/hal/components/xyzbca_trsrn.comp | 2 +- 23 files changed, 1840 insertions(+), 768 deletions(-) create mode 100644 include/kins_rt.h delete mode 100644 include/nonrt_kins.h create mode 100644 src/emc/kinematics/kins_single.c create mode 100644 src/emc/kinematics/kins_single_module.c create mode 100644 src/emc/kinematics/switchkins_setup.c diff --git a/include/kinematics.h b/include/kinematics.h index 4820b806833..7a46a4ab957 100644 --- a/include/kinematics.h +++ b/include/kinematics.h @@ -107,6 +107,27 @@ extern int kinematicsHome(struct EmcPose * world, extern KINEMATICS_TYPE kinematicsType(void); +/* Switchable kinematics: a module provides several kinematics, numbered +** 0..SWITCHKINS_MAX_TYPES-1, and motion runs one of them at a time. +** The count is here, not in switchkins.h, because motion and the NML +** status channel need it; it aliases KINS_MAX_TYPES below. +*/ +#define SWITCHKINS_MAX_TYPES KINS_MAX_TYPES + +/* What a kinematics type IS, declared by the module with +** switchkinsDeclare() and read back with kinematicsTypeFlags(). +** G13.1 resolves "identity" from these flags instead of assuming a +** number; a module that declares nothing leaves its types numeric-only +** and G13.1 refuses to guess. +*/ +#define KINSTYPE_IDENTITY 0x1 /* no transform: the joints are the world */ +#define KINSTYPE_PRIMARY 0x2 /* the module's working transform */ + +/* flags of a kinematics type, 0 when neither applies, or -1 for a type +** the module does not provide (and for every type on a machine with plain +** kinematics); G12.1 refuses a type that returns -1 */ +extern int kinematicsTypeFlags(int ktype); + /* These two give the orientation of the tool and of the workpiece for a set of joint values. Each returns a rotation whose columns are that frame's axes expressed in MACHINE coordinates, the frame fixed to the bed that @@ -161,27 +182,6 @@ extern int kinematicsWorkFrame(const double *joint, PmRotationMatrix *rot, const KINEMATICS_FORWARD_FLAGS *fflags); -/* Switchable kinematics: a module provides several kinematics, numbered -** 0..SWITCHKINS_MAX_TYPES-1, and motion runs one of them at a time. -** The count is here, not in switchkins.h, because motion and the NML -** status channel need it. -*/ -#define SWITCHKINS_MAX_TYPES 9 - -/* What a kinematics type IS, declared by the module with -** switchkinsDeclare() and read back with kinematicsTypeFlags(). -** G13.1 resolves "identity" from these flags instead of assuming a -** number; a module that declares nothing leaves its types numeric-only -** and G13.1 refuses to guess. -*/ -#define KINSTYPE_IDENTITY 0x1 /* no transform: the joints are the world */ -#define KINSTYPE_PRIMARY 0x2 /* the module's working transform */ - -/* flags of a kinematics type, 0 when neither applies, or -1 for a type -** the module does not provide (and for every type on a machine with plain -** kinematics); G12.1 refuses a type that returns -1 */ -extern int kinematicsTypeFlags(int ktype); - /* parameters for use with switchkins.c */ typedef struct kinematics_parms { char* sparm; // module string parameter passed to kins @@ -198,6 +198,8 @@ typedef struct kinematics_parms { // bitmask: 0x4 bit2: switchkins_type==2 int gui_kinstype; // may be reqd for parallel kins with vismach // to select switchkins_type for gui pins + const struct kins_param_desc_tag *params; // geometry table, see below + int nparams; } kparms; /* map letters in a coordinates string to joint numbers @@ -477,6 +479,217 @@ extern int identityKinematicsJacobian(const double *joint, double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], const KINEMATICS_INVERSE_FLAGS *iflags); +/* ------------------------------------------------------------------------ + Kinematics as pure functions of what the caller passes in. + + Everything above reads its geometry from HAL pins the module created and + keeps its mode and scratch in statics, so it can only answer for the + machine as it is now, from inside the module. The forms below take the + same questions with the machine described by the caller: a parameter + block naming the kinematics type, the joint map, the tool and the + geometry, and a scratch block for what an iterative method carries + between calls. Nothing is read from HAL and nothing is kept, so one copy + of the maths serves motion, a planner evaluating poses the machine has + not reached, task checking a program at load, and a tool asking what if. + + A module declares its geometry as a table of named entries. In RT the + shared code makes one HAL pin per entry, with the names configs already + use, and copies the pins into the block before every call; outside RT the + caller fills the block from wherever it likes. The maths reads + p->geometry[i] where it read a pin. + + The existing entry points stay and are supplied once, by kins_single.c + for a module with one kinematics type and by switchkins.c for one with + several, so nothing that calls kinematicsForward() changes. A module + that does not provide these forms keeps working as it did; it just cannot + be evaluated outside RT. + ------------------------------------------------------------------------ */ + +#define KINS_MAX_PARAMS 96 /* genhexkins declares 84 */ +#define KINS_MAX_TYPES 9 /* kinematics types a module may provide */ + +typedef enum { + KINS_PARAM_FLOAT = 0, + KINS_PARAM_BIT, + KINS_PARAM_S32, + KINS_PARAM_U32 +} kins_param_type; + +typedef enum { + KINS_IN = 0, /* read into the block before a call */ + KINS_OUT, /* a result, written from kins_scratch.out[] after it */ + KINS_IO /* read like an input; the pin is HAL_IO so it can be poked */ +} kins_param_dir; + +/* One entry of a module's geometry table. name follows the module's HAL + prefix. An entry with tool set is the tool length along the tool axis: + the shared code puts its value in kins_params.tool.tran.z as well, which + is what the maths should read, so that a caller outside RT can supply + the tool from the tool table without there being a pin. */ +typedef struct kins_param_desc_tag { + const char *name; + kins_param_type type; + kins_param_dir dir; + int tool; + double dflt; +} kins_param_desc; + +/* The machine, as far as the kinematics is concerned. One copy may be + shared by any number of callers: nothing writes it during a call. */ +typedef struct kins_params { + int size; /* sizeof(kins_params) */ + int ktype; /* kinematics type, 0 if one */ + int max_joints; /* joints the map covers */ + int joint_of_axis[EMCMOT_MAX_AXIS]; /* principal joint per letter */ + int joints_of_axis[EMCMOT_MAX_AXIS]; /* bit per joint, duplicates */ + EmcPose tool; /* tool offset, tool.tran.z along the tool axis */ + double geometry[KINS_MAX_PARAMS]; /* the table, in its order */ +} kins_params; + +/* What one caller carries between its own calls: the last pose an + iterative forward found, which seeds the next, and what a module reports + about its last call. Never shared between callers. */ +typedef struct kins_scratch { + EmcPose pose_seed; /* start an iterative forward here */ + int have_pose_seed; + int pose_seed_ok; /* pose_seed came from a solve that succeeded */ + double joint_seed[EMCMOT_MAX_JOINTS]; /* start an iterative inverse here */ + int have_joint_seed; + int iterations; + int failed; + double out[KINS_MAX_PARAMS]; /* the table's KINS_OUT entries */ +} kins_scratch; + +typedef int (*kins_forward_fn)(const kins_params *p, kins_scratch *s, + const double *joint, EmcPose *pos, + const KINEMATICS_FORWARD_FLAGS *fflags, + KINEMATICS_INVERSE_FLAGS *iflags); + +typedef int (*kins_inverse_fn)(const kins_params *p, kins_scratch *s, + const EmcPose *pos, double *joint, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags); + +typedef int (*kins_frame_fn)(const kins_params *p, const double *joint, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags); + +typedef int (*kins_jacobian_fn)(const kins_params *p, const double *joint, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags); + +/* The maths of one kinematics type. forward and inverse are required; the + frames, the native rotation and the Jacobian are optional as before, and + a missing Jacobian is differenced from the inverse. fwd_iterates says the + forward starts from the pose it is handed, so the shared code seeds it + with the last answer after a switch. identity says joints are axes, which + a consumer may use to skip the maths altogether. */ +typedef struct kins_ops { + kins_forward_fn forward; + kins_inverse_fn inverse; + kins_frame_fn work; + kins_frame_fn tool; + const PmRotationMatrix *native; /* NULL means TOOL_FRAME_SPINDLE */ + kins_jacobian_fn jacobian; + int fwd_iterates; + int identity; /* joints are axes */ +} kins_ops; + +/* A module described for a caller outside RT: its table, its joint + conventions and the maths of each type. ops[t] is NULL for a type the + module still implements the old way. */ +typedef struct kins_module_info { + const char *name; + const char *halprefix; + const kins_param_desc *params; + int nparams; + const char *required_coordinates; + int max_joints; /* the most the module allows */ + int allow_duplicates; + int ntypes; + const kins_ops *ops[KINS_MAX_TYPES]; +} kins_module_info; + +/* Exported by every module that provides the forms above. coordinates and + sparm are the module parameters the RT instance was loaded with; a module + whose types depend on them replays that choice here. Meant for a copy of + the module loaded outside RT; the RT instance answers from its own state + without redoing its setup. Returns 0, or -1 with info untouched. */ +extern int kinsDescribe(const char *coordinates, const char *sparm, + kins_module_info *info); + +/* Fill a block for a module: size, the joint map from coordinates (checked + against required_coordinates, the joint limit and the duplicate rule), + ktype 0, no tool, and every geometry entry at its table default. A + caller then overwrites what it knows better. Returns 0 or -1. */ +extern int kinsParamsInit(kins_params *p, + const kins_module_info *info, + const char *coordinates); + +/* The joint map alone, into a block, with no other field touched. */ +extern int kinsParamsMapCoordinates(kins_params *p, + const char *coordinates, + int max_joints, + int allow_duplicates, + const char *required_coordinates); + +/* Reset a scratch to "no seed, nothing reported". */ +extern void kinsScratchInit(kins_scratch *s); + +/* The map helpers above, reading the map from the block instead of from + the statics that map_coordinates_to_jnumbers() fills. */ +extern int kinsMappedJointsToPose(const kins_params *p, + const double *joints, EmcPose *pos); +extern int kinsPoseToMappedJoints(const kins_params *p, + const EmcPose *pos, double *joints); +extern int kinsJacobianFromMappedAxesP(const kins_params *p, + const double dP[EMCMOT_MAX_AXIS][EMCMOT_MAX_AXIS], + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]); + +/* Identity as pure functions: joints are axes through the block's map. */ +extern int kinsIdentityForward(const kins_params *p, kins_scratch *s, + const double *joint, EmcPose *pos, + const KINEMATICS_FORWARD_FLAGS *fflags, + KINEMATICS_INVERSE_FLAGS *iflags); +extern int kinsIdentityInverse(const kins_params *p, kins_scratch *s, + const EmcPose *pos, double *joint, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags); +extern int kinsIdentityFrame(const kins_params *p, const double *joint, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags); +extern int kinsIdentityJacobian(const kins_params *p, const double *joint, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags); +extern const kins_ops KINS_IDENTITY_OPS; + +/* The five questions asked of an ops table, with the defaults applied: + identity for a missing frame, the native rotation applied to the tool + frame, and the Jacobian differenced from the inverse when there is no + closed form. These are what the RT wrappers and a caller outside RT + both go through, so both get the same answers. */ +extern int kinsOpsForward(const kins_ops *ops, const kins_params *p, + kins_scratch *s, const double *joint, EmcPose *pos, + const KINEMATICS_FORWARD_FLAGS *fflags, + KINEMATICS_INVERSE_FLAGS *iflags); +extern int kinsOpsInverse(const kins_ops *ops, const kins_params *p, + kins_scratch *s, const EmcPose *pos, double *joint, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags); +extern int kinsOpsWorkFrame(const kins_ops *ops, const kins_params *p, + const double *joint, PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags); +extern int kinsOpsToolFrame(const kins_ops *ops, const kins_params *p, + const double *joint, PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags); +extern int kinsOpsJacobian(const kins_ops *ops, const kins_params *p, + kins_scratch *s, const double *joint, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags); + extern int kinematicsSwitchable(void); extern int kinematicsSwitch(int switchkins_type); //NOTE: switchable kinematics may require Interp::Synch @@ -493,6 +706,8 @@ EXPORT_SYMBOL(kinematicsTypeFlags); // support for template for user-defined switchkins_type==2 +extern const kins_ops USERK_OPS; + extern int userkKinematicsSetup(const int comp_id, const char* coordinates, kparms* ksetup_parms); diff --git a/include/kinematics_user.h b/include/kinematics_user.h index 8bbabea8727..ea051f4d318 100644 --- a/include/kinematics_user.h +++ b/include/kinematics_user.h @@ -6,10 +6,11 @@ * the RT kinematics interface. Used by the 9D planner to compute joint * positions from world coordinates without requiring RT kernel calls. * - * The kinematics module is loaded into this process and given input pins - * belonging to the caller's HAL component, connected to the same signals - * the running RT instance reads. Its own forward and inverse then work on - * live values, unmodified. + * The kinematics module is loaded into this process and evaluated through + * its parameter block form (see kinematics.h). The block is filled from + * input pins belonging to the caller's HAL component, connected to the + * same signals the running RT instance reads, and from motion's tool + * offset pins where motion is loaded, so the maths runs on live values. * * Author: LinuxCNC * License: GPL Version 2 @@ -64,6 +65,30 @@ KinematicsUserContext* kinematicsUserInit(const char* kins_type, int comp_id, const char* prefix); +/** + * As kinematicsUserInit(), with the module's sparm= parameter as well, for + * a module whose kinematics types depend on it (5axiskins identityfirst). + */ +KinematicsUserContext* kinematicsUserInitSparm(const char* kins_type, + int num_joints, + const char* coordinates, + const char* sparm, + int comp_id, + const char* prefix); + +/** + * Select which kinematics type of a switchable module to evaluate. + * Type 0 is selected after init. + * + * @return 0, or -1 if the module has no such type in the block form + */ +int kinematicsUserSetType(KinematicsUserContext* ctx, int ktype); + +/** + * How many kinematics types the module has (1 for one that does not switch). + */ +int kinematicsUserGetNumTypes(KinematicsUserContext* ctx); + /** * Perform inverse kinematics (world coords -> joint positions) * @@ -88,6 +113,24 @@ int kinematicsUserForward(KinematicsUserContext* ctx, const double* joints, EmcPose* world); +/** + * The Jacobian at a pose, J[joint][axis] = d joint / d axis, from the + * module's closed form where it has one and by differencing its inverse + * where it does not. The inverse is run at the pose first, so the + * derivative is taken on the solution branch the module picks there. + * + * @return 0 on success, -1 on failure + */ +int kinematicsUserJacobian(KinematicsUserContext* ctx, + const EmcPose* world, + double J[KINEMATICS_USER_MAX_JOINTS][AXIS_COUNT]); + +/** + * The parameter block as it stands, refreshed from HAL first. For + * reporting; the block belongs to the context. + */ +const kins_params* kinematicsUserParams(KinematicsUserContext* ctx); + /** * Check if kinematics type is identity (world coords = joint coords) * @@ -121,20 +164,18 @@ KINEMATICS_TYPE kinematicsUserGetType(KinematicsUserContext* ctx); const char* kinematicsUserGetModuleName(KinematicsUserContext* ctx); /** - * Refresh kinematics parameters (no-op) - * - * The bound pins read the live values, so there is nothing to fetch. - * This function is kept for API compatibility but does nothing. + * Copy the bound pins into the block now. Every evaluation does this + * itself; call it only to observe the values. * * @param ctx Kinematics context - * @return 0 always + * @return 0, or -1 for an RT-only context */ int kinematicsUserRefreshParams(KinematicsUserContext* ctx); /** * Check if this context is RT-only * - * An RT-only module exports no nonrt_attach() and so cannot be evaluated + * An RT-only module exports no kinsDescribe() and so cannot be evaluated * outside RT. Planner 2 is unavailable for such modules. * * @param ctx Kinematics context diff --git a/include/kins_rt.h b/include/kins_rt.h new file mode 100644 index 00000000000..00240cc9ecd --- /dev/null +++ b/include/kins_rt.h @@ -0,0 +1,64 @@ +/******************************************************************** +* Description: kins_rt.h +* The RT side of a kinematics module written as pure functions: the HAL +* pins made from its geometry table, and the wrapper that supplies the +* classic entry points for a module with one kinematics type. A module +* with several types gets the same from switchkins.c. +* +* Kept apart from kinematics.h because everything here needs HAL, and +* kinematics.h is read by callers outside RT that do not. +* +* License: GPL Version 2 +********************************************************************/ +#ifndef __LINUXCNC_KINS_RT_H +#define __LINUXCNC_KINS_RT_H + +#include "hal.h" +#include "kinematics.h" + +/* one HAL pin handle per table entry, of whichever type the entry has */ +typedef union { + hal_real_t r; + hal_bool_t b; + hal_sint_t s; + hal_uint_t u; +} kins_pin_ref; + +/* Make one pin per table entry, named ., inputs at their + defaults. *out receives the handles, from hal_malloc(), or NULL for an + empty table. Returns 0 or -1. */ +extern int kinsParamsPinsCreate(int comp_id, const char *prefix, + const kins_param_desc *params, int nparams, + kins_pin_ref **out); + +/* Copy every input pin into p->geometry[], and the tool entry into + p->tool.tran.z as well. */ +extern void kinsParamsPinsRead(const kins_pin_ref *pins, + const kins_param_desc *params, int nparams, + kins_params *p); + +/* Copy s->out[] to every output pin. */ +extern void kinsParamsPinsWrite(const kins_pin_ref *pins, + const kins_param_desc *params, int nparams, + const kins_scratch *s); + +/* A module with one kinematics type describes itself in a + kins_module_info and hands it to kinsSingleInit(); kins_single.c, linked + in or loaded as kins_single_core, supplies kinematicsForward() and the + rest from it. ops[0] is the maths; the other entries are ignored. An + in-tree module names its description kins_module and links + kins_single_module.c for its kinsDescribe(). */ +extern const kins_module_info kins_module; + +/* Called once from the module's rtapi_app_main() or EXTRA_SETUP(), after + hal_init() and before hal_ready(): makes the pins, builds the block for + coordinates and records the KINEMATICS_TYPE that kinematicsType() will + report. m must outlive the module. Returns 0 or -1. */ +extern int kinsSingleInit(int comp_id, const kins_module_info *m, + const char *coordinates, KINEMATICS_TYPE reported); + +/* What the module's kinsDescribe() answers with: m, one type. */ +extern int kinsSingleDescribe(const kins_module_info *m, + kins_module_info *info); + +#endif diff --git a/include/nonrt_kins.h b/include/nonrt_kins.h deleted file mode 100644 index 48e8ea3d86e..00000000000 --- a/include/nonrt_kins.h +++ /dev/null @@ -1,95 +0,0 @@ -/******************************************************************** - * Description: nonrt_kins.h - * Interface a kinematics module exports so that a non-RT caller can - * evaluate it. - * - * A trajectory planner needs forward and inverse kinematics at poses - * the machine has not reached yet, which means calling them outside - * the servo thread. A module opts in by exporting nonrt_attach(). - * - * The caller dlopens the module and calls nonrt_attach() once with - * the coordinates string and a resolver callback. The module names - * each of the pins it reads, keeps the references the resolver - * returns in its own haldata, and hands back its existing forward - * and inverse. The kinematics code itself does not change. - * - * A reference does not point into the RT instance's pin. The - * resolver creates an input pin on the caller's own component and - * connects it to the signal the RT pin reads, so the reference - * belongs to the caller and rewiring cannot strand it. - * - * Name lookup belongs to the caller, userspace code linked against - * liblinuxcnchal. This file is compiled into an RT module, which - * has no business walking the HAL name space and would risk binding - * against rtlib's copy of the same symbols. - * - * Resolve input pins only. Output pins and scratch storage stay - * private to the non-RT copy, or the two copies write to each - * other's state. - * - * Author: LinuxCNC - * License: GPL Version 2 - * System: Linux - * - * Copyright (c) 2024 All rights reserved. - ********************************************************************/ - -#ifndef NONRT_KINS_H -#define NONRT_KINS_H - -#include - -#include -#include "hal.h" -#include "emcpos.h" -#include "kinematics.h" - -/* Supplied by the caller. Finds 'pin_name' in HAL, checks that it has - type 'type', and writes to 'out' a reference carrying that pin's - value. The reference is to storage the caller owns, not to the named - pin itself. Returns 0 on success. */ -typedef int (*nonrt_resolve_fn)(const char *pin_name, - hal_type_t type, - hal_refs_u *out, - void *arg); - -/* Filled in by nonrt_attach(). A module that reports is_identity has - joints equal to axes and the caller needs no module code at all, so - forward and inverse may be left NULL. */ -typedef struct { - int (*forward)(const double *joints, EmcPose *pos, - const KINEMATICS_FORWARD_FLAGS *fflags, - KINEMATICS_INVERSE_FLAGS *iflags); - int (*inverse)(const EmcPose *pos, double *joints, - const KINEMATICS_INVERSE_FLAGS *iflags, - KINEMATICS_FORWARD_FLAGS *fflags); - int is_identity; -} nonrt_ops_t; - -/* Exported by a participating module: - int nonrt_attach(const char *coordinates, nonrt_ops_t *ops, - nonrt_resolve_fn resolve, void *arg); - Returns 0 on success. */ - -/* Convenience for the common case: resolve one float pin, by printf - style name, into a haldata field. */ -static inline int nonrt_resolve_real(nonrt_resolve_fn resolve, void *arg, - hal_real_t *dst, const char *fmt, ...) -{ - char name[HAL_NAME_LEN + 1]; - hal_refs_u ref; - va_list ap; - - if (!resolve || !dst) return -1; - - va_start(ap, fmt); - rtapi_vsnprintf(name, sizeof(name), fmt, ap); - va_end(ap); - - if (resolve(name, HAL_REAL, &ref, arg) != 0) return -1; - - *dst = ref.r; - return 0; -} - -#endif /* NONRT_KINS_H */ diff --git a/include/switchkins.h b/include/switchkins.h index c7114262403..336e662a951 100644 --- a/include/switchkins.h +++ b/include/switchkins.h @@ -7,7 +7,10 @@ #include "kinematics.h" //SWITCHKINS_MAX_TYPES (max number of types a module may provide) -//is in kinematics.h: motion and the NML status channel need it too +//is in kinematics.h as KINS_MAX_TYPES: motion and the NML +//status channel need it too +//max number of switchkins types a module may provide: +#define SWITCHKINS_MAX_TYPES KINS_MAX_TYPES // KinematicsFORWARD functions typedef int (*KF)(const double *joint, @@ -35,6 +38,10 @@ typedef int (*KS)(const int comp_id, // halpins //********************************************************************* // supplied by a module using switchkins_main.c, provides types 0,1,2 +typedef int (*switchkinsSetupFunc)(kparms* ksetup_parms, + KS* kset0, KS* kset1, KS* kset2, + KF* kfwd0, KF* kfwd1, KF* kfwd2, + KI* kinv0, KI* kinv1, KI* kinv2); extern int switchkinsSetup(kparms* ksetup_parms, KS* kset0, KS* kset1, KS* kset2, KF* kfwd0, KF* kfwd1, KF* kfwd2, @@ -85,10 +92,42 @@ typedef int (*KJ)(const double *joint, // otherwise the generic differences of its own inverse. extern int switchkinsRegisterJacobian(int ktype, KJ kjac); +// provide one switchkins-type written as pure functions (see kinematics.h), +// before switchkinsInit(). Its pins come from the table in kparms, shared +// by every type of the module, so it has no setup function. A type may be +// provided this way or through switchkinsRegister(), not both. +extern int switchkinsRegisterOps(int ktype, const kins_ops *ops); + // create the hal pins and start on type 0; the caller owns the hal // component and does hal_init() before and hal_ready() after extern int switchkinsInit(const int comp_id, kparms* ksetup_parms, const char* coordinates ); + +// Fill kp with the defaults, run the module's setup and register the three +// types it may return, so that every type goes in by one route. Returns +// 0 or -1. +extern int switchkinsRunSetupWith(switchkinsSetupFunc setup, + kparms* kp, const char* sparm); + +// What the module's kinsDescribe() answers with: the RT instance describes +// itself, a copy outside RT replays setup once. +extern int switchkinsDescribeWith(switchkinsSetupFunc setup, + const char *coordinates, const char *sparm, + kins_module_info *info); + +// The two above on the module's switchkinsSetup(), in switchkins_setup.c, +// which an in-tree module links; a module built out of tree calls the +// With forms itself. +extern int switchkinsRunSetup(kparms* kp, const char* sparm); + +// The module as the core knows it after switchkinsInit(): its table and +// the ops of every type, NULL for one provided the old way. Behind +// kinsDescribe() for the RT instance; a copy outside RT that has not been +// initialised is described by switchkins_setup.c after a replay of setup. +// Returns 0, or -1 before switchkinsInit(). +extern int switchkinsDescribe(kins_module_info *info); +extern int switchkinsDescribeSetup(const kparms *kp, kins_module_info *info); + #endif diff --git a/src/Makefile b/src/Makefile index 63f65ba7fad..540d30c6a47 100644 --- a/src/Makefile +++ b/src/Makefile @@ -1057,6 +1057,8 @@ hal_lib-objs := hal/hal_lib.o $(MATHSTUB) obj-m += trivkins.o trivkins-objs := emc/kinematics/trivkins.o trivkins-objs += emc/kinematics/kins_util.o +trivkins-objs += emc/kinematics/kins_single.o +trivkins-objs += emc/kinematics/kins_single_module.o obj-m += maxkins.o maxkins-objs := emc/kinematics/maxkins.o @@ -1112,6 +1114,7 @@ genhexkins-objs += $(MATHSTUB) genhexkins-objs += emc/kinematics/kins_util.o genhexkins-objs += emc/kinematics/switchkins.o genhexkins-objs += emc/kinematics/switchkins_main.o +genhexkins-objs += emc/kinematics/switchkins_setup.o genhexkins-objs += $(USERKFUNCS) obj-m += genserkins.o @@ -1122,6 +1125,7 @@ genserkins-objs += $(MATHSTUB) genserkins-objs += emc/kinematics/kins_util.o genserkins-objs += emc/kinematics/switchkins.o genserkins-objs += emc/kinematics/switchkins_main.o +genserkins-objs += emc/kinematics/switchkins_setup.o genserkins-objs += $(USERKFUNCS) obj-m += xyzac-trt-kins.o @@ -1130,6 +1134,7 @@ xyzac-trt-kins-objs += emc/kinematics/trtfuncs.o xyzac-trt-kins-objs += emc/kinematics/kins_util.o xyzac-trt-kins-objs += emc/kinematics/switchkins.o xyzac-trt-kins-objs += emc/kinematics/switchkins_main.o +xyzac-trt-kins-objs += emc/kinematics/switchkins_setup.o xyzac-trt-kins-objs += $(USERKFUNCS) obj-m += xyzbc-trt-kins.o @@ -1138,6 +1143,7 @@ xyzbc-trt-kins-objs += emc/kinematics/trtfuncs.o xyzbc-trt-kins-objs += emc/kinematics/kins_util.o xyzbc-trt-kins-objs += emc/kinematics/switchkins.o xyzbc-trt-kins-objs += emc/kinematics/switchkins_main.o +xyzbc-trt-kins-objs += emc/kinematics/switchkins_setup.o xyzbc-trt-kins-objs += $(USERKFUNCS) obj-m += scarakins.o @@ -1147,6 +1153,7 @@ scarakins-objs += $(MATHSTUB) scarakins-objs += emc/kinematics/kins_util.o scarakins-objs += emc/kinematics/switchkins.o scarakins-objs += emc/kinematics/switchkins_main.o +scarakins-objs += emc/kinematics/switchkins_setup.o scarakins-objs += $(USERKFUNCS) obj-m += pumakins.o @@ -1156,6 +1163,7 @@ pumakins-objs += $(MATHSTUB) pumakins-objs += emc/kinematics/kins_util.o pumakins-objs += emc/kinematics/switchkins.o pumakins-objs += emc/kinematics/switchkins_main.o +pumakins-objs += emc/kinematics/switchkins_setup.o pumakins-objs += $(USERKFUNCS) obj-m += three21kins.o @@ -1165,6 +1173,7 @@ three21kins-objs += $(MATHSTUB) three21kins-objs += emc/kinematics/kins_util.o three21kins-objs += emc/kinematics/switchkins.o three21kins-objs += emc/kinematics/switchkins_main.o +three21kins-objs += emc/kinematics/switchkins_setup.o three21kins-objs += $(USERKFUNCS) obj-m += switchkins_core.o @@ -1180,6 +1189,7 @@ obj-m += 5axiskins.o 5axiskins-objs += emc/kinematics/kins_util.o 5axiskins-objs += emc/kinematics/switchkins.o 5axiskins-objs += emc/kinematics/switchkins_main.o +5axiskins-objs += emc/kinematics/switchkins_setup.o 5axiskins-objs += $(USERKFUNCS) #---------------------------------------------------------------- diff --git a/src/emc/kinematics/5axiskins.c b/src/emc/kinematics/5axiskins.c index d4798261ff8..cf2e0254da4 100644 --- a/src/emc/kinematics/5axiskins.c +++ b/src/emc/kinematics/5axiskins.c @@ -11,10 +11,9 @@ * Copyright (c) 2007 Chris Radek * * Notes: -* 1) pivot_length hal pin must agree with mechanical -* design (including vismach simulation) and augmented -* with current tool z offset -* (typ: mechanical_pivot_length + motion.tooloffset.z) +* 1) pivot-length must agree with the mechanical design +* (including vismach simulation); the tool length comes +* in on the tool-length pin of its own * 2) C axis: spherical coordinates aziumthal angle (t or theta) * projection of radius to xy plane * 3) B axis: spherical coordinates polar angle (p or phi) @@ -42,8 +41,8 @@ * 9) Coordinates XYZBCW are required, AUV may be used * if specified with the coordinates parameter and will * be mapped one-to-one with the assigned joint. -* 10) The direction of the tilt axis is the opposite of the -* conventional axis direction. See +* 10) The direction of the tilt axis is the opposite of the +* conventional axis direction. See * https://linuxcnc.org/docs/html/gcode/machining-center.html ********************************************************************/ @@ -56,18 +55,29 @@ #include #include #include -#include #include #include #include -#include -static struct haldata { - hal_real_t pivot_length; - hal_real_t tool_length; -} *haldata; -static int fiveaxis_max_joints; +// the geometry, one pin each; the maths reads it from the block +static const kins_param_desc fiveaxis_params[] = { + { "pivot-length", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_PIVOT_LENGTH }, + { "tool-length", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, +}; +enum { P_PIVOT_LENGTH, P_TOOL_LENGTH }; + +// assignments of principal joints to axis letters, from the block +// (-1 means not defined) +#define JX (p->joint_of_axis[0]) +#define JY (p->joint_of_axis[1]) +#define JZ (p->joint_of_axis[2]) +#define JA (p->joint_of_axis[3]) +#define JB (p->joint_of_axis[4]) +#define JC (p->joint_of_axis[5]) +#define JU (p->joint_of_axis[6]) +#define JV (p->joint_of_axis[7]) +#define JW (p->joint_of_axis[8]) static PmCartesian s2r(double r, double t, double p) { // s2r: spherical coordinates to cartesian coordinates @@ -85,28 +95,18 @@ static PmCartesian s2r(double r, double t, double p) { return c; } //s2r() -// assignments of principal joints to axis letters: -// (-1 means not defined (yet)) -static int JX = -1; -static int JY = -1; -static int JZ = -1; -static int JA = -1; -static int JB = -1; -static int JC = -1; -static int JU = -1; -static int JV = -1; -static int JW = -1; - -static int fiveaxis_KinematicsForward(const double *joints, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +static int fiveaxis_forward(const kins_params *p, kins_scratch *s, + const double *joints, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)s; (void)fflags; (void)iflags; - rtapi_real pivot_length = hal_get_real(haldata->pivot_length); - rtapi_real tool_length = hal_get_real(haldata->tool_length); - PmCartesian r = s2r(pivot_length + joints[JW] + tool_length, + double pivot_length = p->geometry[P_PIVOT_LENGTH]; + double tool_length = p->geometry[P_TOOL_LENGTH]; + PmCartesian r = s2r(pivot_length + tool_length + joints[JW], joints[JC], 180.0 - joints[JB]); @@ -124,18 +124,20 @@ static int fiveaxis_KinematicsForward(const double *joints, pos->v = (JV != -1)? joints[JV] : 0; return 0; -} //fiveaxis_KinematicsForward() +} // fiveaxis_forward() -static int fiveaxis_KinematicsInverse(const EmcPose * pos, - double *joints, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int fiveaxis_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, + double *joints, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { + (void)s; (void)iflags; (void)fflags; - rtapi_real pivot_length = hal_get_real(haldata->pivot_length); - rtapi_real tool_length = hal_get_real(haldata->tool_length); - PmCartesian r = s2r(pivot_length + pos->w + tool_length, + double pivot_length = p->geometry[P_PIVOT_LENGTH]; + double tool_length = p->geometry[P_TOOL_LENGTH]; + PmCartesian r = s2r(pivot_length + tool_length + pos->w, pos->c, 180.0 - pos->b); @@ -156,21 +158,18 @@ static int fiveaxis_KinematicsInverse(const EmcPose * pos, // update joints with support for // multiple-joints per-coordinate letter: // based on computed position - position_to_mapped_joints(fiveaxis_max_joints, - &P, - joints); - return 0; -} // fiveaxis_kinematicsInverse() - -static int fiveaxis_KinematicsJacobian(const double *joints, - const EmcPose * pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS * iflags) + return kinsPoseToMappedJoints(p, &P, joints); +} // fiveaxis_inverse() + +static int fiveaxis_jacobian(const kins_params *p, + const double *joints, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) { (void)joints; (void)iflags; - rtapi_real pivot_length = hal_get_real(haldata->pivot_length); - const double R = pivot_length + pos->w; + const double R = p->geometry[P_PIVOT_LENGTH] + p->geometry[P_TOOL_LENGTH] + pos->w; const double sb = sin(TO_RAD*pos->b), cb = cos(TO_RAD*pos->b); const double sc = sin(TO_RAD*pos->c), cc = cos(TO_RAD*pos->c); double dP[EMCMOT_MAX_AXIS][EMCMOT_MAX_AXIS]; @@ -197,112 +196,15 @@ static int fiveaxis_KinematicsJacobian(const double *joints, for (a = 3; a < EMCMOT_MAX_AXIS; a++) { dP[a][a] = 1; } - return kinsJacobianFromMappedAxes(fiveaxis_max_joints, - (const double (*)[EMCMOT_MAX_AXIS])dP, - jac); -} // fiveaxis_KinematicsJacobian() - -// module constants, shared by switchkinsSetup() and nonrt_attach() -static void fiveaxis_kparms(kparms* kp) -{ - kp->kinsname = "5axiskins"; // !!! must agree with filename - kp->halprefix = "5axiskins"; // hal pin names - kp->required_coordinates = REQUIRED_COORDINATES; - kp->allow_duplicates = 1; - kp->max_joints = EMCMOT_MAX_JOINTS; -} - -// assign principal joint numbers from the coordinates string. -// No HAL involvement, so the non-RT path can use it too. -static int fiveaxis_map_joints(const char* coordinates, kparms* kp) -{ - int i,jno; - int axis_idx_for_jno[EMCMOT_MAX_JOINTS]; - int minjoints = strlen(kp->required_coordinates); - fiveaxis_max_joints = strlen(coordinates); // allow for dup coords - - if (fiveaxis_max_joints > kp->max_joints) { - rtapi_print_msg(RTAPI_MSG_ERR, - "ERROR %s: coordinates=%s requires %d joints, max joints=%d\n", - kp->kinsname, - coordinates, - fiveaxis_max_joints, - kp->max_joints); - goto error; - } - - if (map_coordinates_to_jnumbers(coordinates, - kp->max_joints, - kp->allow_duplicates, - axis_idx_for_jno)) { - goto error; - } - // require all chars in reqd_coordinates (order doesn't matter) - for (i=0; i < minjoints; i++) { - char reqd_char; - reqd_char = *(kp->required_coordinates + i); - if ( !strchr(coordinates,toupper(reqd_char)) - && !strchr(coordinates,tolower(reqd_char)) ) { - rtapi_print_msg(RTAPI_MSG_ERR, - "ERROR %s:\nrequired coordinates:%s\n" - "specified coordinates:%s\n", - kp->kinsname, kp->required_coordinates, coordinates); - goto error; - } - } - // assign principal joint numbers (first found in coordinates map) - // duplicates are handled by position_to_mapped_joints() - for (jno=0; jnopivot_length), - DEFAULT_PIVOT_LENGTH, "%s.pivot-length", kp->halprefix); - if(result < 0) goto error; - - result = hal_pin_new_real(comp_id, HAL_IN, &(haldata->tool_length), - 0.0, "%s.tool-length", kp->halprefix); - if(result < 0) goto error; - - rtapi_print("Kinematics Module %s\n",__FILE__); - rtapi_print(" module name = %s\n" - " coordinates = %s Requires: [KINS]JOINTS>=%d\n" - " sparm = %s\n", - kp->kinsname, - coordinates,fiveaxis_max_joints, - kp->sparm?kp->sparm:"NOTSPECIFIED"); - rtapi_print(" default pivot-length = %.3f\n", hal_get_real(haldata->pivot_length)); - - return 0; + return kinsJacobianFromMappedAxesP(p, (const double (*)[EMCMOT_MAX_AXIS])dP, + jac); +} // fiveaxis_jacobian() -error: - return -1; -} // fiveaxis_KinematicsSetup() +static const kins_ops fiveaxis_ops = { + .forward = fiveaxis_forward, + .inverse = fiveaxis_inverse, + .jacobian = fiveaxis_jacobian, +}; int switchkinsSetup(kparms* kp, KS* kset0, KS* kset1, KS* kset2, @@ -310,61 +212,27 @@ int switchkinsSetup(kparms* kp, KI* kinv0, KI* kinv1, KI* kinv2 ) { - fiveaxis_kparms(kp); + (void)kset0; (void)kset1; (void)kset2; + (void)kfwd0; (void)kfwd1; (void)kfwd2; + (void)kinv0; (void)kinv1; (void)kinv2; + kp->kinsname = "5axiskins"; // !!! must agree with filename + kp->halprefix = "5axiskins"; // hal pin names + kp->required_coordinates = REQUIRED_COORDINATES; + kp->allow_duplicates = 1; + kp->max_joints = EMCMOT_MAX_JOINTS; + kp->params = fiveaxis_params; + kp->nparams = sizeof(fiveaxis_params)/sizeof(fiveaxis_params[0]); if (kp->sparm && strstr(kp->sparm,"identityfirst")) { rtapi_print("\n!!! switchkins-type 0 is IDENTITY\n"); - *kset0 = identityKinematicsSetup; - *kfwd0 = identityKinematicsForward; - *kinv0 = identityKinematicsInverse; - - *kset1 = fiveaxis_KinematicsSetup; - *kfwd1 = fiveaxis_KinematicsForward; - *kinv1 = fiveaxis_KinematicsInverse; - switchkinsDeclare(0, KINSTYPE_IDENTITY); - switchkinsDeclare(1, KINSTYPE_PRIMARY); - switchkinsRegisterJacobian(1, fiveaxis_KinematicsJacobian); + switchkinsRegisterOps(0, &KINS_IDENTITY_OPS); + switchkinsRegisterOps(1, &fiveaxis_ops); } else { rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname); - *kset0 = fiveaxis_KinematicsSetup; - *kfwd0 = fiveaxis_KinematicsForward; - *kinv0 = fiveaxis_KinematicsInverse; - switchkinsRegisterJacobian(0, fiveaxis_KinematicsJacobian); - - *kset1 = identityKinematicsSetup; - *kfwd1 = identityKinematicsForward; - *kinv1 = identityKinematicsInverse; - switchkinsDeclare(0, KINSTYPE_PRIMARY); - switchkinsDeclare(1, KINSTYPE_IDENTITY); + switchkinsRegisterOps(0, &fiveaxis_ops); + switchkinsRegisterOps(1, &KINS_IDENTITY_OPS); } - *kset2 = userkKinematicsSetup; - *kfwd2 = userkKinematicsForward; - *kinv2 = userkKinematicsInverse; + switchkinsRegisterOps(2, &USERK_OPS); return 0; } // switchkinsSetup() - -// Non-RT entry point: bind this copy of the module to the pins the -// running RT instance owns, then hand back the unmodified kinematics. -int nonrt_attach(const char* coordinates, nonrt_ops_t* ops, - nonrt_resolve_fn resolve, void* arg) -{ - static struct haldata nonrt_haldata; // private to this copy of the module - kparms kp = {0}; - - fiveaxis_kparms(&kp); - - haldata = &nonrt_haldata; - - if (nonrt_resolve_real(resolve, arg, &haldata->pivot_length, - "%s.pivot-length", kp.halprefix)) return -1; - - if (fiveaxis_map_joints(coordinates, &kp)) return -1; - - ops->forward = fiveaxis_KinematicsForward; - ops->inverse = fiveaxis_KinematicsInverse; - ops->is_identity = 0; - return 0; -} // nonrt_attach() - -EXPORT_SYMBOL(nonrt_attach); diff --git a/src/emc/kinematics/kins_single.c b/src/emc/kinematics/kins_single.c new file mode 100644 index 00000000000..f523b7c394b --- /dev/null +++ b/src/emc/kinematics/kins_single.c @@ -0,0 +1,156 @@ +/******************************************************************** +* Description: kins_single.c +* The classic kinematics entry points for a module with one kinematics +* type written as pure functions. The module hands its description to +* kinsSingleInit(); this file keeps the one RT parameter block, +* fills it from the pins before every call, and hands the call to the +* module's ops. It is the counterpart of switchkins.c for a module that +* does not switch. +* +* License: GPL Version 2 +********************************************************************/ + +#include +#include +#include + +#include +#include + +static kins_params rt_params; +static kins_scratch rt_scratch; +static kins_pin_ref *pins; +static int inited; +static KINEMATICS_TYPE reported_type = KINEMATICS_BOTH; +static const kins_module_info *km; + +static const kins_ops *ops(void) +{ + return inited ? km->ops[0] : NULL; +} + +// the block sees the pins as they are now +static void read_pins(void) +{ + kinsParamsPinsRead(pins, km->params, km->nparams, + &rt_params); +} + +static void write_pins(void) +{ + kinsParamsPinsWrite(pins, km->params, km->nparams, + &rt_scratch); +} + +int kinsSingleInit(int comp_id, const kins_module_info *m, + const char *coordinates, KINEMATICS_TYPE reported) +{ + if (!m) { return -1; } + km = m; + if (!km->ops[0] || !km->ops[0]->forward + || !km->ops[0]->inverse) { + rtapi_print_msg(RTAPI_MSG_ERR, + "kinsSingleInit: %s supplies no forward or inverse\n", + km->name ? km->name : "?"); + return -1; + } + if (kinsParamsInit(&rt_params, km, coordinates)) { return -1; } + kinsScratchInit(&rt_scratch); + if (kinsParamsPinsCreate(comp_id, km->halprefix, + km->params, km->nparams, + &pins)) { + return -1; + } + reported_type = reported; + inited = 1; + return 0; +} // kinsSingleInit() + +int kinematicsForward(const double *joint, + EmcPose *pos, + const KINEMATICS_FORWARD_FLAGS *fflags, + KINEMATICS_INVERSE_FLAGS *iflags) +{ + int r; + if (!inited) { return -1; } + read_pins(); + r = kinsOpsForward(ops(), &rt_params, &rt_scratch, joint, pos, fflags, iflags); + write_pins(); + return r; +} + +int kinematicsInverse(const EmcPose *pos, + double *joint, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags) +{ + int r; + if (!inited) { return -1; } + read_pins(); + r = kinsOpsInverse(ops(), &rt_params, &rt_scratch, pos, joint, iflags, fflags); + write_pins(); + return r; +} + +int kinematicsWorkFrame(const double *joint, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + if (!inited) { return -1; } + read_pins(); + return kinsOpsWorkFrame(ops(), &rt_params, joint, rot, fflags); +} + +int kinematicsToolFrame(const double *joint, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + if (!inited) { return -1; } + read_pins(); + return kinsOpsToolFrame(ops(), &rt_params, joint, rot, fflags); +} + +int kinematicsJacobian(const double *joint, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) +{ + if (!inited) { return -1; } + read_pins(); + return kinsOpsJacobian(ops(), &rt_params, &rt_scratch, joint, pos, jac, iflags); +} + +KINEMATICS_TYPE kinematicsType(void) +{ + return reported_type; +} + +int kinematicsSwitchable(void) { return 0; } + +int kinematicsSwitch(int switchkins_type) +{ + (void)switchkins_type; + return 0; +} + +// The module's description, for a copy of it loaded outside RT. A module +// with one type does not depend on its parameters for its shape, so this +// is the table as declared. +int kinsSingleDescribe(const kins_module_info *m, kins_module_info *info) +{ + if (!m || !info) { return -1; } + *info = *m; + info->ntypes = 1; + return 0; +} + +EXPORT_SYMBOL(kinematicsType); +EXPORT_SYMBOL(kinematicsForward); +EXPORT_SYMBOL(kinematicsInverse); +EXPORT_SYMBOL(kinematicsWorkFrame); +EXPORT_SYMBOL(kinematicsToolFrame); +EXPORT_SYMBOL(kinematicsJacobian); +EXPORT_SYMBOL(kinematicsSwitchable); +EXPORT_SYMBOL(kinematicsSwitch); +EXPORT_SYMBOL(kinsSingleInit); +EXPORT_SYMBOL(kinsSingleDescribe); diff --git a/src/emc/kinematics/kins_single_module.c b/src/emc/kinematics/kins_single_module.c new file mode 100644 index 00000000000..56476267666 --- /dev/null +++ b/src/emc/kinematics/kins_single_module.c @@ -0,0 +1,23 @@ +/******************************************************************** +* Description: kins_single_module.c +* kinsDescribe() for an in-tree module with one kinematics type: the +* description the module names kins_module, for a copy of the module +* loaded outside RT. Kept apart from kins_single.c so that file can be +* loaded as kins_single_core and serve a module built out of tree. +* +* License: GPL Version 2 +********************************************************************/ + +#include + +#include + +int kinsDescribe(const char *coordinates, const char *sparm, + kins_module_info *info) +{ + (void)coordinates; + (void)sparm; + return kinsSingleDescribe(&kins_module, info); +} + +EXPORT_SYMBOL(kinsDescribe); diff --git a/src/emc/kinematics/kins_util.c b/src/emc/kinematics/kins_util.c index bf350e3d791..24508b06f94 100644 --- a/src/emc/kinematics/kins_util.c +++ b/src/emc/kinematics/kins_util.c @@ -48,7 +48,9 @@ #include #include #include +#include #include +#include // principal joint numbers based on module 'coordinates' parameter static int JX = -1; @@ -76,38 +78,23 @@ static int map_initialized = 0; #define MAX_COORDINATES_CHARS 32 static char used_coordinates[MAX_COORDINATES_CHARS+1]; -int map_coordinates_to_jnumbers(const char *coordinates, - const int max_joints, - const int allow_duplicates, - int axis_idx_for_jno[] ) //result +// Letters to joint numbers, in order, with the checks every caller wants: +// a valid letter set, at most max_joints of them, duplicates only where +// allowed. Fills axis_idx_for_jno (-1 past the last letter) and touches +// nothing else, so the block form and the static form share it. +static int kins_scan_coordinates(const char *coordinates, + int max_joints, + int allow_duplicates, + int axis_idx_for_jno[], + const char *errtag) { - char* errtag="map_coordinates_to_jnumbers: ERROR:\n "; - int jno=0; - bool found=0; + int jno = 0; + bool found = 0; int dups[EMCMOT_MAX_AXIS]; const char *coords = coordinates; char coord_letter[] = {'X','Y','Z','A','B','C','U','V','W'}; int i; - if (strlen(coordinates) > MAX_COORDINATES_CHARS) { - rtapi_print_msg(RTAPI_MSG_ERR, - "%s: map_coordinates_to_jnumbers too many chars:%s\n" - ,__FILE__,coordinates); - return -1; - - } - // Note: may be called multiple times for different switchkins - // types but coordinates must agree - if (used_coordinates[0] == 0) { - strcpy(used_coordinates,coordinates); - } else { - if (strcasecmp(coordinates,used_coordinates)) { - rtapi_print_msg(RTAPI_MSG_ERR, - "%s: map_coordinates_to_jnumbers altered:%s %s\n" - ,__FILE__,used_coordinates,coordinates); - return -1; - } - } for (i=0; i EMCMOT_MAX_JOINTS) ) { @@ -168,6 +155,40 @@ int map_coordinates_to_jnumbers(const char *coordinates, } } } + return 0; +} // kins_scan_coordinates() + +int map_coordinates_to_jnumbers(const char *coordinates, + const int max_joints, + const int allow_duplicates, + int axis_idx_for_jno[] ) //result +{ + char* errtag="map_coordinates_to_jnumbers: ERROR:\n "; + int jno=0; + + if (strlen(coordinates) > MAX_COORDINATES_CHARS) { + rtapi_print_msg(RTAPI_MSG_ERR, + "%s: map_coordinates_to_jnumbers too many chars:%s\n" + ,__FILE__,coordinates); + return -1; + + } + // Note: may be called multiple times for different switchkins + // types but coordinates must agree + if (used_coordinates[0] == 0) { + strcpy(used_coordinates,coordinates); + } else { + if (strcasecmp(coordinates,used_coordinates)) { + rtapi_print_msg(RTAPI_MSG_ERR, + "%s: map_coordinates_to_jnumbers altered:%s %s\n" + ,__FILE__,used_coordinates,coordinates); + return -1; + } + } + if (kins_scan_coordinates(coordinates, max_joints, allow_duplicates, + axis_idx_for_jno, errtag)) { + return -1; + } for (jno=0; jno < max_joints; jno++) { int bitnumber = 1< Axis %c\n", jno,*(p+axis_idx_for_jno[jno])); } +#ifndef ULAPI + // the module's own report of its type; this file is also built + // outside RT, where there is no module around it if (kinematicsType() != KINEMATICS_BOTH) { rtapi_print("identityKinematicsSetup: Recommend: kinstype=both\n"); } +#endif rtapi_print("\n"); } @@ -1303,6 +1328,408 @@ int identityKinematicsJacobian(const double *joint, jac); } // identityKinematicsJacobian() +//---------------------------------------------------------------------- +// The parameter block. See kinematics.h for what it is for. +//---------------------------------------------------------------------- + +int kinsParamsMapCoordinates(kins_params *p, + const char *coordinates, + int max_joints, + int allow_duplicates, + const char *required_coordinates) +{ + int axis_idx_for_jno[EMCMOT_MAX_JOINTS]; + int jno, a; + + if (!p) { return -1; } + if (!coordinates) { coordinates = "XYZABCUVW"; } + + if (kins_scan_coordinates(coordinates, max_joints, allow_duplicates, + axis_idx_for_jno, + "kinsParamsMapCoordinates: ERROR:\n ")) { + return -1; + } + + // every letter the module cannot do without has to be there + for (a = 0; required_coordinates && required_coordinates[a]; a++) { + char want = required_coordinates[a]; + const char *c; + int seen = 0; + for (c = coordinates; *c; c++) { + if (*c == want || *c == want + ('a' - 'A') || *c == want - ('a' - 'A')) { + seen = 1; break; + } + } + if (!seen) { + rtapi_print_msg(RTAPI_MSG_ERR, + "kinsParamsMapCoordinates: ERROR:\n required coordinates:%s\n" + " specified coordinates:%s\n", + required_coordinates, coordinates); + return -1; + } + } + + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + p->joint_of_axis[a] = -1; + p->joints_of_axis[a] = 0; + } + p->max_joints = 0; + for (jno = 0; jno < EMCMOT_MAX_JOINTS; jno++) { + a = axis_idx_for_jno[jno]; + if (a < 0) { break; } + if (p->joint_of_axis[a] < 0) { p->joint_of_axis[a] = jno; } + p->joints_of_axis[a] |= 1 << jno; + p->max_joints = jno + 1; + } + return 0; +} // kinsParamsMapCoordinates() + +int kinsParamsInit(kins_params *p, + const kins_module_info *info, + const char *coordinates) +{ + int i; + + if (!p || !info) { return -1; } + if (info->nparams < 0 || info->nparams > KINS_MAX_PARAMS) { + rtapi_print_msg(RTAPI_MSG_ERR, + "kinsParamsInit: %s declares %d parameters, at most %d allowed\n", + info->name ? info->name : "?", info->nparams, KINS_MAX_PARAMS); + return -1; + } + + memset(p, 0, sizeof(*p)); + p->size = sizeof(*p); + p->ktype = 0; + if (!coordinates) { coordinates = info->required_coordinates; } + if (kinsParamsMapCoordinates(p, coordinates, info->max_joints, + info->allow_duplicates, + info->required_coordinates)) { + return -1; + } + for (i = 0; i < info->nparams; i++) { + p->geometry[i] = info->params[i].dflt; + if (info->params[i].tool) { p->tool.tran.z = info->params[i].dflt; } + } + return 0; +} // kinsParamsInit() + +void kinsScratchInit(kins_scratch *s) +{ + if (s) { memset(s, 0, sizeof(*s)); } +} + +int kinsMappedJointsToPose(const kins_params *p, + const double *joints, EmcPose *pos) +{ + int a; + if (!p || !joints || !pos) { return -1; } + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + int j = p->joint_of_axis[a]; + if (j < 0) { continue; } + switch (a) { + case 0: pos->tran.x = joints[j]; break; + case 1: pos->tran.y = joints[j]; break; + case 2: pos->tran.z = joints[j]; break; + case 3: pos->a = joints[j]; break; + case 4: pos->b = joints[j]; break; + case 5: pos->c = joints[j]; break; + case 6: pos->u = joints[j]; break; + case 7: pos->v = joints[j]; break; + default: pos->w = joints[j]; break; + } + } + return 0; +} // kinsMappedJointsToPose() + +static double kins_pose_coord(const EmcPose *pos, int a) +{ + switch (a) { + case 0: return pos->tran.x; + case 1: return pos->tran.y; + case 2: return pos->tran.z; + case 3: return pos->a; + case 4: return pos->b; + case 5: return pos->c; + case 6: return pos->u; + case 7: return pos->v; + default: return pos->w; + } +} + +int kinsPoseToMappedJoints(const kins_params *p, + const EmcPose *pos, double *joints) +{ + int a, jno; + if (!p || !pos || !joints) { return -1; } + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + int bits = p->joints_of_axis[a]; + if (!bits) { continue; } + for (jno = 0; jno < p->max_joints; jno++) { + if (bits & (1 << jno)) { joints[jno] = kins_pose_coord(pos, a); } + } + } + return 0; +} // kinsPoseToMappedJoints() + +int kinsJacobianFromMappedAxesP(const kins_params *p, + const double dP[EMCMOT_MAX_AXIS][EMCMOT_MAX_AXIS], + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]) +{ + int a, jno, col; + if (!p || !dP || !jac) { return -1; } + kj_zero(jac); + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + int bits = p->joints_of_axis[a]; + if (!bits) { continue; } + for (jno = 0; jno < p->max_joints; jno++) { + if (!(bits & (1 << jno))) { continue; } + for (col = 0; col < EMCMOT_MAX_AXIS; col++) { jac[jno][col] = dP[a][col]; } + } + } + return 0; +} // kinsJacobianFromMappedAxesP() + +//---------------------------------------------------------------------- +// identity through the block +//---------------------------------------------------------------------- + +int kinsIdentityForward(const kins_params *p, kins_scratch *s, + const double *joint, EmcPose *pos, + const KINEMATICS_FORWARD_FLAGS *fflags, + KINEMATICS_INVERSE_FLAGS *iflags) +{ + (void)s; (void)fflags; (void)iflags; + return kinsMappedJointsToPose(p, joint, pos); +} + +int kinsIdentityInverse(const kins_params *p, kins_scratch *s, + const EmcPose *pos, double *joint, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags) +{ + (void)s; (void)iflags; (void)fflags; + return kinsPoseToMappedJoints(p, pos, joint); +} + +int kinsIdentityFrame(const kins_params *p, const double *joint, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + (void)p; (void)joint; (void)fflags; + *rot = TOOL_FRAME_SPINDLE; + return 0; +} + +int kinsIdentityJacobian(const kins_params *p, const double *joint, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) +{ + double dP[EMCMOT_MAX_AXIS][EMCMOT_MAX_AXIS]; + int a, b; + (void)joint; (void)pos; (void)iflags; + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + for (b = 0; b < EMCMOT_MAX_AXIS; b++) { dP[a][b] = (a == b) ? 1.0 : 0.0; } + } + return kinsJacobianFromMappedAxesP(p, (const double (*)[EMCMOT_MAX_AXIS])dP, jac); +} + +const kins_ops KINS_IDENTITY_OPS = { + .forward = kinsIdentityForward, + .inverse = kinsIdentityInverse, + .work = kinsIdentityFrame, + .tool = kinsIdentityFrame, + .native = &TOOL_FRAME_SPINDLE, + .jacobian = kinsIdentityJacobian, + .fwd_iterates = 0, + .identity = 1, +}; + +//---------------------------------------------------------------------- +// asking an ops table, defaults applied +//---------------------------------------------------------------------- + +int kinsOpsForward(const kins_ops *ops, const kins_params *p, + kins_scratch *s, const double *joint, EmcPose *pos, + const KINEMATICS_FORWARD_FLAGS *fflags, + KINEMATICS_INVERSE_FLAGS *iflags) +{ + int r; + if (!ops || !ops->forward || !p || !s) { return -1; } + if (ops->fwd_iterates && s->have_pose_seed) { + /* no pose of our own yet: start from the caller's estimate, + which stays in *pos, rather than from a never-solved seed */ + if (s->pose_seed_ok) { *pos = s->pose_seed; } + s->have_pose_seed = 0; + } + r = ops->forward(p, s, joint, pos, fflags, iflags); + if (ops->fwd_iterates && r == 0) { + /* keep the result only when the solve succeeds */ + s->pose_seed = *pos; + s->pose_seed_ok = 1; + } + return r; +} + +int kinsOpsInverse(const kins_ops *ops, const kins_params *p, + kins_scratch *s, const EmcPose *pos, double *joint, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags) +{ + if (!ops || !ops->inverse || !p || !s) { return -1; } + return ops->inverse(p, s, pos, joint, iflags, fflags); +} + +int kinsOpsWorkFrame(const kins_ops *ops, const kins_params *p, + const double *joint, PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + if (!ops || !p || !rot) { return -1; } + if (!ops->work) { return -1; } // not supplied; not an error + return ops->work(p, joint, rot, fflags); +} + +int kinsOpsToolFrame(const kins_ops *ops, const kins_params *p, + const double *joint, PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + int r; + if (!ops || !p || !rot) { return -1; } + if (!ops->tool) { return -1; } // not supplied; not an error + r = ops->tool(p, joint, rot, fflags); + if (r) { return r; } + return toolFrameApplyNative(rot, ops->native ? ops->native + : &TOOL_FRAME_SPINDLE); +} + +int kinsOpsJacobian(const kins_ops *ops, const kins_params *p, + kins_scratch *s, const double *joint, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) +{ + double qp[EMCMOT_MAX_JOINTS], qm[EMCMOT_MAX_JOINTS]; + KINEMATICS_INVERSE_FLAGS ifl = iflags ? *iflags : 0; + KINEMATICS_FORWARD_FLAGS ffl = 0; + EmcPose q; + int j, a; + + if (!ops || !p || !s || !joint || !pos || !jac) { return -1; } + if (ops->jacobian) { return ops->jacobian(p, joint, pos, jac, iflags); } + if (!ops->inverse) { return -1; } + + // the same differences as kinsJacobianFromInverse(), on the block form + kj_zero(jac); + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + q = *pos; + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { qp[j] = qm[j] = joint[j]; } + + *kj_coord(&q, a) += KINS_JACOBIAN_STEP; + if (ops->inverse(p, s, &q, qp, &ifl, &ffl)) { return -1; } + + *kj_coord(&q, a) -= 2 * KINS_JACOBIAN_STEP; + if (ops->inverse(p, s, &q, qm, &ifl, &ffl)) { return -1; } + + for (j = 0; j < p->max_joints && j < EMCMOT_MAX_JOINTS; j++) { + jac[j][a] = (qp[j] - qm[j]) / (2 * KINS_JACOBIAN_STEP); + } + } + return 0; +} // kinsOpsJacobian() + +//---------------------------------------------------------------------- +// the RT side of the table: one HAL pin per entry, copied into the block +// before a call and out of the scratch after it +//---------------------------------------------------------------------- + +int kinsParamsPinsCreate(int comp_id, const char *prefix, + const kins_param_desc *params, int nparams, + kins_pin_ref **out) +{ + kins_pin_ref *pins; + int i, res = 0; + + if (!out) { return -1; } + *out = NULL; + if (nparams < 0 || nparams > KINS_MAX_PARAMS) { return -1; } + if (nparams == 0) { return 0; } + if (!params || !prefix) { return -1; } + + pins = hal_malloc(nparams * sizeof(*pins)); + if (!pins) { + rtapi_print_msg(RTAPI_MSG_ERR, "kinsParamsPinsCreate: hal_malloc failed\n"); + return -1; + } + for (i = 0; i < nparams; i++) { + const kins_param_desc *d = ¶ms[i]; + hal_pdir_t dir = d->dir == KINS_OUT ? HAL_OUT : d->dir == KINS_IO ? HAL_IO : HAL_IN; + switch (d->type) { + case KINS_PARAM_FLOAT: + res += hal_pin_new_real(comp_id, dir, &pins[i].r, d->dflt, "%s.%s", prefix, d->name); + break; + case KINS_PARAM_BIT: + res += hal_pin_new_bool(comp_id, dir, &pins[i].b, d->dflt != 0, "%s.%s", prefix, d->name); + break; + case KINS_PARAM_S32: + res += hal_pin_new_si32(comp_id, dir, &pins[i].s, (rtapi_s32)d->dflt, "%s.%s", prefix, d->name); + break; + case KINS_PARAM_U32: + res += hal_pin_new_ui32(comp_id, dir, &pins[i].u, (rtapi_u32)d->dflt, "%s.%s", prefix, d->name); + break; + default: + res = -1; + } + } + if (res) { + rtapi_print_msg(RTAPI_MSG_ERR, "kinsParamsPinsCreate: pin create failed for %s\n", prefix); + return -1; + } + *out = pins; + return 0; +} // kinsParamsPinsCreate() + +void kinsParamsPinsRead(const kins_pin_ref *pins, + const kins_param_desc *params, int nparams, + kins_params *p) +{ + int i; + if (!pins || !params || !p) { return; } + for (i = 0; i < nparams && i < KINS_MAX_PARAMS; i++) { + const kins_param_desc *d = ¶ms[i]; + double v; + if (d->dir == KINS_OUT) { continue; } + switch (d->type) { + case KINS_PARAM_FLOAT: v = hal_get_real(pins[i].r); break; + case KINS_PARAM_BIT: v = hal_get_bool(pins[i].b) ? 1.0 : 0.0; break; + case KINS_PARAM_S32: v = hal_get_si32(pins[i].s); break; + case KINS_PARAM_U32: v = hal_get_ui32(pins[i].u); break; + default: v = 0; + } + p->geometry[i] = v; + if (d->tool) { p->tool.tran.z = v; } + } +} // kinsParamsPinsRead() + +void kinsParamsPinsWrite(const kins_pin_ref *pins, + const kins_param_desc *params, int nparams, + const kins_scratch *s) +{ + int i; + if (!pins || !params || !s) { return; } + for (i = 0; i < nparams && i < KINS_MAX_PARAMS; i++) { + const kins_param_desc *d = ¶ms[i]; + if (d->dir != KINS_OUT) { continue; } + switch (d->type) { + case KINS_PARAM_FLOAT: hal_set_real(pins[i].r, s->out[i]); break; + case KINS_PARAM_BIT: hal_set_bool(pins[i].b, s->out[i] != 0); break; + case KINS_PARAM_S32: hal_set_si32(pins[i].s, (rtapi_s32)s->out[i]); break; + case KINS_PARAM_U32: hal_set_ui32(pins[i].u, (rtapi_u32)s->out[i]); break; + default: break; + } + } +} // kinsParamsPinsWrite() + #ifdef RTAPI EXPORT_SYMBOL(map_coordinates_to_jnumbers); EXPORT_SYMBOL(mapped_joints_to_position); @@ -1323,4 +1750,23 @@ EXPORT_SYMBOL(kinsJacobianFromMappedAxes); EXPORT_SYMBOL(kinsJacobianFromDhArm); EXPORT_SYMBOL(TOOL_FRAME_SPINDLE); EXPORT_SYMBOL(TOOL_FRAME_FLANGE); +EXPORT_SYMBOL(KINS_IDENTITY_OPS); +EXPORT_SYMBOL(kinsIdentityForward); +EXPORT_SYMBOL(kinsIdentityInverse); +EXPORT_SYMBOL(kinsIdentityFrame); +EXPORT_SYMBOL(kinsIdentityJacobian); +EXPORT_SYMBOL(kinsJacobianFromMappedAxesP); +EXPORT_SYMBOL(kinsMappedJointsToPose); +EXPORT_SYMBOL(kinsPoseToMappedJoints); +EXPORT_SYMBOL(kinsParamsInit); +EXPORT_SYMBOL(kinsParamsMapCoordinates); +EXPORT_SYMBOL(kinsScratchInit); +EXPORT_SYMBOL(kinsOpsForward); +EXPORT_SYMBOL(kinsOpsInverse); +EXPORT_SYMBOL(kinsOpsWorkFrame); +EXPORT_SYMBOL(kinsOpsToolFrame); +EXPORT_SYMBOL(kinsOpsJacobian); +EXPORT_SYMBOL(kinsParamsPinsCreate); +EXPORT_SYMBOL(kinsParamsPinsRead); +EXPORT_SYMBOL(kinsParamsPinsWrite); #endif diff --git a/src/emc/kinematics/switchkins.c b/src/emc/kinematics/switchkins.c index 2509e4a9d47..2734afbd259 100644 --- a/src/emc/kinematics/switchkins.c +++ b/src/emc/kinematics/switchkins.c @@ -27,10 +27,12 @@ * Using modules must supply function: switchkinsSetup() */ #include +#include #include #include #include +#include //********************************************************************* // kinematic functions (default=0 for err detection): @@ -46,6 +48,15 @@ static KTI ktinvs[SWITCHKINS_MAX_TYPES] = {NULL}; static KJ kjacs[SWITCHKINS_MAX_TYPES] = {NULL}; static PmRotationMatrix knative[SWITCHKINS_MAX_TYPES]; +// types written as pure functions (see kinematics.h): the maths of each, +// the one RT parameter block they all read, a scratch per type, and the +// pins made from the module's table +static const kins_ops *kops[SWITCHKINS_MAX_TYPES] = {NULL}; +static kins_params rt_params; +static kins_scratch rt_scratch[SWITCHKINS_MAX_TYPES]; +static kins_pin_ref *pins; +static int inited; + // types provided, counted in rtapi_app_main() once they are all in static int kins_count; static int register_error; @@ -102,6 +113,36 @@ static void get_lastpose(int ktype, EmcPose* pos) pos->w = lastpose[ktype].w; } // get_lastpose() +// the block sees the pins as they are now, and the type asked for +static void read_block(int ktype) +{ + rt_params.ktype = ktype; + kinsParamsPinsRead(pins, kp.params, kp.nparams, &rt_params); +} + +static void write_block(int ktype) +{ + kinsParamsPinsWrite(pins, kp.params, kp.nparams, &rt_scratch[ktype]); +} + +// the forward of one type, whichever way it was provided, from the pose +// it is handed: no seeding, which is the caller's business +static int call_forward(int ktype, const double *joint, EmcPose *pos, + const KINEMATICS_FORWARD_FLAGS *fflags, + KINEMATICS_INVERSE_FLAGS *iflags) +{ + int r; + if (kops[ktype]) { + read_block(ktype); + r = kops[ktype]->forward(&rt_params, &rt_scratch[ktype], + joint, pos, fflags, iflags); + write_block(ktype); + return r; + } + if (!kfwds[ktype]) { return -1; } + return kfwds[ktype](joint, pos, fflags, iflags); +} + static int gui_forward_kins(const double *joints, const EmcPose* estimate) { // the hexapod vismach gui uses these hal pins to @@ -113,7 +154,7 @@ static int gui_forward_kins(const double *joints, const EmcPose* estimate) KINEMATICS_INVERSE_FLAGS iflags; if ( kp.gui_kinstype < 0 || kp.gui_kinstype >= kins_count - || !kfwds[kp.gui_kinstype]) { + || (!kfwds[kp.gui_kinstype] && !kops[kp.gui_kinstype])) { rtapi_print_msg(RTAPI_MSG_ERR, "gui_forward_kins BAD gui_kinstype <%d>\n", kp.gui_kinstype); @@ -123,8 +164,8 @@ static int gui_forward_kins(const double *joints, const EmcPose* estimate) // no pose of our own yet, start from the caller's lastpose[kp.gui_kinstype] = *estimate; } - res = kfwds[kp.gui_kinstype](joints, &lastpose[kp.gui_kinstype], - &fflags, &iflags); + res = call_forward(kp.gui_kinstype, joints, &lastpose[kp.gui_kinstype], + &fflags, &iflags); lastpose_ok[kp.gui_kinstype] = (res == 0); hal_set_real(swdata->gui_x, lastpose[kp.gui_kinstype].tran.x); hal_set_real(swdata->gui_y, lastpose[kp.gui_kinstype].tran.y); @@ -163,6 +204,10 @@ int kinematicsSwitch(int new_switchkins_type) if (fwd_iterates[switchkins_type] && lastpose_ok[switchkins_type]) { use_lastpose[switchkins_type] = 1; // restarting a kins types } + // a pure type keeps the same restart pose in its own scratch + if (kops[switchkins_type] && kops[switchkins_type]->fwd_iterates) { + rt_scratch[switchkins_type].have_pose_seed = 1; + } return 0; // 0==> no error } // kinematicsSwitch() @@ -174,26 +219,37 @@ int kinematicsForward(const double *joint, int r; EmcPose estimate = *pos; // the caller's guess, the only one we get - if ( fwd_iterates[switchkins_type] - && use_lastpose[switchkins_type] - && lastpose_ok[switchkins_type]) { - // initialize iterative forward kins (ok for identity too) - get_lastpose(switchkins_type,pos); - use_lastpose[switchkins_type] = 0; - } - if ( switchkins_type < 0 || switchkins_type >= kins_count - || !kfwds[switchkins_type]) { + || (!kfwds[switchkins_type] && !kops[switchkins_type])) { rtapi_print_msg(RTAPI_MSG_ERR, "switchkins: Forward BAD switchkins_type \n", switchkins_type); return -1; } - r = kfwds[switchkins_type](joint, pos, fflags, iflags); - if (fwd_iterates[switchkins_type]) { - save_lastpose(switchkins_type,pos); - lastpose_ok[switchkins_type] = (r == 0); + + if (kops[switchkins_type]) { + read_block(switchkins_type); + r = kinsOpsForward(kops[switchkins_type], &rt_params, + &rt_scratch[switchkins_type], + joint, pos, fflags, iflags); + write_block(switchkins_type); + // the gui forward below starts from here, as it did for the + // older form + if (kops[switchkins_type]->fwd_iterates) {save_lastpose(switchkins_type,pos);} + } else { + if ( fwd_iterates[switchkins_type] + && use_lastpose[switchkins_type] + && lastpose_ok[switchkins_type]) { + // initialize iterative forward kins (ok for identity too) + get_lastpose(switchkins_type,pos); + use_lastpose[switchkins_type] = 0; + } + r = kfwds[switchkins_type](joint, pos, fflags, iflags); + if (fwd_iterates[switchkins_type]) { + save_lastpose(switchkins_type,pos); + lastpose_ok[switchkins_type] = (r == 0); + } } if (r) return r; @@ -223,12 +279,20 @@ int kinematicsInverse(const EmcPose * pos, if ( switchkins_type < 0 || switchkins_type >= kins_count - || !kinvs[switchkins_type]) { + || (!kinvs[switchkins_type] && !kops[switchkins_type])) { rtapi_print_msg(RTAPI_MSG_ERR, "switchkins: Inverse BAD switchkins_type \n", switchkins_type); return -1; } + if (kops[switchkins_type]) { + read_block(switchkins_type); + r = kinsOpsInverse(kops[switchkins_type], &rt_params, + &rt_scratch[switchkins_type], + pos, joint, iflags, fflags); + write_block(switchkins_type); + return r; + } r = kinvs[switchkins_type](pos, joint, iflags, fflags); return r; } // kinematicsInverse() @@ -239,9 +303,13 @@ int kinematicsToolFrame(const double *joint, { int r; - if ( switchkins_type < 0 - || switchkins_type >= kins_count - || !ktools[switchkins_type]) { + if (switchkins_type < 0 || switchkins_type >= kins_count) { return -1; } + if (kops[switchkins_type]) { + read_block(switchkins_type); + return kinsOpsToolFrame(kops[switchkins_type], &rt_params, + joint, rot, fflags); + } + if (!ktools[switchkins_type]) { return -1; // this type does not supply one; not an error } r = ktools[switchkins_type](joint, rot, fflags); @@ -256,9 +324,13 @@ int kinematicsWorkFrame(const double *joint, PmRotationMatrix *rot, const KINEMATICS_FORWARD_FLAGS *fflags) { - if ( switchkins_type < 0 - || switchkins_type >= kins_count - || !kworks[switchkins_type]) { + if (switchkins_type < 0 || switchkins_type >= kins_count) { return -1; } + if (kops[switchkins_type]) { + read_block(switchkins_type); + return kinsOpsWorkFrame(kops[switchkins_type], &rt_params, + joint, rot, fflags); + } + if (!kworks[switchkins_type]) { return -1; // this type does not supply one; not an error } // no native rotation here: the work frame has no tool axis to point the @@ -275,10 +347,12 @@ int kinematicsToolFrameInverse(const PmCartesian *axis_in_work, int *free_directions, double *tool_spin) { - if ( switchkins_type < 0 - || switchkins_type >= kins_count - || !ktools[switchkins_type] - || !kworks[switchkins_type]) { + if (switchkins_type < 0 || switchkins_type >= kins_count) { return -1; } + if (kops[switchkins_type]) { + if (!kops[switchkins_type]->tool || !kops[switchkins_type]->work) { + return -1; // this type does not report its frames, so it cannot answer + } + } else if (!ktools[switchkins_type] || !kworks[switchkins_type]) { return -1; // this type does not report its frames, so it cannot answer } @@ -307,6 +381,12 @@ int kinematicsJacobian(const double *joint, if (switchkins_type < 0 || switchkins_type >= kins_count) { return -1; } + if (kops[switchkins_type]) { + read_block(switchkins_type); + return kinsOpsJacobian(kops[switchkins_type], &rt_params, + &rt_scratch[switchkins_type], + joint, world, jac, iflags); + } // a closed form is exact and knows its own singular poses if (kjacs[switchkins_type]) { return kjacs[switchkins_type](joint, world, jac, iflags); @@ -333,7 +413,7 @@ int switchkinsRegister(int ktype, KS kset, KF kfwd, KI kinv) register_error = 1; return -1; } - if (ksetups[ktype] || kfwds[ktype] || kinvs[ktype]) { + if (ksetups[ktype] || kfwds[ktype] || kinvs[ktype] || kops[ktype]) { rtapi_print_msg(RTAPI_MSG_ERR, "switchkinsRegister: switchkins-type %d" " already provided\n", ktype); @@ -346,6 +426,65 @@ int switchkinsRegister(int ktype, KS kset, KF kfwd, KI kinv) return 0; } // switchkinsRegister() +int switchkinsDeclare(int ktype, int flags) +{ + if (ktype < 0 || ktype >= SWITCHKINS_MAX_TYPES) { + rtapi_print_msg(RTAPI_MSG_ERR, + "switchkinsDeclare: BAD switchkins_type <%d>" + " (must be 0..%d)\n", + ktype, SWITCHKINS_MAX_TYPES - 1); + register_error = 1; + return -1; + } + ktype_flags[ktype] = flags; + return 0; +} // switchkinsDeclare() + +int kinematicsTypeFlags(int ktype) +{ + if ( ktype < 0 + || ktype >= kins_count + || (!kfwds[ktype] && !kops[ktype])) { return -1; } + return ktype_flags[ktype]; +} // kinematicsTypeFlags() + +int switchkinsRegisterOps(int ktype, const kins_ops *ops) +{ + if (ktype < 0 || ktype >= SWITCHKINS_MAX_TYPES) { + rtapi_print_msg(RTAPI_MSG_ERR, + "switchkinsRegisterOps: BAD switchkins_type <%d>" + " (must be 0..%d)\n", + ktype, SWITCHKINS_MAX_TYPES - 1); + register_error = 1; + return -1; + } + if (ksetups[ktype] || kfwds[ktype] || kinvs[ktype] || kops[ktype]) { + rtapi_print_msg(RTAPI_MSG_ERR, + "switchkinsRegisterOps: switchkins-type %d" + " already provided\n", ktype); + register_error = 1; + return -1; + } + if (!ops || !ops->forward || !ops->inverse) { + rtapi_print_msg(RTAPI_MSG_ERR, + "switchkinsRegisterOps: switchkins-type %d" + " has no forward or inverse\n", ktype); + register_error = 1; + return -1; + } + if (ops->tool && ops->native && !toolFrameIsProper(ops->native)) { + rtapi_print_msg(RTAPI_MSG_ERR, + "switchkinsRegisterOps: switchkins-type %d" + " declared a rotation that is not orthonormal with" + " determinant +1\n", ktype); + register_error = 1; + return -1; + } + kops[ktype] = ops; + if (ops->identity) { ktype_flags[ktype] |= KINSTYPE_IDENTITY; } + return 0; +} // switchkinsRegisterOps() + int switchkinsRegisterFrames(int ktype, KT kwork, KT ktool, const PmRotationMatrix *native) { @@ -400,26 +539,6 @@ int switchkinsRegisterToolFrameInverse(int ktype, KTI kinv) return 0; } // switchkinsRegisterToolFrameInverse() -int switchkinsDeclare(int ktype, int flags) -{ - if (ktype < 0 || ktype >= SWITCHKINS_MAX_TYPES) { - rtapi_print_msg(RTAPI_MSG_ERR, - "switchkinsDeclare: BAD switchkins_type <%d>" - " (must be 0..%d)\n", - ktype, SWITCHKINS_MAX_TYPES - 1); - register_error = 1; - return -1; - } - ktype_flags[ktype] = flags; - return 0; -} // switchkinsDeclare() - -int kinematicsTypeFlags(int ktype) -{ - if (ktype < 0 || ktype >= kins_count || !kfwds[ktype]) { return -1; } - return ktype_flags[ktype]; -} // kinematicsTypeFlags() - EXPORT_SYMBOL(kinematicsSwitchable); EXPORT_SYMBOL(kinematicsSwitch); EXPORT_SYMBOL(kinematicsType); @@ -435,7 +554,127 @@ EXPORT_SYMBOL(switchkinsRegisterToolFrameInverse); EXPORT_SYMBOL(switchkinsDeclare); EXPORT_SYMBOL(kinematicsTypeFlags); EXPORT_SYMBOL(switchkinsRegisterJacobian); +EXPORT_SYMBOL(switchkinsRegisterOps); EXPORT_SYMBOL(switchkinsInit); +EXPORT_SYMBOL(switchkinsDescribe); +EXPORT_SYMBOL(switchkinsDescribeSetup); +EXPORT_SYMBOL(switchkinsRunSetupWith); +EXPORT_SYMBOL(switchkinsDescribeWith); + +//********************************************************************* +// the module as registered so far, described for a caller outside RT +int switchkinsDescribeSetup(const kparms *k, kins_module_info *info) +{ + int i, n = 0; + + if (!k || !info) { return -1; } + if (k->nparams < 0 || k->nparams > KINS_MAX_PARAMS + || (k->nparams > 0 && !k->params)) { + rtapi_print_msg(RTAPI_MSG_ERR, + "switchkins: %s declares a bad parameter table\n", + k->kinsname ? k->kinsname : "?"); + return -1; + } + memset(info, 0, sizeof(*info)); + info->name = k->kinsname; + info->halprefix = k->halprefix ? k->halprefix : k->kinsname; + info->params = k->params; + info->nparams = k->nparams; + info->required_coordinates = k->required_coordinates; + info->max_joints = k->max_joints; + info->allow_duplicates = k->allow_duplicates; + for (i=0; i < SWITCHKINS_MAX_TYPES; i++) { + info->ops[i] = kops[i]; + if (ksetups[i] || kfwds[i] || kinvs[i] || kops[i]) { n = i + 1; } + } + info->ntypes = n; + return 0; +} // switchkinsDescribeSetup() + +int switchkinsDescribe(kins_module_info *info) +{ + if (!inited) { return -1; } + return switchkinsDescribeSetup(&kp, info); +} // switchkinsDescribe() + +int switchkinsRunSetupWith(switchkinsSetupFunc setup, + kparms* kp, const char* sparm) +{ + KS ksetup[3] = {NULL}; + KF kfwd[3] = {NULL}; + KI kinv[3] = {NULL}; + int i; + + if (!setup || !kp) { return -1; } + memset(kp, 0, sizeof(*kp)); + + // defaults prior to switchkinsSetup() call + kp->kinsname = NULL; + kp->halprefix = NULL; + kp->required_coordinates = ""; + kp->max_joints = 0; // Setup must supply + kp->allow_duplicates = 0; + kp->fwd_iterates_mask = 0; + kp->gui_kinstype = -1; // negative means: not used + + kp->sparm = (char*)sparm; // module parm passed to kins + + // setup provides types 0,1,2 and may also call + // switchkinsRegister() or switchkinsRegisterOps() for any others + if (setup(kp, + &ksetup[0], &ksetup[1], &ksetup[2], + &kfwd[0], &kfwd[1], &kfwd[2], + &kinv[0], &kinv[1], &kinv[2])) { + rtapi_print_msg(RTAPI_MSG_ERR,"\nSwitchkins FAIL:\n"); + return -1; + } + + // the types switchkinsSetup() supplied go in by the same route as + // any other, so that providing one twice is caught + for (i=0; i < 3; i++) { + if (!ksetup[i] && !kfwd[i] && !kinv[i]) { continue; } + if (switchkinsRegister(i, ksetup[i], kfwd[i], kinv[i])) { return -1; } + } + + if (!kp->kinsname) { + rtapi_print_msg(RTAPI_MSG_ERR,"\nSwitchkins FAIL:\n"); + return -1; + } + return 0; +} // switchkinsRunSetupWith() + +int switchkinsDescribeWith(switchkinsSetupFunc setup, + const char *coordinates, const char *sparm, + kins_module_info *info) +{ + // One copy of the module per process, however many callers load it: + // the replay registers the types in the module's statics, so it runs + // once and later calls describe what it left. + static kparms kp; + static int replayed; // 0 not yet, 1 done, -1 failed + static char sparm_seen[256]; + const char *s = sparm ? sparm : ""; + (void)coordinates; // the map is the caller's business, see kinsParamsInit() + + if (!info) { return -1; } + + // the RT instance knows itself already + if (switchkinsDescribe(info) == 0) { return 0; } + + if (!replayed) { + rtapi_strlcpy(sparm_seen, s, sizeof(sparm_seen)); + // a copy outside RT: register the types the way the module would + replayed = switchkinsRunSetupWith(setup, &kp, sparm ? sparm_seen : NULL) ? -1 : 1; + } else if (strncmp(sparm_seen, s, sizeof(sparm_seen) - 1)) { + rtapi_print_msg(RTAPI_MSG_ERR, + "switchkinsDescribeWith: already set up with sparm '%s'," + " not '%s'\n", sparm_seen, s); + return -1; + } + if (replayed < 0) { return -1; } + if (switchkinsDescribeSetup(&kp, info)) { return -1; } + return 0; +} // switchkinsDescribeWith() //********************************************************************* // The caller owns the hal component: it does hal_init() before this and @@ -470,7 +709,7 @@ int switchkinsInit(const int comp_id, // the highest type registered sets the count for (i=0; i < SWITCHKINS_MAX_TYPES; i++) { - if (ksetups[i] || kfwds[i] || kinvs[i]) { kins_count = i + 1; } + if (ksetups[i] || kfwds[i] || kinvs[i] || kops[i]) { kins_count = i + 1; } } if (!kins_count) { emsg = "no switchkins-types provided"; goto error; } @@ -517,6 +756,7 @@ int switchkinsInit(const int comp_id, // a type left out below the highest one provided is a gap, not a count for (i=0; i < kins_count; i++) { + if (kops[i]) { continue; } if (ksetups[i] && kfwds[i] && kinvs[i]) { continue; } rtapi_print_msg(RTAPI_MSG_ERR, "switchkins: switchkins-type %d incomplete:%s%s%s\n", @@ -550,10 +790,36 @@ int switchkinsInit(const int comp_id, if (!coordinates) {coordinates = kp.required_coordinates;} + // the pure types share one block and one set of pins from the table + if (kp.params || kp.nparams) { + kins_module_info mi; + if (switchkinsDescribeSetup(&kp, &mi)) { emsg = "bad table"; goto error; } + if (kinsParamsInit(&rt_params, &mi, coordinates)) { + emsg = "coordinates"; goto error; + } + if (kinsParamsPinsCreate(comp_id, kp.halprefix, kp.params, kp.nparams, + &pins)) { + emsg = "table pin create fail"; goto error; + } + } else { + for (i=0; i < kins_count; i++) { + if (kops[i]) { + kins_module_info mi; + if (switchkinsDescribeSetup(&kp, &mi)) { emsg = "bad table"; goto error; } + if (kinsParamsInit(&rt_params, &mi, coordinates)) { + emsg = "coordinates"; goto error; + } + break; + } + } + } + for (i=0; i < SWITCHKINS_MAX_TYPES; i++) { kinsScratchInit(&rt_scratch[i]); } + for (i=0; i < kins_count; i++) { - ksetups[i](comp_id,coordinates,&kp); + if (ksetups[i]) { ksetups[i](comp_id,coordinates,&kp); } } + inited = 1; return 0; error: diff --git a/src/emc/kinematics/switchkins_main.c b/src/emc/kinematics/switchkins_main.c index 38f726e1e43..acecd2add31 100644 --- a/src/emc/kinematics/switchkins_main.c +++ b/src/emc/kinematics/switchkins_main.c @@ -19,9 +19,10 @@ /* switchkins_main.c provides rtapi_app_main() for kinematics modules * built around switchkins.c. A module that gets its rtapi_app_main() * from somewhere else (a halcompile component, for instance) links -* switchkins.c alone and calls switchkinsInit() itself. +* switchkins.c without this file and calls switchkinsInit() itself. * -* Using modules must supply function: switchkinsSetup() +* Using modules must supply function: switchkinsSetup(), which +* switchkinsRunSetup() in switchkins_setup.c runs. */ #include #include @@ -41,43 +42,8 @@ static int comp_id = -1; int rtapi_app_main(void) { kparms kp; - KS ksetup[3] = {NULL}; - KF kfwd[3] = {NULL}; - KI kinv[3] = {NULL}; - int i; - // defaults prior to switchkinsSetup() call - kp.kinsname = NULL; - kp.halprefix = NULL; - kp.required_coordinates = ""; - kp.max_joints = 0; // Setup must supply - kp.allow_duplicates = 0; - kp.fwd_iterates_mask = 0; - kp.gui_kinstype = -1; // negative means: not used - - kp.sparm = sparm; // module parm passed to kins - - // switchkinsSetup() provides types 0,1,2 and may also call - // switchkinsRegister() for any others - if (switchkinsSetup(&kp, - &ksetup[0], &ksetup[1], &ksetup[2], - &kfwd[0], &kfwd[1], &kfwd[2], - &kinv[0], &kinv[1], &kinv[2])) { - rtapi_print_msg(RTAPI_MSG_ERR,"\nSwitchkins FAIL:\n"); - return -1; - } - - // the types switchkinsSetup() supplied go in by the same route as - // any other, so that providing one twice is caught - for (i=0; i < 3; i++) { - if (!ksetup[i] && !kfwd[i] && !kinv[i]) { continue; } - if (switchkinsRegister(i, ksetup[i], kfwd[i], kinv[i])) { return -1; } - } - - if (!kp.kinsname) { - rtapi_print_msg(RTAPI_MSG_ERR,"\nSwitchkins FAIL:\n"); - return -1; - } + if (switchkinsRunSetup(&kp, sparm)) { return -1; } comp_id = hal_init(kp.kinsname); if (comp_id < 0) return comp_id; diff --git a/src/emc/kinematics/switchkins_setup.c b/src/emc/kinematics/switchkins_setup.c new file mode 100644 index 00000000000..d4001dfbaea --- /dev/null +++ b/src/emc/kinematics/switchkins_setup.c @@ -0,0 +1,29 @@ +/* + License GPL Version 2 +*/ + +/* switchkins_setup.c: the part of an in-tree switchkins module that names +* its switchkinsSetup(). Kept apart from switchkins.c so that the core +* names nothing a module defines and can be loaded as switchkins_core. +* +* switchkinsRunSetup() is what rtapi_app_main() calls before +* switchkinsInit(). kinsDescribe() is the description a copy of the +* module loaded outside RT answers with. +*/ +#include + +#include + +int switchkinsRunSetup(kparms* kp, const char* sparm) +{ + return switchkinsRunSetupWith(switchkinsSetup, kp, sparm); +} // switchkinsRunSetup() + +int kinsDescribe(const char *coordinates, const char *sparm, + kins_module_info *info) +{ + return switchkinsDescribeWith(switchkinsSetup, coordinates, sparm, info); +} // kinsDescribe() + +EXPORT_SYMBOL(switchkinsRunSetup); +EXPORT_SYMBOL(kinsDescribe); diff --git a/src/emc/kinematics/trivkins.c b/src/emc/kinematics/trivkins.c index a170c94d6ad..e8c096f92c1 100644 --- a/src/emc/kinematics/trivkins.c +++ b/src/emc/kinematics/trivkins.c @@ -10,63 +10,27 @@ * ********************************************************************/ -#include #include /* RTAPI realtime OS API */ #include /* RTAPI realtime module decls */ -#include #include #include #include #include -#include - - -#define SET(f) pos->f = joints[i] - -int kinematicsForward(const double *joints, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) -{ - return identityKinematicsForward(joints, pos, fflags, iflags); -} - -int kinematicsInverse(const EmcPose * pos, - double *joints, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) -{ - return identityKinematicsInverse(pos, joints, iflags, fflags); -} - -int kinematicsToolFrame(const double *joints, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags) -{ - return identityKinematicsToolFrame(joints, rot, fflags); -} - -int kinematicsWorkFrame(const double *joints, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags) -{ - return identityKinematicsWorkFrame(joints, rot, fflags); -} - -int kinematicsJacobian(const double *joints, - const EmcPose *pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS *iflags) -{ - return identityKinematicsJacobian(joints, pos, jac, iflags); -} - -static KINEMATICS_TYPE ktype = -1; - -KINEMATICS_TYPE kinematicsType() -{ - return ktype; -} +#include + +// joints are axes, through whatever map coordinates= gives; the maths is +// the shared identity and the entry points come from kins_single.c +const kins_module_info kins_module = { + .name = "trivkins", + .halprefix = "trivkins", + .params = NULL, + .nparams = 0, + .required_coordinates = "", + .max_joints = EMCMOT_MAX_JOINTS, + .allow_duplicates = 1, + .ntypes = 1, + .ops = { &KINS_IDENTITY_OPS }, +}; #define TRIVKINS_DEFAULT_COORDINATES "XYZABCUVW" static char *coordinates = TRIVKINS_DEFAULT_COORDINATES; @@ -75,19 +39,40 @@ RTAPI_MP_STRING(coordinates, "Existing Axes"); static char *kinstype = "1"; // use KINEMATICS_IDENTITY RTAPI_MP_STRING(kinstype, "Kinematics Type (Identity,Both)"); -KINS_NOT_SWITCHABLE -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsForward); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsToolFrame); -EXPORT_SYMBOL(kinematicsWorkFrame); -EXPORT_SYMBOL(kinematicsJacobian); MODULE_LICENSE("GPL"); static int comp_id; +// say so when the joints are not in axis order, and which type suits that +static void show_map(KINEMATICS_TYPE ktype) +{ + kins_params p; + int a, unconventional = 0; + + if (kinsParamsInit(&p, &kins_module, coordinates)) { return; } + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + if (p.joint_of_axis[a] >= 0 && p.joint_of_axis[a] != a) { unconventional = 1; } + if (p.joints_of_axis[a] & (p.joints_of_axis[a] - 1)) { unconventional = 1; } + } + if (!unconventional || !strcasecmp(coordinates, "xz")) { return; } + + rtapi_print("\ntrivkins: coordinates:%s\n", coordinates); + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + int j; + for (j = 0; j < p.max_joints; j++) { + if (p.joints_of_axis[a] & (1 << j)) { + rtapi_print(" Joint %d ==> Axis %c\n", j, "XYZABCUVW"[a]); + } + } + } + if (ktype != KINEMATICS_BOTH) { + rtapi_print("trivkins: Recommend: kinstype=both\n"); + } + rtapi_print("\n"); +} + int rtapi_app_main(void) { - kparms ksetup; + KINEMATICS_TYPE ktype; switch (*kinstype) { case 'b': case 'B': ktype = KINEMATICS_BOTH; break; @@ -99,29 +84,14 @@ int rtapi_app_main(void) { comp_id = hal_init("trivkins"); if(comp_id < 0) return comp_id; - // see typedef for KS KinematicsSETUP: - ksetup.max_joints = EMCMOT_MAX_JOINTS; - ksetup.allow_duplicates = 1; - if (identityKinematicsSetup(comp_id, coordinates, &ksetup)) { - return -1; //setup failed + if (kinsSingleInit(comp_id, &kins_module, coordinates, ktype)) { + hal_exit(comp_id); + return -1; } + show_map(ktype); hal_ready(comp_id); return 0; } void rtapi_app_exit(void) { hal_exit(comp_id); } - -// Non-RT entry point: joints are axes, so a non-RT caller needs no -// module code at all and reads nothing from HAL. -int nonrt_attach(const char* coordinates, nonrt_ops_t* ops, - nonrt_resolve_fn resolve, void* arg) -{ - (void)coordinates; (void)resolve; (void)arg; - ops->forward = NULL; - ops->inverse = NULL; - ops->is_identity = 1; - return 0; -} - -EXPORT_SYMBOL(nonrt_attach); diff --git a/src/emc/kinematics/userkfuncs.c b/src/emc/kinematics/userkfuncs.c index 81aa4c2d942..0e51ed651e5 100644 --- a/src/emc/kinematics/userkfuncs.c +++ b/src/emc/kinematics/userkfuncs.c @@ -2,6 +2,12 @@ ** switchable kinematics functions. ** License GPL Version 2 ** +** Two forms are here. USERK_OPS is the current one: identity through +** the parameter block, with no state of its own, registered by a module +** with switchkinsRegisterOps(2, &USERK_OPS). The functions below it are +** the older form, kept for the modules that still register their types +** through switchkinsSetup()'s out parameters. +** ** Example Usage (for customizing the genser-switchkins module): ** (works with rtpreempt only rtai --> Makefile needs work) ** @@ -24,6 +30,34 @@ // #include "genserkins.h" //includes gomath,hal //********************************************************************** +// the current form: pure functions of the block + +static int userk_forward(const kins_params *p, kins_scratch *s, + const double *joint, EmcPose *world, + const KINEMATICS_FORWARD_FLAGS *fflags, + KINEMATICS_INVERSE_FLAGS *iflags) +{ + // replace with the machine's own forward; the block carries the + // geometry (p->geometry[]), the joint map and the tool + return kinsIdentityForward(p, s, joint, world, fflags, iflags); +} + +static int userk_inverse(const kins_params *p, kins_scratch *s, + const EmcPose *world, double *joint, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags) +{ + return kinsIdentityInverse(p, s, world, joint, iflags, fflags); +} + +const kins_ops USERK_OPS = { + .forward = userk_forward, + .inverse = userk_inverse, + // .work, .tool, .native and .jacobian are optional, see kinematics.h +}; + +//********************************************************************** +// the older form // static local variables and functions go here static int userk_inited = 0; diff --git a/src/emc/kinematics_userspace/kinematics_user.c b/src/emc/kinematics_userspace/kinematics_user.c index 66f840a770b..cb9a0eb3dc4 100644 --- a/src/emc/kinematics_userspace/kinematics_user.c +++ b/src/emc/kinematics_userspace/kinematics_user.c @@ -2,14 +2,17 @@ * Description: kinematics_user.c * Non-RT loader for kinematics modules * - * Loads a kinematics .so with dlopen and calls the nonrt_attach() it - * exports, so this process evaluates the kinematics the machine is - * running, at whatever poses it likes. See nonrt_kins.h. + * Loads a kinematics .so with dlopen, asks it to describe itself through + * kinsDescribe(), and evaluates its kinematics through the parameter + * block (see kinematics.h). The block is filled from HAL: one input pin + * of the caller's component per table entry, connected to the signal the + * RT instance's pin reads, so the values are the live ones; and the tool + * from motion's own tooloffset pins where motion is loaded, so that the + * tool the module sees is the one motion has, whether or not the config + * netted it to the module's pin. * - * Identity kinematics needs no module code: the module says so through - * nonrt_ops_t and this file maps joints to axes directly. A module - * exporting no nonrt_attach() is not an error either; the context comes - * back flagged rt_only. + * A module exporting no kinsDescribe() is not an error; the context comes + * back flagged rt_only and answers nothing. * * Author: LinuxCNC * License: GPL Version 2 @@ -19,31 +22,30 @@ ********************************************************************/ #include -#include #include #include #include #include -#include +#include #include "config.h" /* EMC2_HOME */ -typedef int (*nonrt_attach_fn)(const char *coordinates, nonrt_ops_t *ops, - nonrt_resolve_fn resolve, void *arg); +typedef int (*kins_describe_fn)(const char *coordinates, const char *sparm, + kins_module_info *info); -/* One per value a kinematics module reads is a generous bound. */ -#define MAX_MADE_SIGNALS 16 -#define MAX_BOUND_PINS 16 +#define MAX_BOUND_PINS (KINS_MAX_PARAMS + AXIS_COUNT) +#define MAX_MADE_SIGNALS MAX_BOUND_PINS struct KinematicsUserContext { int initialized; - int rt_only; /* 1 if the module exports no nonrt_attach() */ - int is_identity; /* 1 for identity kinematics: no module code needed */ + int rt_only; /* 1 if the module exports no kinsDescribe() */ KINEMATICS_TYPE kins_type; void *rt_handle; /* dlopen handle */ - nonrt_ops_t ops; + kins_module_info info; + kins_params params; + kins_scratch scratch; + int ktype; /* kinematics type being evaluated */ int num_joints; - int joint_to_axis[KINEMATICS_USER_MAX_JOINTS]; /* identity path only */ char module_name[64]; int comp_id; /* the caller's component, owns the pins made here */ const char *prefix; /* its name, which those pin names start with */ @@ -51,6 +53,10 @@ struct KinematicsUserContext { int num_made_signals; hal_refs_u *cell; /* HAL storage those pins are made against */ int num_cells; + int cell_of_param[KINS_MAX_PARAMS]; /* -1 if not bound */ + int cell_of_tool[AXIS_COUNT]; /* motion.tooloffset.*, -1 if absent */ + int tool_param; /* the table's tool entry, -1 if none */ + int warned_tool; }; /* ======================================================================== @@ -58,16 +64,16 @@ struct KinematicsUserContext { * ======================================================================== */ /* - * Give a kinematics module a reference to a value it asked for. + * Give the block a reference to a value it needs. * * The reference is to a pin of ours rather than into the RT instance's, - * so that its lifetime is ours: see nonrt_kins.h. Ours is connected to - * the signal the RT pin reads, or, when the RT pin has no signal, to one - * made here and removed again in kinematicsUserFree(). + * so that its lifetime is ours. Ours is connected to the signal the RT + * pin reads, or, when the RT pin has no signal, to one made here and + * removed again in kinematicsUserFree(). * * The reference has to live in HAL shared memory, since that is where * HAL rewrites it on connect and disconnect, so the pins are made - * against hal_malloc() cells and the module gets what a cell holds once + * against hal_malloc() cells and the block reads what a cell holds once * the connection is in place. */ static int make_signal(KinematicsUserContext *ctx, const char *pin_name, @@ -96,25 +102,34 @@ static int new_pin(int comp_id, hal_type_t type, hal_refs_u *out, const char *name) { switch (type) { - case HAL_BOOL: return hal_pin_new_bool(comp_id, HAL_IN, &out->b, 0, "%s", name); - case HAL_REAL: return hal_pin_new_real(comp_id, HAL_IN, &out->r, 0.0, "%s", name); - case HAL_SINT: return hal_pin_new_sint(comp_id, HAL_IN, &out->s, 0, "%s", name); - case HAL_UINT: return hal_pin_new_uint(comp_id, HAL_IN, &out->u, 0, "%s", name); + case HAL_BOOL: return hal_pin_new_bool(comp_id, HAL_IN, &out->b, 0, "%s", name); + case HAL_REAL: return hal_pin_new_real(comp_id, HAL_IN, &out->r, 0.0, "%s", name); + case HAL_SINT: return hal_pin_new_si32(comp_id, HAL_IN, &out->s, 0, "%s", name); + case HAL_UINT: return hal_pin_new_ui32(comp_id, HAL_IN, &out->u, 0, "%s", name); default: break; } return -1; } -static int bind_pin(const char *pin_name, hal_type_t type, - hal_refs_u *out, void *arg) +/* Does a pin of this name exist? Silent: absence is an answer, not an error. */ +static int pin_exists(const char *pin_name) +{ + hal_query_t q; + memset(&q, 0, sizeof(q)); + q.name = pin_name; + q.qtype = HAL_QTYPE_PIN; + return hal_getref_p(&q) == 0; +} + +/* Bind pin_name; returns the cell index, or -1. */ +static int bind_pin(KinematicsUserContext *ctx, const char *pin_name, + hal_type_t type) { - KinematicsUserContext *ctx = (KinematicsUserContext *)arg; char signal[HAL_NAME_LEN + 1]; char mine[HAL_NAME_LEN + 1]; hal_refs_u *cell; hal_query_t q; - - if (!ctx || !pin_name || !out) return -1; + int idx; memset(&q, 0, sizeof(q)); q.name = pin_name; @@ -147,7 +162,8 @@ static int bind_pin(const char *pin_name, hal_type_t type, fprintf(stderr, "kinematicsUserInit: too many pins to bind\n"); return -1; } - cell = &ctx->cell[ctx->num_cells++]; + idx = ctx->num_cells; + cell = &ctx->cell[idx]; if (new_pin(ctx->comp_id, type, cell, mine) != 0) { fprintf(stderr, "kinematicsUserInit: cannot create pin '%s'\n", mine); @@ -158,30 +174,103 @@ static int bind_pin(const char *pin_name, hal_type_t type, mine, signal); return -1; } + ctx->num_cells++; + return idx; +} - *out = *cell; - return 0; +static hal_type_t hal_type_of(kins_param_type t) +{ + switch (t) { + case KINS_PARAM_BIT: return HAL_BOOL; + case KINS_PARAM_S32: return HAL_SINT; + case KINS_PARAM_U32: return HAL_UINT; + default: return HAL_REAL; + } } -/* ======================================================================== - * Identity joint mapping - * ======================================================================== */ +static double cell_value(const hal_refs_u *cell, kins_param_type t) +{ + switch (t) { + case KINS_PARAM_BIT: return hal_get_bool(cell->b) ? 1.0 : 0.0; + case KINS_PARAM_S32: return hal_get_si32(cell->s); + case KINS_PARAM_U32: return hal_get_ui32(cell->u); + default: return hal_get_real(cell->r); + } +} -static void fill_identity_joint_map(KinematicsUserContext *ctx, const char *coords) +/* Bind every input of the table, and motion's tool where motion is there. */ +static int bind_all(KinematicsUserContext *ctx) { - int i, j = 0; - for (i = 0; i < KINEMATICS_USER_MAX_JOINTS; i++) ctx->joint_to_axis[i] = -1; - if (!coords) return; - for (; *coords && j < ctx->num_joints; coords++) { - int axis; - switch (tolower((unsigned char)*coords)) { - case 'x': axis = 0; break; case 'y': axis = 1; break; - case 'z': axis = 2; break; case 'a': axis = 3; break; - case 'b': axis = 4; break; case 'c': axis = 5; break; - case 'u': axis = 6; break; case 'v': axis = 7; break; - case 'w': axis = 8; break; default: continue; - } - ctx->joint_to_axis[j++] = axis; + static const char letter[AXIS_COUNT] = { 'x','y','z','a','b','c','u','v','w' }; + char name[HAL_NAME_LEN + 1]; + int i; + + for (i = 0; i < KINS_MAX_PARAMS; i++) ctx->cell_of_param[i] = -1; + for (i = 0; i < AXIS_COUNT; i++) ctx->cell_of_tool[i] = -1; + ctx->tool_param = -1; + + for (i = 0; i < ctx->info.nparams; i++) { + const kins_param_desc *d = &ctx->info.params[i]; + if (d->dir == KINS_OUT) continue; + if (d->tool) ctx->tool_param = i; + snprintf(name, sizeof(name), "%s.%s", ctx->info.halprefix, d->name); + ctx->cell_of_param[i] = bind_pin(ctx, name, hal_type_of(d->type)); + if (ctx->cell_of_param[i] < 0) return -1; + } + + /* motion publishes the tool it applies; take it from there when it is + loaded, so the module sees the tool whether or not the config netted + it through. Under halrun with the module alone there is no motion, + and the module's own tool entry is all there is. */ + for (i = 0; i < AXIS_COUNT; i++) { + snprintf(name, sizeof(name), "motion.tooloffset.%c", letter[i]); + if (!pin_exists(name)) continue; + ctx->cell_of_tool[i] = bind_pin(ctx, name, HAL_REAL); + if (ctx->cell_of_tool[i] < 0) return -1; + } + return 0; +} + +/* The block sees the pins as they are now. */ +static void refresh(KinematicsUserContext *ctx) +{ + int i; + double tool[AXIS_COUNT]; + int have_motion_tool = 0; + + for (i = 0; i < ctx->info.nparams; i++) { + int c = ctx->cell_of_param[i]; + if (c < 0) continue; + ctx->params.geometry[i] = cell_value(&ctx->cell[c], ctx->info.params[i].type); + } + if (ctx->tool_param >= 0) { + ctx->params.tool.tran.z = ctx->params.geometry[ctx->tool_param]; + } + + for (i = 0; i < AXIS_COUNT; i++) { + int c = ctx->cell_of_tool[i]; + tool[i] = 0.0; + if (c < 0) continue; + tool[i] = hal_get_real(ctx->cell[c].r); + have_motion_tool = 1; + } + if (!have_motion_tool) return; + + /* the module's pin and motion disagree: the config lost the tool + somewhere between them. Say so once; motion's value is the one + being cut with. */ + if (ctx->tool_param >= 0 && !ctx->warned_tool + && fabs(tool[AXIS_Z] - ctx->params.geometry[ctx->tool_param]) > 1e-9) { + fprintf(stderr, + "kinematics_user: %s.%s is %.6g but motion.tooloffset.z is %.6g;" + " using motion's value\n", + ctx->info.halprefix, ctx->info.params[ctx->tool_param].name, + ctx->params.geometry[ctx->tool_param], tool[AXIS_Z]); + ctx->warned_tool = 1; + } + for (i = 0; i < AXIS_COUNT; i++) emcPoseSetAxis(&ctx->params.tool, i, tool[i]); + if (ctx->tool_param >= 0) { + ctx->params.geometry[ctx->tool_param] = tool[AXIS_Z]; } } @@ -191,11 +280,12 @@ static void fill_identity_joint_map(KinematicsUserContext *ctx, const char *coor static int load_module(KinematicsUserContext *ctx, const char *module_name, - const char *coordinates) + const char *coordinates, + const char *sparm) { char module_path[512]; void *handle; - nonrt_attach_fn attach; + kins_describe_fn describe; snprintf(module_path, sizeof(module_path), "%s/rtlib/%s.so", EMC2_HOME, module_name); @@ -208,18 +298,18 @@ static int load_module(KinematicsUserContext *ctx, } ctx->rt_handle = handle; - attach = (nonrt_attach_fn)dlsym(handle, "nonrt_attach"); - if (!attach) { - fprintf(stderr, "kinematicsUserInit: '%s' exports no nonrt_attach\n", - module_name); + describe = (kins_describe_fn)dlsym(handle, "kinsDescribe"); + if (!describe) { + fprintf(stderr, "kinematicsUserInit: '%s' exports no kinsDescribe;" + " it cannot be evaluated outside RT\n", module_name); dlclose(handle); ctx->rt_handle = NULL; ctx->rt_only = 1; return -1; } - if (attach(coordinates, &ctx->ops, bind_pin, ctx) != 0) { - fprintf(stderr, "kinematicsUserInit: nonrt_attach failed for '%s'\n", + if (describe(coordinates, sparm, &ctx->info) != 0) { + fprintf(stderr, "kinematicsUserInit: kinsDescribe failed for '%s'\n", module_name); dlclose(handle); ctx->rt_handle = NULL; @@ -227,21 +317,28 @@ static int load_module(KinematicsUserContext *ctx, return -1; } - if (ctx->ops.is_identity) { - ctx->is_identity = 1; - ctx->kins_type = KINEMATICS_IDENTITY; - return 0; + if (ctx->info.ntypes < 1 || !ctx->info.ops[0]) { + fprintf(stderr, "kinematicsUserInit: '%s' has no type 0 in the" + " parameter block form\n", module_name); + dlclose(handle); + ctx->rt_handle = NULL; + ctx->rt_only = 1; + return -1; } - if (!ctx->ops.forward || !ctx->ops.inverse) { - fprintf(stderr, "kinematicsUserInit: '%s' set no fwd/inv\n", module_name); + if (kinsParamsInit(&ctx->params, &ctx->info, coordinates) != 0) { + fprintf(stderr, "kinematicsUserInit: '%s' refuses coordinates '%s'\n", + module_name, coordinates ? coordinates : "(default)"); dlclose(handle); ctx->rt_handle = NULL; ctx->rt_only = 1; return -1; } + kinsScratchInit(&ctx->scratch); - ctx->kins_type = KINEMATICS_BOTH; + ctx->ktype = 0; + ctx->kins_type = ctx->info.ops[0]->identity ? KINEMATICS_IDENTITY + : KINEMATICS_BOTH; return 0; } @@ -249,11 +346,12 @@ static int load_module(KinematicsUserContext *ctx, * Public API * ======================================================================== */ -KinematicsUserContext* kinematicsUserInit(const char* kins_type, - int num_joints, - const char* coordinates, - int comp_id, - const char* prefix) +KinematicsUserContext* kinematicsUserInitSparm(const char* kins_type, + int num_joints, + const char* coordinates, + const char* sparm, + int comp_id, + const char* prefix) { KinematicsUserContext *ctx; @@ -278,59 +376,124 @@ KinematicsUserContext* kinematicsUserInit(const char* kins_type, } strncpy(ctx->module_name, kins_type, sizeof(ctx->module_name) - 1); - load_module(ctx, kins_type, coordinates); - - if (ctx->is_identity) { - fill_identity_joint_map(ctx, coordinates); + if (load_module(ctx, kins_type, coordinates, sparm) == 0) { + if (bind_all(ctx) != 0) { + fprintf(stderr, "kinematicsUserInit: cannot bind the pins of '%s'\n", + kins_type); + ctx->rt_only = 1; + } } ctx->initialized = 1; return ctx; } +KinematicsUserContext* kinematicsUserInit(const char* kins_type, + int num_joints, + const char* coordinates, + int comp_id, + const char* prefix) +{ + return kinematicsUserInitSparm(kins_type, num_joints, coordinates, NULL, + comp_id, prefix); +} + +int kinematicsUserSetType(KinematicsUserContext* ctx, int ktype) +{ + if (!ctx || !ctx->initialized || ctx->rt_only) return -1; + if (ktype < 0 || ktype >= ctx->info.ntypes || !ctx->info.ops[ktype]) { + return -1; + } + ctx->ktype = ktype; + ctx->params.ktype = ktype; + kinsScratchInit(&ctx->scratch); + ctx->kins_type = ctx->info.ops[ktype]->identity ? KINEMATICS_IDENTITY + : KINEMATICS_BOTH; + return 0; +} + +int kinematicsUserGetNumTypes(KinematicsUserContext* ctx) +{ + if (!ctx || !ctx->initialized || ctx->rt_only) return 0; + return ctx->info.ntypes; +} + int kinematicsUserInverse(KinematicsUserContext* ctx, const EmcPose* world, double* joints) { + KINEMATICS_INVERSE_FLAGS iflags = 0; + KINEMATICS_FORWARD_FLAGS fflags = 0; + double j[EMCMOT_MAX_JOINTS]; + int i; + if (!ctx || !ctx->initialized || !world || !joints) return -1; + if (ctx->rt_only) return -1; - if (ctx->is_identity) { - int i; - for (i = 0; i < ctx->num_joints; i++) { - int ax = ctx->joint_to_axis[i]; - joints[i] = (ax >= 0) ? emcPoseGetAxis(world, ax) : 0.0; - } - return 0; + refresh(ctx); + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) j[i] = 0.0; + if (kinsOpsInverse(ctx->info.ops[ctx->ktype], &ctx->params, &ctx->scratch, + world, j, &iflags, &fflags) != 0) { + return -1; } - - if (ctx->rt_only) return -1; - return ctx->ops.inverse(world, joints, NULL, NULL); + for (i = 0; i < ctx->num_joints; i++) joints[i] = j[i]; + return 0; } int kinematicsUserForward(KinematicsUserContext* ctx, const double* joints, EmcPose* world) { + KINEMATICS_INVERSE_FLAGS iflags = 0; + KINEMATICS_FORWARD_FLAGS fflags = 0; + double j[EMCMOT_MAX_JOINTS]; + int i; + if (!ctx || !ctx->initialized || !joints || !world) return -1; + if (ctx->rt_only) return -1; - if (ctx->is_identity) { - int i; - memset(world, 0, sizeof(*world)); - for (i = 0; i < ctx->num_joints; i++) { - int ax = ctx->joint_to_axis[i]; - if (ax >= 0) emcPoseSetAxis(world, ax, joints[i]); - } - return 0; + refresh(ctx); + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { + j[i] = (i < ctx->num_joints) ? joints[i] : 0.0; } + memset(world, 0, sizeof(*world)); + return kinsOpsForward(ctx->info.ops[ctx->ktype], &ctx->params, &ctx->scratch, + j, world, &fflags, &iflags); +} +int kinematicsUserJacobian(KinematicsUserContext* ctx, + const EmcPose* world, + double J[KINEMATICS_USER_MAX_JOINTS][AXIS_COUNT]) +{ + KINEMATICS_INVERSE_FLAGS iflags = 0; + KINEMATICS_FORWARD_FLAGS fflags = 0; + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]; + double j[EMCMOT_MAX_JOINTS]; + int r, a; + + if (!ctx || !ctx->initialized || !world || !J) return -1; if (ctx->rt_only) return -1; - return ctx->ops.forward(joints, world, NULL, NULL); + + refresh(ctx); + for (r = 0; r < EMCMOT_MAX_JOINTS; r++) j[r] = 0.0; + if (kinsOpsInverse(ctx->info.ops[ctx->ktype], &ctx->params, &ctx->scratch, + world, j, &iflags, &fflags) != 0) { + return -1; + } + if (kinsOpsJacobian(ctx->info.ops[ctx->ktype], &ctx->params, &ctx->scratch, + j, world, jac, &iflags) != 0) { + return -1; + } + for (r = 0; r < KINEMATICS_USER_MAX_JOINTS; r++) { + for (a = 0; a < AXIS_COUNT; a++) J[r][a] = jac[r][a]; + } + return 0; } int kinematicsUserIsIdentity(KinematicsUserContext* ctx) { - if (!ctx || !ctx->initialized) return 0; - return ctx->is_identity; + if (!ctx || !ctx->initialized || ctx->rt_only) return 0; + return ctx->info.ops[ctx->ktype]->identity; } int kinematicsUserGetNumJoints(KinematicsUserContext* ctx) @@ -353,8 +516,16 @@ const char* kinematicsUserGetModuleName(KinematicsUserContext* ctx) int kinematicsUserRefreshParams(KinematicsUserContext* ctx) { - (void)ctx; - return 0; /* nothing to refresh: the bound pins are the live values */ + if (!ctx || !ctx->initialized || ctx->rt_only) return -1; + refresh(ctx); + return 0; +} + +const kins_params* kinematicsUserParams(KinematicsUserContext* ctx) +{ + if (!ctx || !ctx->initialized || ctx->rt_only) return NULL; + refresh(ctx); + return &ctx->params; } int kinematicsUserIsRtOnly(KinematicsUserContext* ctx) diff --git a/src/emc/motion_planning/Submakefile b/src/emc/motion_planning/Submakefile index e4267fba11e..54a9b384935 100644 --- a/src/emc/motion_planning/Submakefile +++ b/src/emc/motion_planning/Submakefile @@ -8,9 +8,11 @@ LIBKINSLIMITS_CXXSRCS := $(addprefix emc/motion_planning/, \ joint_limits.cc \ ) +# kins_util.c is the shared kinematics code the modules link; the loader +# needs the same block helpers and ops dispatch on this side of dlopen. LIBKINSLIMITS_CSRCS := $(addprefix emc/kinematics_userspace/, \ kinematics_user.c \ - ) + ) emc/kinematics/kins_util.c USERSRCS += $(LIBKINSLIMITS_CXXSRCS) $(LIBKINSLIMITS_CSRCS) diff --git a/src/emc/motion_planning/jacobian.cc b/src/emc/motion_planning/jacobian.cc index a7d5a7661e7..ba8c69fdd42 100644 --- a/src/emc/motion_planning/jacobian.cc +++ b/src/emc/motion_planning/jacobian.cc @@ -12,7 +12,6 @@ #include "jacobian.hh" #include #include -#include namespace motion_planning { @@ -38,128 +37,12 @@ bool JacobianCalculator::init(KinematicsUserContext* kins_ctx) { return true; } -void JacobianCalculator::computeTrivkins(double J[9][9]) { - // Zero the matrix - std::memset(J, 0, sizeof(double) * 9 * 9); - - // For trivkins, the Jacobian is identity (with axis mapping) - // Since trivkins maps: joint[i] = world_axis[mapped_axis[i]] - // The Jacobian is: J[joint][axis] = 1 if axis == mapped_axis[joint], else 0 - - // For a simple XYZ trivkins: - // J[0][AXIS_X] = 1 (joint 0 = X) - // J[1][AXIS_Y] = 1 (joint 1 = Y) - // J[2][AXIS_Z] = 1 (joint 2 = Z) - // etc. - - // We need to query the kinematics context for the mapping. - // Since the context is opaque, we use inverse kinematics to determine - // the mapping. - - // Test each axis: perturb it and see which joint changes - EmcPose zero_pose; - ZERO_EMC_POSE(zero_pose); - double zero_joints[9]; - kinematicsUserInverse(kins_ctx_, &zero_pose, zero_joints); - - for (int axis = 0; axis < AXIS_COUNT; axis++) { - EmcPose test_pose = zero_pose; - emcPoseSetAxis(&test_pose, axis, 1.0); - - double test_joints[9]; - kinematicsUserInverse(kins_ctx_, &test_pose, test_joints); - - for (int joint = 0; joint < num_joints_; joint++) { - double delta = test_joints[joint] - zero_joints[joint]; - if (std::fabs(delta) > 0.5) { - // This axis maps to this joint - J[joint][axis] = 1.0; - } - } - } -} - -bool JacobianCalculator::computeNumerical(const EmcPose& pose, double J[9][9]) { - // Zero the matrix - std::memset(J, 0, sizeof(double) * 9 * 9); - - // Compute joints at nominal pose - double joints_center[9]; - if (kinematicsUserInverse(kins_ctx_, &pose, joints_center) != 0) { - return false; - } - - // Perturb each axis and compute derivatives - for (int axis = 0; axis < AXIS_COUNT; axis++) { - // Choose perturbation size based on axis type - double delta = (axis < 3 || axis >= 6) ? DELTA_LINEAR : DELTA_ROTARY; - - // Positive perturbation - EmcPose pose_plus = pose; - double val_plus = emcPoseGetAxis(&pose_plus, axis) + delta; - emcPoseSetAxis(&pose_plus, axis, val_plus); - - double joints_plus[9]; - if (kinematicsUserInverse(kins_ctx_, &pose_plus, joints_plus) != 0) { - // Kinematics failed - use one-sided difference - for (int joint = 0; joint < num_joints_; joint++) { - J[joint][axis] = (joints_plus[joint] - joints_center[joint]) / delta; - } - continue; - } - - // Negative perturbation - EmcPose pose_minus = pose; - double val_minus = emcPoseGetAxis(&pose_minus, axis) - delta; - emcPoseSetAxis(&pose_minus, axis, val_minus); - - double joints_minus[9]; - if (kinematicsUserInverse(kins_ctx_, &pose_minus, joints_minus) != 0) { - // Use forward difference - for (int joint = 0; joint < num_joints_; joint++) { - J[joint][axis] = (joints_plus[joint] - joints_center[joint]) / delta; - } - continue; - } - - // Central difference (most accurate) - for (int joint = 0; joint < num_joints_; joint++) { - J[joint][axis] = (joints_plus[joint] - joints_minus[joint]) / (2.0 * delta); - } - } - - // Check for NaN/Inf values and replace with safe defaults - bool had_nan = false; - for (int joint = 0; joint < num_joints_; joint++) { - for (int axis = 0; axis < AXIS_COUNT; axis++) { - if (!std::isfinite(J[joint][axis])) { - // Replace NaN/Inf with 0 (assume no coupling) - J[joint][axis] = 0.0; - had_nan = true; - } - } - } - - // If we had NaN values, the Jacobian may be unreliable - // Return true anyway but the condition number check will catch issues - (void)had_nan; // Could log this in debug mode - - return true; -} - bool JacobianCalculator::compute(const EmcPose& pose, double J[9][9]) { if (!kins_ctx_) { return false; } - - if (is_identity_) { - // For trivkins, use the fast identity computation - computeTrivkins(J); - return true; - } else { - // For non-trivial kinematics, use numerical differentiation - return computeNumerical(pose, J); - } + std::memset(J, 0, sizeof(double) * 9 * 9); + return kinematicsUserJacobian(kins_ctx_, &pose, J) == 0; } double JacobianCalculator::conditionNumber(const double J[9][9]) { diff --git a/src/emc/motion_planning/jacobian.hh b/src/emc/motion_planning/jacobian.hh index 8713e89f180..2db3cf00d3e 100644 --- a/src/emc/motion_planning/jacobian.hh +++ b/src/emc/motion_planning/jacobian.hh @@ -3,7 +3,8 @@ * Jacobian calculation for userspace kinematics trajectory planning * * Computes the Jacobian matrix relating world velocities to joint - * velocities. For trivkins this is the identity matrix. + * velocities, from the module's own closed form through the non-RT + * kinematics loader. * * Author: LinuxCNC * License: GPL Version 2 @@ -25,8 +26,8 @@ namespace motion_planning { * Computes the Jacobian matrix J where: * joint_velocities = J × world_velocities * - * For trivkins, J is the identity matrix (with appropriate axis mapping). - * For non-trivial kinematics, J is computed via numerical differentiation. + * The module answers: a closed form where it has one, its inverse + * differenced where it does not. See kinematicsUserJacobian(). */ class JacobianCalculator { public: @@ -72,26 +73,9 @@ public: bool isIdentity() const { return is_identity_; } private: - /** - * Compute Jacobian for trivkins (identity with axis mapping) - */ - void computeTrivkins(double J[9][9]); - - /** - * Compute Jacobian via numerical differentiation - * Uses central differences: J[j][a] = (f(x+h) - f(x-h)) / (2h) - */ - bool computeNumerical(const EmcPose& pose, double J[9][9]); - KinematicsUserContext* kins_ctx_; bool is_identity_; int num_joints_; - - // Perturbation size for numerical differentiation (mm or degrees) - // Must be large enough for kinematics to produce stable results - // but small enough for accurate derivatives - static constexpr double DELTA_LINEAR = 0.1; // 0.1 mm - static constexpr double DELTA_ROTARY = 0.1; // 0.1 degrees }; } // namespace motion_planning diff --git a/src/hal/components/millturn.comp b/src/hal/components/millturn.comp index 4f7c60bb707..29b0c04ce3e 100644 --- a/src/hal/components/millturn.comp +++ b/src/hal/components/millturn.comp @@ -120,7 +120,7 @@ static int turnKinematicsJacobian(const double *j, // halcompile has done hal_init() and does hal_ready() after this returns, // which is what switchkinsInit() expects EXTRA_SETUP() { - kparms kp; + kparms kp = {0}; (void)__comp_inst; (void)prefix; (void)extra_arg; kp.kinsname = "millturn"; diff --git a/src/hal/components/xyzab_tdr_kins.comp b/src/hal/components/xyzab_tdr_kins.comp index 7c6994deabd..f3410ef8fec 100644 --- a/src/hal/components/xyzab_tdr_kins.comp +++ b/src/hal/components/xyzab_tdr_kins.comp @@ -219,7 +219,7 @@ static int tdrKinematicsJacobian(const double *j, // halcompile has done hal_init() and does hal_ready() after this returns, // which is what switchkinsInit() expects EXTRA_SETUP() { - kparms kp; + kparms kp = {0}; (void)__comp_inst; (void)prefix; (void)extra_arg; kp.kinsname = "xyzab_tdr_kins"; diff --git a/src/hal/components/xyzacb_trsrn.comp b/src/hal/components/xyzacb_trsrn.comp index 83c8fcdfc78..ad76e1bacb5 100644 --- a/src/hal/components/xyzacb_trsrn.comp +++ b/src/hal/components/xyzacb_trsrn.comp @@ -589,7 +589,7 @@ static int toolKinematicsJacobian(const double *j, // halcompile has done hal_init() and does hal_ready() after this returns, // which is what switchkinsInit() expects EXTRA_SETUP() { - kparms kp; + kparms kp = {0}; (void)__comp_inst; (void)prefix; (void)extra_arg; kp.kinsname = "xyzacb_trsrn"; diff --git a/src/hal/components/xyzbca_trsrn.comp b/src/hal/components/xyzbca_trsrn.comp index 7e63687c283..b3e8ad89dd9 100644 --- a/src/hal/components/xyzbca_trsrn.comp +++ b/src/hal/components/xyzbca_trsrn.comp @@ -592,7 +592,7 @@ static int toolKinematicsJacobian(const double *j, // halcompile has done hal_init() and does hal_ready() after this returns, // which is what switchkinsInit() expects EXTRA_SETUP() { - kparms kp; + kparms kp = {0}; (void)__comp_inst; (void)prefix; (void)extra_arg; kp.kinsname = "xyzbca_trsrn"; From 7232632e1d1fc5dfe8dfd16bc67d9c80eb04c187 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Fri, 4 Sep 2026 12:48:41 +1000 Subject: [PATCH 03/19] trtfuncs, xyzac-trt-kins, xyzbc-trt-kins, maxkins: move onto the parameter block The trt maths becomes two ops tables over one geometry table, TRT_PARAMS, with the joint map read from the block instead of the JX statics and the tool length from p->tool.tran.z, which the shared code fills from the tool-offset entry. The two modules register the tables with switchkinsRegisterOps() in the order sparm decides, and the identity and userk types come from the shared ops. The joint assignment print and the required-letter check that trtKinematicsSetup() did are now the shared code's, so the setup function goes with the haldata. maxkins keeps its fixed joint order and becomes a kins_single.c module: the table declares pivot-length as the HAL_IO pin it was and conventional-directions as before, and the three functions read the block. Pin names and defaults are unchanged throughout. --- include/kinematics.h | 57 +--- src/Makefile | 3 + src/emc/kinematics/maxkins.c | 131 ++++----- src/emc/kinematics/trtfuncs.c | 402 +++++++++++----------------- src/emc/kinematics/xyzac-trt-kins.c | 48 ++-- src/emc/kinematics/xyzbc-trt-kins.c | 48 ++-- 6 files changed, 266 insertions(+), 423 deletions(-) diff --git a/include/kinematics.h b/include/kinematics.h index 7a46a4ab957..20511017fd5 100644 --- a/include/kinematics.h +++ b/include/kinematics.h @@ -557,6 +557,7 @@ typedef struct kins_scratch { int have_joint_seed; int iterations; int failed; + double aux[8]; /* whatever else a module carries between calls */ double out[KINS_MAX_PARAMS]; /* the table's KINS_OUT entries */ } kins_scratch; @@ -722,57 +723,11 @@ extern int userkKinematicsInverse(const struct EmcPose * world, const KINEMATICS_INVERSE_FLAGS * iflags, KINEMATICS_FORWARD_FLAGS * fflags); //********************************************************************* -// xyzac,xyzbc; -extern int trtKinematicsSetup(const int comp_id, - const char* coordinates, - kparms* ksetup_parms); - -extern int xyzacKinematicsForward(const double *joints, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags); - -extern int xyzacKinematicsInverse(const EmcPose * pos, - double *joints, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags); - -extern int xyzacKinematicsToolFrame(const double *joints, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags); - -extern int xyzacKinematicsWorkFrame(const double *joints, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags); - -extern int xyzacKinematicsJacobian(const double *joints, - const EmcPose *pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS *iflags); - - -extern int xyzbcKinematicsForward(const double *joints, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags); - -extern int xyzbcKinematicsInverse(const EmcPose * pos, - double *joints, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags); - -extern int xyzbcKinematicsToolFrame(const double *joints, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags); - -extern int xyzbcKinematicsWorkFrame(const double *joints, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags); - -extern int xyzbcKinematicsJacobian(const double *joints, - const EmcPose *pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS *iflags); +// xyzac,xyzbc (trtfuncs.c): one geometry table, the maths of each machine +extern const kins_param_desc TRT_PARAMS[]; +extern const int TRT_NPARAMS; +extern const kins_ops XYZAC_OPS; +extern const kins_ops XYZBC_OPS; //********************************************************************* #ifdef __cplusplus diff --git a/src/Makefile b/src/Makefile index 540d30c6a47..8075993dc5d 100644 --- a/src/Makefile +++ b/src/Makefile @@ -1062,6 +1062,9 @@ trivkins-objs += emc/kinematics/kins_single_module.o obj-m += maxkins.o maxkins-objs := emc/kinematics/maxkins.o +maxkins-objs += emc/kinematics/kins_util.o +maxkins-objs += emc/kinematics/kins_single.o +maxkins-objs += emc/kinematics/kins_single_module.o obj-m += rotatekins.o rotatekins-objs := emc/kinematics/rotatekins.o diff --git a/src/emc/kinematics/maxkins.c b/src/emc/kinematics/maxkins.c index a5725480094..b64eee2711e 100644 --- a/src/emc/kinematics/maxkins.c +++ b/src/emc/kinematics/maxkins.c @@ -6,13 +6,13 @@ * * Author: Chris Radek * License: GPL Version 2 -* +* * Copyright (c) 2007 Chris Radek ********************************************************************/ /******************************************************************** -* Note: The direction of the B axis is the opposite of the -* conventional axis direction. See +* Note: The direction of the B axis is the opposite of the +* conventional axis direction. See * https://linuxcnc.org/docs/html/gcode/machining-center.html ********************************************************************/ @@ -22,6 +22,7 @@ #include #include #include /* these decls */ +#include #define d2r(d) ((d)*PM_PI/180.0) #define r2d(r) ((r)*180.0/PM_PI) @@ -30,27 +31,33 @@ #define hypot(a,b) (sqrt((a)*(a)+(b)*(b))) #endif -static struct haldata { - hal_real_t pivot_length; - hal_real_t tool_length; - hal_bool_t conventional_directions; //default is false -} *haldata; - -int kinematicsForward(const double *joints, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +// the geometry, one pin each; the maths reads it from the block +static const kins_param_desc max_params[] = { + { "pivot-length", KINS_PARAM_FLOAT, KINS_IO, 0, 0.666 }, + { "conventional-directions", KINS_PARAM_BIT, KINS_IN, 0, 0 }, // default is unconventional + { "tool-length", KINS_PARAM_FLOAT, KINS_IN, 0, 0 }, +}; +enum { P_PIVOT_LENGTH, P_CON, P_TOOL_LENGTH }; + +#define CON(p) ((p)->geometry[P_CON] != 0 ? 1.0 : -1.0) + +static int max_forward(const kins_params *p, kins_scratch *s, + const double *joints, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)s; (void)fflags; (void)iflags; - rtapi_real con = hal_get_bool(haldata->conventional_directions) ? 1.0 : -1.0; - rtapi_real pivot_length = hal_get_real(haldata->pivot_length); - rtapi_real tool_length = hal_get_real(haldata->tool_length); + const double con = CON(p); + const double pivot_length = p->geometry[P_PIVOT_LENGTH]; + const double tool_length = p->geometry[P_TOOL_LENGTH]; // B correction - const double zb = (pivot_length + joints[8] + tool_length) * cos(d2r(joints[4])); - const double xb = (pivot_length + joints[8] + tool_length) * sin(d2r(joints[4])); + const double zb = (pivot_length + tool_length + joints[8]) * cos(d2r(joints[4])); + const double xb = (pivot_length + tool_length + joints[8]) * sin(d2r(joints[4])); // U correction const double zv = joints[6] * sin(d2r(joints[4])); @@ -82,22 +89,24 @@ int kinematicsForward(const double *joints, return 0; } -int kinematicsInverse(const EmcPose * pos, - double *joints, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int max_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, + double *joints, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { + (void)s; (void)iflags; (void)fflags; - rtapi_real con = hal_get_bool(haldata->conventional_directions) ? 1.0 : -1.0; - rtapi_real pivot_length = hal_get_real(haldata->pivot_length); - rtapi_real tool_length = hal_get_real(haldata->tool_length); + const double con = CON(p); + const double pivot_length = p->geometry[P_PIVOT_LENGTH]; + const double tool_length = p->geometry[P_TOOL_LENGTH]; // B correction - const double zb = (pivot_length + pos->w + tool_length) * cos(d2r(pos->b)); - const double xb = (pivot_length + pos->w + tool_length) * sin(d2r(pos->b)); - + const double zb = (pivot_length + tool_length + pos->w) * cos(d2r(pos->b)); + const double xb = (pivot_length + tool_length + pos->w) * sin(d2r(pos->b)); + // C correction const double xyr = hypot(pos->tran.x, pos->tran.y); const double xytheta = atan2(pos->tran.y, pos->tran.x) - d2r(pos->c); @@ -122,27 +131,27 @@ int kinematicsInverse(const EmcPose * pos, return 0; } -int kinematicsJacobian(const double *joints, - const EmcPose * pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS * iflags) +static int max_jacobian(const kins_params *p, const double *joints, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) { - rtapi_real con = hal_get_bool(haldata->conventional_directions) ? 1.0 : -1.0; - rtapi_real pivot_length = hal_get_real(haldata->pivot_length); + const double con = CON(p); + const double pivot_length = p->geometry[P_PIVOT_LENGTH]; const double k = M_PI/180; const double sb = sin(d2r(pos->b)), cb = cos(d2r(pos->b)); const double sc = sin(d2r(pos->c)), cc = cos(d2r(pos->c)); const double x = pos->tran.x, y = pos->tran.y; - const double R = pivot_length + pos->w; + const double R = pivot_length + p->geometry[P_TOOL_LENGTH] + pos->w; int j; (void)joints; (void)iflags; memset(jac, 0, EMCMOT_MAX_JOINTS * EMCMOT_MAX_AXIS * sizeof(jac[0][0])); - // kinematicsInverse() with the polar form expanded: rotating (x, y) - // by -c is x*cos(c) + y*sin(c) and y*cos(c) - x*sin(c), and the - // B and U corrections are what they are written as + // max_inverse() with the polar form expanded: rotating (x, y) by -c + // is x*cos(c) + y*sin(c) and y*cos(c) - x*sin(c), and the B and U + // corrections are what they are written as jac[0][0] = cc; jac[0][1] = sc; jac[0][4] = (con * R * cb - pos->u * sb) * k; @@ -164,40 +173,40 @@ int kinematicsJacobian(const double *joints, return 0; } -KINEMATICS_TYPE kinematicsType() -{ - return KINEMATICS_BOTH; -} +static const kins_ops max_ops = { + .forward = max_forward, + .inverse = max_inverse, + .jacobian = max_jacobian, +}; + +// joints 0..8 are X..W in order, always; the entry points come from +// kins_single.c +const kins_module_info kins_module = { + .name = "maxkins", + .halprefix = "maxkins", + .params = max_params, + .nparams = sizeof(max_params)/sizeof(max_params[0]), + .required_coordinates = "XYZABCUVW", + .max_joints = 9, + .allow_duplicates = 0, + .ntypes = 1, + .ops = { &max_ops }, +}; -KINS_NOT_SWITCHABLE -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsForward); -EXPORT_SYMBOL(kinematicsJacobian); MODULE_LICENSE("GPL"); static int comp_id; int rtapi_app_main(void) { - int result; comp_id = hal_init("maxkins"); if(comp_id < 0) return comp_id; - haldata = hal_malloc(sizeof(*haldata)); - if(!haldata) { result = -ENOMEM; goto error; } - - result = hal_pin_new_real(comp_id, HAL_IO, &(haldata->pivot_length), 0.666, "maxkins.pivot-length"); - result += hal_pin_new_real(comp_id, HAL_IN, &(haldata->tool_length), 0.0, "maxkins.tool-length"); - // default is unconventional - result += hal_pin_new_bool(comp_id, HAL_IN, &(haldata->conventional_directions), 0, "maxkins.conventional-directions"); - - if(result < 0) goto error; + if (kinsSingleInit(comp_id, &kins_module, "XYZABCUVW", KINEMATICS_BOTH)) { + hal_exit(comp_id); + return -1; + } hal_ready(comp_id); return 0; - -error: - hal_exit(comp_id); - return result; } void rtapi_app_exit(void) { hal_exit(comp_id); } diff --git a/src/emc/kinematics/trtfuncs.c b/src/emc/kinematics/trtfuncs.c index b445524b6e9..023f0a3d1fa 100644 --- a/src/emc/kinematics/trtfuncs.c +++ b/src/emc/kinematics/trtfuncs.c @@ -25,150 +25,68 @@ * This mill has a tilting table (B axis) and horizontal rotary * mounted to the table (C axis). * -* Note: The directions of the rotational axes are the opposite of the -* conventional axis directions. See +* Note: The directions of the rotational axes are the opposite of the +* conventional axis directions. See * https://linuxcnc.org/docs/html/gcode/machining-center.html - +* +* Written as pure functions of the parameter block (see kinematics.h): +* the geometry is the table below, the joint map comes from the block, +* and the tool length is p->tool.tran.z. ********************************************************************/ #include #include -#include -#include #include #include -static int trtfuncs_max_joints; - -// joint number assignments (-1 ==> not assigned) -static int JX = -1; -static int JY = -1; -static int JZ = -1; - -static int JA = -1; -static int JB = -1; -static int JC = -1; - -static int JU = -1; -static int JV = -1; -static int JW = -1; - -struct haldata { - hal_real_t x_rot_point; - hal_real_t y_rot_point; - hal_real_t z_rot_point; - hal_real_t x_offset; - hal_real_t y_offset; - hal_real_t z_offset; - hal_real_t tool_offset; - hal_bool_t conventional_directions; // default: false -} *haldata; - - -int trtKinematicsSetup(const int comp_id, - const char* coordinates, - kparms* kp) -{ - int i,jno,res=0; - int axis_idx_for_jno[EMCMOT_MAX_JOINTS]; - int rqdjoints = strlen(kp->required_coordinates); - - if (rqdjoints > kp->max_joints) { - rtapi_print_msg(RTAPI_MSG_ERR, - "ERROR %s: supports %d joints, <%s> requires %d\n", - kp->kinsname, - kp->max_joints, - coordinates, - rqdjoints); - goto error; - } - trtfuncs_max_joints = kp->max_joints; - - if (map_coordinates_to_jnumbers(coordinates, - kp->max_joints, - kp->allow_duplicates, - axis_idx_for_jno)) { - goto error; - } - // require all chars in reqd_coords (order doesn't matter) - for (i=0; i < rqdjoints; i++) { - char reqd_char; - reqd_char = *(kp->required_coordinates + i); - if ( !strchr(coordinates,toupper(reqd_char)) - && !strchr(coordinates,tolower(reqd_char)) ) { - rtapi_print_msg(RTAPI_MSG_ERR, - "ERROR %s:\nrequired coordinates:%s\n" - "specified coordinates:%s\n", - kp->kinsname, kp->required_coordinates, coordinates); - goto error; - } - } - - // assign principal joint numbers (first found in coordinates map) - // duplicates are handled by position_to_mapped_joints() - for (jno=0; jno < EMCMOT_MAX_JOINTS; jno++) { - if (axis_idx_for_jno[jno] == 0 && JX==-1) {JX = jno;} - if (axis_idx_for_jno[jno] == 1 && JY==-1) {JY = jno;} - if (axis_idx_for_jno[jno] == 2 && JZ==-1) {JZ = jno;} - if (axis_idx_for_jno[jno] == 3 && JA==-1) {JA = jno;} - if (axis_idx_for_jno[jno] == 4 && JB==-1) {JB = jno;} - if (axis_idx_for_jno[jno] == 5 && JC==-1) {JC = jno;} - if (axis_idx_for_jno[jno] == 6 && JU==-1) {JU = jno;} - if (axis_idx_for_jno[jno] == 7 && JV==-1) {JV = jno;} - if (axis_idx_for_jno[jno] == 8 && JW==-1) {JW = jno;} - } - - rtapi_print("%s coordinates=%s assigns:\n", kp->kinsname,coordinates); - for (jno=0; jno Axis %c\n", - jno,"XYZABCUVW"[axis_idx_for_jno[jno]]); - } - - haldata = hal_malloc(sizeof(struct haldata)); - if (!haldata) {goto error;} - - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->x_rot_point), - 0.0, "%s.x-rot-point",kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->y_rot_point), - 0.0, "%s.y-rot-point",kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->z_rot_point), - 0.0, "%s.z-rot-point",kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->x_offset), - 0.0, "%s.x-offset",kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->y_offset), - 0.0, "%s.y-offset",kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->z_offset), - 0.0, "%s.z-offset",kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->tool_offset), - 0.0, "%s.tool-offset",kp->halprefix); - res += hal_pin_new_bool(comp_id, HAL_IN, &(haldata->conventional_directions), - 0, "%s.conventional-directions", kp->halprefix); - if (res) {goto error;} - return 0; - -error: - rtapi_print_msg(RTAPI_MSG_ERR,"trtKinematicsSetup() FAIL\n"); - return -1; -} // trtKinematicsSetup() - -int xyzacKinematicsForward(const double *joints, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +// the geometry both machines share, one pin each +const kins_param_desc TRT_PARAMS[] = { + { "x-rot-point", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "y-rot-point", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "z-rot-point", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "x-offset", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "y-offset", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "z-offset", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "tool-offset", KINS_PARAM_FLOAT, KINS_IN, 1, 0.0 }, + { "conventional-directions", KINS_PARAM_BIT, KINS_IN, 0, 0.0 }, // default: false +}; +const int TRT_NPARAMS = sizeof(TRT_PARAMS)/sizeof(TRT_PARAMS[0]); + +enum { TRT_XR, TRT_YR, TRT_ZR, TRT_XO, TRT_YO, TRT_ZO, TRT_TOOL, TRT_CON }; + +// joint number assignments from the block (-1 ==> not assigned) +#define JX (p->joint_of_axis[0]) +#define JY (p->joint_of_axis[1]) +#define JZ (p->joint_of_axis[2]) +#define JA (p->joint_of_axis[3]) +#define JB (p->joint_of_axis[4]) +#define JC (p->joint_of_axis[5]) +#define JU (p->joint_of_axis[6]) +#define JV (p->joint_of_axis[7]) +#define JW (p->joint_of_axis[8]) + +// the direction sign the conventional-directions pin selects +#define CON(p) ((p)->geometry[TRT_CON] != 0 ? 1.0 : -1.0) + +static int xyzac_forward(const kins_params *p, kins_scratch *s, + const double *joints, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)s; (void)fflags; (void)iflags; - const double x_rot_point = hal_get_real(haldata->x_rot_point); - const double y_rot_point = hal_get_real(haldata->y_rot_point); - const double z_rot_point = hal_get_real(haldata->z_rot_point); - const double dt = hal_get_real(haldata->tool_offset); - const double dy = hal_get_real(haldata->y_offset); - const double dz = hal_get_real(haldata->z_offset) + dt; + const double x_rot_point = p->geometry[TRT_XR]; + const double y_rot_point = p->geometry[TRT_YR]; + const double z_rot_point = p->geometry[TRT_ZR]; + const double dt = p->tool.tran.z; + const double dy = p->geometry[TRT_YO]; + const double dz = p->geometry[TRT_ZO] + dt; const double a_rad = joints[JA]*TO_RAD; const double c_rad = joints[JC]*TO_RAD; - rtapi_real con = hal_get_bool(haldata->conventional_directions) ? 1.0 : -1.0; + const double con = CON(p); pos->tran.x = + cos(c_rad) * (joints[JX] - x_rot_point) - con * sin(c_rad) * cos(a_rad) * (joints[JY] - dy - y_rot_point) @@ -198,25 +116,27 @@ int xyzacKinematicsForward(const double *joints, pos->w = (JW != -1)? joints[JW] : 0; return 0; -} // xyzacKinematicsForward() +} // xyzac_forward() -int xyzacKinematicsInverse(const EmcPose * pos, - double *joints, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int xyzac_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, + double *joints, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { + (void)s; (void)iflags; (void)fflags; - const double x_rot_point = hal_get_real(haldata->x_rot_point); - const double y_rot_point = hal_get_real(haldata->y_rot_point); - const double z_rot_point = hal_get_real(haldata->z_rot_point); - const double dy = hal_get_real(haldata->y_offset); - const double dt = hal_get_real(haldata->tool_offset); - const double dz = hal_get_real(haldata->z_offset) + dt; + const double x_rot_point = p->geometry[TRT_XR]; + const double y_rot_point = p->geometry[TRT_YR]; + const double z_rot_point = p->geometry[TRT_ZR]; + const double dy = p->geometry[TRT_YO]; + const double dt = p->tool.tran.z; + const double dz = p->geometry[TRT_ZO] + dt; const double a_rad = pos->a*TO_RAD; const double c_rad = pos->c*TO_RAD; - rtapi_real con = hal_get_bool(haldata->conventional_directions) ? 1.0 : -1.0; + const double con = CON(p); EmcPose P; // computed position @@ -253,16 +173,12 @@ int xyzacKinematicsInverse(const EmcPose * pos, // update joints with support for // multiple-joints per-coordinate letter: // based on computed position - position_to_mapped_joints(trtfuncs_max_joints, - &P, - joints); - - return 0; -} // xyzacKinematicsInverse() + return kinsPoseToMappedJoints(p, &P, joints); +} // xyzac_inverse() -int xyzacKinematicsWorkFrame(const double *joints, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags) +static int xyzac_work_frame(const kins_params *p, const double *joints, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) { (void)fflags; // the forward transform's coefficients for a displacement of the X, Y and @@ -271,7 +187,7 @@ int xyzacKinematicsWorkFrame(const double *joints, const double a_rad = joints[JA]*TO_RAD; const double c_rad = joints[JC]*TO_RAD; - rtapi_real con = hal_get_bool(haldata->conventional_directions) ? 1.0 : -1.0; + const double con = CON(p); rot->x.x = cos(c_rad); rot->y.x = con * sin(c_rad); @@ -286,32 +202,21 @@ int xyzacKinematicsWorkFrame(const double *joints, rot->z.z = cos(a_rad); return 0; -} // xyzacKinematicsWorkFrame() +} // xyzac_work_frame() -int xyzacKinematicsToolFrame(const double *joints, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags) -{ - (void)joints; - (void)fflags; - // both rotaries carry the work, so the tool never turns in the machine - *rot = TOOL_FRAME_SPINDLE; - return 0; -} // xyzacKinematicsToolFrame() - -int xyzacKinematicsJacobian(const double *joints, - const EmcPose *pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS *iflags) +static int xyzac_jacobian(const kins_params *p, const double *joints, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) { (void)joints; (void)iflags; - const double x_rot_point = hal_get_real(haldata->x_rot_point); - const double y_rot_point = hal_get_real(haldata->y_rot_point); - const double z_rot_point = hal_get_real(haldata->z_rot_point); - const double dy = hal_get_real(haldata->y_offset); - const double dt = hal_get_real(haldata->tool_offset); - const double dz = hal_get_real(haldata->z_offset) + dt; + const double x_rot_point = p->geometry[TRT_XR]; + const double y_rot_point = p->geometry[TRT_YR]; + const double z_rot_point = p->geometry[TRT_ZR]; + const double dy = p->geometry[TRT_YO]; + const double dt = p->tool.tran.z; + const double dz = p->geometry[TRT_ZO] + dt; const double sa = sin(pos->a*TO_RAD), ca = cos(pos->a*TO_RAD); const double sc = sin(pos->c*TO_RAD), cc = cos(pos->c*TO_RAD); const double X = pos->tran.x - x_rot_point; @@ -320,12 +225,12 @@ int xyzacKinematicsJacobian(const double *joints, double dP[EMCMOT_MAX_AXIS][EMCMOT_MAX_AXIS]; int a; - rtapi_real con = hal_get_bool(haldata->conventional_directions) ? 1.0 : -1.0; + const double con = CON(p); memset(dP, 0, sizeof(dP)); - // the computed position P of xyzacKinematicsInverse(), differentiated: - // its coefficients for x, y and z, and the same expressions with the + // the computed position P of xyzac_inverse(), differentiated: its + // coefficients for x, y and z, and the same expressions with the // rotation taken a quarter turn on for a and for c dP[0][0] = cc; dP[0][1] = con * sc; @@ -345,29 +250,41 @@ int xyzacKinematicsJacobian(const double *joints, for (a = 3; a < EMCMOT_MAX_AXIS; a++) { dP[a][a] = 1; } - return kinsJacobianFromMappedAxes(trtfuncs_max_joints, - (const double (*)[EMCMOT_MAX_AXIS])dP, - jac); -} // xyzacKinematicsJacobian() - -int xyzbcKinematicsForward(const double *joints, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) + return kinsJacobianFromMappedAxesP(p, (const double (*)[EMCMOT_MAX_AXIS])dP, + jac); +} // xyzac_jacobian() + +// both rotaries carry the work, so the tool never turns in the machine: +// the tool frame is the shared identity one +const kins_ops XYZAC_OPS = { + .forward = xyzac_forward, + .inverse = xyzac_inverse, + .work = xyzac_work_frame, + .tool = kinsIdentityFrame, + .native = &TOOL_FRAME_SPINDLE, + .jacobian = xyzac_jacobian, +}; + +static int xyzbc_forward(const kins_params *p, kins_scratch *s, + const double *joints, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)s; (void)fflags; (void)iflags; // Note: 'principal' joints are used - const double x_rot_point = hal_get_real(haldata->x_rot_point); - const double y_rot_point = hal_get_real(haldata->y_rot_point); - const double z_rot_point = hal_get_real(haldata->z_rot_point); - const double dx = hal_get_real(haldata->x_offset); - const double dt = hal_get_real(haldata->tool_offset); - const double dz = hal_get_real(haldata->z_offset) + dt; + const double x_rot_point = p->geometry[TRT_XR]; + const double y_rot_point = p->geometry[TRT_YR]; + const double z_rot_point = p->geometry[TRT_ZR]; + const double dx = p->geometry[TRT_XO]; + const double dt = p->tool.tran.z; + const double dz = p->geometry[TRT_ZO] + dt; const double b_rad = joints[JB]*TO_RAD; const double c_rad = joints[JC]*TO_RAD; - rtapi_real con = hal_get_bool(haldata->conventional_directions) ? 1.0 : -1.0; + const double con = CON(p); pos->tran.x = cos(c_rad) * cos(b_rad) * (joints[JX] - dx - x_rot_point) - con * sin(c_rad) * (joints[JY] - y_rot_point) @@ -396,25 +313,27 @@ int xyzbcKinematicsForward(const double *joints, pos->w = (JW != -1)? joints[JW] : 0; return 0; -} // xyzbcKinematicsForward() +} // xyzbc_forward() -int xyzbcKinematicsInverse(const EmcPose * pos, - double *joints, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int xyzbc_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, + double *joints, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { + (void)s; (void)iflags; (void)fflags; - const double x_rot_point = hal_get_real(haldata->x_rot_point); - const double y_rot_point = hal_get_real(haldata->y_rot_point); - const double z_rot_point = hal_get_real(haldata->z_rot_point); - const double dx = hal_get_real(haldata->x_offset); - const double dt = hal_get_real(haldata->tool_offset); - const double dz = hal_get_real(haldata->z_offset) + dt; + const double x_rot_point = p->geometry[TRT_XR]; + const double y_rot_point = p->geometry[TRT_YR]; + const double z_rot_point = p->geometry[TRT_ZR]; + const double dx = p->geometry[TRT_XO]; + const double dt = p->tool.tran.z; + const double dz = p->geometry[TRT_ZO] + dt; const double b_rad = pos->b*TO_RAD; const double c_rad = pos->c*TO_RAD; - rtapi_real con = hal_get_bool(haldata->conventional_directions) ? 1.0 : -1.0; + const double con = CON(p); // the offsets seen from the tilted table: the same rotation the // forward applies to them, in the same sense @@ -451,23 +370,19 @@ int xyzbcKinematicsInverse(const EmcPose * pos, // update joints with support for // multiple-joints per-coordinate letter: // based on computed position - position_to_mapped_joints(trtfuncs_max_joints, - &P, - joints); + return kinsPoseToMappedJoints(p, &P, joints); +} // xyzbc_inverse() - return 0; -} // xyzbcKinematicsInverse() - -int xyzbcKinematicsWorkFrame(const double *joints, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags) +static int xyzbc_work_frame(const kins_params *p, const double *joints, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) { (void)fflags; - // see the comment in xyzacKinematicsWorkFrame() + // see the comment in xyzac_work_frame() const double b_rad = joints[JB]*TO_RAD; const double c_rad = joints[JC]*TO_RAD; - rtapi_real con = hal_get_bool(haldata->conventional_directions) ? 1.0 : -1.0; + const double con = CON(p); rot->x.x = cos(c_rad) * cos(b_rad); rot->y.x = con * sin(c_rad) * cos(b_rad); @@ -482,32 +397,21 @@ int xyzbcKinematicsWorkFrame(const double *joints, rot->z.z = cos(b_rad); return 0; -} // xyzbcKinematicsWorkFrame() - -int xyzbcKinematicsToolFrame(const double *joints, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags) -{ - (void)joints; - (void)fflags; - // both rotaries carry the work, so the tool never turns in the machine - *rot = TOOL_FRAME_SPINDLE; - return 0; -} // xyzbcKinematicsToolFrame() +} // xyzbc_work_frame() -int xyzbcKinematicsJacobian(const double *joints, - const EmcPose *pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS *iflags) +static int xyzbc_jacobian(const kins_params *p, const double *joints, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) { (void)joints; (void)iflags; - const double x_rot_point = hal_get_real(haldata->x_rot_point); - const double y_rot_point = hal_get_real(haldata->y_rot_point); - const double z_rot_point = hal_get_real(haldata->z_rot_point); - const double dx = hal_get_real(haldata->x_offset); - const double dt = hal_get_real(haldata->tool_offset); - const double dz = hal_get_real(haldata->z_offset) + dt; + const double x_rot_point = p->geometry[TRT_XR]; + const double y_rot_point = p->geometry[TRT_YR]; + const double z_rot_point = p->geometry[TRT_ZR]; + const double dx = p->geometry[TRT_XO]; + const double dt = p->tool.tran.z; + const double dz = p->geometry[TRT_ZO] + dt; const double sb = sin(pos->b*TO_RAD), cb = cos(pos->b*TO_RAD); const double sc = sin(pos->c*TO_RAD), cc = cos(pos->c*TO_RAD); const double X = pos->tran.x - x_rot_point; @@ -516,12 +420,12 @@ int xyzbcKinematicsJacobian(const double *joints, double dP[EMCMOT_MAX_AXIS][EMCMOT_MAX_AXIS]; int a; - rtapi_real con = hal_get_bool(haldata->conventional_directions) ? 1.0 : -1.0; + const double con = CON(p); memset(dP, 0, sizeof(dP)); - // see the comment in xyzacKinematicsJacobian(); dpx and dpz of the - // inverse depend on b as well + // see the comment in xyzac_jacobian(); dpx and dpz of the inverse + // depend on b as well dP[0][0] = cc * cb; dP[0][1] = con * sc * cb; dP[0][2] = - con * sb; @@ -540,7 +444,15 @@ int xyzbcKinematicsJacobian(const double *joints, for (a = 3; a < EMCMOT_MAX_AXIS; a++) { dP[a][a] = 1; } - return kinsJacobianFromMappedAxes(trtfuncs_max_joints, - (const double (*)[EMCMOT_MAX_AXIS])dP, - jac); -} // xyzbcKinematicsJacobian() + return kinsJacobianFromMappedAxesP(p, (const double (*)[EMCMOT_MAX_AXIS])dP, + jac); +} // xyzbc_jacobian() + +const kins_ops XYZBC_OPS = { + .forward = xyzbc_forward, + .inverse = xyzbc_inverse, + .work = xyzbc_work_frame, + .tool = kinsIdentityFrame, + .native = &TOOL_FRAME_SPINDLE, + .jacobian = xyzbc_jacobian, +}; diff --git a/src/emc/kinematics/xyzac-trt-kins.c b/src/emc/kinematics/xyzac-trt-kins.c index b6b35538f25..3fc14fc9de4 100644 --- a/src/emc/kinematics/xyzac-trt-kins.c +++ b/src/emc/kinematics/xyzac-trt-kins.c @@ -4,11 +4,12 @@ * * NOTEs: * 1) specify all kparms items -* 2) specify 3 KS,KF,KI functions for switchkins_type=0,1,2 -* 3) the 0th switchkins_type is the startup default -* 4) sparm is a module string parameter for configuration -* 5) The directions of the rotational axes are the opposite of the +* 2) the 0th switchkins_type is the startup default +* 3) sparm is a module string parameter for configuration +* 4) The directions of the rotational axes are the opposite of the * conventional axis directions. +* 5) the maths and the geometry table are in trtfuncs.c, written as +* pure functions of the parameter block (see kinematics.h) */ #include @@ -23,47 +24,28 @@ int switchkinsSetup(kparms* kp, KI* kinv0, KI* kinv1, KI* kinv2 ) { + (void)kset0; (void)kset1; (void)kset2; + (void)kfwd0; (void)kfwd1; (void)kfwd2; + (void)kinv0; (void)kinv1; (void)kinv2; kp->kinsname = "xyzac-trt-kins"; // !!! must agree with filename kp->halprefix = "xyzac-trt-kins"; // hal pin names kp->required_coordinates = "xyzac"; kp->allow_duplicates = 1; kp->max_joints = EMCMOT_MAX_JOINTS; + kp->params = TRT_PARAMS; + kp->nparams = TRT_NPARAMS; if (kp->sparm && strstr(kp->sparm,"identityfirst")) { rtapi_print("\n!!! switchkins-type 0 is IDENTITY\n"); - *kset0 = identityKinematicsSetup; - *kfwd0 = identityKinematicsForward; - *kinv0 = identityKinematicsInverse; - - *kset1 = trtKinematicsSetup; // trt: xyzac,xyzbc - *kfwd1 = xyzacKinematicsForward; - *kinv1 = xyzacKinematicsInverse; - switchkinsRegisterFrames(1, xyzacKinematicsWorkFrame, - xyzacKinematicsToolFrame, - &TOOL_FRAME_SPINDLE); - switchkinsDeclare(0, KINSTYPE_IDENTITY); - switchkinsDeclare(1, KINSTYPE_PRIMARY); - switchkinsRegisterJacobian(1, xyzacKinematicsJacobian); + switchkinsRegisterOps(0, &KINS_IDENTITY_OPS); + switchkinsRegisterOps(1, &XYZAC_OPS); } else { rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname); - *kset0 = trtKinematicsSetup; // trt: xyzac,xyzbc - *kfwd0 = xyzacKinematicsForward; - *kinv0 = xyzacKinematicsInverse; - switchkinsRegisterFrames(0, xyzacKinematicsWorkFrame, - xyzacKinematicsToolFrame, - &TOOL_FRAME_SPINDLE); - switchkinsRegisterJacobian(0, xyzacKinematicsJacobian); - - *kset1 = identityKinematicsSetup; - *kfwd1 = identityKinematicsForward; - *kinv1 = identityKinematicsInverse; - switchkinsDeclare(0, KINSTYPE_PRIMARY); - switchkinsDeclare(1, KINSTYPE_IDENTITY); + switchkinsRegisterOps(0, &XYZAC_OPS); + switchkinsRegisterOps(1, &KINS_IDENTITY_OPS); } - *kset2 = userkKinematicsSetup; - *kfwd2 = userkKinematicsForward; - *kinv2 = userkKinematicsInverse; + switchkinsRegisterOps(2, &USERK_OPS); return 0; } diff --git a/src/emc/kinematics/xyzbc-trt-kins.c b/src/emc/kinematics/xyzbc-trt-kins.c index 401311e4398..45c41b448dd 100644 --- a/src/emc/kinematics/xyzbc-trt-kins.c +++ b/src/emc/kinematics/xyzbc-trt-kins.c @@ -4,11 +4,12 @@ * * NOTEs: * 1) specify all kparms items -* 2) specify 3 KS,KF,KI functions for switchkins_type=0,1,2 -* 3) the 0th switchkins_type is the startup default -* 4) sparm is a module string parameter for configuration -* 5) The directions of the rotational axes are the opposite of the +* 2) the 0th switchkins_type is the startup default +* 3) sparm is a module string parameter for configuration +* 4) The directions of the rotational axes are the opposite of the * conventional axis directions. +* 5) the maths and the geometry table are in trtfuncs.c, written as +* pure functions of the parameter block (see kinematics.h) */ #include @@ -23,47 +24,28 @@ int switchkinsSetup(kparms* kp, KI* kinv0, KI* kinv1, KI* kinv2 ) { + (void)kset0; (void)kset1; (void)kset2; + (void)kfwd0; (void)kfwd1; (void)kfwd2; + (void)kinv0; (void)kinv1; (void)kinv2; kp->kinsname = "xyzbc-trt-kins"; // !!! must agree with filename kp->halprefix = "xyzbc-trt-kins"; // hal pin names kp->required_coordinates = "xyzbc"; kp->allow_duplicates = 1; kp->max_joints = EMCMOT_MAX_JOINTS; + kp->params = TRT_PARAMS; + kp->nparams = TRT_NPARAMS; if (kp->sparm && strstr(kp->sparm,"identityfirst")) { rtapi_print("\n!!! switchkins-type 0 is IDENTITY\n"); - *kset0 = identityKinematicsSetup; - *kfwd0 = identityKinematicsForward; - *kinv0 = identityKinematicsInverse; - - *kset1 = trtKinematicsSetup; // trt: xyzac,xyzbc - *kfwd1 = xyzbcKinematicsForward; - *kinv1 = xyzbcKinematicsInverse; - switchkinsRegisterFrames(1, xyzbcKinematicsWorkFrame, - xyzbcKinematicsToolFrame, - &TOOL_FRAME_SPINDLE); - switchkinsDeclare(0, KINSTYPE_IDENTITY); - switchkinsDeclare(1, KINSTYPE_PRIMARY); - switchkinsRegisterJacobian(1, xyzbcKinematicsJacobian); + switchkinsRegisterOps(0, &KINS_IDENTITY_OPS); + switchkinsRegisterOps(1, &XYZBC_OPS); } else { rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname); - *kset0 = trtKinematicsSetup; // trt: xyzac,xyzbc - *kfwd0 = xyzbcKinematicsForward; - *kinv0 = xyzbcKinematicsInverse; - switchkinsRegisterFrames(0, xyzbcKinematicsWorkFrame, - xyzbcKinematicsToolFrame, - &TOOL_FRAME_SPINDLE); - switchkinsRegisterJacobian(0, xyzbcKinematicsJacobian); - - *kset1 = identityKinematicsSetup; - *kfwd1 = identityKinematicsForward; - *kinv1 = identityKinematicsInverse; - switchkinsDeclare(0, KINSTYPE_PRIMARY); - switchkinsDeclare(1, KINSTYPE_IDENTITY); + switchkinsRegisterOps(0, &XYZBC_OPS); + switchkinsRegisterOps(1, &KINS_IDENTITY_OPS); } - *kset2 = userkKinematicsSetup; - *kfwd2 = userkKinematicsForward; - *kinv2 = userkKinematicsInverse; + switchkinsRegisterOps(2, &USERK_OPS); return 0; } From f46c06c5c8ffafc018662f5ddb3b90d6fee01341 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Fri, 4 Sep 2026 12:57:51 +1000 Subject: [PATCH 04/19] corexykins, rotatekins, rosekins, tripodkins, scorbot-kins, the deltas, matrixkins, userkins: move onto the parameter block Each becomes a kins_module over one ops table and links kins_single.c. The maths is unchanged: where it read a pin it reads the block, what it kept between calls it keeps in the scratch, so rosekins counts its turns per caller and reports them as declared outputs, and tripodkins keeps its HAL_IO pins. kinematicsHome() goes from corexykins and rotatekins; nothing called it. The two deltas share their maths with a python module through a header whose geometry was statics; it is a struct the caller passes now, python API unchanged. matrixkins's nine coefficients were HAL parameters and are pins of the same names. In tree, matrixkins and userkins link kins_single.c; out of tree, userkins gets the same entry points from the new kins_single_core module, which the HAL file loads ahead of it, as switchkins_core serves a switchkins component. --- src/Makefile | 28 +++ src/emc/kinematics/corexykins.c | 72 +++--- src/emc/kinematics/kins_single_core.c | 41 ++++ src/emc/kinematics/lineardeltakins-common.h | 46 ++-- src/emc/kinematics/lineardeltakins.c | 94 ++++---- src/emc/kinematics/lineardeltakins.cc | 14 +- src/emc/kinematics/rosekins.c | 110 +++++---- src/emc/kinematics/rotarydeltakins-common.h | 62 +++-- src/emc/kinematics/rotarydeltakins.c | 116 ++++----- src/emc/kinematics/rotarydeltakins.cc | 15 +- src/emc/kinematics/rotatekins.c | 84 ++++--- src/emc/kinematics/scorbot-kins.c | 127 +++------- src/emc/kinematics/tripodkins.c | 246 +++++--------------- src/hal/components/Submakefile | 2 + src/hal/components/matrixkins.comp | 196 ++++++++-------- src/hal/components/userkins.comp | 191 ++++++++------- 16 files changed, 715 insertions(+), 729 deletions(-) create mode 100644 src/emc/kinematics/kins_single_core.c diff --git a/src/Makefile b/src/Makefile index 8075993dc5d..cef626a61be 100644 --- a/src/Makefile +++ b/src/Makefile @@ -1068,15 +1068,27 @@ maxkins-objs += emc/kinematics/kins_single_module.o obj-m += rotatekins.o rotatekins-objs := emc/kinematics/rotatekins.o +rotatekins-objs += emc/kinematics/kins_util.o +rotatekins-objs += emc/kinematics/kins_single.o +rotatekins-objs += emc/kinematics/kins_single_module.o obj-m += tripodkins.o tripodkins-objs := emc/kinematics/tripodkins.o +tripodkins-objs += emc/kinematics/kins_util.o +tripodkins-objs += emc/kinematics/kins_single.o +tripodkins-objs += emc/kinematics/kins_single_module.o obj-m += corexykins.o corexykins-objs := emc/kinematics/corexykins.o +corexykins-objs += emc/kinematics/kins_util.o +corexykins-objs += emc/kinematics/kins_single.o +corexykins-objs += emc/kinematics/kins_single_module.o obj-m += lineardeltakins.o lineardeltakins-objs := emc/kinematics/lineardeltakins.o +lineardeltakins-objs += emc/kinematics/kins_util.o +lineardeltakins-objs += emc/kinematics/kins_single.o +lineardeltakins-objs += emc/kinematics/kins_single_module.o obj-m += pentakins.o pentakins-objs := emc/kinematics/pentakins.o @@ -1085,14 +1097,23 @@ pentakins-objs += $(MATHSTUB) obj-m += rotarydeltakins.o rotarydeltakins-objs := emc/kinematics/rotarydeltakins.o +rotarydeltakins-objs += emc/kinematics/kins_util.o +rotarydeltakins-objs += emc/kinematics/kins_single.o +rotarydeltakins-objs += emc/kinematics/kins_single_module.o rotarydeltakins-objs += libposemath/_posemath.o rotarydeltakins-objs += $(MATHSTUB) obj-m += rosekins.o rosekins-objs := emc/kinematics/rosekins.o +rosekins-objs += emc/kinematics/kins_util.o +rosekins-objs += emc/kinematics/kins_single.o +rosekins-objs += emc/kinematics/kins_single_module.o obj-m += scorbot-kins.o scorbot-kins-objs := emc/kinematics/scorbot-kins.o +scorbot-kins-objs += emc/kinematics/kins_util.o +scorbot-kins-objs += emc/kinematics/kins_single.o +scorbot-kins-objs += emc/kinematics/kins_single_module.o ifeq ($(origin userkfuncs), undefined) # use template: @@ -1179,6 +1200,12 @@ three21kins-objs += emc/kinematics/switchkins_main.o three21kins-objs += emc/kinematics/switchkins_setup.o three21kins-objs += $(USERKFUNCS) +obj-m += kins_single_core.o +kins_single_core-objs := emc/kinematics/kins_single_core.o +kins_single_core-objs += emc/kinematics/kins_single.o +kins_single_core-objs += emc/kinematics/kins_util.o +kins_single_core-objs += $(MATHSTUB) + obj-m += switchkins_core.o switchkins_core-objs := emc/kinematics/switchkins_core.o switchkins_core-objs += emc/kinematics/switchkins.o @@ -1359,6 +1386,7 @@ endif ../rtlib/trivkins$(MODULE_EXT): $(addprefix objects/rt,$(trivkins-objs)) ../rtlib/5axiskins$(MODULE_EXT): $(addprefix objects/rt,$(5axiskins-objs)) ../rtlib/switchkins_core$(MODULE_EXT): $(addprefix objects/rt,$(switchkins_core-objs)) +../rtlib/kins_single_core$(MODULE_EXT): $(addprefix objects/rt,$(kins_single_core-objs)) ../rtlib/maxkins$(MODULE_EXT): $(addprefix objects/rt,$(maxkins-objs)) ../rtlib/rotatekins$(MODULE_EXT): $(addprefix objects/rt,$(rotatekins-objs)) ../rtlib/tripodkins$(MODULE_EXT): $(addprefix objects/rt,$(tripodkins-objs)) diff --git a/src/emc/kinematics/corexykins.c b/src/emc/kinematics/corexykins.c index 6d3ac4e75ab..3ba1496b6e1 100644 --- a/src/emc/kinematics/corexykins.c +++ b/src/emc/kinematics/corexykins.c @@ -9,12 +9,15 @@ #include #include #include +#include -int kinematicsForward(const double *joints - ,EmcPose *pos - ,const KINEMATICS_FORWARD_FLAGS *fflags - ,KINEMATICS_INVERSE_FLAGS *iflags - ) { +static int corexy_forward(const kins_params *p, kins_scratch *s, + const double *joints, EmcPose *pos, + const KINEMATICS_FORWARD_FLAGS *fflags, + KINEMATICS_INVERSE_FLAGS *iflags) +{ + (void)p; + (void)s; (void)fflags; (void)iflags; pos->tran.x = 0.5 * (joints[0] + joints[1]); @@ -30,11 +33,13 @@ int kinematicsForward(const double *joints return 0; } -int kinematicsInverse(const EmcPose *pos - ,double *joints - ,const KINEMATICS_INVERSE_FLAGS *iflags - ,KINEMATICS_FORWARD_FLAGS *fflags - ) { +static int corexy_inverse(const kins_params *p, kins_scratch *s, + const EmcPose *pos, double *joints, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags) +{ + (void)p; + (void)s; (void)iflags; (void)fflags; joints[0] = pos->tran.x + pos->tran.y; @@ -50,12 +55,13 @@ int kinematicsInverse(const EmcPose *pos return 0; } -int kinematicsJacobian(const double *joints, - const EmcPose *pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS *iflags) +static int corexy_jacobian(const kins_params *p, const double *joints, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) { int j; + (void)p; (void)joints; (void)pos; (void)iflags; @@ -67,23 +73,26 @@ int kinematicsJacobian(const double *joints, return 0; } -int kinematicsHome(EmcPose *world - ,double *joint - ,KINEMATICS_FORWARD_FLAGS *fflags - ,KINEMATICS_INVERSE_FLAGS *iflags - ) { - *fflags = 0; - *iflags = 0; - return kinematicsForward(joint, world, fflags, iflags); -} +static const kins_ops corexy_ops = { + .forward = corexy_forward, + .inverse = corexy_inverse, + .jacobian = corexy_jacobian, +}; -KINEMATICS_TYPE kinematicsType() { return KINEMATICS_BOTH; } +// no geometry: the belts are what they are. Joints 0..8 are the nine +// letters in order; the entry points come from kins_single.c +const kins_module_info kins_module = { + .name = "corexykins", + .halprefix = "corexykins", + .params = NULL, + .nparams = 0, + .required_coordinates = "XYZABCUVW", + .max_joints = 9, + .allow_duplicates = 0, + .ntypes = 1, + .ops = { &corexy_ops }, +}; -KINS_NOT_SWITCHABLE -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsForward); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsJacobian); MODULE_LICENSE("GPL"); static int comp_id; @@ -91,6 +100,11 @@ int rtapi_app_main(void) { comp_id = hal_init("corexykins"); if(comp_id < 0) return comp_id; + if (kinsSingleInit(comp_id, &kins_module, "XYZABCUVW", KINEMATICS_BOTH)) { + hal_exit(comp_id); + return -1; + } + hal_ready(comp_id); return 0; } diff --git a/src/emc/kinematics/kins_single_core.c b/src/emc/kinematics/kins_single_core.c new file mode 100644 index 00000000000..f4b58f1204a --- /dev/null +++ b/src/emc/kinematics/kins_single_core.c @@ -0,0 +1,41 @@ +/* + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program; if not, write to the Free Software + Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. +*/ + +/* kins_single_core.c makes kins_single.c and kins_util.c a module of their +* own, for a kinematics module with one type built out of tree. The +* module loaded after it hands its description to kinsSingleInit(), and +* motion reaches the kinematics through the entry points exported here. +* +* loadrt kins_single_core +* loadrt [KINS]KINEMATICS +*/ +#include +#include +#include + +MODULE_LICENSE("GPL"); + +static int comp_id = -1; + +int rtapi_app_main(void) +{ + comp_id = hal_init("kins_single_core"); + if (comp_id < 0) return comp_id; + hal_ready(comp_id); + return 0; +} + +void rtapi_app_exit(void) { hal_exit(comp_id); } diff --git a/src/emc/kinematics/lineardeltakins-common.h b/src/emc/kinematics/lineardeltakins-common.h index 6e0b0037375..203343a20e1 100644 --- a/src/emc/kinematics/lineardeltakins-common.h +++ b/src/emc/kinematics/lineardeltakins-common.h @@ -32,10 +32,16 @@ // common routines used by the userspace kinematics and the realtime kinematics // user must include a math.h-type header first // Inspired by Marlin delta firmware and https://gist.github.com/kastner/5279172 +// +// The geometry is a value the caller holds and passes in, so the same +// routines serve the realtime module through its parameter block and the +// python module through its own copy. #include -static double L, R; -static double Ax, Ay, Bx, By, Cx, Cy, L2; +typedef struct { + double L, R; + double Ax, Ay, Bx, By, Cx, Cy, L2; +} lineardelta_geometry; #define SQ3 (sqrt(3)) @@ -44,31 +50,30 @@ static double Ax, Ay, Bx, By, Cx, Cy, L2; static double sq(double x) { return x*x; } -static void set_geometry(double r_, double l_) +static void lineardelta_set_geometry(lineardelta_geometry *g, double r_, double l_) { - if(L == l_ && R == r_) return; - - L = l_; - R = r_; + g->L = l_; + g->R = r_; - L2 = sq(L); + g->L2 = sq(g->L); - Ax = 0.0; - Ay = R; + g->Ax = 0.0; + g->Ay = g->R; - Bx = -SIN_60 * R; - By = -COS_60 * R; + g->Bx = -SIN_60 * g->R; + g->By = -COS_60 * g->R; - Cx = SIN_60 * R; - Cy = -COS_60 * R; + g->Cx = SIN_60 * g->R; + g->Cy = -COS_60 * g->R; } -static int kinematics_inverse(const EmcPose *pos, double *joints) +static int lineardelta_inverse(const lineardelta_geometry *g, + const EmcPose *pos, double *joints) { double x = pos->tran.x, y = pos->tran.y, z = pos->tran.z; - joints[0] = z + sqrt(L2 - sq(Ax-x) - sq(Ay-y)); - joints[1] = z + sqrt(L2 - sq(Bx-x) - sq(By-y)); - joints[2] = z + sqrt(L2 - sq(Cx-x) - sq(Cy-y)); + joints[0] = z + sqrt(g->L2 - sq(g->Ax-x) - sq(g->Ay-y)); + joints[1] = z + sqrt(g->L2 - sq(g->Bx-x) - sq(g->By-y)); + joints[2] = z + sqrt(g->L2 - sq(g->Cx-x) - sq(g->Cy-y)); joints[3] = pos->a; joints[4] = pos->b; joints[5] = pos->c; @@ -80,11 +85,14 @@ static int kinematics_inverse(const EmcPose *pos, double *joints) ? -1 : 0; } -static int kinematics_forward(const double *joints, EmcPose *pos) +static int lineardelta_forward(const lineardelta_geometry *g, + const double *joints, EmcPose *pos) { double q1 = joints[0]; double q2 = joints[1]; double q3 = joints[2]; + const double Ay = g->Ay, Bx = g->Bx, By = g->By, Cx = g->Cx, Cy = g->Cy; + const double L = g->L; double den = (By-Ay)*Cx-(Cy-Ay)*Bx; diff --git a/src/emc/kinematics/lineardeltakins.c b/src/emc/kinematics/lineardeltakins.c index d894bb46adb..43a59521dc0 100644 --- a/src/emc/kinematics/lineardeltakins.c +++ b/src/emc/kinematics/lineardeltakins.c @@ -19,50 +19,65 @@ #include #include #include +#include #include "lineardeltakins-common.h" -static struct haldata -{ - hal_real_t r; - hal_real_t l; -} *haldata; +// the two lengths, one pin each +static const kins_param_desc ld_params[] = { + { "R", KINS_PARAM_FLOAT, KINS_IN, 0, DELTA_RADIUS }, + { "L", KINS_PARAM_FLOAT, KINS_IN, 0, DELTA_DIAGONAL_ROD }, +}; +enum { P_R, P_L }; static int comp_id; -int kinematicsForward(const double * joints, +// the tower positions follow from the block's two lengths +static void geometry_of(const kins_params *p, lineardelta_geometry *g) +{ + lineardelta_set_geometry(g, p->geometry[P_R], p->geometry[P_L]); +} + +static int ld_forward(const kins_params *p, kins_scratch *s, + const double * joints, EmcPose * pos, const KINEMATICS_FORWARD_FLAGS * fflags, KINEMATICS_INVERSE_FLAGS * iflags) { + lineardelta_geometry g; + (void)s; (void)fflags; (void)iflags; - set_geometry(hal_get_real(haldata->r), hal_get_real(haldata->l)); - return kinematics_forward(joints, pos); + geometry_of(p, &g); + return lineardelta_forward(&g, joints, pos); } -int kinematicsInverse(const EmcPose *pos, double *joints, - const KINEMATICS_INVERSE_FLAGS *iflags, - KINEMATICS_FORWARD_FLAGS *fflags) { +static int ld_inverse(const kins_params *p, kins_scratch *s, + const EmcPose *pos, double *joints, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags) { + lineardelta_geometry g; + (void)s; (void)iflags; (void)fflags; - set_geometry(hal_get_real(haldata->r), hal_get_real(haldata->l)); - return kinematics_inverse(pos, joints); + geometry_of(p, &g); + return lineardelta_inverse(&g, pos, joints); } -int kinematicsJacobian(const double *joints, +static int ld_jacobian(const kins_params *p, const double *joints, const EmcPose *pos, double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], const KINEMATICS_INVERSE_FLAGS *iflags) { + lineardelta_geometry g; double x = pos->tran.x, y = pos->tran.y, z = pos->tran.z; int i, j; (void)iflags; - set_geometry(hal_get_real(haldata->r), hal_get_real(haldata->l)); + geometry_of(p, &g); memset(jac, 0, EMCMOT_MAX_JOINTS * EMCMOT_MAX_AXIS * sizeof(jac[0][0])); // each carriage is the platform height plus the rise of its rod, and // the rise changes with the horizontal offset from the tower for (i = 0; i < 3; i++) { - double tx = (i == 0) ? Ax : (i == 1) ? Bx : Cx; - double ty = (i == 0) ? Ay : (i == 1) ? By : Cy; + double tx = (i == 0) ? g.Ax : (i == 1) ? g.Bx : g.Cx; + double ty = (i == 0) ? g.Ay : (i == 1) ? g.By : g.Cy; double rise = joints[i] - z; if (rise <= 0) { return -1; } jac[i][0] = (tx - x)/rise; @@ -73,32 +88,38 @@ int kinematicsJacobian(const double *joints, return 0; } -KINEMATICS_TYPE kinematicsType() -{ - return KINEMATICS_BOTH; -} +static const kins_ops ld_ops = { + .forward = ld_forward, + .inverse = ld_inverse, + .jacobian = ld_jacobian, +}; + +// three towers for the three linear coordinates, the rest passed +// through; the entry points come from kins_single.c +const kins_module_info kins_module = { + .name = "lineardeltakins", + .halprefix = "lineardeltakins", + .params = ld_params, + .nparams = sizeof(ld_params)/sizeof(ld_params[0]), + .required_coordinates = "XYZABCUVW", + .max_joints = 9, + .allow_duplicates = 0, + .ntypes = 1, + .ops = { &ld_ops }, +}; int rtapi_app_main(void) { - int retval; - comp_id = hal_init("lineardeltakins"); if(comp_id < 0) return comp_id; - haldata = hal_malloc(sizeof(*haldata)); - if(!haldata) { retval = -ENOMEM; goto error; } - - if((retval = hal_pin_new_real(comp_id, HAL_IN, &haldata->r, DELTA_RADIUS, "lineardeltakins.R")) < 0) - goto error; - if((retval = hal_pin_new_real(comp_id, HAL_IN, &haldata->l, DELTA_DIAGONAL_ROD, "lineardeltakins.L")) < 0) - goto error; + if (kinsSingleInit(comp_id, &kins_module, "XYZABCUVW", KINEMATICS_BOTH)) { + hal_exit(comp_id); + return -1; + } hal_ready(comp_id); return 0; - -error: - hal_exit(comp_id); - return retval; } void rtapi_app_exit(void) @@ -106,9 +127,4 @@ void rtapi_app_exit(void) hal_exit(comp_id); } -KINS_NOT_SWITCHABLE -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsForward); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsJacobian); MODULE_LICENSE("GPL"); diff --git a/src/emc/kinematics/lineardeltakins.cc b/src/emc/kinematics/lineardeltakins.cc index 351746081c7..780542ba380 100644 --- a/src/emc/kinematics/lineardeltakins.cc +++ b/src/emc/kinematics/lineardeltakins.cc @@ -21,11 +21,19 @@ using namespace boost::python; #define isnan(x) std::isnan(x) #include "lineardeltakins-common.h" +// the python module keeps one geometry, set from python +static lineardelta_geometry geometry; + +static void set_geometry(double r, double l) +{ + lineardelta_set_geometry(&geometry, r, l); +} + static object forward(double j0, double j1, double j2) { double joints[9] = {j0, j1, j2}; EmcPose pos; - int result = kinematics_forward(joints, &pos); + int result = lineardelta_forward(&geometry, joints, &pos); if(result == 0) return make_tuple(pos.tran.x, pos.tran.y, pos.tran.z); return object(); @@ -35,7 +43,7 @@ static object inverse(double x, double y, double z) { double joints[9]; EmcPose pos = {{x,y,z}, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; - int result = kinematics_inverse(&pos, joints); + int result = lineardelta_inverse(&geometry, &pos, joints); if(result == 0) return make_tuple(joints[0], joints[1], joints[2]); return object(); @@ -43,7 +51,7 @@ static object inverse(double x, double y, double z) static object get_geometry() { - return make_tuple(R, L); + return make_tuple(geometry.R, geometry.L); } #pragma GCC diagnostic push diff --git a/src/emc/kinematics/rosekins.c b/src/emc/kinematics/rosekins.c index ac7a11159f5..d37ce3584be 100644 --- a/src/emc/kinematics/rosekins.c +++ b/src/emc/kinematics/rosekins.c @@ -22,29 +22,36 @@ #include #include #include +#include -KINS_NOT_SWITCHABLE -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsForward); -EXPORT_SYMBOL(kinematicsJacobian); MODULE_LICENSE("GPL"); #ifndef hypot #define hypot(a,b) (sqrt((a)*(a)+(b)*(b))) #endif -static struct haldata { - hal_real_t revolutions; - hal_real_t theta_degrees; - hal_real_t bigtheta_degrees; -} *haldata; - -int kinematicsForward(const double *joints, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +// the inverse reports the turn count it keeps and the angles it saw +static const kins_param_desc rose_params[] = { + { "revolutions", KINS_PARAM_FLOAT, KINS_OUT, 0, 0.0 }, + { "theta_degrees", KINS_PARAM_FLOAT, KINS_OUT, 0, 0.0 }, + { "bigtheta_degrees", KINS_PARAM_FLOAT, KINS_OUT, 0, 0.0 }, +}; +enum { O_REVOLUTIONS, O_THETA, O_BIGTHETA }; + +// what the inverse carries from one call to the next: the quadrant it +// last saw and the turns it has counted. In the scratch, so that each +// caller counts its own. +#define OLDQUAD(s) ((s)->aux[0]) +#define REVOLUTIONS(s) ((s)->aux[1]) + +static int rose_forward(const kins_params *p, kins_scratch *s, + const double *joints, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)p; + (void)s; (void)fflags; (void)iflags; double radius,z,theta; @@ -66,18 +73,20 @@ int kinematicsForward(const double *joints, return 0; } -int kinematicsInverse(const EmcPose * pos, - double *joints, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int rose_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, + double *joints, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { + (void)p; (void)iflags; (void)fflags; // There is a potential problem when accumulating bigtheta -- loss of // precision based on size of mantissa -- but in practice, it is probably ok - static int oldquad; - static int revolutions; + int oldquad = (int)OLDQUAD(s); + int revolutions = (int)REVOLUTIONS(s); double theta,bigtheta; int nowquad = 0; @@ -96,9 +105,9 @@ int kinematicsInverse(const EmcPose * pos, theta = atan2(y,x); bigtheta = theta + PM_2_PI * revolutions; - hal_set_real(haldata->revolutions, revolutions); - hal_set_real(haldata->theta_degrees, theta * TO_DEG); - hal_set_real(haldata->bigtheta_degrees, bigtheta * TO_DEG); + s->out[O_REVOLUTIONS] = revolutions; + s->out[O_THETA] = theta * TO_DEG; + s->out[O_BIGTHETA] = bigtheta * TO_DEG; joints[0] = hypot(x,y); joints[1] = z; @@ -110,18 +119,20 @@ int kinematicsInverse(const EmcPose * pos, joints[7] = 0; joints[8] = 0; - oldquad = nowquad; + OLDQUAD(s) = nowquad; + REVOLUTIONS(s) = revolutions; return 0; } -int kinematicsJacobian(const double *joints, - const EmcPose *pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS *iflags) +static int rose_jacobian(const kins_params *p, const double *joints, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) { double x = pos->tran.x, y = pos->tran.y; double r2 = x*x + y*y; double r = sqrt(r2); + (void)p; (void)joints; (void)iflags; // on the axis the angle is undefined and its rate unbounded @@ -134,34 +145,39 @@ int kinematicsJacobian(const double *joints, return 0; } -KINEMATICS_TYPE kinematicsType() -{ - return KINEMATICS_BOTH; -} +static const kins_ops rose_ops = { + .forward = rose_forward, + .inverse = rose_inverse, + .jacobian = rose_jacobian, +}; + +// joints 0..2 are radius, z and the unwrapped angle; the entry points +// come from kins_single.c +const kins_module_info kins_module = { + .name = "rosekins", + .halprefix = "rosekins", + .params = rose_params, + .nparams = sizeof(rose_params)/sizeof(rose_params[0]), + .required_coordinates = "XYZ", + .max_joints = 3, + .allow_duplicates = 0, + .ntypes = 1, + .ops = { &rose_ops }, +}; static int comp_id; void rtapi_app_exit(void) { hal_exit(comp_id); } int rtapi_app_main(void) { - int ans; comp_id = hal_init("rosekins"); if(comp_id < 0) return comp_id; - haldata = hal_malloc(sizeof(*haldata)); - if(!haldata) { ans = -ENOMEM; goto error; } - - if((ans = hal_pin_new_real(comp_id, HAL_OUT, &(haldata->revolutions), 0.0, "rosekins.revolutions")) < 0) - goto error; - if((ans = hal_pin_new_real(comp_id, HAL_OUT, &(haldata->theta_degrees), 0.0, "rosekins.theta_degrees")) < 0) - goto error; - if((ans = hal_pin_new_real(comp_id, HAL_OUT, &(haldata->bigtheta_degrees), 0.0, "rosekins.bigtheta_degrees")) < 0) - goto error; + if (kinsSingleInit(comp_id, &kins_module, "XYZ", KINEMATICS_BOTH)) { + hal_exit(comp_id); + return -1; + } hal_ready(comp_id); return 0; - -error: - hal_exit(comp_id); - return ans; } diff --git a/src/emc/kinematics/rotarydeltakins-common.h b/src/emc/kinematics/rotarydeltakins-common.h index 59cc872200c..95c6ce95c9e 100644 --- a/src/emc/kinematics/rotarydeltakins-common.h +++ b/src/emc/kinematics/rotarydeltakins-common.h @@ -40,6 +40,10 @@ positive, the Z coordinate will get more negative. Joint zero is the one whose thigh swings in the YZ plane. + + The geometry is a value the caller holds and passes in, so the same + routines serve the realtime module through its parameter block and the + python module through its own copy. */ #ifndef LINUXCNCROTARYDELTAKINS_COMMON_H @@ -47,17 +51,19 @@ #include -// distance from origin to a hip joint -static double platformradius; +typedef struct { + // distance from origin to a hip joint + double platformradius; -// thigh connects the hip to the knee -static double thighlength; + // thigh connects the hip to the knee + double thighlength; -// shin (the parallelogram) connects the knee to the foot -static double shinlength; + // shin (the parallelogram) connects the knee to the foot + double shinlength; -// distance from center of foot (controlled point) to an ankle joint -static double footradius; + // distance from center of foot (controlled point) to an ankle joint + double footradius; +} rotarydelta_geometry; #ifndef sq #define sq(a) ((a)*(a)) @@ -66,15 +72,21 @@ static double footradius; #define D2R(d) ((d)*M_PI/180.) #endif -static void set_geometry(double pfr, double tl, double sl, double fr) { - platformradius = pfr; - thighlength = tl; - shinlength = sl; - footradius = fr; +static void rotarydelta_set_geometry(rotarydelta_geometry *g, + double pfr, double tl, double sl, double fr) { + g->platformradius = pfr; + g->thighlength = tl; + g->shinlength = sl; + g->footradius = fr; } // Given three hip joint angles, find the controlled point -static int kinematics_forward(const double *joints, EmcPose *pos) { +static int rotarydelta_forward(const rotarydelta_geometry *g, + const double *joints, EmcPose *pos) { + const double platformradius = g->platformradius; + const double thighlength = g->thighlength; + const double shinlength = g->shinlength; + const double footradius = g->footradius; double j0 = joints[0], j1 = joints[1], @@ -139,7 +151,12 @@ static int kinematics_forward(const double *joints, EmcPose *pos) { // Given controlled point, find joint zero's angle // (J0 is the easy one in the ZY plane) -static int inverse_j0(double x, double y, double z, double *theta) { +static int rotarydelta_inverse_j0(const rotarydelta_geometry *g, + double x, double y, double z, double *theta) { + const double platformradius = g->platformradius; + const double thighlength = g->thighlength; + const double shinlength = g->shinlength; + const double footradius = g->footradius; double a, b, d, knee_y, knee_z; a = 0.5 * (sq(x) + sq(y - footradius) + sq(z) + sq(thighlength) - @@ -157,25 +174,26 @@ static int inverse_j0(double x, double y, double z, double *theta) { return 0; } -static void rotate(double *x, double *y, double theta) { +static void rotarydelta_rotate(double *x, double *y, double theta) { double xx, yy; xx = *x, yy = *y; *x = xx * cos(theta) - yy * sin(theta); *y = xx * sin(theta) + yy * cos(theta); } -static int kinematics_inverse(const EmcPose *pos, double *joints) { +static int rotarydelta_inverse(const rotarydelta_geometry *g, + const EmcPose *pos, double *joints) { double xr, yr; - if(inverse_j0(pos->tran.x, pos->tran.y, pos->tran.z, &joints[0])) return -1; + if(rotarydelta_inverse_j0(g, pos->tran.x, pos->tran.y, pos->tran.z, &joints[0])) return -1; // now use symmetry property to get the other two just as easily... xr = pos->tran.x; yr = pos->tran.y; - rotate(&xr, &yr, -2*M_PI/3); - if(inverse_j0(xr, yr, pos->tran.z, &joints[1])) return -1; + rotarydelta_rotate(&xr, &yr, -2*M_PI/3); + if(rotarydelta_inverse_j0(g, xr, yr, pos->tran.z, &joints[1])) return -1; xr = pos->tran.x; yr = pos->tran.y; - rotate(&xr, &yr, 2*M_PI/3); - if(inverse_j0(xr, yr, pos->tran.z, &joints[2])) return -1; + rotarydelta_rotate(&xr, &yr, 2*M_PI/3); + if(rotarydelta_inverse_j0(g, xr, yr, pos->tran.z, &joints[2])) return -1; joints[3] = pos->a; joints[4] = pos->b; diff --git a/src/emc/kinematics/rotarydeltakins.c b/src/emc/kinematics/rotarydeltakins.c index 92e8a763a99..6ce42abf0f0 100644 --- a/src/emc/kinematics/rotarydeltakins.c +++ b/src/emc/kinematics/rotarydeltakins.c @@ -20,73 +20,88 @@ #include #include #include +#include #include "rotarydeltakins-common.h" -static struct haldata -{ - hal_real_t pfr; - hal_real_t tl; - hal_real_t sl; - hal_real_t fr; -} *haldata; +// the four lengths, one pin each +static const kins_param_desc rd_params[] = { + { "platformradius", KINS_PARAM_FLOAT, KINS_IN, 0, RDELTA_PFR }, + { "thighlength", KINS_PARAM_FLOAT, KINS_IN, 0, RDELTA_TL }, + { "shinlength", KINS_PARAM_FLOAT, KINS_IN, 0, RDELTA_SL }, + { "footradius", KINS_PARAM_FLOAT, KINS_IN, 0, RDELTA_FR }, +}; +enum { P_PFR, P_TL, P_SL, P_FR }; static int comp_id; -int kinematicsForward(const double * joints, +static void geometry_of(const kins_params *p, rotarydelta_geometry *g) +{ + rotarydelta_set_geometry(g, p->geometry[P_PFR], p->geometry[P_TL], + p->geometry[P_SL], p->geometry[P_FR]); +} + +static int rd_forward(const kins_params *p, kins_scratch *s, + const double * joints, EmcPose * pos, const KINEMATICS_FORWARD_FLAGS * fflags, KINEMATICS_INVERSE_FLAGS * iflags) { + rotarydelta_geometry g; + (void)s; (void)fflags; (void)iflags; - set_geometry(hal_get_real(haldata->pfr), hal_get_real(haldata->tl), hal_get_real(haldata->sl), hal_get_real(haldata->fr)); - return kinematics_forward(joints, pos); + geometry_of(p, &g); + return rotarydelta_forward(&g, joints, pos); } -int kinematicsInverse(const EmcPose *pos, double *joints, - const KINEMATICS_INVERSE_FLAGS *iflags, - KINEMATICS_FORWARD_FLAGS *fflags) { +static int rd_inverse(const kins_params *p, kins_scratch *s, + const EmcPose *pos, double *joints, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags) { + rotarydelta_geometry g; + (void)s; (void)iflags; (void)fflags; - set_geometry(hal_get_real(haldata->pfr), hal_get_real(haldata->tl), hal_get_real(haldata->sl), hal_get_real(haldata->fr)); - return kinematics_inverse(pos, joints); + geometry_of(p, &g); + return rotarydelta_inverse(&g, pos, joints); } -int kinematicsJacobian(const double *joints, +static int rd_jacobian(const kins_params *p, const double *joints, const EmcPose *pos, double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], const KINEMATICS_INVERSE_FLAGS *iflags) { + rotarydelta_geometry g; int i, j; (void)iflags; - set_geometry(hal_get_real(haldata->pfr), hal_get_real(haldata->tl), hal_get_real(haldata->sl), hal_get_real(haldata->fr)); + geometry_of(p, &g); memset(jac, 0, EMCMOT_MAX_JOINTS * EMCMOT_MAX_AXIS * sizeof(jac[0][0])); // The foot stays a shin length from each knee, so along a leg the // motion of the foot and the motion of the knee agree: // (P - K) . dP = (P - K) . dK/dq dq - // K is the knee less the foot offset, written as kinematics_forward() + // K is the knee less the foot offset, written as rotarydelta_forward() // writes it, and q the hip angle that swings it. for (i = 0; i < 3; i++) { double q = D2R(joints[i]); - double reach = platformradius - footradius + thighlength * cos(q); + double reach = g.platformradius - g.footradius + g.thighlength * cos(q); double kx, ky, kz, dkx, dky, dkz, px, py, pz, denom; switch (i) { case 0: kx = 0; ky = -reach; - dkx = 0; dky = thighlength * sin(q); + dkx = 0; dky = g.thighlength * sin(q); break; case 1: kx = reach * 0.5 * sqrt(3); ky = reach * 0.5; - dkx = -thighlength * sin(q) * 0.5 * sqrt(3); - dky = -thighlength * sin(q) * 0.5; + dkx = -g.thighlength * sin(q) * 0.5 * sqrt(3); + dky = -g.thighlength * sin(q) * 0.5; break; default: kx = -reach * 0.5 * sqrt(3); ky = reach * 0.5; - dkx = thighlength * sin(q) * 0.5 * sqrt(3); - dky = -thighlength * sin(q) * 0.5; + dkx = g.thighlength * sin(q) * 0.5 * sqrt(3); + dky = -g.thighlength * sin(q) * 0.5; break; } - kz = -thighlength * sin(q); - dkz = -thighlength * cos(q); + kz = -g.thighlength * sin(q); + dkz = -g.thighlength * cos(q); px = pos->tran.x - kx; py = pos->tran.y - ky; pz = pos->tran.z - kz; @@ -102,36 +117,38 @@ int kinematicsJacobian(const double *joints, return 0; } -KINEMATICS_TYPE kinematicsType() -{ - return KINEMATICS_BOTH; -} +static const kins_ops rd_ops = { + .forward = rd_forward, + .inverse = rd_inverse, + .jacobian = rd_jacobian, +}; + +// three hips for the three linear coordinates, the rest passed through; +// the entry points come from kins_single.c +const kins_module_info kins_module = { + .name = "rotarydeltakins", + .halprefix = "rotarydeltakins", + .params = rd_params, + .nparams = sizeof(rd_params)/sizeof(rd_params[0]), + .required_coordinates = "XYZABCUVW", + .max_joints = 9, + .allow_duplicates = 0, + .ntypes = 1, + .ops = { &rd_ops }, +}; int rtapi_app_main(void) { - int retval; - comp_id = hal_init("rotarydeltakins"); if(comp_id < 0) return comp_id; - haldata = hal_malloc(sizeof(*haldata)); - if(!haldata) { retval = -ENOMEM; goto error; } - - if((retval = hal_pin_new_real(comp_id, HAL_IN, &haldata->pfr, RDELTA_PFR, "rotarydeltakins.platformradius")) < 0) - goto error; - if((retval = hal_pin_new_real(comp_id, HAL_IN, &haldata->tl, RDELTA_TL, "rotarydeltakins.thighlength")) < 0) - goto error; - if((retval = hal_pin_new_real(comp_id, HAL_IN, &haldata->sl, RDELTA_SL, "rotarydeltakins.shinlength")) < 0) - goto error; - if((retval = hal_pin_new_real(comp_id, HAL_IN, &haldata->fr, RDELTA_FR, "rotarydeltakins.footradius")) < 0) - goto error; + if (kinsSingleInit(comp_id, &kins_module, "XYZABCUVW", KINEMATICS_BOTH)) { + hal_exit(comp_id); + return -1; + } hal_ready(comp_id); return 0; - -error: - hal_exit(comp_id); - return retval; } void rtapi_app_exit(void) @@ -139,9 +156,4 @@ void rtapi_app_exit(void) hal_exit(comp_id); } -KINS_NOT_SWITCHABLE -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsForward); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsJacobian); MODULE_LICENSE("GPL"); diff --git a/src/emc/kinematics/rotarydeltakins.cc b/src/emc/kinematics/rotarydeltakins.cc index 49a26b3153e..a8227573ab9 100644 --- a/src/emc/kinematics/rotarydeltakins.cc +++ b/src/emc/kinematics/rotarydeltakins.cc @@ -20,11 +20,19 @@ #include using namespace boost::python; +// the python module keeps one geometry, set from python +static rotarydelta_geometry geometry; + +static void set_geometry(double pfr, double tl, double sl, double fr) +{ + rotarydelta_set_geometry(&geometry, pfr, tl, sl, fr); +} + static object forward(double j0, double j1, double j2) { double joints[9] = {j0, j1, j2}; EmcPose pos; - int result = kinematics_forward(joints, &pos); + int result = rotarydelta_forward(&geometry, joints, &pos); if(result == 0) return make_tuple(pos.tran.x, pos.tran.y, pos.tran.z); return object(); @@ -34,7 +42,7 @@ static object inverse(double x, double y, double z) { double joints[9]; EmcPose pos = {{x,y,z}, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; - int result = kinematics_inverse(&pos, joints); + int result = rotarydelta_inverse(&geometry, &pos, joints); if(result == 0) return make_tuple(joints[0], joints[1], joints[2]); return object(); @@ -42,7 +50,8 @@ static object inverse(double x, double y, double z) static object get_geometry() { - return make_tuple(platformradius, thighlength, shinlength, footradius); + return make_tuple(geometry.platformradius, geometry.thighlength, + geometry.shinlength, geometry.footradius); } #pragma GCC diagnostic push diff --git a/src/emc/kinematics/rotatekins.c b/src/emc/kinematics/rotatekins.c index 47f95bee2cd..52e198cf539 100644 --- a/src/emc/kinematics/rotatekins.c +++ b/src/emc/kinematics/rotatekins.c @@ -7,7 +7,7 @@ * Author: Chris Radek * License: GPL Version 2 * System: Linux -* +* * Copyright (c) 2006 All rights reserved. * ********************************************************************/ @@ -18,12 +18,16 @@ #include #include #include /* these decls */ +#include -int kinematicsForward(const double *joints, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +static int rotate_forward(const kins_params *p, kins_scratch *s, + const double *joints, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)p; + (void)s; (void)fflags; (void)iflags; double c_rad = -joints[5]*M_PI/180; @@ -40,11 +44,14 @@ int kinematicsForward(const double *joints, return 0; } -int kinematicsInverse(const EmcPose * pos, - double *joints, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int rotate_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, + double *joints, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { + (void)p; + (void)s; (void)iflags; (void)fflags; double c_rad = pos->c*M_PI/180; @@ -61,14 +68,15 @@ int kinematicsInverse(const EmcPose * pos, return 0; } -int kinematicsJacobian(const double *joints, - const EmcPose *pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS *iflags) +static int rotate_jacobian(const kins_params *p, const double *joints, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) { double c_rad = pos->c*M_PI/180; double cc = cos(c_rad), sc = sin(c_rad); int j; + (void)p; (void)joints; (void)iflags; memset(jac, 0, EMCMOT_MAX_JOINTS * EMCMOT_MAX_AXIS * sizeof(jac[0][0])); @@ -82,38 +90,40 @@ int kinematicsJacobian(const double *joints, return 0; } -/* implemented for these kinematics as giving joints preference */ -int kinematicsHome(EmcPose * world, - double *joint, - KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) -{ - *fflags = 0; - *iflags = 0; +static const kins_ops rotate_ops = { + .forward = rotate_forward, + .inverse = rotate_inverse, + .jacobian = rotate_jacobian, +}; - return kinematicsForward(joint, world, fflags, iflags); -} +// no geometry; joints 0..8 are the nine letters in order, and the entry +// points come from kins_single.c +const kins_module_info kins_module = { + .name = "rotatekins", + .halprefix = "rotatekins", + .params = NULL, + .nparams = 0, + .required_coordinates = "XYZABCUVW", + .max_joints = 9, + .allow_duplicates = 0, + .ntypes = 1, + .ops = { &rotate_ops }, +}; -KINEMATICS_TYPE kinematicsType() -{ - return KINEMATICS_BOTH; -} - -KINS_NOT_SWITCHABLE -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsForward); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsJacobian); MODULE_LICENSE("GPL"); int comp_id; int rtapi_app_main(void) { comp_id = hal_init("rotatekins"); - if(comp_id > 0) { - hal_ready(comp_id); - return 0; + if(comp_id < 0) return comp_id; + + if (kinsSingleInit(comp_id, &kins_module, "XYZABCUVW", KINEMATICS_BOTH)) { + hal_exit(comp_id); + return -1; } - return comp_id; + + hal_ready(comp_id); + return 0; } void rtapi_app_exit(void) { hal_exit(comp_id); } diff --git a/src/emc/kinematics/scorbot-kins.c b/src/emc/kinematics/scorbot-kins.c index 828f7b4ec43..2adcdeb29f2 100644 --- a/src/emc/kinematics/scorbot-kins.c +++ b/src/emc/kinematics/scorbot-kins.c @@ -44,6 +44,7 @@ #include #include #include +#include // @@ -76,12 +77,15 @@ static void compute_j1_cartesian_location(double j0, EmcPose *j1_cart) { // Forward kinematics takes the joint positions and computes the cartesian // coordinates of the controlled point. -int kinematicsForward( +static int scorbot_forward( + const kins_params *p, kins_scratch *s, const double *joints, EmcPose *pose, const KINEMATICS_FORWARD_FLAGS *fflags, KINEMATICS_INVERSE_FLAGS *iflags ) { + (void)p; + (void)s; (void)fflags; (void)iflags; EmcPose j1_vector; // the vector from j0 ("base") to joint 1 ("shoulder", end of link 0) @@ -90,16 +94,13 @@ int kinematicsForward( double r; - // rtapi_print("fwd: j0=%f, j1=%f, j2=%f\n", joints[0], joints[1], joints[2]); compute_j1_cartesian_location(joints[0], &j1_vector); - // rtapi_print("fwd: j1=(%f, %f, %f)\n", j1_vector.tran.x, j1_vector.tran.y, j1_vector.tran.z); // Link 1 connects j1 (shoulder) to j2 (elbow). r = L1_LENGTH * cos(TO_RAD * joints[1]); j2_vector.tran.x = r * cos(TO_RAD * joints[0]); j2_vector.tran.y = r * sin(TO_RAD * joints[0]); j2_vector.tran.z = L1_LENGTH * sin(TO_RAD * joints[1]); - // rtapi_print("fwd: j2=(%f, %f, %f)\n", j2_vector.tran.x, j2_vector.tran.y, j2_vector.tran.z); // Link 2 connects j2 (elbow) to j3 (wrist). // J3 is the controlled point. @@ -107,13 +108,11 @@ int kinematicsForward( j3_vector.tran.x = r * cos(TO_RAD * joints[0]); j3_vector.tran.y = r * sin(TO_RAD * joints[0]); j3_vector.tran.z = L2_LENGTH * sin(TO_RAD * joints[2]); - // rtapi_print("fwd: j3=(%f, %f, %f)\n", j3_vector.tran.x, j3_vector.tran.y, j3_vector.tran.z); // The end-effector location is the sum of the linkage vectors. pose->tran.x = j1_vector.tran.x + j2_vector.tran.x + j3_vector.tran.x; pose->tran.y = j1_vector.tran.y + j2_vector.tran.y + j3_vector.tran.y; pose->tran.z = j1_vector.tran.z + j2_vector.tran.z + j3_vector.tran.z; - // rtapi_print("fwd: pose=(%f, %f, %f)\n", pose->tran.x, pose->tran.y, pose->tran.z); // A and B are wrist roll and pitch, handled in hal by external kinematics pose->a = joints[3]; @@ -135,15 +134,17 @@ int kinematicsForward( // is the horizontal distance (ie, in the XY plane) of the controlled // point from J0. // -int kinematicsInverse( +static int scorbot_inverse( + const kins_params *p, kins_scratch *s, const EmcPose *pose, double *joints, const KINEMATICS_INVERSE_FLAGS *iflags, KINEMATICS_FORWARD_FLAGS *fflags ) { + (void)p; + (void)s; (void)iflags; (void)fflags; - // EmcPose j1_cart; double distance_to_cp, distance_to_center; double r_j1, z_j1; // (r_j1, z_j1) is the location of J1 in the RZ plane double r_cp, z_cp; // (r_cp, z_cp) is the location of the controlled point in the RZ plane @@ -153,16 +154,10 @@ int kinematicsInverse( // the location of J2, this is what we're trying to find double z_j2; - // rtapi_print("inv: x=%f, y=%f, z=%f\n", pose->tran.x, pose->tran.y, pose->tran.z); - // J0 is easy. Project the (X, Y, Z) of the pose onto the Z=0 plane. // J0 points at the projected (X, Y) point. tan(J0) = Y/X // J0 then defines the plane that the rest of the arm operates in. joints[0] = TO_DEG * atan2(pose->tran.y, pose->tran.x); - // rtapi_print("inv: j0=%f\n", joints[0]); - - // compute_j1_cartesian_location(joints[0], &j1_cart); - // rtapi_print("inv: j1=(X=%f, Y=%f, Z=%f)\n", j1_cart.tran.x, j1_cart.tran.y, j1_cart.tran.z); // FIXME: Until i figure the wrist differential out, the controlled // point will be the location of the wrist joint, J3/J4. @@ -176,19 +171,16 @@ int kinematicsInverse( // of J0. This is just a known, static vector. r_j1 = L0_HORIZONTAL_DISTANCE; z_j1 = L0_VERTICAL_DISTANCE; - // rtapi_print("inv: r_j1=%f, z_j1=%f\n", r_j1, z_j1); // (r_cp, z_cp) is the location of J3 (the controlled point), again in // the plane defined by the angle of J0, with the origin of the // machine. r_cp = sqrt(pow(pose->tran.x, 2) + pow(pose->tran.y, 2)); z_cp = pose->tran.z; - // rtapi_print("inv: r_cp=%f, z_cp=%f (controlled point)\n", r_cp, z_cp); // translate so (r_j1, z_j1) is the origin of the coordinate system r_cp -= r_j1; z_cp -= z_j1; - // rtapi_print("inv: r_cp=%f, z_cp=%f (translated controlled point)\n", r_cp, z_cp); // // Now the origin (aka J1), J2, and CP define a triangle in the RZ plane. @@ -207,86 +199,23 @@ int kinematicsInverse( distance_to_cp = sqrt(pow(r_cp, 2) + pow(z_cp, 2)); distance_to_center = distance_to_cp / 2; - // rtapi_print("inv: distance to cp: %f\n", distance_to_cp); // find the angle of the vector from the origin to the CP angle_to_cp = TO_DEG * acos(r_cp / distance_to_cp); if (z_cp < 0) { angle_to_cp *= -1; } - // rtapi_print("inv: angle to cp: %f\n", angle_to_cp); // find the angle (Center, J1, J2) j1_angle = TO_DEG * acos(distance_to_center / L1_LENGTH); - // rtapi_print("inv: j1 angle: %f\n", j1_angle); joints[1] = angle_to_cp + j1_angle; - // rtapi_print("inv: j1: %f\n", joints[1]); // now we can compute the location of J2 z_j2 = L1_LENGTH * sin(TO_RAD * joints[1]); - // rtapi_print("inv: r_j2=%f, z_j2=%f (translated j2)\n", r_j2, z_j2); joints[2] = -1.0 * TO_DEG * asin((z_j2 - z_cp) / L2_LENGTH); - -#if 0 - // Distance between controlled point and the location of j1. These two - // points are separated by link 1, joint 1, and link 2. - distance_between_centers = sqrt(pow((r2 - r1), 2) + pow((z2 - z1), 2)); - - if (distance_between_centers > (L1_LENGTH + L2_LENGTH)) { - // trying to reach too far - return GO_RESULT_RANGE_ERROR; - } - - if (distance_between_centers < fabs(L1_LENGTH - L2_LENGTH)) { - // trying to reach too far into armpit - return GO_RESULT_RANGE_ERROR; - } - - delta = (1.0 / 4.0) * sqrt((distance_between_centers + L1_LENGTH + L2_LENGTH) * (distance_between_centers + L1_LENGTH - L2_LENGTH) * (distance_between_centers - L1_LENGTH + L2_LENGTH) * (L1_LENGTH + L2_LENGTH - distance_between_centers)); - - ir1 = ((r1 + r2) / 2) + (((r2 - r1) * (pow(L1_LENGTH, 2) - pow(L2_LENGTH, 2)))/(2 * pow(distance_between_centers, 2))) + ((2 * (z1 - z2) * delta) / pow(distance_between_centers, 2)); - ir2 = ((r1 + r2) / 2) + (((r2 - r1) * (pow(L1_LENGTH, 2) - pow(L2_LENGTH, 2)))/(2 * pow(distance_between_centers, 2))) - ((2 * (z1 - z2) * delta) / pow(distance_between_centers, 2)); - - iz1 = ((z1 + z2) / 2) + (((z2 - z1) * (pow(L1_LENGTH, 2) - pow(L2_LENGTH, 2)))/(2 * pow(distance_between_centers, 2))) - ((2 * (r1 - r2) * delta) / pow(distance_between_centers, 2)); - iz2 = ((z1 + z2) / 2) + (((z2 - z1) * (pow(L1_LENGTH, 2) - pow(L2_LENGTH, 2)))/(2 * pow(distance_between_centers, 2))) + ((2 * (r1 - r2) * delta) / pow(distance_between_centers, 2)); - - - // (ir1, iz1) is one intersection point, (ir2, iz2) is the other. - // These are the possible locations of the J2 joint. - // FIXME: For now we arbitrarily pick the one with the bigger Z. - - if (iz1 > iz2) { - j2_r = ir1; - j2_z = iz1; - } else { - j2_r = ir2; - j2_z = iz2; - } - // rtapi_print("inv: j2_r=%f, j2_z=%f (J2, intersection point)\n", j2_r, j2_z); - - // Make J1 point at J2 (j2_r, j2_z). - { - double l1_r = j2_r - r1; - joints[1] = TO_DEG * acos(l1_r / L1_LENGTH); - // rtapi_print("inv: l1_r=%f, j1=%f\n", l1_r, joints[1]); - } - - // Make J2 point at the controlled point. - { - double l2_r = r2 - j2_r; - double j2; - j2 = TO_DEG * acos(l2_r / L2_LENGTH); - if (j2_z > pose->tran.z) { - j2 *= -1; - } - joints[2] = j2; - // rtapi_print("inv: l2_r=%f, j2=%f\n", l2_r, joints[2]); - } -#endif - // A and B are wrist roll and pitch, handled in hal by external kinematics joints[3] = pose->a; joints[4] = pose->b; @@ -295,13 +224,14 @@ int kinematicsInverse( } -int kinematicsJacobian( +static int scorbot_jacobian( + const kins_params *p, const double *joints, const EmcPose *pose, double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], const KINEMATICS_INVERSE_FLAGS *iflags ) { - // kinematicsInverse() above, differentiated step by step in the same + // scorbot_inverse() above, differentiated step by step in the same // order, each quantity carried as its gradient over (x, y, z) const double x = pose->tran.x, y = pose->tran.y; const double rho2 = x*x + y*y; @@ -311,6 +241,7 @@ int kinematicsJacobian( double q; int i; + (void)p; (void)joints; (void)iflags; if (rho2 <= 0) { return -1; } @@ -363,15 +294,26 @@ int kinematicsJacobian( return 0; } -KINEMATICS_TYPE kinematicsType(void) { - return KINEMATICS_BOTH; -} +static const kins_ops scorbot_ops = { + .forward = scorbot_forward, + .inverse = scorbot_inverse, + .jacobian = scorbot_jacobian, +}; + +// the arm's dimensions are the constants above; no geometry pins. The +// entry points come from kins_single.c +const kins_module_info kins_module = { + .name = "scorbot-kins", + .halprefix = "scorbot-kins", + .params = NULL, + .nparams = 0, + .required_coordinates = "XYZAB", + .max_joints = 5, + .allow_duplicates = 0, + .ntypes = 1, + .ops = { &scorbot_ops }, +}; -KINS_NOT_SWITCHABLE -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsForward); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsJacobian); MODULE_LICENSE("GPL"); static int comp_id; @@ -381,6 +323,10 @@ int rtapi_app_main(void) { if (comp_id < 0) { return comp_id; } + if (kinsSingleInit(comp_id, &kins_module, "XYZAB", KINEMATICS_BOTH)) { + hal_exit(comp_id); + return -1; + } hal_ready(comp_id); return 0; } @@ -388,4 +334,3 @@ int rtapi_app_main(void) { void rtapi_app_exit(void) { hal_exit(comp_id); } - diff --git a/src/emc/kinematics/tripodkins.c b/src/emc/kinematics/tripodkins.c index ab5c7081b19..8b482fdeb4d 100644 --- a/src/emc/kinematics/tripodkins.c +++ b/src/emc/kinematics/tripodkins.c @@ -4,10 +4,10 @@ * * Derived from a work by Fred Proctor * -* Author: +* Author: * License: GPL Version 2 * System: Linux -* +* * Copyright (c) 2004 All rights reserved. * * Last change: @@ -68,19 +68,15 @@ #include #include #include /* these decls */ +#include -/* ident tag */ -#ifndef __GNUC__ -#ifndef __attribute__ -#define __attribute__(x) -#endif -#endif - -static struct haldata { - hal_real_t bx; - hal_real_t cx; - hal_real_t cy; -} *haldata = NULL; +// the base geometry, one pin each, poked from HAL as before +static const kins_param_desc tripod_params[] = { + { "Bx", KINS_PARAM_FLOAT, KINS_IO, 0, 1.0 }, + { "Cx", KINS_PARAM_FLOAT, KINS_IO, 0, 1.0 }, + { "Cy", KINS_PARAM_FLOAT, KINS_IO, 0, 1.0 }, +}; +enum { P_BX, P_CX, P_CY }; #define sq(x) ((x)*(x)) @@ -124,11 +120,13 @@ static struct haldata { solutions. Positive means the tripod is above the xy plane, negative means below. */ -int kinematicsForward(const double * joints, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +static int tripod_forward(const kins_params *p, kins_scratch *s_, + const double * joints, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)s_; (void)iflags; #define AD (joints[0]) #define BD (joints[1]) @@ -138,9 +136,9 @@ int kinematicsForward(const double * joints, #define Dz (pos->tran.z) double P, Q, R; double s, t, u; - rtapi_real Bx = hal_get_real(haldata->bx); - rtapi_real Cx = hal_get_real(haldata->cx); - rtapi_real Cy = hal_get_real(haldata->cy); + const double Bx = p->geometry[P_BX]; + const double Cx = p->geometry[P_CX]; + const double Cy = p->geometry[P_CY]; P = sq(AD); Q = sq(BD) - sq(Bx); @@ -184,11 +182,13 @@ int kinematicsForward(const double * joints, #undef Dz } -int kinematicsInverse(const EmcPose * pos, - double * joints, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int tripod_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, + double * joints, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { + (void)s; (void)iflags; #define AD (joints[0]) #define BD (joints[1]) @@ -196,9 +196,9 @@ int kinematicsInverse(const EmcPose * pos, #define Dx (pos->tran.x) #define Dy (pos->tran.y) #define Dz (pos->tran.z) - rtapi_real Bx = hal_get_real(haldata->bx); - rtapi_real Cx = hal_get_real(haldata->cx); - rtapi_real Cy = hal_get_real(haldata->cy); + const double Bx = p->geometry[P_BX]; + const double Cx = p->geometry[P_CX]; + const double Cy = p->geometry[P_CY]; AD = sqrt(sq(Dx) + sq(Dy) + sq(Dz)); BD = sqrt(sq(Dx - Bx) + sq(Dy) + sq(Dz)); @@ -219,14 +219,14 @@ int kinematicsInverse(const EmcPose * pos, #undef Dz } -int kinematicsJacobian(const double * joints, - const EmcPose * pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS * iflags) +static int tripod_jacobian(const kins_params *p, const double * joints, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) { - rtapi_real Bx = hal_get_real(haldata->bx); - rtapi_real Cx = hal_get_real(haldata->cx); - rtapi_real Cy = hal_get_real(haldata->cy); + const double Bx = p->geometry[P_BX]; + const double Cx = p->geometry[P_CX]; + const double Cy = p->geometry[P_CY]; /* the three strut base points, in the order of the joints */ const double base[3][2] = { {0, 0}, {Bx, 0}, {Cx, Cy} }; int i; @@ -248,170 +248,40 @@ int kinematicsJacobian(const double * joints, return 0; } -KINEMATICS_TYPE kinematicsType() -{ - return KINEMATICS_BOTH; -} - -#ifdef MAIN - -#include -#include - -/* - Interactive testing of kins. - - Syntax: a.out -*/ -int main(int argc, char *argv[]) -{ -#ifndef BUFFERLEN -#define BUFFERLEN 256 -#endif - char buffer[BUFFERLEN]; - char cmd[BUFFERLEN]; - EmcPose pos, vel; - double joints[3]={0.0,0.0,0.0}, jointvels[3]={0.0,0.0,0.0}; - char inverse; - char flags; - KINEMATICS_FORWARD_FLAGS fflags; - - inverse = 0; /* forwards, by default */ - flags = 0; /* didn't provide flags */ - fflags = 0; /* above xy plane, by default */ - if (argc != 4 || - 1 != sscanf(argv[1], "%lf", &Bx) || - 1 != sscanf(argv[2], "%lf", &Cx) || - 1 != sscanf(argv[3], "%lf", &Cy)) { - fprintf(stderr, "syntax: %s Bx Cx Cy\n", argv[0]); - return 1; - } - - while (! feof(stdin)) { - if (inverse) { - printf("inv> "); - } - else { - printf("fwd> "); - } - fflush(stdout); - - if (NULL == fgets(buffer, BUFFERLEN, stdin)) { - break; - } - if (1 != sscanf(buffer, "%255s", cmd)) { - continue; - } - - if (! strcmp(cmd, "quit")) { - break; - } - if (! strcmp(cmd, "i")) { - inverse = 1; - continue; - } - if (! strcmp(cmd, "f")) { - inverse = 0; - continue; - } - if (! strcmp(cmd, "ff")) { - if (1 != sscanf(buffer, "%*s %lu", &fflags)) { - printf("need forward flag\n"); - } - continue; - } - - if (inverse) { /* inverse kins */ - if (3 != sscanf(buffer, "%lf %lf %lf", - &pos.tran.x, - &pos.tran.y, - &pos.tran.z)) { - printf("need X Y Z\n"); - continue; - } - if (0 != kinematicsInverse(&pos, joints, NULL, &fflags)) { - printf("inverse kin error\n"); - } - else { - printf("%f\t%f\t%f\n", joints[0], joints[1], joints[2]); - if (0 != kinematicsForward(joints, &pos, &fflags, NULL)) { - printf("forward kin error\n"); - } - else { - printf("%f\t%f\t%f\n", pos.tran.x, pos.tran.y, pos.tran.z); - } - } - } - else { /* forward kins */ - if (flags) { - if (4 != sscanf(buffer, "%lf %lf %lf %lu", - &joints[0], - &joints[1], - &joints[2], - &fflags)) { - printf("need 3 strut values and flag\n"); - continue; - } - } - else { - if (3 != sscanf(buffer, "%lf %lf %lf", - &joints[0], - &joints[1], - &joints[2])) { - printf("need 3 strut values\n"); - continue; - } - } - if (0 != kinematicsForward(joints, &pos, &fflags, NULL)) { - printf("forward kin error\n"); - } - else { - printf("%f\t%f\t%f\n", pos.tran.x, pos.tran.y, pos.tran.z); - if (0 != kinematicsInverse(&pos, joints, NULL, &fflags)) { - printf("inverse kin error\n"); - } - else { - printf("%f\t%f\t%f\n", joints[0], joints[1], joints[2]); - } - } - } - } /* end while (! feof(stdin)) */ - - return 0; -} - -#endif /* MAIN */ - -KINS_NOT_SWITCHABLE -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsForward); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsJacobian); +static const kins_ops tripod_ops = { + .forward = tripod_forward, + .inverse = tripod_inverse, + .jacobian = tripod_jacobian, +}; + +// three struts for three coordinates; the entry points come from +// kins_single.c +const kins_module_info kins_module = { + .name = "tripodkins", + .halprefix = "tripodkins", + .params = tripod_params, + .nparams = sizeof(tripod_params)/sizeof(tripod_params[0]), + .required_coordinates = "XYZ", + .max_joints = 3, + .allow_duplicates = 0, + .ntypes = 1, + .ops = { &tripod_ops }, +}; MODULE_LICENSE("GPL"); - - static int comp_id; int rtapi_app_main(void) { - int res = 0; - comp_id = hal_init("tripodkins"); if(comp_id < 0) return comp_id; - haldata = hal_malloc(sizeof(struct haldata)); - if(!haldata) goto error; - - if((res = hal_pin_new_real(comp_id, HAL_IO, &(haldata->bx), 1.0, "tripodkins.Bx")) < 0) goto error; - if((res = hal_pin_new_real(comp_id, HAL_IO, &(haldata->cx), 1.0, "tripodkins.Cx")) < 0) goto error; - if((res = hal_pin_new_real(comp_id, HAL_IO, &(haldata->cy), 1.0, "tripodkins.Cy")) < 0) goto error; + if (kinsSingleInit(comp_id, &kins_module, "XYZ", KINEMATICS_BOTH)) { + hal_exit(comp_id); + return -1; + } hal_ready(comp_id); return 0; - -error: - hal_exit(comp_id); - return res; } void rtapi_app_exit(void) { hal_exit(comp_id); } diff --git a/src/hal/components/Submakefile b/src/hal/components/Submakefile index 759fc5bd483..8aae8597402 100644 --- a/src/hal/components/Submakefile +++ b/src/hal/components/Submakefile @@ -80,6 +80,8 @@ obj-m += $(patsubst hal/drivers/%.comp, %.o, $(patsubst hal/components/%.comp, % # -extra-objs. The list is expanded when the .mak is written, # so it has to be defined in this file (which the .mak depends on). SWITCHKINS_OBJS := emc/kinematics/switchkins.o emc/kinematics/kins_util.o +matrixkins-extra-objs := emc/kinematics/kins_util.o emc/kinematics/kins_single.o emc/kinematics/kins_single_module.o +userkins-extra-objs := emc/kinematics/kins_util.o emc/kinematics/kins_single.o millturn-extra-objs := $(SWITCHKINS_OBJS) xyzab_tdr_kins-extra-objs := $(SWITCHKINS_OBJS) xyzacb_trsrn-extra-objs := $(SWITCHKINS_OBJS) diff --git a/src/hal/components/matrixkins.comp b/src/hal/components/matrixkins.comp index 8bf76899c8e..664a41d6f07 100644 --- a/src/hal/components/matrixkins.comp +++ b/src/hal/components/matrixkins.comp @@ -40,7 +40,7 @@ mechanical issues, including: 3. Parallelism between spindle rotational axis and Z movement. 4. Perpendicularity between spindle rotational axis and X/Y movement. -The matrix coefficients are set by parameters C_xx .. C_zz. +The matrix coefficients are set by the pins C_xx .. C_zz. For 3 axis machine, the equations become: .... @@ -152,7 +152,7 @@ Specify matrixkins in LinuxCNC INI file as: KINEMATICS=matrixkins ---- -In your HAL configuration file, set the parameters C_xx .. C_zz: +In your HAL configuration file, set the pins C_xx .. C_zz: [source,hal] ---- @@ -167,7 +167,7 @@ setp matrixkins.C_zy 0 # Skew Y axis towards Z axis setp matrixkins.C_zz 1 # Z axis scale ---- -The parameters can be modified during runtime using halcmd. +The pins can be modified during runtime using halcmd. To avoid sudden movements, it is better to turn off machine power before changes. If recalibration is performed with already existing calibration being in effect, @@ -180,68 +180,30 @@ option extra_setup; license "GPL"; ;; -static struct haldata { - hal_real_t C_xx; - hal_real_t C_xy; - hal_real_t C_xz; - hal_real_t C_yx; - hal_real_t C_yy; - hal_real_t C_yz; - hal_real_t C_zx; - hal_real_t C_zy; - hal_real_t C_zz; -} *haldata; - -EXTRA_SETUP() { - (void)__comp_inst; - (void)prefix; - (void)extra_arg; - int res=0; - - // inherit comp_id from rtapi_main() - if (comp_id < 0) goto error; - - haldata = hal_malloc(sizeof(struct haldata)); - if (!haldata) goto error; - - res |= hal_param_new_real(comp_id, HAL_RW, &haldata->C_xx, 1.0, "matrixkins.C_xx"); - res |= hal_param_new_real(comp_id, HAL_RW, &haldata->C_xy, 0.0, "matrixkins.C_xy"); - res |= hal_param_new_real(comp_id, HAL_RW, &haldata->C_xz, 0.0, "matrixkins.C_xz"); - res |= hal_param_new_real(comp_id, HAL_RW, &haldata->C_yx, 0.0, "matrixkins.C_yx"); - res |= hal_param_new_real(comp_id, HAL_RW, &haldata->C_yy, 1.0, "matrixkins.C_yy"); - res |= hal_param_new_real(comp_id, HAL_RW, &haldata->C_yz, 0.0, "matrixkins.C_yz"); - res |= hal_param_new_real(comp_id, HAL_RW, &haldata->C_zx, 0.0, "matrixkins.C_zx"); - res |= hal_param_new_real(comp_id, HAL_RW, &haldata->C_zy, 0.0, "matrixkins.C_zy"); - res |= hal_param_new_real(comp_id, HAL_RW, &haldata->C_zz, 1.0, "matrixkins.C_zz"); - - if (res) goto error; - - rtapi_print("*** %s setup ok\n",__FILE__); - return 0; -error: - rtapi_print("\n!!! %s setup failed res=%d\n\n",__FILE__,res); - return -1; -} - #include -#include - -KINS_NOT_SWITCHABLE -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsJacobian); -EXPORT_SYMBOL(kinematicsForward); - -KINEMATICS_TYPE kinematicsType() -{ - return KINEMATICS_BOTH; -} - -int kinematicsForward(const double *j, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +#include + +// the calibration matrix, one pin each; the maths reads it from the block +static const kins_param_desc matrix_params[] = { + { "C_xx", KINS_PARAM_FLOAT, KINS_IN, 0, 1.0 }, + { "C_xy", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "C_xz", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "C_yx", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "C_yy", KINS_PARAM_FLOAT, KINS_IN, 0, 1.0 }, + { "C_yz", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "C_zx", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "C_zy", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "C_zz", KINS_PARAM_FLOAT, KINS_IN, 0, 1.0 }, +}; +enum { C_XX, C_XY, C_XZ, C_YX, C_YY, C_YZ, C_ZX, C_ZY, C_ZZ }; + +static int matrix_forward(const kins_params *p, kins_scratch *s, + const double *j, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)s; (void)fflags; (void)iflags; // For forward kinematics (joint to axis position) we @@ -251,20 +213,20 @@ int kinematicsForward(const double *j, // https://ardoris.wordpress.com/2008/07/18/general-formula-for-the-inverse-of-a-3x3-matrix/ // https://en.wikipedia.org/wiki/Invertible_matrix#Inversion_of_3_%C3%97_3_matrices - rtapi_real a = hal_get_real(haldata->C_xx); - rtapi_real b = hal_get_real(haldata->C_xy); - rtapi_real c = hal_get_real(haldata->C_xz); - rtapi_real d = hal_get_real(haldata->C_yx); - rtapi_real e = hal_get_real(haldata->C_yy); - rtapi_real f = hal_get_real(haldata->C_yz); - rtapi_real g = hal_get_real(haldata->C_zx); - rtapi_real h = hal_get_real(haldata->C_zy); - rtapi_real i = hal_get_real(haldata->C_zz); - - rtapi_real det = a * (e * i - f * h) - - b * (d * i - f * g) - + c * (d * h - e * g); - rtapi_real invdet = 1.0 / det; + const double a = p->geometry[C_XX]; + const double b = p->geometry[C_XY]; + const double c = p->geometry[C_XZ]; + const double d = p->geometry[C_YX]; + const double e = p->geometry[C_YY]; + const double f = p->geometry[C_YZ]; + const double g = p->geometry[C_ZX]; + const double h = p->geometry[C_ZY]; + const double i = p->geometry[C_ZZ]; + + const double det = a * (e * i - f * h) + - b * (d * i - f * g) + + c * (d * h - e * g); + const double invdet = 1.0 / det; // Apply inverse matrix transform to the 3 cartesian coordinates pos->tran.x = invdet * ( (e * i - f * h) * j[0] @@ -290,22 +252,24 @@ int kinematicsForward(const double *j, return 0; } -int kinematicsInverse(const EmcPose * pos, - double *j, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int matrix_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, + double *j, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { + (void)s; (void)iflags; (void)fflags; - rtapi_real a = hal_get_real(haldata->C_xx); - rtapi_real b = hal_get_real(haldata->C_xy); - rtapi_real c = hal_get_real(haldata->C_xz); - rtapi_real d = hal_get_real(haldata->C_yx); - rtapi_real e = hal_get_real(haldata->C_yy); - rtapi_real f = hal_get_real(haldata->C_yz); - rtapi_real g = hal_get_real(haldata->C_zx); - rtapi_real h = hal_get_real(haldata->C_zy); - rtapi_real i = hal_get_real(haldata->C_zz); + const double a = p->geometry[C_XX]; + const double b = p->geometry[C_XY]; + const double c = p->geometry[C_XZ]; + const double d = p->geometry[C_YX]; + const double e = p->geometry[C_YY]; + const double f = p->geometry[C_YZ]; + const double g = p->geometry[C_ZX]; + const double h = p->geometry[C_ZY]; + const double i = p->geometry[C_ZZ]; // Apply matrix transform to the 3 cartesian coordinates j[0] = pos->tran.x * a + pos->tran.y * b + pos->tran.z * c; @@ -323,10 +287,10 @@ int kinematicsInverse(const EmcPose * pos, return 0; } -int kinematicsJacobian(const double *j, - const EmcPose * pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS * iflags) +static int matrix_jacobian(const kins_params *p, const double *j, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) { int r, c; (void)j; @@ -337,15 +301,41 @@ int kinematicsJacobian(const double *j, } // the inverse is the calibration matrix itself, so its derivative is // that matrix, and the pass-through axes are ones - jac[0][0] = hal_get_real(haldata->C_xx); - jac[0][1] = hal_get_real(haldata->C_xy); - jac[0][2] = hal_get_real(haldata->C_xz); - jac[1][0] = hal_get_real(haldata->C_yx); - jac[1][1] = hal_get_real(haldata->C_yy); - jac[1][2] = hal_get_real(haldata->C_yz); - jac[2][0] = hal_get_real(haldata->C_zx); - jac[2][1] = hal_get_real(haldata->C_zy); - jac[2][2] = hal_get_real(haldata->C_zz); + jac[0][0] = p->geometry[C_XX]; + jac[0][1] = p->geometry[C_XY]; + jac[0][2] = p->geometry[C_XZ]; + jac[1][0] = p->geometry[C_YX]; + jac[1][1] = p->geometry[C_YY]; + jac[1][2] = p->geometry[C_YZ]; + jac[2][0] = p->geometry[C_ZX]; + jac[2][1] = p->geometry[C_ZY]; + jac[2][2] = p->geometry[C_ZZ]; for (r = 3; r < 9; r++) { jac[r][r] = 1; } return 0; } + +static const kins_ops matrix_ops = { + .forward = matrix_forward, + .inverse = matrix_inverse, + .jacobian = matrix_jacobian, +}; + +// the entry points come from kins_single.c, linked in +const kins_module_info kins_module = { + .name = "matrixkins", + .halprefix = "matrixkins", + .params = matrix_params, + .nparams = sizeof(matrix_params)/sizeof(matrix_params[0]), + .required_coordinates = "XYZABCUVW", + .max_joints = 9, + .allow_duplicates = 0, + .ntypes = 1, + .ops = { &matrix_ops }, +}; + +// halcompile has done hal_init() and does hal_ready() after this returns, +// which is what kinsSingleInit() expects +EXTRA_SETUP() { + (void)__comp_inst; (void)prefix; (void)extra_arg; + return kinsSingleInit(comp_id, &kins_module, "XYZABCUVW", KINEMATICS_BOTH); +} diff --git a/src/hal/components/userkins.comp b/src/hal/components/userkins.comp index a9cdd374bac..fef97bb0061 100644 --- a/src/hal/components/userkins.comp +++ b/src/hal/components/userkins.comp @@ -16,9 +16,8 @@ where '2.8' is the branch name (use 'master' for the master branch). For a RIP (run-in-place) build, the file is located in the git tree as: `src/hal/components/userkins.comp`. -Edit the functions kinematicsForward() and kinematicsInverse() as required. - -If required, add HAL pins following examples in the template code. +Edit the functions userkins_forward() and userkins_inverse() as required, +and list the geometry the maths needs in the *userkins_params* table. Build and install the component using halcompile: @@ -43,6 +42,17 @@ JOINTS=3 # number of joints used in your modified userkins.comp ---- +A module built with halcompile takes the shared code from the +kins_single_core module, which the HAL file loads ahead of it: + +---- +loadrt kins_single_core +loadrt [KINS]KINEMATICS +---- + +The userkins built in tree has that code linked in and needs no +kins_single_core. + Note: the manpage for userkins is not updated by `halcompile --install` To use a different component name, rename the file (example mykins.comp) and @@ -50,16 +60,18 @@ change all instances of `userkins` to `mykins`. === NOTES +* The kinematics are written as functions of a parameter block, see + kinematics.h: the geometry is declared once in the *userkins_params* + table, one HAL pin is made per entry, and the maths reads + *p->geometry[]* where it would have read a pin. The classic entry + points (kinematicsForward() and the rest) are supplied by kins_single.c, + in kins_single_core, so nothing here touches HAL and the same maths can + be evaluated outside realtime. * The *fpin* pin is included to satisfy the requirements of the halcompile utility but it is not accessible to kinematics functions. -* HAL pins and parameters needed in kinematics functions (kinematicsForward(), - kinematicsInverse()) must be setup in the *EXTRA_SETUP()* function, which - halcompile runs once when the module is loaded, before the component is - made ready. """; // The fpin pin is not accessible in kinematics functions. -// Use EXTRA_SETUP() for pins and params used by kinematics. pin out sint fpin=0"pin to demonstrate use of a conventional (non-kinematics) function fdemo"; option period no; option extra_setup; @@ -69,20 +81,17 @@ author "Dewey Garrett"; ;; #include - -static struct haldata { - // Example pin pointers - hal_uint_t in; - hal_uint_t out; - // Example parameters - hal_real_t param_rw; - hal_real_t param_ro; -} *haldata; -// hal pin/param types: -// hal_bool_t boolean bit -// hal_uint_t unsigned integer -// hal_sint_t signed integer -// hal_real_t floating point (double precision) +#include + +// The geometry, one HAL pin per entry, named userkins.. An entry +// is an input (read into p->geometry[] before every call), an output +// (written from s->out[] after it), or an input that can be poked +// (KINS_IO). The example pair below echoes 'in' to 'out'. +static const kins_param_desc userkins_params[] = { + { "in", KINS_PARAM_U32, KINS_IN, 0, 0 }, + { "out", KINS_PARAM_U32, KINS_OUT, 0, 0 }, +}; +enum { P_IN, P_OUT }; FUNCTION(fdemo) { // This function can be added to a thread (addf) for @@ -93,61 +102,16 @@ FUNCTION(fdemo) { fpin_set(fpin + 1); } -EXTRA_SETUP() { - (void)__comp_inst; - (void)prefix; - (void)extra_arg; -#define HAL_PREFIX "userkins" - int res=0; - - // inherit comp_id from rtapi_main() - if (comp_id < 0) goto error; - - haldata = hal_malloc(sizeof(struct haldata)); - if (!haldata) goto error; - - // hal pin examples: - res += hal_pin_new_uint(comp_id, HAL_IN , &(haldata->in) , 0, "%s.in" , HAL_PREFIX); - res += hal_pin_new_uint(comp_id, HAL_OUT, &(haldata->out), 0, "%s.out", HAL_PREFIX); - - // hal parameter examples: - res += hal_param_new_real(comp_id, HAL_RW, &haldata->param_rw, 0.0, "%s.param-rw", HAL_PREFIX); - res += hal_param_new_real(comp_id, HAL_RO, &haldata->param_ro, 0.0, "%s.param-ro", HAL_PREFIX); - - if (res) goto error; - rtapi_print("*** %s setup ok\n",__FILE__); - return 0; -error: - rtapi_print("\n!!! %s setup failed res=%d\n\n",__FILE__,res); - return -1; -#undef HAL_PREFIX -} - -KINS_NOT_SWITCHABLE -// see millturn.comp for example of switchable kinematics - -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsJacobian); -EXPORT_SYMBOL(kinematicsForward); - -KINEMATICS_TYPE kinematicsType() -{ - return KINEMATICS_IDENTITY; // set as required - // Note: If kinematics are identity, using KINEMATICS_BOTH - // may be used in order to allow a gui to display - // joint values in preview prior to homing -} // kinematicsType() - -static bool is_ready=0; -int kinematicsForward(const double *j, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +static int userkins_forward(const kins_params *p, kins_scratch *s, + const double *j, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)p; + (void)s; (void)fflags; (void)iflags; - static bool gave_msg; // [KINS]JOINTS=3 pos->tran.x = j[0]; // X coordinate pos->tran.y = j[1]; // Y coordinate @@ -160,23 +124,17 @@ int kinematicsForward(const double *j, pos->v = 0; pos->w = 0; - if (hal_get_uint(haldata->in) && !is_ready && !gave_msg) { - rtapi_print_msg(RTAPI_MSG_ERR, - "%s The 'in' pin not echoed until Inverse called\n", - __FILE__); - gave_msg=1; - } return 0; -} // kinematicsForward() +} // userkins_forward() -int kinematicsInverse(const EmcPose * pos, - double *j, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int userkins_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, + double *j, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { (void)iflags; (void)fflags; - is_ready = 1; // Inverse is not called until homed for KINEMATICS_BOTH // Update the kinematic joints specified by the // [KINS]JOINTS setting (3 required for this template). @@ -189,25 +147,26 @@ int kinematicsInverse(const EmcPose * pos, j[1] = pos->tran.y; // joint 1 j[2] = pos->tran.z; // joint 2 - //example hal pin update (homing reqd before kinematicsInverse) - hal_set_uint(haldata->out, hal_get_uint(haldata->in)); //dereference - //read from param example: hal_set_uint(haldata->out, hal_get_real(haldata->param_rw)); + // example output: echo the 'in' pin to the 'out' pin + s->out[P_OUT] = p->geometry[P_IN]; return 0; -} // kinematicsInverse() +} // userkins_inverse() -int kinematicsJacobian(const double *j, - const EmcPose * pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS * iflags) +static int userkins_jacobian(const kins_params *p, const double *j, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) { int r, c; + (void)p; (void)j; (void)pos; (void)iflags; // How each joint responds to each pose coordinate, the derivative of - // kinematicsInverse(): for this template joint 0 follows x, joint 1 + // userkins_inverse(): for this template joint 0 follows x, joint 1 // follows y and joint 2 follows z, each one for one. See kinematics.h. + // Leave .jacobian out of the ops below to have it differenced instead. for (r = 0; r < EMCMOT_MAX_JOINTS; r++) { for (c = 0; c < EMCMOT_MAX_AXIS; c++) { jac[r][c] = 0; } } @@ -215,4 +174,44 @@ int kinematicsJacobian(const double *j, jac[1][1] = 1; jac[2][2] = 1; return 0; -} // kinematicsJacobian() +} // userkins_jacobian() + +static const kins_ops userkins_ops = { + .forward = userkins_forward, + .inverse = userkins_inverse, + .jacobian = userkins_jacobian, + // .work, .tool and .native report the frames, see kinematics.h +}; + +const kins_module_info kins_module = { + .name = "userkins", + .halprefix = "userkins", + .params = userkins_params, + .nparams = sizeof(userkins_params)/sizeof(userkins_params[0]), + .required_coordinates = "XYZ", + .max_joints = 3, + .allow_duplicates = 0, + .ntypes = 1, + .ops = { &userkins_ops }, +}; + +// A copy of the module loaded outside realtime asks for its description. +int kinsDescribe(const char *coordinates, const char *sparm, + kins_module_info *info) +{ + (void)coordinates; + (void)sparm; + return kinsSingleDescribe(&kins_module, info); +} + +EXPORT_SYMBOL(kinsDescribe); + +// halcompile has done hal_init() and does hal_ready() after this returns, +// which is what kinsSingleInit() expects. KINEMATICS_IDENTITY is what +// kinematicsType() reports; use KINEMATICS_BOTH for a machine whose +// joints are not the axes, or to let a gui display joint values in the +// preview before homing. +EXTRA_SETUP() { + (void)__comp_inst; (void)prefix; (void)extra_arg; + return kinsSingleInit(comp_id, &kins_module, "XYZ", KINEMATICS_IDENTITY); +} From 1c115ae0fc4a276250e48e07ea7cb13aad5f92ca Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Fri, 4 Sep 2026 13:00:55 +1000 Subject: [PATCH 05/19] scarakins, pumakins, three21kins: move onto the parameter block Each arm declares its dimensions as a table, reads them from the block and registers one ops table for its own type, with the identity and userk types from the shared ops. pumakins keeps its flange frame and declares the half turn through the ops table's native rotation, where switchkinsRegisterFrames() carried it before. The setup functions and haldata go; pin names and defaults are unchanged. --- src/emc/kinematics/pumakins.c | 170 +++++++++++++------------------ src/emc/kinematics/scarakins.c | 155 ++++++++++++---------------- src/emc/kinematics/three21kins.c | 156 +++++++++++++--------------- 3 files changed, 205 insertions(+), 276 deletions(-) diff --git a/src/emc/kinematics/pumakins.c b/src/emc/kinematics/pumakins.c index 8b17fae351e..adac1298e0e 100644 --- a/src/emc/kinematics/pumakins.c +++ b/src/emc/kinematics/pumakins.c @@ -24,9 +24,15 @@ #include "pumakins.h" #include -struct haldata { - hal_real_t a2, a3, d3, d4, d6; -} *haldata = NULL; +// the five dimensions, one pin each; the maths reads them from the block +static const kins_param_desc puma_params[] = { + { "A2", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_PUMA560_A2 }, + { "A3", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_PUMA560_A3 }, + { "D3", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_PUMA560_D3 }, + { "D4", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_PUMA560_D4 }, + { "D6", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_PUMA560_D6 }, +}; +enum { P_A2, P_A3, P_D3, P_D4, P_D6 }; /* the difference of two angles, brought into (-pi, pi] so that a joint a whole turn from the formula still matches it */ @@ -107,11 +113,13 @@ static void pumaFlangeRotation(const double * joint, PmRotationMatrix * rot) *rot = hom.rot; } // pumaFlangeRotation() -static int pumaKinematicsForward(const double * joint, - EmcPose * world, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +static int puma_forward(const kins_params *p, kins_scratch *s, + const double * joint, + EmcPose * world, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)s; (void)fflags; double s1, s2, s3; double c1, c2, c3; @@ -135,10 +143,10 @@ static int pumaKinematicsForward(const double * joint, s23 = c2 * s3 + s2 * c3; c23 = c2 * c3 - s2 * s3; - rtapi_real PUMA_A2 = hal_get_real(haldata->a2); - rtapi_real PUMA_A3 = hal_get_real(haldata->a3); - rtapi_real PUMA_D3 = hal_get_real(haldata->d3); - rtapi_real PUMA_D4 = hal_get_real(haldata->d4); + const double PUMA_A2 = p->geometry[P_A2]; + const double PUMA_A3 = p->geometry[P_A3]; + const double PUMA_D3 = p->geometry[P_D3]; + const double PUMA_D4 = p->geometry[P_D4]; /* Calculate term to be used in definition of... */ /* position vector. */ @@ -191,7 +199,7 @@ static int pumaKinematicsForward(const double * joint, *iflags |= PUMA_WRIST_FLIP; } } - rtapi_real PUMA_D6 = hal_get_real(haldata->d6); + const double PUMA_D6 = p->geometry[P_D6]; /* add effect of d6 parameter */ hom.tran.x = hom.tran.x + hom.rot.z.x*PUMA_D6; hom.tran.y = hom.tran.y + hom.rot.z.y*PUMA_D6; @@ -216,45 +224,38 @@ static int pumaKinematicsForward(const double * joint, the base z, the upper arm and forearm about axes at right angles to it, the wrist about three axes meeting at its centre, and D6 carries the tool point out along the flange z. */ -static int pumaKinematicsJacobian(const double * joint, - const EmcPose * world, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS * iflags) +static int puma_jacobian(const kins_params *p, const double * joint, + const EmcPose * world, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) { (void)iflags; const double alpha[6] = { 0, -90, 0, -90, 90, -90 }; - const double a[6] = { 0, 0, hal_get_real(haldata->a2), hal_get_real(haldata->a3), 0, 0 }; - const double d[6] = { 0, 0, hal_get_real(haldata->d3), hal_get_real(haldata->d4), 0, 0 }; + const double a[6] = { 0, 0, p->geometry[P_A2], p->geometry[P_A3], 0, 0 }; + const double d[6] = { 0, 0, p->geometry[P_D3], p->geometry[P_D4], 0, 0 }; - return kinsJacobianFromDhArm(alpha, a, d, joint, hal_get_real(haldata->d6), world, jac); -} // pumaKinematicsJacobian() + return kinsJacobianFromDhArm(alpha, a, d, joint, p->geometry[P_D6], world, jac); +} // puma_jacobian() -static int pumaKinematicsToolFrame(const double * joint, - PmRotationMatrix * rot, - const KINEMATICS_FORWARD_FLAGS * fflags) +static int puma_tool_frame(const kins_params *p, const double * joint, + PmRotationMatrix * rot, + const KINEMATICS_FORWARD_FLAGS * fflags) { + (void)p; (void)fflags; - // answers in the flange frame; switchkins applies the declared half turn + // answers in the flange frame; the declared half turn is applied by + // the shared code pumaFlangeRotation(joint, rot); return 0; -} // pumaKinematicsToolFrame() +} // puma_tool_frame() -static int pumaKinematicsWorkFrame(const double * joint, - PmRotationMatrix * rot, - const KINEMATICS_FORWARD_FLAGS * fflags) -{ - (void)joint; - (void)fflags; - // the arm carries the tool and nothing carries the work - *rot = TOOL_FRAME_SPINDLE; - return 0; -} // pumaKinematicsWorkFrame() - -static int pumaKinematicsInverse(const EmcPose * world, - double * joint, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int puma_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * world, + double * joint, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { + (void)s; PmHomogeneous hom; PmPose worldPose; PmRpy rpy; @@ -290,11 +291,11 @@ static int pumaKinematicsInverse(const EmcPose * world, pmRpyQuatConvert(&rpy,&worldPose.rot); pmPoseHomConvert(&worldPose, &hom); - rtapi_real PUMA_A2 = hal_get_real(haldata->a2); - rtapi_real PUMA_A3 = hal_get_real(haldata->a3); - rtapi_real PUMA_D3 = hal_get_real(haldata->d3); - rtapi_real PUMA_D4 = hal_get_real(haldata->d4); - rtapi_real PUMA_D6 = hal_get_real(haldata->d6); + const double PUMA_A2 = p->geometry[P_A2]; + const double PUMA_A3 = p->geometry[P_A3]; + const double PUMA_D3 = p->geometry[P_D3]; + const double PUMA_D4 = p->geometry[P_D4]; + const double PUMA_D6 = p->geometry[P_D6]; /* remove effect of d6 parameter */ px = hom.tran.x - PUMA_D6*hom.rot.z.x; @@ -413,29 +414,19 @@ static int pumaKinematicsInverse(const EmcPose * world, return 0; } -int pumaKinematicsSetup(const int comp_id, - const char* coordinates, - kparms* kp) -{ - (void)coordinates; - int res=0; - - haldata = hal_malloc(sizeof(*haldata)); - if (!haldata) goto error; - - - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->a2), DEFAULT_PUMA560_A2, "%s.A2", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->a3), DEFAULT_PUMA560_A3, "%s.A3", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->d3), DEFAULT_PUMA560_D3, "%s.D3", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->d4), DEFAULT_PUMA560_D4, "%s.D4", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->d6), DEFAULT_PUMA560_D6, "%s.D6", kp->halprefix); - if (res) { goto error; } - - return 0; - -error: - return -1; -} // pumaKinematicsSetup() +// the arm carries the tool and nothing carries the work, so the work frame +// is the shared identity one. The maths is the ISO 9787 flange frame, so +// the tool axis it produces runs holder towards tip, the opposite of the +// convention; the declared half turn puts it right. No closed form +// Jacobian: the shared code differences the inverse. +static const kins_ops puma_ops = { + .forward = puma_forward, + .inverse = puma_inverse, + .jacobian = puma_jacobian, + .work = kinsIdentityFrame, + .tool = puma_tool_frame, + .native = &TOOL_FRAME_FLANGE, +}; int switchkinsSetup(kparms* kp, KS* kset0, KS* kset1, KS* kset2, @@ -443,51 +434,28 @@ int switchkinsSetup(kparms* kp, KI* kinv0, KI* kinv1, KI* kinv2 ) { + (void)kset0; (void)kset1; (void)kset2; + (void)kfwd0; (void)kfwd1; (void)kfwd2; + (void)kinv0; (void)kinv1; (void)kinv2; kp->kinsname = "pumakins"; // !!! must agree with filename kp->halprefix = "pumakins"; // hal pin names kp->required_coordinates = "xyzabc"; kp->allow_duplicates = 0; kp->max_joints = strlen(kp->required_coordinates); + kp->params = puma_params; + kp->nparams = sizeof(puma_params)/sizeof(puma_params[0]); if (kp->sparm && strstr(kp->sparm,"identityfirst")) { rtapi_print("\n!!! switchkins-type 0 is IDENTITY\n"); - *kset0 = identityKinematicsSetup; - *kfwd0 = identityKinematicsForward; - *kinv0 = identityKinematicsInverse; - - *kset1 = pumaKinematicsSetup; - *kfwd1 = pumaKinematicsForward; - *kinv1 = pumaKinematicsInverse; - // the maths is the ISO 9787 flange frame, so the tool axis it produces - // runs holder towards tip, the opposite of the convention - switchkinsRegisterFrames(1, pumaKinematicsWorkFrame, - pumaKinematicsToolFrame, - &TOOL_FRAME_FLANGE); - switchkinsRegisterJacobian(1, pumaKinematicsJacobian); - switchkinsDeclare(0, KINSTYPE_IDENTITY); - switchkinsDeclare(1, KINSTYPE_PRIMARY); + switchkinsRegisterOps(0, &KINS_IDENTITY_OPS); + switchkinsRegisterOps(1, &puma_ops); } else { rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname); - *kset0 = pumaKinematicsSetup; - *kfwd0 = pumaKinematicsForward; - *kinv0 = pumaKinematicsInverse; - // the maths is the ISO 9787 flange frame, so the tool axis it produces - // runs holder towards tip, the opposite of the convention - switchkinsRegisterFrames(0, pumaKinematicsWorkFrame, - pumaKinematicsToolFrame, - &TOOL_FRAME_FLANGE); - switchkinsRegisterJacobian(0, pumaKinematicsJacobian); - - *kset1 = identityKinematicsSetup; - *kfwd1 = identityKinematicsForward; - *kinv1 = identityKinematicsInverse; - switchkinsDeclare(0, KINSTYPE_PRIMARY); - switchkinsDeclare(1, KINSTYPE_IDENTITY); + switchkinsRegisterOps(0, &puma_ops); + switchkinsRegisterOps(1, &KINS_IDENTITY_OPS); } - *kset2 = userkKinematicsSetup; - *kfwd2 = userkKinematicsForward; - *kinv2 = userkKinematicsInverse; + switchkinsRegisterOps(2, &USERK_OPS); return 0; } // switchkinsSetup() diff --git a/src/emc/kinematics/scarakins.c b/src/emc/kinematics/scarakins.c index e2dedb241b6..34035b8adac 100644 --- a/src/emc/kinematics/scarakins.c +++ b/src/emc/kinematics/scarakins.c @@ -22,10 +22,6 @@ #include -static struct scara_data { - hal_real_t d1, d2, d3, d4, d5, d6; -} *haldata = NULL; - /* key dimensions joint[0] = Entire arm rotates around a vertical axis at its inner end @@ -62,13 +58,32 @@ static struct scara_data { on the value of joint[3]. */ +#define DEFAULT_D1 490 +#define DEFAULT_D2 340 +#define DEFAULT_D3 50 +#define DEFAULT_D4 250 +#define DEFAULT_D5 50 +#define DEFAULT_D6 50 + +// the six dimensions, one pin each; the maths reads them from the block +static const kins_param_desc scara_params[] = { + { "D1", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_D1 }, + { "D2", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_D2 }, + { "D3", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_D3 }, + { "D4", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_D4 }, + { "D5", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_D5 }, + { "D6", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_D6 }, +}; +enum { P_D1, P_D2, P_D3, P_D4, P_D5, P_D6 }; + /* joint[0], joint[1] and joint[3] are in degrees and joint[2] is in length units */ -static -int scaraKinematicsForward(const double * joint, - EmcPose * world, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +static int scara_forward(const kins_params *p, kins_scratch *s, + const double * joint, + EmcPose * world, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)s; (void)fflags; double a0, a1, a3; double x, y, z, c; @@ -83,12 +98,12 @@ int scaraKinematicsForward(const double * joint, a1 = a1 + a0; a3 = a3 + a1; - rtapi_real D1 = hal_get_real(haldata->d1); - rtapi_real D2 = hal_get_real(haldata->d2); - rtapi_real D3 = hal_get_real(haldata->d3); - rtapi_real D4 = hal_get_real(haldata->d4); - rtapi_real D5 = hal_get_real(haldata->d5); - rtapi_real D6 = hal_get_real(haldata->d6); + const double D1 = p->geometry[P_D1]; + const double D2 = p->geometry[P_D2]; + const double D3 = p->geometry[P_D3]; + const double D4 = p->geometry[P_D4]; + const double D5 = p->geometry[P_D5]; + const double D6 = p->geometry[P_D6]; x = D2*cos(a0) + D4*cos(a1) + D6*cos(a3); y = D2*sin(a0) + D4*sin(a1) + D6*sin(a3); @@ -109,13 +124,15 @@ int scaraKinematicsForward(const double * joint, world->b = joint[5]; return (0); -} //scaraKinematicsForward() +} // scara_forward() -static int scaraKinematicsInverse(const EmcPose * world, - double * joint, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int scara_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * world, + double * joint, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { + (void)s; double a3; double q0, q1; double xt, yt, rsq, cc; @@ -129,12 +146,12 @@ static int scaraKinematicsInverse(const EmcPose * world, /* convert degrees to radians */ a3 = c * ( PM_PI / 180 ); - rtapi_real D1 = hal_get_real(haldata->d1); - rtapi_real D2 = hal_get_real(haldata->d2); - rtapi_real D3 = hal_get_real(haldata->d3); - rtapi_real D4 = hal_get_real(haldata->d4); - rtapi_real D5 = hal_get_real(haldata->d5); - rtapi_real D6 = hal_get_real(haldata->d6); + const double D1 = p->geometry[P_D1]; + const double D2 = p->geometry[P_D2]; + const double D3 = p->geometry[P_D3]; + const double D4 = p->geometry[P_D4]; + const double D5 = p->geometry[P_D5]; + const double D6 = p->geometry[P_D6]; /* center of end effector (correct for D6) */ xt = x - D6*cos(a3); @@ -176,17 +193,17 @@ static int scaraKinematicsInverse(const EmcPose * world, *fflags = 0; return (0); -} // scaraKinematicsInverse() +} // scara_inverse() -static int scaraKinematicsJacobian(const double * joint, - const EmcPose * world, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS * iflags) +static int scara_jacobian(const kins_params *p, const double * joint, + const EmcPose * world, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) { (void)iflags; - rtapi_real D2 = hal_get_real(haldata->d2); - rtapi_real D4 = hal_get_real(haldata->d4); - rtapi_real D6 = hal_get_real(haldata->d6); + const double D2 = p->geometry[P_D2]; + const double D4 = p->geometry[P_D4]; + const double D6 = p->geometry[P_D6]; const double a3 = world->c * (PM_PI / 180); const double q1 = joint[1] * (PM_PI / 180); const double xt = world->tran.x - D6*cos(a3); @@ -226,38 +243,13 @@ static int scaraKinematicsJacobian(const double * joint, jac[4][3] = 1; jac[5][4] = 1; return 0; -} // scaraKinematicsJacobian() - -#define DEFAULT_D1 490 -#define DEFAULT_D2 340 -#define DEFAULT_D3 50 -#define DEFAULT_D4 250 -#define DEFAULT_D5 50 -#define DEFAULT_D6 50 - -static int scaraKinematicsSetup(const int comp_id, - const char* coordinates, - kparms* kp) -{ - (void)coordinates; - int res=0; - - haldata = hal_malloc(sizeof(*haldata)); - if (!haldata) goto error; - - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->d1), DEFAULT_D1, "%s.D1", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->d2), DEFAULT_D2, "%s.D2", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->d3), DEFAULT_D3, "%s.D3", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->d4), DEFAULT_D4, "%s.D4", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->d5), DEFAULT_D5, "%s.D5", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->d6), DEFAULT_D6, "%s.D6", kp->halprefix); - if (res) { goto error; } - - return 0; +} // scara_jacobian() -error: - return -1; -} // scaraKinematicsSetup() +static const kins_ops scara_ops = { + .forward = scara_forward, + .inverse = scara_inverse, + .jacobian = scara_jacobian, +}; int switchkinsSetup(kparms* kp, KS* kset0, KS* kset1, KS* kset2, @@ -265,41 +257,28 @@ int switchkinsSetup(kparms* kp, KI* kinv0, KI* kinv1, KI* kinv2 ) { + (void)kset0; (void)kset1; (void)kset2; + (void)kfwd0; (void)kfwd1; (void)kfwd2; + (void)kinv0; (void)kinv1; (void)kinv2; kp->kinsname = "scarakins"; // !!! must agree with filename kp->halprefix = "scarakins"; // hal pin names kp->required_coordinates = "xyzabc"; // ab are scaragui table tilts kp->allow_duplicates = 0; kp->max_joints = strlen(kp->required_coordinates); + kp->params = scara_params; + kp->nparams = sizeof(scara_params)/sizeof(scara_params[0]); if (kp->sparm && strstr(kp->sparm,"identityfirst")) { rtapi_print("\n!!! switchkins-type 0 is IDENTITY\n"); - *kset0 = identityKinematicsSetup; - *kfwd0 = identityKinematicsForward; - *kinv0 = identityKinematicsInverse; - - *kset1 = scaraKinematicsSetup; - *kfwd1 = scaraKinematicsForward; - *kinv1 = scaraKinematicsInverse; - switchkinsDeclare(0, KINSTYPE_IDENTITY); - switchkinsDeclare(1, KINSTYPE_PRIMARY); - switchkinsRegisterJacobian(1, scaraKinematicsJacobian); + switchkinsRegisterOps(0, &KINS_IDENTITY_OPS); + switchkinsRegisterOps(1, &scara_ops); } else { rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname); - *kset0 = scaraKinematicsSetup; - *kfwd0 = scaraKinematicsForward; - *kinv0 = scaraKinematicsInverse; - switchkinsRegisterJacobian(0, scaraKinematicsJacobian); - - *kset1 = identityKinematicsSetup; - *kfwd1 = identityKinematicsForward; - *kinv1 = identityKinematicsInverse; - switchkinsDeclare(0, KINSTYPE_PRIMARY); - switchkinsDeclare(1, KINSTYPE_IDENTITY); + switchkinsRegisterOps(0, &scara_ops); + switchkinsRegisterOps(1, &KINS_IDENTITY_OPS); } - *kset2 = userkKinematicsSetup; - *kfwd2 = userkKinematicsForward; - *kinv2 = userkKinematicsInverse; + switchkinsRegisterOps(2, &USERK_OPS); return 0; } // switchkinsSetup() diff --git a/src/emc/kinematics/three21kins.c b/src/emc/kinematics/three21kins.c index 5cce5796a30..ae602858389 100644 --- a/src/emc/kinematics/three21kins.c +++ b/src/emc/kinematics/three21kins.c @@ -27,9 +27,18 @@ /* flags for forward kinematics */ #define THREE21_REACH 0x01 -struct haldata { - hal_real_t a1, a2, a3, d1, d2, d3, d4, d6; -} *haldata = NULL; +// the eight dimensions, one pin each; the maths reads them from the block +static const kins_param_desc three21_params[] = { + { "A1", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_THREE21_A1 }, + { "A2", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_THREE21_A2 }, + { "A3", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_THREE21_A3 }, + { "D1", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_THREE21_D1 }, + { "D2", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_THREE21_D2 }, + { "D3", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_THREE21_D3 }, + { "D4", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_THREE21_D4 }, + { "D6", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_THREE21_D6 }, +}; +enum { P_A1, P_A2, P_A3, P_D1, P_D2, P_D3, P_D4, P_D6 }; /* the difference of two angles, brought into (-pi, pi] so that a joint a whole turn from the formula still matches it */ @@ -41,20 +50,22 @@ static double angleDiff(double a, double b) return d; } -static int three21KinematicsForward(const double * joint, - EmcPose * world, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +static int three21_forward(const kins_params *p, kins_scratch *s, + const double * joint, + EmcPose * world, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)s; (void)fflags; - double a1 = hal_get_real(haldata->a1); - double a2 = hal_get_real(haldata->a2); - double a3 = hal_get_real(haldata->a3); - double d1 = hal_get_real(haldata->d1); - double d2 = hal_get_real(haldata->d2); - double d3 = hal_get_real(haldata->d3); - double d4 = hal_get_real(haldata->d4); - double d6 = hal_get_real(haldata->d6); + double a1 = p->geometry[P_A1]; + double a2 = p->geometry[P_A2]; + double a3 = p->geometry[P_A3]; + double d1 = p->geometry[P_D1]; + double d2 = p->geometry[P_D2]; + double d3 = p->geometry[P_D3]; + double d4 = p->geometry[P_D4]; + double d6 = p->geometry[P_D6]; double s1, s2, s3, s4, s5, s6; double c1, c2, c3, c4, c5, c6; @@ -193,38 +204,40 @@ static int three21KinematicsForward(const double * joint, with the shoulder set A1 out along the first link and D1 up the base, D2 and D3 both along the axis the upper arm turns about, and D6 carrying the tool point out along the flange z. */ -static int three21KinematicsJacobian(const double * joint, - const EmcPose * world, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS * iflags) +static int three21_jacobian(const kins_params *p, const double * joint, + const EmcPose * world, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) { (void)iflags; const double alpha[6] = { 0, -90, 0, -90, 90, -90 }; - const double a[6] = { 0, hal_get_real(haldata->a1), hal_get_real(haldata->a2), - hal_get_real(haldata->a3), 0, 0 }; - const double d[6] = { hal_get_real(haldata->d1), hal_get_real(haldata->d2), - hal_get_real(haldata->d3), hal_get_real(haldata->d4), 0, 0 }; - - return kinsJacobianFromDhArm(alpha, a, d, joint, hal_get_real(haldata->d6), world, jac); -} // three21KinematicsJacobian() - -static int three21KinematicsInverse(const EmcPose * world, - double * joint, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) + const double a[6] = { 0, p->geometry[P_A1], p->geometry[P_A2], + p->geometry[P_A3], 0, 0 }; + const double d[6] = { p->geometry[P_D1], p->geometry[P_D2], + p->geometry[P_D3], p->geometry[P_D4], 0, 0 }; + + return kinsJacobianFromDhArm(alpha, a, d, joint, p->geometry[P_D6], world, jac); +} // three21_jacobian() + +static int three21_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * world, + double * joint, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { + (void)s; PmHomogeneous hom; PmPose worldPose; PmRpy rpy; - double a1 = hal_get_real(haldata->a1); - double a2 = hal_get_real(haldata->a2); - double a3 = hal_get_real(haldata->a3); - double d1 = hal_get_real(haldata->d1); - double d2 = hal_get_real(haldata->d2); - double d3 = hal_get_real(haldata->d3); - double d4 = hal_get_real(haldata->d4); - double d6 = hal_get_real(haldata->d6); + double a1 = p->geometry[P_A1]; + double a2 = p->geometry[P_A2]; + double a3 = p->geometry[P_A3]; + double d1 = p->geometry[P_D1]; + double d2 = p->geometry[P_D2]; + double d3 = p->geometry[P_D3]; + double d4 = p->geometry[P_D4]; + double d6 = p->geometry[P_D6]; double t1, t2, t3; double k; @@ -367,31 +380,13 @@ static int three21KinematicsInverse(const EmcPose * world, return 0; } -int three21KinematicsSetup(const int comp_id, - const char* coordinates, - kparms* kp) -{ - (void)coordinates; - int res=0; - - haldata = hal_malloc(sizeof(*haldata)); - if (!haldata) goto error; - - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->a1), DEFAULT_THREE21_A1, "%s.A1", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->a2), DEFAULT_THREE21_A2, "%s.A2", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->a3), DEFAULT_THREE21_A3, "%s.A3", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->d1), DEFAULT_THREE21_D1, "%s.D1", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->d2), DEFAULT_THREE21_D2, "%s.D2", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->d3), DEFAULT_THREE21_D3, "%s.D3", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->d4), DEFAULT_THREE21_D4, "%s.D4", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->d6), DEFAULT_THREE21_D6, "%s.D6", kp->halprefix); - if (res) { goto error; } - - return 0; - -error: - return -1; -} +// no frames reported and no closed form Jacobian: the shared code +// differences the inverse +static const kins_ops three21_ops = { + .forward = three21_forward, + .inverse = three21_inverse, + .jacobian = three21_jacobian, +}; int switchkinsSetup(kparms* kp, KS* kset0, KS* kset1, KS* kset2, @@ -399,41 +394,28 @@ int switchkinsSetup(kparms* kp, KI* kinv0, KI* kinv1, KI* kinv2 ) { + (void)kset0; (void)kset1; (void)kset2; + (void)kfwd0; (void)kfwd1; (void)kfwd2; + (void)kinv0; (void)kinv1; (void)kinv2; kp->kinsname = "three21kins"; kp->halprefix = "three21kins"; kp->required_coordinates = "xyzabc"; kp->allow_duplicates = 0; kp->max_joints = strlen(kp->required_coordinates); + kp->params = three21_params; + kp->nparams = sizeof(three21_params)/sizeof(three21_params[0]); if (kp->sparm && strstr(kp->sparm,"identityfirst")) { rtapi_print("\n!!! switchkins-type 0 is IDENTITY\n"); - *kset0 = identityKinematicsSetup; - *kfwd0 = identityKinematicsForward; - *kinv0 = identityKinematicsInverse; - - *kset1 = three21KinematicsSetup; - *kfwd1 = three21KinematicsForward; - *kinv1 = three21KinematicsInverse; - switchkinsRegisterJacobian(1, three21KinematicsJacobian); - switchkinsDeclare(0, KINSTYPE_IDENTITY); - switchkinsDeclare(1, KINSTYPE_PRIMARY); + switchkinsRegisterOps(0, &KINS_IDENTITY_OPS); + switchkinsRegisterOps(1, &three21_ops); } else { rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname); - *kset0 = three21KinematicsSetup; - *kfwd0 = three21KinematicsForward; - *kinv0 = three21KinematicsInverse; - switchkinsRegisterJacobian(0, three21KinematicsJacobian); - - *kset1 = identityKinematicsSetup; - *kfwd1 = identityKinematicsForward; - *kinv1 = identityKinematicsInverse; - switchkinsDeclare(0, KINSTYPE_PRIMARY); - switchkinsDeclare(1, KINSTYPE_IDENTITY); + switchkinsRegisterOps(0, &three21_ops); + switchkinsRegisterOps(1, &KINS_IDENTITY_OPS); } - *kset2 = userkKinematicsSetup; - *kfwd2 = userkKinematicsForward; - *kinv2 = userkKinematicsInverse; + switchkinsRegisterOps(2, &USERK_OPS); return 0; } From 67e302e2bb8419aed99ddffce2eec143ebadf3e1 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Fri, 4 Sep 2026 13:06:35 +1000 Subject: [PATCH 06/19] millturn, xyzab_tdr_kins, xyzacb_trsrn, xyzbca_trsrn: move onto the parameter block Each component declares its geometry as a table, writes its types as ops over the block, and supplies switchkinsSetup() like the C modules do; EXTRA_SETUP() runs it through switchkinsRunSetup() and initialises, so the components link switchkins_setup.o too and export kinsDescribe() with the rest. The trsrn TCP type registers its frames and Jacobian in its ops table and the TOOL type the identity frames, as they were registered before. The inverses go on reading the rotary angles from their joint argument, as they always have. Pin names and defaults are unchanged. --- src/hal/components/Submakefile | 2 +- src/hal/components/millturn.comp | 103 +++--- src/hal/components/xyzab_tdr_kins.comp | 173 +++++---- src/hal/components/xyzacb_trsrn.comp | 470 +++++++++++-------------- src/hal/components/xyzbca_trsrn.comp | 436 ++++++++++------------- 5 files changed, 540 insertions(+), 644 deletions(-) diff --git a/src/hal/components/Submakefile b/src/hal/components/Submakefile index 8aae8597402..2404cbb6cf7 100644 --- a/src/hal/components/Submakefile +++ b/src/hal/components/Submakefile @@ -79,7 +79,7 @@ obj-m += $(patsubst hal/drivers/%.comp, %.o, $(patsubst hal/components/%.comp, % # A component that links objects besides its own names them here as # -extra-objs. The list is expanded when the .mak is written, # so it has to be defined in this file (which the .mak depends on). -SWITCHKINS_OBJS := emc/kinematics/switchkins.o emc/kinematics/kins_util.o +SWITCHKINS_OBJS := emc/kinematics/switchkins.o emc/kinematics/switchkins_setup.o emc/kinematics/kins_util.o matrixkins-extra-objs := emc/kinematics/kins_util.o emc/kinematics/kins_single.o emc/kinematics/kins_single_module.o userkins-extra-objs := emc/kinematics/kins_util.o emc/kinematics/kins_single.o millturn-extra-objs := $(SWITCHKINS_OBJS) diff --git a/src/hal/components/millturn.comp b/src/hal/components/millturn.comp index 29b0c04ce3e..1cec163018e 100644 --- a/src/hal/components/millturn.comp +++ b/src/hal/components/millturn.comp @@ -26,7 +26,6 @@ chapter (docs/src/motion/switchkins.txt) """; // The fpin pin is not accessible in kinematics functions. -// Use the *_setup() function for pins and params used by kinematics. pin out sint fpin=0"pin to demonstrate use of a conventional (non-kinematics) function fdemo"; option period no; option extra_setup; @@ -49,22 +48,16 @@ FUNCTION(fdemo) { fpin_set(fpin + 1); } -// the turn kinematics need no hal pins of their own -static int turnKinematicsSetup(const int comp_id, - const char* coords, - kparms* kp) -{ - (void)comp_id; - (void)coords; - (void)kp; - return 0; -} // turnKinematicsSetup() - -static int turnKinematicsForward(const double *j, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +// the turn kinematics: no geometry, written as pure functions of the +// parameter block (see kinematics.h) +static int turn_forward(const kins_params *p, kins_scratch *s, + const double *j, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)p; + (void)s; (void)fflags; (void)iflags; @@ -81,13 +74,16 @@ static int turnKinematicsForward(const double *j, pos->w = 0; return 0; -} // turnKinematicsForward() +} // turn_forward() -static int turnKinematicsInverse(const EmcPose * pos, - double *j, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int turn_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, + double *j, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { + (void)p; + (void)s; (void)iflags; (void)fflags; @@ -97,50 +93,59 @@ static int turnKinematicsInverse(const EmcPose * pos, j[3] = pos->a; return 0; -} // turnKinematicsInverse() +} // turn_inverse() -static int turnKinematicsJacobian(const double *j, - const EmcPose * pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS * iflags) +static int turn_jacobian(const kins_params *p, const double *j, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) { + (void)p; (void)j; (void)pos; (void)iflags; memset(jac, 0, EMCMOT_MAX_JOINTS * EMCMOT_MAX_AXIS * sizeof(jac[0][0])); - // the derivative of turnKinematicsInverse(): which joint follows which - // pose coordinate, and in which sense + // the derivative of turn_inverse(): which joint follows which pose + // coordinate, and in which sense jac[2][0] = 1; jac[1][1] = -1; jac[0][2] = 1; jac[3][3] = 1; return 0; -} // turnKinematicsJacobian() +} // turn_jacobian() + +static const kins_ops turn_ops = { + .forward = turn_forward, + .inverse = turn_inverse, + .jacobian = turn_jacobian, +}; + +int switchkinsSetup(kparms* kp, + KS* kset0, KS* kset1, KS* kset2, + KF* kfwd0, KF* kfwd1, KF* kfwd2, + KI* kinv0, KI* kinv1, KI* kinv2 + ) +{ + (void)kset0; (void)kset1; (void)kset2; + (void)kfwd0; (void)kfwd1; (void)kfwd2; + (void)kinv0; (void)kinv1; (void)kinv2; + kp->kinsname = "millturn"; + kp->halprefix = "millturn"; + kp->required_coordinates = "xyza"; + kp->allow_duplicates = 0; + kp->max_joints = strlen(kp->required_coordinates); + + switchkinsRegisterOps(0, &KINS_IDENTITY_OPS); + switchkinsRegisterOps(1, &turn_ops); + return 0; +} // switchkinsSetup() // halcompile has done hal_init() and does hal_ready() after this returns, // which is what switchkinsInit() expects EXTRA_SETUP() { - kparms kp = {0}; + kparms kp; (void)__comp_inst; (void)prefix; (void)extra_arg; - kp.kinsname = "millturn"; - kp.halprefix = "millturn"; - kp.required_coordinates = "xyza"; - kp.allow_duplicates = 0; - kp.fwd_iterates_mask = 0; - kp.gui_kinstype = -1; - kp.sparm = NULL; - kp.max_joints = strlen(kp.required_coordinates); - - if (switchkinsRegister(0, identityKinematicsSetup, - identityKinematicsForward, - identityKinematicsInverse)) { return -1; } - if (switchkinsRegister(1, turnKinematicsSetup, - turnKinematicsForward, - turnKinematicsInverse)) { return -1; } - if (switchkinsRegisterJacobian(1, turnKinematicsJacobian)) { return -1; } - - if (switchkinsDeclare(0, KINSTYPE_IDENTITY)) { return -1; } - + if (switchkinsRunSetup(&kp, NULL)) { return -1; } return switchkinsInit(comp_id, &kp, coordinates); } // EXTRA_SETUP() diff --git a/src/hal/components/xyzab_tdr_kins.comp b/src/hal/components/xyzab_tdr_kins.comp index f3410ef8fec..1125b22ba06 100644 --- a/src/hal/components/xyzab_tdr_kins.comp +++ b/src/hal/components/xyzab_tdr_kins.comp @@ -44,55 +44,33 @@ author "David Mueller"; static char *coordinates; RTAPI_MP_STRING(coordinates, "Axes-to-joints-ordering"); -static struct haldata { - hal_real_t tool_offset_z; - hal_real_t x_offset; - hal_real_t z_offset; - hal_real_t x_rot_point; - hal_real_t y_rot_point; - hal_real_t z_rot_point; -} *tdrdata; - -static int tdrKinematicsSetup(const int comp_id, - const char* coords, - kparms* kp) -{ - int res = 0; - (void)coords; - - tdrdata = hal_malloc(sizeof(*tdrdata)); - if (!tdrdata) return -1; - - res += hal_pin_new_real(comp_id, HAL_IN, &tdrdata->tool_offset_z, 0.0, - "%s.tool-offset-z", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &tdrdata->x_offset, 0.0, - "%s.x-offset", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &tdrdata->z_offset, 0.0, - "%s.z-offset", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &tdrdata->x_rot_point, 0.0, - "%s.x-rot-point", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &tdrdata->y_rot_point, 0.0, - "%s.y-rot-point", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &tdrdata->z_rot_point, 0.0, - "%s.z-rot-point", kp->halprefix); - if (res) return -1; - - return 0; -} // tdrKinematicsSetup() - -static int tdrKinematicsForward(const double *j, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +// the geometry, one pin each; the maths reads it from the block (see +// kinematics.h), and the tool length from p->tool.tran.z +static const kins_param_desc tdr_params[] = { + { "tool-offset-z", KINS_PARAM_FLOAT, KINS_IN, 1, 0.0 }, + { "x-offset", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "z-offset", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "x-rot-point", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "y-rot-point", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "z-rot-point", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, +}; +enum { P_TOOL, P_XO, P_ZO, P_XR, P_YR, P_ZR }; + +static int tdr_forward(const kins_params *p, kins_scratch *s, + const double *j, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)s; (void)fflags; (void)iflags; - double x_rot_point = hal_get_real(tdrdata->x_rot_point); - double y_rot_point = hal_get_real(tdrdata->y_rot_point); - double z_rot_point = hal_get_real(tdrdata->z_rot_point); + double x_rot_point = p->geometry[P_XR]; + double y_rot_point = p->geometry[P_YR]; + double z_rot_point = p->geometry[P_ZR]; - double dz = hal_get_real(tdrdata->z_offset); - double dt = hal_get_real(tdrdata->tool_offset_z); + double dz = p->geometry[P_ZO]; + double dt = p->tool.tran.z; // substitutions as used in mathematical documentation // including degree -> radians angle conversion @@ -125,22 +103,24 @@ static int tdrKinematicsForward(const double *j, pos->w = 0; return 0; -} // tdrKinematicsForward() +} // tdr_forward() -static int tdrKinematicsInverse(const EmcPose * pos, - double *j, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int tdr_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, + double *j, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { + (void)s; (void)iflags; (void)fflags; - double x_rot_point = hal_get_real(tdrdata->x_rot_point); - double y_rot_point = hal_get_real(tdrdata->y_rot_point); - double z_rot_point = hal_get_real(tdrdata->z_rot_point); + double x_rot_point = p->geometry[P_XR]; + double y_rot_point = p->geometry[P_YR]; + double z_rot_point = p->geometry[P_ZR]; - double dx = hal_get_real(tdrdata->x_offset); - double dz = hal_get_real(tdrdata->z_offset); - double dt = hal_get_real(tdrdata->tool_offset_z); + double dx = p->geometry[P_XO]; + double dz = p->geometry[P_ZO]; + double dt = p->tool.tran.z; // substitutions as used in mathematical documentation // including degree -> radians angle conversion @@ -167,21 +147,21 @@ static int tdrKinematicsInverse(const EmcPose * pos, j[4] = pos->b; return 0; -} // tdrKinematicsInverse() +} // tdr_inverse() -static int tdrKinematicsJacobian(const double *j, - const EmcPose * pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS * iflags) +static int tdr_jacobian(const kins_params *p, const double *j, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) { (void)j; (void)iflags; - double x_rot_point = hal_get_real(tdrdata->x_rot_point); - double y_rot_point = hal_get_real(tdrdata->y_rot_point); - double z_rot_point = hal_get_real(tdrdata->z_rot_point); - double dx = hal_get_real(tdrdata->x_offset); - double dz = hal_get_real(tdrdata->z_offset); - double dt = hal_get_real(tdrdata->tool_offset_z); + double x_rot_point = p->geometry[P_XR]; + double y_rot_point = p->geometry[P_YR]; + double z_rot_point = p->geometry[P_ZR]; + double dx = p->geometry[P_XO]; + double dz = p->geometry[P_ZO]; + double dt = p->tool.tran.z; double sa = sin(pos->a*TO_RAD); double ca = cos(pos->a*TO_RAD); double sb = sin(pos->b*TO_RAD); @@ -192,9 +172,9 @@ static int tdrKinematicsJacobian(const double *j, memset(jac, 0, EMCMOT_MAX_JOINTS * EMCMOT_MAX_AXIS * sizeof(jac[0][0])); - // tdrKinematicsInverse() differentiated: its coefficients of qx, qy - // and qz for the linear columns, and the same terms with a or b - // advanced a quarter turn for the rotary columns + // tdr_inverse() differentiated: its coefficients of qx, qy and qz for + // the linear columns, and the same terms with a or b advanced a + // quarter turn for the rotary columns jac[0][0] = cb; jac[0][1] = sa*sb; jac[0][2] = -ca*sb; @@ -214,33 +194,42 @@ static int tdrKinematicsJacobian(const double *j, jac[3][3] = 1; jac[4][4] = 1; return 0; -} // tdrKinematicsJacobian() +} // tdr_jacobian() + +static const kins_ops tdr_ops = { + .forward = tdr_forward, + .inverse = tdr_inverse, + .jacobian = tdr_jacobian, +}; + +int switchkinsSetup(kparms* kp, + KS* kset0, KS* kset1, KS* kset2, + KF* kfwd0, KF* kfwd1, KF* kfwd2, + KI* kinv0, KI* kinv1, KI* kinv2 + ) +{ + (void)kset0; (void)kset1; (void)kset2; + (void)kfwd0; (void)kfwd1; (void)kfwd2; + (void)kinv0; (void)kinv1; (void)kinv2; + kp->kinsname = "xyzab_tdr_kins"; + kp->halprefix = "xyzab_tdr_kins"; + kp->required_coordinates = "xyzab"; + kp->allow_duplicates = 0; + kp->max_joints = strlen(kp->required_coordinates); + kp->params = tdr_params; + kp->nparams = sizeof(tdr_params)/sizeof(tdr_params[0]); + + switchkinsRegisterOps(0, &KINS_IDENTITY_OPS); + switchkinsRegisterOps(1, &tdr_ops); + return 0; +} // switchkinsSetup() // halcompile has done hal_init() and does hal_ready() after this returns, // which is what switchkinsInit() expects EXTRA_SETUP() { - kparms kp = {0}; + kparms kp; (void)__comp_inst; (void)prefix; (void)extra_arg; - kp.kinsname = "xyzab_tdr_kins"; - kp.halprefix = "xyzab_tdr_kins"; - kp.required_coordinates = "xyzab"; - kp.allow_duplicates = 0; - kp.fwd_iterates_mask = 0; - kp.gui_kinstype = -1; - kp.sparm = NULL; - kp.max_joints = strlen(kp.required_coordinates); - - if (switchkinsRegister(0, identityKinematicsSetup, - identityKinematicsForward, - identityKinematicsInverse)) { return -1; } - if (switchkinsRegister(1, tdrKinematicsSetup, - tdrKinematicsForward, - tdrKinematicsInverse)) { return -1; } - if (switchkinsRegisterJacobian(1, tdrKinematicsJacobian)) { return -1; } - - if (switchkinsDeclare(0, KINSTYPE_IDENTITY)) { return -1; } - if (switchkinsDeclare(1, KINSTYPE_PRIMARY)) { return -1; } - + if (switchkinsRunSetup(&kp, NULL)) { return -1; } return switchkinsInit(comp_id, &kp, coordinates); } // EXTRA_SETUP() diff --git a/src/hal/components/xyzacb_trsrn.comp b/src/hal/components/xyzacb_trsrn.comp index ad76e1bacb5..858d5864ec3 100644 --- a/src/hal/components/xyzacb_trsrn.comp +++ b/src/hal/components/xyzacb_trsrn.comp @@ -25,85 +25,46 @@ author "David Mueller"; static char *coordinates; RTAPI_MP_STRING(coordinates, "Axes-to-joints-ordering"); -static struct haldata { - // these should be parameters really but we want to be able to - // change them for demonstration purposes - hal_real_t y_pivot; - hal_real_t z_pivot; - hal_real_t x_offset; - hal_real_t y_offset; - hal_real_t y_rot_axis; - hal_real_t z_rot_axis; - hal_real_t pre_rot; - hal_real_t nut_angle; - hal_real_t prim_angle; - hal_real_t sec_angle; - - // Parameters used for xyzacb_trsrn kinematics: - - // Declare hal pin pointers used for xyzacb_trsrn kinematics: - - hal_real_t tool_offset_z; -} *haldata; - -// the pins are shared by the TCP and TOOL kinematics; the TOOL type has -// no setup routine of its own -static int trsrnKinematicsSetup(const int comp_id, - const char* coords, - kparms* kp) -{ - int res = 0; - (void)coords; - - haldata = hal_malloc(sizeof(struct haldata)); - if (!haldata) return -1; - - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->tool_offset_z, 0.0, "%s.tool-offset-z" ,kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->y_pivot, 0.0, "%s.y-pivot" ,kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->z_pivot, 0.0, "%s.z-pivot" ,kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->x_offset, 0.0, "%s.x-offset" ,kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->y_offset, 0.0, "%s.y-offset" ,kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->y_rot_axis, 0.0, "%s.y-rot-axis" ,kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->z_rot_axis, 0.0, "%s.z-rot-axis" ,kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->pre_rot, 0.0, "%s.pre-rot" ,kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->nut_angle, 0.0, "%s.nut-angle" ,kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->prim_angle, 0.0, "%s.primary-angle" ,kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->sec_angle, 0.0, "%s.secondary-angle" ,kp->halprefix); - if (res) return -1; - - return 0; -} // trsrnKinematicsSetup() - -static int toolKinematicsSetup(const int comp_id, - const char* coords, - kparms* kp) -{ - (void)comp_id; - (void)coords; - (void)kp; - return 0; // pins created by trsrnKinematicsSetup() -} // toolKinematicsSetup() +// The geometry of the universal spindle head, one pin each, shared by the +// TCP and TOOL kinematics; the maths reads it from the block (see +// kinematics.h) and the tool length from p->tool.tran.z. The two angle +// pins are what the TOOL kinematics uses in place of the head joints: +// the remap writes them. +static const kins_param_desc trsrn_params[] = { + { "tool-offset-z", KINS_PARAM_FLOAT, KINS_IN, 1, 0.0 }, + { "y-pivot", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "z-pivot", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "x-offset", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "y-offset", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "y-rot-axis", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "z-rot-axis", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "pre-rot", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "nut-angle", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "primary-angle", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "secondary-angle", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, +}; +enum { P_TOOL, P_PIVOT, P_ZPIVOT, P_XO, P_YO, P_ROT_AXIS, P_ZROT_AXIS, + P_PRE_ROT, P_NUT, P_PRIM, P_SEC }; + +// geometric offsets of the universal spindle head as defined in the ini file +#define GEOMETRY(p) \ + const double Ly = (p)->geometry[P_PIVOT]; \ + const double Lz = (p)->geometry[P_ZPIVOT]; \ + const double Dx = (p)->geometry[P_XO]; \ + const double Dy = (p)->geometry[P_YO]; \ + const double Dray = (p)->geometry[P_ROT_AXIS] - (Dy + Ly); \ + const double Draz = (p)->geometry[P_ZROT_AXIS] - Lz; \ + const double tc = (p)->geometry[P_PRE_ROT]; \ + const double nu = (p)->geometry[P_NUT]; /* degrees */ \ + const double theta_1 = (p)->geometry[P_PRIM]; /* degrees */ \ + const double theta_2 = (p)->geometry[P_SEC]; /* degrees */ \ + const double Dt = (p)->tool.tran.z /* tool-length offset if G43 is used */ // tool_kins==0: TCP kinematics, using the current spindle joint positions // tool_kins==1: TOOL kinematics, using the angles calculated in remap.py -static int trsrnForward(const double *j, EmcPose * pos, int tool_kins) +static int trsrnForward(const kins_params *p, const double *j, EmcPose * pos, int tool_kins) { - // START of custom variable declaration for Forward kinematics - - // geometric offsets of the universal spindle head as defined in the ini file - double Ly = hal_get_real(haldata->y_pivot); - double Lz = hal_get_real(haldata->z_pivot); - double Dx = hal_get_real(haldata->x_offset); - double Dy = hal_get_real(haldata->y_offset); - double Dray = hal_get_real(haldata->y_rot_axis) - (Dy + Ly); - double Draz = hal_get_real(haldata->z_rot_axis) - Lz; - double tc = hal_get_real(haldata->pre_rot); - double nu = hal_get_real(haldata->nut_angle); // degrees - double theta_1 = hal_get_real(haldata->prim_angle); // degrees - double theta_2 = hal_get_real(haldata->sec_angle); // degrees - - // tool-length offset if G43 is used (offset as defined in the tool editor) - double Dt = hal_get_real(haldata->tool_offset_z); + GEOMETRY(p); // variables used in both, TCP and TOOL kinematics double Sw = sin(j[3]*TO_RAD); @@ -130,8 +91,6 @@ static int trsrnForward(const double *j, EmcPose * pos, int tool_kins) double Py = j[1]; double Pz = j[2]; - // END of custom variable declaration for Forward kinematics - if (!tool_kins) { // ========================= TCP kinematics FORWARD // in TCP we use the current positions of the spindle joints Ss = sin(j[4]*TO_RAD); @@ -144,32 +103,32 @@ static int trsrnForward(const double *j, EmcPose * pos, int tool_kins) s = Cs + Cv*Cv*(1-Cs); t = Sv*Cv*(1-Cs); - pos->tran.x = - (Cp*SvSs - Sp*t)*(Dt + Lz) - - Cp*Dx - + (Cp*CvSs + Sp*r)*Ly - + Dy*Sp - + Dx + pos->tran.x = - (Cp*SvSs - Sp*t)*(Dt + Lz) + - Cp*Dx + + (Cp*CvSs + Sp*r)*Ly + + Dy*Sp + + Dx + Px; - pos->tran.y = - Cp*Cw*Dy - - Cw*Dx*Sp - - Cw*(Dray - Py) - - (Cw*Sp*SvSs + Cp*Cw*t - Sw*s)*(Dt + Lz) - + (CvSs*Cw*Sp - Cp*Cw*r + Sw*t)*Ly - + (Draz - Pz)*Sw - + Dray - + Dy + pos->tran.y = - Cp*Cw*Dy + - Cw*Dx*Sp + - Cw*(Dray - Py) + - (Cw*Sp*SvSs + Cp*Cw*t - Sw*s)*(Dt + Lz) + + (CvSs*Cw*Sp - Cp*Cw*r + Sw*t)*Ly + + (Draz - Pz)*Sw + + Dray + + Dy + Ly; - pos->tran.z = - Cp*Dy*Sw - - Dx*Sp*Sw - - Cw*(Draz - Pz) - - (Sp*SvSs*Sw + Cp*Sw*t + Cw*s)*(Dt + Lz) - + (CvSs*Sp*Sw - Cp*Sw*r - Cw*t)*Ly - - (Dray - Py)*Sw - + Draz - + Dt - + Lz; + pos->tran.z = - Cp*Dy*Sw + - Dx*Sp*Sw + - Cw*(Draz - Pz) + - (Sp*SvSs*Sw + Cp*Sw*t + Cw*s)*(Dt + Lz) + + (CvSs*Sp*Sw - Cp*Sw*r - Cw*t)*Ly + - (Dray - Py)*Sw + + Draz + + Dt + + Lz; pos->a = j[3]; pos->b = j[4]; @@ -187,27 +146,27 @@ static int trsrnForward(const double *j, EmcPose * pos, int tool_kins) s = Cs + Cv*Cv*(1-Cs); t = Sv*Cv*(1-Cs); - pos->tran.x = ((Cs*Ctc - CvSs*Stc)*Cp - (Ctc*CvSs + Stc*r)*Sp)*(Dx + Px) - - (Cs*Ctc - CvSs*Stc)*Dx - + ((Ctc*CvSs + Stc*r)*Cp - + (Cs*Ctc - CvSs*Stc)*Sp)*(Dy + Ly + Py) - - (Ctc*CvSs + Stc*r)*Dy - - (Ctc*SvSs - Stc*t)*(Lz + Pz) + pos->tran.x = ((Cs*Ctc - CvSs*Stc)*Cp - (Ctc*CvSs + Stc*r)*Sp)*(Dx + Px) + - (Cs*Ctc - CvSs*Stc)*Dx + + ((Ctc*CvSs + Stc*r)*Cp + + (Cs*Ctc - CvSs*Stc)*Sp)*(Dy + Ly + Py) + - (Ctc*CvSs + Stc*r)*Dy + - (Ctc*SvSs - Stc*t)*(Lz + Pz) - Ly*Stc; - pos->tran.y = - ((Ctc*CvSs + Cs*Stc)*Cp - (CvSs*Stc - Ctc*r)*Sp)*(Dx + Px) - + (Ctc*CvSs + Cs*Stc)*Dx - - ((CvSs*Stc - Ctc*r)*Cp - + (Ctc*CvSs + Cs*Stc)*Sp)*(Dy + Ly + Py) - + (CvSs*Stc - Ctc*r)*Dy - - Ctc*Ly + pos->tran.y = - ((Ctc*CvSs + Cs*Stc)*Cp - (CvSs*Stc - Ctc*r)*Sp)*(Dx + Px) + + (Ctc*CvSs + Cs*Stc)*Dx + - ((CvSs*Stc - Ctc*r)*Cp + + (Ctc*CvSs + Cs*Stc)*Sp)*(Dy + Ly + Py) + + (CvSs*Stc - Ctc*r)*Dy + - Ctc*Ly + (Stc*SvSs + Ctc*t)*(Lz + Pz); - pos->tran.z = (Cp*SvSs - Sp*t)*(Dx + Px) - + (Sp*SvSs + Cp*t)*(Dy + Ly + Py) - - Dx*SvSs - + (Lz + Pz)*s - - Dy*t + pos->tran.z = (Cp*SvSs - Sp*t)*(Dx + Px) + + (Sp*SvSs + Cp*t)*(Dy + Ly + Py) + - Dx*SvSs + + (Lz + Pz)*s + - Dy*t - Lz; pos->a = j[3]; @@ -222,47 +181,35 @@ static int trsrnForward(const double *j, EmcPose * pos, int tool_kins) return 0; } // trsrnForward() -static int tcpKinematicsForward(const double *j, +static int tcpKinematicsForward(const kins_params *p, kins_scratch *s, + const double *j, EmcPose * pos, const KINEMATICS_FORWARD_FLAGS * fflags, KINEMATICS_INVERSE_FLAGS * iflags) { + (void)s; (void)fflags; (void)iflags; - return trsrnForward(j, pos, 0); + return trsrnForward(p, j, pos, 0); } // tcpKinematicsForward() -static int toolKinematicsForward(const double *j, +static int toolKinematicsForward(const kins_params *p, kins_scratch *s, + const double *j, EmcPose * pos, const KINEMATICS_FORWARD_FLAGS * fflags, KINEMATICS_INVERSE_FLAGS * iflags) { + (void)s; (void)fflags; (void)iflags; - return trsrnForward(j, pos, 1); + return trsrnForward(p, j, pos, 1); } // toolKinematicsForward() -static int trsrnInverse(const EmcPose * pos, double *j, int tool_kins) +// The inverses read the rotary angles from the joint argument, where the +// machine is, as they always have. +static int trsrnInverse(const kins_params *p, const EmcPose * pos, double *j, int tool_kins) { - // START of custom variable declaration for Forward kinematics - - // geometric offsets of the universal spindle head as defined in the ini file - double Ly = hal_get_real(haldata->y_pivot); - double Lz = hal_get_real(haldata->z_pivot); - double Dx = hal_get_real(haldata->x_offset); - double Dy = hal_get_real(haldata->y_offset); - double Dray = hal_get_real(haldata->y_rot_axis) - (Dy + Ly); - double Draz = hal_get_real(haldata->z_rot_axis) - Lz; - double tc = hal_get_real(haldata->pre_rot); - double nu = hal_get_real(haldata->nut_angle); // degrees - double theta_1 = hal_get_real(haldata->prim_angle); // degrees - double theta_2 = hal_get_real(haldata->sec_angle); // degrees - - // tool-length offset if G43 is used (offset as defined in the tool editor) - double Dt = hal_get_real(haldata->tool_offset_z); - - // substitutions as used in mathematical documentation - // including degree -> radians angle conversion + GEOMETRY(p); // variables used in both, TCP and TOOL kinematics double Sw = sin(j[3]*TO_RAD); @@ -271,7 +218,7 @@ static int trsrnInverse(const EmcPose * pos, double *j, int tool_kins) double Cv = cos(nu*TO_RAD); double Stc = sin(tc); double Ctc = cos(tc); - + // in TCP we use the current positions of the spindle joints // in TOOL we will use the angle values calculated in remap.py double Ss = 0; @@ -286,10 +233,8 @@ static int trsrnInverse(const EmcPose * pos, double *j, int tool_kins) // onLy used to be consistent with math in documentation double Qx = pos->tran.x; - double Qy = pos->tran.y; - double Qz = pos->tran.z; - - // END of custom variable declaration for Forward kinematics + double Qy = pos->tran.y; + double Qz = pos->tran.z; if (!tool_kins) { // ========================= TCP kinematics INVERSE // in TCP we use the current positions of the spindle joints @@ -303,25 +248,25 @@ static int trsrnInverse(const EmcPose * pos, double *j, int tool_kins) s = Cs + Cv*Cv*(1-Cs); t = Sv*Cv*(1-Cs); - j[0] = (Cp*SvSs - Sp*t)*(Dt + Lz) - + Cp*Dx - - (Cp*CvSs + Sp*r)*Ly - - Dy*Sp - - Dx + j[0] = (Cp*SvSs - Sp*t)*(Dt + Lz) + + Cp*Dx + - (Cp*CvSs + Sp*r)*Ly + - Dy*Sp + - Dx + Qx; - j[1] = Cp*Dy - + Dx*Sp - - Cw*(Dray + Dy + Ly - Qy) - + (Sp*SvSs + Cp*t)*(Dt + Lz) - - (CvSs*Sp - Cp*r)*Ly - - (Draz + Dt + Lz - Qz)*Sw + j[1] = Cp*Dy + + Dx*Sp + - Cw*(Dray + Dy + Ly - Qy) + + (Sp*SvSs + Cp*t)*(Dt + Lz) + - (CvSs*Sp - Cp*r)*Ly + - (Draz + Dt + Lz - Qz)*Sw + Dray; - j[2] = (Dt + Lz)*s - + Ly*t - - Cw*(Draz + Dt + Lz - Qz) - + (Dray + Dy + Ly - Qy)*Sw + j[2] = (Dt + Lz)*s + + Ly*t + - Cw*(Draz + Dt + Lz - Qz) + + (Dray + Dy + Ly - Qy)*Sw + Draz; j[3] = pos->a; @@ -329,80 +274,84 @@ static int trsrnInverse(const EmcPose * pos, double *j, int tool_kins) j[5] = pos->c; } else { // ========================= TOOL kinematics INVERSE - // in TOOL kinematics we use the articulated joint positions from the TWP - Ss = sin(theta_2*TO_RAD); - Cs = cos(theta_2*TO_RAD); - Sp = sin(theta_1*TO_RAD); - Cp = cos(theta_1*TO_RAD); - CvSs = Cv*Ss; - SvSs = Sv*Ss; - r = Cs + Sv*Sv*(1-Cs); - s = Cs + Cv*Cv*(1-Cs); - t = Sv*Cv*(1-Cs); - - j[0] = Cp*Dx - - (Cp*CvSs + Sp*r)*Ly - + (Cp*SvSs - Sp*t)*Lz - + ((Cp*Cs - CvSs*Sp)*Ctc - - (Cp*CvSs + Sp*r)*Stc)*Qx - - ((Cp*CvSs + Sp*r)*Ctc + (Cp*Cs - CvSs*Sp)*Stc)*Qy - + (Cp*SvSs - Sp*t)*Qz - - Dy*Sp - - Dx; - - j[1] = Cp*Dy - - (CvSs*Sp - Cp*r)*Ly - + (Sp*SvSs + Cp*t)*Lz - + ((Cp*CvSs + Cs*Sp)*Ctc - (CvSs*Sp - Cp*r)*Stc)*Qx - - ((CvSs*Sp - Cp*r)*Ctc + (Cp*CvSs + Cs*Sp)*Stc)*Qy - + (Sp*SvSs + Cp*t)*Qz - + Dx*Sp - - Dy - - Ly; - - j[2] = - (Ctc*SvSs - Stc*t)*Qx - + (Stc*SvSs + Ctc*t)*Qy - + Lz*s - + Qz*s - + Ly*t - - Lz; - - j[3] = pos->a; - j[4] = pos->b; - j[5] = pos->c; + // in TOOL kinematics we use the articulated joint positions from the TWP + Ss = sin(theta_2*TO_RAD); + Cs = cos(theta_2*TO_RAD); + Sp = sin(theta_1*TO_RAD); + Cp = cos(theta_1*TO_RAD); + CvSs = Cv*Ss; + SvSs = Sv*Ss; + r = Cs + Sv*Sv*(1-Cs); + s = Cs + Cv*Cv*(1-Cs); + t = Sv*Cv*(1-Cs); + + j[0] = Cp*Dx + - (Cp*CvSs + Sp*r)*Ly + + (Cp*SvSs - Sp*t)*Lz + + ((Cp*Cs - CvSs*Sp)*Ctc + - (Cp*CvSs + Sp*r)*Stc)*Qx + - ((Cp*CvSs + Sp*r)*Ctc + (Cp*Cs - CvSs*Sp)*Stc)*Qy + + (Cp*SvSs - Sp*t)*Qz + - Dy*Sp + - Dx; + + j[1] = Cp*Dy + - (CvSs*Sp - Cp*r)*Ly + + (Sp*SvSs + Cp*t)*Lz + + ((Cp*CvSs + Cs*Sp)*Ctc - (CvSs*Sp - Cp*r)*Stc)*Qx + - ((CvSs*Sp - Cp*r)*Ctc + (Cp*CvSs + Cs*Sp)*Stc)*Qy + + (Sp*SvSs + Cp*t)*Qz + + Dx*Sp + - Dy + - Ly; + + j[2] = - (Ctc*SvSs - Stc*t)*Qx + + (Stc*SvSs + Ctc*t)*Qy + + Lz*s + + Qz*s + + Ly*t + - Lz; + + j[3] = pos->a; + j[4] = pos->b; + j[5] = pos->c; } return 0; } // trsrnInverse() -static int tcpKinematicsInverse(const EmcPose * pos, +static int tcpKinematicsInverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, double *j, const KINEMATICS_INVERSE_FLAGS * iflags, KINEMATICS_FORWARD_FLAGS * fflags) { + (void)s; (void)iflags; (void)fflags; - return trsrnInverse(pos, j, 0); + return trsrnInverse(p, pos, j, 0); } // tcpKinematicsInverse() -static int toolKinematicsInverse(const EmcPose * pos, +static int toolKinematicsInverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, double *j, const KINEMATICS_INVERSE_FLAGS * iflags, KINEMATICS_FORWARD_FLAGS * fflags) { + (void)s; (void)iflags; (void)fflags; - return trsrnInverse(pos, j, 1); + return trsrnInverse(p, pos, j, 1); } // toolKinematicsInverse() // The head answers in the convention already, so the native rotation -// registered with these frames is TOOL_FRAME_SPINDLE. -static int tcpKinematicsToolFrame(const double *j, +// declared with these frames is TOOL_FRAME_SPINDLE. +static int tcpKinematicsToolFrame(const kins_params *p, const double *j, PmRotationMatrix *rot, const KINEMATICS_FORWARD_FLAGS *fflags) { (void)fflags; - double nu = hal_get_real(haldata->nut_angle); // degrees + double nu = p->geometry[P_NUT]; // degrees double Sv = sin(nu*TO_RAD); double Cv = cos(nu*TO_RAD); double Ss = sin(j[4]*TO_RAD); @@ -437,10 +386,11 @@ static int tcpKinematicsToolFrame(const double *j, return 0; } // tcpKinematicsToolFrame() -static int tcpKinematicsWorkFrame(const double *j, +static int tcpKinematicsWorkFrame(const kins_params *p, const double *j, PmRotationMatrix *rot, const KINEMATICS_FORWARD_FLAGS *fflags) { + (void)p; (void)fflags; double Sw = sin(j[3]*TO_RAD); double Cw = cos(j[3]*TO_RAD); @@ -456,23 +406,15 @@ static int tcpKinematicsWorkFrame(const double *j, return 0; } // tcpKinematicsWorkFrame() -static int tcpKinematicsJacobian(const double *j, +static int tcpKinematicsJacobian(const kins_params *p, const double *j, const EmcPose * pos, double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], const KINEMATICS_INVERSE_FLAGS * iflags) { (void)j; (void)iflags; - - // the same geometry as trsrnInverse(), read the same way - double Ly = hal_get_real(haldata->y_pivot); - double Lz = hal_get_real(haldata->z_pivot); - double Dx = hal_get_real(haldata->x_offset); - double Dy = hal_get_real(haldata->y_offset); - double Dray = hal_get_real(haldata->y_rot_axis) - (Dy + Ly); - double Draz = hal_get_real(haldata->z_rot_axis) - Lz; - double nu = hal_get_real(haldata->nut_angle); // degrees - double Dt = hal_get_real(haldata->tool_offset_z); + GEOMETRY(p); + (void)tc; (void)theta_1; (void)theta_2; double Sv = sin(nu*TO_RAD); double Cv = cos(nu*TO_RAD); @@ -536,7 +478,7 @@ static int tcpKinematicsJacobian(const double *j, return 0; } // tcpKinematicsJacobian() -static int toolKinematicsJacobian(const double *j, +static int toolKinematicsJacobian(const kins_params *p, const double *j, const EmcPose * pos, double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], const KINEMATICS_INVERSE_FLAGS * iflags) @@ -547,10 +489,10 @@ static int toolKinematicsJacobian(const double *j, // the head angles come from pins, so the inverse is linear in the pose // and the rows are its coefficients - double tc = hal_get_real(haldata->pre_rot); - double nu = hal_get_real(haldata->nut_angle); // degrees - double theta_1 = hal_get_real(haldata->prim_angle); // degrees - double theta_2 = hal_get_real(haldata->sec_angle); // degrees + double tc = p->geometry[P_PRE_ROT]; + double nu = p->geometry[P_NUT]; // degrees + double theta_1 = p->geometry[P_PRIM]; // degrees + double theta_2 = p->geometry[P_SEC]; // degrees double Sv = sin(nu*TO_RAD); double Cv = cos(nu*TO_RAD); @@ -586,43 +528,55 @@ static int toolKinematicsJacobian(const double *j, return 0; } // toolKinematicsJacobian() +static const kins_ops tcp_ops = { + .forward = tcpKinematicsForward, + .inverse = tcpKinematicsInverse, + .work = tcpKinematicsWorkFrame, + .tool = tcpKinematicsToolFrame, + .native = &TOOL_FRAME_SPINDLE, + .jacobian = tcpKinematicsJacobian, +}; + +// the tool kinematics report in tool axes, so the tool is square with the +// world by construction and nothing turns the work against it +static const kins_ops tool_ops = { + .forward = toolKinematicsForward, + .inverse = toolKinematicsInverse, + .work = kinsIdentityFrame, + .tool = kinsIdentityFrame, + .native = &TOOL_FRAME_SPINDLE, + .jacobian = toolKinematicsJacobian, +}; + +int switchkinsSetup(kparms* kp, + KS* kset0, KS* kset1, KS* kset2, + KF* kfwd0, KF* kfwd1, KF* kfwd2, + KI* kinv0, KI* kinv1, KI* kinv2 + ) +{ + (void)kset0; (void)kset1; (void)kset2; + (void)kfwd0; (void)kfwd1; (void)kfwd2; + (void)kinv0; (void)kinv1; (void)kinv2; + kp->kinsname = "xyzacb_trsrn"; + kp->halprefix = "xyzacb_trsrn_kins"; + kp->required_coordinates = "xyzabc"; + kp->allow_duplicates = 0; + kp->max_joints = strlen(kp->required_coordinates); + kp->params = trsrn_params; + kp->nparams = sizeof(trsrn_params)/sizeof(trsrn_params[0]); + + switchkinsRegisterOps(0, &KINS_IDENTITY_OPS); + switchkinsRegisterOps(1, &tcp_ops); + switchkinsRegisterOps(2, &tool_ops); + return 0; +} // switchkinsSetup() + // halcompile has done hal_init() and does hal_ready() after this returns, // which is what switchkinsInit() expects EXTRA_SETUP() { - kparms kp = {0}; + kparms kp; (void)__comp_inst; (void)prefix; (void)extra_arg; - kp.kinsname = "xyzacb_trsrn"; - kp.halprefix = "xyzacb_trsrn_kins"; - kp.required_coordinates = "xyzabc"; - kp.allow_duplicates = 0; - kp.fwd_iterates_mask = 0; - kp.gui_kinstype = -1; - kp.sparm = NULL; - kp.max_joints = strlen(kp.required_coordinates); - - if (switchkinsRegister(0, identityKinematicsSetup, - identityKinematicsForward, - identityKinematicsInverse)) { return -1; } - if (switchkinsRegister(1, trsrnKinematicsSetup, - tcpKinematicsForward, - tcpKinematicsInverse)) { return -1; } - if (switchkinsRegister(2, toolKinematicsSetup, - toolKinematicsForward, - toolKinematicsInverse)) { return -1; } - if (switchkinsRegisterFrames(1, tcpKinematicsWorkFrame, - tcpKinematicsToolFrame, - &TOOL_FRAME_SPINDLE)) { return -1; } - if (switchkinsRegisterJacobian(1, tcpKinematicsJacobian)) { return -1; } - // the tool kinematics report in tool axes, so the tool is square with - // the world by construction and nothing turns the work against it - if (switchkinsRegisterFrames(2, identityKinematicsWorkFrame, - identityKinematicsToolFrame, - &TOOL_FRAME_SPINDLE)) { return -1; } - if (switchkinsRegisterJacobian(2, toolKinematicsJacobian)) { return -1; } - - if (switchkinsDeclare(0, KINSTYPE_IDENTITY)) { return -1; } - if (switchkinsDeclare(1, KINSTYPE_PRIMARY)) { return -1; } - + if (switchkinsRunSetup(&kp, NULL)) { return -1; } return switchkinsInit(comp_id, &kp, coordinates); } // EXTRA_SETUP() diff --git a/src/hal/components/xyzbca_trsrn.comp b/src/hal/components/xyzbca_trsrn.comp index b3e8ad89dd9..6691a752075 100644 --- a/src/hal/components/xyzbca_trsrn.comp +++ b/src/hal/components/xyzbca_trsrn.comp @@ -25,85 +25,46 @@ author "David Mueller"; static char *coordinates; RTAPI_MP_STRING(coordinates, "Axes-to-joints-ordering"); -static struct haldata { - // these should be parameters really but we want to be able to - // change them for demonstration purposes - hal_real_t x_pivot; - hal_real_t z_pivot; - hal_real_t x_offset; - hal_real_t y_offset; - hal_real_t x_rot_axis; - hal_real_t z_rot_axis; - hal_real_t pre_rot; - hal_real_t nut_angle; - hal_real_t prim_angle; - hal_real_t sec_angle; - - // Parameters used for xyzbca_trsrn kinematics: - - // Declare hal pin pointers used for xyzbca_trsrn kinematics: - - hal_real_t tool_offset_z; -} *haldata; - -// the pins are shared by the TCP and TOOL kinematics; the TOOL type has -// no setup routine of its own -static int trsrnKinematicsSetup(const int comp_id, - const char* coords, - kparms* kp) -{ - int res = 0; - (void)coords; - - haldata = hal_malloc(sizeof(struct haldata)); - if (!haldata) return -1; - - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->tool_offset_z, 0.0, "%s.tool-offset-z", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->x_pivot, 0.0, "%s.x-pivot", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->z_pivot, 0.0, "%s.z-pivot", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->x_offset, 0.0, "%s.x-offset", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->y_offset, 0.0, "%s.y-offset", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->x_rot_axis, 0.0, "%s.x-rot-axis", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->z_rot_axis, 0.0, "%s.z-rot-axis", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->pre_rot, 0.0, "%s.pre-rot", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->nut_angle, 0.0, "%s.nut-angle", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->prim_angle, 0.0, "%s.primary-angle", kp->halprefix); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->sec_angle, 0.0, "%s.secondary-angle", kp->halprefix); - if (res) return -1; - - return 0; -} // trsrnKinematicsSetup() - -static int toolKinematicsSetup(const int comp_id, - const char* coords, - kparms* kp) -{ - (void)comp_id; - (void)coords; - (void)kp; - return 0; // pins created by trsrnKinematicsSetup() -} // toolKinematicsSetup() +// The geometry of the universal spindle head, one pin each, shared by the +// TCP and TOOL kinematics; the maths reads it from the block (see +// kinematics.h) and the tool length from p->tool.tran.z. The two angle +// pins are what the TOOL kinematics uses in place of the head joints: +// the remap writes them. +static const kins_param_desc trsrn_params[] = { + { "tool-offset-z", KINS_PARAM_FLOAT, KINS_IN, 1, 0.0 }, + { "x-pivot", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "z-pivot", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "x-offset", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "y-offset", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "x-rot-axis", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "z-rot-axis", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "pre-rot", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "nut-angle", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "primary-angle", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "secondary-angle", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, +}; +enum { P_TOOL, P_PIVOT, P_ZPIVOT, P_XO, P_YO, P_ROT_AXIS, P_ZROT_AXIS, + P_PRE_ROT, P_NUT, P_PRIM, P_SEC }; + +// geometric offsets of the universal spindle head as defined in the ini file +#define GEOMETRY(p) \ + const double Lx = (p)->geometry[P_PIVOT]; \ + const double Lz = (p)->geometry[P_ZPIVOT]; \ + const double Dx = (p)->geometry[P_XO]; \ + const double Dy = (p)->geometry[P_YO]; \ + const double Drax = (p)->geometry[P_ROT_AXIS] - Lx - Dx; \ + const double Draz = (p)->geometry[P_ZROT_AXIS] - Lz; \ + const double tc = (p)->geometry[P_PRE_ROT]; \ + const double nu = (p)->geometry[P_NUT]; /* degrees */ \ + const double theta_1 = (p)->geometry[P_PRIM]; /* degrees */ \ + const double theta_2 = (p)->geometry[P_SEC]; /* degrees */ \ + const double Dt = (p)->tool.tran.z /* tool-length offset if G43 is used */ // tool_kins==0: TCP kinematics, using the current spindle joint positions // tool_kins==1: TOOL kinematics, using the angles calculated in remap.py -static int trsrnForward(const double *j, EmcPose * pos, int tool_kins) +static int trsrnForward(const kins_params *p, const double *j, EmcPose * pos, int tool_kins) { - // START of custom variable declaration for Forward kinematics - - // geometric offsets of the universal spindle head as defined in the ini file - double Lx = hal_get_real(haldata->x_pivot); - double Lz = hal_get_real(haldata->z_pivot); - double Dx = hal_get_real(haldata->x_offset); - double Dy = hal_get_real(haldata->y_offset); - double Drax = hal_get_real(haldata->x_rot_axis) - Lx- Dx; - double Draz = hal_get_real(haldata->z_rot_axis) - Lz; - double tc = hal_get_real(haldata->pre_rot); - double nu = hal_get_real(haldata->nut_angle); // degrees - double theta_1 = hal_get_real(haldata->prim_angle); // degrees - double theta_2 = hal_get_real(haldata->sec_angle); // degrees - - // tool-length offset if G43 is used (offset as defined in the tool editor) - double Dt = hal_get_real(haldata->tool_offset_z); + GEOMETRY(p); // variables used in both, TCP and TOOL kinematics double Sw = sin(j[4]*TO_RAD); @@ -130,9 +91,6 @@ static int trsrnForward(const double *j, EmcPose * pos, int tool_kins) double Py = j[1]; double Pz = j[2]; - // END of custom variable declaration for Forward kinematics - - if (!tool_kins) { // ========================= TCP kinematics FORWARD // in TCP we use the current positions of the spindle joints Ss = sin(j[3]*TO_RAD); @@ -144,37 +102,33 @@ static int trsrnForward(const double *j, EmcPose * pos, int tool_kins) r = Cs + Sv*Sv*(1-Cs); s = Cs + Cv*Cv*(1-Cs); t = Sv*Cv*(1-Cs); - // onLy used to be consistent with math in documentation - Px = j[0]; - Py = j[1]; - Pz = j[2]; - - pos->tran.x = - Cp*Cw*Dx - + Cw*Dy*Sp - - Cw*(Drax - Px) - - (Cw*Sp*SvSs + Cp*Cw*t + Sw*s)*(Dt + Lz) - + (CvSs*Cw*Sp - Cp*Cw*r - Sw*t)*Lx - - (Draz - Pz)*Sw - + Drax - + Dx + + pos->tran.x = - Cp*Cw*Dx + + Cw*Dy*Sp + - Cw*(Drax - Px) + - (Cw*Sp*SvSs + Cp*Cw*t + Sw*s)*(Dt + Lz) + + (CvSs*Cw*Sp - Cp*Cw*r - Sw*t)*Lx + - (Draz - Pz)*Sw + + Drax + + Dx + Lx; - pos->tran.y = (Cp*SvSs - Sp*t)*(Dt + Lz) - - Cp*Dy - - (Cp*CvSs + Sp*r)*Lx - - Dx*Sp - + Dy + pos->tran.y = (Cp*SvSs - Sp*t)*(Dt + Lz) + - Cp*Dy + - (Cp*CvSs + Sp*r)*Lx + - Dx*Sp + + Dy + Py; - pos->tran.z = Cp*Dx*Sw - - Dy*Sp*Sw - - Cw*(Draz - Pz) - + (Sp*SvSs*Sw + Cp*Sw*t - Cw*s)*(Dt + Lz) - - (CvSs*Sp*Sw - Cp*Sw*r + Cw*t)*Lx - + (Drax - Px)*Sw - + Draz - + Dt - + Lz; + pos->tran.z = Cp*Dx*Sw + - Dy*Sp*Sw + - Cw*(Draz - Pz) + + (Sp*SvSs*Sw + Cp*Sw*t - Cw*s)*(Dt + Lz) + - (CvSs*Sp*Sw - Cp*Sw*r + Cw*t)*Lx + + (Drax - Px)*Sw + + Draz + + Dt + + Lz; pos->a = j[3]; pos->b = j[4]; @@ -192,27 +146,25 @@ static int trsrnForward(const double *j, EmcPose * pos, int tool_kins) s = Cs + Cv*Cv*(1-Cs); t = Sv*Cv*(1-Cs); - pos->tran.x = - ((CvSs*Stc - Ctc*r)*Cp + (Ctc*CvSs + Cs*Stc)*Sp)*(Dx + Lx + Px) - + (CvSs*Stc - Ctc*r)*Dx - + ((Ctc*CvSs + Cs*Stc)*Cp - (CvSs*Stc - Ctc*r)*Sp)*(Dy + Py) - - (Ctc*CvSs + Cs*Stc)*Dy - - Ctc*Lx + pos->tran.x = - ((CvSs*Stc - Ctc*r)*Cp + (Ctc*CvSs + Cs*Stc)*Sp)*(Dx + Lx + Px) + + (CvSs*Stc - Ctc*r)*Dx + + ((Ctc*CvSs + Cs*Stc)*Cp - (CvSs*Stc - Ctc*r)*Sp)*(Dy + Py) + - (Ctc*CvSs + Cs*Stc)*Dy + - Ctc*Lx + (Stc*SvSs + Ctc*t)*(Lz + Pz); - - pos->tran.y = - ((Ctc*CvSs + Stc*r)*Cp + (Cs*Ctc - CvSs*Stc)*Sp)*(Dx + Lx + Px) - + (Ctc*CvSs + Stc*r)*Dx - + ((Cs*Ctc - CvSs*Stc)*Cp - (Ctc*CvSs + Stc*r)*Sp)*(Dy + Py) - - (Cs*Ctc - CvSs*Stc)*Dy - + (Ctc*SvSs - Stc*t)*(Lz + Pz) + pos->tran.y = - ((Ctc*CvSs + Stc*r)*Cp + (Cs*Ctc - CvSs*Stc)*Sp)*(Dx + Lx + Px) + + (Ctc*CvSs + Stc*r)*Dx + + ((Cs*Ctc - CvSs*Stc)*Cp - (Ctc*CvSs + Stc*r)*Sp)*(Dy + Py) + - (Cs*Ctc - CvSs*Stc)*Dy + + (Ctc*SvSs - Stc*t)*(Lz + Pz) + Lx*Stc; - - pos->tran.z = (Sp*SvSs + Cp*t)*(Dx + Lx + Px) - - (Cp*SvSs - Sp*t)*(Dy + Py) - + Dy*SvSs - + (Lz + Pz)*s - - Dx*t + pos->tran.z = (Sp*SvSs + Cp*t)*(Dx + Lx + Px) + - (Cp*SvSs - Sp*t)*(Dy + Py) + + Dy*SvSs + + (Lz + Pz)*s + - Dx*t - Lz; pos->a = j[3]; @@ -227,44 +179,35 @@ static int trsrnForward(const double *j, EmcPose * pos, int tool_kins) return 0; } // trsrnForward() -static int tcpKinematicsForward(const double *j, +static int tcpKinematicsForward(const kins_params *p, kins_scratch *s, + const double *j, EmcPose * pos, const KINEMATICS_FORWARD_FLAGS * fflags, KINEMATICS_INVERSE_FLAGS * iflags) { + (void)s; (void)fflags; (void)iflags; - return trsrnForward(j, pos, 0); + return trsrnForward(p, j, pos, 0); } // tcpKinematicsForward() -static int toolKinematicsForward(const double *j, +static int toolKinematicsForward(const kins_params *p, kins_scratch *s, + const double *j, EmcPose * pos, const KINEMATICS_FORWARD_FLAGS * fflags, KINEMATICS_INVERSE_FLAGS * iflags) { + (void)s; (void)fflags; (void)iflags; - return trsrnForward(j, pos, 1); + return trsrnForward(p, j, pos, 1); } // toolKinematicsForward() -static int trsrnInverse(const EmcPose * pos, double *j, int tool_kins) +// The inverses read the rotary angles from the joint argument, where the +// machine is, as they always have. +static int trsrnInverse(const kins_params *p, const EmcPose * pos, double *j, int tool_kins) { - // START of custom variable declaration for Forward kinematics - - // geometric offsets of the universal spindle head as defined in the ini file - double Lx = hal_get_real(haldata->x_pivot); - double Lz = hal_get_real(haldata->z_pivot); - double Dx = hal_get_real(haldata->x_offset); - double Dy = hal_get_real(haldata->y_offset); - double Drax = hal_get_real(haldata->x_rot_axis) - Lx - Dx; - double Draz = hal_get_real(haldata->z_rot_axis) - Lz; - double tc = hal_get_real(haldata->pre_rot); - double nu = hal_get_real(haldata->nut_angle); // degrees - double theta_1 = hal_get_real(haldata->prim_angle); // degrees - double theta_2 = hal_get_real(haldata->sec_angle); // degrees - - // tool-length offset if G43 is used (offset as defined in the tool editor) - double Dt = hal_get_real(haldata->tool_offset_z); + GEOMETRY(p); // variables used in both, TCP and TOOL kinematics double Sw = sin(j[4]*TO_RAD); @@ -288,11 +231,8 @@ static int trsrnInverse(const EmcPose * pos, double *j, int tool_kins) // onLy used to be consistent with math in documentation double Qx = pos->tran.x; - double Qy = pos->tran.y; - double Qz = pos->tran.z; - - // END of custom variable declaration for Forward kinematics - + double Qy = pos->tran.y; + double Qz = pos->tran.z; if (!tool_kins) { // ========================= TCP kinematics INVERSE // in TCP we use the current positions of the spindle joints @@ -304,27 +244,27 @@ static int trsrnInverse(const EmcPose * pos, double *j, int tool_kins) SvSs = Sv*Ss; r = Cs + Sv*Sv*(1-Cs); s = Cs + Cv*Cv*(1-Cs); - t = Sv*Cv*(1-Cs); - - j[0] = Cp*Dx - - Dy*Sp - - Cw*(Drax + Dx + Lx - Qx) - + (Sp*SvSs + Cp*t)*(Dt + Lz) - - (CvSs*Sp - Cp*r)*Lx - + (Draz + Dt + Lz - Qz)*Sw + t = Sv*Cv*(1-Cs); + + j[0] = Cp*Dx + - Dy*Sp + - Cw*(Drax + Dx + Lx - Qx) + + (Sp*SvSs + Cp*t)*(Dt + Lz) + - (CvSs*Sp - Cp*r)*Lx + + (Draz + Dt + Lz - Qz)*Sw + Drax; - j[1] = - (Cp*SvSs - Sp*t)*(Dt + Lz) - + Cp*Dy - + (Cp*CvSs + Sp*r)*Lx - + Dx*Sp - - Dy + j[1] = - (Cp*SvSs - Sp*t)*(Dt + Lz) + + Cp*Dy + + (Cp*CvSs + Sp*r)*Lx + + Dx*Sp + - Dy + Qy; - j[2] = (Dt + Lz)*s - + Lx*t - - Cw*(Draz + Dt + Lz - Qz) - - (Drax + Dx + Lx - Qx)*Sw + j[2] = (Dt + Lz)*s + + Lx*t + - Cw*(Draz + Dt + Lz - Qz) + - (Drax + Dx + Lx - Qx)*Sw + Draz; j[3] = pos->a; @@ -342,32 +282,31 @@ static int trsrnInverse(const EmcPose * pos, double *j, int tool_kins) r = Cs + Sv*Sv*(1-Cs); s = Cs + Cv*Cv*(1-Cs); t = Sv*Cv*(1-Cs); - - j[0] = Cp*Dx - - (CvSs*Sp - Cp*r)*Lx - + (Sp*SvSs + Cp*t)*Lz - - ((CvSs*Sp - Cp*r)*Ctc + (Cp*CvSs + Cs*Sp)*Stc)*Qx - - ((Cp*CvSs + Cs*Sp)*Ctc - (CvSs*Sp - Cp*r)*Stc)*Qy - + (Sp*SvSs + Cp*t)*Qz - - Dy*Sp - - Dx + + j[0] = Cp*Dx + - (CvSs*Sp - Cp*r)*Lx + + (Sp*SvSs + Cp*t)*Lz + - ((CvSs*Sp - Cp*r)*Ctc + (Cp*CvSs + Cs*Sp)*Stc)*Qx + - ((Cp*CvSs + Cs*Sp)*Ctc - (CvSs*Sp - Cp*r)*Stc)*Qy + + (Sp*SvSs + Cp*t)*Qz + - Dy*Sp + - Dx - Lx; - j[1] = Cp*Dy - + (Cp*CvSs + Sp*r)*Lx - - (Cp*SvSs - Sp*t)*Lz - + ((Cp*CvSs + Sp*r)*Ctc + (Cp*Cs - CvSs*Sp)*Stc)*Qx - + ((Cp*Cs - CvSs*Sp)*Ctc - (Cp*CvSs + Sp*r)*Stc)*Qy - - (Cp*SvSs - Sp*t)*Qz - + Dx*Sp + j[1] = Cp*Dy + + (Cp*CvSs + Sp*r)*Lx + - (Cp*SvSs - Sp*t)*Lz + + ((Cp*CvSs + Sp*r)*Ctc + (Cp*Cs - CvSs*Sp)*Stc)*Qx + + ((Cp*Cs - CvSs*Sp)*Ctc - (Cp*CvSs + Sp*r)*Stc)*Qy + - (Cp*SvSs - Sp*t)*Qz + + Dx*Sp - Dy; - - j[2] = (Stc*SvSs + Ctc*t)*Qx - + (Ctc*SvSs - Stc*t)*Qy - + Lz*s - + Qz*s - + Lx*t + j[2] = (Stc*SvSs + Ctc*t)*Qx + + (Ctc*SvSs - Stc*t)*Qy + + Lz*s + + Qz*s + + Lx*t - Lz; j[3] = pos->a; @@ -378,34 +317,38 @@ static int trsrnInverse(const EmcPose * pos, double *j, int tool_kins) return 0; } // trsrnInverse() -static int tcpKinematicsInverse(const EmcPose * pos, +static int tcpKinematicsInverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, double *j, const KINEMATICS_INVERSE_FLAGS * iflags, KINEMATICS_FORWARD_FLAGS * fflags) { + (void)s; (void)iflags; (void)fflags; - return trsrnInverse(pos, j, 0); + return trsrnInverse(p, pos, j, 0); } // tcpKinematicsInverse() -static int toolKinematicsInverse(const EmcPose * pos, +static int toolKinematicsInverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, double *j, const KINEMATICS_INVERSE_FLAGS * iflags, KINEMATICS_FORWARD_FLAGS * fflags) { + (void)s; (void)iflags; (void)fflags; - return trsrnInverse(pos, j, 1); + return trsrnInverse(p, pos, j, 1); } // toolKinematicsInverse() // The head answers in the convention already, so the native rotation -// registered with these frames is TOOL_FRAME_SPINDLE. -static int tcpKinematicsToolFrame(const double *j, +// declared with these frames is TOOL_FRAME_SPINDLE. +static int tcpKinematicsToolFrame(const kins_params *p, const double *j, PmRotationMatrix *rot, const KINEMATICS_FORWARD_FLAGS *fflags) { (void)fflags; - double nu = hal_get_real(haldata->nut_angle); // degrees + double nu = p->geometry[P_NUT]; // degrees double Sv = sin(nu*TO_RAD); double Cv = cos(nu*TO_RAD); double Ss = sin(j[3]*TO_RAD); @@ -440,10 +383,11 @@ static int tcpKinematicsToolFrame(const double *j, return 0; } // tcpKinematicsToolFrame() -static int tcpKinematicsWorkFrame(const double *j, +static int tcpKinematicsWorkFrame(const kins_params *p, const double *j, PmRotationMatrix *rot, const KINEMATICS_FORWARD_FLAGS *fflags) { + (void)p; (void)fflags; double Sw = sin(j[4]*TO_RAD); double Cw = cos(j[4]*TO_RAD); @@ -459,23 +403,15 @@ static int tcpKinematicsWorkFrame(const double *j, return 0; } // tcpKinematicsWorkFrame() -static int tcpKinematicsJacobian(const double *j, +static int tcpKinematicsJacobian(const kins_params *p, const double *j, const EmcPose * pos, double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], const KINEMATICS_INVERSE_FLAGS * iflags) { (void)j; (void)iflags; - - // the same geometry as trsrnInverse(), read the same way - double Lx = hal_get_real(haldata->x_pivot); - double Lz = hal_get_real(haldata->z_pivot); - double Dx = hal_get_real(haldata->x_offset); - double Dy = hal_get_real(haldata->y_offset); - double Drax = hal_get_real(haldata->x_rot_axis) - Lx - Dx; - double Draz = hal_get_real(haldata->z_rot_axis) - Lz; - double nu = hal_get_real(haldata->nut_angle); // degrees - double Dt = hal_get_real(haldata->tool_offset_z); + GEOMETRY(p); + (void)tc; (void)theta_1; (void)theta_2; double Sv = sin(nu*TO_RAD); double Cv = cos(nu*TO_RAD); @@ -539,7 +475,7 @@ static int tcpKinematicsJacobian(const double *j, return 0; } // tcpKinematicsJacobian() -static int toolKinematicsJacobian(const double *j, +static int toolKinematicsJacobian(const kins_params *p, const double *j, const EmcPose * pos, double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], const KINEMATICS_INVERSE_FLAGS * iflags) @@ -550,10 +486,10 @@ static int toolKinematicsJacobian(const double *j, // the head angles come from pins, so the inverse is linear in the pose // and the rows are its coefficients - double tc = hal_get_real(haldata->pre_rot); - double nu = hal_get_real(haldata->nut_angle); // degrees - double theta_1 = hal_get_real(haldata->prim_angle); // degrees - double theta_2 = hal_get_real(haldata->sec_angle); // degrees + double tc = p->geometry[P_PRE_ROT]; + double nu = p->geometry[P_NUT]; // degrees + double theta_1 = p->geometry[P_PRIM]; // degrees + double theta_2 = p->geometry[P_SEC]; // degrees double Sv = sin(nu*TO_RAD); double Cv = cos(nu*TO_RAD); @@ -589,43 +525,55 @@ static int toolKinematicsJacobian(const double *j, return 0; } // toolKinematicsJacobian() +static const kins_ops tcp_ops = { + .forward = tcpKinematicsForward, + .inverse = tcpKinematicsInverse, + .work = tcpKinematicsWorkFrame, + .tool = tcpKinematicsToolFrame, + .native = &TOOL_FRAME_SPINDLE, + .jacobian = tcpKinematicsJacobian, +}; + +// the tool kinematics report in tool axes, so the tool is square with the +// world by construction and nothing turns the work against it +static const kins_ops tool_ops = { + .forward = toolKinematicsForward, + .inverse = toolKinematicsInverse, + .work = kinsIdentityFrame, + .tool = kinsIdentityFrame, + .native = &TOOL_FRAME_SPINDLE, + .jacobian = toolKinematicsJacobian, +}; + +int switchkinsSetup(kparms* kp, + KS* kset0, KS* kset1, KS* kset2, + KF* kfwd0, KF* kfwd1, KF* kfwd2, + KI* kinv0, KI* kinv1, KI* kinv2 + ) +{ + (void)kset0; (void)kset1; (void)kset2; + (void)kfwd0; (void)kfwd1; (void)kfwd2; + (void)kinv0; (void)kinv1; (void)kinv2; + kp->kinsname = "xyzbca_trsrn"; + kp->halprefix = "xyzbca_trsrn_kins"; + kp->required_coordinates = "xyzabc"; + kp->allow_duplicates = 0; + kp->max_joints = strlen(kp->required_coordinates); + kp->params = trsrn_params; + kp->nparams = sizeof(trsrn_params)/sizeof(trsrn_params[0]); + + switchkinsRegisterOps(0, &KINS_IDENTITY_OPS); + switchkinsRegisterOps(1, &tcp_ops); + switchkinsRegisterOps(2, &tool_ops); + return 0; +} // switchkinsSetup() + // halcompile has done hal_init() and does hal_ready() after this returns, // which is what switchkinsInit() expects EXTRA_SETUP() { - kparms kp = {0}; + kparms kp; (void)__comp_inst; (void)prefix; (void)extra_arg; - kp.kinsname = "xyzbca_trsrn"; - kp.halprefix = "xyzbca_trsrn_kins"; - kp.required_coordinates = "xyzabc"; - kp.allow_duplicates = 0; - kp.fwd_iterates_mask = 0; - kp.gui_kinstype = -1; - kp.sparm = NULL; - kp.max_joints = strlen(kp.required_coordinates); - - if (switchkinsRegister(0, identityKinematicsSetup, - identityKinematicsForward, - identityKinematicsInverse)) { return -1; } - if (switchkinsRegister(1, trsrnKinematicsSetup, - tcpKinematicsForward, - tcpKinematicsInverse)) { return -1; } - if (switchkinsRegister(2, toolKinematicsSetup, - toolKinematicsForward, - toolKinematicsInverse)) { return -1; } - if (switchkinsRegisterFrames(1, tcpKinematicsWorkFrame, - tcpKinematicsToolFrame, - &TOOL_FRAME_SPINDLE)) { return -1; } - if (switchkinsRegisterJacobian(1, tcpKinematicsJacobian)) { return -1; } - // the tool kinematics report in tool axes, so the tool is square with - // the world by construction and nothing turns the work against it - if (switchkinsRegisterFrames(2, identityKinematicsWorkFrame, - identityKinematicsToolFrame, - &TOOL_FRAME_SPINDLE)) { return -1; } - if (switchkinsRegisterJacobian(2, toolKinematicsJacobian)) { return -1; } - - if (switchkinsDeclare(0, KINSTYPE_IDENTITY)) { return -1; } - if (switchkinsDeclare(1, KINSTYPE_PRIMARY)) { return -1; } - + if (switchkinsRunSetup(&kp, NULL)) { return -1; } return switchkinsInit(comp_id, &kp, coordinates); } // EXTRA_SETUP() From e99cf7900d9c26f227b95c149838746f3fff9083 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Fri, 4 Sep 2026 13:13:52 +1000 Subject: [PATCH 07/19] genserkins, genhexkins, pentakins: move onto the parameter block The three that build a geometry from many pins build it from the block on each call: genser its link description, genhex its base and platform points, pentakins its base points and effector circles. What they reported through output pins, iteration counts, strut corrections, the hexapod's fwd-kins-fail and gui pose, goes through the scratch, so each caller keeps its own. genhex and pentakins declare their forward as iterating and the switchkins core seeds it as before. genhexkins's six gui pose pins were inputs written by the module and are outputs now; pentakins's HAL parameters become pins of the same names. genserfuncs.c loses its haldata and globals, and ugenserkins builds a block and calls the ops. --- src/Makefile | 3 + src/emc/kinematics/Submakefile | 1 + src/emc/kinematics/genhexkins.c | 529 ++++++++++++++----------------- src/emc/kinematics/genserfuncs.c | 296 +++++++---------- src/emc/kinematics/genserkins.c | 36 +-- src/emc/kinematics/genserkins.h | 36 +-- src/emc/kinematics/pentakins.c | 280 ++++++++-------- src/emc/kinematics/ugenserkins.c | 36 ++- 8 files changed, 516 insertions(+), 701 deletions(-) diff --git a/src/Makefile b/src/Makefile index cef626a61be..e24509ada47 100644 --- a/src/Makefile +++ b/src/Makefile @@ -1092,6 +1092,9 @@ lineardeltakins-objs += emc/kinematics/kins_single_module.o obj-m += pentakins.o pentakins-objs := emc/kinematics/pentakins.o +pentakins-objs += emc/kinematics/kins_util.o +pentakins-objs += emc/kinematics/kins_single.o +pentakins-objs += emc/kinematics/kins_single_module.o pentakins-objs += libposemath/_posemath.o pentakins-objs += $(MATHSTUB) diff --git a/src/emc/kinematics/Submakefile b/src/emc/kinematics/Submakefile index 77085c21c2e..6cc22df32d3 100644 --- a/src/emc/kinematics/Submakefile +++ b/src/emc/kinematics/Submakefile @@ -2,6 +2,7 @@ GENSERKINSSRCS := emc/kinematics/ugenserkins.c GENSERKINSSRCS += emc/kinematics/genserfuncs.c +GENSERKINSSRCS += emc/kinematics/kins_util.c USERSRCS += $(GENSERKINSSRCS) DELTAMODULESRCS := emc/kinematics/lineardeltakins.cc diff --git a/src/emc/kinematics/genhexkins.c b/src/emc/kinematics/genhexkins.c index 0964fa7d088..fa5276f5a56 100644 --- a/src/emc/kinematics/genhexkins.c +++ b/src/emc/kinematics/genhexkins.c @@ -16,17 +16,17 @@ 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. + configuration. In the functions "genhex_forward" and "genhex_inverse" + 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: + machine can be adjusted by hal pins: genhexkins.base.N.x genhexkins.base.N.y @@ -67,18 +67,18 @@ genhexkins.correction.N - pins showing current values of strut length correction. - The genhexKinematicsInverse function solves the inverse kinematics using + The genhex_inverse 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 + The genhex_forward 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 + the genhex_forward 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 + solutions. The genhex_forward 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 @@ -103,6 +103,10 @@ genhexkins.max-iterations - maximum number of iterations spent for a converged solution during current session. + The maths is written as pure functions of the parameter block (see + kinematics.h): the pins above are the table below, read into the block + before every call and written from the scratch after it. + ----------------------------------------------------------------------------*/ #include @@ -114,49 +118,98 @@ #include "genhexkins.h" #include -static struct haldata { - hal_real_t basex[NUM_STRUTS]; - hal_real_t basey[NUM_STRUTS]; - hal_real_t basez[NUM_STRUTS]; - hal_real_t platformx[NUM_STRUTS]; - hal_real_t platformy[NUM_STRUTS]; - hal_real_t platformz[NUM_STRUTS]; - hal_real_t basenx[NUM_STRUTS]; - hal_real_t baseny[NUM_STRUTS]; - hal_real_t basenz[NUM_STRUTS]; - hal_real_t platformnx[NUM_STRUTS]; - hal_real_t platformny[NUM_STRUTS]; - hal_real_t platformnz[NUM_STRUTS]; - hal_real_t correction[NUM_STRUTS]; - hal_real_t screw_lead; - hal_uint_t last_iter; - hal_uint_t max_iter; - hal_uint_t iter_limit; - hal_real_t max_error; - hal_real_t conv_criterion; - hal_real_t tool_offset; - hal_real_t spindle_offset; - hal_bool_t fwd_kins_fail; - - hal_real_t gui_x; - hal_real_t gui_y; - hal_real_t gui_z; - hal_real_t gui_a; - hal_real_t gui_b; - hal_real_t gui_c; - -} *haldata; - -static int genhex_gui_forward_kins(EmcPose *pos) -{ - hal_set_real(haldata->gui_x, pos->tran.x); - hal_set_real(haldata->gui_y, pos->tran.y); - hal_set_real(haldata->gui_z, pos->tran.z); - hal_set_real(haldata->gui_a, pos->a); - hal_set_real(haldata->gui_b, pos->b); - hal_set_real(haldata->gui_c, pos->c); - return 0; -} // genhex_gui_forward_kins +// the table: thirteen entries per strut, then the iteration controls, +// the offsets and the reports. The macros index it. +#define STRUT_ENTRIES 13 +#define P_BASE_X(i) (STRUT_ENTRIES*(i) + 0) +#define P_BASE_Y(i) (STRUT_ENTRIES*(i) + 1) +#define P_BASE_Z(i) (STRUT_ENTRIES*(i) + 2) +#define P_PLAT_X(i) (STRUT_ENTRIES*(i) + 3) +#define P_PLAT_Y(i) (STRUT_ENTRIES*(i) + 4) +#define P_PLAT_Z(i) (STRUT_ENTRIES*(i) + 5) +#define P_BASE_NX(i) (STRUT_ENTRIES*(i) + 6) +#define P_BASE_NY(i) (STRUT_ENTRIES*(i) + 7) +#define P_BASE_NZ(i) (STRUT_ENTRIES*(i) + 8) +#define P_PLAT_NX(i) (STRUT_ENTRIES*(i) + 9) +#define P_PLAT_NY(i) (STRUT_ENTRIES*(i) + 10) +#define P_PLAT_NZ(i) (STRUT_ENTRIES*(i) + 11) +#define P_CORR(i) (STRUT_ENTRIES*(i) + 12) +enum { + P_LAST_ITER = STRUT_ENTRIES*NUM_STRUTS, + P_MAX_ITER, + P_MAX_ERROR, + P_CONV_CRITERION, + P_ITER_LIMIT, + P_TOOL_OFFSET, + P_SPINDLE_OFFSET, + P_SCREW_LEAD, + P_GUI_X, P_GUI_Y, P_GUI_Z, P_GUI_A, P_GUI_B, P_GUI_C, + P_FWD_FAIL, + P_COUNT +}; + +#define STRUT_ROWS(i, bx, by, bz, px, py, pz, bnx, bny, bnz, pnx, pny, pnz) \ + { "base." #i ".x", KINS_PARAM_FLOAT, KINS_IN, 0, bx }, \ + { "base." #i ".y", KINS_PARAM_FLOAT, KINS_IN, 0, by }, \ + { "base." #i ".z", KINS_PARAM_FLOAT, KINS_IN, 0, bz }, \ + { "platform." #i ".x", KINS_PARAM_FLOAT, KINS_IN, 0, px }, \ + { "platform." #i ".y", KINS_PARAM_FLOAT, KINS_IN, 0, py }, \ + { "platform." #i ".z", KINS_PARAM_FLOAT, KINS_IN, 0, pz }, \ + { "base-n." #i ".x", KINS_PARAM_FLOAT, KINS_IN, 0, bnx }, \ + { "base-n." #i ".y", KINS_PARAM_FLOAT, KINS_IN, 0, bny }, \ + { "base-n." #i ".z", KINS_PARAM_FLOAT, KINS_IN, 0, bnz }, \ + { "platform-n." #i ".x", KINS_PARAM_FLOAT, KINS_IN, 0, pnx }, \ + { "platform-n." #i ".y", KINS_PARAM_FLOAT, KINS_IN, 0, pny }, \ + { "platform-n." #i ".z", KINS_PARAM_FLOAT, KINS_IN, 0, pnz }, \ + { "correction." #i, KINS_PARAM_FLOAT, KINS_OUT, 0, 0.0 } + +static const kins_param_desc genhex_params[P_COUNT] = { + STRUT_ROWS(0, DEFAULT_BASE_0_X, DEFAULT_BASE_0_Y, DEFAULT_BASE_0_Z, + DEFAULT_PLATFORM_0_X, DEFAULT_PLATFORM_0_Y, DEFAULT_PLATFORM_0_Z, + DEFAULT_BASE_0_NX, DEFAULT_BASE_0_NY, DEFAULT_BASE_0_NZ, + DEFAULT_PLATFORM_0_NX, DEFAULT_PLATFORM_0_NY, DEFAULT_PLATFORM_0_NZ), + STRUT_ROWS(1, DEFAULT_BASE_1_X, DEFAULT_BASE_1_Y, DEFAULT_BASE_1_Z, + DEFAULT_PLATFORM_1_X, DEFAULT_PLATFORM_1_Y, DEFAULT_PLATFORM_1_Z, + DEFAULT_BASE_1_NX, DEFAULT_BASE_1_NY, DEFAULT_BASE_1_NZ, + DEFAULT_PLATFORM_1_NX, DEFAULT_PLATFORM_1_NY, DEFAULT_PLATFORM_1_NZ), + STRUT_ROWS(2, DEFAULT_BASE_2_X, DEFAULT_BASE_2_Y, DEFAULT_BASE_2_Z, + DEFAULT_PLATFORM_2_X, DEFAULT_PLATFORM_2_Y, DEFAULT_PLATFORM_2_Z, + DEFAULT_BASE_2_NX, DEFAULT_BASE_2_NY, DEFAULT_BASE_2_NZ, + DEFAULT_PLATFORM_2_NX, DEFAULT_PLATFORM_2_NY, DEFAULT_PLATFORM_2_NZ), + STRUT_ROWS(3, DEFAULT_BASE_3_X, DEFAULT_BASE_3_Y, DEFAULT_BASE_3_Z, + DEFAULT_PLATFORM_3_X, DEFAULT_PLATFORM_3_Y, DEFAULT_PLATFORM_3_Z, + DEFAULT_BASE_3_NX, DEFAULT_BASE_3_NY, DEFAULT_BASE_3_NZ, + DEFAULT_PLATFORM_3_NX, DEFAULT_PLATFORM_3_NY, DEFAULT_PLATFORM_3_NZ), + STRUT_ROWS(4, DEFAULT_BASE_4_X, DEFAULT_BASE_4_Y, DEFAULT_BASE_4_Z, + DEFAULT_PLATFORM_4_X, DEFAULT_PLATFORM_4_Y, DEFAULT_PLATFORM_4_Z, + DEFAULT_BASE_4_NX, DEFAULT_BASE_4_NY, DEFAULT_BASE_4_NZ, + DEFAULT_PLATFORM_4_NX, DEFAULT_PLATFORM_4_NY, DEFAULT_PLATFORM_4_NZ), + STRUT_ROWS(5, DEFAULT_BASE_5_X, DEFAULT_BASE_5_Y, DEFAULT_BASE_5_Z, + DEFAULT_PLATFORM_5_X, DEFAULT_PLATFORM_5_Y, DEFAULT_PLATFORM_5_Z, + DEFAULT_BASE_5_NX, DEFAULT_BASE_5_NY, DEFAULT_BASE_5_NZ, + DEFAULT_PLATFORM_5_NX, DEFAULT_PLATFORM_5_NY, DEFAULT_PLATFORM_5_NZ), + [P_LAST_ITER] = { "last-iterations", KINS_PARAM_U32, KINS_OUT, 0, 0 }, + [P_MAX_ITER] = { "max-iterations", KINS_PARAM_U32, KINS_OUT, 0, 0 }, + [P_MAX_ERROR] = { "max-error", KINS_PARAM_FLOAT, KINS_IN, 0, 500.0 }, + [P_CONV_CRITERION] = { "convergence-criterion", KINS_PARAM_FLOAT, KINS_IN, 0, 1e-9 }, + [P_ITER_LIMIT] = { "limit-iterations", KINS_PARAM_U32, KINS_IN, 0, 120 }, + [P_TOOL_OFFSET] = { "tool-offset", KINS_PARAM_FLOAT, KINS_IN, 1, 0.0 }, + [P_SPINDLE_OFFSET] = { "spindle-offset", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + [P_SCREW_LEAD] = { "screw-lead", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_SCREW_LEAD }, + // the pose the forward found, for a vismach gui; switchkins provides + // the skgui.* pins for the same purpose + [P_GUI_X] = { "x", KINS_PARAM_FLOAT, KINS_OUT, 0, 0.0 }, + [P_GUI_Y] = { "y", KINS_PARAM_FLOAT, KINS_OUT, 0, 0.0 }, + [P_GUI_Z] = { "z", KINS_PARAM_FLOAT, KINS_OUT, 0, 0.0 }, + [P_GUI_A] = { "a", KINS_PARAM_FLOAT, KINS_OUT, 0, 0.0 }, + [P_GUI_B] = { "b", KINS_PARAM_FLOAT, KINS_OUT, 0, 0.0 }, + [P_GUI_C] = { "c", KINS_PARAM_FLOAT, KINS_OUT, 0, 0.0 }, + [P_FWD_FAIL] = { "fwd-kins-fail", KINS_PARAM_BIT, KINS_OUT, 0, 0 }, +}; + +// the most iterations a converged solution has taken this session, kept +// in the caller's scratch so each caller reports its own +#define MAX_ITER_SEEN(s) ((s)->aux[0]) /******************************* MatInvert() ***************************/ @@ -259,45 +312,45 @@ static void MatMult(double J[][6], const double x[], double Ans[]) } } // 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) { +/* the geometry of one call, taken from the block: base and platform + coordinates, the joint axes vectors and the screw lead */ +typedef struct { + PmCartesian b[NUM_STRUTS]; + PmCartesian a[NUM_STRUTS]; + PmCartesian nb1[NUM_STRUTS]; + PmCartesian na0[NUM_STRUTS]; + double screw_lead; +} genhex_geometry; + +static void geometry_of(const kins_params *p, genhex_geometry *g) { int t; - /* set the base and platform coordinates from hal pin values */ - rtapi_real spindle_offset = hal_get_real(haldata->spindle_offset); - rtapi_real tool_offset = hal_get_real(haldata->tool_offset); + /* set the base and platform coordinates from the block */ + const double spindle_offset = p->geometry[P_SPINDLE_OFFSET]; + const double tool_offset = p->tool.tran.z; for (t = 0; t < NUM_STRUTS; t++) { - b[t].x = hal_get_real(haldata->basex[t]); - b[t].y = hal_get_real(haldata->basey[t]); - b[t].z = hal_get_real(haldata->basez[t]) + spindle_offset + tool_offset; - a[t].x = hal_get_real(haldata->platformx[t]); - a[t].y = hal_get_real(haldata->platformy[t]); - a[t].z = hal_get_real(haldata->platformz[t]) + spindle_offset + tool_offset; - - nb1[t].x = hal_get_real(haldata->basenx[t]); - nb1[t].y = hal_get_real(haldata->baseny[t]); - nb1[t].z = hal_get_real(haldata->basenz[t]); - na0[t].x = hal_get_real(haldata->platformnx[t]); - na0[t].y = hal_get_real(haldata->platformny[t]); - na0[t].z = hal_get_real(haldata->platformnz[t]); + g->b[t].x = p->geometry[P_BASE_X(t)]; + g->b[t].y = p->geometry[P_BASE_Y(t)]; + g->b[t].z = p->geometry[P_BASE_Z(t)] + spindle_offset + tool_offset; + g->a[t].x = p->geometry[P_PLAT_X(t)]; + g->a[t].y = p->geometry[P_PLAT_Y(t)]; + g->a[t].z = p->geometry[P_PLAT_Z(t)] + spindle_offset + tool_offset; + + g->nb1[t].x = p->geometry[P_BASE_NX(t)]; + g->nb1[t].y = p->geometry[P_BASE_NY(t)]; + g->nb1[t].z = p->geometry[P_BASE_NZ(t)]; + g->na0[t].x = p->geometry[P_PLAT_NX(t)]; + g->na0[t].y = p->geometry[P_PLAT_NY(t)]; + g->na0[t].z = p->geometry[P_PLAT_NZ(t)]; } - return 0; -} // genhex_read_hal_pins() + g->screw_lead = p->geometry[P_SCREW_LEAD]; +} // geometry_of() /***************************StrutLengthCorrection***************************/ -static int StrutLengthCorrection(const PmCartesian * StrutVectUnit, +static int StrutLengthCorrection(const genhex_geometry *g, + const PmCartesian * StrutVectUnit, const PmRotationMatrix * RMatrix, const int strut_number, double * correction) @@ -306,32 +359,34 @@ static int StrutLengthCorrection(const PmCartesian * StrutVectUnit, double dotprod; /* define base joints axis vectors */ - pmCartCartCross(&nb1[strut_number], StrutVectUnit, &nb2); + pmCartCartCross(&g->nb1[strut_number], StrutVectUnit, &nb2); pmCartCartCross(StrutVectUnit, &nb2, &nb3); pmCartUnitEq(&nb3); /* define platform joints axis vectors */ - pmMatCartMult(RMatrix, &na0[strut_number], &na1); + pmMatCartMult(RMatrix, &g->na0[strut_number], &na1); pmCartCartCross(&na1, StrutVectUnit, &na2); pmCartUnitEq(&na2); /* define dot product */ pmCartCartDot(&nb3, &na2, &dotprod); - *correction = hal_get_real(haldata->screw_lead) * asin(dotprod) / PM_2_PI; + *correction = g->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) +/**************** genhex_forward() *****************/ +static int genhex_forward(const kins_params *p, kins_scratch *s, + const double * joints, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { (void)fflags; (void)iflags; + genhex_geometry g; PmCartesian aw; PmCartesian InvKinStrutVect,InvKinStrutVectUnit; PmCartesian q_trans, RMatrix_a, RMatrix_a_cross_Strut; @@ -350,7 +405,7 @@ static int genhexKinematicsForward(const double * joints, int i; unsigned iteration = 0; - genhex_read_hal_pins(); + geometry_of(p, &g); /* abort on obvious problems, like joints <= 0 */ /* FIXME-- should check against triangle inequality, so that joints @@ -375,13 +430,16 @@ static int genhexKinematicsForward(const double * joints, q_trans.z = pos->tran.z; /* Enter Newton-Raphson iterative method */ - rtapi_real max_error = hal_get_real(haldata->max_error); + const double max_error = p->geometry[P_MAX_ERROR]; + const unsigned iter_limit = (unsigned)p->geometry[P_ITER_LIMIT]; + const double conv_criterion = p->geometry[P_CONV_CRITERION]; while (iterate) { /* check for large error and return error flag if no convergence */ if ((conv_err > +max_error) || (conv_err < -max_error)) { /* we can't converge */ - hal_set_bool(haldata->fwd_kins_fail, 1); + s->failed = 1; + s->out[P_FWD_FAIL] = 1; return -2; }; @@ -389,9 +447,10 @@ static int genhexKinematicsForward(const double * joints, /* check iteration to see if the kinematics can reach the convergence criterion and return error flag if it can't */ - if (iteration > hal_get_ui32(haldata->iter_limit)) { + if (iteration > iter_limit) { /* we can't converge */ - hal_set_bool(haldata->fwd_kins_fail, 1); + s->failed = 1; + s->out[P_FWD_FAIL] = 1; return -5; } @@ -402,18 +461,19 @@ static int genhexKinematicsForward(const double * joints, 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); + pmMatCartMult(&RMatrix, &g.a[i], &RMatrix_a); pmCartCartAdd(&q_trans, &RMatrix_a, &aw); - pmCartCartSub(&aw, &b[i], &InvKinStrutVect); + pmCartCartSub(&aw, &g.b[i], &InvKinStrutVect); if (0 != pmCartUnit(&InvKinStrutVect, &InvKinStrutVectUnit)) { - hal_set_bool(haldata->fwd_kins_fail, 1); + s->failed = 1; + s->out[P_FWD_FAIL] = 1; return -1; } pmCartMag(&InvKinStrutVect, &InvKinStrutLength); - if (hal_get_real(haldata->screw_lead) != 0.0) { + if (g.screw_lead != 0.0) { /* enable strut length correction */ - StrutLengthCorrection(&InvKinStrutVectUnit, &RMatrix, i, &corr); + StrutLengthCorrection(&g, &InvKinStrutVectUnit, &RMatrix, i, &corr); /* define corrected joint lengths */ InvKinStrutLength += corr; } @@ -454,7 +514,6 @@ static int genhexKinematicsForward(const double * joints, /* enter loop to determine if a strut needs another iteration */ iterate = 0; /*assume iteration is done */ - rtapi_real conv_criterion = hal_get_real(haldata->conv_criterion); for (i = 0; i < NUM_STRUTS; i++) { if (fabs(StrutLengthDiff[i]) > conv_criterion) { iterate = 1; @@ -472,33 +531,42 @@ static int genhexKinematicsForward(const double * joints, pos->tran.y = q_trans.y; pos->tran.z = q_trans.z; - hal_set_ui32(haldata->last_iter, iteration); - - if (iteration > hal_get_ui32(haldata->max_iter)){ - hal_set_ui32(haldata->max_iter, iteration); + s->iterations = iteration; + s->failed = 0; + s->out[P_LAST_ITER] = iteration; + if (iteration > MAX_ITER_SEEN(s)) { + MAX_ITER_SEEN(s) = iteration; } - hal_set_bool(haldata->fwd_kins_fail, 0); + s->out[P_MAX_ITER] = MAX_ITER_SEEN(s); + s->out[P_FWD_FAIL] = 0; - genhex_gui_forward_kins(pos); + s->out[P_GUI_X] = pos->tran.x; + s->out[P_GUI_Y] = pos->tran.y; + s->out[P_GUI_Z] = pos->tran.z; + s->out[P_GUI_A] = pos->a; + s->out[P_GUI_B] = pos->b; + s->out[P_GUI_C] = pos->c; return 0; -} // genhexKinematicsForward() +} // genhex_forward() -/************************ genhexKinematicsInverse() ************************/ +/************************ genhex_inverse() ************************/ /* 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) +static int genhex_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, + double * joints, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { (void)iflags; (void)fflags; + genhex_geometry g; PmCartesian aw, temp; PmCartesian InvKinStrutVect, InvKinStrutVectUnit; PmRotationMatrix RMatrix; @@ -506,7 +574,7 @@ static int genhexKinematicsInverse(const EmcPose * pos, int i; double InvKinStrutLength, corr; - genhex_read_hal_pins(); + geometry_of(p, &g); /* define Rotation Matrix */ rpy.r = pos->a * PM_PI / 180.0; @@ -518,22 +586,22 @@ static int genhexKinematicsInverse(const EmcPose * pos, for (i = 0; i < NUM_STRUTS; i++) { /* convert location of platform strut end from platform to world coordinates */ - pmMatCartMult(&RMatrix, &a[i], &temp); + pmMatCartMult(&RMatrix, &g.a[i], &temp); pmCartCartAdd(&pos->tran, &temp, &aw); /* define strut lengths */ - pmCartCartSub(&aw, &b[i], &InvKinStrutVect); + pmCartCartSub(&aw, &g.b[i], &InvKinStrutVect); pmCartMag(&InvKinStrutVect, &InvKinStrutLength); - if (hal_get_real(haldata->screw_lead) != 0.0) { + if (g.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); - hal_set_real(haldata->correction[i], corr); + StrutLengthCorrection(&g, &InvKinStrutVectUnit, &RMatrix, i, &corr); + s->out[P_CORR(i)] = corr; InvKinStrutLength += corr; } @@ -541,9 +609,9 @@ static int genhexKinematicsInverse(const EmcPose * pos, } return 0; -} //genhexKinematicsInverse() +} //genhex_inverse() -/************************ genhexKinematicsJacobian() ***********************/ +/************************ genhex_jacobian() ***********************/ /* A strut length changes by the component of its platform end's motion along the strut. That end moves with the platform, dP + w x (R a), so the row for strut i is [u_i, (R a_i x u_i) . E] with u_i the unit strut @@ -551,11 +619,18 @@ static int genhexKinematicsInverse(const EmcPose * pos, words to the angular velocity w for R = Rz(c) Ry(b) Rx(a). The forward kinematics builds the same rows for its Newton step, in radians. */ -static int genhexKinematicsJacobian(const double * joints, - const EmcPose * pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS * iflags) +// the inverse alone, for differencing where the closed form does not apply +static const kins_ops genhex_diff_ops = { + .forward = genhex_forward, + .inverse = genhex_inverse, +}; + +static int genhex_jacobian(const kins_params *p, const double * joints, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) { + genhex_geometry g; PmCartesian aw, RMatrix_a, strut, u, moment; PmRotationMatrix RMatrix; PmRpy rpy; @@ -563,13 +638,14 @@ static int genhexKinematicsJacobian(const double * joints, double sb, cb, sc, cc; int i, m; - genhex_read_hal_pins(); + geometry_of(p, &g); /* the screw lead correction is a function of the pose too, and this does not differentiate it; difference the inverse instead */ - if (hal_get_real(haldata->screw_lead) != 0.0) { - return kinsJacobianFromInverse(genhexKinematicsInverse, NUM_STRUTS, - joints, pos, iflags, jac); + if (g.screw_lead != 0.0) { + kins_scratch scratch; + kinsScratchInit(&scratch); + return kinsOpsJacobian(&genhex_diff_ops, p, &scratch, joints, pos, jac, iflags); } memset(jac, 0, EMCMOT_MAX_JOINTS * EMCMOT_MAX_AXIS * sizeof(jac[0][0])); @@ -590,9 +666,9 @@ static int genhexKinematicsJacobian(const double * joints, for (i = 0; i < NUM_STRUTS; i++) { double len; - pmMatCartMult(&RMatrix, &a[i], &RMatrix_a); + pmMatCartMult(&RMatrix, &g.a[i], &RMatrix_a); pmCartCartAdd(&pos->tran, &RMatrix_a, &aw); - pmCartCartSub(&aw, &b[i], &strut); + pmCartCartSub(&aw, &g.b[i], &strut); pmCartMag(&strut, &len); if (len <= 0) { return -1; } pmCartScalMult(&strut, 1.0/len, &u); @@ -608,145 +684,16 @@ static int genhexKinematicsJacobian(const double * joints, } } return 0; -} // genhexKinematicsJacobian() - -// HAL pin initializaion values. In small arrays so we can easily -// address them in the pin creation loop. -static const rtapi_real init_basex[NUM_STRUTS] = { - DEFAULT_BASE_0_X, DEFAULT_BASE_1_X, DEFAULT_BASE_2_X, - DEFAULT_BASE_3_X, DEFAULT_BASE_4_X, DEFAULT_BASE_5_X, -}; -static const rtapi_real init_basey[NUM_STRUTS] = { - DEFAULT_BASE_0_Y, DEFAULT_BASE_1_Y, DEFAULT_BASE_2_Y, - DEFAULT_BASE_3_Y, DEFAULT_BASE_4_Y, DEFAULT_BASE_5_Y, -}; -static const rtapi_real init_basez[NUM_STRUTS] = { - DEFAULT_BASE_0_Z, DEFAULT_BASE_1_Z, DEFAULT_BASE_2_Z, - DEFAULT_BASE_3_Z, DEFAULT_BASE_4_Z, DEFAULT_BASE_5_Z, -}; -static const rtapi_real init_platformx[NUM_STRUTS] = { - DEFAULT_PLATFORM_0_X, DEFAULT_PLATFORM_1_X, DEFAULT_PLATFORM_2_X, - DEFAULT_PLATFORM_3_X, DEFAULT_PLATFORM_4_X, DEFAULT_PLATFORM_5_X, -}; -static const rtapi_real init_platformy[NUM_STRUTS] = { - DEFAULT_PLATFORM_0_Y, DEFAULT_PLATFORM_1_Y, DEFAULT_PLATFORM_2_Y, - DEFAULT_PLATFORM_3_Y, DEFAULT_PLATFORM_4_Y, DEFAULT_PLATFORM_5_Y, -}; -static const rtapi_real init_platformz[NUM_STRUTS] = { - DEFAULT_PLATFORM_0_Z, DEFAULT_PLATFORM_1_Z, DEFAULT_PLATFORM_2_Z, - DEFAULT_PLATFORM_3_Z, DEFAULT_PLATFORM_4_Z, DEFAULT_PLATFORM_5_Z, -}; -static const rtapi_real init_basenx[NUM_STRUTS] = { - DEFAULT_BASE_0_NX, DEFAULT_BASE_1_NX, DEFAULT_BASE_2_NX, - DEFAULT_BASE_3_NX, DEFAULT_BASE_4_NX, DEFAULT_BASE_5_NX, -}; -static const rtapi_real init_baseny[NUM_STRUTS] = { - DEFAULT_BASE_0_NY, DEFAULT_BASE_1_NY, DEFAULT_BASE_2_NY, - DEFAULT_BASE_3_NY, DEFAULT_BASE_4_NY, DEFAULT_BASE_5_NY, +} // genhex_jacobian() + +// the forward iterates from the pose it is handed, so it is seeded with +// the last answer after a switch +static const kins_ops genhex_ops = { + .forward = genhex_forward, + .inverse = genhex_inverse, + .jacobian = genhex_jacobian, + .fwd_iterates = 1, }; -static const rtapi_real init_basenz[NUM_STRUTS] = { - DEFAULT_BASE_0_NZ, DEFAULT_BASE_1_NZ, DEFAULT_BASE_2_NZ, - DEFAULT_BASE_3_NZ, DEFAULT_BASE_4_NZ, DEFAULT_BASE_5_NZ, -}; -static const rtapi_real init_platformnx[NUM_STRUTS] = { - DEFAULT_PLATFORM_0_NX, DEFAULT_PLATFORM_1_NX, DEFAULT_PLATFORM_2_NX, - DEFAULT_PLATFORM_3_NX, DEFAULT_PLATFORM_4_NX, DEFAULT_PLATFORM_5_NX, -}; -static const rtapi_real init_platformny[NUM_STRUTS] = { - DEFAULT_PLATFORM_0_NY, DEFAULT_PLATFORM_1_NY, DEFAULT_PLATFORM_2_NY, - DEFAULT_PLATFORM_3_NY, DEFAULT_PLATFORM_4_NY, DEFAULT_PLATFORM_5_NY, -}; -static const rtapi_real init_platformnz[NUM_STRUTS] = { - DEFAULT_PLATFORM_0_NZ, DEFAULT_PLATFORM_1_NZ, DEFAULT_PLATFORM_2_NZ, - DEFAULT_PLATFORM_3_NZ, DEFAULT_PLATFORM_4_NZ, DEFAULT_PLATFORM_5_NZ, -}; - -static -int genhexKinematicsSetup(const int comp_id, - const char* coordinates, - kparms* kp) -{ - (void)coordinates; - int i,res=0; - - if (kp->max_joints < 0 || kp->max_joints > NUM_STRUTS) { - rtapi_print_msg(RTAPI_MSG_ERR, "genhexKinematicsSetup: max_joints %d less than 0 or larger NUM_STRUTS %d\n", - kp->max_joints, NUM_STRUTS); - return -1; - } - - 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_new_real(comp_id, HAL_IN, &(haldata->basex[i]), - init_basex[i], "%s.base.%d.x", kp->halprefix, i); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->basey[i], - init_basey[i], "%s.base.%d.y", kp->halprefix, i); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->basez[i], - init_basez[i], "%s.base.%d.z", kp->halprefix, i); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->platformx[i], - init_platformx[i], "%s.platform.%d.x", kp->halprefix, i); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->platformy[i], - init_platformy[i], "%s.platform.%d.y", kp->halprefix, i); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->platformz[i], - init_platformz[i], "%s.platform.%d.z", kp->halprefix, i); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->basenx[i], - init_basenx[i], "%s.base-n.%d.x", kp->halprefix, i); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->baseny[i], - init_baseny[i], "%s.base-n.%d.y", kp->halprefix, i); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->basenz[i], - init_basenz[i], "%s.base-n.%d.z", kp->halprefix, i); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->platformnx[i], - init_platformnx[i], "%s.platform-n.%d.x", kp->halprefix, i); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->platformny[i], - init_platformny[i], "%s.platform-n.%d.y", kp->halprefix, i); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->platformnz[i], - init_platformnz[i], "%s.platform-n.%d.z", kp->halprefix, i); - res += hal_pin_new_real(comp_id, HAL_OUT, &haldata->correction[i], - 0.0, "%s.correction.%d", kp->halprefix, i); - if (res) {goto error;} - } - - res += hal_pin_new_ui32(comp_id, HAL_OUT, &haldata->last_iter, - 0, "genhexkins.last-iterations"); - res += hal_pin_new_ui32(comp_id, HAL_OUT, &haldata->max_iter, - 0, "genhexkins.max-iterations"); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->max_error, - 500.0, "genhexkins.max-error"); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->conv_criterion, - 1e-9, "genhexkins.convergence-criterion"); - res += hal_pin_new_ui32(comp_id, HAL_IN, &haldata->iter_limit, - 120, "genhexkins.limit-iterations"); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->tool_offset, - 0.0, "genhexkins.tool-offset"); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->spindle_offset, - 0.0, "genhexkins.spindle-offset"); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->screw_lead, - DEFAULT_SCREW_LEAD, "genhexkins.screw-lead"); - - if (res) {goto error;} - - //note: switchkins does not uses these as it provides gui.x, gui.y, etc. - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->gui_x, 0.0, "genhexkins.x"); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->gui_y, 0.0, "genhexkins.y"); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->gui_z, 0.0, "genhexkins.z"); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->gui_a, 0.0, "genhexkins.a"); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->gui_b, 0.0, "genhexkins.b"); - res += hal_pin_new_real(comp_id, HAL_IN, &haldata->gui_c, 0.0, "genhexkins.c"); - - res += hal_pin_new_bool(comp_id, HAL_OUT, &haldata->fwd_kins_fail, - 0, "genhexkins.fwd-kins-fail"); - - if (res) goto error; - return 0; - -error: - return res; -} // genhexKinematicsSetup() int switchkinsSetup(kparms* kp, KS* kset0, KS* kset1, KS* kset2, @@ -754,46 +701,32 @@ int switchkinsSetup(kparms* kp, KI* kinv0, KI* kinv1, KI* kinv2 ) { + (void)kset0; (void)kset1; (void)kset2; + (void)kfwd0; (void)kfwd1; (void)kfwd2; + (void)kinv0; (void)kinv1; (void)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->params = genhex_params; + kp->nparams = P_COUNT; + if (kp->sparm && strstr(kp->sparm,"identityfirst")) { rtapi_print("\n!!! switchkins-type 0 is IDENTITY\n"); kp->fwd_iterates_mask = 0x2; //genhexkins switchkins_type==1 kp->gui_kinstype = 1; //vismach gui for switchkins_type==1 - - *kset0 = identityKinematicsSetup; - *kfwd0 = identityKinematicsForward; - *kinv0 = identityKinematicsInverse; - - *kset1 = genhexKinematicsSetup; - *kfwd1 = genhexKinematicsForward; - *kinv1 = genhexKinematicsInverse; - switchkinsDeclare(0, KINSTYPE_IDENTITY); - switchkinsDeclare(1, KINSTYPE_PRIMARY); - switchkinsRegisterJacobian(1, genhexKinematicsJacobian); + switchkinsRegisterOps(0, &KINS_IDENTITY_OPS); + switchkinsRegisterOps(1, &genhex_ops); } else { rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname); kp->fwd_iterates_mask = 0x1; //genhexkins switchkins_type==0 kp->gui_kinstype = 0; //vismach gui for switchkins_type==0 - - *kset0 = genhexKinematicsSetup; - *kfwd0 = genhexKinematicsForward; - *kinv0 = genhexKinematicsInverse; - switchkinsRegisterJacobian(0, genhexKinematicsJacobian); - - *kset1 = identityKinematicsSetup; - *kfwd1 = identityKinematicsForward; - *kinv1 = identityKinematicsInverse; - switchkinsDeclare(0, KINSTYPE_PRIMARY); - switchkinsDeclare(1, KINSTYPE_IDENTITY); + switchkinsRegisterOps(0, &genhex_ops); + switchkinsRegisterOps(1, &KINS_IDENTITY_OPS); } - *kset2 = userkKinematicsSetup; - *kfwd2 = userkKinematicsForward; - *kinv2 = userkKinematicsInverse; + switchkinsRegisterOps(2, &USERK_OPS); return 0; } //switchkinsSetup() diff --git a/src/emc/kinematics/genserfuncs.c b/src/emc/kinematics/genserfuncs.c index 02703735b2a..7a46d1cfdcc 100644 --- a/src/emc/kinematics/genserfuncs.c +++ b/src/emc/kinematics/genserfuncs.c @@ -28,6 +28,11 @@ Currently the type of the joints is hardcoded to ANGULAR, although the kins support both ANGULAR and LINEAR axes. + The maths is written as pure functions of the parameter block (see + kinematics.h): the pins are the table below, read into the block + before every call, and the link description is built from the block + on each call. + TODO: * make number of joints a loadtime parameter * add HAL pins for all settable parameters, including joint type: ANGULAR / LINEAR @@ -49,44 +54,53 @@ #if __GNUC__ && !defined(__clang__) // The matrix and vector storage is just big. // genser_kin_jac_inv() is 2112 -// genserKinematicsInverse() is 2640 - #pragma GCC diagnostic warning "-Wframe-larger-than=2648" +// genser_inverse() is 2640 plus the link description it builds + #pragma GCC diagnostic warning "-Wframe-larger-than=3400" #endif -static struct haldata { - hal_uint_t max_iterations; - hal_uint_t last_iterations; - hal_real_t a[GENSER_MAX_JOINTS]; - hal_real_t alpha[GENSER_MAX_JOINTS]; - hal_real_t d[GENSER_MAX_JOINTS]; - hal_sint_t unrotate[GENSER_MAX_JOINTS]; - genser_struct *kins; - go_pose *pos; // used in various functions, we malloc it - // only once in genserKinematicsSetup() -} *haldata = NULL; - -static int total_joints; -double j[GENSER_MAX_JOINTS]; +// the table: four entries per joint, then the iteration count in and out +#define P_A(i) (4*(i) + 0) +#define P_ALPHA(i) (4*(i) + 1) +#define P_D(i) (4*(i) + 2) +#define P_UNROT(i) (4*(i) + 3) +enum { + P_LAST_ITER = 4*GENSER_MAX_JOINTS, + P_MAX_ITER, + P_COUNT +}; -#define KINS_PTR (haldata->kins) +#define JOINT_ROWS(i, a, alpha, d) \ + { "A-" #i, KINS_PARAM_FLOAT, KINS_IN, 0, a }, \ + { "ALPHA-" #i, KINS_PARAM_FLOAT, KINS_IN, 0, alpha }, \ + { "D-" #i, KINS_PARAM_FLOAT, KINS_IN, 0, d }, \ + { "unrotate-" #i, KINS_PARAM_S32, KINS_IN, 0, 0 } + +const kins_param_desc GENSER_PARAMS[P_COUNT] = { + JOINT_ROWS(0, DEFAULT_A1, DEFAULT_ALPHA1, DEFAULT_D1), + JOINT_ROWS(1, DEFAULT_A2, DEFAULT_ALPHA2, DEFAULT_D2), + JOINT_ROWS(2, DEFAULT_A3, DEFAULT_ALPHA3, DEFAULT_D3), + JOINT_ROWS(3, DEFAULT_A4, DEFAULT_ALPHA4, DEFAULT_D4), + JOINT_ROWS(4, DEFAULT_A5, DEFAULT_ALPHA5, DEFAULT_D5), + JOINT_ROWS(5, DEFAULT_A6, DEFAULT_ALPHA6, DEFAULT_D6), + [P_LAST_ITER] = { "last-iterations", KINS_PARAM_U32, KINS_OUT, 0, 0 }, + [P_MAX_ITER] = { "max-iterations", KINS_PARAM_U32, KINS_IN, 0, GENSER_DEFAULT_MAX_ITERATIONS }, +}; +const int GENSER_NPARAMS = P_COUNT; #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; +void genser_links_of(const kins_params *p, genser_struct *genser) { 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 = hal_get_real(haldata->a[t]); - genser->links[t].u.dh.alpha = hal_get_real(haldata->alpha[t]); - genser->links[t].u.dh.d = hal_get_real(haldata->d[t]); + genser->links[t].u.dh.a = p->geometry[P_A(t)]; + genser->links[t].u.dh.alpha = p->geometry[P_ALPHA(t)]; + genser->links[t].u.dh.d = p->geometry[P_D(t)]; genser->links[t].u.dh.theta = 0; genser->links[t].type = GO_LINK_DH; genser->links[t].quantity = GO_QUANTITY_ANGLE; @@ -95,8 +109,13 @@ int genser_kin_init(void) { /* 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; + genser->iterations = 0; +} // genser_links_of() - return GO_RESULT_OK; +/* the unrotate coupling of one joint, from the block */ +static rtapi_s32 unrotate_of(const kins_params *p, int link) +{ + return (rtapi_s32)p->geometry[P_UNROT(link)]; } /* compute the forward jacobian function: @@ -315,7 +334,7 @@ int genser_kin_jac_fwd(void *kins, } /* The Jacobian in the terms of kinematics.h: joints in degrees per pose - word in EmcPose units, the derivative of genserKinematicsInverse(). + word in EmcPose units, the derivative of genser_inverse(). compute_jinv() gives the geometric inverse Jacobian, radians of joint per unit of base-frame twist. A pose word rate is not a twist: the roll, @@ -327,13 +346,14 @@ int genser_kin_jac_fwd(void *kins, with the unit conversions and the unrotate coupling applied in the order the inverse applies them. */ -int genserKinematicsJacobian(const double *joint, - const EmcPose *world, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS *iflags) +static int genser_jacobian(const kins_params *p, const double *joint, + const EmcPose *world, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) { (void)iflags; - genser_struct *genser = KINS_PTR; + genser_struct genser_stg; + genser_struct *genser = &genser_stg; 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; @@ -343,21 +363,14 @@ int genserKinematicsJacobian(const double *joint, double sb, cb, sc, cc; int link, i, a, m, retval; -#ifndef ULAPI - genser_kin_init(); - if (!genser_hal_inited) { - rtapi_print_msg(RTAPI_MSG_ERR, - "genserKinematicsJacobian: not initialized\n"); - return -1; - } -#endif + genser_links_of(p, genser); memset(jac, 0, EMCMOT_MAX_JOINTS * EMCMOT_MAX_AXIS * sizeof(jac[0][0])); // the kinematic joint angles, in radians and with the unrotate // coupling removed, exactly as the forward prepares them for (link = 0; link < genser->link_num; link++) { - rtapi_s32 unrotate = hal_get_si32(haldata->unrotate[link]); + rtapi_s32 unrotate = unrotate_of(p, link); jest[link] = joint[link] * (PM_PI / 180); if (link && unrotate) jest[link] -= unrotate * jest[link-1]; @@ -403,7 +416,7 @@ int genserKinematicsJacobian(const double *joint, // the unrotate coupling, in link order as the inverse applies it for (link = 1; link < genser->link_num; link++) { - rtapi_s32 unrotate = hal_get_si32(haldata->unrotate[link]); + rtapi_s32 unrotate = unrotate_of(p, link); if (unrotate) { for (a = 0; a < EMCMOT_MAX_AXIS; a++) { jac[link][a] += unrotate * jac[link-1][a]; @@ -412,86 +425,74 @@ int genserKinematicsJacobian(const double *joint, } // uvw pass through as joints 6, 7, 8 - if (total_joints > 6) jac[6][6] = 1; - if (total_joints > 7) jac[7][7] = 1; - if (total_joints > 8) jac[8][8] = 1; + if (p->max_joints > 6) jac[6][6] = 1; + if (p->max_joints > 7) jac[7][7] = 1; + if (p->max_joints > 8) jac[8][8] = 1; return 0; -} // genserKinematicsJacobian() +} // genser_jacobian() /* main function called by emc2 for forward Kins */ -int genserKinematicsForward(const double *joint, - EmcPose * world, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) { +static int genser_forward(const kins_params *p, kins_scratch *s, + const double *joint, + EmcPose * world, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)s; (void)fflags; (void)iflags; - go_pose *pos; + genser_struct genser; + 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; - } + int i; + + genser_links_of(p, &genser); 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; - rtapi_s32 unrotate = hal_get_si32(haldata->unrotate[i]); + rtapi_s32 unrotate = unrotate_of(p, i); if ((i) && unrotate) jcopy[i] -= unrotate * 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; + 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]; + if (p->max_joints > 6) world->u = joint[6]; + if (p->max_joints > 7) world->v = joint[7]; + if (p->max_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); + ret = genser_kin_fwd(&genser, jcopy, &pos); if (ret < 0) return ret; // AJ: convert back to emc2 coords - ret = go_quat_rpy_convert(&pos->rot, &rpy); + 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->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; } @@ -503,8 +504,6 @@ int genser_kin_fwd(void *kins, const go_real * joints, go_pose * pos) 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) @@ -518,22 +517,25 @@ int genser_kin_fwd(void *kins, const go_real * joints, go_pose * pos) return GO_RESULT_OK; } -int genserKinematicsInverse(const EmcPose * world, - double *joints, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int genser_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * world, + double *joints, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { (void)iflags; (void)fflags; - genser_struct *genser = KINS_PTR; + genser_struct genser_stg; + genser_struct *genser = &genser_stg; 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_pose pos; // converted pose from EmcPose + go_pose pest, pestinv, Tdelta; go_rpy rpy; go_rvec rvec; go_cart cart; @@ -541,30 +543,19 @@ int genserKinematicsInverse(const EmcPose * world, int link; int smalls; int retval; + const unsigned max_iterations = (unsigned)p->geometry[P_MAX_ITER]; - // 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 + genser_links_of(p, genser); // 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_rpy_quat_convert(&rpy, &pos.rot); + pos.tran.x = world->tran.x; + pos.tran.y = world->tran.y; + 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); @@ -576,9 +567,10 @@ int genserKinematicsInverse(const EmcPose * world, } for (genser->iterations = 0; - genser->iterations < hal_get_ui32(haldata->max_iterations); + genser->iterations < max_iterations; genser->iterations++) { - hal_set_ui32(haldata->last_iterations, genser->iterations); + s->iterations = genser->iterations; + s->out[P_LAST_ITER] = genser->iterations; /* update the Jacobians */ for (link = 0; link < genser->link_num; link++) { go_link_joint_set(&genser->links[link], jest[link], &linkout[link]); @@ -597,8 +589,7 @@ int genserKinematicsInverse(const EmcPose * world, } /* 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]); + genser_kin_fwd(genser, jest, &pest); /* pestinv is its inverse */ go_pose_inv(&pest, &pestinv); /* @@ -612,7 +603,7 @@ int genserKinematicsInverse(const EmcPose * world, .Tdelta = pestinv * pos L 0 L */ - go_pose_pose_mult(&pestinv, haldata->pos, &Tdelta); + go_pose_pose_mult(&pestinv, &pos, &Tdelta); /* We need Tdelta in 0 frame, not pest frame, so rotate it @@ -641,9 +632,9 @@ int genserKinematicsInverse(const EmcPose * world, 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; + if (p->max_joints > 6) joints[6] = world->u; + if (p->max_joints > 7) joints[7] = world->v; + if (p->max_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++) { @@ -660,14 +651,10 @@ int genserKinematicsInverse(const EmcPose * world, for (link = 0; link < genser->link_num; link++) { // convert from radians back to angles joints[link] = jest[link] * 180 / PM_PI; - rtapi_s32 unrotate = hal_get_si32(haldata->unrotate[link]); + rtapi_s32 unrotate = unrotate_of(p, link); if ((link) && unrotate) joints[link] += unrotate * 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 */ @@ -681,6 +668,12 @@ int genserKinematicsInverse(const EmcPose * world, return GO_RESULT_ERROR; } +const kins_ops GENSER_OPS = { + .forward = genser_forward, + .inverse = genser_inverse, + .jacobian = genser_jacobian, +}; + /* 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 @@ -691,68 +684,3 @@ 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; - hal_set_ui32(haldata->max_iterations, i); - return GO_RESULT_OK; -} - -int genser_kin_inv_get_max_iterations() -{ - return hal_get_ui32(haldata->max_iterations); -} - -static const rtapi_real init_a[GENSER_MAX_JOINTS] = { - DEFAULT_A1, DEFAULT_A2, DEFAULT_A3, DEFAULT_A4, DEFAULT_A5, DEFAULT_A6 -}; -static const rtapi_real init_alpha[GENSER_MAX_JOINTS] = { - DEFAULT_ALPHA1, DEFAULT_ALPHA2, DEFAULT_ALPHA3, DEFAULT_ALPHA4, DEFAULT_ALPHA5, DEFAULT_ALPHA6 -}; -static const rtapi_real init_d[GENSER_MAX_JOINTS] = { - DEFAULT_D1, DEFAULT_D2, DEFAULT_D3, DEFAULT_D4, DEFAULT_D5, DEFAULT_D6 -}; - - -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 < GENSER_MAX_JOINTS; i++) { - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->a[i]), - init_a[i], "%s.A-%d", kp->halprefix, i); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->alpha[i]), - init_alpha[i], "%s.ALPHA-%d", kp->halprefix, i); - res += hal_pin_new_real(comp_id, HAL_IN, &(haldata->d[i]), - init_d[i], "%s.D-%d", kp->halprefix, i); - res += hal_pin_new_si32(comp_id, HAL_IN, &(haldata->unrotate[i]), - 0, "%s.unrotate-%d", kp->halprefix, i); - } - res += hal_pin_new_ui32(comp_id, HAL_OUT, &(haldata->last_iterations), - 0, "%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_new_ui32(comp_id, HAL_IN, &haldata->max_iterations, - GENSER_DEFAULT_MAX_ITERATIONS, "%s.max-iterations",kp->halprefix); - - if (res) {goto error;} - - genser_hal_inited = 1; - return 0; - -error: - return -1; -} // genserKinematicsSetup() diff --git a/src/emc/kinematics/genserkins.c b/src/emc/kinematics/genserkins.c index be71f33ffc5..09c72a9fcbe 100644 --- a/src/emc/kinematics/genserkins.c +++ b/src/emc/kinematics/genserkins.c @@ -4,7 +4,8 @@ * * NOTEs: * 1) specify all kparms items -* 2) specify 3 KS,KF,KI functions (setup,forward,inverse) +* 2) the maths and the geometry table are in genserfuncs.c, written as +* pure functions of the parameter block (see kinematics.h) */ /******************************************************************** @@ -57,41 +58,28 @@ int switchkinsSetup(kparms* kp, KI* kinv0, KI* kinv1, KI* kinv2 ) { + (void)kset0; (void)kset1; (void)kset2; + (void)kfwd0; (void)kfwd1; (void)kfwd2; + (void)kinv0; (void)kinv1; (void)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; + kp->params = GENSER_PARAMS; + kp->nparams = GENSER_NPARAMS; if (kp->sparm && strstr(kp->sparm,"identityfirst")) { rtapi_print("\n!!! switchkins-type 0 is IDENTITY\n"); - *kset0 = identityKinematicsSetup; - *kfwd0 = identityKinematicsForward; - *kinv0 = identityKinematicsInverse; - - *kset1 = genserKinematicsSetup; - *kfwd1 = genserKinematicsForward; - *kinv1 = genserKinematicsInverse; - switchkinsDeclare(0, KINSTYPE_IDENTITY); - switchkinsDeclare(1, KINSTYPE_PRIMARY); - switchkinsRegisterJacobian(1, genserKinematicsJacobian); + switchkinsRegisterOps(0, &KINS_IDENTITY_OPS); + switchkinsRegisterOps(1, &GENSER_OPS); } else { rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname); - *kset0 = genserKinematicsSetup; - *kfwd0 = genserKinematicsForward; - *kinv0 = genserKinematicsInverse; - switchkinsRegisterJacobian(0, genserKinematicsJacobian); - - *kset1 = identityKinematicsSetup; - *kfwd1 = identityKinematicsForward; - *kinv1 = identityKinematicsInverse; - switchkinsDeclare(0, KINSTYPE_PRIMARY); - switchkinsDeclare(1, KINSTYPE_IDENTITY); + switchkinsRegisterOps(0, &GENSER_OPS); + switchkinsRegisterOps(1, &KINS_IDENTITY_OPS); } - *kset2 = userkKinematicsSetup; - *kfwd2 = userkKinematicsForward; - *kinv2 = userkKinematicsInverse; + switchkinsRegisterOps(2, &USERK_OPS); return 0; } diff --git a/src/emc/kinematics/genserkins.h b/src/emc/kinematics/genserkins.h index b74b826d2ec..c5a2d9526f8 100644 --- a/src/emc/kinematics/genserkins.h +++ b/src/emc/kinematics/genserkins.h @@ -81,8 +81,6 @@ typedef 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); @@ -125,15 +123,6 @@ extern int genser_kin_fwd_interations(genser_struct * genser); 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, @@ -142,23 +131,14 @@ extern int compute_jfwd(go_link * link_params, extern int compute_jinv(go_matrix * Jfwd, go_matrix * Jinv); -extern int genserKinematicsJacobian(const double *joint, - const EmcPose *world, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS *iflags); - -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); +/* The kinematics as functions of the parameter block (see kinematics.h): + the DH parameters and the unrotate couplings are the table, the maths + is the ops. genser_links_of() fills a link description from a block, + for a caller that wants the go_ routines directly. */ +extern const kins_param_desc GENSER_PARAMS[]; +extern const int GENSER_NPARAMS; +extern const kins_ops GENSER_OPS; -extern int genserKinematicsSetup(const int comp_id, - const char* coordinates, - kparms* ksetup_parms); +extern void genser_links_of(const kins_params *p, genser_struct *genser); #endif diff --git a/src/emc/kinematics/pentakins.c b/src/emc/kinematics/pentakins.c index 551b79a4260..2436773d4bd 100644 --- a/src/emc/kinematics/pentakins.c +++ b/src/emc/kinematics/pentakins.c @@ -17,7 +17,7 @@ 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: + machine can be adjusted by hal pins: pentakins.base.N.x pentakins.base.N.y @@ -45,6 +45,10 @@ pentakins.tool-offset - tool length from the origin along z axis, changes the effector pivot point. + The maths is written as pure functions of the parameter block (see + kinematics.h): the pins above are the table below, read into the block + before every call, and the entry points come from kins_single.c. + ----------------------------------------------------------------------------*/ #include @@ -53,23 +57,51 @@ #include #include #include /* these decls, KINEMATICS_FORWARD_FLAGS */ +#include #include "pentakins.h" -struct haldata { - hal_real_t basex[NUM_STRUTS]; - hal_real_t basey[NUM_STRUTS]; - hal_real_t basez[NUM_STRUTS]; - hal_real_t effectorr[NUM_STRUTS]; - hal_real_t effectorz[NUM_STRUTS]; - hal_uint_t last_iter; - hal_uint_t max_iter; - hal_uint_t iter_limit; - hal_real_t max_error; - hal_real_t conv_criterion; - hal_real_t tool_offset; -} *haldata; +// the table: five struts' worth of geometry, then the iteration controls +// and reports. P_BASE_X(i) and the rest index it. +#define P_BASE_X(i) (5*(i) + 0) +#define P_BASE_Y(i) (5*(i) + 1) +#define P_BASE_Z(i) (5*(i) + 2) +#define P_EFF_R(i) (5*(i) + 3) +#define P_EFF_Z(i) (5*(i) + 4) +enum { + P_LAST_ITER = 5*NUM_STRUTS, + P_MAX_ITER, + P_MAX_ERROR, + P_CONV_CRITERION, + P_ITER_LIMIT, + P_TOOL_OFFSET, + P_COUNT +}; +#define STRUT_ROWS(i, bx, by, bz, er, ez) \ + { "base." #i ".x", KINS_PARAM_FLOAT, KINS_IN, 0, bx }, \ + { "base." #i ".y", KINS_PARAM_FLOAT, KINS_IN, 0, by }, \ + { "base." #i ".z", KINS_PARAM_FLOAT, KINS_IN, 0, bz }, \ + { "effector." #i ".r", KINS_PARAM_FLOAT, KINS_IN, 0, er }, \ + { "effector." #i ".z", KINS_PARAM_FLOAT, KINS_IN, 0, ez } + +static const kins_param_desc penta_params[P_COUNT] = { + STRUT_ROWS(0, DEFAULT_BASE_0_X, DEFAULT_BASE_0_Y, DEFAULT_BASE_0_Z, DEFAULT_EFFECTOR_0_R, DEFAULT_EFFECTOR_0_Z), + STRUT_ROWS(1, DEFAULT_BASE_1_X, DEFAULT_BASE_1_Y, DEFAULT_BASE_1_Z, DEFAULT_EFFECTOR_1_R, DEFAULT_EFFECTOR_1_Z), + STRUT_ROWS(2, DEFAULT_BASE_2_X, DEFAULT_BASE_2_Y, DEFAULT_BASE_2_Z, DEFAULT_EFFECTOR_2_R, DEFAULT_EFFECTOR_2_Z), + STRUT_ROWS(3, DEFAULT_BASE_3_X, DEFAULT_BASE_3_Y, DEFAULT_BASE_3_Z, DEFAULT_EFFECTOR_3_R, DEFAULT_EFFECTOR_3_Z), + STRUT_ROWS(4, DEFAULT_BASE_4_X, DEFAULT_BASE_4_Y, DEFAULT_BASE_4_Z, DEFAULT_EFFECTOR_4_R, DEFAULT_EFFECTOR_4_Z), + [P_LAST_ITER] = { "last-iterations", KINS_PARAM_U32, KINS_OUT, 0, 0 }, + [P_MAX_ITER] = { "max-iterations", KINS_PARAM_U32, KINS_OUT, 0, 0 }, + [P_MAX_ERROR] = { "max-error", KINS_PARAM_FLOAT, KINS_IO, 0, 100.0 }, + [P_CONV_CRITERION] = { "convergence-criterion", KINS_PARAM_FLOAT, KINS_IO, 0, 1e-9 }, + [P_ITER_LIMIT] = { "limit-iterations", KINS_PARAM_U32, KINS_IO, 0, 120 }, + [P_TOOL_OFFSET] = { "tool-offset", KINS_PARAM_FLOAT, KINS_IN, 1, 0.0 }, +}; + +// the most iterations a converged solution has taken this session, kept +// in the caller's scratch so each caller reports its own +#define MAX_ITER_SEEN(s) ((s)->aux[0]) /******************************* MatInvert5() ***************************/ @@ -180,31 +212,29 @@ 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**************************/ +/* the base and effector geometry of one call, taken from the block */ +typedef struct { + PmCartesian b[NUM_STRUTS]; + double za[NUM_STRUTS], ra[NUM_STRUTS]; +} penta_geometry; -int pentakins_read_hal_pins(void) { +static void geometry_of(const kins_params *p, penta_geometry *g) { int t; - - /* set the base and effector coordinates from hal pin values */ - rtapi_real tool_offset = hal_get_real(haldata->tool_offset); + const double tool_offset = p->tool.tran.z; for (t = 0; t < NUM_STRUTS; t++) { - b[t].x = hal_get_real(haldata->basex[t]); - b[t].y = hal_get_real(haldata->basey[t]); - b[t].z = hal_get_real(haldata->basez[t]) + tool_offset; - ra[t] = hal_get_real(haldata->effectorr[t]); - za[t] = hal_get_real(haldata->effectorz[t]) + tool_offset; + g->b[t].x = p->geometry[P_BASE_X(t)]; + g->b[t].y = p->geometry[P_BASE_Y(t)]; + g->b[t].z = p->geometry[P_BASE_Z(t)] + tool_offset; + g->ra[t] = p->geometry[P_EFF_R(t)]; + g->za[t] = p->geometry[P_EFF_Z(t)] + tool_offset; } - return 0; } /************************ InvKins() ********************************/ -int InvKins(const double * coord, - double * struts) +static int InvKins(const penta_geometry *g, + const double * coord, + double * struts) { PmCartesian xyz, pmcoord, temp; @@ -212,8 +242,6 @@ int InvKins(const double * coord, PmRpy rpy; int i; -// pentakins_read_hal_pins(); - /* define Rotation Matrix */ pmcoord.x = coord[0]; pmcoord.y = coord[1]; @@ -227,32 +255,30 @@ int InvKins(const double * coord, for (i = 0; i < NUM_STRUTS; i++) { /* convert location of effector strut end from effector to world coordinates */ - pmCartCartSub(&b[i], &pmcoord, &temp); + pmCartCartSub(&g->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]) ); + struts[i] = sqrt( sqr(xyz.z - g->za[i]) + sqr( sqrt(sqr(xyz.x) + sqr(xyz.y)) - g->ra[i]) ); } return 0; } -/**************************** kinematicsForward() ***************************/ +/**************************** penta_forward() ***************************/ -int kinematicsForward(const double * joints, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +static int penta_forward(const kins_params *p, kins_scratch *s, + 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; - + penta_geometry g; double Jacobian[NUM_STRUTS][NUM_STRUTS]; double InverseJacobian[NUM_STRUTS][NUM_STRUTS]; double InvKinStrutLength[NUM_STRUTS], StrutLengthDiff[NUM_STRUTS]; @@ -261,14 +287,11 @@ int kinematicsForward(const double * joints, 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(); + geometry_of(p, &g); /* abort on obvious problems, like joints <= 0 */ if (joints[0] <= 0.0 || @@ -287,12 +310,15 @@ int kinematicsForward(const double * joints, coord[4] = pos->b * PM_PI / 180.0; /* Enter Newton-Raphson iterative method */ - rtapi_real max_error = hal_get_real(haldata->max_error); + const double max_error = p->geometry[P_MAX_ERROR]; + const unsigned iter_limit = (unsigned)p->geometry[P_ITER_LIMIT]; + const double conv_criterion = p->geometry[P_CONV_CRITERION]; while (iterate) { /* check for large error and return error flag if no convergence */ if ((conv_err > +(max_error)) || (conv_err < -(max_error))) { /* we can't converge */ + s->failed = 1; return -2; }; @@ -300,22 +326,23 @@ int kinematicsForward(const double * joints, /* check iteration to see if the kinematics can reach the convergence criterion and return error flag if it can't */ - if (iteration > hal_get_ui32(haldata->iter_limit)) { + if (iteration > iter_limit) { /* we can't converge */ + s->failed = 1; 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); + InvKins(&g, 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); + InvKins(&g, coord, jointdelta); coord[i] -= 1e-4; for (j = 0; j < NUM_STRUTS; j++) { InverseJacobian[j][i] = (jointdelta[j] - InvKinStrutLength[j]) * 1e4; @@ -343,7 +370,6 @@ int kinematicsForward(const double * joints, /* enter loop to determine if a strut needs another iteration */ iterate = 0; /*assume iteration is done */ - rtapi_real conv_criterion = hal_get_real(haldata->conv_criterion); for (i = 0; i < NUM_STRUTS; i++) { if (fabs(StrutLengthDiff[i]) > conv_criterion) { iterate = 1; @@ -358,34 +384,37 @@ int kinematicsForward(const double * joints, pos->a = coord[3] * 180.0 / PM_PI; pos->b = coord[4] * 180.0 / PM_PI; - hal_set_ui32(haldata->last_iter, iteration); - - if (iteration > hal_get_ui32(haldata->max_iter)){ - hal_set_ui32(haldata->max_iter, iteration); + s->iterations = iteration; + s->failed = 0; + s->out[P_LAST_ITER] = iteration; + if (iteration > MAX_ITER_SEEN(s)) { + MAX_ITER_SEEN(s) = iteration; } + s->out[P_MAX_ITER] = MAX_ITER_SEEN(s); return 0; } -/************************ kinematicsInverse() ********************************/ +/************************ penta_inverse() ********************************/ /* 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) +static int penta_inverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, + double * joints, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { + (void)s; (void)iflags; (void)fflags; + penta_geometry g; double coord[NUM_STRUTS]; - pentakins_read_hal_pins(); + geometry_of(p, &g); coord[0] = pos->tran.x; coord[1] = pos->tran.y; @@ -393,18 +422,19 @@ int kinematicsInverse(const EmcPose * pos, coord[3] = pos->a * PM_PI / 180.0; coord[4] = pos->b * PM_PI / 180.0; - if (0 != InvKins(coord,joints)) { + if (0 != InvKins(&g, coord, joints)) { return -1; } return 0; } -int kinematicsJacobian(const double * joints, - const EmcPose * pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS * iflags) +static int penta_jacobian(const kins_params *p, const double * joints, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) { + penta_geometry g; PmRotationMatrix R; PmRpy rpy; PmCartesian P, d, xyz, wa, wb, dxyz[5]; @@ -412,7 +442,7 @@ int kinematicsJacobian(const double * joints, (void)joints; (void)iflags; - pentakins_read_hal_pins(); + geometry_of(p, &g); memset(jac, 0, EMCMOT_MAX_JOINTS * EMCMOT_MAX_AXIS * sizeof(jac[0][0])); /* InvKins() differentiated. The effector end of each strut is found in @@ -433,7 +463,7 @@ int kinematicsJacobian(const double * joints, for (i = 0; i < NUM_STRUTS; i++) { double rho, A, B, len; - pmCartCartSub(&b[i], &P, &d); + pmCartCartSub(&g.b[i], &P, &d); /* R^T d, written out since pmMatCartMult applies R */ xyz.x = R.x.x*d.x + R.x.y*d.y + R.x.z*d.z; xyz.y = R.y.x*d.x + R.y.y*d.y + R.y.z*d.z; @@ -460,8 +490,8 @@ int kinematicsJacobian(const double * joints, } rho = sqrt(sqr(xyz.x) + sqr(xyz.y)); - A = xyz.z - za[i]; - B = rho - ra[i]; + A = xyz.z - g.za[i]; + B = rho - g.ra[i]; len = sqrt(sqr(A) + sqr(B)); if (len <= 0 || rho <= 0) { return -1; } for (col = 0; col < 5; col++) { @@ -472,103 +502,43 @@ int kinematicsJacobian(const double * joints, return 0; } -KINEMATICS_TYPE kinematicsType() -{ - return KINEMATICS_BOTH; -} +// the forward iterates from the pose it is handed +static const kins_ops penta_ops = { + .forward = penta_forward, + .inverse = penta_inverse, + .jacobian = penta_jacobian, + .fwd_iterates = 1, +}; -KINS_NOT_SWITCHABLE -EXPORT_SYMBOL(kinematicsType); -EXPORT_SYMBOL(kinematicsForward); -EXPORT_SYMBOL(kinematicsInverse); -EXPORT_SYMBOL(kinematicsJacobian); +const kins_module_info kins_module = { + .name = "pentakins", + .halprefix = "pentakins", + .params = penta_params, + .nparams = P_COUNT, + .required_coordinates = "XYZAB", + .max_joints = NUM_STRUTS, + .allow_duplicates = 0, + .ntypes = 1, + .ops = { &penta_ops }, +}; MODULE_LICENSE("GPL"); int comp_id; -static const rtapi_real init_basex[NUM_STRUTS] = { - DEFAULT_BASE_0_X, DEFAULT_BASE_1_X, DEFAULT_BASE_2_X, DEFAULT_BASE_3_X, DEFAULT_BASE_4_X -}; -static const rtapi_real init_basey[NUM_STRUTS] = { - DEFAULT_BASE_0_Y, DEFAULT_BASE_1_Y, DEFAULT_BASE_2_Y, DEFAULT_BASE_3_Y, DEFAULT_BASE_4_Y -}; -static const rtapi_real init_basez[NUM_STRUTS] = { - DEFAULT_BASE_0_Z, DEFAULT_BASE_1_Z, DEFAULT_BASE_2_Z, DEFAULT_BASE_3_Z, DEFAULT_BASE_4_Z -}; -static const rtapi_real init_effectorr[NUM_STRUTS] = { - DEFAULT_EFFECTOR_0_R, DEFAULT_EFFECTOR_1_R, DEFAULT_EFFECTOR_2_R, DEFAULT_EFFECTOR_3_R, DEFAULT_EFFECTOR_4_R -}; -static const rtapi_real init_effectorz[NUM_STRUTS] = { - DEFAULT_EFFECTOR_0_Z, DEFAULT_EFFECTOR_1_Z, DEFAULT_EFFECTOR_2_Z, DEFAULT_EFFECTOR_3_Z, DEFAULT_EFFECTOR_4_Z -}; - 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 < NUM_STRUTS; i++) { - - if ((res = hal_param_new_real(comp_id, HAL_RW, &(haldata->basex[i]), - init_basex[i], "pentakins.base.%d.x", i)) < 0) - goto error; - - if ((res = hal_param_new_real(comp_id, HAL_RW, &haldata->basey[i], - init_basey[i], "pentakins.base.%d.y", i)) < 0) - goto error; - - if ((res = hal_param_new_real(comp_id, HAL_RW, &haldata->basez[i], - init_basez[i], "pentakins.base.%d.z", i)) < 0) - goto error; - - if ((res = hal_param_new_real(comp_id, HAL_RW, &haldata->effectorr[i], - init_effectorr[i], "pentakins.effector.%d.r", i)) < 0) - goto error; - - if ((res = hal_param_new_real(comp_id, HAL_RW, &haldata->effectorz[i], - init_effectorz[i], "pentakins.effector.%d.z", i)) < 0) - goto error; + if (kinsSingleInit(comp_id, &kins_module, "XYZAB", KINEMATICS_BOTH)) { + hal_exit(comp_id); + return -1; } - if ((res = hal_pin_new_ui32(comp_id, HAL_OUT, &haldata->last_iter, - 0, "pentakins.last-iterations")) < 0) - goto error; - - if ((res = hal_pin_new_ui32(comp_id, HAL_OUT, &haldata->max_iter, - 0, "pentakins.max-iterations")) < 0) - goto error; - - if ((res = hal_pin_new_real(comp_id, HAL_IO, &haldata->max_error, - 100.0, "pentakins.max-error")) < 0) - goto error; - - if ((res = hal_pin_new_real(comp_id, HAL_IO, &haldata->conv_criterion, - 1e-9, "pentakins.convergence-criterion")) < 0) - goto error; - - if ((res = hal_pin_new_ui32(comp_id, HAL_IO, &haldata->iter_limit, - 120, "pentakins.limit-iterations")) < 0) - goto error; - - if ((res = hal_pin_new_real(comp_id, HAL_IN, &haldata->tool_offset, - 0.0, "pentakins.tool-offset")) < 0) - goto error; - hal_ready(comp_id); return 0; - -error: - hal_exit(comp_id); - return res; } diff --git a/src/emc/kinematics/ugenserkins.c b/src/emc/kinematics/ugenserkins.c index 1d80e5ae70c..0d2c05b5a41 100644 --- a/src/emc/kinematics/ugenserkins.c +++ b/src/emc/kinematics/ugenserkins.c @@ -13,6 +13,7 @@ #include /* ulapi */ +#include #include /* struct timeval */ #include "genserkins.h" @@ -43,14 +44,25 @@ int main(int argc, char *argv[]) int retval = 0; double start, end; int comp_id; - kparms kp; - kp.max_joints = GENSER_MAX_JOINTS; - kp.allow_duplicates = 0; + kins_module_info info; + kins_params params; + kins_scratch scratch; - comp_id = hal_init("usergenserkins"); - if (genserKinematicsSetup(comp_id,"XYZABC",&kp)) printf("unexpected\n"); + /* the module described the way kinsDescribe() would, then a block at + the table defaults; setp has no say here */ + memset(&info, 0, sizeof(info)); + info.name = "genserkins"; + info.halprefix = "genserkins"; + info.params = GENSER_PARAMS; + info.nparams = GENSER_NPARAMS; + info.required_coordinates = "XYZABC"; + info.max_joints = GENSER_MAX_JOINTS; + info.ntypes = 1; + info.ops[0] = &GENSER_OPS; - genser_kin_init(); + comp_id = hal_init("usergenserkins"); + if (kinsParamsInit(¶ms, &info, "XYZABC")) printf("unexpected\n"); + kinsScratchInit(&scratch); /* syntax is a.out {i|f # # # # # #} */ if (argc == 8) { @@ -123,14 +135,14 @@ 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); + retval = GENSER_OPS.inverse(¶ms, &scratch, &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); + retval = GENSER_OPS.forward(¶ms, &scratch, joints, &pos, &fflags, &iflags); if (0 != retval) { printf("fwd kins error %d\n", retval); } @@ -220,14 +232,14 @@ joints[0],joints[1],joints[2],joints[3],joints[4],joints[5]); } else { fprintf(stderr,"gki1:\n"); retval = - genserKinematicsInverse(&pos, joints, &iflags, &fflags); + GENSER_OPS.inverse(¶ms, &scratch, &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); + GENSER_OPS.forward(¶ms, &scratch, 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) { @@ -271,13 +283,13 @@ fprintf(stderr,"gki1:\n"); &joints[0], &joints[1], &joints[2], &joints[3], &joints[4], &joints[5])) { printf("?\n"); } else { - retval = genserKinematicsForward(joints, &pos, &fflags, &iflags); + retval = GENSER_OPS.forward(¶ms, &scratch, 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); + retval = GENSER_OPS.inverse(¶ms, &scratch, &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) { From 1d82d71bfa5d92975ae8550246686d6fbf9398c8 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Fri, 4 Sep 2026 13:24:57 +1000 Subject: [PATCH 08/19] switchkinscomp: write the template on the parameter block The out-of-tree template declares its geometry as a table, writes its example type as ops over the block and supplies switchkinsSetup() like the in-tree modules. EXTRA_SETUP() runs it through switchkinsRunSetupWith() and kinsDescribe() replays it through switchkinsDescribeWith(), both exported by switchkins_core, so the template still builds with halcompile alone and includes no source. The kparms it built was never zeroed, which the grown struct would have turned into a crash; switchkinsRunSetupWith() zeroes it. --- src/hal/components/switchkinscomp.comp | 140 +++++++++++++++---------- 1 file changed, 87 insertions(+), 53 deletions(-) diff --git a/src/hal/components/switchkinscomp.comp b/src/hal/components/switchkinscomp.comp index a732ba0de35..3d04337fe66 100644 --- a/src/hal/components/switchkinscomp.comp +++ b/src/hal/components/switchkinscomp.comp @@ -17,6 +17,12 @@ replace with the kinematics wanted. The switchkins implementation is the switchkins_core module, which the HAL file loads first; this component only registers its types with it. +The kinematics are written as functions of a parameter block, see +kinematics.h and the Kinematics Conventions chapter: the geometry is +declared once in a table, one HAL pin is made per entry, and the maths +reads the block where it would have read a pin. The same maths can +then be evaluated outside realtime. + To avoid updates that overwrite switchkinscomp.comp, best practice is to rename the file and its component name (example: *user_switchkins.comp* creates module: *user_switchkins*). @@ -61,8 +67,8 @@ option extra_setup; // before this one. It provides kinematicsForward(), kinematicsInverse(), // kinematicsSwitch() and the rest of the kinematics interface, dispatches // each call to the currently selected switchkins-type, and supplies the -// identity kinematics and the coordinates letters-to-joints mapping they -// use. This module only registers its types. +// identity kinematics, the parameter block helpers and the coordinates +// letters-to-joints mapping. This module only supplies its types. #include //===================================================================== @@ -72,36 +78,31 @@ static char *coordinates; RTAPI_MP_STRING(coordinates, "Axes-to-joints-ordering"); //--------------------------------------------------------------------- -// Example switchkins-type. A setup routine creating whatever hal pins -// the kinematics need, plus a forward and an inverse routine. Replace -// the arithmetic with the real kinematics. - -static struct { - hal_real_t x_offset; -} *mydata; - -static int myKinematicsSetup(const int comp_id, - const char* coords, - kparms* kp) -{ - (void)coords; // this type does not use the coordinates mapping - - mydata = hal_malloc(sizeof(*mydata)); - if (!mydata) return -1; +// The geometry: one HAL pin per entry, named ., read +// into the block before every call. Add whatever the real kinematics +// need; an entry flagged as the tool arrives in p->tool.tran.z as well. - return hal_pin_new_real(comp_id, HAL_IN, &mydata->x_offset, 0.0, - "%s.x-offset", kp->halprefix); -} // myKinematicsSetup() +static const kins_param_desc my_params[] = { + { "x-offset", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, +}; +enum { P_X_OFFSET }; -static int myKinematicsForward(const double *j, - EmcPose * pos, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags) +//--------------------------------------------------------------------- +// Example switchkins-type: a forward and an inverse over the block. +// Replace the arithmetic with the real kinematics. The frames and the +// Jacobian are optional, see kinematics.h. + +static int myForward(const kins_params *p, kins_scratch *s, + const double *j, + EmcPose * pos, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) { + (void)s; (void)fflags; (void)iflags; - pos->tran.x = j[0] + hal_get_real(mydata->x_offset); + pos->tran.x = j[0] + p->geometry[P_X_OFFSET]; pos->tran.y = j[1]; pos->tran.z = j[2]; @@ -110,50 +111,83 @@ static int myKinematicsForward(const double *j, pos->u = pos->v = pos->w = 0; return 0; -} // myKinematicsForward() +} // myForward() -static int myKinematicsInverse(const EmcPose * pos, - double *j, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags) +static int myInverse(const kins_params *p, kins_scratch *s, + const EmcPose * pos, + double *j, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) { + (void)s; (void)iflags; (void)fflags; - j[0] = pos->tran.x - hal_get_real(mydata->x_offset); + j[0] = pos->tran.x - p->geometry[P_X_OFFSET]; j[1] = pos->tran.y; j[2] = pos->tran.z; return 0; -} // myKinematicsInverse() +} // myInverse() + +static const kins_ops my_ops = { + .forward = myForward, + .inverse = myInverse, +}; + +//--------------------------------------------------------------------- +// The module's configuration and its switchkins-types. Type 0 is the +// startup default. Types run from 0 to SWITCHKINS_MAX_TYPES-1 with no +// gaps. + +int switchkinsSetup(kparms* kp, + KS* kset0, KS* kset1, KS* kset2, + KF* kfwd0, KF* kfwd1, KF* kfwd2, + KI* kinv0, KI* kinv1, KI* kinv2 + ) +{ + // the pointer arguments are the older way of providing types 0 to 2 + (void)kset0; (void)kset1; (void)kset2; + (void)kfwd0; (void)kfwd1; (void)kfwd2; + (void)kinv0; (void)kinv1; (void)kinv2; + + kp->kinsname = "switchkinscomp"; // must agree with the module name + kp->halprefix = "switchkinscomp"; // hal pin names + kp->required_coordinates = "xyz"; + kp->allow_duplicates = 0; + kp->fwd_iterates_mask = 0; // set bit N if type N iterates + kp->gui_kinstype = -1; // negative means: not used + kp->max_joints = strlen(kp->required_coordinates); + kp->params = my_params; + kp->nparams = sizeof(my_params)/sizeof(my_params[0]); + + if (switchkinsRegisterOps(0, &KINS_IDENTITY_OPS)) { return -1; } + if (switchkinsRegisterOps(1, &my_ops)) { return -1; } + return 0; +} // switchkinsSetup() + +//--------------------------------------------------------------------- +// A copy of the module loaded outside realtime describes itself by +// replaying switchkinsSetup(). + +int kinsDescribe(const char *coords, const char *sparm, + kins_module_info *info) +{ + return switchkinsDescribeWith(switchkinsSetup, coords, sparm, info); +} // kinsDescribe() + +EXPORT_SYMBOL(kinsDescribe); //--------------------------------------------------------------------- // rtapi_app_main() is supplied by halcompile, which calls hal_init() // before EXTRA_SETUP() and hal_ready() after it. That is what -// switchkinsInit() expects, so the switchkins-types are registered and -// the implementation started from here. +// switchkinsInit() expects, so setup is run and the implementation +// started from here. EXTRA_SETUP() { kparms kp; (void)__comp_inst; (void)prefix; (void)extra_arg; - kp.kinsname = "switchkinscomp"; // must agree with the module name - kp.halprefix = "switchkinscomp"; // hal pin names - kp.required_coordinates = "xyz"; - kp.allow_duplicates = 0; - kp.fwd_iterates_mask = 0; // set bit N if type N iterates - kp.gui_kinstype = -1; // negative means: not used - kp.sparm = NULL; - kp.max_joints = strlen(kp.required_coordinates); - - // switchkins-type 0 is the startup default. Types run from 0 to - // SWITCHKINS_MAX_TYPES-1 with no gaps. - if (switchkinsRegister(0, identityKinematicsSetup, - identityKinematicsForward, - identityKinematicsInverse)) { return -1; } - if (switchkinsRegister(1, myKinematicsSetup, - myKinematicsForward, - myKinematicsInverse)) { return -1; } - + if (switchkinsRunSetupWith(switchkinsSetup, &kp, NULL)) { return -1; } return switchkinsInit(comp_id, &kp, coordinates); } // EXTRA_SETUP() From 99a495bdc95f40b3bbfcaf942be8a7704ffa0a60 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Fri, 11 Sep 2026 19:55:34 +0800 Subject: [PATCH 09/19] switchkins: declare the primary and the machine frame kinstype from the ops table A kins_ops table now says what its type IS the same way it says the maths: .primary marks the module's working transform, and switchkinsRegisterOps() bridges it into the declared flags exactly like .identity. G43.4 resolves the switch target from the flag, so the number of the working kinematics stops being a guess. Like identity, primary may be declared for at most one kinstype; two answers fail the module load. Every in-tree working transform declares it: fiveaxis, the two trt tables, scara, puma, genser, genhex, three21, millturn, tdr and the trsrn tcp tables. The trsrn tool kinematics stays undeclared: it is a reporting mode, not the working transform. A third flag, KINSTYPE_MACHINE, names the machine frame type: the machine with the orientation left out, XYZ the pivot in machine coordinates and the rotary letters the rotary joints. On a machine whose slides line up with its frame that is the identity type, so a module that declares none has its identity type stand in, and every in-tree module is of that kind. A module whose carriage does not line up, a slanted slide or a composite Y on a mill-turn, declares its machine frame type separately and keeps identity for a type whose joints really are the axes, since the shortcuts taken on the identity flag (the Jacobian, the tool offset change, the user-space loader's KINEMATICS_IDENTITY) would be wrong on a transform. G13.1 and G49 cancel to the machine frame type; one at most, as for the others. --- docs/src/motion/switchkins.adoc | 42 +++++++++++++++++--------- include/kinematics.h | 27 ++++++++++++++--- include/switchkins.h | 6 ++-- src/emc/kinematics/5axiskins.c | 1 + src/emc/kinematics/genhexkins.c | 1 + src/emc/kinematics/genserfuncs.c | 1 + src/emc/kinematics/kins_single.c | 8 +++++ src/emc/kinematics/pumakins.c | 1 + src/emc/kinematics/scarakins.c | 1 + src/emc/kinematics/switchkins.c | 37 ++++++++++++++++++----- src/emc/kinematics/three21kins.c | 1 + src/emc/kinematics/trtfuncs.c | 2 ++ src/emc/rs274ngc/interp_convert.cc | 16 +++++----- src/emc/rs274ngc/rs274ngc_return.hh | 2 +- src/hal/components/millturn.comp | 1 + src/hal/components/xyzab_tdr_kins.comp | 1 + src/hal/components/xyzacb_trsrn.comp | 1 + src/hal/components/xyzbca_trsrn.comp | 1 + 18 files changed, 112 insertions(+), 38 deletions(-) diff --git a/docs/src/motion/switchkins.adoc b/docs/src/motion/switchkins.adoc index e0a4586ffbd..a15c82c1547 100644 --- a/docs/src/motion/switchkins.adoc +++ b/docs/src/motion/switchkins.adoc @@ -181,8 +181,9 @@ meant to be removed in the future. === G-code commands 'G12.1 P-' selects a kinstype and 'G13.1' cancels back to identity -kinematics. Which kinstype is identity is declared by the module (see -Code Notes), not fixed to a number: +kinematics. Which kinstype that is, the module declares (see Code +Notes), not fixed to a number; on a module whose slides do not line up +with its frame it is the machine frame type the module declares instead: [source,ngc] ---- @@ -218,9 +219,9 @@ Selection is not cancelled by the end of a program or by an abort, so that the kinstype continues to match the position readout. A program that should leave the machine in identity kinematics ends with 'G13.1'. -A module that declares no identity kinstype refuses 'G13.1' with an -error and can still be driven by number with 'G12.1'; see Code Notes -for how a module declares its types. +A module that declares no identity or machine frame kinstype refuses +'G13.1' with an error and can still be driven by number with 'G12.1'; +see Code Notes for how a module declares its types. For tool length work there are spellings that name the kinematics by what it is rather than by number: 'G43.4' applies the tool length @@ -561,26 +562,37 @@ kinematics.h: . *KINSTYPE_IDENTITY* no transform: the joints are the world . *KINSTYPE_PRIMARY* the module's working transform +. *KINSTYPE_MACHINE* the machine frame: the pivot in machine coordinates, + the rotaries as joints A kinstype registered with switchkinsRegisterOps() carries its flag in -the ops table itself, as the 'identity' or 'primary' field; a kinstype -registered the older way gets it from a call, again from within +the ops table itself, as the 'identity', 'primary' or 'machine' field; a +kinstype registered the older way gets it from a call, again from within switchkinsSetup(): ---- int switchkinsDeclare(int ktype, int flags); ---- -G-code reads these declarations: 'G13.1' cancels to the kinstype -declared KINSTYPE_IDENTITY, and 'G43.4' switches to the kinstype +G-code reads these declarations: 'G13.1' and 'G49' cancel to the +kinstype declared KINSTYPE_MACHINE, and 'G43.4' switches to the kinstype declared KINSTYPE_PRIMARY, whatever their numbers, so a module whose kinematics are not in the conventional order still gets working -spellings. At most one kinstype may be declared identity and at most -one primary, and declaring a kinstype the module does not provide -fails the module load. A module that declares nothing keeps working -exactly as before for 'G12.1 P-' and 'G49', but 'G13.1' and 'G43.4' -are an error, since the numbers of the identity and primary kinematics -are then a guess. +spellings. The machine frame type is the machine with the orientation +left out, XYZ the pivot in machine coordinates and the rotary letters +the rotary joints; on a machine whose slides line up with its frame that +is the identity type, so a module that declares no machine frame +kinstype has its identity kinstype stand in, and every shipped module is +of that kind. A module whose carriage does not line up, a slanted slide +or a composite Y on a mill-turn, declares its machine frame kinstype +separately and keeps KINSTYPE_IDENTITY for a kinstype whose joints +really are the axes, since motion and the planner skip the maths on that +flag alone. At most one kinstype may be declared identity, at most one +primary and at most one machine frame, and declaring a kinstype the +module does not provide fails the module load. A module that declares +nothing keeps working exactly as before for 'G12.1 P-' and 'G49', but +'G13.1' and 'G43.4' are an error, since the numbers of the identity and +primary kinematics are then a guess. A module that implements kinematicsTypeFlags() itself, without switchkins.c, returns the flags of every kinstype it can switch to, 0 diff --git a/include/kinematics.h b/include/kinematics.h index 20511017fd5..a2911224f6a 100644 --- a/include/kinematics.h +++ b/include/kinematics.h @@ -116,12 +116,25 @@ extern KINEMATICS_TYPE kinematicsType(void); /* What a kinematics type IS, declared by the module with ** switchkinsDeclare() and read back with kinematicsTypeFlags(). -** G13.1 resolves "identity" from these flags instead of assuming a -** number; a module that declares nothing leaves its types numeric-only -** and G13.1 refuses to guess. +** G13.1 resolves the machine frame type from these flags instead of +** assuming a number; a module that declares nothing leaves its types +** numeric-only and G13.1 refuses to guess. +** +** The machine frame type is the one whose world is the machine frame +** with the orientation left out: XYZ is the pivot, the point the rotary +** joints do not move, in machine coordinates, and the rotary letters are +** the rotary joints as they are. On a machine whose slides line up with +** its frame that is the identity type, and where a module declares no +** machine frame type its identity type stands in. A module whose +** carriage does not line up, a slanted slide or an offset pivot, declares +** its machine frame type separately and keeps identity for a type whose +** joints really are the axes, since a consumer skips the maths on that +** flag alone. */ #define KINSTYPE_IDENTITY 0x1 /* no transform: the joints are the world */ #define KINSTYPE_PRIMARY 0x2 /* the module's working transform */ +#define KINSTYPE_MACHINE 0x4 /* the machine frame: the pivot in machine + coordinates, the rotaries as joints */ /* flags of a kinematics type, 0 when neither applies, or -1 for a type ** the module does not provide (and for every type on a machine with plain @@ -585,7 +598,11 @@ typedef int (*kins_jacobian_fn)(const kins_params *p, const double *joint, a missing Jacobian is differenced from the inverse. fwd_iterates says the forward starts from the pose it is handed, so the shared code seeds it with the last answer after a switch. identity says joints are axes, which - a consumer may use to skip the maths altogether. */ + a consumer may use to skip the maths altogether. primary says this is + the module's working transform, the type G43.4 switches to. machine says + this is the machine frame type, the pivot in machine coordinates with the + rotaries as joints, which G13.1 and G49 select and G53.5 moves in; a module + that leaves it unset on every type has its identity type stand in. */ typedef struct kins_ops { kins_forward_fn forward; kins_inverse_fn inverse; @@ -595,6 +612,8 @@ typedef struct kins_ops { kins_jacobian_fn jacobian; int fwd_iterates; int identity; /* joints are axes */ + int primary; /* the working transform */ + int machine; /* the machine frame */ } kins_ops; /* A module described for a caller outside RT: its table, its joint diff --git a/include/switchkins.h b/include/switchkins.h index 336e662a951..dd239dd97ce 100644 --- a/include/switchkins.h +++ b/include/switchkins.h @@ -76,9 +76,9 @@ typedef int (*KTI)(const PmCartesian *axis_in_work, extern int switchkinsRegisterToolFrameInverse(int ktype, KTI kinv); // optionally called from switchkinsSetup() to declare what a type IS -// (KINSTYPE_IDENTITY, KINSTYPE_PRIMARY, kinematics.h). A module that -// never calls it leaves its types numeric-only: G12.1 P still works, -// G13.1 refuses to guess which type is identity. +// (KINSTYPE_IDENTITY, KINSTYPE_PRIMARY, KINSTYPE_MACHINE, kinematics.h). +// A module that never calls it leaves its types numeric-only: G12.1 P +// still works, G13.1 refuses to guess which type is the machine frame. extern int switchkinsDeclare(int ktype, int flags); // KinematicsJACOBIAN function (optional, see kinematics.h) diff --git a/src/emc/kinematics/5axiskins.c b/src/emc/kinematics/5axiskins.c index cf2e0254da4..d1829b483b0 100644 --- a/src/emc/kinematics/5axiskins.c +++ b/src/emc/kinematics/5axiskins.c @@ -204,6 +204,7 @@ static const kins_ops fiveaxis_ops = { .forward = fiveaxis_forward, .inverse = fiveaxis_inverse, .jacobian = fiveaxis_jacobian, + .primary = 1, }; int switchkinsSetup(kparms* kp, diff --git a/src/emc/kinematics/genhexkins.c b/src/emc/kinematics/genhexkins.c index fa5276f5a56..4deb523d9a3 100644 --- a/src/emc/kinematics/genhexkins.c +++ b/src/emc/kinematics/genhexkins.c @@ -693,6 +693,7 @@ static const kins_ops genhex_ops = { .inverse = genhex_inverse, .jacobian = genhex_jacobian, .fwd_iterates = 1, + .primary = 1, }; int switchkinsSetup(kparms* kp, diff --git a/src/emc/kinematics/genserfuncs.c b/src/emc/kinematics/genserfuncs.c index 7a46d1cfdcc..02859829cf3 100644 --- a/src/emc/kinematics/genserfuncs.c +++ b/src/emc/kinematics/genserfuncs.c @@ -672,6 +672,7 @@ const kins_ops GENSER_OPS = { .forward = genser_forward, .inverse = genser_inverse, .jacobian = genser_jacobian, + .primary = 1, }; /* diff --git a/src/emc/kinematics/kins_single.c b/src/emc/kinematics/kins_single.c index f523b7c394b..237a15ea795 100644 --- a/src/emc/kinematics/kins_single.c +++ b/src/emc/kinematics/kins_single.c @@ -133,6 +133,13 @@ int kinematicsSwitch(int switchkins_type) return 0; } +// one type and nothing declared about it: -1, no information +int kinematicsTypeFlags(int ktype) +{ + (void)ktype; + return -1; +} + // The module's description, for a copy of it loaded outside RT. A module // with one type does not depend on its parameters for its shape, so this // is the table as declared. @@ -152,5 +159,6 @@ EXPORT_SYMBOL(kinematicsToolFrame); EXPORT_SYMBOL(kinematicsJacobian); EXPORT_SYMBOL(kinematicsSwitchable); EXPORT_SYMBOL(kinematicsSwitch); +EXPORT_SYMBOL(kinematicsTypeFlags); EXPORT_SYMBOL(kinsSingleInit); EXPORT_SYMBOL(kinsSingleDescribe); diff --git a/src/emc/kinematics/pumakins.c b/src/emc/kinematics/pumakins.c index adac1298e0e..58e50f7c5b6 100644 --- a/src/emc/kinematics/pumakins.c +++ b/src/emc/kinematics/pumakins.c @@ -426,6 +426,7 @@ static const kins_ops puma_ops = { .work = kinsIdentityFrame, .tool = puma_tool_frame, .native = &TOOL_FRAME_FLANGE, + .primary = 1, }; int switchkinsSetup(kparms* kp, diff --git a/src/emc/kinematics/scarakins.c b/src/emc/kinematics/scarakins.c index 34035b8adac..19820930c91 100644 --- a/src/emc/kinematics/scarakins.c +++ b/src/emc/kinematics/scarakins.c @@ -249,6 +249,7 @@ static const kins_ops scara_ops = { .forward = scara_forward, .inverse = scara_inverse, .jacobian = scara_jacobian, + .primary = 1, }; int switchkinsSetup(kparms* kp, diff --git a/src/emc/kinematics/switchkins.c b/src/emc/kinematics/switchkins.c index 2734afbd259..b554e4e96df 100644 --- a/src/emc/kinematics/switchkins.c +++ b/src/emc/kinematics/switchkins.c @@ -61,8 +61,8 @@ static int inited; static int kins_count; static int register_error; -// what each type IS (KINSTYPE_IDENTITY, KINSTYPE_PRIMARY), declared by -// the module with switchkinsDeclare(); 0==the module said nothing +// what each type IS (KINSTYPE_IDENTITY, KINSTYPE_PRIMARY, KINSTYPE_MACHINE), +// declared by the module with switchkinsDeclare(); 0==the module said nothing static int ktype_flags[SWITCHKINS_MAX_TYPES] = {0}; static int switchkins_type; @@ -482,6 +482,8 @@ int switchkinsRegisterOps(int ktype, const kins_ops *ops) } kops[ktype] = ops; if (ops->identity) { ktype_flags[ktype] |= KINSTYPE_IDENTITY; } + if (ops->primary) { ktype_flags[ktype] |= KINSTYPE_PRIMARY; } + if (ops->machine) { ktype_flags[ktype] |= KINSTYPE_MACHINE; } return 0; } // switchkinsRegisterOps() @@ -685,7 +687,7 @@ int switchkinsInit(const int comp_id, const char* coordinates) { int i; - int identities; + int identities, primaries, machines; int res = 0; char* emsg = "other"; @@ -713,9 +715,17 @@ int switchkinsInit(const int comp_id, } if (!kins_count) { emsg = "no switchkins-types provided"; goto error; } - // declarations must name provided types, and identity is unique: - // G13.1 resolves it from the flags, so two answers is a load error + // declarations must name provided types, and each flag is unique: + // G13.1 and G49 resolve the machine frame type from the flags and + // G43.4 the primary, so two answers is a load error. A module that + // names no machine frame type has its identity type stand in, which + // is the truth on every machine whose slides line up with its frame. identities = 0; + primaries = 0; + machines = 0; + for (i=0; i < SWITCHKINS_MAX_TYPES; i++) { + if (ktype_flags[i] & KINSTYPE_MACHINE) { machines++; } + } for (i=0; i < SWITCHKINS_MAX_TYPES; i++) { if (!ktype_flags[i]) { continue; } if (i >= kins_count) { @@ -724,14 +734,25 @@ int switchkinsInit(const int comp_id, " not provided\n", i); emsg = "declared switchkins-type not provided"; goto error; } - if (ktype_flags[i] & KINSTYPE_IDENTITY) { identities++; } - rtapi_print("switchkins-type %d declared:%s%s\n", i, + if (ktype_flags[i] & KINSTYPE_IDENTITY) { + identities++; + if (!machines) { ktype_flags[i] |= KINSTYPE_MACHINE; } + } + if (ktype_flags[i] & KINSTYPE_PRIMARY) { primaries++; } + rtapi_print("switchkins-type %d declared:%s%s%s\n", i, (ktype_flags[i] & KINSTYPE_IDENTITY) ? " identity" : "", - (ktype_flags[i] & KINSTYPE_PRIMARY) ? " primary" : ""); + (ktype_flags[i] & KINSTYPE_PRIMARY) ? " primary" : "", + (ktype_flags[i] & KINSTYPE_MACHINE) ? " machine" : ""); } if (identities > 1) { emsg = "more than one identity switchkins-type declared"; goto error; } + if (primaries > 1) { + emsg = "more than one primary switchkins-type declared"; goto error; + } + if (machines > 1) { + emsg = "more than one machine frame switchkins-type declared"; goto error; + } for (i=0; i < SWITCHKINS_MAX_TYPES; i++) { if (kp.fwd_iterates_mask & (1<kins_by_g43_4) { // G49 undoes what G43.4 did: after the cancel it drops the machine - // to identity kinematics, as if G13.1 had run on the next line. A - // kinematics the program selected itself is left alone, and a - // module that declares no identity type keeps the plain cancel. - int identity = flagged_kins_type(KINSTYPE_IDENTITY); + // to the machine frame type, identity kinematics on a machine whose + // slides line up, as if G13.1 had run on the next line. A kinematics + // the program selected itself is left alone, and a module that + // declares no such type keeps the plain cancel. + int identity = flagged_kins_type(KINSTYPE_MACHINE); if (identity >= 0) { switch_kins_type(identity, settings); } settings->kins_by_g43_4 = false; } @@ -6901,9 +6902,10 @@ int Interp::convert_kins_switch(int code, //!< G_12_1 or G_13_1 int kins_type; if (code == G_13_1) { - // G13.1 cancels to identity kinematics; which type that is, the - // module declares, the number is not the answer - kins_type = flagged_kins_type(KINSTYPE_IDENTITY); + // G13.1 cancels to the machine frame type, identity kinematics on a + // machine whose slides line up; which type that is, the module + // declares, the number is not the answer + kins_type = flagged_kins_type(KINSTYPE_MACHINE); if (kins_type < 0) { CHKS(kins_type_info_available(), NCE_NO_IDENTITY_KINEMATICS_TYPE); kins_type = 0; // no kinematics attached: standalone interpreter diff --git a/src/emc/rs274ngc/rs274ngc_return.hh b/src/emc/rs274ngc/rs274ngc_return.hh index f7f8dfcacfc..620f6ecfee6 100644 --- a/src/emc/rs274ngc/rs274ngc_return.hh +++ b/src/emc/rs274ngc/rs274ngc_return.hh @@ -207,7 +207,7 @@ #define NCE_QUEUE_IS_NOT_EMPTY_AFTER_INPUT _("Queue is not empty after external input") #define NCE_QUEUE_IS_NOT_EMPTY_AFTER_KINS_SWITCH _("Queue is not empty after Kinematics Switch") #define NCE_KINS_TYPE_NOT_PROVIDED _("G12.1 P word does not name a kinematics type this module provides") -#define NCE_NO_IDENTITY_KINEMATICS_TYPE _("G13.1 needs the kinematics module to declare its identity type (see the switchkins documentation)") +#define NCE_NO_IDENTITY_KINEMATICS_TYPE _("G13.1 needs the kinematics module to declare its identity or machine frame type (see the switchkins documentation)") #define NCE_NO_PRIMARY_KINEMATICS_TYPE _("G43.4 needs the kinematics module to declare its primary type (see the switchkins documentation)") #define NCE_ANALOG_INPUT_WITH_WAIT_NOT_IMMEDIATE _("Can't select analog input with wait type != immediate return") #define NCE_ZERO_TIMEOUT_WITH_WAIT_NOT_IMMEDIATE _("Zero timeout with wait type != immediate return") diff --git a/src/hal/components/millturn.comp b/src/hal/components/millturn.comp index 1cec163018e..2f385bff5c5 100644 --- a/src/hal/components/millturn.comp +++ b/src/hal/components/millturn.comp @@ -118,6 +118,7 @@ static const kins_ops turn_ops = { .forward = turn_forward, .inverse = turn_inverse, .jacobian = turn_jacobian, + .primary = 1, }; int switchkinsSetup(kparms* kp, diff --git a/src/hal/components/xyzab_tdr_kins.comp b/src/hal/components/xyzab_tdr_kins.comp index 1125b22ba06..e1616648a51 100644 --- a/src/hal/components/xyzab_tdr_kins.comp +++ b/src/hal/components/xyzab_tdr_kins.comp @@ -200,6 +200,7 @@ static const kins_ops tdr_ops = { .forward = tdr_forward, .inverse = tdr_inverse, .jacobian = tdr_jacobian, + .primary = 1, }; int switchkinsSetup(kparms* kp, diff --git a/src/hal/components/xyzacb_trsrn.comp b/src/hal/components/xyzacb_trsrn.comp index 858d5864ec3..1ed5759ca6a 100644 --- a/src/hal/components/xyzacb_trsrn.comp +++ b/src/hal/components/xyzacb_trsrn.comp @@ -535,6 +535,7 @@ static const kins_ops tcp_ops = { .tool = tcpKinematicsToolFrame, .native = &TOOL_FRAME_SPINDLE, .jacobian = tcpKinematicsJacobian, + .primary = 1, }; // the tool kinematics report in tool axes, so the tool is square with the diff --git a/src/hal/components/xyzbca_trsrn.comp b/src/hal/components/xyzbca_trsrn.comp index 6691a752075..80c7ca20305 100644 --- a/src/hal/components/xyzbca_trsrn.comp +++ b/src/hal/components/xyzbca_trsrn.comp @@ -532,6 +532,7 @@ static const kins_ops tcp_ops = { .tool = tcpKinematicsToolFrame, .native = &TOOL_FRAME_SPINDLE, .jacobian = tcpKinematicsJacobian, + .primary = 1, }; // the tool kinematics report in tool axes, so the tool is square with the From 2729edb5315dd2d94e87656ae6b0da5cce79b66e Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Fri, 4 Sep 2026 13:24:58 +1000 Subject: [PATCH 10/19] docs: describe the parameter block form of a kinematics module The conventions chapter gains a section on the two blocks, the table, the ops table and what the shared code does with them in and outside realtime, and the Writing a Module list gains "no state". The frames and Jacobian sections point at the ops table where they pointed at the register calls. The switchkins chapter's Code Notes describe switchkinsRegisterOps(), the table in kparms, switchkinsRunSetup() and kinsDescribe(), keep the older registration as the older form, and the outline is a module written the new way. --- docs/src/motion/kinematics-conventions.adoc | 107 ++++++++++++- docs/src/motion/switchkins.adoc | 161 ++++++++++++-------- 2 files changed, 197 insertions(+), 71 deletions(-) diff --git a/docs/src/motion/kinematics-conventions.adoc b/docs/src/motion/kinematics-conventions.adoc index 5740fbc7012..3b58d13d653 100644 --- a/docs/src/motion/kinematics-conventions.adoc +++ b/docs/src/motion/kinematics-conventions.adoc @@ -178,7 +178,7 @@ half turn about one of the two transverse axes, and which one is chosen decides where tool X lands. Because it is a rotation in its own right, a module declares it rather than -applying it by hand, as the last argument of `switchkinsRegisterFrames()`. +applying it by hand, in the `native` field of its ops table. Shared code applies it and checks once, at load, that it is orthonormal with determinant +1. `TOOL_FRAME_SPINDLE` is the identity, for a module whose maths is already in the convention; `TOOL_FRAME_FLANGE` is the half turn a @@ -289,6 +289,8 @@ All of these are functions of the joint values and the module's own geometry. None needs state carried between calls, and none needs the module to be running in a realtime thread to be useful: the interesting callers, a limit check before a move and a preview before a program runs, are not in the servo loop. +<> is how a module is written so that they +can call it. [[sec:orientation-inverse]] == The Orientation Inverse @@ -438,28 +440,28 @@ every consumer would otherwise guess it. === What a module has to supply -Nothing. If a module registers no Jacobian, the shared code computes one by +Nothing. If a module supplies no Jacobian, the shared code computes one by central differences: it calls the module's inverse eighteen times, stepping each pose coordinate a small amount to either side on the solution branch the inverse flags select, and differences the results. The answer is as precise as the inverse itself; behind an inverse that iterates, that is the -iteration's convergence tolerance divided by the step. Modules built on `switchkins.c` answer this way for every type that -registers nothing; an identity type answers exactly. +iteration's convergence tolerance divided by the step. Modules built on `switchkins.c` answer this way for every type +whose ops table has no Jacobian; an identity type answers exactly. This cost matters because `kinematicsJacobian()` is meant to run in the servo thread, where checking a feed limit calls it once per cycle: each call then costs eighteen inverses. On the closed-form inverses in the tree that is under two microseconds. `genserkins`, the one module whose inverse iterates, would cost near eighty microseconds per call this way, against -under two for the geometric Jacobian it registers instead. +under two for the geometric Jacobian it supplies instead. -So a module whose inverse iterates should register a closed-form Jacobian, -as below. Nothing enforces this. A module that registers nothing still gets +So a module whose inverse iterates should supply a closed-form Jacobian, +as below. Nothing enforces this. A module that supplies none still gets a correct Jacobian, just one that costs eighteen inverses and carries the iteration's precision, which behind an iterating inverse is too slow for the servo thread. -A module with a closed form registers it with `switchkinsRegisterJacobian()`. +A module with a closed form puts it in the `jacobian` field of its ops table. It is exact, it costs what the inverse costs, and it knows its own singular poses rather than discovering them as an inverse that fails a step away from the pose. Every module in the tree supplies one. The two arms whose inverse is @@ -476,6 +478,88 @@ rather than from the pose, which the nutating heads do, has an inverse whose derivative about the pose is not the coupling the machine has. Such a module supplies the closed form, taken against the pose. +[[sec:parameters]] +== The Parameter Block + +Everything above is a function of the joint values, the tool and the machine's +geometry. A module written the old way reads its geometry from HAL pins it +created, keeps its kinematics type and its iteration scratch in statics, and so +can only answer for the machine as it is now, from inside the realtime thread. +Anything else that needs the same maths, a planner evaluating poses the machine +has not reached, task checking a program at load, a tool asking what if, had to +carry a second copy of it, and the two copies drift. + +A module is written instead as functions of two blocks the caller supplies. +`kins_params` describes the machine: the kinematics type, the joint map from +`coordinates=`, the tool offset, and the geometry as an array of doubles. One +copy may be shared by any number of callers, since nothing writes it during a +call. `kins_scratch` is what one caller carries between its own calls: the pose +an iterating forward last found, which seeds the next, and what the module +reports about the call it just made. It is never shared between callers, so +motion and a planner evaluating the same module cannot disturb each other. + +=== The table + +A module declares its geometry as a table of named entries, one per value it +reads. The name is the pin name the config already uses, less the module +prefix, so nothing in a config changes. + +[source,c] +---- +static const kins_param_desc fiveaxis_params[] = { + { "pivot-length", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_PIVOT_LENGTH }, +}; +enum { P_PIVOT_LENGTH }; +---- + +An entry is an input, an output, or an input that can be poked (`KINS_IO`, a +`HAL_IO` pin). The maths reads `p->geometry[P_PIVOT_LENGTH]` where it read a +pin, and writes an output into `s->out[]` at the same index. An entry flagged +as the tool is the tool length along the tool axis; the shared code puts its +value in `p->tool.tran.z` as well, which is what the maths reads, so that a +caller outside realtime can supply the tool from the tool table without there +being a pin. + +=== The ops table + +The maths of one kinematics type is a `kins_ops` table: the forward and inverse, +the optional work and tool frames with the native rotation that relates the +tool frame to the convention, and the optional Jacobian. A type whose forward +iterates from the pose it is handed says so, and the shared code seeds it with +the last answer after a switch. A type also says what it IS: `identity` marks +the no-transform type `G13.1` cancels to, `primary` the working transform +`G43.4` switches to (see the Switchable Kinematics chapter). A module with +several types has one geometry table and one ops table per type, registered +with `switchkinsRegisterOps()`; a module with one type describes itself in a +`kins_module` and links `kins_single.c`. + +=== What the shared code does + +In realtime it makes one HAL pin per table entry, copies the pins into the +block before every call and the outputs back after it, and supplies the classic +entry points, `kinematicsForward()` and the rest, so that motion sees no +difference. Outside realtime a module exports `kinsDescribe()`, which hands a +caller its table and the ops of each type; the caller fills a block from +wherever it likes and asks the same functions through `kinsOpsForward()`, +`kinsOpsInverse()`, `kinsOpsJacobian()` and the frame calls, with the same +defaults applied, so both sides get the same answers. The non-realtime loader +in `kinematics_userspace/` binds the pins of the running module by the table's +names and takes the tool from motion's own offset pins, and says once when the +module's tool pin disagrees with them, which is a config that lost the tool on +the way. `kinslimits` is built on it. + +A module that does not provide the form keeps working as it did. It just cannot +be evaluated outside realtime, which the loader reports. + +=== What stays outside the block + +The kinematics type is in the block, so a caller evaluating a program that +switches type puts the type each block will run under in its own block, and +nothing is switched globally. The tool is in the block, from motion. The joint +map is in the block, from `coordinates=`. Nothing else the maths needs exists, +and a module that finds it needs something else has found a parameter it should +declare. + [[sec:writing-a-module]] == Writing a Module @@ -516,6 +600,13 @@ Geometry stays in the module:: the module. A consumer that restates it has taken a copy that nothing keeps in step, which is the situation this chapter exists to end. +No state:: + Write the maths as functions of the parameter block and the scratch, as + <> describes: geometry in the table, + the kinematics type and the tool from the block, and anything carried + between calls in the scratch. A static in a module is a second machine + that only the realtime thread can see. + Mount orientation is not this:: A tool or holder mount orientation is a different quantity: a right-angle head, a tool held at an angle, an end effector clocked on its flange. Those diff --git a/docs/src/motion/switchkins.adoc b/docs/src/motion/switchkins.adoc index a15c82c1547..70265a9e06f 100644 --- a/docs/src/motion/switchkins.adoc +++ b/docs/src/motion/switchkins.adoc @@ -441,7 +441,8 @@ Custom kinematics can be coded and tested on Run-In-Place ('RIP') builds. A template file src/emc/kinematics/userkfuncs.c is provided in the distribution. This file can be copied/renamed to a user directory and edited to supply custom kinematics with -kinstype==2. +kinstype==2: the in-tree modules register its USERK_OPS as that +kinstype, so the forward and inverse in the copy are what runs. The user custom kinematics file can be compiled from out-of-tree source locations for rt-preempt implementations or by replacing @@ -464,13 +465,14 @@ it gets the kinematics switching, the 'kinstype.is-N' pins, the without reimplementing any of them. The implementation is the switchkins_core module. It provides no -kinstypes of its own; it exports switchkinsRegister(), switchkinsInit() -and the rest of the switchkins.h interface, and the identity kinematics -and helpers kinematics.h declares. A module of your own includes -switchkins.h, registers each of its kinstypes and calls switchkinsInit() -from EXTRA_SETUP(), which halcompile runs after hal_init() and before -hal_ready(). See <> for both -calls. +kinstypes of its own; it exports switchkinsInit() and the rest of the +switchkins.h interface, and the identity kinematics and helpers +kins_module.h declares. A module of your own includes switchkins.h, +supplies switchkinsSetup() and a kinsDescribe() that replays it through +switchkinsDescribeWith(), and calls switchkinsRunSetupWith() and +switchkinsInit() from EXTRA_SETUP(), which halcompile runs after +hal_init() and before hal_ready(). See +<> for the calls. The template is src/hal/components/switchkinscomp.comp. Copy and rename it (both the file and the component name) and replace the @@ -540,17 +542,26 @@ kinstype currently selected, and it creates the HAL pins common to all switchkins modules. It does not provide the module 'main' program, so a module can get that from wherever suits it. -A kinstype is supplied by calling switchkinsRegister(), once per -kinstype: +A kinstype is supplied by calling switchkinsRegisterOps(), once per +kinstype, with the maths of that type written as functions of the +parameter block (see the Kinematics Conventions chapter): + +---- +int switchkinsRegisterOps(int ktype, const kins_ops *ops); +---- + +The geometry of the whole module is one table, named in the kparms +fields 'params' and 'nparams'; every kinstype reads it from the block. +The older form, switchkinsRegister() with a setup, forward and inverse +routine per kinstype that read pins of their own, is still accepted: ---- int switchkinsRegister(int ktype, KS kset, KF kfwd, KI kinv); ---- 'ktype' runs from 0 to SWITCHKINS_MAX_TYPES-1 (defined in -kinematics.h). A kinstype has to come from one route or the -other, so registering one that switchkinsSetup() has already -filled in is an error, and so is leaving a gap below the highest +kinematics.h as KINS_MAX_TYPES). Registering a kinstype twice, by +either route, is an error, and so is leaving a gap below the highest kinstype provided. Either mistake fails the module load and says which kinstype is at fault. @@ -605,15 +616,15 @@ When every kinstype is registered, the module calls: int switchkinsInit(const int comp_id, kparms* kp, const char* coordinates); ---- -which checks the supplied parameters, creates the HAL pins, selects -kinstype 0, and then invokes the setup routine registered for each -kinstype. The caller owns the HAL component: it does hal_init() -before switchkinsInit() and hal_ready() after it. +which checks the supplied parameters, creates the HAL pins, the +table's among them, selects kinstype 0, and then invokes the setup +routine of each kinstype registered the older way. The caller owns +the HAL component: it does hal_init() before switchkinsInit() and +hal_ready() after it. -Each kinstype setup routine can (optionally) create HAL -pins and set them to default values. A setup routine is called -once per kinstype it is registered for, so a routine used for two -kinstypes must not create the same pin twice. +A module built this way also exports kinsDescribe(), through which a +copy of it loaded outside realtime learns its table and the maths of +each kinstype; the non-realtime loader and kinslimits use it. === Module main program @@ -630,53 +641,90 @@ int switchkinsSetup(kparms* kp, KI* kinv0, KI* kinv1, KI* kinv2); ---- -which identifies the setup, forward and inverse routines for -kinstypes 0,1,2 and sets a number of configuration settings. Those -three are registered for the module, so it can supply further -kinstypes by calling switchkinsRegister() itself, and registering -one that switchkinsSetup() has already filled in is the same error -as any other duplicate. +which sets the configuration settings, names the geometry table and +registers the kinstypes with switchkinsRegisterOps(). The pointer +arguments are the older route for kinstypes 0,1,2; a module using +them leaves the rest alone. switchkinsRunSetup() in +switchkins_setup.c is what runs switchkinsSetup() and registers what +it returned, for the 'main' program and for kinsDescribe() alike. A module written as a halcompile component gets rtapi_app_main() -from halcompile instead. It registers its kinstypes and calls -switchkinsInit() from its EXTRA_SETUP() routine, which halcompile -runs after hal_init() and before hal_ready(): +from halcompile instead. It supplies the same switchkinsSetup() and +a kinsDescribe() that replays it, and from its EXTRA_SETUP() routine, +which halcompile runs after hal_init() and before hal_ready(), runs +switchkinsSetup() and then switchkinsInit(): [source,c] ---- +int kinsDescribe(const char *coords, const char *sparm, + kins_module_info *info) +{ + return switchkinsDescribeWith(switchkinsSetup, coords, sparm, info); +} + +EXPORT_SYMBOL(kinsDescribe); + EXTRA_SETUP() { kparms kp; (void)__comp_inst; (void)prefix; (void)extra_arg; - kp.kinsname = "mykins"; // must agree with the module name - // ... the other kparms settings - if (switchkinsRegister(0, identityKinematicsSetup, - identityKinematicsForward, - identityKinematicsInverse)) { return -1; } - if (switchkinsRegister(1, mySetup, myForward, myInverse)) { return -1; } + if (switchkinsRunSetupWith(switchkinsSetup, &kp, NULL)) { return -1; } return switchkinsInit(comp_id, &kp, coordinates); } ---- +switchkinsRunSetupWith() and switchkinsDescribeWith() take the +module's switchkinsSetup() as an argument, so they work from +switchkins_core. switchkinsRunSetup() and the kinsDescribe() in +switchkins_setup.c call the switchkinsSetup() they were linked with, +and only a module built in tree links them. + Where the switchkins code comes from depends on where the component is built. Out of tree, the switchkins_core module provides it, loaded ahead of the component (see <>). In tree, the component links the objects into -itself instead, named in hal/components/Submakefile: +own>>). In tree, the component links the objects into itself instead, +named in hal/components/Submakefile: ---- -millturn-extra-objs := emc/kinematics/switchkins.o emc/kinematics/kins_util.o +millturn-extra-objs := emc/kinematics/switchkins.o emc/kinematics/switchkins_setup.o emc/kinematics/kins_util.o ---- === Outline -The two routes in one switchkinsSetup(), with the kinematics itself -left out. Types 0 to 2 are filled in through the pointer arguments as -they always were, and a fourth is registered: +A switchkinsSetup() with the kinematics itself left out: the table, +the ops table of the machine's own kinstype, and the shared identity +and userk ops for the other two: [source,c] ---- +static const kins_param_desc my_params[] = { + { "pivot-length", KINS_PARAM_FLOAT, KINS_IN, 0, 100.0 }, + { "tool-offset", KINS_PARAM_FLOAT, KINS_IN, 1, 0.0 }, // the tool length +}; +enum { P_PIVOT_LENGTH, P_TOOL_OFFSET }; + +static int my_forward(const kins_params *p, kins_scratch *s, + const double *joint, EmcPose *pos, + const KINEMATICS_FORWARD_FLAGS *fflags, + KINEMATICS_INVERSE_FLAGS *iflags) +{ + double pivot = p->geometry[P_PIVOT_LENGTH]; // where a pin was read + double tool = p->tool.tran.z; // the tool, from wherever the caller has it + // ... +} + +static int my_inverse(const kins_params *p, kins_scratch *s, + const EmcPose *pos, double *joint, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags); + +static const kins_ops my_ops = { + .forward = my_forward, + .inverse = my_inverse, + // .work, .tool, .native and .jacobian are optional, see kinematics.h +}; + int switchkinsSetup(kparms* kp, KS* kset0, KS* kset1, KS* kset2, KF* kfwd0, KF* kfwd1, KF* kfwd2, @@ -687,37 +735,24 @@ int switchkinsSetup(kparms* kp, kp->halprefix = "mykins"; // hal pin names kp->required_coordinates = "xyzab"; kp->max_joints = strlen(kp->required_coordinates); + kp->params = my_params; + kp->nparams = sizeof(my_params)/sizeof(my_params[0]); // remaining kparms fields - *kset0 = identityKinematicsSetup; // kinstype 0 is the startup default - *kfwd0 = identityKinematicsForward; - *kinv0 = identityKinematicsInverse; - - *kset1 = myKinematicsSetup; - *kfwd1 = myKinematicsForward; - *kinv1 = myKinematicsInverse; - - *kset2 = userkKinematicsSetup; - *kfwd2 = userkKinematicsForward; - *kinv2 = userkKinematicsInverse; - - // any further kinstype comes from switchkinsRegister(), and the - // numbering carries on from the three above with no gaps - if (switchkinsRegister(3, myOtherKinematicsSetup, - myOtherKinematicsForward, - myOtherKinematicsInverse)) { return -1; } + switchkinsRegisterOps(0, &my_ops); // kinstype 0 is the startup default + switchkinsRegisterOps(1, &KINS_IDENTITY_OPS); + switchkinsRegisterOps(2, &USERK_OPS); + // any further kinstype is registered the same way, and the + // numbering carries on with no gaps return 0; } // switchkinsSetup() ---- -A module wanting fewer than three kinstypes leaves the unused pointer -arguments alone and starts registering at the first free number. - For the surrounding shape, the in-tree switchkinsSetup() routines are in src/emc/kinematics: 5axiskins.c, xyzac-trt-kins.c, genserkins.c, scarakins.c and the others listed at the top of this document. None of -them registers a fourth kinstype yet, so the call above has no in-tree +them registers a fourth kinstype yet, so a call for one has no in-tree example to copy. // vim: set syntax=asciidoc: From 26b5f1b3dfd3df0b489fdf2094096f78639018e3 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Fri, 4 Sep 2026 13:24:57 +1000 Subject: [PATCH 11/19] kinematics_user: take the joints in as well as out, and bind modules lazily The loader zeroed the joints before a module's inverse and before an iterating forward. Motion hands a module the joints the machine is at and some read them, a nutating head its rotary angles, the hexapod its starting pose, so zeros put the loader on a different branch from realtime for xyzacb_trsrn. The caller's joints are the seed now, an iterating forward keeps the caller's pose, and the Jacobian runs its inverse from what the last inverse found. A halcompile component references hal_export_funct() and the rest of what its rtapi_app_main() needs, which only the realtime HAL library provides, so dlopen with RTLD_NOW refused every component; nothing here calls that main, so bind lazily. The module is looked up in the directory it is installed in, with the HAL library within reach. A module built out of tree takes the shared kinematics code from switchkins_core or kins_single_core, as it does in realtime. The loader finds which in the module's undefined symbols and puts that core in scope before the module, since the module takes the identity ops from it too, data that binds as the module loads, lazily or not. A module that does not load at all is one only realtime can run. --- include/kinematics_user.h | 17 +- .../kinematics_userspace/kinematics_user.c | 151 ++++++++++++++++-- 2 files changed, 148 insertions(+), 20 deletions(-) diff --git a/include/kinematics_user.h b/include/kinematics_user.h index ea051f4d318..f28ac0f06ad 100644 --- a/include/kinematics_user.h +++ b/include/kinematics_user.h @@ -92,9 +92,14 @@ int kinematicsUserGetNumTypes(KinematicsUserContext* ctx); /** * Perform inverse kinematics (world coords -> joint positions) * + * The joint array goes in as well as out: motion hands a module the + * joints the machine is at, and a module may read them (a nutating + * head takes its rotary angles from there, an iterating inverse starts + * there), so pass the current joints, not zeros. + * * @param ctx Kinematics context from kinematicsUserInit * @param world World coordinates (X, Y, Z, A, B, C, U, V, W) - * @param joints Output array of joint positions [KINEMATICS_USER_MAX_JOINTS] + * @param joints Joint positions in and out [KINEMATICS_USER_MAX_JOINTS] * @return 0 on success, -1 on failure */ int kinematicsUserInverse(KinematicsUserContext* ctx, @@ -104,9 +109,12 @@ int kinematicsUserInverse(KinematicsUserContext* ctx, /** * Perform forward kinematics (joint positions -> world coords) * + * A module whose forward iterates (the hexapod, the pentapod) starts + * from the pose in *world, so hand it one near the answer. + * * @param ctx Kinematics context from kinematicsUserInit * @param joints Array of joint positions [KINEMATICS_USER_MAX_JOINTS] - * @param world Output world coordinates + * @param world Output world coordinates, and the seed on input * @return 0 on success, -1 on failure */ int kinematicsUserForward(KinematicsUserContext* ctx, @@ -116,8 +124,9 @@ int kinematicsUserForward(KinematicsUserContext* ctx, /** * The Jacobian at a pose, J[joint][axis] = d joint / d axis, from the * module's closed form where it has one and by differencing its inverse - * where it does not. The inverse is run at the pose first, so the - * derivative is taken on the solution branch the module picks there. + * where it does not. The inverse is run at the pose first, seeded with + * what the last kinematicsUserInverse() found, so the derivative is + * taken on the solution branch the caller is on. * * @return 0 on success, -1 on failure */ diff --git a/src/emc/kinematics_userspace/kinematics_user.c b/src/emc/kinematics_userspace/kinematics_user.c index cb9a0eb3dc4..3a69af3e270 100644 --- a/src/emc/kinematics_userspace/kinematics_user.c +++ b/src/emc/kinematics_userspace/kinematics_user.c @@ -23,12 +23,14 @@ #include #include +#include +#include #include #include #include #include -#include "config.h" /* EMC2_HOME */ +#include "config.h" /* EMC2_RTLIB_DIR, MODULE_EXT */ typedef int (*kins_describe_fn)(const char *coordinates, const char *sparm, kins_module_info *info); @@ -41,6 +43,7 @@ struct KinematicsUserContext { int rt_only; /* 1 if the module exports no kinsDescribe() */ KINEMATICS_TYPE kins_type; void *rt_handle; /* dlopen handle */ + void *core_handle; /* the core module it takes shared code from */ kins_module_info info; kins_params params; kins_scratch scratch; @@ -57,6 +60,7 @@ struct KinematicsUserContext { int cell_of_tool[AXIS_COUNT]; /* motion.tooloffset.*, -1 if absent */ int tool_param; /* the table's tool entry, -1 if none */ int warned_tool; + double last_joints[EMCMOT_MAX_JOINTS]; /* what the last inverse found */ }; /* ======================================================================== @@ -278,22 +282,130 @@ static void refresh(KinematicsUserContext *ctx) * Module loading * ======================================================================== */ +/* The core module a module built out of tree takes the shared kinematics + code from, named by what its dynamic symbols leave undefined; NULL for a + module that needs none. Read from the file, since a lazily loaded + module cannot be asked what it has not yet bound. */ +static const char *module_core(const char *path) +{ + const char *core = NULL; + FILE *f; + long size; + unsigned char *buf; + const ElfW(Ehdr) *eh; + const ElfW(Shdr) *sh; + int i; + + f = fopen(path, "rb"); + if (!f) return NULL; + if (fseek(f, 0, SEEK_END) || (size = ftell(f)) <= 0 || fseek(f, 0, SEEK_SET)) { + fclose(f); + return NULL; + } + buf = malloc(size); + if (!buf || fread(buf, 1, size, f) != (size_t)size) { + free(buf); + fclose(f); + return NULL; + } + fclose(f); + + eh = (const ElfW(Ehdr) *)buf; + if ((size_t)size < sizeof(*eh) || memcmp(eh->e_ident, ELFMAG, SELFMAG) + || eh->e_shoff + (size_t)eh->e_shnum * sizeof(ElfW(Shdr)) > (size_t)size) { + free(buf); + return NULL; + } + sh = (const ElfW(Shdr) *)(buf + eh->e_shoff); + for (i = 0; i < eh->e_shnum && !core; i++) { + const ElfW(Sym) *sym; + const char *names; + size_t n, k; + if (sh[i].sh_type != SHT_DYNSYM || sh[i].sh_link >= eh->e_shnum) continue; + if (sh[i].sh_offset + sh[i].sh_size > (size_t)size + || sh[sh[i].sh_link].sh_offset + sh[sh[i].sh_link].sh_size > (size_t)size) break; + sym = (const ElfW(Sym) *)(buf + sh[i].sh_offset); + names = (const char *)(buf + sh[sh[i].sh_link].sh_offset); + n = sh[i].sh_size / sizeof(ElfW(Sym)); + for (k = 0; k < n && !core; k++) { + const char *name; + if (sym[k].st_shndx != SHN_UNDEF + || sym[k].st_name >= sh[sh[i].sh_link].sh_size) continue; + name = names + sym[k].st_name; + if (!strcmp(name, "switchkinsInit")) core = "switchkins_core"; + else if (!strcmp(name, "kinsSingleInit")) core = "kins_single_core"; + } + } + free(buf); + return core; +} + +static void unload_module(KinematicsUserContext *ctx) +{ + if (ctx->rt_handle) dlclose(ctx->rt_handle); + if (ctx->core_handle) dlclose(ctx->core_handle); + ctx->rt_handle = NULL; + ctx->core_handle = NULL; +} + static int load_module(KinematicsUserContext *ctx, const char *module_name, const char *coordinates, const char *sparm) { char module_path[512]; - void *handle; + void *handle, *hal_lib; + const char *core; kins_describe_fn describe; snprintf(module_path, sizeof(module_path), - "%s/rtlib/%s.so", EMC2_HOME, module_name); + "%s/%s%s", EMC2_RTLIB_DIR, module_name, MODULE_EXT); + + /* A module calls rtapi_print() and the rest of the HAL library, and a + program holding that library under a shared object of its own (a GUI + holds it under the interpreter it loaded) keeps those symbols out of + the scope a module resolves against: the module loads and then dies + at the first call it cannot bind. Failing here is not itself an + error, since a program that links the library has them in reach. */ + hal_lib = dlopen("liblinuxcnchal.so.0", RTLD_LAZY | RTLD_GLOBAL); + if (!hal_lib) { + fprintf(stderr, "kinematicsUserInit: dlopen 'liblinuxcnchal.so.0':" + " %s\n", dlerror()); + } + + /* A module built out of tree takes the shared kinematics code from + switchkins_core or kins_single_core, as it does in RT; that core goes + in scope for it and goes with it. Only such a module brings a core + in, so the core never stands in for code a module carries itself. + The core goes first: the module takes data from it as well, the + identity ops, and data binds as the module loads, lazily or not, + where a program holding this library under a shared object of its + own (a GUI again) has no other copy in scope. */ + core = module_core(module_path); + if (core) { + char core_path[512]; + snprintf(core_path, sizeof(core_path), + "%s/%s%s", EMC2_RTLIB_DIR, core, MODULE_EXT); + ctx->core_handle = dlopen(core_path, RTLD_LAZY | RTLD_GLOBAL); + if (!ctx->core_handle) { + fprintf(stderr, "kinematicsUserInit: '%s' needs %s: %s\n", + module_name, core, dlerror()); + ctx->rt_only = 1; + return -1; + } + } - handle = dlopen(module_path, RTLD_NOW | RTLD_LOCAL); + /* lazily: a halcompile component references hal_export_funct() and + the rest of what its rtapi_app_main() needs, which only the realtime + HAL library provides, and nothing here calls that main. What is + called, kinsDescribe() and the ops, resolves when it is called. A + module that does not load here at all is one only RT can run. */ + handle = dlopen(module_path, RTLD_LAZY | RTLD_LOCAL); if (!handle) { fprintf(stderr, "kinematicsUserInit: dlopen '%s': %s\n", module_path, dlerror()); + unload_module(ctx); + ctx->rt_only = 1; return -1; } ctx->rt_handle = handle; @@ -302,8 +414,7 @@ static int load_module(KinematicsUserContext *ctx, if (!describe) { fprintf(stderr, "kinematicsUserInit: '%s' exports no kinsDescribe;" " it cannot be evaluated outside RT\n", module_name); - dlclose(handle); - ctx->rt_handle = NULL; + unload_module(ctx); ctx->rt_only = 1; return -1; } @@ -311,8 +422,7 @@ static int load_module(KinematicsUserContext *ctx, if (describe(coordinates, sparm, &ctx->info) != 0) { fprintf(stderr, "kinematicsUserInit: kinsDescribe failed for '%s'\n", module_name); - dlclose(handle); - ctx->rt_handle = NULL; + unload_module(ctx); ctx->rt_only = 1; return -1; } @@ -320,8 +430,7 @@ static int load_module(KinematicsUserContext *ctx, if (ctx->info.ntypes < 1 || !ctx->info.ops[0]) { fprintf(stderr, "kinematicsUserInit: '%s' has no type 0 in the" " parameter block form\n", module_name); - dlclose(handle); - ctx->rt_handle = NULL; + unload_module(ctx); ctx->rt_only = 1; return -1; } @@ -329,8 +438,7 @@ static int load_module(KinematicsUserContext *ctx, if (kinsParamsInit(&ctx->params, &ctx->info, coordinates) != 0) { fprintf(stderr, "kinematicsUserInit: '%s' refuses coordinates '%s'\n", module_name, coordinates ? coordinates : "(default)"); - dlclose(handle); - ctx->rt_handle = NULL; + unload_module(ctx); ctx->rt_only = 1; return -1; } @@ -431,12 +539,18 @@ int kinematicsUserInverse(KinematicsUserContext* ctx, if (ctx->rt_only) return -1; refresh(ctx); - for (i = 0; i < EMCMOT_MAX_JOINTS; i++) j[i] = 0.0; + /* the joints go in as well as out: motion hands a module where the + machine is, and some read that (a nutating head takes its rotary + angles from it), so the caller's array is the seed */ + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { + j[i] = (i < ctx->num_joints) ? joints[i] : 0.0; + } if (kinsOpsInverse(ctx->info.ops[ctx->ktype], &ctx->params, &ctx->scratch, world, j, &iflags, &fflags) != 0) { return -1; } for (i = 0; i < ctx->num_joints; i++) joints[i] = j[i]; + memcpy(ctx->last_joints, j, sizeof(ctx->last_joints)); return 0; } @@ -456,7 +570,11 @@ int kinematicsUserForward(KinematicsUserContext* ctx, for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { j[i] = (i < ctx->num_joints) ? joints[i] : 0.0; } - memset(world, 0, sizeof(*world)); + /* a forward that iterates starts from the pose it is handed, so the + caller's world is the seed; any other gets a clean one */ + if (!ctx->info.ops[ctx->ktype]->fwd_iterates) { + memset(world, 0, sizeof(*world)); + } return kinsOpsForward(ctx->info.ops[ctx->ktype], &ctx->params, &ctx->scratch, j, world, &fflags, &iflags); } @@ -475,7 +593,8 @@ int kinematicsUserJacobian(KinematicsUserContext* ctx, if (ctx->rt_only) return -1; refresh(ctx); - for (r = 0; r < EMCMOT_MAX_JOINTS; r++) j[r] = 0.0; + /* the joints at this pose, on the branch the last inverse was on */ + memcpy(j, ctx->last_joints, sizeof(j)); if (kinsOpsInverse(ctx->info.ops[ctx->ktype], &ctx->params, &ctx->scratch, world, j, &iflags, &fflags) != 0) { return -1; @@ -545,6 +664,6 @@ void kinematicsUserFree(KinematicsUserContext* ctx) for (i = 0; i < ctx->num_made_signals; i++) { hal_signal_delete(ctx->made_signal[i]); } - if (ctx->rt_handle) dlclose(ctx->rt_handle); + unload_module(ctx); free(ctx); } From 0a934d27aac7a1699ec7e97a32e6b304185d3675 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Fri, 4 Sep 2026 13:24:58 +1000 Subject: [PATCH 12/19] tests: check that a module answers the same outside realtime tests/kins-params loads each module, evaluates it once in realtime through the classic entry points (paritycheck.c publishes the forward, the inverse and the Jacobian on pins) and once through the non-realtime loader from python (check.py: kinsDescribe(), the block filled from the module's pins, the same ops), and requires the same success and the same numbers to rounding. 34 runs over the 24 modules, every switchable type that has geometry of its own, the parallel machines from a pose their forward can be seeded with. Checked by mutation: the loader not refreshing the geometry fails 10 comparisons in the first module with a table; the loader seeding the inverse with zeros instead of the caller's joints fails the three translation joints of xyzacb_trsrn, which reads its rotary angles from that array. check.py holds the loader's libraries local, the way a GUI holds them under the interpreter it loads, so whatever a module needs in scope has to come from the loader: the out of tree switchkins module does not load without its core put in scope first. --- tests/kins-params/check.py | 137 +++++++++++++++++++++++++ tests/kins-params/checkresult | 4 + tests/kins-params/paritycheck.c | 148 +++++++++++++++++++++++++++ tests/kins-params/skip | 4 + tests/kins-params/test.sh | 174 ++++++++++++++++++++++++++++++++ 5 files changed, 467 insertions(+) create mode 100755 tests/kins-params/check.py create mode 100755 tests/kins-params/checkresult create mode 100644 tests/kins-params/paritycheck.c create mode 100755 tests/kins-params/skip create mode 100755 tests/kins-params/test.sh diff --git a/tests/kins-params/check.py b/tests/kins-params/check.py new file mode 100755 index 00000000000..0dc67110708 --- /dev/null +++ b/tests/kins-params/check.py @@ -0,0 +1,137 @@ +#!/usr/bin/env python3 +# The non-realtime half of the parameter block parity test. +# +# Evaluates the module paritycheck was loaded after through the +# non-realtime loader (libkinslimits, kinematicsUserInit and friends), +# which dlopens the module, asks it to describe itself through +# kinsDescribe(), fills a parameter block from the module's own pins and +# calls the same ops the realtime wrapper calls. The answers have to +# match what paritycheck published, to rounding, or the module is not +# the pure function of its parameters it claims to be. +# +# Usage: check.py MODULE JOINTS COORDS KTYPE FROMPOSE POSE JNT SPARM +# POSE and JNT are comma separated numbers, as given to paritycheck; +# COORDS and SPARM are a dash when the module was loaded without them. + +import ctypes +import os +import sys + +import hal + +EMC2_HOME = os.environ.get("EMC2_HOME") +# local, the way a GUI holds them under the interpreter it loads: what a +# module needs in scope, the HAL library and an out of tree module's core, +# the loader puts there itself +def lib(name): + if EMC2_HOME: + return ctypes.CDLL(os.path.join(EMC2_HOME, "lib", name)) + return ctypes.CDLL(name) + +class EmcPose(ctypes.Structure): + _fields_ = [(n, ctypes.c_double) for n in "xyzabcuvw"] + +MAX_JOINTS = 9 +AXES = 9 +Joints = ctypes.c_double * MAX_JOINTS +Jac = (ctypes.c_double * AXES) * MAX_JOINTS + +module, joints, coords, ktype, frompose = sys.argv[1], int(sys.argv[2]), sys.argv[3], int(sys.argv[4]), int(sys.argv[5]) +pose_in = [float(v) for v in sys.argv[6].split(",")] +jnt_in = [float(v) for v in sys.argv[7].split(",")] +# a dash stands for an absent value, since halcmd hands quotes through +if coords == "-": + coords = "" +sparm = sys.argv[8].encode() if len(sys.argv) > 8 and sys.argv[8] not in ("", "-") else None +pose_in += [0.0] * (AXES - len(pose_in)) +jnt_in += [0.0] * (MAX_JOINTS - len(jnt_in)) + +halc = lib("liblinuxcnchal.so.0") +kins = lib("libkinslimits.so.0") + +kins.kinematicsUserInitSparm.restype = ctypes.c_void_p +kins.kinematicsUserInitSparm.argtypes = [ctypes.c_char_p, ctypes.c_int, ctypes.c_char_p, + ctypes.c_char_p, ctypes.c_int, ctypes.c_char_p] +for fn in ("kinematicsUserIsRtOnly", "kinematicsUserGetNumTypes"): + getattr(kins, fn).argtypes = [ctypes.c_void_p] +kins.kinematicsUserSetType.argtypes = [ctypes.c_void_p, ctypes.c_int] +kins.kinematicsUserInverse.argtypes = [ctypes.c_void_p, ctypes.POINTER(EmcPose), Joints] +kins.kinematicsUserForward.argtypes = [ctypes.c_void_p, Joints, ctypes.POINTER(EmcPose)] +kins.kinematicsUserJacobian.argtypes = [ctypes.c_void_p, ctypes.POINTER(EmcPose), Jac] +kins.kinematicsUserFree.argtypes = [ctypes.c_void_p] + +comp_id = halc.hal_init(b"kpcheck") +if comp_id < 0: + print("kins-params: FAIL hal_init") + sys.exit(1) +ctx = kins.kinematicsUserInitSparm(module.encode(), joints, coords.encode() if coords else None, sparm, + comp_id, b"kpcheck") +halc.hal_ready(comp_id) +failures = 0 + +def fail(what): + global failures + failures += 1 + print("kins-params: FAIL %s" % what) + +if not ctx or kins.kinematicsUserIsRtOnly(ctx): + fail("%s cannot be evaluated outside realtime" % module) + sys.exit(1) +if ktype and kins.kinematicsUserSetType(ctx, ktype): + fail("%s has no type %d in the block form" % (module, ktype)) + sys.exit(1) + +def pose_of(values): + p = EmcPose() + for n, v in zip("xyzabcuvw", values): + setattr(p, n, v) + return p + +def close(a, b): + return abs(a - b) <= 1e-9 * max(1.0, abs(a), abs(b)) + +def compare(what, ours, theirs): + if not close(ours, theirs): + fail("%s: loader %.12g, realtime %.12g" % (what, ours, theirs)) + +rc_fwd = hal.get_value("paritycheck.rc-fwd") +rc_inv = hal.get_value("paritycheck.rc-inv") +rc_jac = hal.get_value("paritycheck.rc-jac") + +q = Joints(*jnt_in) +qi = Joints(*jnt_in) +J = Jac() +if frompose: + P = pose_of(pose_in) + r_inv = kins.kinematicsUserInverse(ctx, ctypes.byref(P), qi) + F = pose_of(pose_in) + r_fwd = kins.kinematicsUserForward(ctx, qi, ctypes.byref(F)) + r_jac = kins.kinematicsUserJacobian(ctx, ctypes.byref(P), J) +else: + F = pose_of(pose_in) + r_fwd = kins.kinematicsUserForward(ctx, q, ctypes.byref(F)) + r_inv = kins.kinematicsUserInverse(ctx, ctypes.byref(F), qi) + r_jac = kins.kinematicsUserJacobian(ctx, ctypes.byref(F), J) + +# the same success or failure on both sides, then the same numbers +for what, ours, theirs in (("forward", r_fwd, rc_fwd), ("inverse", r_inv, rc_inv), ("jacobian", r_jac, rc_jac)): + if (ours != 0) != (theirs != 0): + fail("%s returned %d in the loader and %d in realtime" % (what, ours, theirs)) + +if r_fwd == 0 and rc_fwd == 0: + for n in "xyzabcuvw": + compare("forward %s" % n, getattr(F, n), hal.get_value("paritycheck.fwd-%s" % n)) +if r_inv == 0 and rc_inv == 0: + for j in range(joints): + compare("inverse joint %d" % j, qi[j], hal.get_value("paritycheck.inv-%d" % j)) +if r_jac == 0 and rc_jac == 0: + for j in range(joints): + for a, n in enumerate("xyzabcuvw"): + compare("jacobian [%d][%s]" % (j, n), J[j][a], hal.get_value("paritycheck.jac-%d-%s" % (j, n))) + +kins.kinematicsUserFree(ctx) +halc.hal_exit(comp_id) + +if failures: + sys.exit(1) +print("kins-params: %s type %d agrees" % (module, ktype)) diff --git a/tests/kins-params/checkresult b/tests/kins-params/checkresult new file mode 100755 index 00000000000..d3eba1a4da0 --- /dev/null +++ b/tests/kins-params/checkresult @@ -0,0 +1,4 @@ +#!/bin/sh +[ "$(grep -c 'agrees' "$1")" = "$(grep -c '^=== ' "$1")" ] \ + && [ "$(grep -c '^=== ' "$1")" -ge 20 ] \ + && ! grep -q "FAIL" "$1" diff --git a/tests/kins-params/paritycheck.c b/tests/kins-params/paritycheck.c new file mode 100644 index 00000000000..9043df69dac --- /dev/null +++ b/tests/kins-params/paritycheck.c @@ -0,0 +1,148 @@ +/* + * paritycheck: the realtime half of the parameter block parity test. + * + * Loaded after a kinematics module, it evaluates the module through the + * classic entry points once, at load, and publishes the answers on HAL + * pins: the forward pose, the inverse joints and the Jacobian. check.py + * then evaluates the same module through the non-realtime loader, which + * goes through kinsDescribe() and the parameter block, and compares. + * + * Two flows. With frompose=0 the input is a joint set: the pose is the + * forward of it, the joints published are the inverse of that pose, and + * the Jacobian is taken there. With frompose=1 the input is a pose, for + * the parallel machines whose forward wants a seed: the joints are its + * inverse, the forward is run from the pose as seed, and the Jacobian is + * taken there. + * + * Module parameters + * joints joint count the module was loaded for + * ktype switchkins type to select first, 0 for none + * frompose 0 or 1, as above + * pose up to nine integers, the pose (frompose=1) or the forward + * seed (frompose=0) + * jnt up to sixteen integers, the joint set (frompose=0) or the + * inverse seed (frompose=1) + */ +#include +#include +#include +#include +#include + +MODULE_LICENSE("GPL"); + +static int joints = 3; +RTAPI_MP_INT(joints, "joint count the module under test was loaded for"); +static int ktype = 0; +RTAPI_MP_INT(ktype, "switchkins type to select first"); +static int frompose = 0; +RTAPI_MP_INT(frompose, "1 to take the pose as the input"); +static int pose[EMCMOT_MAX_AXIS] = { 0 }; +RTAPI_MP_ARRAY_INT(pose, EMCMOT_MAX_AXIS, "pose, x y z a b c u v w"); +static int jnt[EMCMOT_MAX_JOINTS] = { 10, 20, 30, 40, 50, 60, 70, 80, 90 }; +RTAPI_MP_ARRAY_INT(jnt, EMCMOT_MAX_JOINTS, "joint values, from joint 0"); + +static int comp_id = -1; + +static struct { + hal_real_t fwd[EMCMOT_MAX_AXIS]; + hal_real_t inv[EMCMOT_MAX_JOINTS]; + hal_real_t jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]; + hal_sint_t rc_fwd; + hal_sint_t rc_inv; + hal_sint_t rc_jac; +} *pins; + +static const char letter[EMCMOT_MAX_AXIS] = { 'x','y','z','a','b','c','u','v','w' }; + +static double *coord(EmcPose *p, int a) +{ + switch (a) { + case 0: return &p->tran.x; + case 1: return &p->tran.y; + case 2: return &p->tran.z; + case 3: return &p->a; + case 4: return &p->b; + case 5: return &p->c; + case 6: return &p->u; + case 7: return &p->v; + default: return &p->w; + } +} + +int rtapi_app_main(void) +{ + KINEMATICS_FORWARD_FLAGS fflags = 0; + KINEMATICS_INVERSE_FLAGS iflags = 0; + double q[EMCMOT_MAX_JOINTS], qi[EMCMOT_MAX_JOINTS]; + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]; + EmcPose P, F, seed; + int a, j, res = 0; + + if (joints < 1 || joints > EMCMOT_MAX_JOINTS) { return -1; } + + comp_id = hal_init("paritycheck"); + if (comp_id < 0) { return comp_id; } + + pins = hal_malloc(sizeof(*pins)); + if (!pins) { hal_exit(comp_id); return -1; } + + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + res += hal_pin_new_real(comp_id, HAL_OUT, &pins->fwd[a], 0.0, + "paritycheck.fwd-%c", letter[a]); + } + for (j = 0; j < joints; j++) { + res += hal_pin_new_real(comp_id, HAL_OUT, &pins->inv[j], 0.0, + "paritycheck.inv-%d", j); + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + res += hal_pin_new_real(comp_id, HAL_OUT, &pins->jac[j][a], 0.0, + "paritycheck.jac-%d-%c", j, letter[a]); + } + } + res += hal_pin_new_si32(comp_id, HAL_OUT, &pins->rc_fwd, 0, "paritycheck.rc-fwd"); + res += hal_pin_new_si32(comp_id, HAL_OUT, &pins->rc_inv, 0, "paritycheck.rc-inv"); + res += hal_pin_new_si32(comp_id, HAL_OUT, &pins->rc_jac, 0, "paritycheck.rc-jac"); + if (res) { hal_exit(comp_id); return -1; } + + if (ktype > 0 && kinematicsSwitchable()) { + if (kinematicsSwitch(ktype)) { hal_exit(comp_id); return -1; } + } + + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { *coord(&seed, a) = pose[a]; } + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { q[j] = jnt[j]; qi[j] = jnt[j]; } + + // a switchable module's first forward after load restarts from the + // pose it saved, which is nothing yet; take that call here so the one + // measured starts from the seed like the loader's does + F = seed; + kinematicsForward(q, &F, &fflags, &iflags); + fflags = 0; iflags = 0; + + if (frompose) { + P = seed; + hal_set_si32(pins->rc_inv, kinematicsInverse(&P, qi, &iflags, &fflags)); + F = seed; + fflags = 0; iflags = 0; + hal_set_si32(pins->rc_fwd, kinematicsForward(qi, &F, &fflags, &iflags)); + iflags = 0; + hal_set_si32(pins->rc_jac, kinematicsJacobian(qi, &P, jac, &iflags)); + } else { + F = seed; + hal_set_si32(pins->rc_fwd, kinematicsForward(q, &F, &fflags, &iflags)); + iflags = 0; fflags = 0; + hal_set_si32(pins->rc_inv, kinematicsInverse(&F, qi, &iflags, &fflags)); + iflags = 0; + hal_set_si32(pins->rc_jac, kinematicsJacobian(qi, &F, jac, &iflags)); + } + + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { hal_set_real(pins->fwd[a], *coord(&F, a)); } + for (j = 0; j < joints; j++) { + hal_set_real(pins->inv[j], qi[j]); + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { hal_set_real(pins->jac[j][a], jac[j][a]); } + } + + hal_ready(comp_id); + return 0; +} + +void rtapi_app_exit(void) { hal_exit(comp_id); } diff --git a/tests/kins-params/skip b/tests/kins-params/skip new file mode 100755 index 00000000000..a12f31a77c2 --- /dev/null +++ b/tests/kins-params/skip @@ -0,0 +1,4 @@ +#!/bin/sh +# Builds a realtime component with halcompile, which needs the build +# tools present. Skip when testing installed packages. +[ -z "$SYSTEM_BUILD" ] diff --git a/tests/kins-params/test.sh b/tests/kins-params/test.sh new file mode 100755 index 00000000000..3a755b8f098 --- /dev/null +++ b/tests/kins-params/test.sh @@ -0,0 +1,174 @@ +#!/bin/bash +set -e + +${SUDO} halcompile --install paritycheck.c >/dev/null + +# The two templates the way a module built out of tree is: renamed, +# compiled by halcompile on their own, and loaded behind the core that +# supplies the shared code. +sed 's/switchkinscomp/oot_switchkins/g' "$EMC2_HOME/src/hal/components/switchkinscomp.comp" > oot_switchkins.comp +sed 's/userkins/oot_userkins/g' "$EMC2_HOME/src/hal/components/userkins.comp" > oot_userkins.comp +${SUDO} halcompile --install oot_switchkins.comp >/dev/null +${SUDO} halcompile --install oot_userkins.comp >/dev/null +rm -f oot_switchkins.comp oot_userkins.comp + +# One hal file per module. paritycheck evaluates the module in realtime +# through the classic entry points and publishes the answers; check.py +# evaluates it through the non-realtime loader, kinsDescribe() and the +# parameter block, and compares. Where they disagree the module keeps +# state its table does not declare. +# ONLY= in the environment runs the entries for that module alone +run() { + local loadrt="$1" setp="$2" parms="$3" ktype="$4" + local module coords sparm joints frompose pose jnt hal tok + case "$loadrt" in "${ONLY:-}"*) ;; *) return 0 ;; esac + module=${loadrt%% *} + coords=""; sparm="" + for tok in $loadrt; do + case "$tok" in + coordinates=*) coords=${tok#coordinates=} ;; + sparm=*) sparm=${tok#sparm=} ;; + esac + done + joints=3; frompose=0; pose="0,0,0,0,0,0,0,0,0"; jnt="10,20,30,40,50,60,70,80,90" + for tok in $parms; do + case "$tok" in + joints=*) joints=${tok#joints=} ;; + frompose=*) frompose=${tok#frompose=} ;; + pose=*) pose=${tok#pose=} ;; + jnt=*) jnt=${tok#jnt=} ;; + esac + done + hal=$(mktemp --suffix=.hal) + { if [ -n "${CORE:-}" ]; then printf 'loadrt %s\n' "$CORE"; fi + printf 'loadrt %s\n' "$loadrt" + printf '%s\n' "$setp" + printf 'loadrt paritycheck %s ktype=%s\n' "$parms" "${ktype:-0}" + # halcmd keeps quotes, so an absent value travels as a dash + printf 'loadusr -w python3 check.py %s %s %s %s %s %s %s %s\n' \ + "$module" "$joints" "${coords:--}" "${ktype:-0}" "$frompose" "$pose" "$jnt" "${sparm:--}" + } > "$hal" + echo "=== $loadrt type ${ktype:-0}" + halrun -f "$hal" + rm -f "$hal" +} + +# identity, a gantry included +run "trivkins coordinates=XYZ" "" "joints=3 jnt=10,20,30" +run "trivkins coordinates=XYZY kinstype=BOTH" "" "joints=4 jnt=10,20,30,20" +run "trivkins coordinates=XYZABCUVW" "" "joints=9" +run "userkins" "" "joints=3 jnt=10,20,30" +CORE=kins_single_core run "oot_userkins" "" "joints=3 jnt=10,20,30" +CORE=switchkins_core run "oot_switchkins" "" "joints=3 jnt=10,20,30" +CORE=switchkins_core run "oot_switchkins" "" "joints=3 jnt=10,20,30" 1 +run "millturn" "" "joints=4 jnt=10,20,30,40" +run "millturn" "" "joints=4 jnt=10,20,30,40" 1 + +# linear maps and one rotation +run "corexykins" "" "joints=9" +run "rotatekins" "" "joints=9" +run "matrixkins" \ + "setp matrixkins.C_xy 0.02 +setp matrixkins.C_xz -0.01 +setp matrixkins.C_yx 0.03 +setp matrixkins.C_yz 0.015 +setp matrixkins.C_zx -0.02 +setp matrixkins.C_zy 0.01 +setp matrixkins.C_zz 1.001" \ + "joints=9" + +# tables and heads, offsets set so no term drops out +run "maxkins" \ + "setp maxkins.pivot-length 100" \ + "joints=9 jnt=10,20,30,0,15,25,7,0,3" + +run "5axiskins coordinates=XYZBCW" "" "joints=6 jnt=10,20,30,15,25,5" +run "5axiskins coordinates=XYZBCW sparm=identityfirst" "" "joints=6 jnt=10,20,30,15,25,5" 1 + +run "xyzac-trt-kins coordinates=XYZAC" \ + "setp xyzac-trt-kins.y-offset 3 +setp xyzac-trt-kins.z-offset 11 +setp xyzac-trt-kins.tool-offset 7 +setp xyzac-trt-kins.x-rot-point 1 +setp xyzac-trt-kins.y-rot-point 2 +setp xyzac-trt-kins.z-rot-point 5" \ + "joints=5 jnt=10,20,30,15,25" + +run "xyzbc-trt-kins coordinates=XYZBC" \ + "setp xyzbc-trt-kins.conventional-directions 1 +setp xyzbc-trt-kins.x-offset 3 +setp xyzbc-trt-kins.z-offset 11 +setp xyzbc-trt-kins.tool-offset 7 +setp xyzbc-trt-kins.x-rot-point 1 +setp xyzbc-trt-kins.y-rot-point 2 +setp xyzbc-trt-kins.z-rot-point 5" \ + "joints=5 jnt=10,20,30,15,25" + +run "xyzab_tdr_kins" \ + "setp xyzab_tdr_kins.x-offset 3 +setp xyzab_tdr_kins.z-offset 11 +setp xyzab_tdr_kins.tool-offset-z 7 +setp xyzab_tdr_kins.x-rot-point 1 +setp xyzab_tdr_kins.y-rot-point 2 +setp xyzab_tdr_kins.z-rot-point 5" \ + "joints=5 jnt=10,20,30,15,25" 1 + +run "xyzacb_trsrn" \ + "setp xyzacb_trsrn_kins.nut-angle 45 +setp xyzacb_trsrn_kins.y-pivot 100 +setp xyzacb_trsrn_kins.z-pivot 200 +setp xyzacb_trsrn_kins.x-offset 5 +setp xyzacb_trsrn_kins.y-offset 7 +setp xyzacb_trsrn_kins.y-rot-axis 300 +setp xyzacb_trsrn_kins.z-rot-axis 400 +setp xyzacb_trsrn_kins.tool-offset-z 50 +setp xyzacb_trsrn_kins.pre-rot 0.3 +setp xyzacb_trsrn_kins.primary-angle 20 +setp xyzacb_trsrn_kins.secondary-angle 35" \ + "joints=6 jnt=10,20,30,15,25,35" 1 + +run "xyzacb_trsrn" \ + "setp xyzacb_trsrn_kins.nut-angle 45 +setp xyzacb_trsrn_kins.y-pivot 100 +setp xyzacb_trsrn_kins.z-pivot 200 +setp xyzacb_trsrn_kins.pre-rot 0.3 +setp xyzacb_trsrn_kins.primary-angle 20 +setp xyzacb_trsrn_kins.secondary-angle 35" \ + "joints=6 jnt=10,20,30,15,25,35" 2 + +run "xyzbca_trsrn" \ + "setp xyzbca_trsrn_kins.nut-angle 45 +setp xyzbca_trsrn_kins.x-pivot 100 +setp xyzbca_trsrn_kins.z-pivot 200 +setp xyzbca_trsrn_kins.x-offset 5 +setp xyzbca_trsrn_kins.y-offset 7 +setp xyzbca_trsrn_kins.x-rot-axis 300 +setp xyzbca_trsrn_kins.z-rot-axis 400 +setp xyzbca_trsrn_kins.tool-offset-z 50 +setp xyzbca_trsrn_kins.pre-rot 0.3 +setp xyzbca_trsrn_kins.primary-angle 20 +setp xyzbca_trsrn_kins.secondary-angle 35" \ + "joints=6 jnt=10,20,30,15,25,35" 1 + +# polar +run "rosekins" "" "joints=3 jnt=10,5,30" + +# arms +run "scarakins" "" "joints=6 jnt=30,40,20,10,0,0" +run "scorbot-kins" "" "joints=5 jnt=40,60,-20,0,0" +run "pumakins" "setp pumakins.D6 50" "joints=6 jnt=15,20,-35,10,70,20" +run "three21kins" "" "joints=6 jnt=15,20,-35,10,70,20" +run "genserkins" "" "joints=9 jnt=15,20,-35,10,70,20,0,0,0" +run "genserkins" "setp genserkins.unrotate-3 1" "joints=9 jnt=15,20,-35,10,70,20,0,0,0" + +# parallel machines, from a pose the forward can be seeded with +run "tripodkins" \ + "setp tripodkins.Bx 2 +setp tripodkins.Cx 1 +setp tripodkins.Cy 2" \ + "joints=3 frompose=1 pose=1,1,2" +run "lineardeltakins" "" "joints=9 frompose=1 pose=20,30,-200" +run "rotarydeltakins" "" "joints=9 frompose=1 pose=0,0,-12" +run "genhexkins" "setp genhexkins.screw-lead 0" "joints=6 frompose=1 pose=2,3,20,0,5,-7" +run "genhexkins" "setp genhexkins.screw-lead 5" "joints=6 frompose=1 pose=2,3,20,0,5,-7" +run "pentakins" "" "joints=5 frompose=1 pose=10,20,0,5,-7" From 29bf45541368414d109936c7127d0a332dc7672a Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Fri, 4 Sep 2026 16:33:19 +1000 Subject: [PATCH 13/19] kinematics: take the tool offset from motion, not from a net A module whose maths needs the tool length read it from a pin the config had to net from motion.tooloffset.z: a copy of controller state motion already holds, one cycle late, and a missing net gave wrong joints with no error. The block carries the tool, so motion hands it over: kinematicsSetTool() is called whenever the offset changes, referenced weakly so an older module still loads. kins_single.c and switchkins.c export it for every module on the block; the pin is read only until motion has sent anything, and a pin left disagreeing is reported once, since it is a config setting a tool length where the tool table should. A pin nobody set is not a disagreement. The non-RT loader takes the tool from the caller through kinematicsUserSetTool() where one is given, a planner knowing what a segment runs under better than the machine does. tests/kins-tool-offset runs xyzac-trt-kins with nothing on its tool pin and checks that G43 reaches the joints, that netting the pin changes nothing and that G49 takes the length out; tests/kins-params checks the caller's tool on the loader. --- docs/src/code/code-notes.adoc | 3 +- docs/src/motion/5-axis-kinematics.adoc | 21 ++-- docs/src/motion/kinematics-conventions.adoc | 34 ++++-- docs/src/motion/switchkins.adoc | 8 +- include/kinematics.h | 9 ++ include/kinematics_user.h | 15 ++- include/kins_rt.h | 20 ++++ src/emc/kinematics/kins_single.c | 13 +- src/emc/kinematics/kins_util.c | 39 ++++++ src/emc/kinematics/switchkins.c | 16 ++- .../kinematics_userspace/kinematics_user.c | 33 ++++- src/emc/motion/command.c | 8 ++ tests/kins-params/check.py | 29 +++++ tests/kins-tool-offset/README | 7 ++ tests/kins-tool-offset/checkresult | 2 + tests/kins-tool-offset/sim.hal | 20 ++++ tests/kins-tool-offset/test-ui.py | 113 ++++++++++++++++++ tests/kins-tool-offset/test.ini | 112 +++++++++++++++++ tests/kins-tool-offset/test.sh | 2 + tests/kins-tool-offset/tool.tbl | 1 + 20 files changed, 471 insertions(+), 34 deletions(-) create mode 100644 tests/kins-tool-offset/README create mode 100755 tests/kins-tool-offset/checkresult create mode 100644 tests/kins-tool-offset/sim.hal create mode 100755 tests/kins-tool-offset/test-ui.py create mode 100644 tests/kins-tool-offset/test.ini create mode 100755 tests/kins-tool-offset/test.sh create mode 100644 tests/kins-tool-offset/tool.tbl diff --git a/docs/src/code/code-notes.adoc b/docs/src/code/code-notes.adoc index dc81d1257e9..e3b72e492a9 100644 --- a/docs/src/code/code-notes.adoc +++ b/docs/src/code/code-notes.adoc @@ -1312,8 +1312,7 @@ settings.tool_offset:: + * Used to compute position in various places. * Sent to Motion via the +EMCMOT_SET_OFFSET+ message. - All motion does with the offsets is export them to the HAL pins +motion.0.tooloffset.[xyzabcuvw]+. - FIXME: export these from someplace closer to the tool table (io or interp, probably) and remove the EMCMOT_SET_OFFSET message. + Motion exports the offsets to the HAL pins +motion.0.tooloffset.[xyzabcuvw]+ and hands them to the kinematics module through +kinematicsSetTool()+, for a module whose maths needs the tool length. settings.pockets_max:: Used interchangeably with +CANON_POCKETS_MAX+ (a #defined constant, set to 1000 as of April 2020). diff --git a/docs/src/motion/5-axis-kinematics.adoc b/docs/src/motion/5-axis-kinematics.adoc index 9391f8611a9..1c6e0fe406c 100644 --- a/docs/src/motion/5-axis-kinematics.adoc +++ b/docs/src/motion/5-axis-kinematics.adoc @@ -317,23 +317,17 @@ See the simulation INI files for details of the HAL connections used for the vis === Tool-Length Compensation -In order to use tools from a tool table sequentially with tool-length compensation applied automatically, a further Z-offset is required. For a tool that is longer than the "master" tool, which typically has a tool length of zero, LinuxCNC has a variable called "motion.tooloffset.z". If this variable is passed on to the kinematic component (and vismach python script), then the necessary additional Z-offset for a new tool can be accounted for by adding the component statement, for example: +In order to use tools from a tool table sequentially with tool-length compensation applied automatically, a further Z-offset is required. For a tool that is longer than the "master" tool, which typically has a tool length of zero, the kinematics accounts for the tool length in effect, for example: image::5-axis-figures/equation__38.png[align="center"] -The required HAL connection (for xyzac-trt) is: +Motion hands the tool offset in effect (G43, G49) to the kinematics module directly, so the module sees the tool from the tool table with no HAL connection. The module's tool-offset pin (xyzac-trt-kins.tool-offset) remains for a configuration that connects it, and is read only until motion has sent an offset; a value set on it that disagrees with the tool table is reported once and not used. -[source,hal] ----- -net :tool-offset motion.tooloffset.z xyzac-trt-kins.tool-offset ----- - -where: +Motion also publishes the offset on the HAL pin "motion.tooloffset.z", which is what a vismach python script reads to draw the tool: +[source,hal] ---- -:tool-offset ---------------- signal name -motion.tooloffset.z --------- output HAL pin from LinuxCNC motion module -xyzac-trt-kins.tool-offset -- input HAL pin to xyzac-trt-kins +net :tool-offset motion.tooloffset.z xyzac-trt-gui.tool-offset ---- == Custom Kinematics Components @@ -383,17 +377,18 @@ KINEMATICS = kinsname where "kinsname" is the name of your kins program. Additional HAL pins may be created by the module for variable configuration items -such as the D~x~, D~y~, D~z~, tool-offset used in the xyzac-trt kinematics module. +such as the D~x~, D~y~, D~z~ used in the xyzac-trt kinematics module. These pins can be connected to a signal for dynamic control or set once with HAL connections like: [source,hal] ---- # set offset parameters -net :tool-offset motion.tooloffset.z xyzac-trt-kins.tool-offset setp xyzac-trt-kins.y-offset 0 setp xyzac-trt-kins.z-offset 20 ---- +The tool length is not among them: motion hands it to the module from the tool table. + == Figures .Table tilting/rotating configuration diff --git a/docs/src/motion/kinematics-conventions.adoc b/docs/src/motion/kinematics-conventions.adoc index 3b58d13d653..02446cf425a 100644 --- a/docs/src/motion/kinematics-conventions.adoc +++ b/docs/src/motion/kinematics-conventions.adoc @@ -538,15 +538,26 @@ with `switchkinsRegisterOps()`; a module with one type describes itself in a In realtime it makes one HAL pin per table entry, copies the pins into the block before every call and the outputs back after it, and supplies the classic entry points, `kinematicsForward()` and the rest, so that motion sees no -difference. Outside realtime a module exports `kinsDescribe()`, which hands a -caller its table and the ops of each type; the caller fills a block from -wherever it likes and asks the same functions through `kinsOpsForward()`, -`kinsOpsInverse()`, `kinsOpsJacobian()` and the frame calls, with the same -defaults applied, so both sides get the same answers. The non-realtime loader -in `kinematics_userspace/` binds the pins of the running module by the table's -names and takes the tool from motion's own offset pins, and says once when the -module's tool pin disagrees with them, which is a config that lost the tool on -the way. `kinslimits` is built on it. +difference. It also exports `kinematicsSetTool()`, through which motion hands +the module the tool offset in effect whenever that changes, so the tool comes +from the tool table and not from a net the config had to remember. A table +entry flagged as the tool is read from its pin only until motion has sent +anything; after that the pin is overwritten with motion's value, and the shared +code says once if the pin is left disagreeing with it, which is a config +setting a tool length where the tool table should. Motion references the call +weakly, so a module written before it still loads and keeps its pin. + +Outside realtime a module exports `kinsDescribe()`, which hands a caller its +table and the ops of each type; the caller fills a block from wherever it likes +and asks the same functions through `kinsOpsForward()`, `kinsOpsInverse()`, +`kinsOpsJacobian()` and the frame calls, with the same defaults applied, so +both sides get the same answers. The non-realtime loader in +`kinematics_userspace/` binds the pins of the running module by the table's +names. Its tool is the caller's where the caller gives one through +`kinematicsUserSetTool()`, since a planner knows what a segment runs under +better than the machine does; otherwise it is motion's, from motion's own +offset pins, and the loader says once when the module's tool pin disagrees with +them. `kinslimits` is built on it. A module that does not provide the form keeps working as it did. It just cannot be evaluated outside realtime, which the loader reports. @@ -555,8 +566,9 @@ be evaluated outside realtime, which the loader reports. The kinematics type is in the block, so a caller evaluating a program that switches type puts the type each block will run under in its own block, and -nothing is switched globally. The tool is in the block, from motion. The joint -map is in the block, from `coordinates=`. Nothing else the maths needs exists, +nothing is switched globally. The tool is in the block, from motion in realtime +and from the caller outside it. The joint map is in the block, from +`coordinates=`. Nothing else the maths needs exists, and a module that finds it needs something else has found a parameter it should declare. diff --git a/docs/src/motion/switchkins.adoc b/docs/src/motion/switchkins.adoc index 70265a9e06f..fb946f38bf8 100644 --- a/docs/src/motion/switchkins.adoc +++ b/docs/src/motion/switchkins.adoc @@ -624,7 +624,13 @@ hal_ready() after it. A module built this way also exports kinsDescribe(), through which a copy of it loaded outside realtime learns its table and the maths of -each kinstype; the non-realtime loader and kinslimits use it. +each kinstype; the non-realtime loader and kinslimits use it. It +exports kinematicsSetTool() as well, through which motion hands it +the tool offset in effect whenever that changes. A table entry +flagged as the tool is overwritten with it, and the entry's pin only +matters until motion has sent anything, so a config need not net +motion.tooloffset.z to the module. A kinstype registered the older +way reads its own pins and is not affected. === Module main program diff --git a/include/kinematics.h b/include/kinematics.h index a2911224f6a..4d6871ddc1e 100644 --- a/include/kinematics.h +++ b/include/kinematics.h @@ -712,6 +712,15 @@ extern int kinsOpsJacobian(const kins_ops *ops, const kins_params *p, extern int kinematicsSwitchable(void); extern int kinematicsSwitch(int switchkins_type); + +/* The tool offset motion applies, handed to the module. Motion calls this + whenever the offset changes (G43, G49) and references it weakly, so a + module that does not export it still loads and keeps reading whatever + tool pin it has. kins_single.c and switchkins.c export it for every + module written on the parameter block: the tool then comes from the tool + table through motion, and the module's tool pin, where it has one, is + read only until motion has spoken. */ +extern int kinematicsSetTool(const EmcPose *tool); //NOTE: switchable kinematics may require Interp::Synch // before/after invoking kinematicsSwitch() // A convenient command to synch is: M66 E0 L0 diff --git a/include/kinematics_user.h b/include/kinematics_user.h index f28ac0f06ad..739df8fc41d 100644 --- a/include/kinematics_user.h +++ b/include/kinematics_user.h @@ -9,8 +9,9 @@ * The kinematics module is loaded into this process and evaluated through * its parameter block form (see kinematics.h). The block is filled from * input pins belonging to the caller's HAL component, connected to the - * same signals the running RT instance reads, and from motion's tool - * offset pins where motion is loaded, so the maths runs on live values. + * same signals the running RT instance reads, so the maths runs on live + * values; the tool is the caller's where it gives one, and motion's + * otherwise, from motion's tool offset pins where motion is loaded. * * Author: LinuxCNC * License: GPL Version 2 @@ -89,6 +90,16 @@ int kinematicsUserSetType(KinematicsUserContext* ctx, int ktype); */ int kinematicsUserGetNumTypes(KinematicsUserContext* ctx); +/** + * The tool offset to evaluate with: what the caller knows the segment + * runs under, from canon or the tool table, rather than the offset the + * machine happens to have now. It stands until replaced, or until NULL + * puts the context back to taking the tool from motion. + * + * @return 0, or -1 for an RT-only context + */ +int kinematicsUserSetTool(KinematicsUserContext* ctx, const EmcPose* tool); + /** * Perform inverse kinematics (world coords -> joint positions) * diff --git a/include/kins_rt.h b/include/kins_rt.h index 00240cc9ecd..a2db0323b56 100644 --- a/include/kins_rt.h +++ b/include/kins_rt.h @@ -42,6 +42,26 @@ extern void kinsParamsPinsWrite(const kins_pin_ref *pins, const kins_param_desc *params, int nparams, const kins_scratch *s); +/* Where the RT block's tool comes from. kinematicsSetTool() records what + motion sends in one of these; kinsToolSourceApply() writes it into a + block after the pins have been read, over the tool entry, once motion + has sent anything. Until then the tool entry's pin is all there is, as + under halrun with the module alone. A config that still nets the tool + to the module's pin loses nothing; one that sets that pin to something + else is told, once, after the two have disagreed for a thousand calls, + since the pin lags the send by a cycle. */ +typedef struct { + EmcPose tool; + int have; /* motion has sent a tool */ + int disagreeing; /* consecutive calls with the pin elsewhere */ + int warned; +} kins_tool_source; + +extern void kinsToolSourceSet(kins_tool_source *src, const EmcPose *tool); +extern void kinsToolSourceApply(kins_tool_source *src, const char *prefix, + const kins_param_desc *params, int nparams, + kins_params *p); + /* A module with one kinematics type describes itself in a kins_module_info and hands it to kinsSingleInit(); kins_single.c, linked in or loaded as kins_single_core, supplies kinematicsForward() and the diff --git a/src/emc/kinematics/kins_single.c b/src/emc/kinematics/kins_single.c index 237a15ea795..6d6937bd444 100644 --- a/src/emc/kinematics/kins_single.c +++ b/src/emc/kinematics/kins_single.c @@ -20,6 +20,7 @@ static kins_params rt_params; static kins_scratch rt_scratch; static kins_pin_ref *pins; +static kins_tool_source tool_source; static int inited; static KINEMATICS_TYPE reported_type = KINEMATICS_BOTH; static const kins_module_info *km; @@ -29,11 +30,13 @@ static const kins_ops *ops(void) return inited ? km->ops[0] : NULL; } -// the block sees the pins as they are now +// the block sees the pins as they are now, and the tool motion sent static void read_pins(void) { kinsParamsPinsRead(pins, km->params, km->nparams, &rt_params); + kinsToolSourceApply(&tool_source, km->halprefix, + km->params, km->nparams, &rt_params); } static void write_pins(void) @@ -120,6 +123,13 @@ int kinematicsJacobian(const double *joint, return kinsOpsJacobian(ops(), &rt_params, &rt_scratch, joint, pos, jac, iflags); } +int kinematicsSetTool(const EmcPose *tool) +{ + if (!tool) { return -1; } + kinsToolSourceSet(&tool_source, tool); + return 0; +} + KINEMATICS_TYPE kinematicsType(void) { return reported_type; @@ -157,6 +167,7 @@ EXPORT_SYMBOL(kinematicsInverse); EXPORT_SYMBOL(kinematicsWorkFrame); EXPORT_SYMBOL(kinematicsToolFrame); EXPORT_SYMBOL(kinematicsJacobian); +EXPORT_SYMBOL(kinematicsSetTool); EXPORT_SYMBOL(kinematicsSwitchable); EXPORT_SYMBOL(kinematicsSwitch); EXPORT_SYMBOL(kinematicsTypeFlags); diff --git a/src/emc/kinematics/kins_util.c b/src/emc/kinematics/kins_util.c index 24508b06f94..9dc51889fb2 100644 --- a/src/emc/kinematics/kins_util.c +++ b/src/emc/kinematics/kins_util.c @@ -1730,6 +1730,43 @@ void kinsParamsPinsWrite(const kins_pin_ref *pins, } } // kinsParamsPinsWrite() +void kinsToolSourceSet(kins_tool_source *src, const EmcPose *tool) +{ + if (!src || !tool) { return; } + src->tool = *tool; + src->have = 1; +} // kinsToolSourceSet() + +void kinsToolSourceApply(kins_tool_source *src, const char *prefix, + const kins_param_desc *params, int nparams, + kins_params *p) +{ + int i; + if (!src || !p || !src->have) { return; } + for (i = 0; i < nparams && i < KINS_MAX_PARAMS; i++) { + const kins_param_desc *d = ¶ms[i]; + double diff; + if (!d->tool || d->dir == KINS_OUT) { continue; } + /* a pin nobody set reads zero, which is not a disagreement */ + diff = p->geometry[i] - src->tool.tran.z; + if (p->geometry[i] != 0.0 && (diff > 1e-9 || diff < -1e-9)) { + if (src->disagreeing < 1000) { + src->disagreeing++; + } else if (!src->warned) { + rtapi_print_msg(RTAPI_MSG_ERR, + "%s.%s disagrees with the tool offset motion applies;" + " motion's is used, the pin is not needed\n", + prefix ? prefix : "kins", d->name); + src->warned = 1; + } + } else { + src->disagreeing = 0; + } + p->geometry[i] = src->tool.tran.z; + } + p->tool = src->tool; +} // kinsToolSourceApply() + #ifdef RTAPI EXPORT_SYMBOL(map_coordinates_to_jnumbers); EXPORT_SYMBOL(mapped_joints_to_position); @@ -1769,4 +1806,6 @@ EXPORT_SYMBOL(kinsOpsJacobian); EXPORT_SYMBOL(kinsParamsPinsCreate); EXPORT_SYMBOL(kinsParamsPinsRead); EXPORT_SYMBOL(kinsParamsPinsWrite); +EXPORT_SYMBOL(kinsToolSourceSet); +EXPORT_SYMBOL(kinsToolSourceApply); #endif diff --git a/src/emc/kinematics/switchkins.c b/src/emc/kinematics/switchkins.c index b554e4e96df..d770bf035a0 100644 --- a/src/emc/kinematics/switchkins.c +++ b/src/emc/kinematics/switchkins.c @@ -55,6 +55,7 @@ static const kins_ops *kops[SWITCHKINS_MAX_TYPES] = {NULL}; static kins_params rt_params; static kins_scratch rt_scratch[SWITCHKINS_MAX_TYPES]; static kins_pin_ref *pins; +static kins_tool_source tool_source; static int inited; // types provided, counted in rtapi_app_main() once they are all in @@ -113,13 +114,25 @@ static void get_lastpose(int ktype, EmcPose* pos) pos->w = lastpose[ktype].w; } // get_lastpose() -// the block sees the pins as they are now, and the type asked for +// the block sees the pins as they are now, the tool motion sent, and +// the type asked for static void read_block(int ktype) { rt_params.ktype = ktype; kinsParamsPinsRead(pins, kp.params, kp.nparams, &rt_params); + kinsToolSourceApply(&tool_source, kp.halprefix, kp.params, kp.nparams, + &rt_params); } +// the tool from motion, for the types written on the block; a type +// provided the older way reads its own pins and does not see it +int kinematicsSetTool(const EmcPose *tool) +{ + if (!tool) { return -1; } + kinsToolSourceSet(&tool_source, tool); + return 0; +} // kinematicsSetTool() + static void write_block(int ktype) { kinsParamsPinsWrite(pins, kp.params, kp.nparams, &rt_scratch[ktype]); @@ -550,6 +563,7 @@ EXPORT_SYMBOL(kinematicsToolFrame); EXPORT_SYMBOL(kinematicsWorkFrame); EXPORT_SYMBOL(kinematicsToolFrameInverse); EXPORT_SYMBOL(kinematicsJacobian); +EXPORT_SYMBOL(kinematicsSetTool); EXPORT_SYMBOL(switchkinsRegister); EXPORT_SYMBOL(switchkinsRegisterFrames); EXPORT_SYMBOL(switchkinsRegisterToolFrameInverse); diff --git a/src/emc/kinematics_userspace/kinematics_user.c b/src/emc/kinematics_userspace/kinematics_user.c index 3a69af3e270..34e31d09348 100644 --- a/src/emc/kinematics_userspace/kinematics_user.c +++ b/src/emc/kinematics_userspace/kinematics_user.c @@ -6,7 +6,9 @@ * kinsDescribe(), and evaluates its kinematics through the parameter * block (see kinematics.h). The block is filled from HAL: one input pin * of the caller's component per table entry, connected to the signal the - * RT instance's pin reads, so the values are the live ones; and the tool + * RT instance's pin reads, so the values are the live ones. The tool is + * the caller's where it has given one, since a planner knows what a + * segment runs under better than the machine does; otherwise it comes * from motion's own tooloffset pins where motion is loaded, so that the * tool the module sees is the one motion has, whether or not the config * netted it to the module's pin. @@ -60,6 +62,8 @@ struct KinematicsUserContext { int cell_of_tool[AXIS_COUNT]; /* motion.tooloffset.*, -1 if absent */ int tool_param; /* the table's tool entry, -1 if none */ int warned_tool; + EmcPose caller_tool; /* from kinematicsUserSetTool() */ + int have_caller_tool; double last_joints[EMCMOT_MAX_JOINTS]; /* what the last inverse found */ }; @@ -235,7 +239,8 @@ static int bind_all(KinematicsUserContext *ctx) return 0; } -/* The block sees the pins as they are now. */ +/* The block sees the pins as they are now, and the tool of whoever + knows it best: the caller, then motion, then the module's own pin. */ static void refresh(KinematicsUserContext *ctx) { int i; @@ -251,6 +256,14 @@ static void refresh(KinematicsUserContext *ctx) ctx->params.tool.tran.z = ctx->params.geometry[ctx->tool_param]; } + if (ctx->have_caller_tool) { + ctx->params.tool = ctx->caller_tool; + if (ctx->tool_param >= 0) { + ctx->params.geometry[ctx->tool_param] = ctx->caller_tool.tran.z; + } + return; + } + for (i = 0; i < AXIS_COUNT; i++) { int c = ctx->cell_of_tool[i]; tool[i] = 0.0; @@ -262,8 +275,10 @@ static void refresh(KinematicsUserContext *ctx) /* the module's pin and motion disagree: the config lost the tool somewhere between them. Say so once; motion's value is the one - being cut with. */ + being cut with. A pin nobody set reads zero, which is not a + disagreement. */ if (ctx->tool_param >= 0 && !ctx->warned_tool + && ctx->params.geometry[ctx->tool_param] != 0.0 && fabs(tool[AXIS_Z] - ctx->params.geometry[ctx->tool_param]) > 1e-9) { fprintf(stderr, "kinematics_user: %s.%s is %.6g but motion.tooloffset.z is %.6g;" @@ -526,6 +541,18 @@ int kinematicsUserGetNumTypes(KinematicsUserContext* ctx) return ctx->info.ntypes; } +int kinematicsUserSetTool(KinematicsUserContext* ctx, const EmcPose* tool) +{ + if (!ctx || !ctx->initialized || ctx->rt_only) return -1; + if (tool) { + ctx->caller_tool = *tool; + ctx->have_caller_tool = 1; + } else { + ctx->have_caller_tool = 0; + } + return 0; +} + int kinematicsUserInverse(KinematicsUserContext* ctx, const EmcPose* world, double* joints) diff --git a/src/emc/motion/command.c b/src/emc/motion/command.c index b8094e03e9b..a9b16b6185c 100644 --- a/src/emc/motion/command.c +++ b/src/emc/motion/command.c @@ -70,6 +70,11 @@ #include "homing.h" #include "axis.h" +// the kinematics module takes the tool offset from here when it can; a +// module written before the call exports no such symbol, and the weak +// reference leaves it NULL rather than refusing to load motion +#pragma weak kinematicsSetTool + #define ABS(x) (((x) < 0) ? -(x) : (x)) @@ -2038,6 +2043,9 @@ void emcmotCommandHandler_locked(void *arg, long servo_period) case EMCMOT_SET_OFFSET: rtapi_print_msg(RTAPI_MSG_DBG, "SET_OFFSET"); emcmotStatus->tool_offset = emcmotCommand->tool_offset; + if (kinematicsSetTool) { + kinematicsSetTool(&emcmotStatus->tool_offset); + } break; case EMCMOT_SET_AXIS_POSITION_LIMITS: diff --git a/tests/kins-params/check.py b/tests/kins-params/check.py index 0dc67110708..cf7529e2506 100755 --- a/tests/kins-params/check.py +++ b/tests/kins-params/check.py @@ -129,6 +129,35 @@ def compare(what, ours, theirs): for a, n in enumerate("xyzabcuvw"): compare("jacobian [%d][%s]" % (j, n), J[j][a], hal.get_value("paritycheck.jac-%d-%s" % (j, n))) +# the caller's tool wins over the module's pin: for a module with a tool +# entry, a length of the caller's must move the inverse, and handing the +# tool back to HAL must return it to what realtime found +kins.kinematicsUserSetTool.argtypes = [ctypes.c_void_p, ctypes.POINTER(EmcPose)] +tool_pin = None +for name in ("tool-offset", "tool-offset-z"): + try: + hal.get_value("%s.%s" % (module, name)) + tool_pin = name + except RuntimeError: + pass +if r_inv == 0 and rc_inv == 0 and tool_pin: + P = pose_of(pose_in) if frompose else F + T = pose_of([0.0] * AXES) + T.z = hal.get_value("%s.%s" % (module, tool_pin)) + 10.0 + kins.kinematicsUserSetTool(ctx, ctypes.byref(T)) + qt = Joints(*jnt_in) + if kins.kinematicsUserInverse(ctx, ctypes.byref(P), qt) != 0: + fail("inverse with the caller's tool") + elif all(close(qt[j], qi[j]) for j in range(joints)): + fail("the caller's tool did not move the inverse") + kins.kinematicsUserSetTool(ctx, None) + qt = Joints(*jnt_in) + if kins.kinematicsUserInverse(ctx, ctypes.byref(P), qt) != 0: + fail("inverse with the tool handed back") + else: + for j in range(joints): + compare("inverse joint %d after the tool is handed back" % j, qt[j], qi[j]) + kins.kinematicsUserFree(ctx) halc.hal_exit(comp_id) diff --git a/tests/kins-tool-offset/README b/tests/kins-tool-offset/README new file mode 100644 index 00000000000..db4bd42a726 --- /dev/null +++ b/tests/kins-tool-offset/README @@ -0,0 +1,7 @@ +The kinematics module takes the tool offset from motion, not from a net. + +Runs xyzac-trt-kins under motion with nothing connected to its tool-offset +pin, applies a tool length through G43, and checks that the joints move as +the tool length requires. Then connects motion.tooloffset.z to the pin the +old way and checks that nothing changes, and that G49 takes the length back +out through motion alone. diff --git a/tests/kins-tool-offset/checkresult b/tests/kins-tool-offset/checkresult new file mode 100755 index 00000000000..24dc9aa53e3 --- /dev/null +++ b/tests/kins-tool-offset/checkresult @@ -0,0 +1,2 @@ +#!/bin/sh +exit 0 # test failure is indicated by test.sh exit value diff --git a/tests/kins-tool-offset/sim.hal b/tests/kins-tool-offset/sim.hal new file mode 100644 index 00000000000..81a0df64444 --- /dev/null +++ b/tests/kins-tool-offset/sim.hal @@ -0,0 +1,20 @@ +# the module under test, with nothing on its tool-offset pin +loadrt [KINS]KINEMATICS +loadrt [EMCMOT]EMCMOT servo_period_nsec=[EMCMOT]SERVO_PERIOD num_joints=[KINS]JOINTS + +addf motion-command-handler servo-thread +addf motion-controller servo-thread + +# offsets, so that the tool length reaches the joints through a rotation +setp xyzac-trt-kins.y-offset 20 +setp xyzac-trt-kins.z-offset 10 + +net J0 joint.0.motor-pos-cmd => joint.0.motor-pos-fb +net J1 joint.1.motor-pos-cmd => joint.1.motor-pos-fb +net J2 joint.2.motor-pos-cmd => joint.2.motor-pos-fb +net J3 joint.3.motor-pos-cmd => joint.3.motor-pos-fb +net J4 joint.4.motor-pos-cmd => joint.4.motor-pos-fb + +net estop-loop iocontrol.0.user-enable-out iocontrol.0.emc-enable-in +net tool-prep-loop iocontrol.0.tool-prepare iocontrol.0.tool-prepared +net tool-change-loop iocontrol.0.tool-change iocontrol.0.tool-changed diff --git a/tests/kins-tool-offset/test-ui.py b/tests/kins-tool-offset/test-ui.py new file mode 100755 index 00000000000..dbd174caa66 --- /dev/null +++ b/tests/kins-tool-offset/test-ui.py @@ -0,0 +1,113 @@ +#!/usr/bin/env python3 +# The kinematics module takes the tool offset from motion. +# +# xyzac-trt-kins runs with nothing connected to its tool-offset pin. A +# tool length applied with G43 must still reach the joints, since motion +# hands the offset to the module; connecting motion.tooloffset.z to the +# pin afterwards, the old way, must change nothing; and G49 must take the +# length back out again through motion alone. + +import linuxcnc +import hal +import subprocess +import sys +import os +import time + +TOOL_LENGTH = 25.0 +POSE = "G0 X10 Y20 Z30 A30 C45" +AWAY = "G0 X0 Y0 Z0 A0 C0" + +c = linuxcnc.command() +s = linuxcnc.stat() + +c.state(linuxcnc.STATE_ESTOP_RESET) +c.state(linuxcnc.STATE_ON) +c.home(-1) +c.wait_complete() +c.mode(linuxcnc.MODE_MDI) + +errors = 0 + +def error(msg): + global errors + errors += 1 + print("*** ERROR " + msg) + +def mdi(*cmds): + for cmd in cmds: + c.mdi(cmd) + c.wait_complete(30) + +def joints(): + # the commanded joint positions once the move has settled: in position, + # nothing queued, and the same answer twice in a row, since the in + # position flag can go up a cycle before the last increment lands + deadline = time.time() + 30 + last = None + while time.time() < deadline: + s.poll() + now = [s.joint_position[i] for i in range(5)] + if s.inpos and not s.queue and now == last: + return now + last = now + time.sleep(0.1) + error("timed out waiting for the move") + return last + +def same(a, b, tol=1e-6): + return all(abs(x - y) <= tol for x, y in zip(a, b)) + +def show(what, j): + print("%-28s %s" % (what, " ".join("%.6f" % v for v in j))) + +# no tool: the pose with nothing applied +mdi("G49", POSE) +base = joints() +show("G49", base) + +# tool applied through motion, the pin still at its default +mdi("G43 H1", AWAY, POSE) +with_tool = joints() +show("G43 H1, pin unconnected", with_tool) +pin = hal.get_value("xyzac-trt-kins.tool-offset") +if pin != 0.0: + error("the tool-offset pin reads %g with nothing connected" % pin) +if same(base, with_tool): + error("the tool length did not reach the joints") + +# the table on rotaries at A30 C45: the tool length moves Y and Z joints, +# by a known amount, since the pivot geometry is the module's alone +tool_z = hal.get_value("motion.tooloffset.z") +if abs(tool_z - TOOL_LENGTH) > 1e-9: + error("motion.tooloffset.z is %g, expected %g" % (tool_z, TOOL_LENGTH)) +if abs(with_tool[0] - base[0]) > 1e-6: + error("the tool length moved joint 0, which the A rotation does not touch") + +# the old connection: nothing may change +subprocess.check_call(["halcmd", "net", ":tool-offset", + "motion.tooloffset.z", "xyzac-trt-kins.tool-offset"]) +mdi(AWAY, POSE) +with_net = joints() +show("G43 H1, pin connected", with_net) +pin = hal.get_value("xyzac-trt-kins.tool-offset") +if abs(pin - TOOL_LENGTH) > 1e-9: + error("the connected tool-offset pin reads %g" % pin) +if not same(with_tool, with_net): + error("connecting the pin changed the joints") + +# and back out, through motion, with the pin connected +mdi("G49", AWAY, POSE) +without = joints() +show("G49, pin connected", without) +if not same(base, without): + error("G49 did not take the tool length back out") + +for f in ("sim.var", "sim.var.bak"): + try: + os.unlink(f) + except OSError: + pass + +print("Exiting with %d errors" % errors) +sys.exit(1 if errors else 0) diff --git a/tests/kins-tool-offset/test.ini b/tests/kins-tool-offset/test.ini new file mode 100644 index 00000000000..bb7839671ed --- /dev/null +++ b/tests/kins-tool-offset/test.ini @@ -0,0 +1,112 @@ +[EMC] +VERSION = 1.1 +DEBUG = 0x0 + +[DISPLAY] +DISPLAY = ./test-ui.py + +[RS274NGC] +PARAMETER_FILE = sim.var + +[EMCMOT] +EMCMOT = motmod +COMM_TIMEOUT = 4.0 +SERVO_PERIOD = 1000000 + +[TASK] +TASK = milltask +CYCLE_TIME = 0.001 + +[HAL] +HALFILE = sim.hal + +[TRAJ] +COORDINATES = XYZAC +LINEAR_UNITS = mm +ANGULAR_UNITS = deg +DEFAULT_LINEAR_VELOCITY = 20 +MAX_LINEAR_VELOCITY = 200 +MAX_LINEAR_ACCELERATION = 2000 +NO_FORCE_HOMING = 1 + +[EMCIO] +TOOL_TABLE = tool.tbl + +[KINS] +KINEMATICS = xyzac-trt-kins +JOINTS = 5 + +[AXIS_X] +MIN_LIMIT = -200 +MAX_LIMIT = 200 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 2000 + +[AXIS_Y] +MIN_LIMIT = -200 +MAX_LIMIT = 200 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 2000 + +[AXIS_Z] +MIN_LIMIT = -200 +MAX_LIMIT = 200 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 2000 + +[AXIS_A] +MIN_LIMIT = -100 +MAX_LIMIT = 100 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 2000 + +[AXIS_C] +MIN_LIMIT = -36000 +MAX_LIMIT = 36000 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 2000 + +[JOINT_0] +TYPE = LINEAR +HOME = 0 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 2000 +MIN_LIMIT = -200 +MAX_LIMIT = 200 +HOME_SEQUENCE = 0 + +[JOINT_1] +TYPE = LINEAR +HOME = 0 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 2000 +MIN_LIMIT = -200 +MAX_LIMIT = 200 +HOME_SEQUENCE = 0 + +[JOINT_2] +TYPE = LINEAR +HOME = 0 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 2000 +MIN_LIMIT = -200 +MAX_LIMIT = 200 +HOME_SEQUENCE = 0 + +[JOINT_3] +TYPE = ANGULAR +HOME = 0 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 2000 +MIN_LIMIT = -100 +MAX_LIMIT = 100 +HOME_SEQUENCE = 0 + +[JOINT_4] +TYPE = ANGULAR +HOME = 0 +MAX_VELOCITY = 200 +MAX_ACCELERATION = 2000 +MIN_LIMIT = -36000 +MAX_LIMIT = 36000 +HOME_SEQUENCE = 0 diff --git a/tests/kins-tool-offset/test.sh b/tests/kins-tool-offset/test.sh new file mode 100755 index 00000000000..a31b772a81c --- /dev/null +++ b/tests/kins-tool-offset/test.sh @@ -0,0 +1,2 @@ +#!/bin/bash -e +linuxcnc -r test.ini diff --git a/tests/kins-tool-offset/tool.tbl b/tests/kins-tool-offset/tool.tbl new file mode 100644 index 00000000000..acb961918d9 --- /dev/null +++ b/tests/kins-tool-offset/tool.tbl @@ -0,0 +1 @@ +T1 P1 Z25 D6 ;the tool with a length From d2b05bbb8ebbc8f8e1209f0a06233d264592084d Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Mon, 14 Sep 2026 13:41:31 +1000 Subject: [PATCH 14/19] 5axiskins, maxkins: take the tool length from motion The trt modules, genhexkins and pentakins already have motion hand them the offset in effect; these two still read a pin the config had to net from motion.tooloffset.z, and a missing net gave wrong joints with no error. Flag the entry as the tool, drop the nets from the sims, and have the kins-switch test check that G43.4 reaches the joints with nothing netted: in the tilted pose the length moves them by its tilt term, the same length along the tool instead of along Z. --- .../vismach/5axis/bridgemill/5axisgui.hal | 1 - .../sim/axis/vismach/5axis/max5/max5kins.hal | 4 +- docs/src/man/man9/kins.9.adoc | 20 ++++--- src/emc/kinematics/5axiskins.c | 6 +- src/emc/kinematics/maxkins.c | 2 +- tests/kins-switch/README | 5 +- tests/kins-switch/test-ui.py | 55 +++++++++++++++++-- 7 files changed, 70 insertions(+), 23 deletions(-) diff --git a/configs/sim/axis/vismach/5axis/bridgemill/5axisgui.hal b/configs/sim/axis/vismach/5axis/bridgemill/5axisgui.hal index c45e7c5765b..3780f9ce3d0 100644 --- a/configs/sim/axis/vismach/5axis/bridgemill/5axisgui.hal +++ b/configs/sim/axis/vismach/5axis/bridgemill/5axisgui.hal @@ -17,7 +17,6 @@ net :gui-pivot-len <= 5axisgui.pivot_len net :gui-pivot-len => 5axiskins.pivot-length net :tool-len <= motion.tooloffset.z -net :tool-len => 5axiskins.tool-length net :tool-len => 5axisgui.tool_length net :tool-diam <= halui.tool.diameter diff --git a/configs/sim/axis/vismach/5axis/max5/max5kins.hal b/configs/sim/axis/vismach/5axis/max5/max5kins.hal index 85b9d37c2be..04b5dc47843 100644 --- a/configs/sim/axis/vismach/5axis/max5/max5kins.hal +++ b/configs/sim/axis/vismach/5axis/max5/max5kins.hal @@ -16,9 +16,7 @@ loadusr -W ./max5gui.py # set a visible tool setp max5gui.tool-radius 3 -# the tool length is applied along the tool, not along Z, so the tip stays -# on the programmed point as B tilts -net tool-len motion.tooloffset.z max5gui.tool-length maxkins.tool-length +net tool-len motion.tooloffset.z max5gui.tool-length # add motion controller functions to servo thread addf motion-command-handler servo-thread diff --git a/docs/src/man/man9/kins.9.adoc b/docs/src/man/man9/kins.9.adoc index f2bd1051d4c..79a3a116e21 100644 --- a/docs/src/man/man9/kins.9.adoc +++ b/docs/src/man/man9/kins.9.adoc @@ -252,9 +252,10 @@ replacing it. Put a given length in one column or the other, not both. *maxkins.tool-length*:: Tool length, applied along the tool rather than along Z, so that the tip - stays on the programmed point as B tilts. Net it from *motion.tooloffset.z*. - Left unconnected, the tool length stays where canon put it, along machine - Z, which is only correct at B0. + stays on the programmed point as B tilts. Motion hands the module the + offset in effect (G43, G49), so the pin needs no connection; it is read + only until motion has sent anything. To avoid a joint jump, change the + tool offset only when B is 0. === pentakins - Pentapod Kinematics @@ -413,12 +414,13 @@ expected by it (XYZBCW `->` joints 0..5) *5axiskins.tool-length*:: Tool length, applied along the tool rather than along Z, so that the tip - stays on the programmed point as B and C move. Net it from - *motion.tooloffset.z*. Left unconnected, the tool length stays where canon - put it, along machine Z, which is only correct at B0. A tool length in the - W column of the tool table reaches the same place, once a block commands W, - and adds to this pin rather than replacing it. Put a given length in one - column or the other, not both. + stays on the programmed point as B and C move. Motion hands the module the + offset in effect (G43, G49), so the pin needs no connection; it is read + only until motion has sent anything. To avoid a joint jump, change the + tool offset only when B is 0. A tool length in the W column of the tool + table reaches the same place, once a block commands W, and adds to this + one rather than replacing it. Put a given length in one column or the + other, not both. == SEE ALSO diff --git a/src/emc/kinematics/5axiskins.c b/src/emc/kinematics/5axiskins.c index d1829b483b0..7af800fa693 100644 --- a/src/emc/kinematics/5axiskins.c +++ b/src/emc/kinematics/5axiskins.c @@ -12,8 +12,8 @@ * * Notes: * 1) pivot-length must agree with the mechanical design -* (including vismach simulation); the tool length comes -* in on the tool-length pin of its own +* (including vismach simulation); the tool length is +* the offset motion applies, handed over by motion * 2) C axis: spherical coordinates aziumthal angle (t or theta) * projection of radius to xy plane * 3) B axis: spherical coordinates polar angle (p or phi) @@ -63,7 +63,7 @@ // the geometry, one pin each; the maths reads it from the block static const kins_param_desc fiveaxis_params[] = { { "pivot-length", KINS_PARAM_FLOAT, KINS_IN, 0, DEFAULT_PIVOT_LENGTH }, - { "tool-length", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, + { "tool-length", KINS_PARAM_FLOAT, KINS_IN, 1, 0.0 }, }; enum { P_PIVOT_LENGTH, P_TOOL_LENGTH }; diff --git a/src/emc/kinematics/maxkins.c b/src/emc/kinematics/maxkins.c index b64eee2711e..e1558d89152 100644 --- a/src/emc/kinematics/maxkins.c +++ b/src/emc/kinematics/maxkins.c @@ -35,7 +35,7 @@ static const kins_param_desc max_params[] = { { "pivot-length", KINS_PARAM_FLOAT, KINS_IO, 0, 0.666 }, { "conventional-directions", KINS_PARAM_BIT, KINS_IN, 0, 0 }, // default is unconventional - { "tool-length", KINS_PARAM_FLOAT, KINS_IN, 0, 0 }, + { "tool-length", KINS_PARAM_FLOAT, KINS_IN, 1, 0 }, }; enum { P_PIVOT_LENGTH, P_CON, P_TOOL_LENGTH }; diff --git a/tests/kins-switch/README b/tests/kins-switch/README index e031f2cd0ff..e426b4d7554 100644 --- a/tests/kins-switch/README +++ b/tests/kins-switch/README @@ -7,5 +7,6 @@ block after the switch is planned in the kinematics that runs it, that the selection reaches the motion controller and the interpreter, that a negative P word and a kinematics the module does not provide are refused, that G13.1 cancels to the identity kinematics the module declares (type 1 -here, not 0), and that G13.1 in an ON_ABORT_COMMAND routine does not -swallow the rest of the routine. +here, not 0), that the tool length G43.4 puts in effect reaches the +kinematics with nothing netted to its pin, and that G13.1 in an +ON_ABORT_COMMAND routine does not swallow the rest of the routine. diff --git a/tests/kins-switch/test-ui.py b/tests/kins-switch/test-ui.py index 7e999b9aa76..e62800c5cd7 100755 --- a/tests/kins-switch/test-ui.py +++ b/tests/kins-switch/test-ui.py @@ -76,6 +76,7 @@ def mdi(cmd): said = [] at_switch = None +holding = False strayed = [0.0] * JOINTS w_reached = 0.0 deadline = time.time() + 60 @@ -88,10 +89,25 @@ def mdi(cmd): last = k if k == 1 and at_switch is None: at_switch = now - if k == 1 and at_switch is not None: - for j in CARRIED: - strayed[j] = max(strayed[j], abs(now[j] - at_switch[j])) + holding = True + else: + holding = False + # the joints are held over the first stretch in identity, the one the + # W stroke runs in; later on the program applies a tool length in the + # tilted pose, which moves the joints on purpose + if holding: + # only a sample with W out on its stroke counts: the lines after + # it switch kinematics and apply a tool length in the tilted pose, + # which moves the joints on purpose, and they can all be over + # before the first sample that sees W back home + if now[5] > 1e-6: + for j in CARRIED: + strayed[j] = max(strayed[j], abs(now[j] - at_switch[j])) w_reached = max(w_reached, now[5]) + # the stretch ends with W back home: a later switch to the primary + # and back can be over between two samples, so do not wait to see it + if w_reached > 10 - 1e-3 and abs(now[5]) < 1e-6: + holding = False said += drain() if s.interp_state == linuxcnc.INTERP_IDLE and time.time() > deadline - 58: break @@ -147,10 +163,41 @@ def mdi(cmd): else: print("G43.4 switched to primary with the offset, G49 cancelled both") -# ---- a negative kinematics number is refused ----------------------------- +# ---- the tool length reaches the kinematics with nothing netted ---------- +# +# Motion hands the module the offset G43 puts in effect. The head is +# tilted, so the offset moves the joints, by the tilt term of the length: +# the same length along the tool axis instead of along Z. +import math c.mode(linuxcnc.MODE_MDI) c.wait_complete(30) +drain() +mdi("G12.1 P0") +mdi("G0 X10 Y10 Z-5 B-22.5 C45") +mdi("G49") +before = mdi("G0 X10 Y10 Z-5") +after = mdi("G43.4 H1") +L = 12.5 # tool 1 in tool.tbl +b, cc = math.radians(-22.5), math.radians(45) +want = [-L * math.sin(math.pi - b) * math.cos(cc), + -L * math.sin(math.pi - b) * math.sin(cc), + -L * (1 + math.cos(math.pi - b))] +got = [after[j] - before[j] for j in (0, 1, 2)] +if max(abs(g - w) for g, w in zip(got, want)) > 1e-3: + error("G43.4 in the tilted pose moved the joints by %s, not %s" + % (" ".join("%.4f" % v for v in got), " ".join("%.4f" % v for v in want))) +else: + print("G43.4 moved the joints by the tilt term of the tool length") +# G49 would drop to identity and hold the joints where they are; a zero +# offset without a switch takes the length back out +back = mdi("G43.1 Z0") +if max(abs(back[j] - before[j]) for j in (0, 1, 2)) > 1e-3: + error("a zero tool length did not take the length back out of the joints") +mdi("G49") + +# ---- a negative kinematics number is refused ----------------------------- + drain() c.mdi("G12.1 P-1") c.wait_complete(30) From e8f2572057db8ab44aa4d373884e829b74c8c73d Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Mon, 7 Sep 2026 16:30:15 +1000 Subject: [PATCH 15/19] 5axiskins: supply the work and tool frames The head carries the tool and nothing turns the work, so the work frame is the machine frame and the tool frame is Rz(C) Ry(-B), the tilt left handed about y as the module's note 10 says. Its third column is the tool axis the forward already uses, from the tip towards the holder, so no native rotation is needed. Without them G53.1, G53.3, G53.6 and G68.3 refused on the bridgemill sim while every other five-axis module took them. tests/kins-frames takes the module and gains two checks: the type the module models must supply frames of its own, since a switchable module's identity type passed on its neighbour's answers; and the W joint runs the tool out along the reported axis reversed, which ties the frame to the forward. Reversing the tilt, reversing the head's turn and dropping the frames each fail. --- src/emc/kinematics/5axiskins.c | 21 +++++++++++++++++++++ tests/kins-frames/checkresult | 2 +- tests/kins-frames/framecheck.c | 27 ++++++++++++++++++++++++++- tests/kins-frames/test.sh | 3 +++ 4 files changed, 51 insertions(+), 2 deletions(-) diff --git a/src/emc/kinematics/5axiskins.c b/src/emc/kinematics/5axiskins.c index 7af800fa693..81a765253b4 100644 --- a/src/emc/kinematics/5axiskins.c +++ b/src/emc/kinematics/5axiskins.c @@ -200,9 +200,30 @@ static int fiveaxis_jacobian(const kins_params *p, jac); } // fiveaxis_jacobian() +// The head carries the tool and nothing turns the work: the work frame is the +// machine frame and the tool frame Rz(C) Ry(-B), the tilt left handed about y +// (note 10), its third column the tool axis from the tip towards the holder. +static int fiveaxis_tool_frame(const kins_params *p, const double *joints, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + (void)fflags; + const double sb = sin(joints[JB]*TO_RAD), cb = cos(joints[JB]*TO_RAD); + const double sc = sin(joints[JC]*TO_RAD), cc = cos(joints[JC]*TO_RAD); + + rot->x.x = cb * cc; rot->y.x = -sc; rot->z.x = -sb * cc; + rot->x.y = cb * sc; rot->y.y = cc; rot->z.y = -sb * sc; + rot->x.z = sb; rot->y.z = 0; rot->z.z = cb; + + return 0; +} // fiveaxis_tool_frame() + static const kins_ops fiveaxis_ops = { .forward = fiveaxis_forward, .inverse = fiveaxis_inverse, + .work = kinsIdentityFrame, + .tool = fiveaxis_tool_frame, + .native = &TOOL_FRAME_SPINDLE, .jacobian = fiveaxis_jacobian, .primary = 1, }; diff --git a/tests/kins-frames/checkresult b/tests/kins-frames/checkresult index 5fefd687bac..48c09ee0d98 100755 --- a/tests/kins-frames/checkresult +++ b/tests/kins-frames/checkresult @@ -1,3 +1,3 @@ #!/bin/sh -[ "$(grep -c 'frames agree' "$1")" = 5 ] \ +[ "$(grep -c 'frames agree' "$1")" = 6 ] \ && ! grep -q "FAIL" "$1" diff --git a/tests/kins-frames/framecheck.c b/tests/kins-frames/framecheck.c index 3388d938055..ed6acf27a08 100644 --- a/tests/kins-frames/framecheck.c +++ b/tests/kins-frames/framecheck.c @@ -52,6 +52,9 @@ RTAPI_MP_INT(ktype, "switchkins type where the module models its own machine"); static int spin = -1; RTAPI_MP_INT(spin, "joint that turns the whole head about the machine's z, -1 for none"); +static int quill = -1; +RTAPI_MP_INT(quill, "joint that extends the tool along its own axis, -1 for none"); + static int r1 = -1, r2 = -1, r3 = -1; RTAPI_MP_INT(r1, "joint number of the first rotary to sweep"); RTAPI_MP_INT(r2, "joint number of the second rotary, -1 for none"); @@ -206,6 +209,16 @@ static void check(const double *j, int own_kinematics) && close3(&tool.z, 0, 0, 1), "the spindle stays square", j); } + if (quill >= 0) { + /* the joint runs the tool out along its own axis, away from the + holder, so the tip moves along the tool axis reversed: the one + tie between the reported frame and the forward transform on a + machine that turns nothing but the tool */ + response(j, quill, &d); + expect(close3(&d, -tool.z.x, -tool.z.y, -tool.z.z), + "the quill runs out along the tool axis", j); + } + if (spin >= 0) { memcpy(t, j, sizeof(t)); t[spin] = j[spin] + TURN; @@ -228,7 +241,7 @@ int rtapi_app_main(void) const int angles = sizeof(angle) / sizeof(angle[0]); double j[EMCMOT_MAX_JOINTS]; int a, b, c, t; - int checked = 0; + int checked = 0, own = 0; if (joints < 1 || joints > EMCMOT_MAX_JOINTS) { rtapi_print_msg(RTAPI_MSG_ERR, "framecheck: joints=%d\n", joints); @@ -246,6 +259,7 @@ int rtapi_app_main(void) memset(j, 0, sizeof(j)); if (!carries_tool) { j[0] = 10; j[1] = 20; j[2] = 30; } + own = 0; /* every kinematics the module offers, not just the one it starts in: the frames a switchable module reports are per type, and the @@ -254,6 +268,7 @@ int rtapi_app_main(void) if (kinematicsSwitchable() && kinematicsSwitch(t)) { break; } if (!supplies_frames(j)) { continue; } checked++; + if (t == ktype) { own = 1; } for (a = 0; a < angles; a++) { if (r1 >= 0) { j[r1] = angle[a]; } @@ -272,6 +287,16 @@ int rtapi_app_main(void) if (!kinematicsSwitchable()) { break; } } + /* the identity type a switchable module carries supplies frames of + its own, so a module that reports none for the machine it models + would otherwise pass on its neighbour's answers */ + if (checked && !own) { + rtapi_print_msg(RTAPI_MSG_ERR, + "framecheck: FAIL the module reports no frames for" + " kinematics type %d, the machine it models\n", ktype); + failures++; + } + if (!checked) { rtapi_print_msg(RTAPI_MSG_ERR, "framecheck: the module reports frames for no type\n"); diff --git a/tests/kins-frames/test.sh b/tests/kins-frames/test.sh index b1471ea45b7..109e69821dc 100755 --- a/tests/kins-frames/test.sh +++ b/tests/kins-frames/test.sh @@ -49,5 +49,8 @@ setp xyzbca_trsrn_kins.z-pivot 200 setp xyzbca_trsrn_kins.tool-offset-z 50" \ "joints=6 r1=3 r2=4 r3=5 spin=5 ktype=1" +run "5axiskins coordinates=XYZBCW" "setp 5axiskins.pivot-length 250" \ + "joints=6 carries_tool=1 r1=3 r2=4 spin=4 quill=5" + run "pumakins" "setp pumakins.A2 300" \ "joints=6 carries_tool=1 r1=0 r2=3 r3=4 spin=0" From 264553bb7564f6849f956de6daf8e1efd4ff3eff Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Sat, 5 Sep 2026 00:11:48 +1000 Subject: [PATCH 16/19] kinematics: reach the frames and the tool frame inverse from outside realtime The loader gains the two frames, the tool frame inverse and a survey of which joints turn the work, on kins_util.c code the library already carries; only the glue between the block form and the joint-only frame functions is new. The survey lets a caller hold the table still without a config entry naming it. A second survey tells the two orienting rotaries apart, the primary whose axis is fixed in the machine from the secondary whose axis it carries, by turning each a little and reading the axis of the resulting rotation; the sign of the secondary is how a caller names a pose rather than counting them. It answers only for exactly two such rotaries, so a robot wrist gets nothing. tests/kins-params checks the pair every module reports. kinematicsUserInitString() takes [KINS] KINEMATICS as the HAL file hands it to loadrt. --- include/kinematics.h | 24 +++ include/kinematics_user.h | 65 +++++++ src/emc/kinematics/kins_util.c | 126 +++++++++++++ .../kinematics_userspace/kinematics_user.c | 178 ++++++++++++++++++ tests/kins-params/check.py | 43 ++++- tests/kins-params/test.sh | 46 +++-- tests/tool-frame/toolframecheck.c | 19 ++ 7 files changed, 483 insertions(+), 18 deletions(-) diff --git a/include/kinematics.h b/include/kinematics.h index 4d6871ddc1e..c89d7e4ffc3 100644 --- a/include/kinematics.h +++ b/include/kinematics.h @@ -373,6 +373,30 @@ typedef int (*kinsFrameFunc)(const double *joint, PmRotationMatrix *rot, const KINEMATICS_FORWARD_FLAGS *fflags); +/* Which joints turn the work: a bit per joint whose motion changes the + work frame at the seed. This is what a caller needs to hold the table + still while the head orients the tool (Heidenhain COORD ROT), or to let + it take part (TABLE ROT), without a config entry naming it. Returns 0 + or -1 if the frame cannot be evaluated. */ +extern int toolFrameWorkJoints(kinsFrameFunc work, int num_joints, + const double *seed, unsigned int *mask); + +/* The two rotaries that orient the tool, told apart. One has its axis + fixed in the machine frame, the primary, and the other has its axis + carried by the first, the secondary. The two poses that reach one tool + direction differ in the sign of the secondary, which is what a caller + needs to name a pose rather than count them, Heidenhain's SEQ+ and SEQ-. + + Both are found from the module's own tool frame, by turning each joint a + little and reading the axis of the rotation that results, so a module + declares nothing and a switchkins type that turns nothing answers -1. + Returns 0 with both joints set, or -1 where the machine has any number + of orienting rotaries but two, a robot wrist among them, or where the + frame cannot be evaluated. */ +extern int toolFrameOrientJoints(kinsFrameFunc tool, int num_joints, + const double *seed, + int *primary, int *secondary); + extern int toolFrameSolve(kinsFrameFunc work, kinsFrameFunc tool, int num_joints, diff --git a/include/kinematics_user.h b/include/kinematics_user.h index 739df8fc41d..0920a1c0350 100644 --- a/include/kinematics_user.h +++ b/include/kinematics_user.h @@ -203,6 +203,71 @@ int kinematicsUserRefreshParams(KinematicsUserContext* ctx); */ int kinematicsUserIsRtOnly(KinematicsUserContext* ctx); +/** + * The frames, as the module reports them: the work frame and the tool + * frame at a joint set, each against the machine (see kinematics.h). + * + * @return 0, or -1 if the module supplies no frame for the selected type + */ +int kinematicsUserWorkFrame(KinematicsUserContext* ctx, const double* joints, + PmRotationMatrix* rot); +int kinematicsUserToolFrame(KinematicsUserContext* ctx, const double* joints, + PmRotationMatrix* rot); + +/** + * The tool frame inverse of kinematics.h, on the loaded module and the + * selected type: the joint sets that point the tool axis, and where given + * the tool x, along the directions asked for, in work coordinates. Same + * arguments and answers as kinematicsToolFrameInverse(). + */ +int kinematicsUserToolFrameInverse(KinematicsUserContext* ctx, + const PmCartesian* axis_in_work, + const PmCartesian* x_in_work, + const double* seed, + unsigned int held, + double* solutions, + int max_solutions, + int* free_directions, + double* tool_spin); + +/** + * Which joints turn the work at the seed, a bit per joint; what a caller + * passes as held to keep the table still. See toolFrameWorkJoints(). + */ +int kinematicsUserWorkJoints(KinematicsUserContext* ctx, const double* seed, + unsigned int* mask); + +/** + * The two rotaries that orient the tool, primary and secondary, told apart + * by which one carries the other's axis. The sign of the secondary names + * the pose a five axis machine reaches a tool direction in. Returns 0, or + * -1 where the machine has any number of orienting rotaries but two. + * See toolFrameOrientJoints(). + */ +int kinematicsUserOrientJoints(KinematicsUserContext* ctx, const double* seed, + int* primary, int* secondary); + +/** + * The rotary whose sign tells apart the two poses that reach one tool + * direction, Heidenhain's master axis for SEQ+ and SEQ-: the first rotary + * met going from the tool to the work. Two heads, the secondary, with or + * without a table behind them; two tables, the primary, the one carrying + * the other; one head and a table, the head. Returns 0 with the joint, or + * -1 where the machine has any other number or mix of orienting rotaries. + */ +int kinematicsUserPoseJoint(KinematicsUserContext* ctx, const double* seed, + int* joint); + +/** + * kinematicsUserInitSparm() from the value of [KINS] KINEMATICS as the + * HAL file hands it to loadrt: the module name first, then any of + * coordinates=, sparm= and kinstype=, in any order. + */ +KinematicsUserContext* kinematicsUserInitString(const char* kinematics, + int num_joints, + int comp_id, + const char* prefix); + /** * Free kinematics context * diff --git a/src/emc/kinematics/kins_util.c b/src/emc/kinematics/kins_util.c index 9dc51889fb2..66df10d35e7 100644 --- a/src/emc/kinematics/kins_util.c +++ b/src/emc/kinematics/kins_util.c @@ -583,6 +583,9 @@ int identityKinematicsToolFrame(const double *joints, #define TFS_ITERS 60 #define TFS_FD_STEP 1e-6 // internal radians #define TFS_MOVED_TOL 1e-9 // frame difference that counts as movement +#define TFS_PROBE_STEP 0.05 // joint units, to read a joint's own axis +#define TFS_CARRY_STEP 40.0 // joint units, far enough to swing a carried axis +#define TFS_CARRY_TOL 1e-6 // axes closer than this counted as the same #define TFS_RANK_TOL 1e-4 // a direction worth less than this is free #define TFS_SOLVED 1e-18 // sum of squared residuals #define TFS_STEP_LIMIT 0.4 // internal radians per iteration @@ -991,6 +994,127 @@ static int tfs_spin(tfs_ctx *c, const double *joint, return 0; } +// A rotation's nine entries as one array, columns of axes: element +// (row, col) is c[3*col + row]. +static void tfs_entries(const PmRotationMatrix *m, double c[9]) +{ + c[0] = m->x.x; c[1] = m->x.y; c[2] = m->x.z; + c[3] = m->y.x; c[4] = m->y.y; c[5] = m->y.z; + c[6] = m->z.x; c[7] = m->z.y; c[8] = m->z.z; +} + +int toolFrameWorkJoints(kinsFrameFunc work, int num_joints, + const double *seed, unsigned int *mask) +{ + KINEMATICS_FORWARD_FLAGS fflags = 0; + PmRotationMatrix base, moved; + double joint[EMCMOT_MAX_JOINTS]; + int i, j; + + if (!work || !seed || !mask || num_joints <= 0 || num_joints > EMCMOT_MAX_JOINTS) { + return -1; + } + *mask = 0; + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { joint[i] = (i < num_joints) ? seed[i] : 0; } + if (work(joint, &base, &fflags)) { return -1; } + + // a step of one joint unit: a degree on every module in the tree, and + // a linear joint never turns a frame whatever its unit + for (j = 0; j < num_joints; j++) { + double diff = 0, a[9], b[9]; + + joint[j] = seed[j] + 1.0; + if (work(joint, &moved, &fflags)) { return -1; } + joint[j] = seed[j]; + tfs_entries(&base, a); + tfs_entries(&moved, b); + for (i = 0; i < 9; i++) { diff += fabs(a[i] - b[i]); } + if (diff > TFS_MOVED_TOL) { *mask |= 1u << j; } + } + return 0; +} + +// The axis a joint turns the tool frame about, in machine coordinates: move +// the joint a little and read the rotation that took the frame there. +// Returns 0 and a unit axis where the joint turns the tool, 1 where it does +// not, which is every linear joint and every joint the module ignores. +static int tfs_joint_axis(kinsFrameFunc tool, const double *joint, + int j, double step, double axis[3]) +{ + KINEMATICS_FORWARD_FLAGS fflags = 0; + PmRotationMatrix r1, r2; + double moved[EMCMOT_MAX_JOINTS]; + double a[9], b[9]; + double len; + int i; + + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { moved[i] = joint[i]; } + moved[j] += step; + if (tool(joint, &r1, &fflags)) { return -1; } + if (tool(moved, &r2, &fflags)) { return -1; } + + // The rotation from one frame to the other is r2 * transpose(r1), and + // the axis of a small rotation is the skew part of it. Both matrices + // are columns of axes, so element (row, col) is entry 3*col + row. + tfs_entries(&r1, a); + tfs_entries(&r2, b); + axis[0] = axis[1] = axis[2] = 0; + for (i = 0; i < 3; i++) { + // m[2][1] - m[1][2], m[0][2] - m[2][0], m[1][0] - m[0][1] + axis[0] += b[3*i + 2] * a[3*i + 1] - b[3*i + 1] * a[3*i + 2]; + axis[1] += b[3*i + 0] * a[3*i + 2] - b[3*i + 2] * a[3*i + 0]; + axis[2] += b[3*i + 1] * a[3*i + 0] - b[3*i + 0] * a[3*i + 1]; + } + len = sqrt(axis[0]*axis[0] + axis[1]*axis[1] + axis[2]*axis[2]); + if (len < 1e-9) { return 1; } + for (i = 0; i < 3; i++) { axis[i] /= len; } + return 0; +} + +int toolFrameOrientJoints(kinsFrameFunc tool, int num_joints, + const double *seed, int *primary, int *secondary) +{ + double joint[EMCMOT_MAX_JOINTS], elsewhere[EMCMOT_MAX_JOINTS]; + double axis[3], turned[3]; + int turns[2], count = 0, carried = -1; + int i, j, k, r; + + if (!tool || !seed || !primary || !secondary + || num_joints <= 0 || num_joints > EMCMOT_MAX_JOINTS) { + return -1; + } + *primary = *secondary = -1; + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { joint[i] = (i < num_joints) ? seed[i] : 0; } + + for (j = 0; j < num_joints; j++) { + r = tfs_joint_axis(tool, joint, j, TFS_PROBE_STEP, axis); + if (r < 0) { return -1; } + if (r > 0) { continue; } + if (count >= 2) { return -1; } // a wrist, not a head + turns[count++] = j; + } + if (count != 2) { return -1; } + + // whichever axis swings when the other joint moves is the carried one + for (i = 0; i < 2; i++) { + j = turns[i]; + k = turns[1 - i]; + if (tfs_joint_axis(tool, joint, j, TFS_PROBE_STEP, axis) != 0) { return -1; } + for (r = 0; r < EMCMOT_MAX_JOINTS; r++) { elsewhere[r] = joint[r]; } + elsewhere[k] += TFS_CARRY_STEP; + if (tfs_joint_axis(tool, elsewhere, j, TFS_PROBE_STEP, turned) != 0) { return -1; } + if (fabs(axis[0]*turned[0] + axis[1]*turned[1] + axis[2]*turned[2] - 1.0) + > TFS_CARRY_TOL) { + if (carried >= 0) { return -1; } // both carried: not a head + carried = j; + } + } + if (carried < 0) { return -1; } + *secondary = carried; + *primary = (turns[0] == carried) ? turns[1] : turns[0]; + return 0; +} + int toolFrameSolve(kinsFrameFunc work, kinsFrameFunc tool, int num_joints, @@ -1787,6 +1911,8 @@ EXPORT_SYMBOL(kinsJacobianFromMappedAxes); EXPORT_SYMBOL(kinsJacobianFromDhArm); EXPORT_SYMBOL(TOOL_FRAME_SPINDLE); EXPORT_SYMBOL(TOOL_FRAME_FLANGE); +EXPORT_SYMBOL(toolFrameWorkJoints); +EXPORT_SYMBOL(toolFrameOrientJoints); EXPORT_SYMBOL(KINS_IDENTITY_OPS); EXPORT_SYMBOL(kinsIdentityForward); EXPORT_SYMBOL(kinsIdentityInverse); diff --git a/src/emc/kinematics_userspace/kinematics_user.c b/src/emc/kinematics_userspace/kinematics_user.c index 34e31d09348..e47de9b1320 100644 --- a/src/emc/kinematics_userspace/kinematics_user.c +++ b/src/emc/kinematics_userspace/kinematics_user.c @@ -680,6 +680,184 @@ int kinematicsUserIsRtOnly(KinematicsUserContext* ctx) return ctx->rt_only; } +/* ======================================================================== + * Frames and the tool frame inverse + * + * toolFrameSolve() drives a pair of frame functions that take joints alone, + * the shape the RT modules export; the block form takes the parameters as + * well. The context the solver is running for is parked in a file static + * for the duration of the call, which is fine for the single threaded + * callers this has (the interpreter, a planner), and would not be for two + * threads solving at once. + * ======================================================================== */ + +static KinematicsUserContext *frame_ctx; + +static int frame_work(const double *joint, PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + KinematicsUserContext *ctx = frame_ctx; + return kinsOpsWorkFrame(ctx->info.ops[ctx->ktype], &ctx->params, joint, rot, fflags); +} + +static int frame_tool(const double *joint, PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags) +{ + KinematicsUserContext *ctx = frame_ctx; + return kinsOpsToolFrame(ctx->info.ops[ctx->ktype], &ctx->params, joint, rot, fflags); +} + +static void pad_joints(KinematicsUserContext *ctx, const double *in, double *out) +{ + int i; + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { + out[i] = (i < ctx->num_joints) ? in[i] : 0.0; + } +} + +int kinematicsUserWorkFrame(KinematicsUserContext* ctx, const double* joints, + PmRotationMatrix* rot) +{ + KINEMATICS_FORWARD_FLAGS fflags = 0; + double j[EMCMOT_MAX_JOINTS]; + + if (!ctx || !ctx->initialized || ctx->rt_only || !joints || !rot) return -1; + refresh(ctx); + pad_joints(ctx, joints, j); + return kinsOpsWorkFrame(ctx->info.ops[ctx->ktype], &ctx->params, j, rot, &fflags); +} + +int kinematicsUserToolFrame(KinematicsUserContext* ctx, const double* joints, + PmRotationMatrix* rot) +{ + KINEMATICS_FORWARD_FLAGS fflags = 0; + double j[EMCMOT_MAX_JOINTS]; + + if (!ctx || !ctx->initialized || ctx->rt_only || !joints || !rot) return -1; + refresh(ctx); + pad_joints(ctx, joints, j); + return kinsOpsToolFrame(ctx->info.ops[ctx->ktype], &ctx->params, j, rot, &fflags); +} + +int kinematicsUserToolFrameInverse(KinematicsUserContext* ctx, + const PmCartesian* axis_in_work, + const PmCartesian* x_in_work, + const double* seed, + unsigned int held, + double* solutions, + int max_solutions, + int* free_directions, + double* tool_spin) +{ + double j[EMCMOT_MAX_JOINTS]; + int found; + + if (!ctx || !ctx->initialized || ctx->rt_only || !seed) return -1; + if (!ctx->info.ops[ctx->ktype]->work || !ctx->info.ops[ctx->ktype]->tool) return -1; + refresh(ctx); + pad_joints(ctx, seed, j); + frame_ctx = ctx; + found = toolFrameSolve(frame_work, frame_tool, ctx->num_joints, + axis_in_work, x_in_work, j, held, + solutions, max_solutions, free_directions, tool_spin); + frame_ctx = NULL; + return found; +} + +int kinematicsUserWorkJoints(KinematicsUserContext* ctx, const double* seed, + unsigned int* mask) +{ + double j[EMCMOT_MAX_JOINTS]; + int r; + + if (!ctx || !ctx->initialized || ctx->rt_only || !seed || !mask) return -1; + if (!ctx->info.ops[ctx->ktype]->work) return -1; + refresh(ctx); + pad_joints(ctx, seed, j); + frame_ctx = ctx; + r = toolFrameWorkJoints(frame_work, ctx->num_joints, j, mask); + frame_ctx = NULL; + return r; +} + +int kinematicsUserOrientJoints(KinematicsUserContext* ctx, const double* seed, + int* primary, int* secondary) +{ + double j[EMCMOT_MAX_JOINTS]; + int r; + + if (!ctx || !ctx->initialized || ctx->rt_only || !seed || !primary || !secondary) return -1; + if (!ctx->info.ops[ctx->ktype]->tool) return -1; + refresh(ctx); + pad_joints(ctx, seed, j); + frame_ctx = ctx; + r = toolFrameOrientJoints(frame_tool, ctx->num_joints, j, primary, secondary); + frame_ctx = NULL; + return r; +} + +int kinematicsUserPoseJoint(KinematicsUserContext* ctx, const double* seed, + int* joint) +{ + double j[EMCMOT_MAX_JOINTS]; + unsigned int table_mask = 0, head_mask = 0; + int primary = -1, secondary = -1, r = -1; + + if (!ctx || !ctx->initialized || ctx->rt_only || !seed || !joint) return -1; + *joint = -1; + if (!ctx->info.ops[ctx->ktype]->tool || !ctx->info.ops[ctx->ktype]->work) return -1; + refresh(ctx); + pad_joints(ctx, seed, j); + frame_ctx = ctx; + if (toolFrameWorkJoints(frame_work, ctx->num_joints, j, &table_mask) == 0 + && toolFrameWorkJoints(frame_tool, ctx->num_joints, j, &head_mask) == 0 + && !(table_mask & head_mask)) { + int tables = __builtin_popcount(table_mask), heads = __builtin_popcount(head_mask); + // the first rotary met going from the tool to the work; a table + // behind two heads is the one Q0 holds still + if (heads == 2) { + r = toolFrameOrientJoints(frame_tool, ctx->num_joints, j, &primary, &secondary); + *joint = secondary; + } else if (heads == 1 && tables > 0) { + *joint = __builtin_ctz(head_mask); + r = 0; + } else if (heads == 0 && tables == 2) { + r = toolFrameOrientJoints(frame_work, ctx->num_joints, j, &primary, &secondary); + *joint = primary; + } + } + frame_ctx = NULL; + if (r != 0) { *joint = -1; } + return r; +} + +KinematicsUserContext* kinematicsUserInitString(const char* kinematics, + int num_joints, + int comp_id, + const char* prefix) +{ + char buf[256], *tok, *save = NULL; + char module[64] = "", coords[64] = "", sparm[64] = ""; + + if (!kinematics) return NULL; + snprintf(buf, sizeof(buf), "%s", kinematics); + for (tok = strtok_r(buf, " \t", &save); tok; tok = strtok_r(NULL, " \t", &save)) { + if (!module[0]) { + snprintf(module, sizeof(module), "%s", tok); + } else if (!strncmp(tok, "coordinates=", 12)) { + snprintf(coords, sizeof(coords), "%s", tok + 12); + } else if (!strncmp(tok, "sparm=", 6)) { + snprintf(sparm, sizeof(sparm), "%s", tok + 6); + } + /* kinstype= and anything else is the RT loader's business */ + } + if (!module[0]) return NULL; + return kinematicsUserInitSparm(module, num_joints, + coords[0] ? coords : NULL, + sparm[0] ? sparm : NULL, + comp_id, prefix); +} + void kinematicsUserFree(KinematicsUserContext* ctx) { int i; diff --git a/tests/kins-params/check.py b/tests/kins-params/check.py index cf7529e2506..ca2b15df9a9 100755 --- a/tests/kins-params/check.py +++ b/tests/kins-params/check.py @@ -9,7 +9,7 @@ # match what paritycheck published, to rounding, or the module is not # the pure function of its parameters it claims to be. # -# Usage: check.py MODULE JOINTS COORDS KTYPE FROMPOSE POSE JNT SPARM +# Usage: check.py MODULE JOINTS COORDS KTYPE FROMPOSE POSE JNT SPARM ORIENT POSEJOINT # POSE and JNT are comma separated numbers, as given to paritycheck; # COORDS and SPARM are a dash when the module was loaded without them. @@ -43,6 +43,10 @@ class EmcPose(ctypes.Structure): if coords == "-": coords = "" sparm = sys.argv[8].encode() if len(sys.argv) > 8 and sys.argv[8] not in ("", "-") else None +# the orientation joints this machine is known to have, "primary,secondary"; +# "-" where it has no such pair, "no-frame" where it reports no tool frame +orient_in = sys.argv[9] if len(sys.argv) > 9 else "no-frame" +posejoint_in = sys.argv[10] if len(sys.argv) > 10 else "no-frame" pose_in += [0.0] * (AXES - len(pose_in)) jnt_in += [0.0] * (MAX_JOINTS - len(jnt_in)) @@ -158,6 +162,43 @@ def compare(what, ours, theirs): for j in range(joints): compare("inverse joint %d after the tool is handed back" % j, qt[j], qi[j]) +# the two rotaries that orient the tool, told apart by which carries the +# other. The sign of the secondary is what G53.1 P names, so a module that +# gets this wrong sends the machine to the other pose without saying so. +kins.kinematicsUserOrientJoints.argtypes = [ctypes.c_void_p, Joints, + ctypes.POINTER(ctypes.c_int), + ctypes.POINTER(ctypes.c_int)] +primary, secondary = ctypes.c_int(-1), ctypes.c_int(-1) +r_orient = kins.kinematicsUserOrientJoints(ctx, Joints(*jnt_in), + ctypes.byref(primary), ctypes.byref(secondary)) +class Rot(ctypes.Structure): + _fields_ = [(n, ctypes.c_double * 3) for n in "xyz"] +kins.kinematicsUserToolFrame.argtypes = [ctypes.c_void_p, Joints, ctypes.POINTER(Rot)] +frame = Rot() +has_frame = kins.kinematicsUserToolFrame(ctx, Joints(*jnt_in), ctypes.byref(frame)) == 0 +if r_orient == 0: + got = "%d,%d" % (primary.value, secondary.value) +else: + got = "-" if has_frame else "no-frame" +if got != orient_in: + fail("orientation joints are %s, expected %s" % (got, orient_in)) +else: + print("kins-params: %s type %d orientation joints %s" % (module, ktype, got)) + +# the rotary whose sign G53.1 P1 and P2 name, the first met going from the +# tool to the work: the secondary of two heads, the carrier of two tables, +# the head of one of each +kins.kinematicsUserPoseJoint.argtypes = [ctypes.c_void_p, Joints, ctypes.POINTER(ctypes.c_int)] +pose_joint = ctypes.c_int(-1) +if kins.kinematicsUserPoseJoint(ctx, Joints(*jnt_in), ctypes.byref(pose_joint)) == 0: + got = "%d" % pose_joint.value +else: + got = "-" if has_frame else "no-frame" +if got != posejoint_in: + fail("the joint naming the pose is %s, expected %s" % (got, posejoint_in)) +else: + print("kins-params: %s type %d pose joint %s" % (module, ktype, got)) + kins.kinematicsUserFree(ctx) halc.hal_exit(comp_id) diff --git a/tests/kins-params/test.sh b/tests/kins-params/test.sh index 3a755b8f098..1e623690fa4 100755 --- a/tests/kins-params/test.sh +++ b/tests/kins-params/test.sh @@ -20,7 +20,7 @@ rm -f oot_switchkins.comp oot_userkins.comp # ONLY= in the environment runs the entries for that module alone run() { local loadrt="$1" setp="$2" parms="$3" ktype="$4" - local module coords sparm joints frompose pose jnt hal tok + local module coords sparm joints frompose pose jnt orient posejoint rtparms hal tok case "$loadrt" in "${ONLY:-}"*) ;; *) return 0 ;; esac module=${loadrt%% *} coords=""; sparm="" @@ -31,22 +31,34 @@ run() { esac done joints=3; frompose=0; pose="0,0,0,0,0,0,0,0,0"; jnt="10,20,30,40,50,60,70,80,90" + # the two rotaries that orient the tool, "primary,secondary", or "-" + # where the machine has no such pair and "no-frame" where the module + # reports no tool frame at all, which most of the tree still does + orient="no-frame" + # the rotary whose sign G53.1 P names, the same three ways; where the + # entry gives none, what it gives for the pair + posejoint="" for tok in $parms; do case "$tok" in joints=*) joints=${tok#joints=} ;; frompose=*) frompose=${tok#frompose=} ;; pose=*) pose=${tok#pose=} ;; jnt=*) jnt=${tok#jnt=} ;; + orient=*) orient=${tok#orient=} ;; + posejoint=*) posejoint=${tok#posejoint=} ;; esac done + [ -n "$posejoint" ] || posejoint=$orient + # what the module under test is loaded with: everything but our own words + rtparms=$(printf ' %s ' "$parms" | sed 's/ orient=[^ ]*//g; s/ posejoint=[^ ]*//g') hal=$(mktemp --suffix=.hal) { if [ -n "${CORE:-}" ]; then printf 'loadrt %s\n' "$CORE"; fi printf 'loadrt %s\n' "$loadrt" printf '%s\n' "$setp" - printf 'loadrt paritycheck %s ktype=%s\n' "$parms" "${ktype:-0}" + printf 'loadrt paritycheck %s ktype=%s\n' "$rtparms" "${ktype:-0}" # halcmd keeps quotes, so an absent value travels as a dash - printf 'loadusr -w python3 check.py %s %s %s %s %s %s %s %s\n' \ - "$module" "$joints" "${coords:--}" "${ktype:-0}" "$frompose" "$pose" "$jnt" "${sparm:--}" + printf 'loadusr -w python3 check.py %s %s %s %s %s %s %s %s %s %s\n' \ + "$module" "$joints" "${coords:--}" "${ktype:-0}" "$frompose" "$pose" "$jnt" "${sparm:--}" "$orient" "$posejoint" } > "$hal" echo "=== $loadrt type ${ktype:-0}" halrun -f "$hal" @@ -54,14 +66,14 @@ run() { } # identity, a gantry included -run "trivkins coordinates=XYZ" "" "joints=3 jnt=10,20,30" -run "trivkins coordinates=XYZY kinstype=BOTH" "" "joints=4 jnt=10,20,30,20" -run "trivkins coordinates=XYZABCUVW" "" "joints=9" +run "trivkins coordinates=XYZ" "" "joints=3 jnt=10,20,30 orient=-" +run "trivkins coordinates=XYZY kinstype=BOTH" "" "joints=4 jnt=10,20,30,20 orient=-" +run "trivkins coordinates=XYZABCUVW" "" "joints=9 orient=-" run "userkins" "" "joints=3 jnt=10,20,30" CORE=kins_single_core run "oot_userkins" "" "joints=3 jnt=10,20,30" -CORE=switchkins_core run "oot_switchkins" "" "joints=3 jnt=10,20,30" +CORE=switchkins_core run "oot_switchkins" "" "joints=3 jnt=10,20,30 orient=-" CORE=switchkins_core run "oot_switchkins" "" "joints=3 jnt=10,20,30" 1 -run "millturn" "" "joints=4 jnt=10,20,30,40" +run "millturn" "" "joints=4 jnt=10,20,30,40 orient=-" run "millturn" "" "joints=4 jnt=10,20,30,40" 1 # linear maps and one rotation @@ -82,8 +94,8 @@ run "maxkins" \ "setp maxkins.pivot-length 100" \ "joints=9 jnt=10,20,30,0,15,25,7,0,3" -run "5axiskins coordinates=XYZBCW" "" "joints=6 jnt=10,20,30,15,25,5" -run "5axiskins coordinates=XYZBCW sparm=identityfirst" "" "joints=6 jnt=10,20,30,15,25,5" 1 +run "5axiskins coordinates=XYZBCW" "" "joints=6 jnt=10,20,30,15,25,5 orient=4,3 posejoint=3" +run "5axiskins coordinates=XYZBCW sparm=identityfirst" "" "joints=6 jnt=10,20,30,15,25,5 orient=4,3 posejoint=3" 1 run "xyzac-trt-kins coordinates=XYZAC" \ "setp xyzac-trt-kins.y-offset 3 @@ -92,7 +104,7 @@ setp xyzac-trt-kins.tool-offset 7 setp xyzac-trt-kins.x-rot-point 1 setp xyzac-trt-kins.y-rot-point 2 setp xyzac-trt-kins.z-rot-point 5" \ - "joints=5 jnt=10,20,30,15,25" + "joints=5 jnt=10,20,30,15,25 orient=- posejoint=3" run "xyzbc-trt-kins coordinates=XYZBC" \ "setp xyzbc-trt-kins.conventional-directions 1 @@ -102,7 +114,7 @@ setp xyzbc-trt-kins.tool-offset 7 setp xyzbc-trt-kins.x-rot-point 1 setp xyzbc-trt-kins.y-rot-point 2 setp xyzbc-trt-kins.z-rot-point 5" \ - "joints=5 jnt=10,20,30,15,25" + "joints=5 jnt=10,20,30,15,25 orient=- posejoint=3" run "xyzab_tdr_kins" \ "setp xyzab_tdr_kins.x-offset 3 @@ -125,7 +137,7 @@ setp xyzacb_trsrn_kins.tool-offset-z 50 setp xyzacb_trsrn_kins.pre-rot 0.3 setp xyzacb_trsrn_kins.primary-angle 20 setp xyzacb_trsrn_kins.secondary-angle 35" \ - "joints=6 jnt=10,20,30,15,25,35" 1 + "joints=6 jnt=10,20,30,15,25,35 orient=5,4 posejoint=4" 1 run "xyzacb_trsrn" \ "setp xyzacb_trsrn_kins.nut-angle 45 @@ -134,7 +146,7 @@ setp xyzacb_trsrn_kins.z-pivot 200 setp xyzacb_trsrn_kins.pre-rot 0.3 setp xyzacb_trsrn_kins.primary-angle 20 setp xyzacb_trsrn_kins.secondary-angle 35" \ - "joints=6 jnt=10,20,30,15,25,35" 2 + "joints=6 jnt=10,20,30,15,25,35 orient=-" 2 run "xyzbca_trsrn" \ "setp xyzbca_trsrn_kins.nut-angle 45 @@ -148,7 +160,7 @@ setp xyzbca_trsrn_kins.tool-offset-z 50 setp xyzbca_trsrn_kins.pre-rot 0.3 setp xyzbca_trsrn_kins.primary-angle 20 setp xyzbca_trsrn_kins.secondary-angle 35" \ - "joints=6 jnt=10,20,30,15,25,35" 1 + "joints=6 jnt=10,20,30,15,25,35 orient=5,3 posejoint=3" 1 # polar run "rosekins" "" "joints=3 jnt=10,5,30" @@ -156,7 +168,7 @@ run "rosekins" "" "joints=3 jnt=10,5,30" # arms run "scarakins" "" "joints=6 jnt=30,40,20,10,0,0" run "scorbot-kins" "" "joints=5 jnt=40,60,-20,0,0" -run "pumakins" "setp pumakins.D6 50" "joints=6 jnt=15,20,-35,10,70,20" +run "pumakins" "setp pumakins.D6 50" "joints=6 jnt=15,20,-35,10,70,20 orient=-" run "three21kins" "" "joints=6 jnt=15,20,-35,10,70,20" run "genserkins" "" "joints=9 jnt=15,20,-35,10,70,20,0,0,0" run "genserkins" "setp genserkins.unrotate-3 1" "joints=9 jnt=15,20,-35,10,70,20,0,0,0" diff --git a/tests/tool-frame/toolframecheck.c b/tests/tool-frame/toolframecheck.c index ceab3acee12..8896d737534 100644 --- a/tests/tool-frame/toolframecheck.c +++ b/tests/tool-frame/toolframecheck.c @@ -237,10 +237,29 @@ static int holds(const double *sols, int count, int njoints, return 0; } +/* the joints that turn the work, read off the work frame rather than + declared, so that a caller can hold the table without naming it */ +static void test_work_joints(void) +{ + const double seed[6] = {0, 0, 0, 10, 20, 30}; + unsigned int mask = 99; + + check(toolFrameWorkJoints(xyzacWork, 5, seed, &mask) == 0 && mask == ((1u << 3) | (1u << 4)), + "xyzac: both rotaries carry the work"); + check(toolFrameWorkJoints(identityFrame, 5, seed, &mask) == 0 && mask == 0, + "a head machine: nothing turns the work"); + check(toolFrameWorkJoints(mixedWork, 6, seed, &mask) == 0 && mask == (1u << 3), + "table and head: the table joint alone"); + check(toolFrameWorkJoints(NULL, 5, seed, &mask) == -1, + "no frame function is refused"); +} + static void run_checks(void) { PmRotationMatrix m, r; + test_work_joints(); + /* the supplied constants are usable as declarations */ check(toolFrameIsProper(&TOOL_FRAME_SPINDLE), "TOOL_FRAME_SPINDLE is proper"); check(toolFrameIsProper(&TOOL_FRAME_FLANGE), "TOOL_FRAME_FLANGE is proper"); From f3af29496d0ec7c53c6441d7611ed6b244339b72 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Mon, 14 Sep 2026 16:48:22 +1000 Subject: [PATCH 17/19] kinematics_user: read the module's pins by name, making nothing in HAL The loader read the RT instance's pins through pins of its own, linked to the signal each RT pin was on, and where a pin had no signal it made one and linked the RT pin to it. That leaves the module's pins connected to signals the config never made, so a net of one of them after the module was first evaluated fails on a pin that is already connected. Read each pin by name with hal_get_p() instead, at every refresh: nothing is made in HAL, a pin netted later reads the signal from then on, and the loader no longer has to be initialised before the caller's hal_ready(). --- include/kinematics_user.h | 7 +- .../kinematics_userspace/kinematics_user.c | 199 +++++------------- 2 files changed, 59 insertions(+), 147 deletions(-) diff --git a/include/kinematics_user.h b/include/kinematics_user.h index 0920a1c0350..d2e7ea7cd77 100644 --- a/include/kinematics_user.h +++ b/include/kinematics_user.h @@ -49,15 +49,14 @@ typedef struct KinematicsUserContext KinematicsUserContext; /** * Initialize userspace kinematics context * - * The pins this creates belong to the caller's component, so call this - * after hal_init() and before hal_ready(): HAL refuses new pins once a - * component is ready. + * The module's pins are read through HAL, by name, so call this after + * hal_init(). Nothing is made in HAL: no pin, no signal. * * @param kins_type Kinematics module name (e.g., "trivkins", "5axiskins", "maxkins") * @param num_joints Number of joints in the machine * @param coordinates Coordinate string (e.g., "XYZABC", "XYZBCW") * @param comp_id Caller's HAL component, from hal_init() - * @param prefix Its name, which the created pin names start with + * @param prefix Its name * @return Allocated context, or NULL if kinematics type not supported */ KinematicsUserContext* kinematicsUserInit(const char* kins_type, diff --git a/src/emc/kinematics_userspace/kinematics_user.c b/src/emc/kinematics_userspace/kinematics_user.c index e47de9b1320..9ed7e401a99 100644 --- a/src/emc/kinematics_userspace/kinematics_user.c +++ b/src/emc/kinematics_userspace/kinematics_user.c @@ -4,9 +4,10 @@ * * Loads a kinematics .so with dlopen, asks it to describe itself through * kinsDescribe(), and evaluates its kinematics through the parameter - * block (see kinematics.h). The block is filled from HAL: one input pin - * of the caller's component per table entry, connected to the signal the - * RT instance's pin reads, so the values are the live ones. The tool is + * block (see kinematics.h). The block is filled from HAL: the RT + * instance's own pins, read by name whenever the block is refreshed, so + * the values are the live ones and nothing is made in HAL to get at them. + * The tool is * the caller's where it has given one, since a planner knows what a * segment runs under better than the machine does; otherwise it comes * from motion's own tooloffset pins where motion is loaded, so that the @@ -37,8 +38,7 @@ typedef int (*kins_describe_fn)(const char *coordinates, const char *sparm, kins_module_info *info); -#define MAX_BOUND_PINS (KINS_MAX_PARAMS + AXIS_COUNT) -#define MAX_MADE_SIGNALS MAX_BOUND_PINS +#define MAX_PINS (KINS_MAX_PARAMS + AXIS_COUNT) struct KinematicsUserContext { int initialized; @@ -52,14 +52,11 @@ struct KinematicsUserContext { int ktype; /* kinematics type being evaluated */ int num_joints; char module_name[64]; - int comp_id; /* the caller's component, owns the pins made here */ - const char *prefix; /* its name, which those pin names start with */ - char made_signal[MAX_MADE_SIGNALS][HAL_NAME_LEN + 1]; - int num_made_signals; - hal_refs_u *cell; /* HAL storage those pins are made against */ - int num_cells; - int cell_of_param[KINS_MAX_PARAMS]; /* -1 if not bound */ - int cell_of_tool[AXIS_COUNT]; /* motion.tooloffset.*, -1 if absent */ + int comp_id; /* the caller's component, which has HAL mapped */ + char pin_name[MAX_PINS][HAL_NAME_LEN + 1]; /* the RT instance's pins read here */ + int num_pins; + int pin_of_param[KINS_MAX_PARAMS]; /* -1 if not read */ + int pin_of_tool[AXIS_COUNT]; /* motion.tooloffset.*, -1 if absent */ int tool_param; /* the table's tool entry, -1 if none */ int warned_tool; EmcPose caller_tool; /* from kinematicsUserSetTool() */ @@ -68,55 +65,28 @@ struct KinematicsUserContext { }; /* ======================================================================== - * Pin binding + * Pin reading * ======================================================================== */ -/* - * Give the block a reference to a value it needs. - * - * The reference is to a pin of ours rather than into the RT instance's, - * so that its lifetime is ours. Ours is connected to the signal the RT - * pin reads, or, when the RT pin has no signal, to one made here and - * removed again in kinematicsUserFree(). - * - * The reference has to live in HAL shared memory, since that is where - * HAL rewrites it on connect and disconnect, so the pins are made - * against hal_malloc() cells and the block reads what a cell holds once - * the connection is in place. - */ -static int make_signal(KinematicsUserContext *ctx, const char *pin_name, - hal_type_t type, char *out, size_t outlen) -{ - if (ctx->num_made_signals >= MAX_MADE_SIGNALS) { - fprintf(stderr, "kinematicsUserInit: too many signals to create\n"); - return -1; - } - if ((size_t)snprintf(out, outlen, "%s-nonrt", pin_name) >= outlen) { - fprintf(stderr, "kinematicsUserInit: signal name for '%s' too long\n", - pin_name); - return -1; - } - if (hal_signal_new(out, type) != 0) return -1; - if (hal_link(pin_name, out) != 0) { - hal_signal_delete(out); - return -1; - } - snprintf(ctx->made_signal[ctx->num_made_signals++], - sizeof(ctx->made_signal[0]), "%s", out); - return 0; -} - -static int new_pin(int comp_id, hal_type_t type, hal_refs_u *out, - const char *name) +/* The block is read from the RT instance's own pins by name at every + refresh, so a pin netted later reads its signal; nothing is made in HAL + to get at them. */ +static int pin_value(const char *pin_name, hal_type_t type, double *out) { + hal_query_t q; + + memset(&q, 0, sizeof(q)); + q.name = pin_name; + q.qtype = HAL_QTYPE_PIN; + q.pp.type = type; + if (hal_get_p(&q, NULL, NULL) != 0) return -1; switch (type) { - case HAL_BOOL: return hal_pin_new_bool(comp_id, HAL_IN, &out->b, 0, "%s", name); - case HAL_REAL: return hal_pin_new_real(comp_id, HAL_IN, &out->r, 0.0, "%s", name); - case HAL_SINT: return hal_pin_new_si32(comp_id, HAL_IN, &out->s, 0, "%s", name); - case HAL_UINT: return hal_pin_new_ui32(comp_id, HAL_IN, &out->u, 0, "%s", name); - default: break; + case HAL_BOOL: *out = q.pp.value.b ? 1.0 : 0.0; break; + case HAL_SINT: *out = q.pp.value.s; break; + case HAL_UINT: *out = q.pp.value.u; break; + default: *out = q.pp.value.r; break; } - return -1; + return 0; } /* Does a pin of this name exist? Silent: absence is an answer, not an error. */ @@ -129,61 +99,24 @@ static int pin_exists(const char *pin_name) return hal_getref_p(&q) == 0; } -/* Bind pin_name; returns the cell index, or -1. */ -static int bind_pin(KinematicsUserContext *ctx, const char *pin_name, +/* Note pin_name for reading, once it has answered with the type; returns + its index, or -1. */ +static int note_pin(KinematicsUserContext *ctx, const char *pin_name, hal_type_t type) { - char signal[HAL_NAME_LEN + 1]; - char mine[HAL_NAME_LEN + 1]; - hal_refs_u *cell; - hal_query_t q; - int idx; - - memset(&q, 0, sizeof(q)); - q.name = pin_name; - q.qtype = HAL_QTYPE_PIN; + double value; - if (hal_getref_p(&q) != 0) { - fprintf(stderr, "kinematicsUserInit: no such pin '%s'\n", pin_name); - return -1; - } - if (q.pp.type != type) { - fprintf(stderr, "kinematicsUserInit: pin '%s' has the wrong type\n", + if (pin_value(pin_name, type, &value) != 0) { + fprintf(stderr, "kinematicsUserInit: no pin '%s' of the type expected\n", pin_name); return -1; } - - if (q.pp.signal) { - snprintf(signal, sizeof(signal), "%s", q.pp.signal); - } else if (make_signal(ctx, pin_name, type, signal, sizeof(signal))) { - fprintf(stderr, "kinematicsUserInit: cannot reach '%s'\n", pin_name); + if (ctx->num_pins >= MAX_PINS) { + fprintf(stderr, "kinematicsUserInit: too many pins to read\n"); return -1; } - - if ((size_t)snprintf(mine, sizeof(mine), "%s.%s", ctx->prefix, pin_name) - >= sizeof(mine)) { - fprintf(stderr, "kinematicsUserInit: pin name for '%s' too long\n", - pin_name); - return -1; - } - if (ctx->num_cells >= MAX_BOUND_PINS) { - fprintf(stderr, "kinematicsUserInit: too many pins to bind\n"); - return -1; - } - idx = ctx->num_cells; - cell = &ctx->cell[idx]; - - if (new_pin(ctx->comp_id, type, cell, mine) != 0) { - fprintf(stderr, "kinematicsUserInit: cannot create pin '%s'\n", mine); - return -1; - } - if (hal_link(mine, signal) != 0) { - fprintf(stderr, "kinematicsUserInit: cannot link '%s' to '%s'\n", - mine, signal); - return -1; - } - ctx->num_cells++; - return idx; + snprintf(ctx->pin_name[ctx->num_pins], sizeof(ctx->pin_name[0]), "%s", pin_name); + return ctx->num_pins++; } static hal_type_t hal_type_of(kins_param_type t) @@ -196,25 +129,15 @@ static hal_type_t hal_type_of(kins_param_type t) } } -static double cell_value(const hal_refs_u *cell, kins_param_type t) -{ - switch (t) { - case KINS_PARAM_BIT: return hal_get_bool(cell->b) ? 1.0 : 0.0; - case KINS_PARAM_S32: return hal_get_si32(cell->s); - case KINS_PARAM_U32: return hal_get_ui32(cell->u); - default: return hal_get_real(cell->r); - } -} - -/* Bind every input of the table, and motion's tool where motion is there. */ -static int bind_all(KinematicsUserContext *ctx) +/* Note every input of the table, and motion's tool where motion is there. */ +static int note_all(KinematicsUserContext *ctx) { static const char letter[AXIS_COUNT] = { 'x','y','z','a','b','c','u','v','w' }; char name[HAL_NAME_LEN + 1]; int i; - for (i = 0; i < KINS_MAX_PARAMS; i++) ctx->cell_of_param[i] = -1; - for (i = 0; i < AXIS_COUNT; i++) ctx->cell_of_tool[i] = -1; + for (i = 0; i < KINS_MAX_PARAMS; i++) ctx->pin_of_param[i] = -1; + for (i = 0; i < AXIS_COUNT; i++) ctx->pin_of_tool[i] = -1; ctx->tool_param = -1; for (i = 0; i < ctx->info.nparams; i++) { @@ -222,8 +145,8 @@ static int bind_all(KinematicsUserContext *ctx) if (d->dir == KINS_OUT) continue; if (d->tool) ctx->tool_param = i; snprintf(name, sizeof(name), "%s.%s", ctx->info.halprefix, d->name); - ctx->cell_of_param[i] = bind_pin(ctx, name, hal_type_of(d->type)); - if (ctx->cell_of_param[i] < 0) return -1; + ctx->pin_of_param[i] = note_pin(ctx, name, hal_type_of(d->type)); + if (ctx->pin_of_param[i] < 0) return -1; } /* motion publishes the tool it applies; take it from there when it is @@ -233,8 +156,8 @@ static int bind_all(KinematicsUserContext *ctx) for (i = 0; i < AXIS_COUNT; i++) { snprintf(name, sizeof(name), "motion.tooloffset.%c", letter[i]); if (!pin_exists(name)) continue; - ctx->cell_of_tool[i] = bind_pin(ctx, name, HAL_REAL); - if (ctx->cell_of_tool[i] < 0) return -1; + ctx->pin_of_tool[i] = note_pin(ctx, name, HAL_REAL); + if (ctx->pin_of_tool[i] < 0) return -1; } return 0; } @@ -247,10 +170,14 @@ static void refresh(KinematicsUserContext *ctx) double tool[AXIS_COUNT]; int have_motion_tool = 0; + /* a pin that stops answering, its module unloaded, keeps its last value */ for (i = 0; i < ctx->info.nparams; i++) { - int c = ctx->cell_of_param[i]; + int c = ctx->pin_of_param[i]; + double value; if (c < 0) continue; - ctx->params.geometry[i] = cell_value(&ctx->cell[c], ctx->info.params[i].type); + if (pin_value(ctx->pin_name[c], hal_type_of(ctx->info.params[i].type), &value) == 0) { + ctx->params.geometry[i] = value; + } } if (ctx->tool_param >= 0) { ctx->params.tool.tran.z = ctx->params.geometry[ctx->tool_param]; @@ -265,10 +192,10 @@ static void refresh(KinematicsUserContext *ctx) } for (i = 0; i < AXIS_COUNT; i++) { - int c = ctx->cell_of_tool[i]; + int c = ctx->pin_of_tool[i]; tool[i] = 0.0; if (c < 0) continue; - tool[i] = hal_get_real(ctx->cell[c].r); + if (pin_value(ctx->pin_name[c], HAL_REAL, &tool[i]) != 0) continue; have_motion_tool = 1; } if (!have_motion_tool) return; @@ -489,19 +416,12 @@ KinematicsUserContext* kinematicsUserInitSparm(const char* kins_type, ctx->num_joints = num_joints; ctx->comp_id = comp_id; - ctx->prefix = prefix; - - ctx->cell = (hal_refs_u *)hal_malloc(MAX_BOUND_PINS * sizeof(hal_refs_u)); - if (!ctx->cell) { - fprintf(stderr, "kinematicsUserInit: out of HAL memory\n"); - free(ctx); - return NULL; - } + (void)prefix; strncpy(ctx->module_name, kins_type, sizeof(ctx->module_name) - 1); if (load_module(ctx, kins_type, coordinates, sparm) == 0) { - if (bind_all(ctx) != 0) { - fprintf(stderr, "kinematicsUserInit: cannot bind the pins of '%s'\n", + if (note_all(ctx) != 0) { + fprintf(stderr, "kinematicsUserInit: cannot read the pins of '%s'\n", kins_type); ctx->rt_only = 1; } @@ -860,15 +780,8 @@ KinematicsUserContext* kinematicsUserInitString(const char* kinematics, void kinematicsUserFree(KinematicsUserContext* ctx) { - int i; - if (!ctx) return; - /* Removing one hands its value back to the RT pin, leaving the - machine as it was found. */ - for (i = 0; i < ctx->num_made_signals; i++) { - hal_signal_delete(ctx->made_signal[i]); - } unload_module(ctx); free(ctx); } From ad3d6d0a08b4ad2d0884dfc17298658a0a58f38c Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Fri, 11 Sep 2026 20:25:25 +0800 Subject: [PATCH 18/19] docs: single-type modules in tree link kins_single.c The note on kinematicsSwitchable() said the in-tree single-type modules use KINS_NOT_SWITCHABLE. They are on the parameter block now and get these entry points from kins_single.c. The macro stays for a module written without the block. --- docs/src/motion/kinematics.adoc | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/docs/src/motion/kinematics.adoc b/docs/src/motion/kinematics.adoc index 26f3e8a03d0..32b87d51a4a 100644 --- a/docs/src/motion/kinematics.adoc +++ b/docs/src/motion/kinematics.adoc @@ -320,9 +320,12 @@ See <>. [NOTE] The majority of provided kinematics modules support a single -kinematics type and use the directive "*KINS_NOT_SWITCHABLE*" to -supply defaults for the required kinematicsSwitchable() and -kinematicsSwitch() functions. +kinematics type; they are written on the parameter block and link +'kins_single.c', which supplies these entry points and answers "not +switchable" for them. A single-type module written without the +parameter block can use the directive "*KINS_NOT_SWITCHABLE*" to +supply defaults for kinematicsSwitchable(), kinematicsSwitch() and +kinematicsTypeFlags(). ---- int kinematicsHome(EmcPose *world, double *joint, From 10d8fa8e6a2fd080870e9e1f57bd8dbbae8996da Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Tue, 15 Sep 2026 22:07:11 +1000 Subject: [PATCH 19/19] kinematics.h: keep the interface, move the module side to kins_module.h kinematics.h carried three things: the entry points motion, task and the interpreter call on a module, the helpers a module is written with, and the prototypes of particular modules that share their maths between two loadable modules. A consumer that wanted the flag values or the switch entry point read all of it. kinematics.h now declares what a module presents to its callers: the kinematics type, the flags, forward, inverse, home, the tool and work frames, the tool frame inverse, the Jacobian, the switch, the type flags and the tool offset. kins_module.h, exported alongside it, takes the module side: the joint map and identity helpers, the tool frame library, the generic Jacobian, the parameter block and the shared code that runs an ops table. What trtfuncs.c and userkfuncs.c share with the modules that load them is in trtfuncs.h and userkfuncs.h next to them, in tree only. SWITCHKINS_MAX_TYPES is defined in kinematics.h itself, where motion and NML read it, and kins_module.h aliases KINS_MAX_TYPES to it. Nothing declared moves or changes; only which header declares it. --- docs/src/motion/switchkins.adoc | 4 +- include/kinematics.h | 455 +----------------- include/kinematics_user.h | 4 +- include/kins_module.h | 447 +++++++++++++++++ include/kins_rt.h | 2 +- include/switchkins.h | 11 +- src/emc/kinematics/5axiskins.c | 1 + src/emc/kinematics/corexykins.c | 2 +- src/emc/kinematics/genhexkins.c | 3 +- src/emc/kinematics/genhexkins.h | 2 +- src/emc/kinematics/genserfuncs.c | 4 +- src/emc/kinematics/genserkins.c | 3 +- src/emc/kinematics/genserkins.h | 4 +- src/emc/kinematics/kins_single.c | 2 +- src/emc/kinematics/kins_util.c | 2 +- src/emc/kinematics/lineardeltakins.c | 2 +- src/emc/kinematics/maxkins.c | 2 +- src/emc/kinematics/pentakins.c | 4 +- src/emc/kinematics/pumakins.c | 1 + src/emc/kinematics/rosekins.c | 2 +- src/emc/kinematics/rotarydeltakins.c | 2 +- src/emc/kinematics/rotatekins.c | 2 +- src/emc/kinematics/scarakins.c | 1 + src/emc/kinematics/scorbot-kins.c | 2 +- src/emc/kinematics/switchkins.c | 2 +- src/emc/kinematics/three21kins.c | 1 + src/emc/kinematics/tripodkins.c | 2 +- src/emc/kinematics/trivkins.c | 2 +- src/emc/kinematics/trtfuncs.c | 5 +- src/emc/kinematics/trtfuncs.h | 20 + src/emc/kinematics/userkfuncs.c | 5 +- src/emc/kinematics/userkfuncs.h | 30 ++ src/emc/kinematics/xyzac-trt-kins.c | 4 +- src/emc/kinematics/xyzbc-trt-kins.c | 4 +- .../kinematics_userspace/kinematics_user.c | 2 +- src/hal/components/matrixkins.comp | 2 +- src/hal/components/millturn.comp | 2 +- src/hal/components/switchkinscomp.comp | 4 +- src/hal/components/userkins.comp | 8 +- src/hal/components/xyzab_tdr_kins.comp | 2 +- src/hal/components/xyzacb_trsrn.comp | 2 +- src/hal/components/xyzbca_trsrn.comp | 2 +- tests/kins-params/paritycheck.c | 2 +- tests/tool-frame/toolframecheck.c | 2 +- 44 files changed, 562 insertions(+), 505 deletions(-) create mode 100644 include/kins_module.h create mode 100644 src/emc/kinematics/trtfuncs.h create mode 100644 src/emc/kinematics/userkfuncs.h diff --git a/docs/src/motion/switchkins.adoc b/docs/src/motion/switchkins.adoc index fb946f38bf8..edb93924ae2 100644 --- a/docs/src/motion/switchkins.adoc +++ b/docs/src/motion/switchkins.adoc @@ -560,7 +560,7 @@ int switchkinsRegister(int ktype, KS kset, KF kfwd, KI kinv); ---- 'ktype' runs from 0 to SWITCHKINS_MAX_TYPES-1 (defined in -kinematics.h as KINS_MAX_TYPES). Registering a kinstype twice, by +kinematics.h). Registering a kinstype twice, by either route, is an error, and so is leaving a gap below the highest kinstype provided. Either mistake fails the module load and says which kinstype is at fault. @@ -728,7 +728,7 @@ static int my_inverse(const kins_params *p, kins_scratch *s, static const kins_ops my_ops = { .forward = my_forward, .inverse = my_inverse, - // .work, .tool, .native and .jacobian are optional, see kinematics.h + // .work, .tool, .native and .jacobian are optional, see kins_module.h }; int switchkinsSetup(kparms* kp, diff --git a/include/kinematics.h b/include/kinematics.h index c89d7e4ffc3..f19a8dba1e0 100644 --- a/include/kinematics.h +++ b/include/kinematics.h @@ -108,11 +108,10 @@ extern int kinematicsHome(struct EmcPose * world, extern KINEMATICS_TYPE kinematicsType(void); /* Switchable kinematics: a module provides several kinematics, numbered -** 0..SWITCHKINS_MAX_TYPES-1, and motion runs one of them at a time. -** The count is here, not in switchkins.h, because motion and the NML -** status channel need it; it aliases KINS_MAX_TYPES below. +** 0..SWITCHKINS_MAX_TYPES-1, and motion runs one of them at a time. The +** count is here because motion and the NML status channel need it. */ -#define SWITCHKINS_MAX_TYPES KINS_MAX_TYPES +#define SWITCHKINS_MAX_TYPES 9 /* What a kinematics type IS, declared by the module with ** switchkinsDeclare() and read back with kinematicsTypeFlags(). @@ -195,100 +194,6 @@ extern int kinematicsWorkFrame(const double *joint, PmRotationMatrix *rot, const KINEMATICS_FORWARD_FLAGS *fflags); -/* parameters for use with switchkins.c */ -typedef struct kinematics_parms { - char* sparm; // module string parameter passed to kins - char* kinsname; // must agree with module(file) name - char* halprefix; // for hal pin hames - char* required_coordinates; - int max_joints; - int allow_duplicates; - int fwd_iterates_mask; // identify kins types that use iterative - // forward kinematics (typ: genhex) - // bitmask: 0x0 none - // bitmask: 0x1 bit0: switchkins_type==0 - // bitmask: 0x2 bit1: switchkins_type==1 - // bitmask: 0x4 bit2: switchkins_type==2 - int gui_kinstype; // may be reqd for parallel kins with vismach - // to select switchkins_type for gui pins - const struct kins_param_desc_tag *params; // geometry table, see below - int nparams; -} kparms; - -/* map letters in a coordinates string to joint numbers -** sequentially. Axis indices are 0:x,1:y,...,etc -** Example: coordinates=XYZYAC -** Result: axis_idx_for_jno[0] = 0 ==> X -** axis_idx_for_jno[1] = 1 ==> Y -** axis_idx_for_jno[2] = 2 ==> Z -** axis_idx_for_jno[3] = 1 ==> Y (duplicate allowed) -** axis_idx_for_jno[4] = 1 ==> A -** axis_idx_for_jno[5] = 1 ==> C -*/ -extern int map_coordinates_to_jnumbers(const char *coordinates, - const int max_joints, - const int allow_duplicates, - int axis_idx_for_jno[]); - -extern int mapped_joints_to_position(const int max_joints, - const double* joints, - EmcPose* pose); - -extern int position_to_mapped_joints(const int max_joints, - const EmcPose* pos, - double* joints); - -/* bitmap of joints assigned to an axis index (0:x,1:y,...,8:w) by the -** coordinates mapping; 0 when unused or not initialized */ -extern int identityKinematicsAxisJointsBitmap(int axis_num); - -extern int identityKinematicsSetup(const int comp_id, - const char* coordinates, - kparms* ksetup_parms); - -extern int identityKinematicsForward(const double *joint, - struct EmcPose * world, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags); - -extern int identityKinematicsInverse(const struct EmcPose * world, - double *joint, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags); - -/* joints are axes, so neither frame ever turns */ -extern int identityKinematicsToolFrame(const double *joint, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags); - -extern int identityKinematicsWorkFrame(const double *joint, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags); - -/* Rotations relating a module's own frame to the tool frame convention. - TOOL_FRAME_SPINDLE is the identity, for maths already in the convention. - TOOL_FRAME_FLANGE is the half turn about tool x that turns an ISO 9787 - flange frame, whose z points out of the mechanical interface towards the - work, into the convention. */ -extern const PmRotationMatrix TOOL_FRAME_SPINDLE; -extern const PmRotationMatrix TOOL_FRAME_FLANGE; - -/* Post-multiply a module's native frame by the rotation it declared, in - place. Modules built on switchkins.c never call this, the dispatch does it - for them; a standalone module calls it before returning. - Returns 0, or -1 if native is not a proper rotation. */ -extern int toolFrameApplyNative(PmRotationMatrix *rot, - const PmRotationMatrix *native); - -/* out = transpose(work) * tool, the tool frame in workpiece coordinates. - out may alias neither input. */ -extern int toolFrameInWork(const PmRotationMatrix *work, - const PmRotationMatrix *tool, - PmRotationMatrix *out); - -/* True if m is orthonormal with determinant +1, so a frame a machine can - actually hold. Used to check a declared rotation once, at load. */ -extern int toolFrameIsProper(const PmRotationMatrix *m); /* The inverse of kinematicsToolFrame(): which joint values point the tool along a requested direction. This is the question a tilted work plane asks @@ -363,52 +268,6 @@ extern int kinematicsToolFrameInverse(const PmCartesian *axis_in_work, int *free_directions, double *tool_spin); -/* The generic implementation of the above, driven by a module's own frame - functions, so that a module gets it for free once it supplies them. A - module with a closed form registers that instead: it is faster, and it - knows its own degenerate poses without having to find them. - - num_joints is the length of seed and of each row of solutions. */ -typedef int (*kinsFrameFunc)(const double *joint, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags); - -/* Which joints turn the work: a bit per joint whose motion changes the - work frame at the seed. This is what a caller needs to hold the table - still while the head orients the tool (Heidenhain COORD ROT), or to let - it take part (TABLE ROT), without a config entry naming it. Returns 0 - or -1 if the frame cannot be evaluated. */ -extern int toolFrameWorkJoints(kinsFrameFunc work, int num_joints, - const double *seed, unsigned int *mask); - -/* The two rotaries that orient the tool, told apart. One has its axis - fixed in the machine frame, the primary, and the other has its axis - carried by the first, the secondary. The two poses that reach one tool - direction differ in the sign of the secondary, which is what a caller - needs to name a pose rather than count them, Heidenhain's SEQ+ and SEQ-. - - Both are found from the module's own tool frame, by turning each joint a - little and reading the axis of the rotation that results, so a module - declares nothing and a switchkins type that turns nothing answers -1. - Returns 0 with both joints set, or -1 where the machine has any number - of orienting rotaries but two, a robot wrist among them, or where the - frame cannot be evaluated. */ -extern int toolFrameOrientJoints(kinsFrameFunc tool, int num_joints, - const double *seed, - int *primary, int *secondary); - -extern int toolFrameSolve(kinsFrameFunc work, - kinsFrameFunc tool, - int num_joints, - const PmCartesian *axis_in_work, - const PmCartesian *x_in_work, - const double *seed, - unsigned int held, - double *solutions, - int max_solutions, - int *free_directions, - double *tool_spin); - /* How each joint responds to a unit rate of each pose coordinate: jac[j][a] = d joint[j] / d pose[a] @@ -454,286 +313,6 @@ extern int kinematicsJacobian(const double *joint, double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], const KINEMATICS_INVERSE_FLAGS *iflags); -typedef int (*kinsInverseFunc)(const EmcPose *world, - double *joint, - const KINEMATICS_INVERSE_FLAGS *iflags, - KINEMATICS_FORWARD_FLAGS *fflags); - -/* The generic Jacobian, by central differences of an inverse about world: - two inverse calls per pose coordinate, eighteen in all, on the solution - branch iflags selects. The joint array handed to every call starts from - joint, so a module that reads its joint argument sees the machine where - it is. - - The answer is as good as the inverse: a closed form gives it to rounding, - an inverse that iterates to a tolerance gives it to that tolerance over - the step, and should supply its own. num_joints is the module's joint - count. Returns 0, or -1 if any inverse fails. */ -#define KINS_JACOBIAN_STEP 1e-3 /* pose units, either kind */ - -extern int kinsJacobianFromInverse(kinsInverseFunc inverse, - int num_joints, - const double *joint, - const EmcPose *world, - const KINEMATICS_INVERSE_FLAGS *iflags, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]); - -/* For a module whose inverse computes a position P and then hands it to - position_to_mapped_joints(): given dP[axis][pose], how each coordinate of - P responds to each pose coordinate, fill in jac so that every joint gets - the row of the letter it is mapped to. Duplicate letters get duplicate - rows, which is the gantry case. */ -extern int kinsJacobianFromMappedAxes(int max_joints, - const double dP[EMCMOT_MAX_AXIS][EMCMOT_MAX_AXIS], - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]); - -/* The Jacobian of a serial arm of six revolute joints from its - Denavit-Hartenberg chain. Two conventions carry that name and put the - four parameters on different links; this is the modified one of John J. - Craig, Introduction to Robotics: Mechanics and Control, where link i is - Rx(alpha[i]) Tx(a[i]) Rz(joint[i]) Tz(d[i]), the joint turning about the - z of the frame Rx and Tx leave it in. (The original 1955 convention is - Rz(theta) Tz(d) Tx(a) Rx(alpha), and a table written for it does not fit - here.) The tool point `tool` lies along the z of the last frame, and - the pose of that point is reported as X Y Z and the RPY of the last - frame, R = Rz(C) Ry(B) Rx(A), as pmMatRpyConvert() does. - Each joint's axis crossed with the vector from it to the tool point - gives the point's rate per radian of the joint, the axis itself the - angular rate; that 6x6 inverted is the joint rate per unit of twist, and - the RPY rates reach the twist through the matrix of the axes each one - turns about, which is where world's B and C come in. alpha and joint in - degrees, a, d and tool in the module's length unit. Rows 0 to 5 of jac - are filled, the rest zero. Returns 0, or -1 at a singular pose, where no - finite joint rate follows the pose. */ -extern int kinsJacobianFromDhArm(const double alpha[6], const double a[6], - const double d[6], const double *joint, - double tool, const EmcPose *world, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]); - -/* joints are axes: a 1 per joint in the column of its letter */ -extern int identityKinematicsJacobian(const double *joint, - const EmcPose *world, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS *iflags); - -/* ------------------------------------------------------------------------ - Kinematics as pure functions of what the caller passes in. - - Everything above reads its geometry from HAL pins the module created and - keeps its mode and scratch in statics, so it can only answer for the - machine as it is now, from inside the module. The forms below take the - same questions with the machine described by the caller: a parameter - block naming the kinematics type, the joint map, the tool and the - geometry, and a scratch block for what an iterative method carries - between calls. Nothing is read from HAL and nothing is kept, so one copy - of the maths serves motion, a planner evaluating poses the machine has - not reached, task checking a program at load, and a tool asking what if. - - A module declares its geometry as a table of named entries. In RT the - shared code makes one HAL pin per entry, with the names configs already - use, and copies the pins into the block before every call; outside RT the - caller fills the block from wherever it likes. The maths reads - p->geometry[i] where it read a pin. - - The existing entry points stay and are supplied once, by kins_single.c - for a module with one kinematics type and by switchkins.c for one with - several, so nothing that calls kinematicsForward() changes. A module - that does not provide these forms keeps working as it did; it just cannot - be evaluated outside RT. - ------------------------------------------------------------------------ */ - -#define KINS_MAX_PARAMS 96 /* genhexkins declares 84 */ -#define KINS_MAX_TYPES 9 /* kinematics types a module may provide */ - -typedef enum { - KINS_PARAM_FLOAT = 0, - KINS_PARAM_BIT, - KINS_PARAM_S32, - KINS_PARAM_U32 -} kins_param_type; - -typedef enum { - KINS_IN = 0, /* read into the block before a call */ - KINS_OUT, /* a result, written from kins_scratch.out[] after it */ - KINS_IO /* read like an input; the pin is HAL_IO so it can be poked */ -} kins_param_dir; - -/* One entry of a module's geometry table. name follows the module's HAL - prefix. An entry with tool set is the tool length along the tool axis: - the shared code puts its value in kins_params.tool.tran.z as well, which - is what the maths should read, so that a caller outside RT can supply - the tool from the tool table without there being a pin. */ -typedef struct kins_param_desc_tag { - const char *name; - kins_param_type type; - kins_param_dir dir; - int tool; - double dflt; -} kins_param_desc; - -/* The machine, as far as the kinematics is concerned. One copy may be - shared by any number of callers: nothing writes it during a call. */ -typedef struct kins_params { - int size; /* sizeof(kins_params) */ - int ktype; /* kinematics type, 0 if one */ - int max_joints; /* joints the map covers */ - int joint_of_axis[EMCMOT_MAX_AXIS]; /* principal joint per letter */ - int joints_of_axis[EMCMOT_MAX_AXIS]; /* bit per joint, duplicates */ - EmcPose tool; /* tool offset, tool.tran.z along the tool axis */ - double geometry[KINS_MAX_PARAMS]; /* the table, in its order */ -} kins_params; - -/* What one caller carries between its own calls: the last pose an - iterative forward found, which seeds the next, and what a module reports - about its last call. Never shared between callers. */ -typedef struct kins_scratch { - EmcPose pose_seed; /* start an iterative forward here */ - int have_pose_seed; - int pose_seed_ok; /* pose_seed came from a solve that succeeded */ - double joint_seed[EMCMOT_MAX_JOINTS]; /* start an iterative inverse here */ - int have_joint_seed; - int iterations; - int failed; - double aux[8]; /* whatever else a module carries between calls */ - double out[KINS_MAX_PARAMS]; /* the table's KINS_OUT entries */ -} kins_scratch; - -typedef int (*kins_forward_fn)(const kins_params *p, kins_scratch *s, - const double *joint, EmcPose *pos, - const KINEMATICS_FORWARD_FLAGS *fflags, - KINEMATICS_INVERSE_FLAGS *iflags); - -typedef int (*kins_inverse_fn)(const kins_params *p, kins_scratch *s, - const EmcPose *pos, double *joint, - const KINEMATICS_INVERSE_FLAGS *iflags, - KINEMATICS_FORWARD_FLAGS *fflags); - -typedef int (*kins_frame_fn)(const kins_params *p, const double *joint, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags); - -typedef int (*kins_jacobian_fn)(const kins_params *p, const double *joint, - const EmcPose *pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS *iflags); - -/* The maths of one kinematics type. forward and inverse are required; the - frames, the native rotation and the Jacobian are optional as before, and - a missing Jacobian is differenced from the inverse. fwd_iterates says the - forward starts from the pose it is handed, so the shared code seeds it - with the last answer after a switch. identity says joints are axes, which - a consumer may use to skip the maths altogether. primary says this is - the module's working transform, the type G43.4 switches to. machine says - this is the machine frame type, the pivot in machine coordinates with the - rotaries as joints, which G13.1 and G49 select and G53.5 moves in; a module - that leaves it unset on every type has its identity type stand in. */ -typedef struct kins_ops { - kins_forward_fn forward; - kins_inverse_fn inverse; - kins_frame_fn work; - kins_frame_fn tool; - const PmRotationMatrix *native; /* NULL means TOOL_FRAME_SPINDLE */ - kins_jacobian_fn jacobian; - int fwd_iterates; - int identity; /* joints are axes */ - int primary; /* the working transform */ - int machine; /* the machine frame */ -} kins_ops; - -/* A module described for a caller outside RT: its table, its joint - conventions and the maths of each type. ops[t] is NULL for a type the - module still implements the old way. */ -typedef struct kins_module_info { - const char *name; - const char *halprefix; - const kins_param_desc *params; - int nparams; - const char *required_coordinates; - int max_joints; /* the most the module allows */ - int allow_duplicates; - int ntypes; - const kins_ops *ops[KINS_MAX_TYPES]; -} kins_module_info; - -/* Exported by every module that provides the forms above. coordinates and - sparm are the module parameters the RT instance was loaded with; a module - whose types depend on them replays that choice here. Meant for a copy of - the module loaded outside RT; the RT instance answers from its own state - without redoing its setup. Returns 0, or -1 with info untouched. */ -extern int kinsDescribe(const char *coordinates, const char *sparm, - kins_module_info *info); - -/* Fill a block for a module: size, the joint map from coordinates (checked - against required_coordinates, the joint limit and the duplicate rule), - ktype 0, no tool, and every geometry entry at its table default. A - caller then overwrites what it knows better. Returns 0 or -1. */ -extern int kinsParamsInit(kins_params *p, - const kins_module_info *info, - const char *coordinates); - -/* The joint map alone, into a block, with no other field touched. */ -extern int kinsParamsMapCoordinates(kins_params *p, - const char *coordinates, - int max_joints, - int allow_duplicates, - const char *required_coordinates); - -/* Reset a scratch to "no seed, nothing reported". */ -extern void kinsScratchInit(kins_scratch *s); - -/* The map helpers above, reading the map from the block instead of from - the statics that map_coordinates_to_jnumbers() fills. */ -extern int kinsMappedJointsToPose(const kins_params *p, - const double *joints, EmcPose *pos); -extern int kinsPoseToMappedJoints(const kins_params *p, - const EmcPose *pos, double *joints); -extern int kinsJacobianFromMappedAxesP(const kins_params *p, - const double dP[EMCMOT_MAX_AXIS][EMCMOT_MAX_AXIS], - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]); - -/* Identity as pure functions: joints are axes through the block's map. */ -extern int kinsIdentityForward(const kins_params *p, kins_scratch *s, - const double *joint, EmcPose *pos, - const KINEMATICS_FORWARD_FLAGS *fflags, - KINEMATICS_INVERSE_FLAGS *iflags); -extern int kinsIdentityInverse(const kins_params *p, kins_scratch *s, - const EmcPose *pos, double *joint, - const KINEMATICS_INVERSE_FLAGS *iflags, - KINEMATICS_FORWARD_FLAGS *fflags); -extern int kinsIdentityFrame(const kins_params *p, const double *joint, - PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags); -extern int kinsIdentityJacobian(const kins_params *p, const double *joint, - const EmcPose *pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS *iflags); -extern const kins_ops KINS_IDENTITY_OPS; - -/* The five questions asked of an ops table, with the defaults applied: - identity for a missing frame, the native rotation applied to the tool - frame, and the Jacobian differenced from the inverse when there is no - closed form. These are what the RT wrappers and a caller outside RT - both go through, so both get the same answers. */ -extern int kinsOpsForward(const kins_ops *ops, const kins_params *p, - kins_scratch *s, const double *joint, EmcPose *pos, - const KINEMATICS_FORWARD_FLAGS *fflags, - KINEMATICS_INVERSE_FLAGS *iflags); -extern int kinsOpsInverse(const kins_ops *ops, const kins_params *p, - kins_scratch *s, const EmcPose *pos, double *joint, - const KINEMATICS_INVERSE_FLAGS *iflags, - KINEMATICS_FORWARD_FLAGS *fflags); -extern int kinsOpsWorkFrame(const kins_ops *ops, const kins_params *p, - const double *joint, PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags); -extern int kinsOpsToolFrame(const kins_ops *ops, const kins_params *p, - const double *joint, PmRotationMatrix *rot, - const KINEMATICS_FORWARD_FLAGS *fflags); -extern int kinsOpsJacobian(const kins_ops *ops, const kins_params *p, - kins_scratch *s, const double *joint, - const EmcPose *pos, - double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], - const KINEMATICS_INVERSE_FLAGS *iflags); - extern int kinematicsSwitchable(void); extern int kinematicsSwitch(int switchkins_type); @@ -745,9 +324,6 @@ extern int kinematicsSwitch(int switchkins_type); table through motion, and the module's tool pin, where it has one, is read only until motion has spoken. */ extern int kinematicsSetTool(const EmcPose *tool); -//NOTE: switchable kinematics may require Interp::Synch -// before/after invoking kinematicsSwitch() -// A convenient command to synch is: M66 E0 L0 #define KINS_NOT_SWITCHABLE \ extern int kinematicsSwitchable() {return 0;} \ @@ -757,31 +333,6 @@ EXPORT_SYMBOL(kinematicsSwitchable); \ EXPORT_SYMBOL(kinematicsSwitch); \ EXPORT_SYMBOL(kinematicsTypeFlags); - -// support for template for user-defined switchkins_type==2 -extern const kins_ops USERK_OPS; - -extern int userkKinematicsSetup(const int comp_id, - const char* coordinates, - kparms* ksetup_parms); - -extern int userkKinematicsForward(const double *joint, - struct EmcPose * world, - const KINEMATICS_FORWARD_FLAGS * fflags, - KINEMATICS_INVERSE_FLAGS * iflags); - -extern int userkKinematicsInverse(const struct EmcPose * world, - double *joint, - const KINEMATICS_INVERSE_FLAGS * iflags, - KINEMATICS_FORWARD_FLAGS * fflags); -//********************************************************************* -// xyzac,xyzbc (trtfuncs.c): one geometry table, the maths of each machine -extern const kins_param_desc TRT_PARAMS[]; -extern const int TRT_NPARAMS; -extern const kins_ops XYZAC_OPS; -extern const kins_ops XYZBC_OPS; - -//********************************************************************* #ifdef __cplusplus } #endif diff --git a/include/kinematics_user.h b/include/kinematics_user.h index d2e7ea7cd77..8ee1891c065 100644 --- a/include/kinematics_user.h +++ b/include/kinematics_user.h @@ -7,7 +7,7 @@ * positions from world coordinates without requiring RT kernel calls. * * The kinematics module is loaded into this process and evaluated through - * its parameter block form (see kinematics.h). The block is filled from + * its parameter block form (see kins_module.h). The block is filled from * input pins belonging to the caller's HAL component, connected to the * same signals the running RT instance reads, so the maths runs on live * values; the tool is the caller's where it gives one, and motion's @@ -23,7 +23,7 @@ #define KINEMATICS_USER_H #include "emcpos.h" /* EmcPose */ -#include "kinematics.h" /* KINEMATICS_TYPE, flags */ +#include "kins_module.h" /* KINEMATICS_TYPE, flags */ #include "hal.h" /* hal_type_t, HAL_NAME_LEN */ #ifdef __cplusplus diff --git a/include/kins_module.h b/include/kins_module.h new file mode 100644 index 00000000000..13dc8f62411 --- /dev/null +++ b/include/kins_module.h @@ -0,0 +1,447 @@ +/******************************************************************** +* Description: kins_module.h +* The module side of a kinematics module: what a module is written with, +* as opposed to kinematics.h, which is what motion and the interpreter +* call. The joint map and identity helpers, the tool frame library, the +* generic Jacobian, and the parameter block form of a module with the +* shared code that runs an ops table. Nothing here needs HAL; the pins +* made from a module's table are in kins_rt.h. +* +* License: GPL Version 2 +********************************************************************/ +#ifndef __LINUXCNC_KINS_MODULE_H +#define __LINUXCNC_KINS_MODULE_H + +#include "kinematics.h" + +#ifdef __cplusplus +extern "C" { +#endif + +/* parameters for use with switchkins.c */ +typedef struct kinematics_parms { + char* sparm; // module string parameter passed to kins + char* kinsname; // must agree with module(file) name + char* halprefix; // for hal pin hames + char* required_coordinates; + int max_joints; + int allow_duplicates; + int fwd_iterates_mask; // identify kins types that use iterative + // forward kinematics (typ: genhex) + // bitmask: 0x0 none + // bitmask: 0x1 bit0: switchkins_type==0 + // bitmask: 0x2 bit1: switchkins_type==1 + // bitmask: 0x4 bit2: switchkins_type==2 + int gui_kinstype; // may be reqd for parallel kins with vismach + // to select switchkins_type for gui pins + const struct kins_param_desc_tag *params; // geometry table, see below + int nparams; +} kparms; + +/* map letters in a coordinates string to joint numbers +** sequentially. Axis indices are 0:x,1:y,...,etc +** Example: coordinates=XYZYAC +** Result: axis_idx_for_jno[0] = 0 ==> X +** axis_idx_for_jno[1] = 1 ==> Y +** axis_idx_for_jno[2] = 2 ==> Z +** axis_idx_for_jno[3] = 1 ==> Y (duplicate allowed) +** axis_idx_for_jno[4] = 1 ==> A +** axis_idx_for_jno[5] = 1 ==> C +*/ +extern int map_coordinates_to_jnumbers(const char *coordinates, + const int max_joints, + const int allow_duplicates, + int axis_idx_for_jno[]); + +extern int mapped_joints_to_position(const int max_joints, + const double* joints, + EmcPose* pose); + +extern int position_to_mapped_joints(const int max_joints, + const EmcPose* pos, + double* joints); + +/* bitmap of joints assigned to an axis index (0:x,1:y,...,8:w) by the +** coordinates mapping; 0 when unused or not initialized */ +extern int identityKinematicsAxisJointsBitmap(int axis_num); + +extern int identityKinematicsSetup(const int comp_id, + const char* coordinates, + kparms* ksetup_parms); + +extern int identityKinematicsForward(const double *joint, + struct EmcPose * world, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags); + +extern int identityKinematicsInverse(const struct EmcPose * world, + double *joint, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags); + +/* joints are axes, so neither frame ever turns */ +extern int identityKinematicsToolFrame(const double *joint, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags); + +extern int identityKinematicsWorkFrame(const double *joint, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags); + +/* Rotations relating a module's own frame to the tool frame convention. + TOOL_FRAME_SPINDLE is the identity, for maths already in the convention. + TOOL_FRAME_FLANGE is the half turn about tool x that turns an ISO 9787 + flange frame, whose z points out of the mechanical interface towards the + work, into the convention. */ +extern const PmRotationMatrix TOOL_FRAME_SPINDLE; +extern const PmRotationMatrix TOOL_FRAME_FLANGE; + +/* Post-multiply a module's native frame by the rotation it declared, in + place. Modules built on switchkins.c never call this, the dispatch does it + for them; a standalone module calls it before returning. + Returns 0, or -1 if native is not a proper rotation. */ +extern int toolFrameApplyNative(PmRotationMatrix *rot, + const PmRotationMatrix *native); + +/* out = transpose(work) * tool, the tool frame in workpiece coordinates. + out may alias neither input. */ +extern int toolFrameInWork(const PmRotationMatrix *work, + const PmRotationMatrix *tool, + PmRotationMatrix *out); + +/* True if m is orthonormal with determinant +1, so a frame a machine can + actually hold. Used to check a declared rotation once, at load. */ +extern int toolFrameIsProper(const PmRotationMatrix *m); + + +/* The generic implementation of the above, driven by a module's own frame + functions, so that a module gets it for free once it supplies them. A + module with a closed form registers that instead: it is faster, and it + knows its own degenerate poses without having to find them. + + num_joints is the length of seed and of each row of solutions. */ +typedef int (*kinsFrameFunc)(const double *joint, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags); + +/* Which joints turn the work: a bit per joint whose motion changes the + work frame at the seed. This is what a caller needs to hold the table + still while the head orients the tool (Heidenhain COORD ROT), or to let + it take part (TABLE ROT), without a config entry naming it. Returns 0 + or -1 if the frame cannot be evaluated. */ +extern int toolFrameWorkJoints(kinsFrameFunc work, int num_joints, + const double *seed, unsigned int *mask); + +/* The two rotaries that orient the tool, told apart. One has its axis + fixed in the machine frame, the primary, and the other has its axis + carried by the first, the secondary. The two poses that reach one tool + direction differ in the sign of the secondary, which is what a caller + needs to name a pose rather than count them, Heidenhain's SEQ+ and SEQ-. + + Both are found from the module's own tool frame, by turning each joint a + little and reading the axis of the rotation that results, so a module + declares nothing and a switchkins type that turns nothing answers -1. + Returns 0 with both joints set, or -1 where the machine has any number + of orienting rotaries but two, a robot wrist among them, or where the + frame cannot be evaluated. */ +extern int toolFrameOrientJoints(kinsFrameFunc tool, int num_joints, + const double *seed, + int *primary, int *secondary); + +extern int toolFrameSolve(kinsFrameFunc work, + kinsFrameFunc tool, + int num_joints, + const PmCartesian *axis_in_work, + const PmCartesian *x_in_work, + const double *seed, + unsigned int held, + double *solutions, + int max_solutions, + int *free_directions, + double *tool_spin); + +typedef int (*kinsInverseFunc)(const EmcPose *world, + double *joint, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags); + +/* The generic Jacobian, by central differences of an inverse about world: + two inverse calls per pose coordinate, eighteen in all, on the solution + branch iflags selects. The joint array handed to every call starts from + joint, so a module that reads its joint argument sees the machine where + it is. + + The answer is as good as the inverse: a closed form gives it to rounding, + an inverse that iterates to a tolerance gives it to that tolerance over + the step, and should supply its own. num_joints is the module's joint + count. Returns 0, or -1 if any inverse fails. */ +#define KINS_JACOBIAN_STEP 1e-3 /* pose units, either kind */ + +extern int kinsJacobianFromInverse(kinsInverseFunc inverse, + int num_joints, + const double *joint, + const EmcPose *world, + const KINEMATICS_INVERSE_FLAGS *iflags, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]); + +/* For a module whose inverse computes a position P and then hands it to + position_to_mapped_joints(): given dP[axis][pose], how each coordinate of + P responds to each pose coordinate, fill in jac so that every joint gets + the row of the letter it is mapped to. Duplicate letters get duplicate + rows, which is the gantry case. */ +extern int kinsJacobianFromMappedAxes(int max_joints, + const double dP[EMCMOT_MAX_AXIS][EMCMOT_MAX_AXIS], + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]); + +/* The Jacobian of a serial arm of six revolute joints from its + Denavit-Hartenberg chain. Two conventions carry that name and put the + four parameters on different links; this is the modified one of John J. + Craig, Introduction to Robotics: Mechanics and Control, where link i is + Rx(alpha[i]) Tx(a[i]) Rz(joint[i]) Tz(d[i]), the joint turning about the + z of the frame Rx and Tx leave it in. (The original 1955 convention is + Rz(theta) Tz(d) Tx(a) Rx(alpha), and a table written for it does not fit + here.) The tool point `tool` lies along the z of the last frame, and + the pose of that point is reported as X Y Z and the RPY of the last + frame, R = Rz(C) Ry(B) Rx(A), as pmMatRpyConvert() does. + Each joint's axis crossed with the vector from it to the tool point + gives the point's rate per radian of the joint, the axis itself the + angular rate; that 6x6 inverted is the joint rate per unit of twist, and + the RPY rates reach the twist through the matrix of the axes each one + turns about, which is where world's B and C come in. alpha and joint in + degrees, a, d and tool in the module's length unit. Rows 0 to 5 of jac + are filled, the rest zero. Returns 0, or -1 at a singular pose, where no + finite joint rate follows the pose. */ +extern int kinsJacobianFromDhArm(const double alpha[6], const double a[6], + const double d[6], const double *joint, + double tool, const EmcPose *world, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]); + +/* joints are axes: a 1 per joint in the column of its letter */ +extern int identityKinematicsJacobian(const double *joint, + const EmcPose *world, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags); + +/* ------------------------------------------------------------------------ + Kinematics as pure functions of what the caller passes in. + + Everything above reads its geometry from HAL pins the module created and + keeps its mode and scratch in statics, so it can only answer for the + machine as it is now, from inside the module. The forms below take the + same questions with the machine described by the caller: a parameter + block naming the kinematics type, the joint map, the tool and the + geometry, and a scratch block for what an iterative method carries + between calls. Nothing is read from HAL and nothing is kept, so one copy + of the maths serves motion, a planner evaluating poses the machine has + not reached, task checking a program at load, and a tool asking what if. + + A module declares its geometry as a table of named entries. In RT the + shared code makes one HAL pin per entry, with the names configs already + use, and copies the pins into the block before every call; outside RT the + caller fills the block from wherever it likes. The maths reads + p->geometry[i] where it read a pin. + + The existing entry points stay and are supplied once, by kins_single.c + for a module with one kinematics type and by switchkins.c for one with + several, so nothing that calls kinematicsForward() changes. A module + that does not provide these forms keeps working as it did; it just cannot + be evaluated outside RT. + ------------------------------------------------------------------------ */ + +#define KINS_MAX_PARAMS 96 /* genhexkins declares 84 */ +#define KINS_MAX_TYPES SWITCHKINS_MAX_TYPES + +typedef enum { + KINS_PARAM_FLOAT = 0, + KINS_PARAM_BIT, + KINS_PARAM_S32, + KINS_PARAM_U32 +} kins_param_type; + +typedef enum { + KINS_IN = 0, /* read into the block before a call */ + KINS_OUT, /* a result, written from kins_scratch.out[] after it */ + KINS_IO /* read like an input; the pin is HAL_IO so it can be poked */ +} kins_param_dir; + +/* One entry of a module's geometry table. name follows the module's HAL + prefix. An entry with tool set is the tool length along the tool axis: + the shared code puts its value in kins_params.tool.tran.z as well, which + is what the maths should read, so that a caller outside RT can supply + the tool from the tool table without there being a pin. */ +typedef struct kins_param_desc_tag { + const char *name; + kins_param_type type; + kins_param_dir dir; + int tool; + double dflt; +} kins_param_desc; + +/* The machine, as far as the kinematics is concerned. One copy may be + shared by any number of callers: nothing writes it during a call. */ +typedef struct kins_params { + int size; /* sizeof(kins_params) */ + int ktype; /* kinematics type, 0 if one */ + int max_joints; /* joints the map covers */ + int joint_of_axis[EMCMOT_MAX_AXIS]; /* principal joint per letter */ + int joints_of_axis[EMCMOT_MAX_AXIS]; /* bit per joint, duplicates */ + EmcPose tool; /* tool offset, tool.tran.z along the tool axis */ + double geometry[KINS_MAX_PARAMS]; /* the table, in its order */ +} kins_params; + +/* What one caller carries between its own calls: the last pose an + iterative forward found, which seeds the next, and what a module reports + about its last call. Never shared between callers. */ +typedef struct kins_scratch { + EmcPose pose_seed; /* start an iterative forward here */ + int have_pose_seed; + int pose_seed_ok; /* pose_seed came from a solve that succeeded */ + double joint_seed[EMCMOT_MAX_JOINTS]; /* start an iterative inverse here */ + int have_joint_seed; + int iterations; + int failed; + double aux[8]; /* whatever else a module carries between calls */ + double out[KINS_MAX_PARAMS]; /* the table's KINS_OUT entries */ +} kins_scratch; + +typedef int (*kins_forward_fn)(const kins_params *p, kins_scratch *s, + const double *joint, EmcPose *pos, + const KINEMATICS_FORWARD_FLAGS *fflags, + KINEMATICS_INVERSE_FLAGS *iflags); + +typedef int (*kins_inverse_fn)(const kins_params *p, kins_scratch *s, + const EmcPose *pos, double *joint, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags); + +typedef int (*kins_frame_fn)(const kins_params *p, const double *joint, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags); + +typedef int (*kins_jacobian_fn)(const kins_params *p, const double *joint, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags); + +/* The maths of one kinematics type. forward and inverse are required; the + frames, the native rotation and the Jacobian are optional as before, and + a missing Jacobian is differenced from the inverse. fwd_iterates says the + forward starts from the pose it is handed, so the shared code seeds it + with the last answer after a switch. identity says joints are axes, which + a consumer may use to skip the maths altogether. primary says this is + the module's working transform, the type G43.4 switches to. machine says + this is the machine frame type, the pivot in machine coordinates with the + rotaries as joints, which G13.1 and G49 select and G53.5 moves in; a module + that leaves it unset on every type has its identity type stand in. */ +typedef struct kins_ops { + kins_forward_fn forward; + kins_inverse_fn inverse; + kins_frame_fn work; + kins_frame_fn tool; + const PmRotationMatrix *native; /* NULL means TOOL_FRAME_SPINDLE */ + kins_jacobian_fn jacobian; + int fwd_iterates; + int identity; /* joints are axes */ + int primary; /* the working transform */ + int machine; /* the machine frame */ +} kins_ops; + +/* A module described for a caller outside RT: its table, its joint + conventions and the maths of each type. ops[t] is NULL for a type the + module still implements the old way. */ +typedef struct kins_module_info { + const char *name; + const char *halprefix; + const kins_param_desc *params; + int nparams; + const char *required_coordinates; + int max_joints; /* the most the module allows */ + int allow_duplicates; + int ntypes; + const kins_ops *ops[KINS_MAX_TYPES]; +} kins_module_info; + +/* Exported by every module that provides the forms above. coordinates and + sparm are the module parameters the RT instance was loaded with; a module + whose types depend on them replays that choice here. Meant for a copy of + the module loaded outside RT; the RT instance answers from its own state + without redoing its setup. Returns 0, or -1 with info untouched. */ +extern int kinsDescribe(const char *coordinates, const char *sparm, + kins_module_info *info); + +/* Fill a block for a module: size, the joint map from coordinates (checked + against required_coordinates, the joint limit and the duplicate rule), + ktype 0, no tool, and every geometry entry at its table default. A + caller then overwrites what it knows better. Returns 0 or -1. */ +extern int kinsParamsInit(kins_params *p, + const kins_module_info *info, + const char *coordinates); + +/* The joint map alone, into a block, with no other field touched. */ +extern int kinsParamsMapCoordinates(kins_params *p, + const char *coordinates, + int max_joints, + int allow_duplicates, + const char *required_coordinates); + +/* Reset a scratch to "no seed, nothing reported". */ +extern void kinsScratchInit(kins_scratch *s); + +/* The map helpers above, reading the map from the block instead of from + the statics that map_coordinates_to_jnumbers() fills. */ +extern int kinsMappedJointsToPose(const kins_params *p, + const double *joints, EmcPose *pos); +extern int kinsPoseToMappedJoints(const kins_params *p, + const EmcPose *pos, double *joints); +extern int kinsJacobianFromMappedAxesP(const kins_params *p, + const double dP[EMCMOT_MAX_AXIS][EMCMOT_MAX_AXIS], + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]); + +/* Identity as pure functions: joints are axes through the block's map. */ +extern int kinsIdentityForward(const kins_params *p, kins_scratch *s, + const double *joint, EmcPose *pos, + const KINEMATICS_FORWARD_FLAGS *fflags, + KINEMATICS_INVERSE_FLAGS *iflags); +extern int kinsIdentityInverse(const kins_params *p, kins_scratch *s, + const EmcPose *pos, double *joint, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags); +extern int kinsIdentityFrame(const kins_params *p, const double *joint, + PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags); +extern int kinsIdentityJacobian(const kins_params *p, const double *joint, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags); +extern const kins_ops KINS_IDENTITY_OPS; + +/* The five questions asked of an ops table, with the defaults applied: + identity for a missing frame, the native rotation applied to the tool + frame, and the Jacobian differenced from the inverse when there is no + closed form. These are what the RT wrappers and a caller outside RT + both go through, so both get the same answers. */ +extern int kinsOpsForward(const kins_ops *ops, const kins_params *p, + kins_scratch *s, const double *joint, EmcPose *pos, + const KINEMATICS_FORWARD_FLAGS *fflags, + KINEMATICS_INVERSE_FLAGS *iflags); +extern int kinsOpsInverse(const kins_ops *ops, const kins_params *p, + kins_scratch *s, const EmcPose *pos, double *joint, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags); +extern int kinsOpsWorkFrame(const kins_ops *ops, const kins_params *p, + const double *joint, PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags); +extern int kinsOpsToolFrame(const kins_ops *ops, const kins_params *p, + const double *joint, PmRotationMatrix *rot, + const KINEMATICS_FORWARD_FLAGS *fflags); +extern int kinsOpsJacobian(const kins_ops *ops, const kins_params *p, + kins_scratch *s, const double *joint, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags); + +#ifdef __cplusplus +} +#endif + +#endif // __LINUXCNC_KINS_MODULE_H diff --git a/include/kins_rt.h b/include/kins_rt.h index a2db0323b56..a0c3d3f33ff 100644 --- a/include/kins_rt.h +++ b/include/kins_rt.h @@ -14,7 +14,7 @@ #define __LINUXCNC_KINS_RT_H #include "hal.h" -#include "kinematics.h" +#include "kins_module.h" /* one HAL pin handle per table entry, of whichever type the entry has */ typedef union { diff --git a/include/switchkins.h b/include/switchkins.h index dd239dd97ce..27d50757cbf 100644 --- a/include/switchkins.h +++ b/include/switchkins.h @@ -4,13 +4,10 @@ #ifndef __LINUXCNC_SWITCHKINS_H #define __LINUXCNC_SWITCHKINS_H -#include "kinematics.h" +#include "kins_module.h" -//SWITCHKINS_MAX_TYPES (max number of types a module may provide) -//is in kinematics.h as KINS_MAX_TYPES: motion and the NML -//status channel need it too -//max number of switchkins types a module may provide: -#define SWITCHKINS_MAX_TYPES KINS_MAX_TYPES +// SWITCHKINS_MAX_TYPES, the most types a module may provide, is in +// kinematics.h: motion and the NML status channel need it too // KinematicsFORWARD functions typedef int (*KF)(const double *joint, @@ -92,7 +89,7 @@ typedef int (*KJ)(const double *joint, // otherwise the generic differences of its own inverse. extern int switchkinsRegisterJacobian(int ktype, KJ kjac); -// provide one switchkins-type written as pure functions (see kinematics.h), +// provide one switchkins-type written as pure functions (see kins_module.h), // before switchkinsInit(). Its pins come from the table in kparms, shared // by every type of the module, so it has no setup function. A type may be // provided this way or through switchkinsRegister(), not both. diff --git a/src/emc/kinematics/5axiskins.c b/src/emc/kinematics/5axiskins.c index 81a765253b4..d1df89d9f36 100644 --- a/src/emc/kinematics/5axiskins.c +++ b/src/emc/kinematics/5axiskins.c @@ -59,6 +59,7 @@ #include #include +#include "userkfuncs.h" // the geometry, one pin each; the maths reads it from the block static const kins_param_desc fiveaxis_params[] = { diff --git a/src/emc/kinematics/corexykins.c b/src/emc/kinematics/corexykins.c index 3ba1496b6e1..eaaaa060df9 100644 --- a/src/emc/kinematics/corexykins.c +++ b/src/emc/kinematics/corexykins.c @@ -8,7 +8,7 @@ #include #include #include -#include +#include #include static int corexy_forward(const kins_params *p, kins_scratch *s, diff --git a/src/emc/kinematics/genhexkins.c b/src/emc/kinematics/genhexkins.c index 4deb523d9a3..34b124ddef4 100644 --- a/src/emc/kinematics/genhexkins.c +++ b/src/emc/kinematics/genhexkins.c @@ -104,7 +104,7 @@ a converged solution during current session. The maths is written as pure functions of the parameter block (see - kinematics.h): the pins above are the table below, read into the block + kins_module.h): the pins above are the table below, read into the block before every call and written from the scratch after it. ----------------------------------------------------------------------------*/ @@ -117,6 +117,7 @@ #include "genhexkins.h" #include +#include "userkfuncs.h" // the table: thirteen entries per strut, then the iteration controls, // the offsets and the reports. The macros index it. diff --git a/src/emc/kinematics/genhexkins.h b/src/emc/kinematics/genhexkins.h index 671419efcc9..60b48f7ec54 100644 --- a/src/emc/kinematics/genhexkins.h +++ b/src/emc/kinematics/genhexkins.h @@ -20,7 +20,7 @@ #ifndef GENHEXKINS_H #define GENHEXKINS_H -#include +#include #include #define GENHEX_MAX_JOINTS 6 diff --git a/src/emc/kinematics/genserfuncs.c b/src/emc/kinematics/genserfuncs.c index 02859829cf3..c43156490c9 100644 --- a/src/emc/kinematics/genserfuncs.c +++ b/src/emc/kinematics/genserfuncs.c @@ -29,7 +29,7 @@ the kins support both ANGULAR and LINEAR axes. The maths is written as pure functions of the parameter block (see - kinematics.h): the pins are the table below, read into the block + kins_module.h): the pins are the table below, read into the block before every call, and the link description is built from the block on each call. @@ -46,7 +46,7 @@ #include #include "libposemath/gotypes.h" /* go_result, go_integer */ #include "libposemath/gomath.h" /* go_pose */ -#include +#include #include "genserkins.h" /* these decls */ diff --git a/src/emc/kinematics/genserkins.c b/src/emc/kinematics/genserkins.c index 09c72a9fcbe..f2783b31951 100644 --- a/src/emc/kinematics/genserkins.c +++ b/src/emc/kinematics/genserkins.c @@ -5,7 +5,7 @@ * NOTEs: * 1) specify all kparms items * 2) the maths and the geometry table are in genserfuncs.c, written as -* pure functions of the parameter block (see kinematics.h) +* pure functions of the parameter block (see kins_module.h) */ /******************************************************************** @@ -44,6 +44,7 @@ frame-larger-than: #include "genserkins.h" #include +#include "userkfuncs.h" //-7 is system defined -3 ok, -4 ok, -5 ok,-6 ok (mm system) #undef GO_REAL_EPSILON diff --git a/src/emc/kinematics/genserkins.h b/src/emc/kinematics/genserkins.h index c5a2d9526f8..c50cddde3e3 100644 --- a/src/emc/kinematics/genserkins.h +++ b/src/emc/kinematics/genserkins.h @@ -36,7 +36,7 @@ #include /* HAL data types */ #include "libposemath/gotypes.h" /* go_result, go_integer */ #include "libposemath/gomath.h" /* go_pose */ -#include +#include /*! The maximum number of joints supported by the general serial @@ -131,7 +131,7 @@ extern int compute_jfwd(go_link * link_params, extern int compute_jinv(go_matrix * Jfwd, go_matrix * Jinv); -/* The kinematics as functions of the parameter block (see kinematics.h): +/* The kinematics as functions of the parameter block (see kins_module.h): the DH parameters and the unrotate couplings are the table, the maths is the ops. genser_links_of() fills a link description from a block, for a caller that wants the go_ routines directly. */ diff --git a/src/emc/kinematics/kins_single.c b/src/emc/kinematics/kins_single.c index 6d6937bd444..839313ef965 100644 --- a/src/emc/kinematics/kins_single.c +++ b/src/emc/kinematics/kins_single.c @@ -14,7 +14,7 @@ #include #include -#include +#include #include static kins_params rt_params; diff --git a/src/emc/kinematics/kins_util.c b/src/emc/kinematics/kins_util.c index 66df10d35e7..b66b3d9b9e7 100644 --- a/src/emc/kinematics/kins_util.c +++ b/src/emc/kinematics/kins_util.c @@ -49,7 +49,7 @@ #include #include #include -#include +#include #include // principal joint numbers based on module 'coordinates' parameter diff --git a/src/emc/kinematics/lineardeltakins.c b/src/emc/kinematics/lineardeltakins.c index 43a59521dc0..d61f3c58ff9 100644 --- a/src/emc/kinematics/lineardeltakins.c +++ b/src/emc/kinematics/lineardeltakins.c @@ -18,7 +18,7 @@ #include #include #include -#include +#include #include #include "lineardeltakins-common.h" diff --git a/src/emc/kinematics/maxkins.c b/src/emc/kinematics/maxkins.c index e1558d89152..a32981d12cf 100644 --- a/src/emc/kinematics/maxkins.c +++ b/src/emc/kinematics/maxkins.c @@ -21,7 +21,7 @@ #include #include #include -#include /* these decls */ +#include /* these decls */ #include #define d2r(d) ((d)*PM_PI/180.0) diff --git a/src/emc/kinematics/pentakins.c b/src/emc/kinematics/pentakins.c index 2436773d4bd..4be3a0918cf 100644 --- a/src/emc/kinematics/pentakins.c +++ b/src/emc/kinematics/pentakins.c @@ -46,7 +46,7 @@ changes the effector pivot point. The maths is written as pure functions of the parameter block (see - kinematics.h): the pins above are the table below, read into the block + kins_module.h): the pins above are the table below, read into the block before every call, and the entry points come from kins_single.c. ----------------------------------------------------------------------------*/ @@ -56,7 +56,7 @@ #include #include #include -#include /* these decls, KINEMATICS_FORWARD_FLAGS */ +#include /* these decls, KINEMATICS_FORWARD_FLAGS */ #include #include "pentakins.h" diff --git a/src/emc/kinematics/pumakins.c b/src/emc/kinematics/pumakins.c index 58e50f7c5b6..7f02f1777d6 100644 --- a/src/emc/kinematics/pumakins.c +++ b/src/emc/kinematics/pumakins.c @@ -23,6 +23,7 @@ #include "pumakins.h" #include +#include "userkfuncs.h" // the five dimensions, one pin each; the maths reads them from the block static const kins_param_desc puma_params[] = { diff --git a/src/emc/kinematics/rosekins.c b/src/emc/kinematics/rosekins.c index d37ce3584be..1c807d5c2ce 100644 --- a/src/emc/kinematics/rosekins.c +++ b/src/emc/kinematics/rosekins.c @@ -21,7 +21,7 @@ #include #include #include -#include +#include #include MODULE_LICENSE("GPL"); diff --git a/src/emc/kinematics/rotarydeltakins.c b/src/emc/kinematics/rotarydeltakins.c index 6ce42abf0f0..d406406e76d 100644 --- a/src/emc/kinematics/rotarydeltakins.c +++ b/src/emc/kinematics/rotarydeltakins.c @@ -19,7 +19,7 @@ #include #include #include -#include +#include #include #include "rotarydeltakins-common.h" diff --git a/src/emc/kinematics/rotatekins.c b/src/emc/kinematics/rotatekins.c index 52e198cf539..abcf0676d36 100644 --- a/src/emc/kinematics/rotatekins.c +++ b/src/emc/kinematics/rotatekins.c @@ -17,7 +17,7 @@ #include #include #include -#include /* these decls */ +#include /* these decls */ #include static int rotate_forward(const kins_params *p, kins_scratch *s, diff --git a/src/emc/kinematics/scarakins.c b/src/emc/kinematics/scarakins.c index 19820930c91..a416724a5c2 100644 --- a/src/emc/kinematics/scarakins.c +++ b/src/emc/kinematics/scarakins.c @@ -21,6 +21,7 @@ #include #include +#include "userkfuncs.h" /* key dimensions diff --git a/src/emc/kinematics/scorbot-kins.c b/src/emc/kinematics/scorbot-kins.c index 2adcdeb29f2..b3be3c53db4 100644 --- a/src/emc/kinematics/scorbot-kins.c +++ b/src/emc/kinematics/scorbot-kins.c @@ -43,7 +43,7 @@ #include #include #include -#include +#include #include diff --git a/src/emc/kinematics/switchkins.c b/src/emc/kinematics/switchkins.c index d770bf035a0..e90b35d14c8 100644 --- a/src/emc/kinematics/switchkins.c +++ b/src/emc/kinematics/switchkins.c @@ -48,7 +48,7 @@ static KTI ktinvs[SWITCHKINS_MAX_TYPES] = {NULL}; static KJ kjacs[SWITCHKINS_MAX_TYPES] = {NULL}; static PmRotationMatrix knative[SWITCHKINS_MAX_TYPES]; -// types written as pure functions (see kinematics.h): the maths of each, +// types written as pure functions (see kins_module.h): the maths of each, // the one RT parameter block they all read, a scratch per type, and the // pins made from the module's table static const kins_ops *kops[SWITCHKINS_MAX_TYPES] = {NULL}; diff --git a/src/emc/kinematics/three21kins.c b/src/emc/kinematics/three21kins.c index 903e3e08a07..438d36ecf0c 100644 --- a/src/emc/kinematics/three21kins.c +++ b/src/emc/kinematics/three21kins.c @@ -4,6 +4,7 @@ #include #include +#include "userkfuncs.h" /* default values for ar2 robot */ #define DEFAULT_THREE21_A1 64.2 diff --git a/src/emc/kinematics/tripodkins.c b/src/emc/kinematics/tripodkins.c index 8b482fdeb4d..7ce648fc581 100644 --- a/src/emc/kinematics/tripodkins.c +++ b/src/emc/kinematics/tripodkins.c @@ -67,7 +67,7 @@ #include #include #include -#include /* these decls */ +#include /* these decls */ #include // the base geometry, one pin each, poked from HAL as before diff --git a/src/emc/kinematics/trivkins.c b/src/emc/kinematics/trivkins.c index e8c096f92c1..0dac5708344 100644 --- a/src/emc/kinematics/trivkins.c +++ b/src/emc/kinematics/trivkins.c @@ -15,7 +15,7 @@ #include #include #include -#include +#include #include // joints are axes, through whatever map coordinates= gives; the maths is diff --git a/src/emc/kinematics/trtfuncs.c b/src/emc/kinematics/trtfuncs.c index 86a24e88410..4aa6c99a925 100644 --- a/src/emc/kinematics/trtfuncs.c +++ b/src/emc/kinematics/trtfuncs.c @@ -29,7 +29,7 @@ * conventional axis directions. See * https://linuxcnc.org/docs/html/gcode/machining-center.html * -* Written as pure functions of the parameter block (see kinematics.h): +* Written as pure functions of the parameter block (see kins_module.h): * the geometry is the table below, the joint map comes from the block, * and the tool length is p->tool.tran.z. ********************************************************************/ @@ -37,7 +37,8 @@ #include #include #include -#include +#include +#include "trtfuncs.h" // the geometry both machines share, one pin each const kins_param_desc TRT_PARAMS[] = { diff --git a/src/emc/kinematics/trtfuncs.h b/src/emc/kinematics/trtfuncs.h new file mode 100644 index 00000000000..f913322d798 --- /dev/null +++ b/src/emc/kinematics/trtfuncs.h @@ -0,0 +1,20 @@ +/******************************************************************** +* Description: trtfuncs.h +* The two table-rotary tilting geometries, xyzac and xyzbc, written +* once in trtfuncs.c for the two modules that load them. +* +* License: GPL Version 2 +********************************************************************/ +#ifndef __LINUXCNC_TRTFUNCS_H +#define __LINUXCNC_TRTFUNCS_H + +#include + + +// xyzac,xyzbc (trtfuncs.c): one geometry table, the maths of each machine +extern const kins_param_desc TRT_PARAMS[]; +extern const int TRT_NPARAMS; +extern const kins_ops XYZAC_OPS; +extern const kins_ops XYZBC_OPS; + +#endif diff --git a/src/emc/kinematics/userkfuncs.c b/src/emc/kinematics/userkfuncs.c index 0e51ed651e5..9dc5ee17ec2 100644 --- a/src/emc/kinematics/userkfuncs.c +++ b/src/emc/kinematics/userkfuncs.c @@ -24,7 +24,8 @@ // typical includes: //#include // if reqd #include -#include +#include +#include "userkfuncs.h" // Add for kins based on genserkins: // #include "genserkins.h" //includes gomath,hal @@ -53,7 +54,7 @@ static int userk_inverse(const kins_params *p, kins_scratch *s, const kins_ops USERK_OPS = { .forward = userk_forward, .inverse = userk_inverse, - // .work, .tool, .native and .jacobian are optional, see kinematics.h + // .work, .tool, .native and .jacobian are optional, see kins_module.h }; //********************************************************************** diff --git a/src/emc/kinematics/userkfuncs.h b/src/emc/kinematics/userkfuncs.h new file mode 100644 index 00000000000..d0f06b78ccf --- /dev/null +++ b/src/emc/kinematics/userkfuncs.h @@ -0,0 +1,30 @@ +/******************************************************************** +* Description: userkfuncs.h +* The user-defined kinematics template, userkfuncs.c, shared by the +* switchkins modules as their third type. +* +* License: GPL Version 2 +********************************************************************/ +#ifndef __LINUXCNC_USERKFUNCS_H +#define __LINUXCNC_USERKFUNCS_H + +#include + + +extern const kins_ops USERK_OPS; + +extern int userkKinematicsSetup(const int comp_id, + const char* coordinates, + kparms* ksetup_parms); + +extern int userkKinematicsForward(const double *joint, + struct EmcPose * world, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags); + +extern int userkKinematicsInverse(const struct EmcPose * world, + double *joint, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags); + +#endif diff --git a/src/emc/kinematics/xyzac-trt-kins.c b/src/emc/kinematics/xyzac-trt-kins.c index 3fc14fc9de4..2d99eb5f6aa 100644 --- a/src/emc/kinematics/xyzac-trt-kins.c +++ b/src/emc/kinematics/xyzac-trt-kins.c @@ -9,7 +9,7 @@ * 4) The directions of the rotational axes are the opposite of the * conventional axis directions. * 5) the maths and the geometry table are in trtfuncs.c, written as -* pure functions of the parameter block (see kinematics.h) +* pure functions of the parameter block (see kins_module.h) */ #include @@ -17,6 +17,8 @@ #include #include +#include "userkfuncs.h" +#include "trtfuncs.h" int switchkinsSetup(kparms* kp, KS* kset0, KS* kset1, KS* kset2, diff --git a/src/emc/kinematics/xyzbc-trt-kins.c b/src/emc/kinematics/xyzbc-trt-kins.c index 45c41b448dd..acb5fcc3b05 100644 --- a/src/emc/kinematics/xyzbc-trt-kins.c +++ b/src/emc/kinematics/xyzbc-trt-kins.c @@ -9,7 +9,7 @@ * 4) The directions of the rotational axes are the opposite of the * conventional axis directions. * 5) the maths and the geometry table are in trtfuncs.c, written as -* pure functions of the parameter block (see kinematics.h) +* pure functions of the parameter block (see kins_module.h) */ #include @@ -17,6 +17,8 @@ #include #include +#include "userkfuncs.h" +#include "trtfuncs.h" int switchkinsSetup(kparms* kp, KS* kset0, KS* kset1, KS* kset2, diff --git a/src/emc/kinematics_userspace/kinematics_user.c b/src/emc/kinematics_userspace/kinematics_user.c index 9ed7e401a99..92de369697a 100644 --- a/src/emc/kinematics_userspace/kinematics_user.c +++ b/src/emc/kinematics_userspace/kinematics_user.c @@ -4,7 +4,7 @@ * * Loads a kinematics .so with dlopen, asks it to describe itself through * kinsDescribe(), and evaluates its kinematics through the parameter - * block (see kinematics.h). The block is filled from HAL: the RT + * block (see kins_module.h). The block is filled from HAL: the RT * instance's own pins, read by name whenever the block is refreshed, so * the values are the live ones and nothing is made in HAL to get at them. * The tool is diff --git a/src/hal/components/matrixkins.comp b/src/hal/components/matrixkins.comp index 664a41d6f07..ff542cde5d7 100644 --- a/src/hal/components/matrixkins.comp +++ b/src/hal/components/matrixkins.comp @@ -180,7 +180,7 @@ option extra_setup; license "GPL"; ;; -#include +#include #include // the calibration matrix, one pin each; the maths reads it from the block diff --git a/src/hal/components/millturn.comp b/src/hal/components/millturn.comp index 2f385bff5c5..aeee6b3b76f 100644 --- a/src/hal/components/millturn.comp +++ b/src/hal/components/millturn.comp @@ -49,7 +49,7 @@ FUNCTION(fdemo) { } // the turn kinematics: no geometry, written as pure functions of the -// parameter block (see kinematics.h) +// parameter block (see kins_module.h) static int turn_forward(const kins_params *p, kins_scratch *s, const double *j, EmcPose * pos, diff --git a/src/hal/components/switchkinscomp.comp b/src/hal/components/switchkinscomp.comp index 3d04337fe66..4550d7d1fb4 100644 --- a/src/hal/components/switchkinscomp.comp +++ b/src/hal/components/switchkinscomp.comp @@ -18,7 +18,7 @@ the switchkins_core module, which the HAL file loads first; this component only registers its types with it. The kinematics are written as functions of a parameter block, see -kinematics.h and the Kinematics Conventions chapter: the geometry is +kins_module.h and the Kinematics Conventions chapter: the geometry is declared once in a table, one HAL pin is made per entry, and the maths reads the block where it would have read a pin. The same maths can then be evaluated outside realtime. @@ -90,7 +90,7 @@ enum { P_X_OFFSET }; //--------------------------------------------------------------------- // Example switchkins-type: a forward and an inverse over the block. // Replace the arithmetic with the real kinematics. The frames and the -// Jacobian are optional, see kinematics.h. +// Jacobian are optional, see kins_module.h. static int myForward(const kins_params *p, kins_scratch *s, const double *j, diff --git a/src/hal/components/userkins.comp b/src/hal/components/userkins.comp index fef97bb0061..263aee5a86a 100644 --- a/src/hal/components/userkins.comp +++ b/src/hal/components/userkins.comp @@ -61,7 +61,7 @@ change all instances of `userkins` to `mykins`. === NOTES * The kinematics are written as functions of a parameter block, see - kinematics.h: the geometry is declared once in the *userkins_params* + kins_module.h: the geometry is declared once in the *userkins_params* table, one HAL pin is made per entry, and the maths reads *p->geometry[]* where it would have read a pin. The classic entry points (kinematicsForward() and the rest) are supplied by kins_single.c, @@ -80,7 +80,7 @@ license "GPL"; author "Dewey Garrett"; ;; -#include +#include #include // The geometry, one HAL pin per entry, named userkins.. An entry @@ -165,7 +165,7 @@ static int userkins_jacobian(const kins_params *p, const double *j, (void)iflags; // How each joint responds to each pose coordinate, the derivative of // userkins_inverse(): for this template joint 0 follows x, joint 1 - // follows y and joint 2 follows z, each one for one. See kinematics.h. + // follows y and joint 2 follows z, each one for one. See kins_module.h. // Leave .jacobian out of the ops below to have it differenced instead. for (r = 0; r < EMCMOT_MAX_JOINTS; r++) { for (c = 0; c < EMCMOT_MAX_AXIS; c++) { jac[r][c] = 0; } @@ -180,7 +180,7 @@ static const kins_ops userkins_ops = { .forward = userkins_forward, .inverse = userkins_inverse, .jacobian = userkins_jacobian, - // .work, .tool and .native report the frames, see kinematics.h + // .work, .tool and .native report the frames, see kins_module.h }; const kins_module_info kins_module = { diff --git a/src/hal/components/xyzab_tdr_kins.comp b/src/hal/components/xyzab_tdr_kins.comp index e1616648a51..0444c68e1d1 100644 --- a/src/hal/components/xyzab_tdr_kins.comp +++ b/src/hal/components/xyzab_tdr_kins.comp @@ -45,7 +45,7 @@ static char *coordinates; RTAPI_MP_STRING(coordinates, "Axes-to-joints-ordering"); // the geometry, one pin each; the maths reads it from the block (see -// kinematics.h), and the tool length from p->tool.tran.z +// kins_module.h), and the tool length from p->tool.tran.z static const kins_param_desc tdr_params[] = { { "tool-offset-z", KINS_PARAM_FLOAT, KINS_IN, 1, 0.0 }, { "x-offset", KINS_PARAM_FLOAT, KINS_IN, 0, 0.0 }, diff --git a/src/hal/components/xyzacb_trsrn.comp b/src/hal/components/xyzacb_trsrn.comp index 1ed5759ca6a..2ee44e38849 100644 --- a/src/hal/components/xyzacb_trsrn.comp +++ b/src/hal/components/xyzacb_trsrn.comp @@ -27,7 +27,7 @@ RTAPI_MP_STRING(coordinates, "Axes-to-joints-ordering"); // The geometry of the universal spindle head, one pin each, shared by the // TCP and TOOL kinematics; the maths reads it from the block (see -// kinematics.h) and the tool length from p->tool.tran.z. The two angle +// kins_module.h) and the tool length from p->tool.tran.z. The two angle // pins are what the TOOL kinematics uses in place of the head joints: // the remap writes them. static const kins_param_desc trsrn_params[] = { diff --git a/src/hal/components/xyzbca_trsrn.comp b/src/hal/components/xyzbca_trsrn.comp index 80c7ca20305..f914794a1f4 100644 --- a/src/hal/components/xyzbca_trsrn.comp +++ b/src/hal/components/xyzbca_trsrn.comp @@ -27,7 +27,7 @@ RTAPI_MP_STRING(coordinates, "Axes-to-joints-ordering"); // The geometry of the universal spindle head, one pin each, shared by the // TCP and TOOL kinematics; the maths reads it from the block (see -// kinematics.h) and the tool length from p->tool.tran.z. The two angle +// kins_module.h) and the tool length from p->tool.tran.z. The two angle // pins are what the TOOL kinematics uses in place of the head joints: // the remap writes them. static const kins_param_desc trsrn_params[] = { diff --git a/tests/kins-params/paritycheck.c b/tests/kins-params/paritycheck.c index 9043df69dac..1a3ef4664ab 100644 --- a/tests/kins-params/paritycheck.c +++ b/tests/kins-params/paritycheck.c @@ -27,7 +27,7 @@ #include #include #include -#include +#include MODULE_LICENSE("GPL"); diff --git a/tests/tool-frame/toolframecheck.c b/tests/tool-frame/toolframecheck.c index 8896d737534..095756589e4 100644 --- a/tests/tool-frame/toolframecheck.c +++ b/tests/tool-frame/toolframecheck.c @@ -23,7 +23,7 @@ #include #include #include -#include +#include MODULE_LICENSE("GPL"); MODULE_DESCRIPTION("tool frame helper checker");