5-axis kinematics B axis invert rotation
12 Jan 2024 11:38 #290496
by tommy
5-axis kinematics B axis invert rotation was created by tommy
I would need a little help modifying "default" 5axiskins.c used also by configs/sim/axis/vismach/5axis/bridgemill, and there B axis rotates in opposite direction as I would need for my machine (clockwise around Y axis, looking from machine coordinate 0).
/********************************************************************
* Description: 5axiskins.c
* kinematics for XYZBC 5 axis bridge mill
*
* Derived from a work by Fred Proctor & Will Shackleford
*
* Author:
* License: GPL Version 2
* System: Linux
*
* 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)
* 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)
* wrt z axis
* 4) W axis: tool motion. Negative values increase tool radial
* motion example: drilling into body at b,c angles
* 5) W axis motion is incorporated into the motion of the
* joints used for X,Y,Z positioning and no motor or
* hal pin connections are required for the joint specified
* as JW. However, a joint must be configured for W to
* support display of the W axis letter value for
* complicated reasons. (motion/control.c computes joint
* positions only for the number of configured kinematic
* joints (NO_OF_KINS_JOINTS) and the joint positions
* are needed to display axis letters via inverse
* kinematics.
* 6) If no coordinates module parameter is supplied, kins
* will use the required coordinates XYZBCW mapped
* to joints 0..5 in sequence.
* 7) Multiple joints may be assigned to an axis letter
* with the module coordinates parameter
* 8) If a coordinates module parameter is supplied,
* the kins will map coordinate letters in sequence
* to joint numbers beginning with joint 0.
* 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
* linuxcnc.org/docs/html/gcode/machining-center.html
********************************************************************/
// non-required coordinates (A,U,V) can be set by using
// the module coordinates parameter
#define REQUIRED_COORDINATES "XYZBCW"
#define DEFAULT_PIVOT_LENGTH 250
#include "motion.h"
#include "hal.h"
#include "rtapi.h"
#include "rtapi_math.h"
#include "rtapi_string.h"
#include "rtapi_ctype.h"
#include "kinematics.h"
#include "posemath.h"
#include "switchkins.h"
struct haldata {
hal_float_t *pivot_length;
} *haldata;
static int fiveaxis_max_joints;
static PmCartesian s2r(double r, double t, double p) {
// s2r: spherical coordinates to cartesian coordinates
// r = length of vector
// p=phi = angle of vector wrt z axis
// t=theta = angle of vector projected onto xy plane
// (projection length in xy plane is r*sin(p)
PmCartesian c;
t = TO_RAD*t; p = TO_RAD*p; // degrees to radians
c.x = r * sin(p) * cos(t);
c.y = r * sin(p) * sin(t);
c.z = r * cos(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)
{
PmCartesian r = s2r(*(haldata->pivot_length) + joints[JW],
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] + *(haldata->pivot_length) + r.z;
pos->b = joints[JB];
pos->c = joints[JC];
pos->w = joints[JW];
// optional letters (specify with coordinates module parameter)
pos->a = (JA != -1)? joints[JA] : 0;
pos->u = (JU != -1)? joints[JU] : 0;
pos->v = (JV != -1)? joints[JV] : 0;
return 0;
} //fiveaxis_KinematicsForward()
static int fiveaxis_KinematicsInverse(const EmcPose * pos,
double *joints,
const KINEMATICS_INVERSE_FLAGS * iflags,
KINEMATICS_FORWARD_FLAGS * fflags)
{
PmCartesian r = s2r(*(haldata->pivot_length) + pos->w,
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 - *(haldata->pivot_length) - r.z;
P.b = pos->b;
P.c = pos->c;
P.w = pos->w;
// optional letters (specify with coordinates module parameter)
P.a = (JA != -1)? pos->a : 0;
P.u = (JU != -1)? pos->u : 0;
P.v = (JV != -1)? pos->v : 0;
// 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()
int fiveaxis_KinematicsSetup(const int comp_id,
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);
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; jno<EMCMOT_MAX_JOINTS; jno++) {
if (axis_idx_for_jno[jno] == 0) {if (JX == -1) JX=jno;}
if (axis_idx_for_jno[jno] == 1) {if (JY == -1) JY=jno;}
if (axis_idx_for_jno[jno] == 2) {if (JZ == -1) JZ=jno;}
if (axis_idx_for_jno[jno] == 3) {if (JA == -1) JA=jno;}
if (axis_idx_for_jno[jno] == 4) {if (JB == -1) JB=jno;}
if (axis_idx_for_jno[jno] == 5) {if (JC == -1) JC=jno;}
if (axis_idx_for_jno[jno] == 6) {if (JU == -1) JU=jno;}
if (axis_idx_for_jno[jno] == 7) {if (JV == -1) JV=jno;}
if (axis_idx_for_jno[jno] == 8) {if (JW == -1) JW=jno;}
}
haldata = hal_malloc(sizeof(struct haldata));
result = hal_pin_float_newf(HAL_IN,&(haldata->pivot_length),comp_id,
"%s.pivot-length",kp->halprefix);
if(result < 0) goto error;
*haldata->pivot_length = DEFAULT_PIVOT_LENGTH;
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",*haldata->pivot_length);
return 0;
error:
return -1;
} // fiveaxis_KinematicsSetup()
int switchkinsSetup(kparms* kp,
KS* kset0, KS* kset1, KS* kset2,
KF* kfwd0, KF* kfwd1, KF* kfwd2,
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;
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;
} else {
rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname);
*kset0 = fiveaxis_KinematicsSetup;
*kfwd0 = fiveaxis_KinematicsForward;
*kinv0 = fiveaxis_KinematicsInverse;
*kset1 = identityKinematicsSetup;
*kfwd1 = identityKinematicsForward;
*kinv1 = identityKinematicsInverse;
}
*kset2 = userkKinematicsSetup;
*kfwd2 = userkKinematicsForward;
*kinv2 = userkKinematicsInverse;
return 0;
} // switchkinsSetup()
Warning: Spoiler!
/********************************************************************
* Description: 5axiskins.c
* kinematics for XYZBC 5 axis bridge mill
*
* Derived from a work by Fred Proctor & Will Shackleford
*
* Author:
* License: GPL Version 2
* System: Linux
*
* 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)
* 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)
* wrt z axis
* 4) W axis: tool motion. Negative values increase tool radial
* motion example: drilling into body at b,c angles
* 5) W axis motion is incorporated into the motion of the
* joints used for X,Y,Z positioning and no motor or
* hal pin connections are required for the joint specified
* as JW. However, a joint must be configured for W to
* support display of the W axis letter value for
* complicated reasons. (motion/control.c computes joint
* positions only for the number of configured kinematic
* joints (NO_OF_KINS_JOINTS) and the joint positions
* are needed to display axis letters via inverse
* kinematics.
* 6) If no coordinates module parameter is supplied, kins
* will use the required coordinates XYZBCW mapped
* to joints 0..5 in sequence.
* 7) Multiple joints may be assigned to an axis letter
* with the module coordinates parameter
* 8) If a coordinates module parameter is supplied,
* the kins will map coordinate letters in sequence
* to joint numbers beginning with joint 0.
* 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
* linuxcnc.org/docs/html/gcode/machining-center.html
********************************************************************/
// non-required coordinates (A,U,V) can be set by using
// the module coordinates parameter
#define REQUIRED_COORDINATES "XYZBCW"
#define DEFAULT_PIVOT_LENGTH 250
#include "motion.h"
#include "hal.h"
#include "rtapi.h"
#include "rtapi_math.h"
#include "rtapi_string.h"
#include "rtapi_ctype.h"
#include "kinematics.h"
#include "posemath.h"
#include "switchkins.h"
struct haldata {
hal_float_t *pivot_length;
} *haldata;
static int fiveaxis_max_joints;
static PmCartesian s2r(double r, double t, double p) {
// s2r: spherical coordinates to cartesian coordinates
// r = length of vector
// p=phi = angle of vector wrt z axis
// t=theta = angle of vector projected onto xy plane
// (projection length in xy plane is r*sin(p)
PmCartesian c;
t = TO_RAD*t; p = TO_RAD*p; // degrees to radians
c.x = r * sin(p) * cos(t);
c.y = r * sin(p) * sin(t);
c.z = r * cos(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)
{
PmCartesian r = s2r(*(haldata->pivot_length) + joints[JW],
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] + *(haldata->pivot_length) + r.z;
pos->b = joints[JB];
pos->c = joints[JC];
pos->w = joints[JW];
// optional letters (specify with coordinates module parameter)
pos->a = (JA != -1)? joints[JA] : 0;
pos->u = (JU != -1)? joints[JU] : 0;
pos->v = (JV != -1)? joints[JV] : 0;
return 0;
} //fiveaxis_KinematicsForward()
static int fiveaxis_KinematicsInverse(const EmcPose * pos,
double *joints,
const KINEMATICS_INVERSE_FLAGS * iflags,
KINEMATICS_FORWARD_FLAGS * fflags)
{
PmCartesian r = s2r(*(haldata->pivot_length) + pos->w,
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 - *(haldata->pivot_length) - r.z;
P.b = pos->b;
P.c = pos->c;
P.w = pos->w;
// optional letters (specify with coordinates module parameter)
P.a = (JA != -1)? pos->a : 0;
P.u = (JU != -1)? pos->u : 0;
P.v = (JV != -1)? pos->v : 0;
// 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()
int fiveaxis_KinematicsSetup(const int comp_id,
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);
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; jno<EMCMOT_MAX_JOINTS; jno++) {
if (axis_idx_for_jno[jno] == 0) {if (JX == -1) JX=jno;}
if (axis_idx_for_jno[jno] == 1) {if (JY == -1) JY=jno;}
if (axis_idx_for_jno[jno] == 2) {if (JZ == -1) JZ=jno;}
if (axis_idx_for_jno[jno] == 3) {if (JA == -1) JA=jno;}
if (axis_idx_for_jno[jno] == 4) {if (JB == -1) JB=jno;}
if (axis_idx_for_jno[jno] == 5) {if (JC == -1) JC=jno;}
if (axis_idx_for_jno[jno] == 6) {if (JU == -1) JU=jno;}
if (axis_idx_for_jno[jno] == 7) {if (JV == -1) JV=jno;}
if (axis_idx_for_jno[jno] == 8) {if (JW == -1) JW=jno;}
}
haldata = hal_malloc(sizeof(struct haldata));
result = hal_pin_float_newf(HAL_IN,&(haldata->pivot_length),comp_id,
"%s.pivot-length",kp->halprefix);
if(result < 0) goto error;
*haldata->pivot_length = DEFAULT_PIVOT_LENGTH;
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",*haldata->pivot_length);
return 0;
error:
return -1;
} // fiveaxis_KinematicsSetup()
int switchkinsSetup(kparms* kp,
KS* kset0, KS* kset1, KS* kset2,
KF* kfwd0, KF* kfwd1, KF* kfwd2,
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;
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;
} else {
rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname);
*kset0 = fiveaxis_KinematicsSetup;
*kfwd0 = fiveaxis_KinematicsForward;
*kinv0 = fiveaxis_KinematicsInverse;
*kset1 = identityKinematicsSetup;
*kfwd1 = identityKinematicsForward;
*kinv1 = identityKinematicsInverse;
}
*kset2 = userkKinematicsSetup;
*kfwd2 = userkKinematicsForward;
*kinv2 = userkKinematicsInverse;
return 0;
} // switchkinsSetup()
Please Log in or Create an account to join the conversation.
12 Jan 2024 11:54 #290497
by andypugh
Replied by andypugh on topic 5-axis kinematics B axis invert rotation
What exactly is the problem? Is it that the B axis rotates in the wrong direction when commanded to move directly, or that it moves in the wrong direction when it is part of a TCP coordinated move? Or both?
Please Log in or Create an account to join the conversation.
12 Jan 2024 12:22 #290499
by tommy
Replied by tommy on topic 5-axis kinematics B axis invert rotation
You can test it in 5axis bridgemill sim, where B axis rotate/tilt opposite as image below shows for "normal" vertical machining center:
Please Log in or Create an account to join the conversation.
12 Jan 2024 12:40 #290501
by andypugh
Replied by andypugh on topic 5-axis kinematics B axis invert rotation
Yes, but, is it purely reversed, or only reversed in TCP mode?
Please Log in or Create an account to join the conversation.
13 Jan 2024 16:53 #290568
by Aciera
Replied by Aciera on topic 5-axis kinematics B axis invert rotation
I'm not familiar with the posemath library use in the bridgemill config but I've made a sim config for a CB-spindle-rotary-tilting machine that includes TCP and TOOL kinematics (this also includes an A-axis table-rotary).
Documentation is included in the README and the 'Documentation'-folder
Documentation is included in the README and the 'Documentation'-folder
Attachments:
Please Log in or Create an account to join the conversation.
13 Jan 2024 18:51 #290574
by tommy
Replied by tommy on topic 5-axis kinematics B axis invert rotation
I figured it out it is much easier for me to modify my postprocessor.
Anyway I still need to adjust #define DEFAULT_PIVOT_LENGTH 250 to different number than 250 inside kinematics file, but can't compile
Anyway I still need to adjust #define DEFAULT_PIVOT_LENGTH 250 to different number than 250 inside kinematics file, but can't compile
Compiling realtime 5axiskins.c
Linking 5axiskins.so
/usr/ld:5axiskins.ver:2 syntax error in VERSION script
collect2: error: ld returned 1 exit status
make: *** [/usr/share/linuxcnc/Makefile.modinc:125 5axiskins.so] Error 1Please Log in or Create an account to join the conversation.
- tommylight
-
- Away
- Moderator
-
Less
More
- Posts: 17877
- Thank you received: 5942
13 Jan 2024 19:48 #290577
by tommylight
Replied by tommylight on topic 5-axis kinematics B axis invert rotation
Syntax error is usually a missing or added : or ; or , or typo somewhere, not easy to pinpoint as the report does not give much to go on.
Please Log in or Create an account to join the conversation.
13 Jan 2024 20:02 #290580
by Aciera
Replied by Aciera on topic 5-axis kinematics B axis invert rotation
Looking at '5axisgui.hal' I don't think you need to change the kinematic component to change the pivot-length:
loadusr -W 5axisgui
# to set different pivot_len, use:
# 'loadusr -W 5axisgui pivot_len=value'Please Log in or Create an account to join the conversation.
14 Jan 2024 17:28 #290673
by tommy
I missed this one, it works!
Replied by tommy on topic 5-axis kinematics B axis invert rotation
loadusr -W 5axisgui pivot_len=valuePlease Log in or Create an account to join the conversation.
23 Jan 2024 19:41 #291455
by tommy
Replied by tommy on topic 5-axis kinematics B axis invert rotation
Is loading Tk vismach machine simulation window on startup caused by kinematics module or somewhere in .ini? I really don't need it anymore, and would like to turn it off.
Please Log in or Create an account to join the conversation.
Time to create page: 0.117 seconds