415 SL/include/SL.h
View file

Large diffs are not rendered by default.

54 SL/include/SL_collect_data.h
View file
@@ -0,0 +1,54 @@
/*!=============================================================================
==============================================================================
\file SL_collect_data.h
\author Stefan Schaal
\date Feb 1999
==============================================================================
\remarks
collect_data specific defines
============================================================================*/

#ifndef _SL_collect_data_
#define _SL_collect_data_

/* possible data types */

#define FLOAT 1
#define DOUBLE 2
#define INT 3
#define SHORT 4
#define LONG 5
#define CHAR 6

#ifdef __cplusplus
extern "C" {
#endif

/* shared functions */

void addVarToCollect(char *ptr,const char *name,const char *units, int type, int flag);
void initCollectData( int freq );
void writeToBuffer(void);
void saveData(void);
void scds(void);
void mscds(int num);
void scd(void);
void stopcd(void);
int dscd(int parm);
int startCollectData(void);
void outMenu(void);
void changeCollectFreq( int freq );
void updateDataCollectScript( void );
int getDataCollectPtr( char *name, int n_char_units, void **vptr, int *type, char *units );


#ifdef __cplusplus
}
#endif

#endif /* _SL_collect_data_ */
104 SL/include/SL_common.h
View file
@@ -0,0 +1,104 @@
/*!=============================================================================
==============================================================================
\file SL_common.h
\author Stefan Schaal
\date 2000
==============================================================================
\remarks
declarations needed by SL_common.c
============================================================================*/

#ifndef _SL_common_
#define _SL_common_

#ifdef __cplusplus
extern "C" {
#endif

/* global functions */
int where_utility(int start, int n_dofs);
int where_des_utility(int start, int n_dofs);
int read_sensor_offsets(char *fname);
int read_gains(char *fname, double *gth, double *gthd, double *gint);
int read_link_parameters(char *fname);
int read_whichDOFs(char *fname, char *keyword);
int read_sensor_calibration(char *fname, Matrix joint_lin_rot,
Vector pos_polar, Vector load_polar);
void setDefaultPosture(void);
void cSL_Jstate(SL_Jstate *sd, SL_fJstate *sf, int n, int flag);
void cSL_SDJstate(SL_DJstate *sd, SL_fSDJstate *sf, int n, int flag);
void cSL_DJstate(SL_DJstate *sd, SL_fDJstate *sf, int n, int flag);
void cSL_Cstate(SL_Cstate *sd, SL_fCstate *sf, int n, int flag);
void cSL_Corient(SL_Corient *sd, SL_fCorient *sf, int n, int flag);
void cSL_VisionBlob(SL_VisionBlob *sd, SL_fVisionBlob *sf, int n, int flag);
void cSL_VisionBlobaux(Blob2D sd[][2+1], SL_fVisionBlobaux *sf, int n, int flag);
void cSL_quat(SL_quat *sd, SL_fquat *sf, int n, int flag);
void cBlob3D(Blob3D *sd, fBlob3D *sf, int n, int flag);
void cBlob2D(Blob2D sd[][2+1], fBlob2D *sf, int n, int flag);
void linkQuat(Matrix R, SL_quat *q);
void quatDerivatives(SL_quat *q);
void quatToAngularVelocity(SL_quat *q);
void quatToRotMat(SL_quat *q, Matrix R);
void quatToRotMatInv(SL_quat *q, Matrix R);
void eulerToQuat(Vector a, SL_quat *q);
void eulerToQuatInv(Vector a, SL_quat *q);
void eulerToRotMat(Vector a, Matrix R);
void eulerToRotMatInv(Vector a, Matrix R);
void quatMatrix(SL_quat *q, Matrix Q);
void quatToEuler(SL_quat *q, Vector a);
void quatToEulerInv(SL_quat *q, Vector a);
void rotMatToEuler(Matrix R, Vector a);
void rotMatToEulerInv(Matrix R, Vector a);
void setRealRobotOptions(void);
int read_config_files(char *fname);
double quatError(double* q1, double* q2);
void quatErrorVector(double* q1, double* q2, double *ad);
int read_servoParameters(const char *fname, const char *keyword, int *priority,
int *stacksize, int *cpuID, int *dns);

int
read_parameter_pool_double(const char *fname, const char *keyword, double *value);
int
read_parameter_pool_double_array(const char *fname, const char *keyword, int n_values, double *values);
int
read_parameter_pool_int(const char *fname, const char *keyword, int *ivalue);
int
read_parameter_pool_int_array(const char *fname, const char *keyword, int n_values, int *ivalues);
int
read_parameter_pool_string(const char *fname, const char *keyword, char *svalue);
void
init_parameter_pool(void);
int
parseWindowSpecs(char *string, int dw, int dh, char *xstring, int *x, int *y, int *w, int *h);


int count_extra_contact_points(char *fname);

void rbwhere(void);
void rbwhere2D(void);
void where_base(void);
void where_cog(void);
void where_misc(void);
void revoluteGJacColumn(Vector p, Vector pi, Vector zi, Vector c);
void prismaticGJacColumn(Vector p, Vector pi, Vector zi, Vector c);
void compute_cog(void);

void
compute_local_interface_forces(double *xdd, double *ad, double *add, double *g,
SL_link li, double *f, double *t);



// external variables
extern char config_files[][100];

#ifdef __cplusplus
}
#endif

#endif
43 SL/include/SL_controller.h
View file
@@ -0,0 +1,43 @@
/*!=============================================================================
==============================================================================
\file SL_controller.h
\author Stefan Schaal
\date Feb 1999
==============================================================================
\remarks
SL_controller.c specific header file
============================================================================*/

#ifndef _SL_controller_
#define _SL_controller_

#ifdef __cplusplus
extern "C" {
#endif

/* external variables */

extern int controller_kind;
extern int power_on;

/* share functions */

void ck(void);
void controllerKind(void);
int generate_total_commands( void);
int init_controller(void);
void zero_integrator(void);
void setGains(int);
void setGainsSim(void);
int stop(char *);

#ifdef __cplusplus
}
#endif

#endif /* _SL_controller_ */
89 SL/include/SL_dynamics.h
View file
@@ -0,0 +1,89 @@
/*!=============================================================================
==============================================================================
\file SL_dynamics.h
\author Stefan Schaal
\date Feb 1999
==============================================================================
\remarks
SL_dynamics.c specific header file
============================================================================*/

#ifndef _SL_dynamics_
#define _SL_dynamics_

#define COULOMB_FUNCTION(thd,slope) (tanh(thd*slope))

enum RBDParms {
MASS = 1,
MCMX,
MCMY,
MCMZ,
I11,
I12,
I13,
I22,
I23,
I33,
VIS,
COUL,
STIFF,
CONS,

N_RBDParms
};

#define N_RBD_PARMS (N_RBDParms-1)

/* shared functions */

#ifdef __cplusplus
extern "C" {
#endif

int init_dynamics(void);
void setDefaultEndeffector(void);

void SL_ForDyn(SL_Jstate *lstate,SL_Cstate *cbase,
SL_quat *obase, SL_uext *ux, SL_endeff *leff);
void SL_ForDynArt(SL_Jstate *lstate,SL_Cstate *cbase,
SL_quat *obase, SL_uext *ux, SL_endeff *leff);
void SL_ForDynComp(SL_Jstate *lstate,SL_Cstate *cbase,
SL_quat *obase, SL_uext *ux, SL_endeff *leff,
Matrix rbdM, Vector rbdCG);
void SL_ForwardDynamics(SL_Jstate *lstate,SL_Cstate *cbase,
SL_quat *obase, SL_uext *ux, SL_endeff *leff);
void SL_InverseDynamics(SL_Jstate *cstate,SL_DJstate *state,SL_endeff *endeff);
void SL_InverseDynamicsArt(SL_Jstate *cstate, SL_DJstate *lstate, SL_Cstate *cbase,
SL_quat *obase, SL_uext *ux, SL_endeff *leff);

void SL_InvDyn(SL_Jstate *cstate, SL_DJstate *lstate, SL_endeff *leff,
SL_Cstate *cbase, SL_quat *obase);
void SL_InvDynNE(SL_Jstate *cstate, SL_DJstate *lstate, SL_endeff *leff,
SL_Cstate *cbase, SL_quat *obase);
void SL_InvDynArt(SL_Jstate *cstate, SL_DJstate *lstate, SL_endeff *leff,
SL_Cstate *cbase, SL_quat *obase);
void SL_InvDynNEBase(SL_Jstate *cstate, SL_DJstate *lstate, SL_endeff *leff,
SL_Cstate *cbase, SL_quat *obase, double *fbase);
void test_NEvsForComp( void );
void test_ForArtvsForComp( void );
double compute_independent_joint_forces(SL_Jstate state, SL_link li);



// external variables
extern int freeze_base;
extern double freeze_base_pos[];
extern double freeze_base_quat[];
extern const int floating_base_flag;
extern double coulomb_slope;

#ifdef __cplusplus
}
#endif

#endif /* _SL_dynamics_ */
39 SL/include/SL_filters.h
View file
@@ -0,0 +1,39 @@
/*!=============================================================================
==============================================================================
\file SL_filters.h
\author Stefan Schaal
\date April 2000
==============================================================================
the header file for the filters.c
============================================================================*/


#ifndef _SL_filters_
#define _SL_filters_

#define FILTER_ORDER 2
#define N_FILTERS 50

typedef struct Filter {
int cutoff;
double raw[3];
double filt[3];
} Filter;

#ifdef __cplusplus
extern "C" {
#endif

int init_filters(void);
double filt(double raw, Filter *fptr);

#ifdef __cplusplus
}
#endif

#endif // _SL_filters_
103 SL/include/SL_forDynArt_body.h
View file
@@ -0,0 +1,103 @@
/*!=============================================================================
==============================================================================
\file SL_forDynArt_body.h
\author Stefan Schaal
\date Sept 2010
==============================================================================
\remarks
the articulated body forward dynamics function
Note: this file was converted to a header file as it requires to include
robot specific header files, such that it cannot be added to a SL
library. SL_for_dynamics.c is just an empty file which includes
this header.
============================================================================*/

/* system headers */
#include "SL_system_headers.h"

/* private includes */
#include "utility.h"
#include "utility_macros.h"
#include "SL.h"
#include "SL_user.h"
#include "SL_dynamics.h"
#include "SL_integrate.h"
#include "mdefs.h"


/* global variables */

/* local variables */

/* global functions */

/* local functions */

/* external variables */

/*!*****************************************************************************
*******************************************************************************
\note SL_ForDynArt
\date Sept 2010
\remarks
computes the forward dynamics accelerations from the articulate body inertia
method
*******************************************************************************
Function Parameters: [in]=input,[out]=output
\param[in,out] state : the state containing th, thd, thdd, and receiving the
appropriate u
\param[in,out] cbase : the position state of the base
\param[in,out] obase : the orientational state of the base
\param[in] ux : the external forces acting on each joint,
in world coordinates, e.g., as computed from contact
forces
\param[in] endeff : the endeffector parameters
******************************************************************************/
void
SL_ForDynArt(SL_Jstate *lstate,SL_Cstate *cbase,
SL_quat *obase, SL_uext *ux, SL_endeff *leff)
{
#include "ForDynArt_declare.h"
int i,j;
double fbase[2*N_CART+1];
SL_Jstate *state;
SL_endeff *eff;
SL_Cstate *basec;
SL_quat *baseo;
SL_uext *uex;

// this makes the arguments global variables
state = lstate;
eff = leff;
basec = cbase;
baseo = obase;
uex = ux;

#include "ForDynArt_functions.h"

// subtract the friction term temporarily
for (i=1; i<=N_DOFS; ++i) {
state[i].u -= compute_independent_joint_forces(state[i],links[i]);
}

#include "ForDynArt_math.h"

// add back the friction term
for (i=1; i<=N_DOFS; ++i) {
state[i].u += compute_independent_joint_forces(state[i],links[i]);
}

}


117 SL/include/SL_forDynComp_body.h
View file
@@ -0,0 +1,117 @@
/*!=============================================================================
==============================================================================
\file SL_forDynComp_body.h
\author Stefan Schaal
\date Sept. 2010
==============================================================================
\remarks
the composite inertia forward dynamics function
Note: this file was converted to a header file as it requires to include
robot specific header files, such that it cannot be added to a SL
library. SL_for_dynamics.c is just an empty file which includes
this header.
============================================================================*/

/* system headers */
#include "SL_system_headers.h"

/* private includes */
#include "utility.h"
#include "utility_macros.h"
#include "SL.h"
#include "SL_user.h"
#include "SL_dynamics.h"
#include "SL_integrate.h"
#include "mdefs.h"


/* global variables */

/* local variables */

/* global functions */

/* local functions */

/* external variables */

/*!*****************************************************************************
*******************************************************************************
\note SL_ForDynComp
\date Sept 2010
\remarks
computes the forward dynamics accelerations from the composite inertia
method. Returned are also the pointers to the RBD inertia matrix and the
RBD Coriolis/Centripedal and Gravity vector
*******************************************************************************
Function Parameters: [in]=input,[out]=output
\param[in,out] state : the state containing th, thd, thdd, and receiving the
appropriate u
\param[in,out] cbase : the position state of the base
\param[in,out] obase : the orientational state of the base
\param[in] ux : the external forces acting on each joint,
in world coordinates, e.g., as computed from contact
forces
\param[in] endeff : the endeffector parameters
\param[out] rbdM : point to RBD Inertia Matrix (pass NULL to not use)
\param[out] rbdCG : point to RBD C plus G vector (pass NULL to not use)
******************************************************************************/
void
SL_ForDynComp(SL_Jstate *lstate,SL_Cstate *cbase,
SL_quat *obase, SL_uext *ux, SL_endeff *leff,
Matrix rbdM, Vector rbdCG)

{
#include "ForDynComp_declare.h"
int i,j;
MY_MATRIX(Hmat,1,N_DOFS+6,1,N_DOFS+6);
MY_VECTOR(cvec,1,N_DOFS+6);
MY_VECTOR(ucvec,1,N_DOFS+6);
double fbase[2*N_CART+1];

SL_Jstate *state;
SL_endeff *eff;
SL_Cstate *basec;
SL_quat *baseo;
SL_uext *uex;

#include "ForDynComp_functions.h"

/* this makes the arguments global variables */
state = lstate;
eff = leff;
basec = cbase;
baseo = obase;
uex = ux;

/* subtract the friction term temporarily */
for (i=1; i<=N_DOFS; ++i) {
state[i].u -= compute_independent_joint_forces(state[i],links[i]);
}

#include "ForDynComp_math.h"

/* add back the friction term */
for (i=1; i<=N_DOFS; ++i) {
state[i].u += compute_independent_joint_forces(state[i],links[i]);
}

if (rbdM != NULL)
mat_equal_size(Hmat,N_DOFS+6,N_DOFS+6,rbdM);

if (rbdCG != NULL)
vec_equal_size(cvec,N_DOFS+6,rbdCG);
}


28 SL/include/SL_integrate.h
View file
@@ -0,0 +1,28 @@
#ifndef SL_INTEGRATE_H
#define SL_INTEGRATE_H

#define INTEGRATE_EULER 1
#define INTEGRATE_RK 2

#ifdef __cplusplus
extern "C" {
#endif

void
SL_IntegrateEuler(SL_Jstate *state, SL_Cstate *cbase,
SL_quat *obase, SL_uext *ux,
SL_endeff *leff, double dt, int ndofs,
int flag);
void
SL_IntegrateRK(SL_Jstate *state, SL_Cstate *cbase,
SL_quat *obase, SL_uext *ux,
SL_endeff *leff, double dt, int ndofs);

void freezeBase(int);
void freezeBaseToggle(void);

#ifdef __cplusplus
}
#endif

#endif
160 SL/include/SL_invDynArt_body.h
View file
@@ -0,0 +1,160 @@
/*!=============================================================================
==============================================================================
\file SL_invDynArt_body.h
\author Stefan Schaal
\date Sept. 2010
==============================================================================
\remarks
articulated body inverse dynamics, suitable for floating base inverse
dynamics, but much more expensive to compute that normal inverse
dynamics algorithms.
Note: this file was converted to a header file as it requires to include
robot specific header files, such that it cannot be added to a SL
library. SL_inv_dynamics_art.c is just an empty file which includes
this header.
============================================================================*/

/* system headers */
#include "SL_system_headers.h"

/* private includes */
#include "utility.h"
#include "utility_macros.h"
#include "SL.h"
#include "SL_user.h"
#include "mdefs.h"
#include "SL_dynamics.h"
#include "SL_integrate.h"

/* global variables */

/* local variables */

/* global functions */

/* local functions */

/* external variables */

/*!*****************************************************************************
*******************************************************************************
\note SL_InvDynArt
\date June 1999
\remarks
computes the torques for all joints, and the acceleration of the base
*******************************************************************************
Function Parameters: [in]=input,[out]=output
\param[in] cstate : the current state (pass NULL to use only desired state)
\param[in,out] lstate : the desired state
\param[in] endeff : the endeffector parameters
\param[in,out] cbase : the position state of the base
\param[in,out] obase : the orientational state of the base
******************************************************************************/
void
SL_InvDynArt(SL_Jstate *cstate, SL_DJstate *lstate, SL_endeff *leff,
SL_Cstate *cbase, SL_quat *obase)

{
#include "InvDynArt_declare.h"
int i,j;
SL_uext ux[N_DOFS+1];

SL_DJstate state[N_DOFS+1];
SL_endeff *eff;
SL_Cstate *basec;
SL_quat *baseo;
SL_uext *uex;

#include "InvDynArt_functions.h"

/* the following assignments make the arguments global variables */

/* create a mixed desired/current state for proper inverse dynamics */
for (i=1; i<=N_DOFS; ++i) {
state[i] = lstate[i];
if (cstate != NULL) {
state[i].th = cstate[i].th;
state[i].thd = cstate[i].thd;
}
for (j=1; j<=N_CART; ++j)
ux[i].f[j] = ux[i].t[j] = 0.0;

}

// zero out the base forces
for (j=1; j<=N_CART; ++j)
ux[0].f[j] = ux[0].t[j] = 0.0;

eff = leff;
basec = cbase;
baseo = obase;
uex = ux;


#include "InvDynArt_math.h"

/* add the friction term */
for (i=1; i<=N_DOFS; ++i) {
SL_Jstate jt;
jt.th = state[i].th;
jt.thd = state[i].thd;
state[i].uff += compute_independent_joint_forces(jt,links[i]);
if (i > N_DOFS-N_DOFS_EST_SKIP)
state[i].uff = 0.0;
lstate[i].uff = state[i].uff;
}

}

/*!*****************************************************************************
*******************************************************************************
\note SL_InverseDynamicsArt
\date June 1999
\remarks
computes the torques for all joints, and the acceleration of the
base
*******************************************************************************
Function Parameters: [in]=input,[out]=output
\param[in] cstate : the current state (pass NULL to use only desired state)
\param[in,out] lstate : the desired state
\param[in,out] cbase : the position state of the base
\param[in,out] obase : the orientational state of the base
\param[in] ux : the external forces acting on each joint, in local
(link) coordinates, e.g., as measured by a local force
or torque cell
\param[in] endeff : the endeffector parameters
******************************************************************************/
void
SL_InverseDynamicsArt(SL_Jstate *cstate, SL_DJstate *lstate, SL_Cstate *cbase,
SL_quat *obase, SL_uext *ux, SL_endeff *leff)

{
int i,j;
static int firsttime = TRUE;
static int counter = 0;

if (firsttime) {
printf("SL_InverseDynamicsArt is depricated -- use SL_InvDynArt instead\n");
if (++counter > 10)
firsttime = FALSE;
}

SL_InvDynArt(cstate, lstate, leff, cbase, obase);
}

226 SL/include/SL_invDynNE_body.h
View file
@@ -0,0 +1,226 @@
/*!=============================================================================
==============================================================================
\file SL_invDynNE_body.h
\author Stefan Schaal
\date Sept. 2010
==============================================================================
\remarks
The Recursive Newton Euler inverse dynamics functions
Note: this file was converted to a header file as it requires to include
robot specific header files, such that it cannot be added to a SL
library. SL_invDynNE.c is just an empty file which includes
this header.
============================================================================*/

// system headers
#include "SL_system_headers.h"

// private includes
#include "utility.h"
#include "utility_macros.h"
#include "SL.h"
#include "SL_user.h"
#include "SL_common.h"
#include "mdefs.h"
#include "SL_dynamics.h"

// local variables

// global functions

// local functions
static void
SL_InvDynNEGeneral(SL_Jstate *cstate,SL_DJstate *lstate,SL_endeff *leff,
SL_Cstate *cbase, SL_quat *obase, SL_uext *ux,
double *fbase);

// external variables

/*!*****************************************************************************
*******************************************************************************
\note SL_InvDynNE
\date Sept 2010
\remarks
Standard Newton Euler inverse dynamics for fixed base robot.
*******************************************************************************
Function Parameters: [in]=input,[out]=output
\param[in] cstate : the current state (pass NULL to use only desired state)
\param[in,out] lstate : the desired state
\param[in] endeff : the endeffector parameters
\param[in] cbase : the position state of the base
\param[in] obase : the orientational state of the base
Returns:
The appropriate feedforward torques are added in the uff component of the lstate
structure.
******************************************************************************/
void
SL_InvDynNE(SL_Jstate *cstate, SL_DJstate *lstate, SL_endeff *leff,
SL_Cstate *cbase, SL_quat *obase)
{
int i,j;
double fbase[2*N_CART+1];
SL_Cstate cb;
SL_quat ob;
SL_uext ux[N_DOFS+1];

// this simple version of NE does only take the base position/orientation
// into account, but otherwise the base is fixed.
cb = *cbase;
ob = *obase;

// eliminate all velocity and acceleration information
for (i=1; i<=N_CART; ++i)
cb.xd[i] = cb.xdd[i] = 0.0;

for (i=1; i<=N_CART; ++i)
ob.ad[i] = ob.add[i] = 0.0;

for (i=1; i<=N_QUAT; ++i)
ob.qd[i] = ob.qdd[i] = 0.0;

for (i=1; i<=N_DOFS; ++i)
for (j=1; j<=N_CART; ++j)
ux[i].f[j] = ux[i].t[j] = 0.0;


SL_InvDynNEGeneral(cstate, lstate, leff, &cb, &ob, ux, fbase);

}

/*!*****************************************************************************
*******************************************************************************
\note SL_InvDynNEBase
\date Sept 2010
\remarks
Standard Newton Euler inverse dynamics for fixed base robot, but this function
allows a full base state (including velocities and accelerations). This is to
be interpretated as if the base if full actuated, and the force/torque vector
in world coordinates is returned to reflect this required base command.
*******************************************************************************
Function Parameters: [in]=input,[out]=output
\param[in] cstate : the current state (pass NULL to use only desired state)
\param[in,out] lstate : the desired state
\param[in] endeff : the endeffector parameters
\param[in] cbase : the position state of the base
\param[in] obase : the orientational state of the base
\param[out] fbase : the force/torque vector of the base in world coordinates
Returns:
The appropriate feedforward torques are added in the uff component of the lstate
structure. The force/torque vector for the base is returned
******************************************************************************/
void
SL_InvDynNEBase(SL_Jstate *cstate, SL_DJstate *lstate, SL_endeff *leff,
SL_Cstate *cbase, SL_quat *obase, double *fbase)
{
int i,j;
SL_Cstate cb;
SL_quat ob;
SL_uext ux[N_DOFS+1];

// this simple version of NE does only take the base position/orientation
// into account, but otherwise the base is fixed.
cb = *cbase;
ob = *obase;

// eliminate all external forces
for (i=1; i<=N_DOFS; ++i)
for (j=1; j<=N_CART; ++j)
ux[i].f[j] = ux[i].t[j] = 0.0;

SL_InvDynNEGeneral(cstate, lstate, leff, &cb, &ob, ux, fbase);

}


/*!*****************************************************************************
*******************************************************************************
\note SL_InvDynNEGeneral
\date Sept 2010
\remarks
This is the generalized inverse dynamics function which computes the NE
inverse dynamics for various special cases, as intialized by specific
subroutines.
*******************************************************************************
Function Parameters: [in]=input,[out]=output
\param[in] cstate : the current state (pass NULL to use only desired state)
\param[in,out] lstate : the desired state
\param[in] endeff : the endeffector parameters
\param[in] cbase : the position state of the base
\param[in] obase : the orientational state of the base
\param[in] ux : the external forces acting on each joint, in world
coordinates, e.g., as computed from contact forces
\param[out] fbase : the force/torque vector of the base in world coordinates
******************************************************************************/
static void
SL_InvDynNEGeneral(SL_Jstate *cstate,SL_DJstate *lstate,SL_endeff *leff,
SL_Cstate *cbase, SL_quat *obase, SL_uext *ux,
double *fbase)

{
#include "InvDynNE_declare.h"
int i;

SL_DJstate state[N_DOFS+1];
SL_endeff *eff;
SL_Cstate *basec;
SL_quat *baseo;
SL_uext *uex;

// this makes the arguments global variables
eff = leff;
basec = cbase;
baseo = obase;
uex = ux;

#include "InvDynNE_functions.h"

// create a mixed desired/current state for proper inverse dynamics
for (i=1; i<=N_DOFS; ++i) {
state[i] = lstate[i];
if (cstate != NULL) {
state[i].th = cstate[i].th;
state[i].thd = cstate[i].thd;
}
}

#include "InvDynNE_math.h"

// add the friction term
for (i=1; i<=N_DOFS; ++i) {
SL_Jstate jt;
jt.th = state[i].th;
jt.thd = state[i].thd;
state[i].uff += compute_independent_joint_forces(jt,links[i]);
if (i > N_DOFS-N_DOFS_EST_SKIP)
state[i].uff = 0.0;
lstate[i].uff = state[i].uff;
}

}


66 SL/include/SL_kinematics.h
View file
@@ -0,0 +1,66 @@
/*!=============================================================================
==============================================================================
\file SL_kinematics.h
\author Stefan Schaal
\date Feb 1999
==============================================================================
\remarks
SL_kinematics.c specific header file
============================================================================*/

#ifndef _SL_kinematics_
#define _SL_kinematics_

/* external variables */


/* shared functions */

#ifdef __cplusplus
extern "C" {
#endif

void init_kinematics(void);
void genJacobian(Vector point, int link, Matrix jop, Matrix jap, Matrix J);
void jacobian(Matrix lp, Matrix jop, Matrix jap, Matrix J);
void baseJacobian(Matrix lp, Matrix jop, Matrix jap, Matrix Jb);
double inverseKinematics(SL_DJstate *state, SL_endeff *endeff, SL_OJstate *rest,
Vector cart, iVector status, double dt);
double inverseKinematicsClip(SL_DJstate *state, SL_endeff *eff, SL_OJstate *rest,
Vector cart, iVector status, double dt, double max_rev,
double max_pris);

void linkInformation(SL_Jstate *state,SL_Cstate *basec,
SL_quat *baseo, SL_endeff *eff,
double **Xmcog, double **Xaxis, double **Xorigin,
double **Xlink, double ***Ahmat, double ***Ahmatdof);
void linkInformationDes(SL_DJstate *state,SL_Cstate *basec,
SL_quat *baseo, SL_endeff *eff,
double **Xmcog, double **Xaxis, double **Xorigin,
double **Xlink, double ***Ahmat, double ***Ahmatdof);
void
computeLinkVelocity(int lID, Matrix lp, Matrix jop, Matrix jap,
SL_Jstate *js, double *v);
void
computeConstraintJacobian(SL_Jstate *state,SL_Cstate *basec,
SL_quat *baseo, SL_endeff *eff,
Matrix Jc, int *nr, int *nc);

void
computeQR(Matrix Jc, int nr, int nc, Matrix Q, Matrix Qu, Matrix R, Vector sv);

int
checkIKTarget(SL_DJstate *js, SL_Cstate *bs, SL_quat *bo, SL_endeff *eff,
SL_Cstate *ct, int *status, int max_iter);


#ifdef __cplusplus
}
#endif

#endif /* _SL_kinematics_ */
977 SL/include/SL_kinematics_body.h
View file

Large diffs are not rendered by default.

35 SL/include/SL_man.h
View file
@@ -0,0 +1,35 @@
/*!=============================================================================
==============================================================================
\file SL_man.h
\author Stefan Schaal
\date Feb 1999
==============================================================================
\remarks
SL_man.c specific header file
============================================================================*/

#ifndef _SL_man_
#define _SL_man_

/* external variables */

/* share functions */

#ifdef __cplusplus
extern "C" {
#endif

void initMan(void);
void man(void);
void addToMan(const char *abr, const char *string, void (*fptr)(void));

#ifdef __cplusplus
}
#endif

#endif /* _SL_man_ */
52 SL/include/SL_motor_servo.h
View file
@@ -0,0 +1,52 @@
/*!=============================================================================
==============================================================================
\file SL_motor_servo.h
\author Stefan Schaal
\date Feb 1999
==============================================================================
\remarks
motor_servo specific header file
============================================================================*/

#ifndef _SL_motor_servo_
#define _SL_motor_servo_

#ifndef SYS_CLOCK_RATE
#define SYS_CLOCK_RATE 60
#endif

#ifdef __cplusplus
extern "C" {
#endif

extern long motor_servo_calls;
extern int motor_servo_rate;
extern double last_motor_servo_time;
extern double motor_servo_time;
extern double servo_time;
extern int servo_enabled;
extern int *zero_ufb_P_flag;
extern int *zero_ufb_D_flag;
extern int real_time_clock_flag;

/* share functions */

int read_register_file(char *fname_in);
void dms(void);
void disable_motor_servo(void);
void status(void);
void init_motor_servo(void);
int run_motor_servo(void);
int init_vxworks( void );
int init_user_motor(void);

#ifdef __cplusplus
}
#endif

#endif /* _SL_motor_servo_ */
130 SL/include/SL_objects.h
View file
@@ -0,0 +1,130 @@
/*!=============================================================================
==============================================================================
\file SL_objects.h
\author Stefan Schaal
\date Nov. 2007
==============================================================================
\remarks
SL_objects.c specific header file
============================================================================*/

#ifndef _SL_objects_
#define _SL_objects_


#include "SL_objects_defines.h"


/*! structure to create objects in the environment */
typedef struct Object {
char name[STRING100]; /*!< object name */
int type; /*!< object type */
int contact_model; /*!< which contact model to be used */
double trans[N_CART+1]; /*!< translatory offset of object */
double rot[N_CART+1]; /*!< rotational offset of object */
double scale[N_CART+1]; /*!< scaling in x,y,z */
double rgb[N_CART+1]; /*!< color information */
double *contact_parms; /*!< contact parameters */
double *object_parms; /*!< object parameters */
char *nptr; /*!< pointer to next object */
double f[N_CART+1]; /*!< forces acting on object in world coordinates */
double t[N_CART+1]; /*!< torques acting on object in world coordinates */
int display_list_active; /*!< display list for open GL (only for terrains) */
int hide; /*!< allows hiding the object in the display */
} Object, *ObjectPtr;


#define MAX_CONNECTED 25

/*! structure to deal with contact forces */
typedef struct Contact {
int active; /*!< TRUE/FALSE: indicates whether this point should be checked for contacts */
int status; /*!< contact is true or false */
int friction_flag; /*!< flag for switching between different friction models */
ObjectPtr optr; /*!< ptr ofx object that is contacted */
int base_dof_start; /*!< to which DOF does this point connect? */
int off_link_start; /*!< which link should be used for moment arm */
int base_dof_end; /*!< to which DOF does this point connect? */
int off_link_end; /*!< which link should be used for moment arm */
int id_start; /*!< link ID where line starts */
int id_end; /*!< link ID where line ends */
double fraction_start; /*!< fraction relative to start point */
double fraction_end; /*!< fraction relative to end point */
double x[N_CART+1]; /*!< point of contact in object coordintates */
double x_start[N_CART+1]; /*!< point of first contact */
double normal[N_CART+1]; /*!< normal displacement vector */
double normvel[N_CART+1]; /*!< normal velocity vector */
double tangent[N_CART+1]; /*!< tangential displacement vector */
double tanvel[N_CART+1]; /*!< tangential velocity vector */
double viscvel[N_CART+1]; /*!< velocity vector for viscous friction */
double f[N_CART+1]; /*!< contact forces in world coordinates */
double n[N_CART+1]; /*!< contact normal in world coordinates */
double face_index; /*!< _X_, _Y_, or _Z_ to indicate with which face we are in contact */
int n_connected_links; /*!< number of connected links */
int connected_links[MAX_CONNECTED+1]; /*!< list of connected links (only used for link end points, not intermediate points */
int force_condition[MAX_CONNECTED+1]; /*!< what force conditions are permitted */
// these options are only used for special point contacts
int point_contact_flag; /*!< indicates that this is a special point contact in local coordinates with norm vector */
double local_point_pos[N_CART+1]; /*!< position of point contact in local coordinates */
double local_point_norm[N_CART+1]; /*!< normal vector of point contact in local coordinates */

} Contact, *ContactPtr;


#ifdef __cplusplus
extern "C" {
#endif

// external variables

// shared functions
int initObjects(void);

ObjectPtr addSphere(char *name, double *rgb, double *pos, double *rot,
double *scale, int contact,
double *parms, int n_faces);
ObjectPtr addCube(char *name, double *rgb, double *pos, double *rot,
double *scale, int contact,
double *parms);
ObjectPtr addCylinder(char *name, double *rgb, double *pos, double *rot,
double *scale, int contact,
Vector parms, int n_faces);
ObjectPtr addObject(char *name, int type, int contact,
double *rgb, double *trans, double *rot,
double *scale, Vector cspecs, Vector ospecs);

void checkContacts(void);
void readObjects(char *fname);

int changeObjPosByName(char *name, double *pos, double *rot);
int deleteObjByName(char *name);
int changeHideObjByName(char *name, int hide);
int getObjForcesByName(char *name, double *f, double *t);

void changeObjPosByPtr(ObjectPtr ptr, double *pos, double *rot);
int getObjForcesByPtr(ObjectPtr ptr, double *f, double *t);
ObjectPtr getObjPtrByName(char *name);

int read_extra_contact_points(char *fname);
void computeContactPoint(ContactPtr cptr, double **lp, double ***al, double *x);




// external variables
extern ObjectPtr objs;
extern ContactPtr contacts;
extern SL_uext *ucontact;
extern int n_contacts;


#ifdef __cplusplus
}
#endif

#endif /* _SL_objects_ */
58 SL/include/SL_objects_defines.h
View file
@@ -0,0 +1,58 @@
/*!=============================================================================
==============================================================================
\file SL_objects_defines.h
\author Stefan Schaal
\date May 2010
==============================================================================
\remarks
SL_objects.c specific header file -- just the defines
============================================================================*/

#ifndef _SL_objects_defines_
#define _SL_objects_defines_

/*! possible object types */
#define CUBE 1
#define SPHERE 2
#define TERRAIN 3
#define CYLINDER 4

#define N_CUBE_PARMS 0
#define N_SPHERE_PARMS 1
#define N_TERRAIN_PARMS 0
#define N_CYLINDER_PARMS 1

/*! possible contact models */
#define NO_CONTACT 0
#define DAMPED_SPRING_STATIC_FRICTION 1
#define DAMPED_SPRING_VISCOUS_FRICTION 2
#define DAMPED_SPRING_LIMITED_REBOUND 3

#define N_NO_CONTACT_PARMS 0
#define N_DAMPED_SPRING_STATIC_FRICTION_PARMS 6
#define N_DAMPED_SPRING_VISCOUS_FRICTION_PARMS 3
#define N_DAMPED_SPRING_LIMITED_REBOUND_PARMS 8

#define MAX_OBJ_PARMS 30
#define MAX_CONTACT_PARMS 30

#ifdef __cplusplus
extern "C" {
#endif

// external variables

// shared functions

// external variables

#ifdef __cplusplus
}
#endif

#endif /* _SL_objects_defines_ */
139 SL/include/SL_openGL.h
View file
@@ -0,0 +1,139 @@
/*!=============================================================================
==============================================================================
\file SL_openGL.h
\author Stefan Schaal
\date Feb. 1999
==============================================================================
\remarks
Header file for SL_OpenGL.c and related programs
============================================================================*/

#ifndef _SL_openGL_
#define _SL_openGL_

/*! defines */
#define LEFT_MENU 1
#define MIDDLE_MENU 2
#define RIGHT_MENU 3

/*! basic window settings */
#define EYEX0 0.7
#define EYEY0 2.0
#define EYEZ0 0.3
#define CENTERX0 0.0
#define CENTERY0 0.0
#define CENTERZ0 0.0
#define UPX0 0.0
#define UPY0 0.0
#define UPZ0 1.0

/*! structure to create and maintain openGL windows */
typedef struct OpenGLWindow {
int openGLId; /*!< unique integer assigned by OpenGL
to window */
int ID; /*!< ID assigned by SL */
char *wptr; /*!< window pointer, assigned by the
OS-specific operating system */
int x; /*!< x position of window */
int y; /*!< y position of window */
int height; /*!< height of window */
int width; /*!< width of window */
char name[100]; /*!< name of window */
int hide; /*!< hide window or not */
int hide_me; /*!< request to hide */
int show_me; /*!< request to show */
int draw_axis; /*!< TRUE/FALSE */
int follow_basis; /*!< TRUE/FALSE */
double fovea; /*!< fovea angle vertical (image y) in degrees */
/*!< view variables for the window based on gluLookAt */
GLdouble eye[N_CART+1];
GLdouble center[N_CART+1];
GLdouble up[N_CART+1];
/*!< default view variables for the window needed to reset the view */
GLdouble eye0[N_CART+1];
GLdouble center0[N_CART+1];
GLdouble up0[N_CART+1];
/*!< glut function associated with the window */
void (*idle)();
void (*display)();
void (*keyboard)(unsigned char, int, int);
void (*mouse)(int, int, int, int);
void (*motion)(int, int);
void (*reshape)();
void (*special)();
void (*menu)();
/*!< pointer to next window */
char *next_wptr;

} OpenGLWindow, *OpenGLWPtr;

#ifdef __cplusplus
extern "C" {
#endif

/* global functions */
int initGraphics(int* argc, char*** argv);
int init_user_openGL(int argc, char** argv);
int createWindow(OpenGLWindow *wptr);
OpenGLWPtr getOpenGLWindow(void);
void addMenuItem(char *name, void (*fptr)(void), int mID);
void drawObjects(void);
void drawContacts(double);
void glutPostRedisplayAll(void);
OpenGLWPtr whichGLWindow(void);
void toggleHideWindow(OpenGLWPtr ptr);
void changeWindowUpdateRate(double rate);
void hideWindowByName(char *name, int hide);
void clmcplotUpdateState(void);
void playbackUpdateState(void);
void userGraphicsUpdateState(void);
int initUserGraphics(void);
int checkForMessages(void);
void followBaseByName(char *name, int follow);
void followBaseByPtr(OpenGLWPtr ptr, int follow);
void setUserGraphicsUpdateMode(int mode);
void switchCheckerBoard(int flag);
void displayComet(void);
void updateComet(void);
void switchEndeffectorCometDisplay(int id, int s);
void switchLinkCometDisplay(int id, int s);
void switchCometDisplay(int flag, int n_steps);
void resetCometDisplay(void);
void displayCoord(void);
void receiveOscilloscopeData(void);
int initOscWindow(void);
int displayListFromObjFile(char *fname, double scale);
int displayListFromObjFileFlag(char *fname, double scale, int flag);
void toggleShowAxesByName(char *name, int status);
void toggleShowAxesByPtr(OpenGLWPtr ptr,int status);
void drawArrow(double *sp, double *ep, double width);


/* exported variables */
extern int solid;
extern int pause_flag;
extern double window_update_rate;
extern int real_time;
extern int clmcplot_mode;
extern int playback_mode;
extern int userGraphics_mode;
extern Matrix playback_D;
extern int playback_n_cols;
extern int playback_current_row;
extern char **playback_vnames;
extern SL_Jstate* userGraphics_joint_state;
extern SL_Cstate userGraphics_base_state;
extern SL_quat userGraphics_base_orient;
extern int cometDisplay;

#ifdef __cplusplus
}
#endif


#endif /* _SL_openGL_ */
43 SL/include/SL_openGL_servo.h
View file
@@ -0,0 +1,43 @@
/*!=============================================================================
==============================================================================
\file SL_openGL_servo.h
\author Stefan Schaal
\date Nov. 2007
==============================================================================
\remarks
SL_openGL_servo specific header file
============================================================================*/

#ifndef _SL_openGL_servo_
#define _SL_openGL_servo_

#ifdef __cplusplus
extern "C" {
#endif

// external variables
extern double servo_time;
extern double openGL_servo_time;
extern long openGL_servo_calls;
extern long last_openGL_servo_calls;
extern int openGL_servo_rate;
extern int openGL_servo_errors;
extern int stand_alone_flag;
extern int servo_enabled;

// shared functions
int receive_sim_state(void);
int receive_misc_sensors(void);
int receive_contacts(void);
int init_openGL_servo(int argc, char** argv);

#ifdef __cplusplus
}
#endif

#endif /* _SL_openGL_servo_ */
38 SL/include/SL_oscilloscope.h
View file
@@ -0,0 +1,38 @@
/*!=============================================================================
==============================================================================
\file SL_oscilloscope.h
\author Peter Pastor, Stefan Schaal
\date Nov. 2009
==============================================================================
\remarks
declarations needed by SL_oscilloscope.c
============================================================================*/

#ifndef _SL_oscilloscope_
#define _SL_oscilloscope_

#ifdef __cplusplus
extern "C" {
#endif

// global functions
void initOsc(void);
int setOsc(int channel, double pval);
void setD2AFunction(int (*fptr)(int,double));
void addEntryOscBuffer(char *name, double v, double ts, int cID);
void sendOscilloscopeData(void);
void updateOscVars(void);

// external variables


#ifdef __cplusplus
}
#endif

#endif
1,753 SL/include/SL_parm_estimate_body.h
View file

Large diffs are not rendered by default.

56 SL/include/SL_ros_communicator.h
View file
@@ -0,0 +1,56 @@
/*!=============================================================================
==============================================================================
\ingroup SLros
\file SL_ros_communication.c
\author Peter Pastor
\date July 2010
==============================================================================
\remarks
this file contains the functionality to publish SL variables to ROS
============================================================================*/
#ifndef SL_ROS_PUBLISHER_H_
#define SL_ROS_PUBLISHER_H_

// boost includes
#include <boost/shared_ptr.hpp>

// ROS includes
#include <ros/ros.h>

// ROS msgs includes
#include <sensor_msgs/JointState.h>
#include <geometry_msgs/PoseStamped.h>

namespace sl2ros
{

class SL_ros_communicator
{

public:

SL_ros_communicator();
virtual ~SL_ros_communicator();

bool initialize();
bool publish();

private:

boost::shared_ptr<ros::NodeHandle> node_handle_;
ros::Publisher nrt_joint_state_publisher_;
ros::Publisher nrt_endeffector_publisher_;
boost::shared_ptr<sensor_msgs::JointState> joint_state_msg_;
boost::shared_ptr<geometry_msgs::PoseStamped> pose_stamped_msg_;

};

}

#endif /* SL_ROS_PUBLISHER_H_ */
58 SL/include/SL_ros_publisher.h
View file
@@ -0,0 +1,58 @@
/*!=============================================================================
==============================================================================
\ingroup SLros
\file SL_ros_publisher.c
\author Peter Pastor
\date July 2010
==============================================================================
\remarks
this file contains the functionality to publish SL variables to ROS
============================================================================*/
#ifndef SL_ROS_PUBLISHER_H_
#define SL_ROS_PUBLISHER_H_

// first include ROS
#include <ros/ros.h>
#include <rosrt/rosrt.h>
#include <boost/shared_ptr.hpp>

#include <sensor_msgs/JointState.h>
#include <geometry_msgs/PoseStamped.h>
// #include <sl2ros_msgs/SL2ROS.h>

#include "SL.h"

namespace sl2ros
{

class SL_ros_publisher
{

public:

SL_ros_publisher();
virtual ~SL_ros_publisher();

bool initialize();

bool publish();

private:

ros::NodeHandle *node_handle_;

boost::shared_ptr<rosrt::Publisher<sensor_msgs::JointState> > joint_state_publisher_;
boost::shared_ptr<rosrt::Publisher<geometry_msgs::PoseStamped> > endeffector_publisher_;
// boost::shared_ptr<rosrt::Publisher<sl2ros_msgs::SL2ROS> > other_msg_publisher_;

};

}

#endif /* SL_ROS_PUBLISHER_H_ */
42 SL/include/SL_ros_servo.h
View file
@@ -0,0 +1,42 @@
/*!=============================================================================
==============================================================================
\file SL_ros_servo.h
\author Stefan Schaal
\date July 2010
==============================================================================
the header file for the ros_servo.c
============================================================================*/


#ifndef _SL_ros_servo_
#define _SL_ros_servo_

#ifdef __cplusplus
extern "C" {
#endif

extern int ros_servo_errors;
extern long ros_servo_calls;
extern int ros_servo_initialized;
extern double last_ros_servo_time;
extern double ros_servo_time;
extern double servo_time;
extern int servo_enabled;
extern int ros_servo_rate;

void init_ros_servo(void);
int run_ros_servo(void);
int init_user_ros(void);
int run_user_ros(void);
void status(void);

#ifdef __cplusplus
}
#endif

#endif // _SL_ros_servo_
247 SL/include/SL_rt_mutex.h
View file
@@ -0,0 +1,247 @@
/*!=============================================================================
==============================================================================
\file SL_rt_mutex.h
\author Mrinal Kalakrishnan
\date Nov. 2010
==============================================================================
\remarks
This file defines a type "sl_rt_mutex", which is a wrapper for a real-time mutex.
It uses Xenomai mutexes on the Xenomai RTOS, and pthread mutexes otherwise.
We attempt to mimic the pthread_mutex_* api.
Pthread condition functions are also wrapped as sl_rt_cond_*.
Using this wrappers turned out to be more stable than using the xenomai
posix-skin, which caused problems in 64-bit Xenomai.
TODOs:
Error codes are not converted yet.
Calls with timeout values are not implemented yet.
============================================================================*/

#ifndef SL_RT_MUTEX_H_
#define SL_RT_MUTEX_H_

#define SL_RT_MUTEX_WARNINGS 1

#include <stdio.h>

#ifdef __XENO__
#include <native/mutex.h>
#include <native/cond.h>
#include <native/timer.h>
#else
#include <pthread.h>
#include <sys/time.h>
#endif

#ifdef __cplusplus
extern "C" {
#endif

#ifdef __XENO__
typedef RT_MUTEX sl_rt_mutex;
typedef RT_COND sl_rt_cond;
#else
typedef pthread_mutex_t sl_rt_mutex;
typedef pthread_cond_t sl_rt_cond;
#endif

typedef unsigned long long sl_rt_time; //time in nanoseconds

static int sl_rt_mutex_init(sl_rt_mutex* mutex);
static int sl_rt_mutex_destroy(sl_rt_mutex* mutex);
static int sl_rt_mutex_lock(sl_rt_mutex* mutex);
static int sl_rt_mutex_trylock(sl_rt_mutex* mutex);
static int sl_rt_mutex_unlock(sl_rt_mutex* mutex);

static int sl_rt_cond_init(sl_rt_cond* cond);
static int sl_rt_cond_destroy(sl_rt_cond* cond);
static int sl_rt_cond_signal(sl_rt_cond* cond);
static int sl_rt_cond_broadcast(sl_rt_cond* cond);
static int sl_rt_cond_wait(sl_rt_cond* cond, sl_rt_mutex* mutex);
static int sl_rt_cond_timedwait(sl_rt_cond* cond, sl_rt_mutex* mutex, sl_rt_time timeout);
static int sl_rt_cond_timedwait_relative(sl_rt_cond* cond, sl_rt_mutex* mutex, sl_rt_time timeout);

static void sl_rt_warning(const char* function_name, int error_code);

#ifdef __cplusplus
}
#endif

/////////////////////////////////////////
// inline function definitions follow:

static inline int sl_rt_mutex_init(sl_rt_mutex* mutex)
{
#ifdef __XENO__
int res = rt_mutex_create(mutex, NULL);
if (SL_RT_MUTEX_WARNINGS && res)
sl_rt_warning("rt_mutex_create", res);
return res;
#else
return pthread_mutex_init(mutex, NULL);
#endif
}

static inline int sl_rt_mutex_destroy(sl_rt_mutex* mutex)
{
#ifdef __XENO__
int res = rt_mutex_delete(mutex);
if (SL_RT_MUTEX_WARNINGS && res)
sl_rt_warning("rt_mutex_delete", res);
return res;
#else
return pthread_mutex_destroy(mutex);
#endif
}

static inline int sl_rt_mutex_lock(sl_rt_mutex* mutex)
{
#ifdef __XENO__
int res = rt_mutex_acquire(mutex, TM_INFINITE);
if (SL_RT_MUTEX_WARNINGS && res)
sl_rt_warning("rt_mutex_acquire", res);
return res;
#else
return pthread_mutex_lock(mutex);
#endif
}

static inline int sl_rt_mutex_trylock(sl_rt_mutex* mutex)
{
#ifdef __XENO__
int res = rt_mutex_acquire(mutex, TM_NONBLOCK);
if (SL_RT_MUTEX_WARNINGS && res && res !=-EWOULDBLOCK)
sl_rt_warning("rt_mutex_acquire", res);
return res;
#else
return pthread_mutex_trylock(mutex);
#endif
}

static inline int sl_rt_mutex_unlock(sl_rt_mutex* mutex)
{
#ifdef __XENO__
int res = rt_mutex_release(mutex);
if (SL_RT_MUTEX_WARNINGS && res)
sl_rt_warning("rt_mutex_unlock", res);
return res;
#else
return pthread_mutex_unlock(mutex);
#endif
}

static inline int sl_rt_cond_init(sl_rt_cond* cond)
{
#ifdef __XENO__
int res = rt_cond_create(cond, NULL);
if (SL_RT_MUTEX_WARNINGS && res)
sl_rt_warning("rt_cond_create", res);
return res;
#else
return pthread_cond_init(cond, NULL);
#endif
}

static inline int sl_rt_cond_destroy(sl_rt_cond* cond)
{
#ifdef __XENO__
int res = rt_cond_delete(cond);
if (SL_RT_MUTEX_WARNINGS && res)
sl_rt_warning("rt_cond_delete", res);
return res;
#else
return pthread_cond_destroy(cond);
#endif
}

static inline int sl_rt_cond_signal(sl_rt_cond* cond)
{
#ifdef __XENO__
int res = rt_cond_signal(cond);
if (SL_RT_MUTEX_WARNINGS && res)
sl_rt_warning("rt_cond_signal", res);
return res;
#else
return pthread_cond_signal(cond);
#endif
}

static inline int sl_rt_cond_broadcast(sl_rt_cond* cond)
{
#ifdef __XENO__
int res = rt_cond_broadcast(cond);
if (SL_RT_MUTEX_WARNINGS && res)
sl_rt_warning("rt_cond_signal", res);
return res;
#else
return pthread_cond_broadcast(cond);
#endif
}

static inline int sl_rt_cond_wait(sl_rt_cond* cond, sl_rt_mutex* mutex)
{
#ifdef __XENO__
int res = rt_cond_wait(cond, mutex, TM_INFINITE);
if (SL_RT_MUTEX_WARNINGS && res)
sl_rt_warning("rt_cond_wait", res);
return res;
#else
return pthread_cond_wait(cond, mutex);
#endif
}

static inline int sl_rt_cond_timedwait(sl_rt_cond* cond, sl_rt_mutex* mutex, sl_rt_time timeout)
{
#ifdef __XENO__
int res = rt_cond_wait_until(cond, mutex, timeout);
if (SL_RT_MUTEX_WARNINGS && res && res!=-ETIMEDOUT)
sl_rt_warning("rt_cond_wait_until", res);
return res;
#else
struct timespec ts;
ts.tv_sec = (time_t) (timeout / 1000000000);
ts.tv_nsec = (long) (timeout % 1000000000);
return pthread_cond_timedwait(cond, mutex, &ts);
#endif
}

static inline int sl_rt_cond_timedwait_relative(sl_rt_cond* cond, sl_rt_mutex* mutex, sl_rt_time timeout)
{
#ifdef __XENO__
RTIME abs_timeout = rt_timer_read() + timeout;
return sl_rt_cond_timedwait(cond, mutex, abs_timeout);
#else
struct timeval t;
gettimeofday(&t, NULL);

struct timespec ts;
ts.tv_sec = (time_t) (timeout / 1000000000);
ts.tv_nsec = (long) (timeout % 1000000000);

ts.tv_sec += t.tv_sec;
ts.tv_nsec += t.tv_usec*1000;
if (ts.tv_nsec >= 1000000000)
{
ts.tv_sec += 1;
ts.tv_nsec -= 1000000000;
}
return pthread_cond_timedwait(cond, mutex, &ts);

#endif
}

static inline void sl_rt_warning(const char* function_name, int error_code)
{
//char error_str[1000];
//strerror_r(error_code, error_str, 1000);
printf("ERROR: %s failed with error code: %d\n", function_name, error_code);
}

#endif /* SL_RT_MUTEX_H_ */
46 SL/include/SL_sensor_proc.h
View file
@@ -0,0 +1,46 @@
/*!=============================================================================
==============================================================================
\file SL_sensor_proc.h
\author Stefan Schaal
\date May 2000
==============================================================================
\remarks
SL_sensor_proc.c specific header file
============================================================================*/

#ifndef _SL_sensor_proc_
#define _SL_sensor_proc_

#ifdef __cplusplus
extern "C" {
#endif
int init_sensor_processing(void);
int read_sensors(void);
int send_commands(void);
int process_sensors(void);

int init_user_sensor_processing(void);
int read_user_sensors(SL_Jstate *raw, double *misc_raw);
int send_user_commands(SL_Jstate *com);
int user_kill(void);
int user_power_status(void);
void userCheckForMessage(char *name, int k);
void user_controller(double *u, double *uf);
void init_joint_state_filters(SL_Jstate *js);
void init_misc_sensor_filters(double *ms);


extern double **joint_lin_rot;
extern double *pos_polar;
extern double *load_polar;

#ifdef __cplusplus
}
#endif

#endif /* _SL_sensor_proc_ */
46 SL/include/SL_serial_unix.h
View file
@@ -0,0 +1,46 @@
/*!=============================================================================
==============================================================================
\file SL_serial_unix.h
\author Stefan Schaal
\date Sept. 2011
==============================================================================
supports SL_serial_unix.c
============================================================================*/


#ifndef _SL_serial_unix_
#define _SL_serial_unix_

#include "termios.h"

#define BAUD9K B9600
#define BAUD19K B19200
#define BAUD38K B38400
#define BAUD115K B115200

#define SERIALPORT1 "/dev/ttyS0"
#define SERIALPORT2 "/dev/ttyS1"
#define SERIALPORT3 "/dev/ttyS2"
#define SERIALPORT4 "/dev/ttyS3"

#ifdef __cplusplus
extern "C" {
#endif

int open_serial(char *fname,int baud, int mode);
void close_serial(int fd);
int clear_serial(int fd);
int read_serial(int fd,int n_bytes, char *buffer);
int write_serial(int fd,int n_bytes, char *buffer);
int check_serial(int fd);

#ifdef __cplusplus
}
#endif

#endif /* _SL_serial_unix_ */
44 SL/include/SL_serial_xeno.h
View file
@@ -0,0 +1,44 @@
/*!=============================================================================
==============================================================================
\file SL_serial_xeno.h
\author Stefan Schaal
\date Sept. 2011
==============================================================================
supports SL_serial_xeno.c
============================================================================*/


#ifndef _SL_serial_xeno_
#define _SL_serial_xeno_

#define BAUD9K 9600
#define BAUD19K 19200
#define BAUD38K 38400
#define BAUD115K 115200

#define SERIALPORT1 "rtser0"
#define SERIALPORT2 "rtser1"
#define SERIALPORT3 "rtser2"
#define SERIALPORT4 "rtser3"

#ifdef __cplusplus
extern "C" {
#endif

int open_serial(char *fname,int baud, int mode);
void close_serial(int fd);
int clear_serial(int fd);
int read_serial(int fd,int n_bytes, char *buffer);
int write_serial(int fd,int n_bytes, char *buffer);
int check_serial(int fd);

#ifdef __cplusplus
}
#endif

#endif /* _SL_serial_xeno_ */
288 SL/include/SL_shared_memory.h
View file
@@ -0,0 +1,288 @@
/*!=============================================================================
==============================================================================
\file SL_shared_memory.h
\author Stefan Schaal
\date Feb 1999
==============================================================================
\remarks
SL_shared_memory.c specific header file
============================================================================*/

#ifndef _SL_shared_memory_
#define _SL_shared_memory_

/* note that the flexible length data structure array needs to be the
last element in the shared memory structure, allocated as an array
with only one element. We actually allocate more memory at run time
depending and the desired array length */

typedef struct smVisionBlobs {
SEM_ID sm_sem;
int frame_counter;
char pp_name[30];
SL_fVisionBlob blobs[1];
} smVisionBlobs;

typedef struct smVisionBlobsaux {
SL_fVisionBlobaux blobs[1];
} smVisionBlobsaux;

typedef struct smMiscSensors {
SEM_ID sm_sem;
float ts;
float value[1];
} smMiscSensors;

typedef struct smMiscSimSensors {
SEM_ID sm_sem;
float ts;
float value[1];
} smMiscSimSensors;

typedef struct smCartStates {
SEM_ID sm_sem;
float ts;
SL_fCstate state[1];
} smCartStates;

typedef struct smCartOrients {
SEM_ID sm_sem;
float ts;
SL_fCorient orient[1];
} smCartOrients;

typedef struct smRawBlobs {
SEM_ID sm_sem;
fBlob3D blobs[1];
} smRawBlobs;

typedef struct smRawBlobs2D {
SEM_ID sm_sem;
fBlob2D blobs[1];
} smRawBlobs2D;

typedef struct smSJointDesStates {
SEM_ID sm_sem;
float ts;
SL_fSDJstate sjoint_des_state[1];
} smSJointDesStates;

typedef struct smJointStates {
SEM_ID sm_sem;
float ts;
SL_fJstate joint_state[1];
} smJointStates;

typedef struct smROSState {
SEM_ID sm_sem;
float ts;
char data[1];
} smROSState;

typedef struct smJointDesStates {
SEM_ID sm_sem;
float ts;
SL_fDJstate joint_des_state[1];
} smJointDesStates;

typedef struct SL_fDCommands{
float th; //!< desired th
float thd; //!< desired thd
float uff; //!< feedfoward commands
float vff;
float upd; //!< pd feedback command
float u; //!< final command from controller.c
} SL_fDCommands;

typedef struct smDCommands {
SEM_ID sm_sem;
float ts;
SL_fDCommands des_command[1];
} smDCommands;

typedef struct smJointSimStates {
SEM_ID sm_sem;
float ts;
SL_fJstate joint_sim_state[1];
} smJointSimStates;

typedef struct contactShort {
int status; /*!< contact status */
float f[N_CART+1]; /*!< contact force */
float n[N_CART+1]; /*!< contact normal */
char name[STRING100]; /*!< object name in contact */
} contactShort;

typedef struct smContacts {
SEM_ID sm_sem;
contactShort contact[1];
} smContacts;

typedef struct smBaseState {
SEM_ID sm_sem;
float ts;
SL_fCstate state[1];
} smBaseState;

typedef struct smBaseOrient {
SEM_ID sm_sem;
float ts;
SL_fquat orient[1];
} smBaseOrient;

typedef struct smUserGraphics {
SEM_ID sm_sem;
int n_entries;
int n_bytes_used;
char name[MAX_N_MESSAGES+1][20];
int moff[MAX_N_MESSAGES+1];
unsigned char buf[MAX_BYTES_USER_GRAPHICS];
} smUserGraphics;

typedef struct smMessage {
SEM_ID sm_sem;
int n_msgs;
int n_bytes_used;
char name[MAX_N_MESSAGES+1][20];
int moff[MAX_N_MESSAGES+1];
unsigned char buf[MAX_BYTES_MESSAGES];
} smMessage;

typedef struct smOscilloscope {
SEM_ID sm_sem;
int n_entries;
SL_oscEntry entries[1];
} smOscilloscope;


#ifdef __cplusplus
extern "C" {
#endif

/* external variables */
extern SEM_ID sm_joint_des_state_ready_sem;
extern SEM_ID sm_sjoint_des_state_ready_sem;
extern SEM_ID sm_raw_blobs_ready_sem;
extern SEM_ID sm_learn_invdyn_sem;
extern SEM_ID sm_learn_blob2body_sem;
extern SEM_ID sm_task_servo_sem; /* task servo synchronization */
extern SEM_ID sm_vision_servo_sem; /* vision servo synchronization */
extern SEM_ID sm_openGL_servo_sem; /* openGL servo synchronization */
extern SEM_ID sm_motor_servo_sem; /* motor servo synchronization */
extern SEM_ID sm_simulation_servo_sem; /* simulation servo synchronization */
extern SEM_ID sm_ros_servo_sem; /* ros servo synchronization */
extern SEM_ID sm_pause_sem; /* needed to signal pause to simulation servo */
extern SEM_ID sm_user_graphics_ready_sem; /* signals user graphics available */
extern SEM_ID sm_openGL_message_ready_sem; /* signals message to openGl servo */
extern SEM_ID sm_vision_message_ready_sem; /* signals message to openGl servo */
extern SEM_ID sm_simulation_message_ready_sem; /* signals message to sim servo */
extern SEM_ID sm_task_message_ready_sem; /* signals message to task servo */
extern SEM_ID sm_ros_message_ready_sem; /* signals message to ros servo */
extern SEM_ID sm_motor_message_ready_sem; /* signals message to motor servo */
extern SEM_ID sm_objects_ready_sem; /* signals that objects are ready for read */
extern SEM_ID sm_init_process_ready_sem; /* for starting up the SL processes */
extern SEM_ID sm_oscilloscope_sem; /* for writing to the oscilloscope */

extern smVisionBlobs *sm_vision_blobs;
extern SEM_ID sm_vision_blobs_sem;
extern SL_fVisionBlob *sm_vision_blobs_data;

extern smVisionBlobsaux *sm_vision_blobs_aux;
extern SEM_ID sm_vision_blobs_aux_sem;
extern SL_fVisionBlobaux *sm_vision_blobs_data_aux;

extern smCartStates *sm_cart_states;
extern SEM_ID sm_cart_states_sem;
extern SL_fCstate *sm_cart_states_data;

extern smRawBlobs *sm_raw_blobs;
extern SEM_ID sm_raw_blobs_sem;
extern fBlob3D *sm_raw_blobs_data;

extern smRawBlobs2D *sm_raw_blobs2D;
extern SEM_ID sm_raw_blobs2D_sem;
extern fBlob2D *sm_raw_blobs2D_data;

extern smSJointDesStates *sm_sjoint_des_state;
extern SEM_ID sm_sjoint_des_state_sem;
extern SL_fSDJstate *sm_sjoint_des_state_data;

extern smJointStates *sm_joint_state;
extern SEM_ID sm_joint_state_sem;
extern SL_fJstate *sm_joint_state_data;

extern smJointDesStates *sm_joint_des_state;
extern SEM_ID sm_joint_des_state_sem;
extern SL_fDJstate *sm_joint_des_state_data;

extern smJointSimStates *sm_joint_sim_state;
extern SEM_ID sm_joint_sim_state_sem;
extern SL_fJstate *sm_joint_sim_state_data;

extern smMiscSensors *sm_misc_sensor;
extern SEM_ID sm_misc_sensor_sem;
extern float *sm_misc_sensor_data;

extern smMiscSimSensors *sm_misc_sim_sensor;
extern SEM_ID sm_misc_sim_sensor_sem;
extern float *sm_misc_sim_sensor_data;

extern smContacts *sm_contacts;
extern SEM_ID sm_contacts_sem;
extern contactShort *sm_contacts_data;

extern smBaseState *sm_base_state;
extern SEM_ID sm_base_state_sem;
extern SL_fCstate *sm_base_state_data;

extern smBaseOrient *sm_base_orient;
extern SEM_ID sm_base_orient_sem;
extern SL_fquat *sm_base_orient_data;

extern smUserGraphics *sm_user_graphics;
extern SEM_ID sm_user_graphics_sem;

extern smMessage *sm_simulation_message;
extern SEM_ID sm_simulation_message_sem;
extern smMessage *sm_openGL_message;
extern SEM_ID sm_openGL_message_sem;
extern smMessage *sm_task_message;
extern SEM_ID sm_task_message_sem;
extern smMessage *sm_ros_message;
extern SEM_ID sm_ros_message_sem;
extern smMessage *sm_motor_message;
extern SEM_ID sm_motor_message_sem;
extern smMessage *sm_vision_message;
extern SEM_ID sm_vision_message_sem;

extern smDCommands *sm_des_commands;
extern SEM_ID sm_des_commands_sem;

extern smROSState *sm_ros_state;
extern SEM_ID sm_ros_state_sem;

extern smOscilloscope *sm_oscilloscope;
extern SEM_ID sm_oscilloscope_sem;

int init_shared_memory(void);
void sendMessageToServo(smMessage *sm_message, SEM_ID sm_message_sem,
SEM_ID ready_sem,
char *message, void *buf, int n_bytes);
void sendMessageSimulationServo(char *message, void *buf, int n_bytes);
void sendMessageTaskServo(char *message, void *buf, int n_bytes);
void sendMessageMotorServo(char *message, void *buf, int n_bytes);
void sendMessageOpenGLServo(char *message, void *buf, int n_bytes);
void sendMessageROSServo(char *message, void *buf, int n_bytes);
void sendMessageVisionServo(char *message, void *buf, int n_bytes);


#ifdef __cplusplus
}
#endif

#endif // _SL_shared_memory_
57 SL/include/SL_simulation_servo.h
View file
@@ -0,0 +1,57 @@
/*!=============================================================================
==============================================================================
\file SL_simulation_servo.h
\author Stefan Schaal
\date Nov. 2007
==============================================================================
\remarks
simulation_servo specific header file
============================================================================*/

#ifndef _SL_simulation_servo_
#define _SL_simulation_servo_

#ifdef __cplusplus
extern "C" {
#endif

// external variables
extern int servo_enabled;
extern long simulation_servo_calls;
extern int simulation_servo_rate;
extern int simulation_servo_errors;
extern double last_simulation_servo_time;
extern double simulation_servo_time;
extern double servo_time;


extern int n_integration;
extern int integrate_method;
extern int real_time;

// shared functions
int init_simulation_servo(void);
int init_user_simulation(void);
int run_user_simulation(void);
int receive_des_commands(void);
int send_sim_state(void);
int send_misc_sensors(void);
int send_contacts(void);
int run_simulation_servo(void);
int checkForMessages(void);
void reset(void);
void dss(void);
void disable_simulation_servo(void);
int initUserSimulation(void);


#ifdef __cplusplus
}
#endif

#endif // _SL_simulation_servo_
64 SL/include/SL_system_headers.h
View file
@@ -0,0 +1,64 @@
/*!=============================================================================
==============================================================================
\file SL_system_headers.h
\author Stefan Schaal
\date Oct 2009
==============================================================================
Common system header files for unix, vxworks, and xenomai. This
should be the first include file everywhere, to make sure the
xenomai headers come at the right place.
============================================================================*/


#ifndef _SL_system_headers_
#define _SL_system_headers_

#ifdef VX

#include "vxWorks.h"
#include "vxLib.h"
#include "sysLib.h"
#include "intLib.h"
#include "semLib.h"
#include "fppLib.h"
#include "taskLib.h"
#include "smNameLib.h"
#include "semSmLib.h"
#include "smMemLib.h"
#include "smObjLib.h"
#include "usrLib.h"
#include "usrConfig.h"
#include "logLib.h"
#include "usrLib.h"
#include "tickLib.h"

/* device drivers etc. */
#include "dta.h"

#else

#ifdef __XENO__
#include "SL_xeno_headers.h"
#else
#include "pthread.h"
#endif

#include "stdio.h"
#include "math.h"
#include "stdlib.h"
#include "string.h"
#include "strings.h"
#include "limits.h"
#include "sys/time.h"
#include "signal.h"

#include "SL_vx_wrappers.h"

#endif

#endif /* _SL_system_headers_ */
86 SL/include/SL_task_servo.h
View file
@@ -0,0 +1,86 @@
/*!=============================================================================
==============================================================================
\file SL_task_servo.h
\author Stefan Schaal
\date April 1999
==============================================================================
the header file for the task_servo.c
============================================================================*/


#ifndef _SL_task_servo_
#define _SL_task_servo_

#ifndef SYS_CLOCK_RATE
#define SYS_CLOCK_RATE 60
#endif

#define MOTORSERVO 1
#define INVDYNSERVO 2
#define CARTSERVO 3

#ifdef __cplusplus
extern "C" {
#endif

extern int task_servo_errors;
extern long task_servo_calls;
extern int task_servo_initialized;
extern double last_task_servo_time;
extern double task_servo_time;
extern double servo_time;
extern int servo_enabled;
extern int task_servo_rate;
extern int frame_counter;
extern int exit_on_stop;
extern char initial_user_command[];

void dts(void);
void disable_task_servo(void);
int go(int jID);
void go0(void);
int go0_wait(void);
int go_target_wait(SL_DJstate *target);
int go_target_wait_ID(SL_DJstate *target);
void freeze(void);
void f(void);
void init_task_servo(void);
int run_task_servo(void);
int init_vxworks( void );
int send_raw_blobs(void);
int send_raw_blobs2D(void);
int go_cart_target_wait(SL_Cstate *ctar,int *stat, double mt);
void goVisTarget(void);
void status(void);
int stop(char *);
int init_user_task(void);
int run_user_task(void);
int sendUserGraphics(char *name, void *buf, int n_bytes);
void reset(void);
void followBase(void);
void changePIDGains(double *pGain, double *dGain, double *iGain);
void sendUextSim(void);
void changeRealTime(int flag);
void hideWindowByName(char *name, int hide);
void freezeBase(int flag);
void setGravity(double grav);
void setG(void);
void switchCometDisplay(int status, int n_steps);
void resetCometDisplay(void);
void switchCometDisplayVars(int endeffID, int endeffStatus, int linkID, int linkStatus);
void resetCometDisplayVars(void);
void scdMotor(void);
void toggleShowAxes(int status);



#ifdef __cplusplus
}
#endif

#endif // _SL_task_servo_
54 SL/include/SL_tasks.h
View file
@@ -0,0 +1,54 @@
/*!=============================================================================
==============================================================================
\file SL_tasks.h
\author Stefan Schaal
\date April 1999
==============================================================================
the header file for the SL_task.c
============================================================================*/

#ifndef _SL_tasks_
#define _SL_tasks_

#define NO_TASK "No Task"

typedef struct Task_Def {

char name[100]; /*!< the name of the task */
int active; /*!< TRUE/FALSE */
int (*init_function)(void); /*!< the initialization function */
int (*run_function)(void); /*!< the execution function */
int (*change_function)(void); /*!< the function to change the task parameters */
char *next_task; /*!< pointer to the next task structure */

} Task_Def;

#ifdef __cplusplus
extern "C" {
#endif

extern char current_task_name[100];

/* functions */
void setTask(void);
void st(void);
void addTask(char *tname, int (*init_function)(void), int (*run_function)(void),
int (*change_function)(void));
void deleteTask(char *tname);
void initTasks(void);
void changeTaskParm(void);
void ctp(void);
int setTaskByName(char *name);
void runTask(void);
void redo(void);

#ifdef __cplusplus
}
#endif

#endif // _SL_tasks_
140 SL/include/SL_terrains.h
View file
@@ -0,0 +1,140 @@
/*!=============================================================================
==============================================================================
\file SL_terrains.h
\author
\date
==============================================================================
\remarks
declarations needed by SL_terrains.c
============================================================================*/

#ifndef _SL_terrains_
#define _SL_terrains_

#define MAX_TERRAINS 5

typedef struct { //!< terrain board structure
int status; //!< TRUE or FALSE for active or inactive
int ID; //!< terrain identifier
char tfname[100]; //!< terrain file associated with this terrain
char tfnasc[100]; //!< terrain file name with .asc appended
SL_Cstate pos; //!< origin of terrain in world coordinates (from Vicon)
SL_quat orient; //!< terrain orientation as unit quaternion (from Vicon)
double dxorg_local; //!< x-offset of Z(1,1) relative to local origin on board
double dyorg_local; //!< y-offset of Z(1,1) relative to local origin on board
double c_dxorg; //!< x-offset of Z(1,1) relative to local origin on board for contacts
double c_dyorg; //!< y-offset of Z(1,1) relative to local origin on board for contacts
double xorg; //!< x origin of global board in global coordinates
double yorg; //!< y origin of global board in gloabl coordinates
double dx; //!< delta x of terrain grid
double dy; //!< delta y of terrain grid
double min_z; //!< min z on terrain
double max_z; //!< max z on terrain
int nx_local; //!< number of states in x direction local board
int ny_local; //!< number of states in y direction local board
int c_nx; //!< number of states in x direction for contact checking
int c_ny; //!< number of states in y direction for contact checking
int nx; //!< number of states in x direction
int ny; //!< number of states in y direction
int reg_rad; //!< radius used for regression on terrain
int reg_down; //!< down sampling for computing the regression
int reg_crad; //!< regression radius used for contact checking
int reg_frad; //!< regression radius used for foot z height
int padx; //!< amount of padding in x
int pady; //!< amount of padding in y
int c_padx; //!< amount of padding in x for contacts
int c_pady; //!< amount of padding in y for contacts
fMatrix z_local; //!< matrix of heights of terrain at each x,y position
fMatrix no_go_local; //!< matrix of heights of terrain at each x,y position
fMatrix z; //!< matrix of heights of terrain at each x,y position
fMatrix pz; //!< matrix of predicted heights of terrain at each x,y position
fMatrix fz; //!< matrix of foot placement heights at each x,y position
fMatrix n_nMSE; //!< matrix of nMSE of normals
fMatrix n_x; //!< matrix of x component of normal
fMatrix n_y; //!< matrix of y component of normal
fMatrix n_z; //!< matrix of z component of normal
fMatrix c_z; //!< matrix of z component for contact checking
fMatrix c_no_go; //!< matrix of no_go information in contact coordinates
fMatrix cn_x; //!< matrix of x component of normal for contact
fMatrix cn_y; //!< matrix of y component of normal for contact
fMatrix cn_z; //!< matrix of z component of normal for contact
fMatrix no_go; //!< matrix of go/no-go information
iMatrix cached; //!< indicator matrix with TRUE/FALSE whether terrain info is cached
iMatrix ccached; //!< indicator matrix with TRUE/FALSE whether terrain contact info is cached
} Terrain;

// a useful structure for terrain information
typedef struct { //!< terrain board structure
double z; //!< height above ground at query point
double pz; //!< predicted height above ground at query point from regression
double fz; //!< height used for foot placement
double maxz; //!< max height within a 1cm radius
double n[N_CART+1];//!< normal vector at query point averaged over neighbors
double n_nMSE; //!< nMSE of normal fitting
double slope; //!< the slope as absolute angle relative to horizontal
double no_go; //!< in [1,0], where 1 is bad, 0 is fine.
int ID; //!< terrain ID
int in_padding; //!< TRUE/FALSE for in padding
} TerrainInfo;



#ifdef __cplusplus
extern "C" {
#endif

// global functions
int
readTerrainBoard(char *fname, int ID);
int
setTerrainInfo(int ID, int tID, char *tfname, double *pos, double *orient,
int reg_rad, int reg_down, int reg_crad);
int
getTerrainInfo(double x, double y, TerrainInfo *tinfo);
int
getTerrainInfoZOnly(double x, double y, TerrainInfo *tinfo);
int
getNextTerrainID(void);
int
getContactTerrainInfo(double x, double y, char *tfname, double *z, double *n, double *no_go);
void
setTerrainGroundZ(double z);
void
cacheTerrainInfo(void);
void
fillTerrainPadding(void);
int
read_terrain_file(char *fname, Matrix *array,
int *nx, int *ny, double *min_x, double *max_x,
double *min_y, double *max_y, int flag, int *count);
int
getTerrainFileName(int ID, char *tfname);
int
setTerrainID(int ID, int tID);
int
getTerrainLocalCoordinates(double x, double y, int *tID, double *xl, double *yl);
int
getTerrainLocalCoordinatesID(double x, double y, double z, int tID,
double *xl, double *yl, double *zl);
int
getTerrainWorldCoordinatesID(double xl, double yl, double zl, int tID,
double *x, double *y, double *z);
int
getContactTerrainMinMax(char *tfname,
double *x_min, double *x_max, double *y_min, double *y_max);

// external variables
extern double terrain_bounding_box_max[];
extern double terrain_bounding_box_min[];
extern Terrain terrains[MAX_TERRAINS+1];

#ifdef __cplusplus
}
#endif

#endif // _SL_terrains_
51 SL/include/SL_unix_common.h
View file
@@ -0,0 +1,51 @@
/*!=============================================================================
==============================================================================
\file SL_unix_common.h
\author
\date
==============================================================================
\remarks
declarations needed by SL_unix_common.c
============================================================================*/

#ifndef _SL_unix_common_
#define _SL_unix_common_

#include "sys/time.h"
#include "unistd.h"
#include "pthread.h"

#include "SL_rt_mutex.h"

#ifdef __cplusplus
extern "C" {
#endif

// external variables
//extern pthread_mutex_t mutex1;
extern sl_rt_mutex mutex1;
extern int run_command_line_thread_flag;
extern int (*window_check_function)(char *);
extern int global_argc;
extern char **global_argv;


// global functions
void spawnCommandLineThread(char *initial_command);
void addCommand(char *name, void (*fptr)(void));
void installSignalHandlers(void);
void printSLBanner(void);
void printPrompt(void);
void parseOptions(int argc, char**argv);
void sendCommandLineCmd(char *name);

#ifdef __cplusplus
}
#endif

#endif
34 SL/include/SL_unix_wrappers.h
View file
@@ -0,0 +1,34 @@
/*!=============================================================================
==============================================================================
\file SL_unix_wrappers.h
\author Stefan Schaal
\date August 2008
==============================================================================
fake declarations for unix to allow vxWorks to run the
same files
============================================================================*/


#ifndef _SL_unix_wrappers_
#define _SL_unix_wrappers_

/* function declarations */

#ifdef __cplusplus
extern "C" {
#endif

int sendUserGraphics(char *name, void *buf, int n_bytes);
void changeRealTime(int flag);
void hideWindowByName(char *name, int hide);

#ifdef __cplusplus
}
#endif

#endif /* _SL_unix_wrappers_ */
39 SL/include/SL_userGraphics.h
View file
@@ -0,0 +1,39 @@
/*!=============================================================================
==============================================================================
\file SL_userGraphics.h
\author Stefan Schaal
\date Nov. 2007
==============================================================================
\remarks
SL_userGraphics.c specific header file
============================================================================*/

#ifndef _SL_userGraphics_
#define _SL_userGraphics_

/* external variables */

/* share functions */

#ifdef __cplusplus
extern "C" {
#endif

void addToUserGraphics(char *abr, char *string, void (*fptr)(void *),int n_bytes);
void runUserGraphics(void);
int checkForUserGraphics(void);
void clearUserGraphics(void);
void initUserGraph(void);

extern int user_graphics_update;

#ifdef __cplusplus
}
#endif

#endif /* _SL_userGraphics_ */
39 SL/include/SL_userSimulation.h
View file
@@ -0,0 +1,39 @@
/*!=============================================================================
==============================================================================
\file SL_userSimulation.h
\author Stefan Schaal
\date Sept. 2010
==============================================================================
\remarks
SL_userSimulation.c specific header file
============================================================================*/

#ifndef _SL_userSimulation_
#define _SL_userSimulation_

/* external variables */

/* shared functions */

#ifdef __cplusplus
extern "C" {
#endif

void addToUserSimulation(char *abr, char *string, void (*fptr)(void));
void runUserSimulation(void);
void clearUserSimulation(void);
int activateUserSimulation(char *name);
void initUserSim(void);

extern int user_simulation_update;

#ifdef __cplusplus
}
#endif

#endif /* _SL_userSimulation_ */