Question

#include <stdio.h>
#define _USE_MATH_DEFINES
#include <math.h>


// OMNI Encoder value -0.000000, -0.268890, 3.781295, -0.000106, -2.366794, 0.250667

float encoder[6] = {-0.000000f, -0.268890f, 3.781295f, -0.000106f, -2.366794f, 0.250667f};
float d_g[6] = {0.125f, 0, 0.2f, 0.1325f, 0, 0.039f};
float a_g[6] = {0, 0, 0.134f, 0, 0, 0};
float alpha_g[6] = {0, -M_PI/2, 0, M_PI/2, -M_PI/2, -M_PI/2};


#define ARR_ROW_SIZE 4
#define ARR_COL_SIZE 4

float(*MM())[4];
float(*DH_Master())[4];
float(*FK_Master())[4];
void PrintArray(float(*arr)[4], int row, int col); 

void PrintArray(float(*arr)[4], int row, int col){

    for (int i = 0; i < row; i++){
        for (int j = 0; j < col; j++){
            printf("%f ", arr[i][j]);
        }
        printf("\n");
    }
}


// Matrix Multiplication 

float(*MM(float M1[][4], float M2[][4]))[4] {

    static float C[4][4] =      {
        { 0.000f        , 0.000f      , 0.000f      , 0.000f      }, 
        { 0.000f        , 0.000f      , 0.000f      , 0.000f      }, 
        { 0.000f        , 0.000f      , 0.000f      , 0.000f      }, 
        { 0.000f        , 0.000f      , 0.000f      , 0.000f      }};

    float tmp; 

    for (int i = 0; i < 4; i++) {
        for (int j = 0; j < 4; j++) {
            tmp = 0.0f; 

            for (int k = 0; k < 4; k++) {
                tmp += M1[i][k] * M2[k][j];
            }
            C[i][j] = tmp; 
        }
    }
    return C;
}

float(*DH_Master(float alpha, float a, float d, float theta))[4]{
    printf("alpha %f / theta %f / a %f / d %f\n", alpha, theta, a, d);

    float T_a[4][4] = {
        { 1.000f        , 0.000f      , 0.000f      , a           }, 
        { 0.000f        , 1.000f      , 0.000f      , 0.000f      }, 
        { 0.000f        , 0.000f      , 1.000f      , 0.000f      }, 
        { 0.000f        , 0.000f      , 0.000f      , 1.000f      }};

    float T_d[4][4] = {
        { 1.000f        , 0.000f      , 0.000f      , 0.000f      }, 
        { 0.000f        , 1.000f      , 0.000f      , 0.000f      }, 
        { 0.000f        , 0.000f      , 1.000f      , d           }, 
        { 0.000f        , 0.000f      , 0.000f      , 1.000f      }};

    float Rzt[4][4] = {
        { cosf(theta)   , -sinf(theta), 0.000f      , 0.000f      }, 
        { sinf(theta)   ,  cosf(theta), 0.000f      , 0.000f      }, 
        { 0.000f        , 0.000f      , 1.000f      , 0.000f      }, 
        { 0.000f        , 0.000f      , 0.000f      , 1.000f      }};

    float Rxa[4][4] = {
        { 1.000f        , 0.000f      , 0.000f      , 0.000f      }, 
        { 0.000f        , cosf(alpha) , -sinf(alpha), 0.000f      }, 
        { 0.000f        , sinf(alpha) ,  cosf(alpha), 0.000f      }, 
        { 0.000f        , 0.000f      , 0.000f      , 1.000f      }};

    float(*C1)[4] = MM(Rxa, T_a);
    float(*C2)[4] = MM(C1, T_d);
    float(*C3)[4] = MM(C2, Rzt);
    PrintArray(C3, ARR_ROW_SIZE, ARR_COL_SIZE);

    printf("//////////////////////////////////////////////////////////\n");

    return C3;  
}


float(*FK_Master(float encoder[]))[4]{
    printf("T01\n");
    float(*T01)[ARR_COL_SIZE] = DH_Master(0, 0, 0.125, encoder[0] -M_PI/2);
    printf("T12\n");
    float(*T12)[ARR_COL_SIZE] = DH_Master(-M_PI/2, 0, 0, encoder[1]);
    printf("T23\n");
    float(*T23)[ARR_COL_SIZE] = DH_Master(0, 0.134, 0.2, encoder[2]);
    printf("T34\n");
    float(*T34)[ARR_COL_SIZE] = DH_Master(M_PI/2, 0, 0.1325, encoder[3]);
    printf("T45\n");
    float(*T45)[ARR_COL_SIZE] = DH_Master(-M_PI/2, 0, 0, encoder[4]);
    printf("T56\n");
    float(*T56)[ARR_COL_SIZE] = DH_Master(-M_PI/2, 0, 0.039, encoder[5]);

    printf("T01\n");
    PrintArray(T01, ARR_ROW_SIZE, ARR_COL_SIZE);

    float(*T02)[ARR_COL_SIZE] = (MM(T01, T12));
    printf("T02\n");
    PrintArray(T02, ARR_ROW_SIZE, ARR_COL_SIZE);

    float(*T03)[ARR_COL_SIZE] = (MM(T02, T23));
    printf("T03\n");
    PrintArray(T03, ARR_ROW_SIZE, ARR_COL_SIZE);

    float(*T04)[ARR_COL_SIZE] = (MM(T03, T34));
    printf("T04\n");
    PrintArray(T04, ARR_ROW_SIZE, ARR_COL_SIZE);

    float(*T05)[ARR_COL_SIZE] = (MM(T04, T45));
    printf("T05\n");
    PrintArray(T05, ARR_ROW_SIZE, ARR_COL_SIZE);

    float(*T06)[ARR_COL_SIZE] = (MM(T05, T56));
    printf("T06\n");
    PrintArray(T06, ARR_ROW_SIZE, ARR_COL_SIZE);

    return 0;  
}


int main(void) {

    float(*Art)[ARR_COL_SIZE] = FK_Master(encoder);

    return 0;

}

让我解释一下问题。

之前，我在MATLAB或Python环境中计算过。

这次我要再次使用 C 支付。

我进行了多次二维矩阵运算，并且正在编写再次返回计算值的代码。

简单来说，使用上面定义的a,d,alpha,theta进行二维矩阵运算，T01,T12,...我们正在写代码，得到T01*T12*T23*T34*T45*T56后得到一共T56 的 6 个值。

然而，当前的计算结果并没有正确出来。

如果您能帮我解决问题，我将不胜感激。

我会附上正确的计算结果作为图片。

enter image description here

C指针，二维矩阵返回

0 个答案: