How can I modify trivkins.c? (and also other kinematics files)

More
07 Aug 2024 05:03 - 07 Aug 2024 05:10 #307141 by winyk
Hello, I am a beginner trying to learn LinuxCNC. Please note that not only that I am totally new to LinuxCNC, but I am also totally new to working with CNC machine, G code, and the C programming language, so please forgive me if I ask stupid questions. Since I don't have any lathe or mill that I can experiment with (without messing things up), I am playing with LinuxCNC on a virtual machine (Debian inside VirtualBox). I have successfully managed to build LinuxCNC using run-in-place method as instructed in this guide linuxcnc.org/docs/master/html/code/building-linuxcnc.html .

Right now, I want to understand how .c kinematics files work in LinuxCNC. So, as a first step, I did a little experiment by modifying the inverse kinematics function in trivkins.c (located at src/emc/kinematics/) as shown.
int kinematicsInverse(const EmcPose * pos,
                      double *joints,
                      const KINEMATICS_INVERSE_FLAGS * iflags,
                      KINEMATICS_FORWARD_FLAGS * fflags)
{
    EmcPose P;
    P.tran.x = - pos -> tran.x;
    P.tran.y = pos -> tran.y;
    P.tran.z = pos -> tran.z;
    return identityKinematicsInverse(&P, joints, iflags, fflags);
    // return identityKinematicsInverse(pos, joints, iflags, fflags);
}

My understanding is that the inverse kinematics translates the position of the tool into joints variables, so by putting a negative sign in front of pos -> tran.x before passing that to identityKinematicsInverse function, I expect the machine to move in -x direction whenever I command the machine with G code to move in +x direction (like G0 X10). 

After I have modified trivkins.c, I compiled the file and rebuild LinuxCNC by going into /src and use  ./autogen.sh, then ./configure, and then make. 
However, as you can see in the photo attached   , when I open axis.ini config (which I believe it uses trivkins.c), the machine still do engraving in the +x direction. Nor did it go to X-10 when I command it with G X10.

I tried modifying trivkins.c in different ways, such as setting some directions to a constant, but I still experience the same result. So what did I do wrong here? Why I see no changes in AXIS after I modified the kinematics files. I am pretty sure the trivkins.c was compiled because when I deliberately put syntax error line in there, make threw an error
Attachments:
Last edit: 07 Aug 2024 05:10 by winyk.

Please Log in or Create an account to join the conversation.

More
07 Aug 2024 05:39 - 07 Aug 2024 06:04 #307142 by Aciera
There are easier ways to start playing around with kinematics. I would suggest to have a look at 'millturn', also have a look at the linked paper:

forum.linuxcnc.org/38-general-linuxcnc-q...atic-mechanis#306700

[edit]
Just for completness, 'trivkins.c' uses functions in 'kins_util.c':

 

But again, unless you know your way around with C I would strongly recommend using the .comp method with 'halcompile --install  your_path/your_kinematic.comp' as in the 'millturn' example pointed out in the link above.
Attachments:
Last edit: 07 Aug 2024 06:04 by Aciera. Reason: Fix typo
The following user(s) said Thank You: winyk

Please Log in or Create an account to join the conversation.

More
08 Aug 2024 11:01 #307253 by winyk
Thank you very much for providing the resources. Those will surely be helpful when I have to implement switchable kinematics (but still a lot more to learn to get to that point).

As you suggested, I have tried modifying kinematics using the userkins.comp method. I modify the inverse kinematics part of that file as followed
int kinematicsInverse(const EmcPose * pos,
                      double *j,
                      const KINEMATICS_INVERSE_FLAGS * iflags,
                      KINEMATICS_FORWARD_FLAGS * fflags)
{
    is_ready = 1; // Inverse is not called until homed for KINEMATICS_BOTH

    // Update the kinematic joints specified by the
    // [KINS]JOINTS setting (3 required for this template).
    // Maximum number of joints is defined in include file:
    //         emcmotcfg:EMCMOT_MAX_JOINTS (typ 16)
    // Some joints may be claimed as 'extrajoints' and
    // do not participate in kinematics calculations --
    // $ man motion for more info
    j[0] = - pos->tran.x; // joint 0
    j[1] = pos->tran.y; // joint 1
    j[2] = pos->tran.z; // joint 2

    //example hal pin update (homing reqd before kinematicsInverse)
    *haldata->out = *haldata->in; //dereference
    //read from param example: *haldata->out = haldata->param_rw;

    return 0;
} // kinematicsInverse()

Again, my intention here is to reverse the machine's movement in x-direction by 'intercepting' X value in G code, change that to negative value of itself, and set the joint variables to be that value using inverse kinematics (please correct me if my understanding of inverse kinematics is wrong.)

I followed the instruction listed in the description of that file to get the kinematics installed and modify the KINS part in axis_mm.ini accordingly. When I run the engraving G code in axis_mm.ini, I observed in the top left corner of the graphic display that the x-coordinates was showing some negative numbers. However, the +x direction vector (the green one) in the graphic display is also pointing in that same direction.



So, why does this happen? Did I do something wrong? I am not sure whether I have successfully reversed the x-direction or not .

Also, I noticed that the code in userkins.comp is very similar to C code, so is userkins.comp also written in C?, or is it a different language?
Attachments:

Please Log in or Create an account to join the conversation.

More
08 Aug 2024 11:45 #307256 by Aciera
The component language is very similar to C but saves the user a lot of tedious typing work.

Generally you need to look at the Forward and Inverse kinematic as a pair where one is the inverse of the other. Hence changing one will also require changing the other.

As an example have a look at 'millturn.comp'. 'Case 1' reverts the direction of the y-axis / j[1] :
static bool is_ready=0;
int kinematicsForward(const double *j,
                      EmcPose * pos,
                      const KINEMATICS_FORWARD_FLAGS * fflags,
                      KINEMATICS_INVERSE_FLAGS * iflags)
{
    static bool gave_msg;
    // define forward kinematic models using case structure for
    // for switchable kinematics
    switch (switchkins_type) {
        case 0:
            pos->tran.x = j[0];
            pos->tran.y = j[1];
            pos->tran.z = j[2];
            pos->a      = j[3];
            break;
        case 1:
            pos->tran.x = j[2];
            pos->tran.y = -j[1];
            pos->tran.z = j[0];
            pos->a      = j[3];
            break;
    }
    // unused coordinates:
    pos->b = 0;
    pos->c = 0;
    pos->u = 0;
    pos->v = 0;
    pos->w = 0;

    if (*haldata->in && !is_ready && !gave_msg) {
       rtapi_print_msg(RTAPI_MSG_ERR,
                       "%s the 'in' pin not echoed until Inverse called\n",
                      __FILE__);
       gave_msg=1;
    }
    return 0;
} // kinematicsForward()

int kinematicsInverse(const EmcPose * pos,
                      double *j,
                      const KINEMATICS_INVERSE_FLAGS * iflags,
                      KINEMATICS_FORWARD_FLAGS * fflags)
{
    is_ready = 1; // Inverse is not called until homed for KINEMATICS_BOTH

    // Update the kinematic joints specified by the
    // [KINS]JOINTS setting (4 required for this template).
    // define forward kinematic models using case structure for
    // for switchable kinematics
    switch (switchkins_type) {
        case 0:
            j[0] = pos->tran.x;
            j[1] = pos->tran.y;
            j[2] = pos->tran.z;
            j[3] = pos->a;
            break;
        case 1:
            j[2] = pos->tran.x;
            j[1] = -pos->tran.y;
            j[0] = pos->tran.z;
            j[3] = pos->a;
            break;
    }

    //example hal pin update (homing reqd before kinematicsInverse)
    *haldata->out = *haldata->in; //dereference
    //read from param example: *haldata->out = haldata->param_rw;

    return 0;
} // kinematicsInverse()
The following user(s) said Thank You: winyk

Please Log in or Create an account to join the conversation.

More
09 Aug 2024 01:48 #307285 by winyk
I tried reversing x-direction in both forward kinematics and inverse kinematics as you have suggested, but the machine still move in the +x direction. Now, the x in graphic display also shown as positive



here are my kinematicsForward and kinematicsInverse functions. 
int kinematicsForward(const double *j,
                      EmcPose * pos,
                      const KINEMATICS_FORWARD_FLAGS * fflags,
                      KINEMATICS_INVERSE_FLAGS * iflags)
{
    static bool gave_msg;
    // [KINS]JOINTS=3
    pos->tran.x = - j[0]; // X coordinate
    pos->tran.y = j[1]; // Y coordinate
    pos->tran.z = j[2]; // Z coordinate
    // unused coordinates:
    pos->a = 0;
    pos->b = 0;
    pos->c = 0;
    pos->u = 0;
    pos->v = 0;
    pos->w = 0;

    if (*haldata->in && !is_ready && !gave_msg) {
       rtapi_print_msg(RTAPI_MSG_ERR,
                       "%s The 'in' pin not echoed until Inverse called\n",
                      __FILE__);
       gave_msg=1;
    }
    return 0;
} // kinematicsForward()

int kinematicsInverse(const EmcPose * pos,
                      double *j,
                      const KINEMATICS_INVERSE_FLAGS * iflags,
                      KINEMATICS_FORWARD_FLAGS * fflags)
{
    is_ready = 1; // Inverse is not called until homed for KINEMATICS_BOTH

    // Update the kinematic joints specified by the
    // [KINS]JOINTS setting (3 required for this template).
    // Maximum number of joints is defined in include file:
    //         emcmotcfg:EMCMOT_MAX_JOINTS (typ 16)
    // Some joints may be claimed as 'extrajoints' and
    // do not participate in kinematics calculations --
    // $ man motion for more info
    j[0] = - pos->tran.x; // joint 0
    j[1] = pos->tran.y; // joint 1
    j[2] = pos->tran.z; // joint 2

    //example hal pin update (homing reqd before kinematicsInverse)
    *haldata->out = *haldata->in; //dereference
    //read from param example: *haldata->out = haldata->param_rw;

    return 0;
} // kinematicsInverse()

I have already made sure that I reinstall the component with halcompile --install mykins.comp after editing the file.
Attachments:

Please Log in or Create an account to join the conversation.

More
09 Aug 2024 07:26 #307296 by Aciera
I'm sorry, I forgot to mention this, the preview does not reflect machine movement (ie joint position) when using custom kinematics all it shows is axes position in a fixed coordinate system. If you want to see the effects off your custom kinematic then you would need a machine visualization (vismach) for your config. For a simple config with a 3axis mill see 'configs/sim/axis/vismach/3axis-tutorial' you can then change KINEMATICS = trivkins' to your kinematics. You should then see the spindle move to the left when jogging X in the positive direction:

 
Attachments:
The following user(s) said Thank You: winyk

Please Log in or Create an account to join the conversation.

More
14 Aug 2024 10:03 #307706 by winyk
Thank you. Although I am not sure what is the difference between 'machine movement' and 'axes position' that you have mentioned, I presume that AXIS preview does not reflect the underlying kinematics of the machine, correct? I think the fact that AXIS preview works this way can cause a lot of confusion to newbies like me. If this is the case, then what do the red, green, blue directional vectors shown in the AXIS preview tell us? Is there any way to configure AXIS preview so that the directional vectors agree with the underlying kinematics?

Anyway, after experimenting with 3axis-tutorial.ini, I can now see the effect of reversing the X coordinate in vismach. It turns out that reversing the coordinate in inverse kinematics alone is enough to see this effect, no need to modify the forward kinematics part. I also try with Y and Z as well. Reversing Y works as expected. However, as I try to reverse Z, I encountered error messages telling me that linear move would exceed joint 2's positive limit (no matter I move to +z or -z), and I am not sure why.
 
int kinematicsInverse(const EmcPose * pos,
                      double *j,
                      const KINEMATICS_INVERSE_FLAGS * iflags,
                      KINEMATICS_FORWARD_FLAGS * fflags)
{
    is_ready = 1; // Inverse is not called until homed for KINEMATICS_BOTH

    // Update the kinematic joints specified by the
    // [KINS]JOINTS setting (3 required for this template).
    // Maximum number of joints is defined in include file:
    //         emcmotcfg:EMCMOT_MAX_JOINTS (typ 16)
    // Some joints may be claimed as 'extrajoints' and
    // do not participate in kinematics calculations --
    // $ man motion for more info
    j[0] = pos->tran.x; // joint 0
    j[1] = pos->tran.y; // joint 1
    j[2] = - pos->tran.z; // joint 2

    //example hal pin update (homing reqd before kinematicsInverse)
    *haldata->out = *haldata->in; //dereference
    //read from param example: *haldata->out = haldata->param_rw;

    return 0;
} // kinematicsInverse()

 
Attachments:

Please Log in or Create an account to join the conversation.

Time to create page: 0.222 seconds
Powered by Kunena Forum