I've just overlooked to your problem.
At a first glance:
The first thing that looks strange is that you have 2 joint and two world
coordinates, but are using 9 joint and 9 coordinates.
I suppose that some of them are not being used.
I would reduce that number, and I would put in the firs places the
meningful coordinates in joints, angle and translation
Also kee in mind that your coordinate system is somehow singular at the
origin, in the sense that velocity should be perpendicular to the linear
motion. That can pose some problems when generating the trajectory.
Also in the inverse kinematics, atan(y/x) only works for angles within
[-pi/2,+pi/2] if you intend to go farther than that
you have to consider the sign of x to determine if you are in that region
or in (+pi/2 3pi/2).
Javier
On Fri, Mar 23, 2012 at 7:57 AM, marc gulik van der <[email protected]>wrote:
> Hello,
> I’m trying to make my little machine to work.
> I have a problem with the math of kinematicsInverse i think ?
> The machine is an horizontal rotating table with X=0,Y=0 in the center.
> and one linear movement on top.
>
> I’m trying to do a movement within X,Y coordinates with a rotating
> movement A en a linear movement B, this is working fine.
> So
> A = angle of 45 deg.
> B= linear 7.07
> Equals
> X=5
> Y=5
>
> but if i run the code:
> G00 X5.
> G00 Y5.
> G00 X-5.
> G00 Y-5
> It stops with a joint[3] following error on (+/- 180 degree’s)
>
> I hope that someone can help me with this.
> Kind regards,
>
> my modded kins.
>
> /********************************************************************
> * Author:
> * License: GPL Version 2
> * System: Linux
> *
> * Copyright (c) 2004 All rights reserved.
> *
> * Last change:
> ********************************************************************/
> #include "rtapi_math.h"
> #include "kinematics.h"
> #include "rtapi.h" /* RTAPI realtime OS API */
> #include "rtapi_app.h" /* RTAPI realtime module decls */
>
>
>
>
> int kinematicsForward(const double *joints,
> EmcPose * pos,
> const KINEMATICS_FORWARD_FLAGS * fflags,
> KINEMATICS_INVERSE_FLAGS * iflags)
> {
> double a3,radangle ;
> // rotating correction
> a3 =joints[3];
> rtapi_set_msg_level(4);
>
> radangle= a3* ( PM_PI / 180 );
> pos->tran.x = cos(radangle)*joints[4];
> pos->tran.y = sin(radangle)*joints[4];
> pos->tran.z = joints[2];
> pos->a = a3;
> 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 x, y, angle, radangle, ilenght;
> double currenthoek, berekendhoek ;
> double anglediff ,targetangle;
>
> currenthoek = pos->a;
> berekendhoek = currenthoek -(floor(currenthoek /360)* 360);
>
> x = pos->tran.x;
> y = pos->tran.y;
> radangle= atan2 (y,x);
> targetangle= radangle * 180 / PM_PI;
>
> ilenght=0;
> radangle=0;
>
> ilenght= (y==0) ?fabs(x) : y / sin(atan2 (y,x));
>
> anglediff = targetangle -berekendhoek ;
> anglediff += (anglediff>180) ? -360 : (anglediff<-180) ? 360 : 0;
> //anglediff difference between angle's
>
>
> angle = currenthoek+anglediff;
>
> joints[0] = pos->tran.x;
> joints[1] = pos->tran.y;
> joints[2] = pos->tran.z;
> joints[3] = angle;
> joints[4] = ilenght;
> 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_IDENTITY;
> }
>
> #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("trivkins");
> if(comp_id > 0) {
> hal_ready(comp_id);
> return 0;
> }
> return comp_id;
> }
>
> void rtapi_app_exit(void) { hal_exit(comp_id); }
>
>
> ------------------------------------------------------------------------------
> This SF email is sponsosred by:
> Try Windows Azure free for 90 days Click Here
> http://p.sf.net/sfu/sfd2d-msazure
> _______________________________________________
> Emc-developers mailing list
> [email protected]
> https://lists.sourceforge.net/lists/listinfo/emc-developers
>
>
------------------------------------------------------------------------------
This SF email is sponsosred by:
Try Windows Azure free for 90 days Click Here
http://p.sf.net/sfu/sfd2d-msazure
_______________________________________________
Emc-developers mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/emc-developers
