Настройка кинематики в EMC2

[sasha_89]

День добрый! помогите пожалуйста написать параметры в EMC2 для робота чтоб сам считало углы между осями чтоб нарисовать линию! вот сам робот
и вот зона обслуживание

[PKM]

Формулы кинематики есть?

[sasha_89]

что то так -

x0 = 0
y0 = L0
cos(psi) = x1/L1 => x1 = L1*cos(psi)
sin(psi) = y1/L1 => y1 = L1*sin(psi)
sin(theta) = x2/L2 => x2 = L2*sin(theta)
cos(theta) = y2/L2 => y2 = L2*cos(theta)
x0 + x1 + x2, or 0 + L1*cos(psi) + L2*sin(theta)
y0 + y1 + y2, or L0 + L1*sin(psi) + L2*cos(theta)
psi = arccos((x^2 + y^2 - L1^2 - L2^2) / (2 * L1 * L2))
theta = arcsin((y * (L1 + L2 * c2) - x * L2 * s2) / (x^2 + y^2))
where c2 = (x^2 + y^2 - L1^2 - L2^2) / (2 * L1 * L2);
and s2 = sqrt(1 - c2^2);

может оно!!!!

[sasha_89]

сама программа должна выглядеть примерно так!!!

Код: Выделить всё

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

[PKM]

... "может"? Надо точно знать, проверить, сделать пробный расчет. Потом можно формулы написать в С++.

Это кинематика для дельта-робота, не то. Возможно в линукснс есть кинематика подходящая, не помню точно.

[Nick]

А может это кинематика XZB?

О, или ты хочешь управлять в координатах XYZ, вместо Z,R,alpha?

[Nick]

Я так понимаю для этого есть кинематика rotatekins:
http://git.linuxcnc.org/gitweb?p=linuxc ... 23;hb=HEAD

Т.е. вместо loadrt trivkins надо написать loadrt rotatekins и потом просто настроить оси...

[sasha_89]

я в этом деле еще лузер ))) знаю только как в файлах прописать пины, настроить оси !!! подскажите процедуру что делать с этим файлом куда прописывать !

[Nick]

Настрой базовую конфигурацию, чтобы станок ездил по обычным осям, проверь, что ездит.
Потом присылай сюда, будем ее переделывать.

[sasha_89]

настроил ездит по обычным осям, вот настройки в архиве ! http://file.sampo.ru/3nw744/

[Nick]

Файлы кидай прямо на форум!
http://www.cnc-club.ru/wiki/index.php/% ... 8%D1%8E%3F

[sasha_89]

Вот скинул!

[Impartial]

Это задача преобразования координат из прямоугольной системы в цилиндрическую.
Все преобразования есть в исходниках емс (файлы libmnl/posemath.*)
Т.е. это всего две строки.

PM_CARTESIAN crt = PM_CARTESIAN(x,y,z);
PM_CYLINDRICAL cil = crt;

theta, r, z из cil и будут искомыми углом, радиусом и высотой.

Углы в радианах а ось Z в прямоугольной системе координат направлена вверх.

[sasha_89]

как я понимаю тут не всё так просто, тут нужно прописать формулы для расчета кутов!!!

[nkp]

ты пользуешь скорей всего старую версию емс2 и файлы нашел древние...))
лучше бы обновиться (но должно все работать и так))
----------------------------
попробуй этот конфиг:

rotatekins1.rar

(3.4 КБ) 437 скачиваний

тут уже прописана поворотная кинематика(может это то , что тебе нужно)

[nkp]

Я так понимаю для этого есть кинематика rotatekins:

выложил TC-у конфиг - но что то у меня сомнения ,что это то что надо...
по моему нам нужно преобразование " простого" Gкода в полярные координаты...
что то навроде:
для поворотной оси С = arctg(x/y)
а для линейной X = x / cos(C)
тогда мы задаем в Gкоде как обычно x и y (z работает без преобразований) ,
а на выходе имеем угол поворота для X (если исходить из рисунка ТС) и линейное перемещение для Y соответственно...

[nkp]

для углового движения можно взять кусок отсюда:

Код: Выделить всё

#include "kinematics.h"		/* these decls */
#include "hal.h"
#include "rtapi_math.h"

static PmCartesian old; //  sudo comp --install tangentkins.c
typedef struct { hal_bit_t *pin_auto ;} tangentkins_hal_t;
tangentkins_hal_t *data;

int kinematicsForward(const double *joints,
                      EmcPose * pos,
                      const KINEMATICS_FORWARD_FLAGS * fflags,
                      KINEMATICS_INVERSE_FLAGS * iflags)
{
    pos->tran.x = joints[0];
    pos->tran.y = joints[1];
    pos->tran.z = joints[2];
    pos->a = joints[3];
    pos->b = joints[4];
    pos->c = joints[5];
    pos->u = joints[6];
    pos->v = joints[7];
    pos->w = joints[8];
    
    return 0;
}

int kinematicsInverse(const EmcPose * pos,
                      double *joints,
                      const KINEMATICS_INVERSE_FLAGS * iflags,
                      KINEMATICS_FORWARD_FLAGS * fflags)
{
    
    static double dx = 0, dy = 0, A = 0;
    
    dy = pos->tran.y - old.y;
    dx = pos->tran.x - old.x;
    if (*data->pin_auto){
        if (dx != 0 || dy != 0) {
            A = atan2(dx, dy)*(180.0/3.141592);
        }
    }else{
        A=pos->a;
    }
    
    joints[0] = pos->tran.x;
    joints[1] = pos->tran.y;
    joints[2] = pos->tran.z;
    joints[3] = A;
    joints[4] = pos->b;
    joints[5] = pos->c;
    joints[6] = pos->u;
    joints[7] = pos->v;
    joints[8] = pos->w;
    
    
    old.x = pos->tran.x; // store previous value here to exclude kins-based movements
    old.y = pos->tran.y;
    
    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("tangentkins");
    if(comp_id > 0) {
	data = hal_malloc(sizeof(tangentkins_hal_t));
 	if(hal_pin_bit_new("tangentkins.auto", HAL_IN, &(data->pin_auto), comp_id) < 0 ){
            rtapi_print("Failed to create pin in tangentkins");
 	    return -EINVAL;
        }
 	hal_ready(comp_id);
 	return 0;
    }
    return comp_id;
}

void rtapi_app_exit(void) { hal_exit(comp_id); }

только нам уже не нужно:

Код: Выделить всё

    dy = pos->tran.y - old.y;
    dx = pos->tran.x - old.x;

[sasha_89]

а это не подходит ?

Код: Выделить всё

/********************************************************************
* 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 */

int kinematicsForward(const double *joints,
		      EmcPose * pos,
		      const KINEMATICS_FORWARD_FLAGS * fflags,
		      KINEMATICS_INVERSE_FLAGS * iflags)
{
    double c_rad = -joints[5]*M_PI/180;
    pos->tran.x = joints[0] * cos(c_rad) - joints[1] * sin(c_rad);
    pos->tran.y = joints[0] * sin(c_rad) + joints[1] * cos(c_rad);
    pos->tran.z = joints[2];
    pos->a = joints[3];
    pos->b = joints[4];
    pos->c = joints[5];
    pos->u = joints[6];
    pos->v = joints[7];
    pos->w = joints[8];

    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 * cos(c_rad) - pos->tran.y * sin(c_rad);
    joints[1] = pos->tran.x * sin(c_rad) + pos->tran.y * cos(c_rad);
    joints[2] = pos->tran.z;
    joints[3] = pos->a;
    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); }

[Nick]

Да, точно, rotatekins это немного не то.

Я правильно понимаю, что дожно быть вот так?

Код: Выделить всё

int kinematicsForward(const double *joints,
		      EmcPose * pos,
		      const KINEMATICS_FORWARD_FLAGS * fflags,
		      KINEMATICS_INVERSE_FLAGS * iflags)
{
    pos->tran.x = joints[0]*cos(joints[4]);
    pos->tran.y = joints[0]*sin(joints[4]);;
    pos->tran.z = joints[2];
    pos->a = joints[3];
    pos->b = joints[4];
    pos->c = joints[5];
    pos->u = joints[6];
    pos->v = joints[7];
    pos->w = joints[8];

    return 0;
}

int kinematicsInverse(const EmcPose * pos,
		      double *joints,
		      const KINEMATICS_INVERSE_FLAGS * iflags,
		      KINEMATICS_FORWARD_FLAGS * fflags)
{
	if (pos->tran.x == 0)
	{
		joints[0] = pos->tran.y;
		if (pos->tran.y>0)
		{
			joints[4] = 1.570796327;
		} else {
		    joints[4] = -1.570796327;
	    }
	}
	else
	{
		joints[4] = arctg(pos->tran.x/pos->tran.y);
		joints[1] = sqrt(pos->tran.x**2 + pos->tran.y**2)
	}
    
    joints[1] = pos->tran.y;
    joints[2] = pos->tran.z;
    joints[3] = pos->a;
    joints[5] = pos->c;
    joints[6] = pos->u;
    joints[7] = pos->v;
    joints[8] = pos->w;

    return 0;
}

[nkp]

вот это похоже на то что надо
только странно как то - такое нужное преобразование в контексте емс вроде бы не описывается...

=========