example_tg.cc

Go to the documentation of this file.

#include "create_robot_model.hh"
#include "motion_control/lmc_tg_config.h"
#include "motion_control/lmc_tg.h"

int main(int argc, char **argv)
{
    auto robot_model_config = createLM3Config();
    lmc_robot_model_t* robot_model = lmc_rm_new();    
    lmc_rm_init(robot_model, &robot_model_config,STRUCT_TYPE_61);
    lmc_tg_config_t tg_config;
    lmc_tg_limit_t tg_limit;
    for (int i = 0; i < 6; i++)
    {
        tg_limit.jnt_max_acc[i] = 4;
        tg_limit.jnt_max_v[i] = 2;
        tg_limit.jnt_pos_positive_limit[i] = 6.28;
        tg_limit.jnt_pos_negative_limit[i] = -6.28;
    }
    tg_limit.cartesian_max_v = 1;
    tg_limit.cartesian_max_acc = 1;
    tg_limit.cartesian_max_rot_v = 1;
    tg_limit.cartesian_max_rot_acc = 1;
    tg_limit.jnt_acc_time_constant = 10;

    tg_config.cycle_time = 0.01;
    tg_config.buffer_size = 10;
    lmc_tg_t * tg = lmc_tg_new();

    lmc_joint_state_t joint_state[6];
    lmc_joint_cmd_t joint_cmd[6];
    lmc_tg_runtime_data_t runtime_data;

    joint_state[0].pos = 0.0;
    joint_state[1].pos = 0.1;
    joint_state[2].pos = 0.2;
    joint_state[3].pos = 0.3;
    joint_state[4].pos = 0.4;
    joint_state[5].pos = 0.5;

    lmc_tg_init(tg, robot_model, &tg_config);
    lmc_tg_set_limit(tg, &tg_limit);
    lmc_tg_set_joint_pos(tg, joint_state);
    lmc_tg_update_ret_t tg_ret = lmc_tg_update(tg);

    double move_joint_pos1[6];
    for(size_t i = 0; i < 6; ++i) {
            move_joint_pos1[i] = 0.8;
    }
    lmc_tg_move_param_t move_joint_param;
    move_joint_param.v = 1.0;
    move_joint_param.acc = 1.0;
    move_joint_param.blend_radius = 0.0;
    move_joint_param.interruptable = 0;
    move_joint_param.smooth_move_to_next = 0;
    move_joint_param.time = 0.0;
    lmc_tg_buffer_move_joint(tg, 1, move_joint_pos1, &move_joint_param);
    while(tg_ret.state != TGS_IDLE)
    {
        tg_ret = lmc_tg_update(tg);
        lmc_tg_get_updated_date(tg, joint_cmd, &runtime_data);
    }
    lmc_tg_delete(&tg);
    lmc_rm_delete(&robot_model);
    return 0;
}