lmc_tg.h

Go to the documentation of this file.

#ifndef LMC_TG
#define LMC_TG
#include <motion_control/lmc_robot_model.h>
#include <motion_control/lmc_joint.h>
#include <motion_control/lmc_tg_config.h>
#include <stdbool.h>



#ifdef __cplusplus
extern "C" {
#endif /* __cplusplus */

typedef struct lmc_tg lmc_tg_t;

typedef enum lmc_tg_update_state {
    TGS_RUNNING,  
    TGS_IDLE,     
    TGS_ERROR,     
} lmc_tg_update_state_t;


typedef enum lmc_tg_buffer_ret {
    BUFFER_RET_SUCCESS,  
    BUFFER_RET_FULL,     
    BUFFER_RET_IGNORE,   
    BUFFER_RET_INTERNAL_ERROR, 
    BUFFER_RET_COMPUTE_ERROR, 
    BUFFER_RET_CIRCLE_AFTER_UNDERMINIED_TRAJ, 
    BUFFER_RET_CIRCLE_COMPUTE_ERROR, 
    BUFFER_RET_UNTIL_BLEND_CONFLICT, 
    BUFFER_RET_TRAJ_IK_ERROR,    
    BUFFER_RET_MISMATCH_GROUP,   
    BUFFER_RET_VEL_EXCEED,   
    BUFFER_RET_PT_ERROR,   
    BUFFER_RET_PUSH_BUFFER_ERROR,   
} lmc_tg_buffer_ret_t;

typedef enum lmc_tg_update_error_code {
    UPDATE_RET_NO_ERROR = 0,  
    UPDATE_RET_IK_ERROR = -1,  
    UPDATE_RET_SINGULARITY_ERROR = -2,  
    UPDATE_RET_JOINT_POS_LOGIC_ERROR = -3,  
    UPDATE_INTERNAL_ERROR = -4  
} lmc_tg_update_error_code_t;

typedef struct lmc_tg_update_ret {
    lmc_tg_update_state_t state; 
    lmc_tg_update_error_code_t error_code;  
} lmc_tg_update_ret_t;


typedef enum lmc_tg_group_type {
    COMMON,  
    PIECEWISE,     
    TOWARD,     
    SINGROBUST,     
} lmc_tg_group_type_t;

typedef enum lmc_tg_space_type {
    JOINT,  
    CARTESIAN,     
} lmc_tg_space_type_t;

typedef struct lmc_tg_runtime_data {
    unsigned int id; 
    unsigned int is_stopped; 
    double time_cost; 
    lmc_tg_group_type_t group_type; 
    lmc_tg_space_type_t space_type; 
    unsigned int blending;  
} lmc_tg_runtime_data_t;

typedef struct lmc_tg_move_param {
    double v    ;
    double acc; ;
    double time;    ;
    unsigned int smooth_move_to_next; 
    double blend_radius;    ;
    unsigned int interruptable; ;
} lmc_tg_move_param_t;

typedef struct lmc_tg_speed_param {
    double acc; 
    double time;    
    unsigned int pos_constraints;  
} lmc_tg_speed_param_t;

typedef struct lmc_tg_toward_param {
    double v    ;
    double acc; 
} lmc_tg_toward_param_t;

lmc_tg_t* lmc_tg_new();

void lmc_tg_delete(lmc_tg_t** tg_addr);

int lmc_tg_init(lmc_tg_t * const tg, lmc_robot_model_t const* const robot_model, 
lmc_tg_config_t const* const config);

void lmc_tg_cleanup(lmc_tg_t * const tg);



int lmc_tg_set_limit(lmc_tg_t * const tg, lmc_tg_limit_t const* const limit);


void lmc_tg_set_joint_pos(lmc_tg_t * const tg, lmc_joint_state_t const * const joint_state);


int lmc_tg_set_group(lmc_tg_t * const tg, lmc_tg_group_type_t group);


lmc_tg_group_type_t lmc_tg_get_group(lmc_tg_t * const tg);


void lmc_tg_recovery(lmc_tg_t * const tg);


lmc_tg_update_ret_t lmc_tg_update(lmc_tg_t * const tg); 


int lmc_tg_get_updated_date(lmc_tg_t const * const tg, lmc_joint_cmd_t * const joint_cmd, lmc_tg_runtime_data_t * const tg_runtime_data);

lmc_tg_update_state_t lmc_tg_get_update_state(lmc_tg_t const * const tg);



lmc_tg_buffer_ret_t lmc_tg_buffer_move_joint(lmc_tg_t * const tg, int id, double const * const target, lmc_tg_move_param_t const * const param);



lmc_tg_buffer_ret_t lmc_tg_buffer_move_line(lmc_tg_t * const tg, int id, lmc_transform_t const * const target, lmc_tg_move_param_t const * const param);

lmc_tg_buffer_ret_t lmc_tg_buffer_move_circle(lmc_tg_t * const tg, int id, lmc_translation_t const* const way_point, lmc_transform_t const * const target, lmc_tg_move_param_t const * const param);

lmc_tg_buffer_ret_t lmc_tg_buffer_move_circle_by_angle(lmc_tg_t * const tg, int id, lmc_translation_t const* const way_point1, lmc_translation_t const* const way_point2, double angle, lmc_euler_zyx_t const* const target_rot, lmc_tg_move_param_t const * const param);

lmc_tg_buffer_ret_t lmc_tg_buffer_speed_joint(lmc_tg_t * const tg, int id, double const * const vel, lmc_tg_speed_param_t const * const param);


lmc_tg_buffer_ret_t lmc_tg_buffer_speed_line(lmc_tg_t * const tg, int id, lmc_twist_t const * const t, lmc_tg_speed_param_t const * const param);


lmc_tg_buffer_ret_t lmc_tg_buffer_move_joint_pt(lmc_tg_t * const tg, int id, double duration, lmc_joint_cmd_t const* const joint_data);

lmc_tg_buffer_ret_t lmc_tg_buffer_move_joint_pvt(lmc_tg_t * const tg, int id, double duration, lmc_joint_cmd_t const* const joint_data);

lmc_tg_buffer_ret_t lmc_tg_buffer_move_joint_pvat(lmc_tg_t * const tg, int id, double duration, lmc_joint_cmd_t const* const joint_data);

lmc_tg_buffer_ret_t lmc_tg_set_toward_joint(lmc_tg_t * const tg, int id, double const * const target, lmc_tg_toward_param_t const* const param);



lmc_tg_buffer_ret_t lmc_tg_buffer_singularity_robust_move_line(lmc_tg_t * const tg, int id, lmc_transform_t const * const target, lmc_tg_move_param_t const * const param);


void lmc_tg_stop_trajectory(lmc_tg_t * const tg);



void lmc_tg_interrupt_trajectory(lmc_tg_t * const tg);

void lmc_tg_set_vel_scale(lmc_tg_t * const tg, double scale);

void lmc_tg_release_control(lmc_tg_t * const tg, double t);

void lmc_tg_sieze_control(lmc_tg_t * const tg, double vel_scale);


size_t lmc_tg_get_buffer_size(lmc_tg_t const* const tg);

int lmc_tg_get_buffer_empty(lmc_tg_t const* const tg);


int lmc_tg_get_buffer_full(lmc_tg_t const* const tg);

double lmc_tg_get_remain_move_time(lmc_tg_t const* const tg);

int lmc_tg_update_joint_state(lmc_tg_t * const tg, lmc_joint_state_t const * const joint_state);

#ifdef __cplusplus
}
#endif /* __cplusplus */

#endif /* LMC_TG */