include/barrett/cdlbt/dynamics.h

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

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

#include <barrett/cdlbt/kinematics.h>


struct bt_dynamics_link
{
   struct bt_dynamics_link * next;
   struct bt_dynamics_link * prev;
   double mass;      
   gsl_vector * com; 
   gsl_matrix * I;   
   /* NOTE:
    * Do we need rotor inertia here?? */
   
   /* Next, a place to hold the results from calculations */
   
   gsl_vector * omega;      
   gsl_vector * omega_prev; 
   gsl_vector * alpha;      
   gsl_vector * a;          
   /* A couple of caches, also expressed in local link frame */

   gsl_vector * fnet;
   gsl_vector * tnet;
   gsl_vector * f;      
   gsl_vector * f_next; 
   gsl_vector * t;      
   gsl_matrix * com_jacobian;
   gsl_matrix * com_jacobian_linear;  
   gsl_matrix * com_jacobian_angular; 
};


struct bt_dynamics
{
   int dof;
   int nlinks;
   struct bt_dynamics_link ** link_array;
   
   struct bt_dynamics_link * base;
   struct bt_dynamics_link ** link; /* Moving links array */
   struct bt_dynamics_link * toolplate;
   
   /* Yay JSIM! */
   gsl_matrix * jsim;
   
   /* Temporary Vectors */
   gsl_vector * temp1_v3;
   gsl_vector * temp2_v3;
   
   /* Temporary Matrices */
   gsl_matrix * temp3x3_1;
   gsl_matrix * temp3x3_2;
   gsl_matrix * temp3xn_1;
};


int bt_dynamics_create(struct bt_dynamics ** dynptr,
                       config_setting_t * dynconfig, int ndofs);


int bt_dynamics_destroy(struct bt_dynamics * dyn);


int bt_dynamics_eval_inverse(struct bt_dynamics * dyn,
                             const struct bt_kinematics * kin,
                             const gsl_vector * jvel, const gsl_vector * jacc,
                             gsl_vector * jtor);


int bt_dynamics_eval_jsim(struct bt_dynamics * dyn, struct bt_kinematics * kin);


#ifdef __cplusplus
}
#endif
#endif /* BARRETT_CDLBT_DYNAMICS_H_ */