diff --git a/configs/sim/axis/vismach/5axis/bridgemill/5axisgui.hal b/configs/sim/axis/vismach/5axis/bridgemill/5axisgui.hal index d86238d477b..0c222cd251b 100644 --- a/configs/sim/axis/vismach/5axis/bridgemill/5axisgui.hal +++ b/configs/sim/axis/vismach/5axis/bridgemill/5axisgui.hal @@ -13,20 +13,12 @@ net :jy2 joint.6.pos-fb # Note: no joint or motor connections are used # for the w coordinate -# compute pivot-length needed for 5axiskins -# according to gui pivot_len and tool z offset -loadrt sum2 names=pivotsum -addf pivotsum servo-thread - net :gui-pivot-len <= 5axisgui.pivot_len -net :gui-pivot-len => pivotsum.in0 +net :gui-pivot-len => 5axiskins.pivot-length net :tool-len <= motion.tooloffset.z -net :tool-len => pivotsum.in1 +net :tool-len => 5axiskins.tool-length net :tool-len => 5axisgui.tool_length -net :pivot-len <= pivotsum.out -net :pivot-len => 5axiskins.pivot-length - net :tool-diam <= halui.tool.diameter net :tool-diam => 5axisgui.tool_diam diff --git a/configs/sim/axis/vismach/5axis/max5/max5kins.hal b/configs/sim/axis/vismach/5axis/max5/max5kins.hal index be4e668b1de..78deb36d013 100644 --- a/configs/sim/axis/vismach/5axis/max5/max5kins.hal +++ b/configs/sim/axis/vismach/5axis/max5/max5kins.hal @@ -15,9 +15,10 @@ loadrt not count=5 loadusr -W max5gui # set a visible tool -#setp max5gui.tool-length 35 setp max5gui.tool-radius 3 -net tool-len motion.tooloffset.w max5gui.tool-length +# 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 # 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 4e354d0faef..f2bd1051d4c 100644 --- a/docs/src/man/man9/kins.9.adoc +++ b/docs/src/man/man9/kins.9.adoc @@ -240,6 +240,22 @@ tilting head (B axis) and horizontal rotary mounted to the table (C axis). Provides UVW motion in the rotated coordinate system. The source file, maxkins.c, may be a useful starting point for other 5-axis systems. +The W coordinate is added to the pivot to tip distance, so a quill or +tool slide programmed on W extends the tool and the B correction follows +it. This is the same treatment 5axiskins gives W. A tool length in the W +column of the tool table therefore reaches the kinematics too, once a +block commands W, and it adds to *maxkins.tool-length* rather than +replacing it. Put a given length in one column or the other, not both. + +*maxkins.pivot-length*:: + Distance from the B rotation point to the tool holder gauge line. + +*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. + === pentakins - Pentapod Kinematics Gives five degrees of freedom in position and orientation (XYZAB). @@ -392,6 +408,18 @@ Note: These kinematics may be used with the vismach 5axisgui providing that the joint-letter assignments agree with the default ordering expected by it (XYZBCW `->` joints 0..5) +*5axiskins.pivot-length*:: + Distance from the B rotation point to the tool holder gauge line. + +*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. + == SEE ALSO For additional information, see following subsections of the section 'Advanced Topics' of the LinuxCNC documentation: diff --git a/src/emc/kinematics/5axiskins.c b/src/emc/kinematics/5axiskins.c index 387c32e23df..bd914cad897 100644 --- a/src/emc/kinematics/5axiskins.c +++ b/src/emc/kinematics/5axiskins.c @@ -65,6 +65,7 @@ static struct haldata { hal_real_t pivot_length; + hal_real_t tool_length; } *haldata; static int fiveaxis_max_joints; @@ -104,14 +105,15 @@ static int fiveaxis_KinematicsForward(const double *joints, (void)fflags; (void)iflags; rtapi_real pivot_length = hal_get_real(haldata->pivot_length); - PmCartesian r = s2r(pivot_length + joints[JW], + rtapi_real tool_length = hal_get_real(haldata->tool_length); + PmCartesian r = s2r(pivot_length + joints[JW] + tool_length, joints[JC], 180.0 - joints[JB]); // Note: 'principal' joints are used pos->tran.x = joints[JX] + r.x; pos->tran.y = joints[JY] + r.y; - pos->tran.z = joints[JZ] + pivot_length + r.z; + pos->tran.z = joints[JZ] + pivot_length + tool_length + r.z; pos->b = joints[JB]; pos->c = joints[JC]; pos->w = joints[JW]; @@ -132,14 +134,15 @@ static int fiveaxis_KinematicsInverse(const EmcPose * pos, (void)iflags; (void)fflags; rtapi_real pivot_length = hal_get_real(haldata->pivot_length); - PmCartesian r = s2r(pivot_length + pos->w, + rtapi_real tool_length = hal_get_real(haldata->tool_length); + PmCartesian r = s2r(pivot_length + pos->w + tool_length, pos->c, 180.0 - pos->b); EmcPose P; // computed position P.tran.x = pos->tran.x - r.x; P.tran.y = pos->tran.y - r.y; - P.tran.z = pos->tran.z - pivot_length - r.z; + P.tran.z = pos->tran.z - pivot_length - tool_length - r.z; P.b = pos->b; P.c = pos->c; @@ -219,6 +222,10 @@ int fiveaxis_KinematicsSetup(const int comp_id, 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" diff --git a/src/emc/kinematics/maxkins.c b/src/emc/kinematics/maxkins.c index edd242206d7..773525b3566 100644 --- a/src/emc/kinematics/maxkins.c +++ b/src/emc/kinematics/maxkins.c @@ -31,6 +31,7 @@ static struct haldata { hal_real_t pivot_length; + hal_real_t tool_length; hal_bool_t conventional_directions; //default is false } *haldata; @@ -44,10 +45,11 @@ int kinematicsForward(const double *joints, 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); // B correction - const double zb = (pivot_length + joints[8]) * cos(d2r(joints[4])); - const double xb = (pivot_length + joints[8]) * sin(d2r(joints[4])); + 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])); // C correction const double xyr = hypot(joints[0], joints[1]); @@ -61,7 +63,7 @@ int kinematicsForward(const double *joints, pos->tran.x = xyr * cos(xytheta) - (con * xb) - xv; pos->tran.y = xyr * sin(xytheta) - joints[7]; - pos->tran.z = joints[2] - zb - (con * zv) + pivot_length; + pos->tran.z = joints[2] - zb - (con * zv) + pivot_length + tool_length; pos->a = joints[3]; pos->b = joints[4]; @@ -83,10 +85,11 @@ int kinematicsInverse(const EmcPose * pos, 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); // B correction - const double zb = (pivot_length + pos->w) * cos(d2r(pos->b)); - const double xb = (pivot_length + pos->w) * sin(d2r(pos->b)); + 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)); // C correction const double xyr = hypot(pos->tran.x, pos->tran.y); @@ -100,7 +103,7 @@ int kinematicsInverse(const EmcPose * pos, joints[0] = xyr * cos(xytheta) + (con * xb) + xv; joints[1] = xyr * sin(xytheta) + pos->v; - joints[2] = pos->tran.z + zb - (con * zv) - pivot_length; + joints[2] = pos->tran.z + zb - (con * zv) - pivot_length - tool_length; joints[3] = pos->a; joints[4] = pos->b; @@ -133,6 +136,7 @@ int rtapi_app_main(void) { 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");