Hello,
i want to be able to move the Coordinate system of the Workpiece around the
Rotational axis by using custom M Codes (M122 turns it on and M123 turns it
off). I already have a theoretically working Kinematics with the help of
someone from youtube (theoretically because it wont find the pin i assigned
to change the kinematics from TCP to Trivial but when i set the normal
state to 0 (Trivial) in the Kinematics the tooltip does not follow
anymore), where i can switch between Trivial Kinematics and Tool Center
point, where the tooltip follows the same spot in the Work Coordinate
system when i rotate the rotational axis. I now want to do the same with
PLANE SPATIAL Function where it only calculates the the new position of the
work Coordinate system if i command for example A 45 C90. It must not
rotate the coordinate system, only move around the Rotational axes so XYZ
moves will always be true XYZ moves in the Machine. I made a Picture to
Explain it easier. The red Line is the X axis and also the A Rotational
Axis, the Blue is Z and also C rotational axis. Are both Kinematics Forward
and Inverse needed for this function?
Thanks for any help
--
website: http://www.machinekit.io blog: http://blog.machinekit.io github:
https://github.com/machinekit
---
You received this message because you are subscribed to the Google Groups
"Machinekit" group.
To unsubscribe from this group and stop receiving emails from it, send an email
to [email protected].
Visit this group at https://groups.google.com/group/machinekit.
For more options, visit https://groups.google.com/d/optout.
/********************************************************************
* Description: XYZACkinsTEST.c
* Kinematics for 5 axis mill named XYZAC.
* This mill has a tilting table (A axis) and horizontal rotary
* mounted to the table (C axis).
* with rotary axis offsets
*
* Author: Rudy du Preez
* License: GPL Version 2
*
********************************************************************/
#include "kinematics.h" /* these decls */
#include "posemath.h"
#include "hal.h"
#include "rtapi.h"
#include "rtapi_math.h"
#define VTVERSION VTKINEMATICS_VERSION1
struct haldata {
hal_float_t *PLANE_enable; // neu
hal_float_t *enable; // neu
hal_float_t *X_offset; // neu
hal_float_t *Y_offset;
hal_float_t *Z_offset;
hal_float_t *Tool_offset;
} *haldata;
static int kinematicsForward(const double *joints,
EmcPose * pos,
const KINEMATICS_FORWARD_FLAGS * fflags,
KINEMATICS_INVERSE_FLAGS * iflags)
{
//TCP-Funktion
double dx = *(haldata->X_offset); // neu
double dy = *(haldata->Y_offset);
double dz = *(haldata->Z_offset);
double dT = *(haldata->Tool_offset);
double a_rad = joints[3] * M_PI/180;
double c_rad = joints[5] * M_PI/180;
dz = dz + dT;
if (*(haldata->enable)==1.0){
pos->tran.x = rtapi_cos(c_rad) * (joints[0] - dx) +
rtapi_sin(c_rad) * rtapi_cos(a_rad) * (joints[1] - dy) +
rtapi_sin(c_rad) * rtapi_sin(a_rad) * (joints[2] - dz) +
rtapi_sin(c_rad) * dy;
pos->tran.y = -rtapi_sin(c_rad) * (joints[0] - dx) +
rtapi_cos(c_rad) * rtapi_cos(a_rad) * (joints[1] - dy) +
rtapi_cos(c_rad) * rtapi_sin(a_rad) * (joints[2] - dz) +
rtapi_cos(c_rad) * dy;
pos->tran.z = -rtapi_sin(a_rad) * (joints[1] - dy) +
rtapi_cos(a_rad) * (joints[2] - dz) + dz;
}else{
pos->tran.x = joints[0];
pos->tran.y = joints[1];
pos->tran.z = joints[2];
}
//pos->tran.x = rtapi_cos(c_rad) * (joints[0]) +
//rtapi_sin(c_rad) * rtapi_cos(a_rad) * (joints[1] - dy) +
//rtapi_sin(c_rad) * rtapi_sin(a_rad) * (joints[2] - dz) +
//rtapi_sin(c_rad) * dy;
//pos->tran.y = -rtapi_sin(c_rad) * (joints[0]) +
//rtapi_cos(c_rad) * rtapi_cos(a_rad) * (joints[1] - dy) +
//rtapi_cos(c_rad) * rtapi_sin(a_rad) * (joints[2] - dz) +
//rtapi_cos(c_rad) * dy;
//pos->tran.z = -rtapi_sin(a_rad) * (joints[1] - dy) +
//rtapi_cos(a_rad) * (joints[2] - dz) + dz;
pos->a = joints[3];
pos->b = joints[4];
pos->c = joints[5];
return 0;
}
static int kinematicsInverse(const EmcPose * pos,
double *joints,
const KINEMATICS_INVERSE_FLAGS * iflags,
KINEMATICS_FORWARD_FLAGS * fflags)
{
//TCP-Funktion
double dx = *(haldata->X_offset); // neu
double dy = *(haldata->Y_offset);
double dz = *(haldata->Z_offset);
double dT = *(haldata->Tool_offset);
double c_rad = pos->c * M_PI/180;
double a_rad = pos->a * M_PI/180;
dz = dz + dT;
if (*(haldata->enable)==1.0){
joints[0] = rtapi_cos(c_rad) * pos->tran.x -
rtapi_sin(c_rad) * pos->tran.y;
joints[1] = rtapi_sin(c_rad) * rtapi_cos(a_rad) * pos->tran.x +
rtapi_cos(c_rad) * rtapi_cos(a_rad) * pos->tran.y -
rtapi_sin(a_rad) * pos->tran.z -
rtapi_cos(a_rad) * dy + rtapi_sin(a_rad) * dz + dy;
joints[2] = rtapi_sin(c_rad) * rtapi_sin(a_rad) * pos->tran.x +
rtapi_cos(c_rad) * rtapi_sin(a_rad) * pos->tran.y +
rtapi_cos(a_rad) * pos->tran.z -
rtapi_sin(a_rad) * dy - rtapi_cos(a_rad) * dz + dz;
}else{
joints[0] = pos->tran.x;
joints[1] = pos->tran.y;
joints[2] = pos->tran.z;
}
//joints[0] = rtapi_cos(c_rad) * pos->tran.x -
//rtapi_sin(c_rad) * pos->tran.y;
//joints[1] = rtapi_sin(c_rad) * rtapi_cos(a_rad) * pos->tran.x +
//rtapi_cos(c_rad) * rtapi_cos(a_rad) * pos->tran.y -
//rtapi_sin(a_rad) * pos->tran.z -
//rtapi_cos(a_rad) * dy + rtapi_sin(a_rad) * dz + dy;
//joints[2] = rtapi_sin(c_rad) * rtapi_sin(a_rad) * pos->tran.x +
//rtapi_cos(c_rad) * rtapi_sin(a_rad) * pos->tran.y +
//rtapi_cos(a_rad) * pos->tran.z -
//rtapi_sin(a_rad) * dy - rtapi_cos(a_rad) * dz + dz;
joints[3] = pos->a;
joints[4] = pos->b;
joints[5] = pos->c;
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 kinematicsType()
{
return KINEMATICS_BOTH;
}
#include "rtapi.h" /* RTAPI realtime OS API */
#include "rtapi_app.h" /* RTAPI realtime module decls */
#include "hal.h"
MODULE_LICENSE("GPL");
static vtkins_t vtk = {
.kinematicsForward = kinematicsForward,
.kinematicsInverse = kinematicsInverse,
// .kinematicsHome = kinematicsHome,
.kinematicsType = kinematicsType
};
static int comp_id, vtable_id;
static const char *name = "XYZACkinsTEST";
int rtapi_app_main(void)
{
int res = 0;
comp_id = hal_init(name);
if (comp_id < 0) return comp_id;
haldata = hal_malloc(sizeof(struct haldata));
if
(((res = hal_pin_newf(HAL_FLOAT, HAL_IO, (void **) &(haldata->enable),
comp_id, "%s.XYZACkinsTEST.enable", name)) < 0) ||
((res = hal_pin_newf(HAL_FLOAT, HAL_IN, (void **) &(haldata->Tool_offset),
comp_id, "%s.Tool-offset", name)) < 0) ||
((res = hal_pin_newf(HAL_FLOAT, HAL_IN, (void **) &(haldata->X_offset),
comp_id, "%s.X-offset", name)) < 0) ||
((res = hal_pin_newf(HAL_FLOAT, HAL_IN, (void **) &(haldata->Y_offset),
comp_id, "%s.Y-offset", name)) < 0) ||
((res = hal_pin_newf(HAL_FLOAT, HAL_IN, (void **) &(haldata->Z_offset),
comp_id, "%s.Z-offset", name)) < 0))
goto error;
*(haldata->enable) = 0.0;
*(haldata->X_offset) = 0.0;
*(haldata->Y_offset) = 0.0;
*(haldata->Z_offset) = 0.0;
vtable_id = hal_export_vtable(name, VTVERSION, &vtk, comp_id);
if (vtable_id < 0) {
rtapi_print_msg(RTAPI_MSG_ERR,
"%s: ERROR: hal_export_vtable(%s,%d,%p) failed: %d\n",
name, name, VTVERSION, &vtk, vtable_id );
return -ENOENT;
}
hal_ready(comp_id);
return 0;
error:
hal_exit(comp_id);
return res;
}
void rtapi_app_exit(void) { hal_exit(comp_id); }
