include/barrett/cdlbt/kinematics.h

Go to the documentation of this file.

#ifndef BARRETT_CDLBT_KINEMATICS_H_
#define BARRETT_CDLBT_KINEMATICS_H_
#ifdef __cplusplus
extern "C" {
#endif

#include <libconfig.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>


struct bt_kinematics_link
{
   struct bt_kinematics_link * next; 
   struct bt_kinematics_link * prev; 
   double alpha;
   double theta;
   double a;
   double d;
   double cos_alpha;
   double sin_alpha;
   gsl_matrix * trans_to_prev; 
   gsl_matrix * trans_to_world; 
   gsl_matrix * rot_to_prev; 
   gsl_vector * prev_axis_z; 
   gsl_vector * prev_origin_pos; 
   gsl_matrix * rot_to_world; 
   gsl_vector * axis_z; 
   gsl_vector * origin_pos; 
};


struct bt_kinematics
{
   int dof;
   int nlinks;
   struct bt_kinematics_link ** link_array;
   
   struct bt_kinematics_link * base;
   struct bt_kinematics_link ** link; /* Moving links array */
   struct bt_kinematics_link * toolplate;
   struct bt_kinematics_link * tool;
   
   /* Toolplate Jacobian */
   gsl_matrix * tool_jacobian;
   gsl_matrix * tool_jacobian_linear; /* matrix view */
   gsl_matrix * tool_jacobian_angular; /* matrix view */
   gsl_vector * tool_velocity;
   gsl_vector * tool_velocity_angular;
   
   /* Temp vector */
   gsl_vector * temp_v3;
};


int bt_kinematics_create(struct bt_kinematics ** kinptr,
                         config_setting_t * kinconfig, int ndofs);


int bt_kinematics_destroy(struct bt_kinematics * kin);


int bt_kinematics_eval(struct bt_kinematics * kin, const gsl_vector * jposition,
                       const gsl_vector * jvelocity);


int bt_kinematics_eval_jacobian(struct bt_kinematics * kin, int jlimit,
                                gsl_vector * point, gsl_matrix * jac);

#ifdef __cplusplus
}
#endif
#endif /* BARRETT_CDLBT_KINEMATICS_H_ */