模型

运动学数据R7_KINE初始化

void Test()
{
    int err=0;
    char* _robot_name=get_name_robotEC_deviceHandle_c(getEC_deviceName(0,NULL),0);
    R7_KINE rkine;
    init_R7_KINE(&rkine);
    double angle[6]={-1.5010, -0.4363,0.3840,-0.0279, -1.7802, 3.0194};
    init_R7_KINE1(&rkine,angle,NULL);
    double joint[6]={0,0,0,0,0,0};
    init_R7_KINE1(&rkine, joint, &dof);
}

init_R7_KINE 初始化运动学数据(R7_KINE)

init_R7_KINE1 初始化运动学数据(R7_KINE)

R7_KINE 描述机器人笛卡尔位姿数据和关节数据的结构体

angle 机器人关节, 单位: rad

运动速度数据R_KINE_VEL初始化

void Test()
{
    double joint_vel[6]={1,1.1,1.2,1.3,1.4,1.5};
    char* robot_name=get_name_robotEC_deviceHandle_c(getEC_deviceName(0,NULL),0);
    int dof=getRobotDOF_c(robot_name);
    R_KINE_VEL rkine_vel;
    init_R_KINE_VEL1(&rkine_vel, joint,joint_vel, dof);
}

init_R_KINE_VEL1 初始化运动速度数据(R_KINE_VEL)

joint 关节角度，单位：rad

joint_vel 关节运动速度，单位：rad/s

dof 机器人自由度

运动速度数据R_KINE_VEL设置

void joint_end_velocity_test()
{
    double joint[6]={1,1,1,1,1,1};
    double joint_vel[6]={1,1.1,1.2,1.3,1.4,1.5};
    char* robot_name=get_name_robotEC_deviceHandle_c(getEC_deviceName(0,NULL),0);
    int dof=getRobotDOF_c(robot_name);
    R_KINE_VEL rkine_vel;
    init_R_KINE_VEL1(&rkine_vel, joint,joint_vel, dof);

    TOOL _tool;
    WOBJ _wobj;
    tool to;
    wobj wo;
    gettool("tool11", &to);
    getwobj("wobj10", &wo);

    tool_to_TOOL(&to, &_tool);
    wobj_to_WOBJ(&wo, &_wobj);

    setToolWobjToR_KINE_VEL(&rkine_vel,  &_tool, &_wobj);

    int ret=Kine_JointToEndVelocity(robot_name, &rkine_vel);
    printf("%d,%f,%f,%f,%f,%f,%f\n",ret, rkine_vel.pose_vel[0],rkine_vel.pose_vel[1],rkine_vel.pose_vel[2],rkine_vel.pose_vel[3],rkine_vel.pose_vel[4],rkine_vel.pose_vel[5]);
    rkine_vel.joint_vel[0]=2;
    ret=Kine_EndToJointVelocity(robot_name, &rkine_vel);
    printf("%d,%f,%f,%f,%f,%f,%f\n",ret, rkine_vel.joint_vel[0],rkine_vel.joint_vel[1],rkine_vel.joint_vel[2],rkine_vel.joint_vel[3],rkine_vel.joint_vel[4],rkine_vel.joint_vel[5]);

}

setToolWobjToR_KINE_VEL 设置运动速度数据(R_KINE_VEL)

Kine_JointToEndVelocity 根据关节速度求解笛卡尔速度

Kine_EndToJointVelocity 根据笛卡尔速度求解关节速度

rkine_vel 运动速度

tool 工具描述

wobj 工件描述

robot_name 机器人名字

正/逆运动学

void delta_kinematicTest()
{
    int err=0;
    char* robot_name=get_name_robotEC_deviceHandle_c(EC_deviceName[0],0);
    R7_KINE rkine;
    double angle[3]={0.1676,0.2339, -0.6281};
    double angle_[3]={0.16,0.23, -0.62};
    init_R7_KINE1(&rkine,angle,NULL);
    // 正运动学
    err=Kine_Forward(robot_name, &rkine);
    if (0!=err)
    {
        printf("Kine_Forward failure!\n");
        return;
    }
    int i=0;
    for (i=0;i<3;i++)
    {
        rkine.joint[i]=angle_[i];
    }
    int j=0;
    for (i=0;i<3;i++)
    {
        for (j=0;j<3;j++)
        {
            printf("%f  ",rkine.R[i][j]);
        }
        printf("\n");
    }
    printf("%f,%f,%f\n",rkine.X[0],rkine.X[1],rkine.X[2]);
    // 逆运动学
    err=Kine_Inverse(robot_name, &rkine);
    if (0!=err)
    {
        printf("Kine_Inverse failure, err%d!\n",err);
        return;
    }
    printf("%f,%f,%f\n",rkine.joint[0],rkine.joint[1],rkine.joint[2]);
}

Kine_Forward 正运动学

Kine_Inverse 逆运动学

robot_name 机器人名字

rkine 运动学数据

动力学数据R_DYNAMICS初始化/设置

void test_Dyn_Forward()
{
    printf("test_Dyn_Forward start\n");
    int i=0;
    int count=0;
    int rob_index=0;// robot index,    0,1,2.....
    char* robot_name=get_name_robotEC_deviceHandle_c(EC_deviceName[0],rob_index);
    int _dof=getRobotDOF_c(robot_name);
    TarjectoryParam tarj;
    initTarjectoryParam(&tarj, get_tarj_f(), get_tarj_para(rob_index), get_tarj_order(), _dof);
    double _q[10],_qd[10],_qdd[10];
    double q[10],qd[10],qdd[10];
    getTarjectoryData(&tarj, 0, _q, _qd, _qdd);
    getTarjectoryData(&tarj, 0, q, qd, qdd);
    FILE* out=fopen("test_Dyn_Forward.txt","w");
    R_DYNAMICS rdyn;
    //init_R_DYNAMICS_ROBOT2(&rdyn, q, qd, qdd,  _dof);
    PayLoad payload;
    getPayLoad(&payload, "payload0");
    init_R_DYNAMICS(&rdyn, q, qd, qdd, NULL,  NULL, 1, 1, _dof,&payload);
    static struct timeval start,end;
    int Dyn_Inverse_time=0;
    int Dyn_Forward_time=0;
    int status=0;
    double _time=0;
    while(0==status)
    {
        moveTimer(0);
        _time=count*0.001;
        count++;
        printf("%f\n",_time);
        status=getTarjectoryData(&tarj, _time, _q, _qd, _qdd);
        set_joint_state_to_R_DYNAMICS(&rdyn, _q, _qd, _qdd);
        gettimeofday(&start,NULL);
        int err=Dyn_Inverse(robot_name, &rdyn);
        gettimeofday(&end,NULL);
        if (0!=err)
        {
            printf("Dyn_Inverse failure!\n");
            return;
        }
        Dyn_Inverse_time=(end.tv_sec*1000000+end.tv_usec)-(start.tv_sec*1000000+start.tv_usec);
        set_joint_state_to_R_DYNAMICS(&rdyn, q, qd, qdd);//为了测试正动力学，设置为上一时刻关节状态
        gettimeofday(&start,NULL);
        err=Dyn_Forward(robot_name, &rdyn);
        gettimeofday(&end,NULL);
        if (0!=err)
        { 
            fclose(out);
            printf("Dyn_Inverse failure!\n");
            return;
        }
        Dyn_Forward_time=(end.tv_sec*1000000+end.tv_usec)-(start.tv_sec*1000000+start.tv_usec);
        get_joint_state_from_R_DYNAMICS(&rdyn, q,qd,qdd);
        for (i=0;i<_dof;i++)
        {
            fprintf(out,"%f,",q[i]);
        }
        for (i=0;i<_dof;i++)
        {
            fprintf(out,"%f,",qd[i]);
        }
        for (i=0;i<_dof;i++)
        {
            fprintf(out,"%f,",qdd[i]);
        }
        for (i=0;i<_dof;i++)
        {
            fprintf(out,"%f,",_q[i]);
        }
        for (i=0;i<_dof;i++)
        {
            fprintf(out,"%f,",_qd[i]);
        }
        for (i=0;i<_dof;i++)
        {
            fprintf(out,"%f,",_qdd[i]);
        }
        fprintf(out,"%d,%d\n",Dyn_Inverse_time,Dyn_Forward_time);
    }
    fclose(out);
    printf("test_Dyn_Forward end\n");
}

init_R_DYNAMICS 初始化动力学数据(R_DYNAMICS)

init_R_DYNAMICS2 初始化动力学数据(R_DYNAMICS)

set_joint_state_to_R_DYNAMICS 设置动力学关节状态数据(R_DYNAMICS)

get_joint_state_from_R_DYNAMICS 获取动力学关节状态数据(R_DYNAMICS)

_rdyn 动力学数据

q 关节位置(rad 或 mm)

qd 关节速度(rad/s 或 mm/s)

qdd 关节加速度(rad/s^2 或 mm/s^2)

torque 关节力矩(Nmm 或 N)

fext 外部力

fcv_flag 是否使用摩擦力标识，0: 不使用，1: 使用

g_flag 是否使用重力标识，0: 不使用，1: 使用

_dof 机器人自由度

payload 负载

正/逆动力学

void test_Dyn_Forward()
{
    printf("test_Dyn_Forward start\n");
    int i=0;
    int count=0;
    int rob_index=0;// robot index,    0,1,2.....
    char* robot_name=get_name_robotEC_deviceHandle_c(EC_deviceName[0],rob_index);

    ······

    FILE* out=fopen("test_Dyn_Forward.txt","w");
    R_DYNAMICS rdyn;
    //init_R_DYNAMICS_ROBOT2(&rdyn, q, qd, qdd,  _dof);
    PayLoad payload;
    getPayLoad(&payload, "payload0");
    init_R_DYNAMICS(&rdyn, q, qd, qdd, NULL,  NULL, 1, 1, _dof,&payload);
    static struct timeval start,end;
    int Dyn_Inverse_time=0;
    int Dyn_Forward_time=0;
    int status=0;
    double _time=0;
    while(0==status)
    {

        ······

        int err=Dyn_Inverse(robot_name, &rdyn);
        gettimeofday(&end,NULL);
        if (0!=err)
        {
            printf("Dyn_Inverse failure!\n");
            return;
        }
        Dyn_Inverse_time=(end.tv_sec*1000000+end.tv_usec)-(start.tv_sec*1000000+start.tv_usec);
        set_joint_state_to_R_DYNAMICS(&rdyn, q, qd, qdd);//为了测试正动力学，设置为上一时刻关节状态
        gettimeofday(&start,NULL);
        err=Dyn_Forward(robot_name, &rdyn);
        gettimeofday(&end,NULL);
        if (0!=err)
        { 
            fclose(out);
            printf("Dyn_Inverse failure!\n");
            return;
        }
        Dyn_Forward_time=(end.tv_sec*1000000+end.tv_usec)-(start.tv_sec*1000000+start.tv_usec);

        ······

    }
    fclose(out);
    printf("test_Dyn_Forward end\n");
}

Dyn_Forward 正运动学

Dyn_Inverse 逆运动学

rdyn 动力学数据