Walking System ID

I got Ljung's 1999 book in the mail last night and started reading it. After some bits on linear regression with multiple prediction variables I thought I could rewrite my code to generate the design matrix in the normal equations more generally. But I didn't really work out, here's some math about it.

There is another way to formulate this problem which is described in Ljung 1999 on page 561 for the same set of regressors for each predictor variable. If we define the \( q \) predictors, \( y(t) \), as linear functions of the unknowns, \( \theta \), and the regressors, \( \varphi \) then we get an equation that looks like this:

\[ y(t) = \mathbf{\theta}^T\varphi(t) \]

where \( y(t) \) is a \( q \times 1 \) vector, \( \mathbf{\theta} \) is a \( p+1 \times q \) matrix of the unknown parameters, and \( \varphi \) are the regressors which are a vector \( p+1 \times 1 \). Using our previous variables for the controls and sensors these look like:

\[ y(t) = \begin{bmatrix} m_{m1} \\\\ \vdots \\\\ m_{mq} \end{bmatrix} \]\[ \mathbf{\theta}^T= \begin{bmatrix} k_{11} & k_{12} & \ldots & k_{1p} & m^*_1 \\\\ \vdots & & \vdots & & \\\\ k_{q1} & k_{q2} & \ldots & k_{qp} & m^*_q \end{bmatrix} \]\[ \varphi(t)= \begin{bmatrix} s_0 \\\\ s_1 \\\\ \vdots \\\\ s_{p-1} \\\\ 1 \end{bmatrix} \]

Gait 2D

Ton shared a 2D walking model with us. Obinna's been using it for his project and needed to recompile the mex files for his OS instead of Win32. Ton sent us the source for the project which is based some some Autolev generated models, the C code generated from Autolev, and the mex wrapper for Matlab. We started trying to wrap the code with SWIG so we could use it in other languages and it wasn't too hard. First we created a .i file for SWIG which basically contains a copy of the header file:

/* gait2d.i */
%module gait2d
%{
#define NDOF 9
#define NMOM 6
#define NSTICK 10

typedef struct {

  double TrunkMass, TrunkInertia, TrunkCMy;
  double ThighMass, ThighInertia, ThighCMy, ThighLen;
  double ShankMass, ShankInertia, ShankCMy, ShankLen;
  double FootMass, FootInertia, FootCMx, FootCMy;

  double ContactY;
  double ContactHeelX, ContactToeX;
  double ContactStiff, ContactDamp, ContactY0, ContactV0, ContactFric;

  double MinAngle[NMOM], MaxAngle[NMOM];
  double JointK[NMOM], JointD0[NMOM], JointB[NMOM];
  double HillA;
  double Gmax;

} param_struct;

void gait2d_dyn(param_struct* par,
  double q[NDOF],
  double qd[NDOF],
  double qdd[NDOF],
  double mom[NMOM],
  double Zero[NDOF],
  double dz_dq[NDOF][NDOF],
  double dz_dqd[NDOF][NDOF],
  double dz_dqdd[NDOF][NDOF],
  double dz_dmom[NDOF][NMOM],
  double GRF[4],
  double dGRF_dq[4][NDOF],
  double dGRF_dqd[4][NDOF],
  double Stick[NSTICK][2], double tmp[16]);

void gait2d_acc(param_struct* par,
  double q[NDOF],
  double qd[NDOF],
  double qdd[NDOF],
  double acc[42],
  double dacc_dq[42][NDOF],
  double dacc_dqd[42][NDOF],
  double dacc_dqdd[42][NDOF]);
%}

#define NDOF 9
#define NMOM 6
#define NSTICK 10

typedef struct {

  double TrunkMass, TrunkInertia, TrunkCMy;
  double ThighMass, ThighInertia, ThighCMy, ThighLen;
  double ShankMass, ShankInertia, ShankCMy, ShankLen;
  double FootMass, FootInertia, FootCMx, FootCMy;

  double ContactY;
  double ContactHeelX, ContactToeX;
  double ContactStiff, ContactDamp, ContactY0, ContactV0, ContactFric;

  double MinAngle[NMOM], MaxAngle[NMOM];
  double JointK[NMOM], JointD0[NMOM], JointB[NMOM];
  double HillA;
  double Gmax;

} param_struct;

void gait2d_dyn(param_struct* par,
  double q[NDOF],
  double qd[NDOF],
  double qdd[NDOF],
  double mom[NMOM],
  double Zero[NDOF],
  double dz_dq[NDOF][NDOF],
  double dz_dqd[NDOF][NDOF],
  double dz_dqdd[NDOF][NDOF],
  double dz_dmom[NDOF][NMOM],
  double GRF[4],
  double dGRF_dq[4][NDOF],
  double dGRF_dqd[4][NDOF],
  double Stick[NSTICK][2], double tmp[16]);

void gait2d_acc(param_struct* par,
  double q[NDOF],
  double qd[NDOF],
  double qdd[NDOF],
  double acc[42],
  double dacc_dq[42][NDOF],
  double dacc_dqd[42][NDOF],
  double dacc_dqdd[42][NDOF]);

Then I run these commands:

swig -python gait2d.i
gcc -fPIC -c gait2d_acc.c -c gait2d_dyn.c -c gait2d_wrap.c -I /usr/include/python2.7
ld -shared gait2d_acc.o gait2d_dyn.o gait2d_wrap.o -o _gait2d.so

Then in Python you can import the autogenerated module with:

>>> import gait2d

The two functions require many arguments but only some are acutually inputs. The others are technically outputs. I'm not sure how to deal with that yet. Maybe I need to pass in empty arrays.

Looks like using the numpy.i can help make mappings from Python data types to the ones that the c code expects:

http://docs.scipy.org/doc/numpy/reference/swig.interface-file.html

I also started going through this book:

http://c.learncodethehardway.org/book/

to bring back memories of my CS 101 class in undergrad. I'm managed to avoid C since then, but now's a good as time as any to get back into it.