Greetings,
I have a weird problem with my tripod kinematics file. It compiles OK and
works well with any sim config, but don't work with realtime configs on the
same PC.
The file is attached. It is deeply modified genhexkins with iterative
forward kinematics.
It has InvKin function called from both kinematicsInverse
and kinematicsForward functions.
In any realtime config it gives different (wrong) results for inverse kins,
and won't work for forward kins at all.
How can it be that the same code gives different results?
All variable names looks unique, I remember that lesson. Moreover, I just
changed them to be even more unique.
What can be the problem, just where should I look? Maybe I call the
function wrong way? But it works for sim...
Some LinuxCNC problem? It's 2.6.0pre, don't know where to look for build
number.
I'm completely stuck, please help.
Thanks,
Andrew
/********************************************************************
* Description: genhexkins.c
* License: GPL Version 2
* System: Linux
*******************************************************************/
#include "rtapi_math.h"
#include "posemath.h"
#include "genhexkins.h"
#include "kinematics.h" /* these decls, KINEMATICS_FORWARD_FLAGS */
/* define position of base strut ends in base (world) coordinate system */
static PmCartesian v_base[3] = {{BASE_0_X, BASE_0_Y, BASE_0_Z},
{BASE_1_X, BASE_1_Y, BASE_1_Z},
{BASE_2_X, BASE_2_Y, BASE_2_Z}};
static PmCartesian v_platf[3] = {{PLATFORM_0_X, PLATFORM_0_Y, PLATFORM_0_Z},
{PLATFORM_1_X, PLATFORM_1_Y, PLATFORM_1_Z},
{PLATFORM_2_X, PLATFORM_2_Y, PLATFORM_2_Z}};
/******************************** MatMult() *********************************/
/*---------------------------------------------------------------------------
This function simply multiplies a 3x3 matrix by a 1x3 vector
---------------------------------------------------------------------------*/
static void MatMult(double Jac[][3], const double x[], double Ans[])
{
int i, j;
for (i=0; i<=2; ++i){
Ans[i] = 0;
for (j=0; j<=2; ++j){
Ans[i] = Jac[i][j]*x[j]+Ans[i];
}
}
}
/******************************* MatInvert() ***************************/
/*-----------------------------------------------------------------------------
This is a function that inverts a 3x3 matrix.
-----------------------------------------------------------------------------*/
static int MatInvert(double Jac[][3], double InvJac[][3])
{
double DetJ;
DetJ = Jac[0][0]*Jac[1][1]*Jac[2][2] - Jac[0][0]*Jac[1][2]*Jac[2][1] - Jac[0][1]*Jac[1][0]*Jac[2][2] + Jac[0][1]*Jac[1][2]*Jac[2][0] + Jac[0][2]*Jac[1][0]*Jac[2][1] - Jac[0][2]*Jac[1][1]*Jac[2][0];
InvJac[0][0] = (Jac[1][1]*Jac[2][2] - Jac[1][2]*Jac[2][1])/DetJ;
InvJac[0][1] = (Jac[0][1]*Jac[2][2] - Jac[0][2]*Jac[2][1])*(-1)/DetJ;
InvJac[0][2] = (Jac[0][1]*Jac[1][2] - Jac[0][2]*Jac[1][1])/DetJ;
InvJac[1][0] = (Jac[1][0]*Jac[2][2] - Jac[1][2]*Jac[2][0])*(-1)/DetJ;
InvJac[1][1] = (Jac[0][0]*Jac[2][2] - Jac[0][2]*Jac[2][0])/DetJ;
InvJac[1][2] = (Jac[0][0]*Jac[1][2] - Jac[0][2]*Jac[1][0])*(-1)/DetJ;
InvJac[2][0] = (Jac[1][0]*Jac[2][1] - Jac[1][1]*Jac[2][0])/DetJ;
InvJac[2][1] = (Jac[0][0]*Jac[2][1] - Jac[0][1]*Jac[2][0])*(-1)/DetJ;
InvJac[2][2] = (Jac[0][0]*Jac[1][1] - Jac[0][1]*Jac[1][0])/DetJ;
return 0; /* FIXME-- check divisors for 0 above */
}
/******************************** InvKin() *********************************/
int InvKin(const PmRpy q_RPY, double * q_pos)
{
PmCartesian v_platf0;
PmRotationMatrix RMatrix;
double Ddx, Ddy, Ddz;
int i;
/* define Rotation Matrix */
pmRpyMatConvert(q_RPY, &RMatrix);
/* enter for loop to calculate joints position */
for (i = 0; i < NUM_STRUTS; i++) {
/* convert location of platform joint from platform to world coordinates */
pmMatCartMult(RMatrix, v_platf[i], &v_platf0);
Ddx = v_base[i].x - v_platf0.x;
Ddy = v_base[i].y - v_platf0.y;
Ddz = v_base[i].z - v_platf0.z;
/* define joint lenghts */
q_pos[i] = Ddz - sqrt(strut_lenght*strut_lenght - Ddx*Ddx - Ddy*Ddy);
}
return 0;
}
/************************ KinematicsForward() ********************************/
static int iteration = 0; /* global so we can report it */
int kinematicsForward(const double * joints,
EmcPose * pos,
const KINEMATICS_FORWARD_FLAGS * fflags,
KINEMATICS_INVERSE_FLAGS * iflags)
{
double Jac[NUM_STRUTS][NUM_STRUTS];
double InvJac[NUM_STRUTS][NUM_STRUTS];
double QInvKin[NUM_STRUTS], QDiff[NUM_STRUTS];
double Qdx[NUM_STRUTS];
double Qdy[NUM_STRUTS];
double Qdz[NUM_STRUTS];
PmRpy q_RPY, dx_RPY, dy_RPY, dz_RPY;
double q_delta[NUM_STRUTS];
double conv_err = 1.0;
int iterate = 1;
int i;
#define HIGH_CONV_CRITERION (1e-12)
#define MEDIUM_CONV_CRITERION (1e-7)
#define LOW_CONV_CRITERION (1e-3)
#define MEDIUM_CONV_ITERATIONS 50
#define LOW_CONV_ITERATIONS 100
#define FAIL_CONV_ITERATIONS 150
#define LARGE_CONV_ERROR 1000
double conv_criterion = MEDIUM_CONV_CRITERION;
iteration = 0;
/* assign x,y,z to roll, pitch, yaw angles */
q_RPY.r = pos->tran.x * PM_PI / 180.0;
q_RPY.p = pos->tran.y * PM_PI / 180.0;
q_RPY.y = pos->tran.z * PM_PI / 180.0;
/* Enter Newton-Raphson iterative method */
while (iterate) {
/* check for large error and return error flag if no convergence */
if ((conv_err > +LARGE_CONV_ERROR) ||
(conv_err < -LARGE_CONV_ERROR)) {
/* we can't converge */
return -2;
};
iteration++;
/* check iteration to see if the kinematics can reach the
convergence criterion and return error flag if it can't */
if (iteration > FAIL_CONV_ITERATIONS) {
/* we can't converge */
return -5;
}
/* compute StrutLengthDiff[] by running inverse kins on Cartesian
estimate to get joint estimate, subtract joints to get joint deltas,
and compute inv J while we're at it */
InvKin(q_RPY, QInvKin);
for (i = 0; i < NUM_STRUTS; i++) {
QDiff[i] = QInvKin[i] - joints[i];
}
/* Build Inverse Jacobian Matrix */
dx_RPY.r = q_RPY.r + (1e-5);
dx_RPY.p = q_RPY.p;
dx_RPY.y = q_RPY.y;
dy_RPY.r = q_RPY.r;
dy_RPY.p = q_RPY.p + (1e-5);
dy_RPY.y = q_RPY.y;
dz_RPY.r = q_RPY.r;
dz_RPY.p = q_RPY.p;
dz_RPY.y = q_RPY.y + (1e-5);
InvKin(dx_RPY, Qdx);
InvKin(dy_RPY, Qdy);
InvKin(dz_RPY, Qdz);
InvJac[0][0] = (Qdx[0] - QInvKin[0]) * 1e5;
InvJac[1][0] = (Qdx[1] - QInvKin[1]) * 1e5;
InvJac[2][0] = (Qdx[2] - QInvKin[2]) * 1e5;
InvJac[0][1] = (Qdy[0] - QInvKin[0]) * 1e5;
InvJac[1][1] = (Qdy[1] - QInvKin[1]) * 1e5;
InvJac[2][1] = (Qdy[2] - QInvKin[2]) * 1e5;
InvJac[0][2] = (Qdz[0] - QInvKin[0]) * 1e5;
InvJac[1][2] = (Qdz[1] - QInvKin[1]) * 1e5;
InvJac[2][2] = (Qdz[2] - QInvKin[2]) * 1e5;
/* invert Inverse Jacobian */
MatInvert(InvJac, Jac);
/* multiply Jacobian by LegLengthDiff */
MatMult(Jac, QDiff, q_delta);
/* subtract delta from last iterations pos values */
q_RPY.r -= q_delta[0];
q_RPY.p -= q_delta[1];
q_RPY.y -= q_delta[2];
/* determine value of conv_error (used to determine if no convergence) */
conv_err = 0.0;
for (i = 0; i < NUM_STRUTS; i++) {
conv_err += fabs(QDiff[i]);
}
/* enter loop to determine if a strut needs another iteration */
iterate = 0; /*assume iteration is done */
for (i = 0; i < NUM_STRUTS; i++) {
if (fabs(QDiff[i]) > conv_criterion) {
iterate = 1;
}
}
} /* exit Newton-Raphson Iterative loop */
/* assign r,p,y to x,y,z */
pos->tran.x = q_RPY.r * 180.0 / PM_PI;
pos->tran.y = q_RPY.p * 180.0 / PM_PI;
pos->tran.z = q_RPY.y * 180.0 / PM_PI;
return 0;
}
/************************ kinematicsInverse() ********************************/
int kinematicsInverse(const EmcPose * pos,
double * joints,
const KINEMATICS_INVERSE_FLAGS * iflags,
KINEMATICS_FORWARD_FLAGS * fflags)
{
PmRpy q_rpy;
/* define Rotation Matrix */
q_rpy.r = pos->tran.x * PM_PI / 180.0;
q_rpy.p = pos->tran.y * PM_PI / 180.0;
q_rpy.y = pos->tran.z * PM_PI / 180.0;
InvKin(q_rpy, joints);
return 0;
}
int kinematicsHome(EmcPose * world,
double * joint,
KINEMATICS_FORWARD_FLAGS * fflags,
KINEMATICS_INVERSE_FLAGS * iflags)
{
*fflags = 0;
*iflags = 0;
/* return kinematicsInverse(world, joint, iflags, fflags); */
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("genhexkins");
if(comp_id > 0) {
hal_ready(comp_id);
return 0;
}
return comp_id;
}
void rtapi_app_exit(void) { hal_exit(comp_id); }
#define NUM_STRUTS 3
#define strut_lenght 200
/* Define position of base strut ends in base (world) coordinates */
#define BASE_0_X 73.910
#define BASE_0_Y 30.615
#define BASE_0_Z 200
#define BASE_1_X -63.468
#define BASE_1_Y 48.701
#define BASE_1_Z 200
#define BASE_2_X -10.442
#define BASE_2_Y -79.316
#define BASE_2_Z 200
/* Define position of platform strut end in platform coordinate system */
#define PLATFORM_0_X 35.000
#define PLATFORM_0_Y 60.622
#define PLATFORM_0_Z 20.000
#define PLATFORM_1_X -70.000
#define PLATFORM_1_Y 0.000
#define PLATFORM_1_Z 20.000
#define PLATFORM_2_X 35.000
#define PLATFORM_2_Y -60.622
#define PLATFORM_2_Z 20.000
------------------------------------------------------------------------------
Live Security Virtual Conference
Exclusive live event will cover all the ways today's security and
threat landscape has changed and how IT managers can respond. Discussions
will include endpoint security, mobile security and the latest in malware
threats. http://www.accelacomm.com/jaw/sfrnl04242012/114/50122263/
_______________________________________________
Emc-users mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/emc-users
