From 3ecf9a142234187e2ff3a6a533b80c4358fd9c95 Mon Sep 17 00:00:00 2001 From: joseph calderon Date: Tue, 21 Jul 2026 15:11:44 -0700 Subject: [PATCH 1/3] port three21kins --- src/Makefile | 9 + src/emc/kinematics/meson.build | 4 + src/emc/kinematics/three21kins.c | 357 +++++++++++++++++++++++++++++++ src/emc/kinematics/three21kins.h | 26 +++ 4 files changed, 396 insertions(+) create mode 100644 src/emc/kinematics/three21kins.c create mode 100644 src/emc/kinematics/three21kins.h diff --git a/src/Makefile b/src/Makefile index 69952ddeac9..5386e79394a 100644 --- a/src/Makefile +++ b/src/Makefile @@ -1196,6 +1196,14 @@ pumakins-objs += emc/kinematics/kins_util.o pumakins-objs += emc/kinematics/switchkins.o pumakins-objs += $(USERKFUNCS) +obj-m += three21kins.o +three21kins-objs := emc/kinematics/three21kins.o +three21kins-objs += libnml/posemath/_posemath.o +three21kins-objs += libnml/posemath/sincos.o $(MATHSTUB) +three21kins-objs += emc/kinematics/kins_util.o +three21kins-objs += emc/kinematics/switchkins.o +three21kins-objs += $(USERKFUNCS) + obj-m += 5axiskins.o 5axiskins-objs := emc/kinematics/5axiskins.o 5axiskins-objs += libnml/posemath/_posemath.o @@ -1415,6 +1423,7 @@ endif ../rtlib/xyzbc-trt-kins$(MODULE_EXT): $(addprefix objects/rt,$(xyzbc-trt-kins-objs)) ../rtlib/scarakins$(MODULE_EXT): $(addprefix objects/rt,$(scarakins-objs)) ../rtlib/pumakins$(MODULE_EXT): $(addprefix objects/rt,$(pumakins-objs)) +../rtlib/three21kins$(MODULE_EXT): $(addprefix objects/rt,$(three21kins-objs)) #---------------------------------------------------------------- ifeq ($(TRIVIAL_BUILD),no) diff --git a/src/emc/kinematics/meson.build b/src/emc/kinematics/meson.build index 3608107cda5..c49a58412c6 100644 --- a/src/emc/kinematics/meson.build +++ b/src/emc/kinematics/meson.build @@ -10,6 +10,7 @@ kinematics_srcs = files([ 'maxkins.c', 'rotatekins.c', 'tripodkins.c', +'three21kins.c', ]) kinematics_inc = include_directories('.') @@ -32,3 +33,6 @@ genserkins_srcs = files('genserkins.c') pumakins_srcs = files('pumakins.c') scarakins_srcs = files('scarakins.c') + +three21kins_srcs = files('three21kins.c') + diff --git a/src/emc/kinematics/three21kins.c b/src/emc/kinematics/three21kins.c new file mode 100644 index 00000000000..658254da1df --- /dev/null +++ b/src/emc/kinematics/three21kins.c @@ -0,0 +1,357 @@ +#include +#include +#include +#include +#include + +#include "three21kins.h" +#include "switchkins.h" + +struct haldata { + hal_float_t *a1, *a2, *a3, *d1, *d2, *d3, *d4, *d6; +} *haldata = 0; + +#define THREE21_A1 (*(haldata->a1)) +#define THREE21_A2 (*(haldata->a2)) +#define THREE21_A3 (*(haldata->a3)) +#define THREE21_D1 (*(haldata->d1)) +#define THREE21_D2 (*(haldata->d2)) +#define THREE21_D3 (*(haldata->d3)) +#define THREE21_D4 (*(haldata->d4)) +#define THREE21_D6 (*(haldata->d6)) + +static int three21KinematicsForward(const double * joint, + EmcPose * world, + const KINEMATICS_FORWARD_FLAGS * fflags, + KINEMATICS_INVERSE_FLAGS * iflags) +{ + (void)fflags; + double s1, s2, s3, s4, s5, s6; + double c1, c2, c3, c4, c5, c6; + double s23; + double c23; + double t1, t2, t3, t4, t5; + double sumSq, k; + double d23; + PmHomogeneous hom; + PmPose worldPose; + PmRpy rpy; + + /* calculate sin of joints for future use */ + s1 = sin(joint[0]*PM_PI/180); + s2 = sin(joint[1]*PM_PI/180); + s3 = sin(joint[2]*PM_PI/180); + s4 = sin(joint[3]*PM_PI/180); + s5 = sin(joint[4]*PM_PI/180); + s6 = sin(joint[5]*PM_PI/180); + + /* calculate cos of joints for future use */ + c1 = cos(joint[0]*PM_PI/180); + c2 = cos(joint[1]*PM_PI/180); + c3 = cos(joint[2]*PM_PI/180); + c4 = cos(joint[3]*PM_PI/180); + c5 = cos(joint[4]*PM_PI/180); + c6 = cos(joint[5]*PM_PI/180); + + s23 = c2 * s3 + s2 * c3; + c23 = c2 * c3 - s2 * s3; + + /* calculate terms for first column of rotation matrix */ + t1 = c4 * c5 * c6 - s4 * s6; + t2 = s23 * s5 * c6; + t3 = s4 * c5 * c6 + c4 * s6; + t4 = c23 * t1 - t2; + t5 = c23 * s5 * c6; + + /* define first column of rotation matrix */ + hom.rot.x.x = c1 * t4 + s1 * t3; + hom.rot.x.y = s1 * t4 - c1 * t3; + hom.rot.x.z = -s23 * t1 - t5; + + /* calculate terms for second column of rotation matrix */ + t1 = -c4 * c5 * s6 - s4 * c6; + t2 = s23 * s5 * s6; + t3 = c4 * c6 - s4 * c5 * s6; + t4 = c23 * t1 + t2; + t5 = c23 * s5 * s6; + + /* define second column of rotation matrix */ + hom.rot.y.x = c1 * t4 + s1 * t3; + hom.rot.y.y = s1 * t4 - c1 * t3; + hom.rot.y.z = -s23 * t1 + t5; + + /* calculate terms for third column of rotation matrix */ + t1 = c23 * c4 * s5 + s23 * c5; + + /* define third column of rotation matrix */ + hom.rot.z.x = -c1 * t1 - s1 * s4 * s5; + hom.rot.z.y = -s1 * t1 + c1 * s4 * s5; + hom.rot.z.z = s23 * c4 * s5 - c23 * c5; + + /* calculate term for position vector */ + t1 = THREE21_A1 + THREE21_A2 * c2 + THREE21_A3 * c23 - THREE21_D4 * s23; + + /* define position vector */ + d23 = THREE21_D2 + THREE21_D3; + hom.tran.x = c1 * t1 - d23 * s1; + hom.tran.y = s1 * t1 + d23 * c1; + hom.tran.z = THREE21_D1 - THREE21_A3 * s23 - THREE21_A2 * s2 - THREE21_D4 * c23; + + /* calculate terms to determine flags */ + sumSq = hom.tran.x * hom.tran.x + hom.tran.y * hom.tran.y - d23 * d23; + k = (sumSq + (hom.tran.z - THREE21_D1) * (hom.tran.z - THREE21_D1) + THREE21_A1 * THREE21_A1 - + 2.0 * THREE21_A1 * (c1 * hom.tran.x + s1 * hom.tran.y) - + THREE21_A2 * THREE21_A2 - THREE21_A3 * THREE21_A3 - THREE21_D4 * THREE21_D4) / + (2.0 * THREE21_A2); + + /* reset flags */ + *iflags = 0; + + /* set shoulder flag */ + if (fabs(joint[0]*PM_PI/180 - atan2(hom.tran.y, hom.tran.x) + + atan2(d23, -sqrt(sumSq))) < FLAG_FUZZ) + { + *iflags |= THREE21_SHOULDER_RIGHT; + } + + /* set elbow flag */ + if (fabs(joint[2]*PM_PI/180 - atan2(THREE21_A3, THREE21_D4) + + atan2(k, -sqrt(THREE21_A3 * THREE21_A3 + THREE21_D4 * THREE21_D4 - k * k))) < FLAG_FUZZ) + { + *iflags |= THREE21_ELBOW_DOWN; + } + + /* set singular flag */ + t1 = -hom.rot.z.x * s1 + hom.rot.z.y * c1; + t2 = -hom.rot.z.x * c1 * c23 - hom.rot.z.y * s1 * c23 + hom.rot.z.z * s23; + if (fabs(t1) < SINGULAR_FUZZ && fabs(t2) < SINGULAR_FUZZ) + { + *iflags |= THREE21_SINGULAR; + } + else + { + if (! (fabs(joint[3]*PM_PI/180 - atan2(t1, t2)) < FLAG_FUZZ)) + { + *iflags |= THREE21_WRIST_FLIP; + } + } + + /* add effect of d6 parameter */ + hom.tran.x = hom.tran.x + hom.rot.z.x*THREE21_D6; + hom.tran.y = hom.tran.y + hom.rot.z.y*THREE21_D6; + hom.tran.z = hom.tran.z + hom.rot.z.z*THREE21_D6; + + /* convert hom to pose */ + pmHomPoseConvert(&hom, &worldPose); + pmQuatRpyConvert(&worldPose.rot,&rpy); + world->tran = worldPose.tran; + world->a = rpy.r * 180.0/PM_PI; + world->b = rpy.p * 180.0/PM_PI; + world->c = rpy.y * 180.0/PM_PI; + + return 0; +} + +static int three21KinematicsInverse(const EmcPose * world, + double * joint, + const KINEMATICS_INVERSE_FLAGS * iflags, + KINEMATICS_FORWARD_FLAGS * fflags) +{ + PmHomogeneous hom; + PmPose worldPose; + PmRpy rpy; + + double t1, t2, t3; + double k; + double sumSq; + double d23; + + double th1; + double th3; + double th23; + double th2; + double th4; + double th5; + double th6; + + double s1, c1; + double s3, c3; + double s23, c23; + double s4, c4; + double s5, c5; + double s6, c6; + double px, py, pz; + + /* reset flags */ + *fflags = 0; + + /* convert pose to hom */ + worldPose.tran = world->tran; + rpy.r = world->a*PM_PI/180.0; + rpy.p = world->b*PM_PI/180.0; + rpy.y = world->c*PM_PI/180.0; + pmRpyQuatConvert(&rpy,&worldPose.rot); + pmPoseHomConvert(&worldPose, &hom); + + /* remove effect of d6 parameter */ + px = hom.tran.x - THREE21_D6*hom.rot.z.x; + py = hom.tran.y - THREE21_D6*hom.rot.z.y; + pz = hom.tran.z - THREE21_D1 - THREE21_D6*hom.rot.z.z; + + /* joint 1 (2 independent solutions) */ + d23 = THREE21_D2 + THREE21_D3; + sumSq = px * px + py * py - d23 * d23; + + if (*iflags & THREE21_SHOULDER_RIGHT) { + th1 = atan2(py, px) - atan2(d23, -sqrt(sumSq)); + } else { + th1 = atan2(py, px) - atan2(d23, sqrt(sumSq)); + } + + /* save sin, cos for later calcs */ + s1 = sin(th1); + c1 = cos(th1); + + /* joint 3 (2 independent solutions) */ + k = (sumSq + pz * pz + THREE21_A1 * THREE21_A1 - + 2.0 * THREE21_A1 * (c1 * px + s1 * py) - + THREE21_A2 * THREE21_A2 - THREE21_A3 * THREE21_A3 - THREE21_D4 * THREE21_D4) / + (2.0 * THREE21_A2); + + if (*iflags & THREE21_ELBOW_DOWN) { + th3 = atan2(THREE21_A3, THREE21_D4) - + atan2(k, -sqrt(THREE21_A3 * THREE21_A3 + THREE21_D4 * THREE21_D4 - k * k)); + } else { + th3 = atan2(THREE21_A3, THREE21_D4) - + atan2(k, sqrt(THREE21_A3 * THREE21_A3 + THREE21_D4 * THREE21_D4 - k * k)); + } + + /* compute sin, cos for later calcs */ + s3 = sin(th3); + c3 = cos(th3); + + /* joint 2 */ + t1 = (-THREE21_A3 - THREE21_A2 * c3) * pz + + (c1 * px + s1 * py - THREE21_A1) * (THREE21_A2 * s3 - THREE21_D4); + t2 = (THREE21_A2 * s3 - THREE21_D4) * pz + + (THREE21_A3 + THREE21_A2 * c3) * (c1 * px + s1 * py - THREE21_A1); + t3 = pz * pz + (c1 * px + s1 * py - THREE21_A1) * (c1 * px + s1 * py - THREE21_A1); + + th23 = atan2(t1, t2); + th2 = th23 - th3; + + /* compute sin, cos for later calcs */ + s23 = t1 / t3; + c23 = t2 / t3; + + /* joint 4 */ + t1 = -hom.rot.z.x * s1 + hom.rot.z.y * c1; + t2 = -hom.rot.z.x * c1 * c23 - hom.rot.z.y * s1 * c23 + hom.rot.z.z * s23; + if (fabs(t1) < SINGULAR_FUZZ && fabs(t2) < SINGULAR_FUZZ) { + *fflags |= THREE21_REACH; + th4 = joint[3]*PM_PI/180; /* use current value */ + } else { + th4 = atan2(t1, t2); + } + + /* compute sin, cos for later calcs */ + s4 = sin(th4); + c4 = cos(th4); + + /* joint 5 */ + s5 = hom.rot.z.z * (s23 * c4) - + hom.rot.z.x * (c1 * c23 * c4 + s1 * s4) - + hom.rot.z.y * (s1 * c23 * c4 - c1 * s4); + c5 = -hom.rot.z.x * (c1 * s23) - hom.rot.z.y * + (s1 * s23) - hom.rot.z.z * c23; + th5 = atan2(s5, c5); + + /* joint 6 */ + s6 = hom.rot.x.z * (s23 * s4) - hom.rot.x.x * + (c1 * c23 * s4 - s1 * c4) - hom.rot.x.y * + (s1 * c23 * s4 + c1 * c4); + c6 = hom.rot.x.x * ((c1 * c23 * c4 + s1 * s4) * + c5 - c1 * s23 * s5) + hom.rot.x.y * + ((s1 * c23 * c4 - c1 * s4) * c5 - s1 * s23 * s5) - + hom.rot.x.z * (s23 * c4 * c5 + c23 * s5); + th6 = atan2(s6, c6); + + /* wrist flip */ + if (*iflags & THREE21_WRIST_FLIP) { + th4 = th4 + PM_PI; + th5 = -th5; + th6 = th6 + PM_PI; + } + + /* copy out */ + joint[0] = th1*180/PM_PI; + joint[1] = th2*180/PM_PI; + joint[2] = th3*180/PM_PI; + joint[3] = th4*180/PM_PI; + joint[4] = th5*180/PM_PI; + joint[5] = th6*180/PM_PI; + + 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_float_newf(HAL_IN, &(haldata->a1), comp_id,"%s.A1",kp->halprefix); + res += hal_pin_float_newf(HAL_IN, &(haldata->a2), comp_id,"%s.A2",kp->halprefix); + res += hal_pin_float_newf(HAL_IN, &(haldata->a3), comp_id,"%s.A3",kp->halprefix); + res += hal_pin_float_newf(HAL_IN, &(haldata->d1), comp_id,"%s.D1",kp->halprefix); + res += hal_pin_float_newf(HAL_IN, &(haldata->d2), comp_id,"%s.D2",kp->halprefix); + res += hal_pin_float_newf(HAL_IN, &(haldata->d3), comp_id,"%s.D3",kp->halprefix); + res += hal_pin_float_newf(HAL_IN, &(haldata->d4), comp_id,"%s.D4",kp->halprefix); + res += hal_pin_float_newf(HAL_IN, &(haldata->d6), comp_id,"%s.D6",kp->halprefix); + if (res) { goto error; } + + THREE21_A1 = DEFAULT_THREE21_A1; + THREE21_A2 = DEFAULT_THREE21_A2; + THREE21_A3 = DEFAULT_THREE21_A3; + THREE21_D1 = DEFAULT_THREE21_D1; + THREE21_D2 = DEFAULT_THREE21_D2; + THREE21_D3 = DEFAULT_THREE21_D3; + THREE21_D4 = DEFAULT_THREE21_D4; + THREE21_D6 = DEFAULT_THREE21_D6; + + return 0; + +error: + return -1; +} + +int switchkinsSetup(kparms* kp, + KS* kset0, KS* kset1, KS* kset2, + KF* kfwd0, KF* kfwd1, KF* kfwd2, + KI* kinv0, KI* kinv1, KI* kinv2 + ) +{ + kp->kinsname = "three21kins"; + kp->halprefix = "three21kins"; + kp->required_coordinates = "xyzabc"; + kp->allow_duplicates = 0; + kp->max_joints = strlen(kp->required_coordinates); + + *kset0 = three21KinematicsSetup; + *kfwd0 = three21KinematicsForward; + *kinv0 = three21KinematicsInverse; + + *kset1 = identityKinematicsSetup; + *kfwd1 = identityKinematicsForward; + *kinv1 = identityKinematicsInverse; + + *kset2 = userkKinematicsSetup; + *kfwd2 = userkKinematicsForward; + *kinv2 = userkKinematicsInverse; + + return 0; +} diff --git a/src/emc/kinematics/three21kins.h b/src/emc/kinematics/three21kins.h new file mode 100644 index 00000000000..4851210fd9b --- /dev/null +++ b/src/emc/kinematics/three21kins.h @@ -0,0 +1,26 @@ +#ifndef THREE21_H +#define THREE21_H + +/* default values for ar2 robot */ +#define DEFAULT_THREE21_A1 64.2 +#define DEFAULT_THREE21_A2 305.0 +#define DEFAULT_THREE21_A3 0.0 +#define DEFAULT_THREE21_D1 169.77 +#define DEFAULT_THREE21_D2 0.0 +#define DEFAULT_THREE21_D3 -6.25 +#define DEFAULT_THREE21_D4 223.63 +#define DEFAULT_THREE21_D6 36.5 + +#define SINGULAR_FUZZ 0.000001 +#define FLAG_FUZZ 0.000001 + +/* flags for inverse kinematics */ +#define THREE21_SHOULDER_RIGHT 0x01 +#define THREE21_ELBOW_DOWN 0x02 +#define THREE21_WRIST_FLIP 0x04 +#define THREE21_SINGULAR 0x08 + +/* flags for forward kinematics */ +#define THREE21_REACH 0x01 + +#endif /* THREE21_H */ From 6dca4a7398512919fd15e3ce69d6453f071ba196 Mon Sep 17 00:00:00 2001 From: joseph calderon Date: Tue, 28 Jul 2026 22:38:20 -0700 Subject: [PATCH 2/3] address pr feedback for three21kins --- docs/src/hal/components.adoc | 1 + docs/src/man/man9/kins.9.adoc | 25 +++++++- docs/src/motion/switchkins.adoc | 1 + src/emc/kinematics/three21kins.c | 99 ++++++++++++++++++++++---------- src/emc/kinematics/three21kins.h | 26 --------- 5 files changed, 95 insertions(+), 57 deletions(-) delete mode 100644 src/emc/kinematics/three21kins.h diff --git a/docs/src/hal/components.adoc b/docs/src/hal/components.adoc index 4584fce477c..6b52bdf0bf6 100644 --- a/docs/src/hal/components.adoc +++ b/docs/src/hal/components.adoc @@ -338,6 +338,7 @@ Limit its slew rate to less than maxv per second. Limit its second derivative to | link:../man/man9/rosekins.9.html[rosekins] |Kinematics for a rose engine || | link:../man/man9/rotatekins.9.html[rotatekins] |The X and Y axes are rotated 45 degrees compared to the joints 0 and 1. || | link:../man/man9/scarakins.9.html[scarakins] |Kinematics for SCARA-type robots. || +| link:../man/man9/kins.9.html[three21kins] |Analytical kinematics solver for 6-DOF arm + wrist robots. || | link:../man/man9/tripodkins.9.html[tripodkins] |The joints represent the distance of the controlled point from three predefined locations (the motors), giving three degrees of freedom in position (XYZ). || | link:../man/man9/userkins.9.html[userkins] |Template for user-built kinematics || | link:../man/man9/xyzab_tdr_kins.9.html[xyzab_tdr_kins] |Switchable kinematics for 5 axis machine with rotary table A and B|| diff --git a/docs/src/man/man9/kins.9.adoc b/docs/src/man/man9/kins.9.adoc index c6fc0ff36f0..4e354d0faef 100644 --- a/docs/src/man/man9/kins.9.adoc +++ b/docs/src/man/man9/kins.9.adoc @@ -2,7 +2,7 @@ == NAME -kins, genhexkins, genserkins, maxkins, pentakins, pumakins, rotatekins, scarakins, tripodkins, trivkins - kinematics definitions for LinuxCNC +kins, genhexkins, genserkins, maxkins, pentakins, pumakins, rotatekins, scarakins, three21kins, tripodkins, trivkins - kinematics definitions for LinuxCNC == SYNOPSIS @@ -32,6 +32,8 @@ kins, genhexkins, genserkins, maxkins, pentakins, pumakins, rotatekins, scarakin *loadrt scarakins* +*loadrt three21kins* + *loadrt tripodkins* *loadrt xyzab_tdr_kins* @@ -287,6 +289,27 @@ Kinematics for a puma-style robot with 6 joints: *pumakins.D4*:: Describe the geometry of the robot +=== three21kins - analytical kinematics solver for 6-DOF arm + wrist robots + +Kinematics for 6-DOF arm + wrist style robots: + +*three21kins.A1*:: + + +*three21kins.A2*:: + + +*three21kins.A3*:: + + +*three21kins.D1*:: + + +*three21kins.D2*:: + + +*three21kins.D3*:: + + +*three21kins.D4*:: + + +*three21kins.D6*:: + Describe the geometry of the robot + === rosekins - kinematics for a rose engine using a transverse, longitudinal, and rotary joint (3 joints) diff --git a/docs/src/motion/switchkins.adoc b/docs/src/motion/switchkins.adoc index adeecb2d1cc..bf4156f8274 100644 --- a/docs/src/motion/switchkins.adoc +++ b/docs/src/motion/switchkins.adoc @@ -41,6 +41,7 @@ The following kinematics modules support switchable kinematics: . *genhexkins* (type0:genhexkins type1:identity) . *genserkins* (type0:genserkins type1:identity) (puma560 example) . *pumakins* (type0:pumakins type1:identity) +. *three21kins* (type0:three21kins type1:identity) . *scarakins* (type0:scarakins type1:identity) . *5axiskins* (type0:5axiskins type1:identity) (bridgemill) diff --git a/src/emc/kinematics/three21kins.c b/src/emc/kinematics/three21kins.c index 658254da1df..b0b44ff7ccc 100644 --- a/src/emc/kinematics/three21kins.c +++ b/src/emc/kinematics/three21kins.c @@ -4,9 +4,30 @@ #include #include -#include "three21kins.h" #include "switchkins.h" +/* default values for ar2 robot */ +#define DEFAULT_THREE21_A1 64.2 +#define DEFAULT_THREE21_A2 305.0 +#define DEFAULT_THREE21_A3 0.0 +#define DEFAULT_THREE21_D1 169.77 +#define DEFAULT_THREE21_D2 0.0 +#define DEFAULT_THREE21_D3 -6.25 +#define DEFAULT_THREE21_D4 223.63 +#define DEFAULT_THREE21_D6 36.5 + +#define SINGULAR_FUZZ 0.000001 +#define FLAG_FUZZ 0.000001 + +/* flags for inverse kinematics */ +#define THREE21_SHOULDER_RIGHT 0x01 +#define THREE21_ELBOW_DOWN 0x02 +#define THREE21_WRIST_FLIP 0x04 +#define THREE21_SINGULAR 0x08 + +/* flags for forward kinematics */ +#define THREE21_REACH 0x01 + struct haldata { hal_float_t *a1, *a2, *a3, *d1, *d2, *d3, *d4, *d6; } *haldata = 0; @@ -26,6 +47,15 @@ static int three21KinematicsForward(const double * joint, KINEMATICS_INVERSE_FLAGS * iflags) { (void)fflags; + double a1 = THREE21_A1; + double a2 = THREE21_A2; + double a3 = THREE21_A3; + double d1 = THREE21_D1; + double d2 = THREE21_D2; + double d3 = THREE21_D3; + double d4 = THREE21_D4; + double d6 = THREE21_D6; + double s1, s2, s3, s4, s5, s6; double c1, c2, c3, c4, c5, c6; double s23; @@ -89,20 +119,20 @@ static int three21KinematicsForward(const double * joint, hom.rot.z.z = s23 * c4 * s5 - c23 * c5; /* calculate term for position vector */ - t1 = THREE21_A1 + THREE21_A2 * c2 + THREE21_A3 * c23 - THREE21_D4 * s23; + t1 = a1 + a2 * c2 + a3 * c23 - d4 * s23; /* define position vector */ - d23 = THREE21_D2 + THREE21_D3; + d23 = d2 + d3; hom.tran.x = c1 * t1 - d23 * s1; hom.tran.y = s1 * t1 + d23 * c1; - hom.tran.z = THREE21_D1 - THREE21_A3 * s23 - THREE21_A2 * s2 - THREE21_D4 * c23; + hom.tran.z = d1 - a3 * s23 - a2 * s2 - d4 * c23; /* calculate terms to determine flags */ sumSq = hom.tran.x * hom.tran.x + hom.tran.y * hom.tran.y - d23 * d23; - k = (sumSq + (hom.tran.z - THREE21_D1) * (hom.tran.z - THREE21_D1) + THREE21_A1 * THREE21_A1 - - 2.0 * THREE21_A1 * (c1 * hom.tran.x + s1 * hom.tran.y) - - THREE21_A2 * THREE21_A2 - THREE21_A3 * THREE21_A3 - THREE21_D4 * THREE21_D4) / - (2.0 * THREE21_A2); + k = (sumSq + (hom.tran.z - d1) * (hom.tran.z - d1) + a1 * a1 - + 2.0 * a1 * (c1 * hom.tran.x + s1 * hom.tran.y) - + a2 * a2 - a3 * a3 - d4 * d4) / + (2.0 * a2); /* reset flags */ *iflags = 0; @@ -115,8 +145,8 @@ static int three21KinematicsForward(const double * joint, } /* set elbow flag */ - if (fabs(joint[2]*PM_PI/180 - atan2(THREE21_A3, THREE21_D4) + - atan2(k, -sqrt(THREE21_A3 * THREE21_A3 + THREE21_D4 * THREE21_D4 - k * k))) < FLAG_FUZZ) + if (fabs(joint[2]*PM_PI/180 - atan2(a3, d4) + + atan2(k, -sqrt(a3 * a3 + d4 * d4 - k * k))) < FLAG_FUZZ) { *iflags |= THREE21_ELBOW_DOWN; } @@ -137,9 +167,9 @@ static int three21KinematicsForward(const double * joint, } /* add effect of d6 parameter */ - hom.tran.x = hom.tran.x + hom.rot.z.x*THREE21_D6; - hom.tran.y = hom.tran.y + hom.rot.z.y*THREE21_D6; - hom.tran.z = hom.tran.z + hom.rot.z.z*THREE21_D6; + hom.tran.x = hom.tran.x + hom.rot.z.x * d6; + hom.tran.y = hom.tran.y + hom.rot.z.y * d6; + hom.tran.z = hom.tran.z + hom.rot.z.z * d6; /* convert hom to pose */ pmHomPoseConvert(&hom, &worldPose); @@ -161,6 +191,15 @@ static int three21KinematicsInverse(const EmcPose * world, PmPose worldPose; PmRpy rpy; + double a1 = THREE21_A1; + double a2 = THREE21_A2; + double a3 = THREE21_A3; + double d1 = THREE21_D1; + double d2 = THREE21_D2; + double d3 = THREE21_D3; + double d4 = THREE21_D4; + double d6 = THREE21_D6; + double t1, t2, t3; double k; double sumSq; @@ -194,12 +233,12 @@ static int three21KinematicsInverse(const EmcPose * world, pmPoseHomConvert(&worldPose, &hom); /* remove effect of d6 parameter */ - px = hom.tran.x - THREE21_D6*hom.rot.z.x; - py = hom.tran.y - THREE21_D6*hom.rot.z.y; - pz = hom.tran.z - THREE21_D1 - THREE21_D6*hom.rot.z.z; + px = hom.tran.x - d6 * hom.rot.z.x; + py = hom.tran.y - d6 * hom.rot.z.y; + pz = hom.tran.z - d1 - d6 * hom.rot.z.z; /* joint 1 (2 independent solutions) */ - d23 = THREE21_D2 + THREE21_D3; + d23 = d2 + d3; sumSq = px * px + py * py - d23 * d23; if (*iflags & THREE21_SHOULDER_RIGHT) { @@ -213,17 +252,17 @@ static int three21KinematicsInverse(const EmcPose * world, c1 = cos(th1); /* joint 3 (2 independent solutions) */ - k = (sumSq + pz * pz + THREE21_A1 * THREE21_A1 - - 2.0 * THREE21_A1 * (c1 * px + s1 * py) - - THREE21_A2 * THREE21_A2 - THREE21_A3 * THREE21_A3 - THREE21_D4 * THREE21_D4) / - (2.0 * THREE21_A2); + k = (sumSq + pz * pz + a1 * a1 - + 2.0 * a1 * (c1 * px + s1 * py) - + a2 * a2 - a3 * a3 - d4 * d4) / + (2.0 * a2); if (*iflags & THREE21_ELBOW_DOWN) { - th3 = atan2(THREE21_A3, THREE21_D4) - - atan2(k, -sqrt(THREE21_A3 * THREE21_A3 + THREE21_D4 * THREE21_D4 - k * k)); + th3 = atan2(a3, d4) - + atan2(k, -sqrt(a3 * a3 + d4 * d4 - k * k)); } else { - th3 = atan2(THREE21_A3, THREE21_D4) - - atan2(k, sqrt(THREE21_A3 * THREE21_A3 + THREE21_D4 * THREE21_D4 - k * k)); + th3 = atan2(a3, d4) - + atan2(k, sqrt(a3 * a3 + d4 * d4 - k * k)); } /* compute sin, cos for later calcs */ @@ -231,11 +270,11 @@ static int three21KinematicsInverse(const EmcPose * world, c3 = cos(th3); /* joint 2 */ - t1 = (-THREE21_A3 - THREE21_A2 * c3) * pz + - (c1 * px + s1 * py - THREE21_A1) * (THREE21_A2 * s3 - THREE21_D4); - t2 = (THREE21_A2 * s3 - THREE21_D4) * pz + - (THREE21_A3 + THREE21_A2 * c3) * (c1 * px + s1 * py - THREE21_A1); - t3 = pz * pz + (c1 * px + s1 * py - THREE21_A1) * (c1 * px + s1 * py - THREE21_A1); + t1 = (-a3 - a2 * c3) * pz + + (c1 * px + s1 * py - a1) * (a2 * s3 - d4); + t2 = (a2 * s3 - d4) * pz + + (a3 + a2 * c3) * (c1 * px + s1 * py - a1); + t3 = pz * pz + (c1 * px + s1 * py - a1) * (c1 * px + s1 * py - a1); th23 = atan2(t1, t2); th2 = th23 - th3; diff --git a/src/emc/kinematics/three21kins.h b/src/emc/kinematics/three21kins.h deleted file mode 100644 index 4851210fd9b..00000000000 --- a/src/emc/kinematics/three21kins.h +++ /dev/null @@ -1,26 +0,0 @@ -#ifndef THREE21_H -#define THREE21_H - -/* default values for ar2 robot */ -#define DEFAULT_THREE21_A1 64.2 -#define DEFAULT_THREE21_A2 305.0 -#define DEFAULT_THREE21_A3 0.0 -#define DEFAULT_THREE21_D1 169.77 -#define DEFAULT_THREE21_D2 0.0 -#define DEFAULT_THREE21_D3 -6.25 -#define DEFAULT_THREE21_D4 223.63 -#define DEFAULT_THREE21_D6 36.5 - -#define SINGULAR_FUZZ 0.000001 -#define FLAG_FUZZ 0.000001 - -/* flags for inverse kinematics */ -#define THREE21_SHOULDER_RIGHT 0x01 -#define THREE21_ELBOW_DOWN 0x02 -#define THREE21_WRIST_FLIP 0x04 -#define THREE21_SINGULAR 0x08 - -/* flags for forward kinematics */ -#define THREE21_REACH 0x01 - -#endif /* THREE21_H */ From 5c5aed95a0c35814f30b9ff3364b8b40737acf19 Mon Sep 17 00:00:00 2001 From: joseph calderon Date: Tue, 28 Jul 2026 22:38:20 -0700 Subject: [PATCH 3/3] add domain clamping to three21kins for unreachable poses --- src/emc/kinematics/three21kins.c | 21 ++++++++++++++++++--- 1 file changed, 18 insertions(+), 3 deletions(-) diff --git a/src/emc/kinematics/three21kins.c b/src/emc/kinematics/three21kins.c index b0b44ff7ccc..4e7bc1fd090 100644 --- a/src/emc/kinematics/three21kins.c +++ b/src/emc/kinematics/three21kins.c @@ -129,6 +129,9 @@ static int three21KinematicsForward(const double * joint, /* calculate terms to determine flags */ sumSq = hom.tran.x * hom.tran.x + hom.tran.y * hom.tran.y - d23 * d23; + if (sumSq < 0.0) { + sumSq = 0.0; + } k = (sumSq + (hom.tran.z - d1) * (hom.tran.z - d1) + a1 * a1 - 2.0 * a1 * (c1 * hom.tran.x + s1 * hom.tran.y) - a2 * a2 - a3 * a3 - d4 * d4) / @@ -145,8 +148,12 @@ static int three21KinematicsForward(const double * joint, } /* set elbow flag */ + double discr = a3 * a3 + d4 * d4 - k * k; + if (discr < 0.0) { + discr = 0.0; + } if (fabs(joint[2]*PM_PI/180 - atan2(a3, d4) + - atan2(k, -sqrt(a3 * a3 + d4 * d4 - k * k))) < FLAG_FUZZ) + atan2(k, -sqrt(discr))) < FLAG_FUZZ) { *iflags |= THREE21_ELBOW_DOWN; } @@ -240,6 +247,9 @@ static int three21KinematicsInverse(const EmcPose * world, /* joint 1 (2 independent solutions) */ d23 = d2 + d3; sumSq = px * px + py * py - d23 * d23; + if (sumSq < 0.0) { + sumSq = 0.0; + } if (*iflags & THREE21_SHOULDER_RIGHT) { th1 = atan2(py, px) - atan2(d23, -sqrt(sumSq)); @@ -257,12 +267,17 @@ static int three21KinematicsInverse(const EmcPose * world, a2 * a2 - a3 * a3 - d4 * d4) / (2.0 * a2); + double discr = a3 * a3 + d4 * d4 - k * k; + if (discr < 0.0) { + discr = 0.0; + } + if (*iflags & THREE21_ELBOW_DOWN) { th3 = atan2(a3, d4) - - atan2(k, -sqrt(a3 * a3 + d4 * d4 - k * k)); + atan2(k, -sqrt(discr)); } else { th3 = atan2(a3, d4) - - atan2(k, sqrt(a3 * a3 + d4 * d4 - k * k)); + atan2(k, sqrt(discr)); } /* compute sin, cos for later calcs */