Kinematics issue with master branch (2.8 pre1 2959)
07 Apr 2017 02:29 #90986
by cts1085
Kinematics issue with master branch (2.8 pre1 2959) was created by cts1085
I just switched over to the development branch from 2.7.8 and the traj planner is much better. However, I may have found an issue.
Within my kins modules i am leveraging the pos->tran.z value and when a 'Z' tool offset is established the pos->tran.z value does not include the offset even after a G0 Zx command.
Does this make sense?
Within my kins modules i am leveraging the pos->tran.z value and when a 'Z' tool offset is established the pos->tran.z value does not include the offset even after a G0 Zx command.
Does this make sense?
/********************************************************************
* Description: XZABkins.c
* Derived from trivkins for 3 axis Cartesian machine
* Established for 4-axis lathe (XZAB) where A rotates around X in Z-Yplane and B rotates around Y in Z-Xplane
*
* Derived from a work by Fred Proctor & Will Shackleford
*
* License: GPL Version 2
*
* Copyright (c) 2009 All rights reserved.
*
********************************************************************/
#include "motion.h"
#include "posemath.h"
#include "hal.h"
#include "rtapi.h"
#include "rtapi.h" /* RTAPI realtime OS API */
#include "rtapi_app.h" /* RTAPI realtime module decls */
#include "rtapi_math.h"
#include "rtapi_string.h"
struct haldata {
hal_float_t *Tool_offset;
hal_s32_t *Tool_number;
hal_float_t *J0;
hal_float_t *J1;
hal_float_t *J3;
hal_float_t *Px;
hal_float_t *Pz;
hal_float_t *Pb;
hal_float_t *FwdRad;
hal_float_t *InvRad;
} *haldata;
hal_float_t Last_Tool_offset = 0.00;
hal_s32_t Last_Tool_number = 0;
int kinematicsForward(const double *joints,
EmcPose * pos,
const KINEMATICS_FORWARD_FLAGS * fflags,
KINEMATICS_INVERSE_FLAGS * iflags)
{
double b_rad;
double dT = *(haldata->Tool_offset);
long lTN = *(haldata->Tool_number);
double b_adj = 0.00;
if (lTN == 100 && dT != 0.00) {
b_adj = 90.00;
}
b_rad = (joints[3] + b_adj)*TO_RAD; /* B-axis offset */
pos->tran.x = joints[0] - (dT * sin(b_rad));
pos->tran.z = joints[1] - (dT * cos(b_rad)) + dT;
pos->a = joints[2];
pos->b = joints[3] + b_adj ;
*(haldata->Px) = pos->tran.x;
*(haldata->Pz) = pos->tran.z;
*(haldata->Pb) = pos->b;
*(haldata->InvRad) = b_rad;
return 0;
}
int kinematicsInverse(const EmcPose * pos,
double *joints,
const KINEMATICS_INVERSE_FLAGS * iflags,
KINEMATICS_FORWARD_FLAGS * fflags)
{
double b_rad;
double dT = *(haldata->Tool_offset);
long lTN = *(haldata->Tool_number);
double b_adj = 0.00;
if (lTN == 100 && dT != 0.00) {
b_adj = 90.00;
}
b_rad = (pos->b)*TO_RAD; /* B-axis offset */
joints[0] = pos->tran.x + (dT * sin(b_rad));
joints[1] = (pos->tran.z - dT) + (dT * cos(b_rad)) ;
joints[2] = pos->a;
joints[3] = pos->b - b_adj;
*(haldata->J0) = joints[0];
*(haldata->J1) = joints[1];
*(haldata->J3) = joints[3];
*(haldata->FwdRad) = b_rad;
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);
}
static KINEMATICS_TYPE ktype = -1;
KINEMATICS_TYPE kinematicsType()
{
return ktype;
}
static char *kinstype = "b"; // use KINEMATICS_BOTH
RTAPI_MP_STRING(kinstype, "Kinematics Type (Identity,Both)");
EXPORT_SYMBOL(kinematicsType);
EXPORT_SYMBOL(kinematicsForward);
EXPORT_SYMBOL(kinematicsInverse);
MODULE_LICENSE("GPL");
int comp_id;
int rtapi_app_main(void) {
int res = 0;
comp_id = hal_init("XZABkins");
if(comp_id < 0) return comp_id;
haldata = hal_malloc(sizeof(struct haldata));
switch (*kinstype) {
case 'b': case 'B': ktype = KINEMATICS_BOTH; break;
case 'f': case 'F': ktype = KINEMATICS_FORWARD_ONLY; break;
case 'i': case 'I': ktype = KINEMATICS_INVERSE_ONLY; break;
case '1': default: ktype = KINEMATICS_IDENTITY;
}
if((res = hal_pin_float_new("XZABkins.Tool-offset", HAL_IN, &(haldata->Tool_offset),
comp_id)) < 0) goto error;
if((res = hal_pin_s32_new("XZABkins.Tool-number", HAL_IN, &(haldata->Tool_number),
comp_id)) < 0) goto error;
if((res = hal_pin_float_new("XZABkins.Joint0", HAL_OUT, &(haldata->J0),
comp_id)) < 0) goto error;
if((res = hal_pin_float_new("XZABkins.Joint1", HAL_OUT, &(haldata->J1),
comp_id)) < 0) goto error;
if((res = hal_pin_float_new("XZABkins.Joint3", HAL_OUT, &(haldata->J3),
comp_id)) < 0) goto error;
if((res = hal_pin_float_new("XZABkins.PosX", HAL_OUT, &(haldata->Px),
comp_id)) < 0) goto error;
if((res = hal_pin_float_new("XZABkins.PosZ", HAL_OUT, &(haldata->Pz),
comp_id)) < 0) goto error;
if((res = hal_pin_float_new("XZABkins.PosB", HAL_OUT, &(haldata->Pb),
comp_id)) < 0) goto error;
if((res = hal_pin_float_new("XZABkins.FwdRad", HAL_OUT, &(haldata->FwdRad),
comp_id)) < 0) goto error;
if((res = hal_pin_float_new("XZABkins.InvRad", HAL_OUT, &(haldata->InvRad),
comp_id)) < 0) goto error;
hal_ready(comp_id);
return 0;
error:
hal_exit(comp_id);
return res;
}
void rtapi_app_exit(void) { hal_exit(comp_id); }
Please Log in or Create an account to join the conversation.
07 Apr 2017 12:49 #91001
by cts1085
Replied by cts1085 on topic Kinematics issue with master branch (2.8 pre1 2959)
With 2.8 if using a Kinstype of KINEMATICS_BOTH vs KINEMATICS_IDENTITY is the EmcPos populated differently?
Please Log in or Create an account to join the conversation.
- tommylight
- Away
- Moderator
Less
More
- Posts: 18712
- Thank you received: 6286
07 Apr 2017 13:28 #91005
by tommylight
Replied by tommylight on topic Kinematics issue with master branch (2.8 pre1 2959)
You should ask that on the IRC as you might get a quicker response.
The link is on the main page.
Regards
The link is on the main page.
Regards
Please Log in or Create an account to join the conversation.
07 Apr 2017 14:14 #91007
by cts1085
Replied by cts1085 on topic Kinematics issue with master branch (2.8 pre1 2959)
Resolved - this was my confusion - the EmcPos being sent to Kins is in agreement with the machine coordinates displayed in Axis - this is the correct information. Thank you for your patience!
Please Log in or Create an account to join the conversation.
Time to create page: 0.181 seconds