L
laidfroid
Apprenti
?Bonjour.
Voici un problème que je n'arrive pas à résoudre.
Ma config : ubuntu 10.04
linuxcnc 2.5.1
linuxcnc-dev
La configuration machine à été crée par stepconf et ce lance sans problème.
étant intéressé par la réalisation d'un tripode delta, j'ai téléchargé le logiciel « deltakins » écrit en langage C
crée par hg5bsd pour piloté cette machine sous linuxcnc.
#include "kinematics.h" /* these decls */
#include "rtapi_math.h"
double e = 115.0;
double f = 457.3;
double re = 232.0;
double 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, a, b, d, yj, zj;
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*/
a = (x0*x0 + y0*y0 + z0*z0 +delta_rf*delta_rf - delta_re*delta_re - y1*y1)/(2*z0);
b = (y1-y0)/z0;
/* discriminant*/
d = -(a+b*y1)*(a+b*y1)+delta_rf*(b*b*delta_rf+delta_rf);
if (d < 0) return -1; /* non-existing point*/
yj = (y1 - a*b - sqrt(d))/(b*b + 1); /* choosing outer point*/
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;
int status;
theta1 = theta2 = theta3 = 0;
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)) != 0) goto error;
if((res = hal_pin_float_new("deltakins.f", HAL_IO, &(haldata->f), comp_id)) != 0) goto error;
if((res = hal_pin_float_new("deltakins.re", HAL_IO, &(haldata->re), comp_id)) != 0) goto error;
if((res = hal_pin_float_new("deltakins.rf", HAL_IO, &(haldata->rf), comp_id)) != 0) 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
La compilation avec la commande « sudo comp --install deltakins.c » c'est dérouler sans problème et le fichier « deltakins.ko » à été généré sans erreur.
Par contre quand dans custom.hal je mets « loadrt deltakins » linuxcnc ce bloque et affiche le message
d'erreur que voici.
message d'erreur de linuxcnc
Tous les renseignements que j'ai pu glané sur la toile ne mon rien apportés, le problème persiste
linuxcnc refuse cette commande, Je ne comprend pas le pourquoi de cette erreur ?
Si un génie de linuxcnc pouvait m'indiquer ou j'ai faux je lui en serais très reconnaissant.
D'avance merci. J.P.
Voici un problème que je n'arrive pas à résoudre.
Ma config : ubuntu 10.04
linuxcnc 2.5.1
linuxcnc-dev
La configuration machine à été crée par stepconf et ce lance sans problème.
étant intéressé par la réalisation d'un tripode delta, j'ai téléchargé le logiciel « deltakins » écrit en langage C
crée par hg5bsd pour piloté cette machine sous linuxcnc.
#include "kinematics.h" /* these decls */
#include "rtapi_math.h"
double e = 115.0;
double f = 457.3;
double re = 232.0;
double 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, a, b, d, yj, zj;
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*/
a = (x0*x0 + y0*y0 + z0*z0 +delta_rf*delta_rf - delta_re*delta_re - y1*y1)/(2*z0);
b = (y1-y0)/z0;
/* discriminant*/
d = -(a+b*y1)*(a+b*y1)+delta_rf*(b*b*delta_rf+delta_rf);
if (d < 0) return -1; /* non-existing point*/
yj = (y1 - a*b - sqrt(d))/(b*b + 1); /* choosing outer point*/
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;
int status;
theta1 = theta2 = theta3 = 0;
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)) != 0) goto error;
if((res = hal_pin_float_new("deltakins.f", HAL_IO, &(haldata->f), comp_id)) != 0) goto error;
if((res = hal_pin_float_new("deltakins.re", HAL_IO, &(haldata->re), comp_id)) != 0) goto error;
if((res = hal_pin_float_new("deltakins.rf", HAL_IO, &(haldata->rf), comp_id)) != 0) 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
La compilation avec la commande « sudo comp --install deltakins.c » c'est dérouler sans problème et le fichier « deltakins.ko » à été généré sans erreur.
Par contre quand dans custom.hal je mets « loadrt deltakins » linuxcnc ce bloque et affiche le message
d'erreur que voici.
Tous les renseignements que j'ai pu glané sur la toile ne mon rien apportés, le problème persiste
linuxcnc refuse cette commande, Je ne comprend pas le pourquoi de cette erreur ?
Si un génie de linuxcnc pouvait m'indiquer ou j'ai faux je lui en serais très reconnaissant.
D'avance merci. J.P.
