Kod: Zaznacz cały
/********************************************************************
* Description: deltakins.c
* Kinematics for 3 axis Delta machine
* Derived from a work of mzavatsky at Trossen Robotics
* at forums.trossenrobotics.com
* Author: Jozsef Papp / hg5bsd
* and: István Ábel at kvarc.extra.hu
* License: coding and fitting it into EMC2 from our part is GPL alike
* but we won't place the code itself under GPL because we
* don't know if this would hurt or not the delta robot
* inventors rights, if you are shure that it doesn't hurt,
* then you are free to place it under GPL ( in this case place
* your name in the comment lines, and keep original comments )
* System: realtime Linux, EMC2
* Copyright (c) 2010 All rights reserved.
* Last change: 2010.07.14.14.28.
********************************************************************/
#include "kinematics.h" /* these decls */
#include "rtapi_math.h"
/* robot geometry /Ref - mzavatsky: Delta robot kinematics/
e = side length of end effector triangle, middle arm - "re"
f = side length of base triangle, middle drive joints - "rf"
re = length of end effector arm
rf = length of drive arm
sample:
e = 115.0;
f = 457.3;
re = 232.0;
rf = 112.0; */
#ifdef RTAPI
#include "rtapi.h" /* RTAPI realtime OS API */
#include "rtapi_app.h" /* RTAPI realtime module decls */
#include "hal.h"
struct haldata {
hal_float_t *e, *f, *re, *rf;
} *haldata = 0;
#define delta_e (*(haldata->e))
#define delta_f (*(haldata->f))
#define delta_re (*(haldata->re))
#define delta_rf (*(haldata->rf))
#else
double delta_e, delta_f, delta_re, delta_rf;
#endif
/* trigonometric constants */
const double sqrt3 = 1.7320508075688772935274463415059; /* sqrt(3.0);*/
#ifndef PI
#define PI 3.14159265358979323846
#endif
const double sin120 = 0.86602540378443864676372317075294; /* (sqrt3)/2;*/
const double cos120 = -0.5;
const double tan60 = 1.7320508075688772935274463415059; /* sqrt3;*/
const double sin30 = 0.5;
const double tan30 = 0.57735026918962576450914878050196; /* 1/(sqrt3);*/
/* forward kinematics: (joints[0], joints[1], joints[2]) -> (pos->tran.x, pos->tran.y, pos->tran.z)
// returned status: 0=OK, -1=non-existing position*/
int kinematicsForward( const double *joints,
EmcPose *pos,
const KINEMATICS_FORWARD_FLAGS *fflags,
KINEMATICS_INVERSE_FLAGS *iflags)
{
double t = (delta_f-delta_e)*tan30/2;
/* float dtr = pi/(float)180.0; TO_RAD */
double theta1 = joints[0] * TO_RAD;
double theta2 = joints[1] * TO_RAD;
double theta3 = joints[2] * TO_RAD;
double y1 = -(t + delta_rf*cos(theta1));
double z1 = -delta_rf*sin(theta1);
double y2 = (t + delta_rf*cos(theta2))*sin30;
double x2 = y2*tan60;
double z2 = -delta_rf*sin(theta2);
double y3 = (t + delta_rf*cos(theta3))*sin30;
double x3 = -y3*tan60;
double z3 = -delta_rf*sin(theta3);
double dnm = (y2-y1)*x3-(y3-y1)*x2;
double w1 = y1*y1 + z1*z1;
double w2 = x2*x2 + y2*y2 + z2*z2;
double w3 = x3*x3 + y3*y3 + z3*z3;
/* x = (a1*z + b1)/dnm*/
double a1 = (z2-z1)*(y3-y1)-(z3-z1)*(y2-y1);
double b1 = -((w2-w1)*(y3-y1)-(w3-w1)*(y2-y1))/2.0;
/* y = (a2*z + b2)/dnm;*/
double a2 = -(z2-z1)*x3+(z3-z1)*x2;
double b2 = ((w2-w1)*x3 - (w3-w1)*x2)/2.0;
/* a*z^2 + b*z + c = 0*/
double a = a1*a1 + a2*a2 + dnm*dnm;
double b = 2*(a1*b1 + a2*(b2-y1*dnm) - z1*dnm*dnm);
double c = (b2-y1*dnm)*(b2-y1*dnm) + b1*b1 + dnm*dnm*(z1*z1 - delta_re*delta_re);
/* discriminant*/
double d = b*b - (double)4.0*a*c;
if (d < 0) return -1; /* non-existing point*/
pos->tran.z = -(double)0.5*(b+sqrt(d))/a;
pos->tran.x = (a1*pos->tran.z + b1)/dnm;
pos->tran.y = (a2*pos->tran.z + b2)/dnm;
return 0;
}
/* inverse kinematics
helper functions, calculates angle theta1 (for YZ-pane)*/
int delta_calcAngleYZ( double x0, double y0, double z0, double *theta )
{
double y1 = -0.5 * 0.57735 * delta_f; // f/2 * tg 30
y0 -= 0.5 * 0.57735 * delta_e; // shift center to edge
/* z = a + b*y*/
double a = (x0*x0 + y0*y0 + z0*z0 +delta_rf*delta_rf - delta_re*delta_re - y1*y1)/(2*z0);
double b = (y1-y0)/z0;
/* discriminant*/
double d = -(a+b*y1)*(a+b*y1)+delta_rf*(b*b*delta_rf+delta_rf);
if (d < 0) return -1; /* non-existing point*/
double yj = (y1 - a*b - sqrt(d))/(b*b + 1); /* choosing outer point*/
double zj = a + b*yj;
*theta = TO_DEG * atan2(-zj,(y1 - yj)) + ((yj>y1)?180.0:0.0);
return 0;
}
/* inverse kinematics: (pos->tran.x, pos->tran.y, pos->tran.z) -> (joints[0], joints[1], joints[2])
returned status: 0=OK, -1=non-existing position*/
int kinematicsInverse(const EmcPose *pos,
double *joints,
const KINEMATICS_INVERSE_FLAGS *iflags,
KINEMATICS_FORWARD_FLAGS *fflags)
{
double x0 = pos->tran.x;
double y0 = pos->tran.y;
double z0 = pos->tran.z;
double theta1;
double theta2;
double theta3;
theta1 = theta2 = theta3 = 0;
int status = delta_calcAngleYZ(x0, y0, z0, &theta1);
if (status == 0) joints[0] = theta1;
if (status == 0) status = delta_calcAngleYZ(x0*cos120 + y0*sin120, y0*cos120-x0*sin120, z0, &theta2); /*rotate coords to +120 deg*/
if (status == 0) joints[1] = theta2;
if (status == 0) status = delta_calcAngleYZ(x0*cos120 - y0*sin120, y0*cos120+x0*sin120, z0, &theta3); /*rotate coords to -120 deg*/
if (status == 0) joints[2] = theta3;
return status;
}
/* 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;
}
#ifdef RTAPI
#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) {
int res = 0;
comp_id = hal_init("deltakins");
if(comp_id < 0) return comp_id;
haldata = hal_malloc(sizeof(struct haldata));
if(!haldata) goto error;
if((res = hal_pin_float_new("deltakins.e", HAL_IO, &(haldata->e), comp_id)) != HAL_SUCCESS) goto error;
if((res = hal_pin_float_new("deltakins.f", HAL_IO, &(haldata->f), comp_id)) != HAL_SUCCESS) goto error;
if((res = hal_pin_float_new("deltakins.re", HAL_IO, &(haldata->re), comp_id)) != HAL_SUCCESS) goto error;
if((res = hal_pin_float_new("deltakins.rf", HAL_IO, &(haldata->rf), comp_id)) != HAL_SUCCESS) goto error;
delta_e = delta_f = delta_re = delta_rf = 1.0;
hal_ready(comp_id);
return 0;
error:
hal_exit(comp_id);
return res;
}
void rtapi_app_exit(void) { hal_exit(comp_id); }
#endif
może popróbowalibyśmy wyprowadzić na nowo równania tam zawarte?... takie małe zadanie z geometrii...
