Hello!
I have been trying to write my kinematics module, with extensive help
from Andy Pugh I have made some little progess and I would like to ask
anyone, who has some knowledge and experience in developing kinematics
modules, to comment on what I have done.
I have attached 2 files:
ABkins.c - the kinematics module that I am trying to develop. I took
rotatekins.c file and edited it (I have made it till line 57, so
anything below line 57 is still unedited rotatekins.c, so do not pay
attention to code below line 57).
gantrykins.c - I have attached it, because I have a feeling, that
current contents of ABkins.c is not compatible with gantry style
machine. Forward kinematics calculations do not include joint[1] (that
is the second motor on X axis).
Following lines are included to calculate tangent (obviously, more
code has to be added):
PmCartesian old;
double xy_tan = atan2(pos->tran.y - old.y,pos->tran.x - old.x);
I would appreciate, if:
1) anyone could comment, what else is necessary to be added not only
for successful tangent calculation, but also appropriate cutting head
position adjustment;
2) anyone could comment, how to make it suitable for gantry machine. I
do not mind excluding flexibility to assign any joint to any axis. It
is o.k. to hard-code, that joint 0 and 1 are X axis, joint 2 is Y,
joint 3 is Z, joint 4 is A and joint 5 is B (could be other sequence
as well).
with best regards,
Viesturs
P.S. I have started thinking that I might need to use 2 separate
configs with 2 separate kinematics modules - one for true 5 axis
machine, which is controlled by 5 axis code (tangent calculation not
required here) and second - for taper-compensated cutting with 2 axis
code (Z is not mandatory) and dynamically adjusted position of rotary
axis to compensate the taper - tangent calculation required here. I
believe that I can get the first one by removing tangent calculation
functions and some other minor changes in the second. Is that correct?
/********************************************************************
* Description: rotatekins.c
* Simple example kinematics for a rotary table in software
*
* Derived from a work by Fred Proctor & Will Shackleford
*
* Author: Chris Radek
* License: GPL Version 2
* System: Linux
*
* Copyright (c) 2006 All rights reserved.
*
********************************************************************/
#include "rtapi_math.h"
#include "kinematics.h" /* these decls */
const double head_offset = 200;
PmCartesian old;
int kinematicsForward(const double *joints,
EmcPose * pos,
const KINEMATICS_FORWARD_FLAGS * fflags,
KINEMATICS_INVERSE_FLAGS * iflags)
{
double xy_tan = atan2(pos->tran.y - old.y,pos->tran.x - old.x);
pos->tran.x = joints[0] - head_offset * sin(joints[5]);
pos->tran.y = joints[2] + head_offset * sin(joints[4]);
pos->tran.z = joints[3] + cos(joints[4]) * cos(joints[5]) * head_offset;
pos->a = joints[4];
pos->b = joints[5];
pos->c = joints[6];
pos->u = joints[7];
pos->v = joints[8];
pos->w = joints[9];
return 0;
}
int kinematicsInverse(const EmcPose * pos,
double *joints,
const KINEMATICS_INVERSE_FLAGS * iflags,
KINEMATICS_FORWARD_FLAGS * fflags)
{
double c_rad = pos->c*M_PI/180;
joints[0] = pos->tran.x + head_offset * sin(joints[5]);
joints[1] = pos->tran.x + head_offset * sin(joints[5]);
joints[2] = pos->tran.y - head_offset * sin(joints[4]);
joints[3] = pos->tran.z - cos(joints[4]) * cos(joints[5]) * head_offset;
joints[4] = pos->b;
joints[5] = pos->c;
joints[6] = pos->u;
joints[7] = pos->v;
joints[8] = pos->w;
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;
return kinematicsForward(joint, world, fflags, iflags);
}
KINEMATICS_TYPE kinematicsType()
{
return KINEMATICS_BOTH;
}
#include "rtapi.h" /* RTAPI realtime OS API */
#include "rtapi_app.h" /* RTAPI realtime module decls */
#include "hal.h"
EXPORT_SYMBOL(kinematicsType);
EXPORT_SYMBOL(kinematicsForward);
EXPORT_SYMBOL(kinematicsInverse);
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;
}
return comp_id;
}
void rtapi_app_exit(void) { hal_exit(comp_id); }
/********************************************************************
* Description: gantrykins.c
* Gantry (ganged joint) kinematics for 3 axis Cartesian machine
*
* Derived from a work by Fred Proctor & Will Shackleford
*
* License: GPL Version 2
*
* Copyright (c) 2006 All rights reserved.
*
********************************************************************/
#include "motion.h" /* these decls */
#include "hal.h"
#include "rtapi.h"
#include "rtapi_math.h"
#include "rtapi_string.h"
struct data {
hal_s32_t joints[EMCMOT_MAX_JOINTS];
} *data;
#define SET(f) pos->f = joints[i]
int kinematicsForward(const double *joints,
EmcPose * pos,
const KINEMATICS_FORWARD_FLAGS * fflags,
KINEMATICS_INVERSE_FLAGS * iflags)
{
int i;
for(i=0; i<9; i++) {
switch(data->joints[i]) {
case 0: SET(tran.x); break;
case 1: SET(tran.y); break;
case 2: SET(tran.z); break;
case 3: SET(a); break;
case 4: SET(b); break;
case 5: SET(c); break;
case 6: SET(u); break;
case 7: SET(v); break;
case 8: SET(w); break;
}
}
return 0;
}
int kinematicsInverse(const EmcPose * pos,
double *joints,
const KINEMATICS_INVERSE_FLAGS * iflags,
KINEMATICS_FORWARD_FLAGS * fflags)
{
int i;
for(i=0; i<9; i++) {
switch(data->joints[i]) {
case 0: joints[i] = pos->tran.x; break;
case 1: joints[i] = pos->tran.y; break;
case 2: joints[i] = pos->tran.z; break;
case 3: joints[i] = pos->a; break;
case 4: joints[i] = pos->b; break;
case 5: joints[i] = pos->c; break;
case 6: joints[i] = pos->u; break;
case 7: joints[i] = pos->v; break;
case 8: joints[i] = pos->w; break;
}
}
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;
return kinematicsForward(joint, world, fflags, iflags);
}
KINEMATICS_TYPE kinematicsType()
{
return KINEMATICS_BOTH;
}
#include "rtapi.h" /* RTAPI realtime OS API */
#include "rtapi_app.h" /* RTAPI realtime module decls */
#include "hal.h"
char *coordinates = "XYZABC";
RTAPI_MP_STRING(coordinates, "Mapping from axes to joints");
EXPORT_SYMBOL(kinematicsType);
EXPORT_SYMBOL(kinematicsForward);
EXPORT_SYMBOL(kinematicsInverse);
MODULE_LICENSE("GPL");
static int next_axis_number(void) {
while(*coordinates) {
switch(*coordinates) {
case 'x': case 'X': coordinates++; return 0;
case 'y': case 'Y': coordinates++; return 1;
case 'z': case 'Z': coordinates++; return 2;
case 'a': case 'A': coordinates++; return 3;
case 'b': case 'B': coordinates++; return 4;
case 'c': case 'C': coordinates++; return 5;
case 'u': case 'U': coordinates++; return 6;
case 'v': case 'V': coordinates++; return 7;
case 'w': case 'W': coordinates++; return 8;
case ' ': case '\t': coordinates++; continue;
}
rtapi_print_msg(RTAPI_MSG_ERR,
"GANTRYKINS: ERROR: Invalid character '%c' in coordinates",
*coordinates);
}
return -1;
}
int comp_id;
int rtapi_app_main(void) {
int result, i;
comp_id = hal_init("gantrykins");
if(comp_id < 0) return comp_id;
data = hal_malloc(sizeof(struct data));
for(i=0; i<EMCMOT_MAX_JOINTS; i++) {
result = hal_param_s32_newf(HAL_RW, &(data->joints[i]), comp_id,
"gantrykins.joint-%d", i);
if(result < 0) goto error;
data->joints[i] = next_axis_number();
rtapi_print_msg(RTAPI_MSG_ERR,
"GANTRYKINS: joints[%d] = %d\n", i, data->joints[i]);
}
hal_ready(comp_id);
return 0;
error:
hal_exit(comp_id);
return result;
}
void rtapi_app_exit(void) { hal_exit(comp_id); }
------------------------------------------------------------------------------
ThinkGeek and WIRED's GeekDad team up for the Ultimate
GeekDad Father's Day Giveaway. ONE MASSIVE PRIZE to the
lucky parental unit. See the prize list and enter to win:
http://p.sf.net/sfu/thinkgeek-promo
_______________________________________________
Emc-users mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/emc-users
