1  /********************************************************************
  2  * Description: genserkins.h
  3  *   Kinematics for a generalised serial kinematics machine
  4  *
  5  *   Derived from a work by Fred Proctor,
  6  *   changed to work with emc2 and HAL
  7  *
  8  * Adapting Author: Alex Joni
  9  * License: GPL Version 2
 10  * System: Linux
 11  *    
 12  *******************************************************************
 13  
 14    These are the forward and inverse kinematic functions for a general
 15    serial-link manipulator. Thanks to Herman Bruyninckx and John
 16    Hallam at http://www.roble.info/ for this.
 17  
 18    The functions are general enough to be configured for any serial
 19    configuration.  
 20    The kinematics use Denavit-Hartenberg definition for the joint and
 21    links. The DH definitions are the ones used by John J Craig in
 22    "Introduction to Robotics: Mechanics and Control"
 23    The parameters for the manipulator are defined by hal pins.
 24    Currently the type of the joints is hardcoded to ANGULAR, although 
 25    the kins support both ANGULAR and LINEAR axes.
 26    
 27  */
 28  
 29  /*
 30    genserkins.h
 31  */
 32  
 33  #ifndef GENSERKINS_H
 34  #define GENSERKINS_H
 35  
 36  #include "gomath.h"		/* go_pose */
 37  #include "hal.h"		/* HAL data types */
 38  
 39  /*! 
 40    The maximum number of joints supported by the general serial
 41    kinematics. Make this at least 6; a device can have fewer than these.
 42  */
 43  #define GENSER_MAX_JOINTS 6
 44  
 45  #define PI_2 GO_PI_2
 46  
 47  /* default DH parameters, these should be ok for a puma - at least according to Craig */
 48  #define DEFAULT_A1 0
 49  #define DEFAULT_ALPHA1 0
 50  #define DEFAULT_D1 0
 51  
 52  #define DEFAULT_A2 0
 53  #define DEFAULT_ALPHA2 -PI_2
 54  #define DEFAULT_D2 0
 55  
 56  #define DEFAULT_A3 300
 57  #define DEFAULT_ALPHA3 0
 58  #define DEFAULT_D3 70
 59  
 60  #define DEFAULT_A4 50
 61  #define DEFAULT_ALPHA4 -PI_2
 62  #define DEFAULT_D4 400
 63  
 64  #define DEFAULT_A5 0
 65  #define DEFAULT_ALPHA5 PI_2
 66  #define DEFAULT_D5 0
 67  
 68  #define DEFAULT_A6 0
 69  #define DEFAULT_ALPHA6 -PI_2
 70  #define DEFAULT_D6 0
 71  
 72  typedef struct {
 73    go_link links[GENSER_MAX_JOINTS]; /*!< The link description of the device. */
 74    int link_num;		/*!< How many are actually present. */
 75    hal_s32_t iterations;	/*!< How many iterations were actually used to compute the inverse kinematics. */
 76    hal_s32_t max_iterations;	/*!< Number of iterations after which to give up and report an error. */
 77  } genser_struct;
 78  
 79  extern int genser_kin_size(void); 
 80  
 81  extern int genser_kin_init(void); 
 82  
 83  extern const char * genser_kin_get_name(void); 
 84  
 85  extern int genser_kin_num_joints(void * kins);
 86  
 87  extern int genser_kin_fwd(void * kins,
 88  				const go_real *joint,
 89  				go_pose * world);
 90  
 91  extern int genser_kin_inv(void * kins,
 92  				const go_pose * world,
 93  				go_real *joint);
 94  
 95  extern int genser_kin_set_parameters(void * kins, go_link * params, int num); 
 96  
 97  extern int genser_kin_get_parameters(void * kins, go_link * params, int num); 
 98  
 99  extern int genser_kin_jac_inv(void * kins,
100  				    const go_pose * pos,
101  				    const go_screw * vel,
102  				    const go_real * joints, 
103  				    go_real * jointvels); 
104  
105  
106  extern int genser_kin_jac_fwd(void * kins,
107  				    const go_real * joints,
108  				    const go_real * jointvels,
109  				    const go_pose * pos, 
110  				    go_screw * vel); 
111  
112  extern int genser_kin_fwd_interations(genser_struct * genser);
113  
114  
115  /*
116    Extras, not callable using go_kin_ wrapper but if you know you have
117    linked in these kinematics, go ahead and call these for your ad hoc
118    purposes.
119  */
120  
121  /*! Returns the number of iterations used during the last call to the
122    inverse kinematics functions */
123  extern int genser_kin_inv_iterations(genser_struct * genser);
124  
125  /*! Sets the maximum number of iterations to use in future calls to
126    the inverse kinematics functions, after which an error will be
127    reported */
128  extern int genser_kin_inv_set_max_iterations(genser_struct * genser, int i);
129  
130  /*! Returns the maximum number of iterations that will be used to
131   compute inverse kinematics functions */
132  extern int genser_kin_inv_get_max_iterations(genser_struct * genser);
133  
134  #endif