/***************************************************************************
* RobotRecursiveInverseDynamics.h *
* ------------------- *
* copyright : (C) 2013 by Richard R. Carrillo and Niceto R. Luque *
* and Peter I. Corke *
* email : rcarrillo,nluque at ugr.es *
***************************************************************************/
/***************************************************************************
* *
* This program is free software; you can redistribute it and/or modify *
* it under the terms of the GNU General Public License as published by *
* the Free Software Foundation; either version 3 of the License, or *
* (at your option) any later version. *
* *
***************************************************************************/
#ifndef _rne_h_
#define _rne_h_
#include <math.h>
#include "MatrixOperations.h"
#define TRUE 1
#define FALSE 0
/*!
* \brief Accessing information within a MATLAB structure is inconvenient and slow. To get around this we build our own robot and link data structures, and copy the information from the MATLAB objects once per call. If the call is for multiple states values then our efficiency becomes very high.
*/
/* Robot kinematic convention */
typedef
enum _dhtype {
STANDARD,
MODIFIED
} DHType;
/* Link joint type */
typedef
enum _axistype {
REVOLUTE = 0,
PRISMATIC = 1
} Sigma;
/* A robot link structure */
typedef struct _link {
/**********************************************************
*************** kinematic parameters *********************
**********************************************************/
double alpha; /* link twist */
double A; /* link offset */
double D; /* link length */
double theta; /* link rotation angle */
double offset; /* link coordinate offset */
int sigma; /* axis type; revolute or prismatic */
/**********************************************************
***************** dynamic parameters *********************
**********************************************************/
/**************** of links ********************************/
Vect *rbar; /* centre of mass of link wrt link origin */
double m; /* mass of link */
double *I; /* inertia tensor of link wrt link origin */
/**************** of actuators *****************************/
/* these parameters are motor referenced */
double Jm; /* actuator inertia */
double G; /* gear ratio */
double B; /* actuator friction damping coefficient */
double *Tc; /* actuator Coulomb friction coeffient */
/**********************************************************
**************** intermediate variables ******************
**********************************************************/
Vect r; /* distance of ith origin from i-1th wrt ith */
Rot R; /* link rotation matrix */
Vect omega; /* angular velocity */
Vect omega_d; /* angular acceleration */
Vect acc; /* acceleration */
Vect abar; /* acceleration of centre of mass */
Vect f; /* inter-link force */
Vect n; /* inter-link moment */
} Link;
/* A robot */
typedef struct _robot {
int njoints; /* number of joints */
Vect *gravity; /* gravity vector */
DHType dhtype; /* kinematic convention */
Link *links; /* the links */
} Robot;
/*!
* \brief Recursive Newton-Euler algorithm.
* It computes the inverse dynamics through the recursive Newton-Euler formulation
* \param robot Pointer to the robot object
* \param tau Pointer to a vector in which the bias torques will be stored
* \param qd Pointer to an array containing the current joint velocities
* \param qdd Pointer to an array containing current joint accelerations
* \param fext Pointer to an array containing the applied force (or load) at the robot's end
* \param stride Is used to allow for input and output arrays which are 2-dimensional but in
* column-major (Matlab) order. We need to access rows from the arrays.
*/
void newton_euler (Robot *robot, double *tau, double *qd, double *qdd, double *fext, int stride);
#endif
| 
