5-axis kinematics B axis invert rotation
- tommy
- Offline
- Premium Member
Less
More
- Posts: 107
- Thank you received: 1
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.
- andypugh
- Offline
- Moderator
Less
More
- Posts: 23310
- Thank you received: 4858
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.
- tommy
- Offline
- Premium Member
Less
More
- Posts: 107
- Thank you received: 1
12 Jan 2024 12:22 #290499
by tommy
Replied by tommy on topic 5-axis kinematics B axis invert rotation
Please Log in or Create an account to join the conversation.
- andypugh
- Offline
- Moderator
Less
More
- Posts: 23310
- Thank you received: 4858
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.
- Aciera
- Offline
- Administrator
Less
More
- Posts: 3958
- Thank you received: 1714
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.
- tommy
- Offline
- Premium Member
Less
More
- Posts: 107
- Thank you received: 1
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 1
Please Log in or Create an account to join the conversation.
- tommylight
- Away
- Moderator
Less
More
- Posts: 19419
- Thank you received: 6512
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.
- Aciera
- Offline
- Administrator
Less
More
- Posts: 3958
- Thank you received: 1714
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.
- tommy
- Offline
- Premium Member
Less
More
- Posts: 107
- Thank you received: 1
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=value
Please Log in or Create an account to join the conversation.
- tommy
- Offline
- Premium Member
Less
More
- Posts: 107
- Thank you received: 1
23 Jan 2024 19:41 #291455
by tommy
Replied by tommy on topic 5-axis kinematics B axis invert rotation
Please Log in or Create an account to join the conversation.
Time to create page: 0.091 seconds