/* LICENSE:
  =========================================================================
    CMPack'04 Source Code Release for OPEN-R SDK 1.1.5-r2 for ERS7
    Copyright (C) 2004 Multirobot Lab [Project Head: Manuela Veloso]
    School of Computer Science, Carnegie Mellon University
    All rights reserved.
  ========================================================================= */

#include <stdlib.h>
#include <stdio.h>

#include "../headers/Util.h"

#include "Kinematics.h"
#include "Motion.h"
#include "MotionInterface.h"

const unsigned NumPIDJoints = 18;
const int TimeStep = Motion::TimeStep;

void BodyPos(Motion::BodyState &b,double height,double angle)
{
  b.pos.loc.set(0,0,height);
  b.pos.angle.set(0,angle,0);
}

void LegPos(Motion::BodyState &b,int leg,double x,double y,double z)
{
  b.leg[leg].attr = ATTR_POSITION;
  b.leg[leg].pos.set(x,y,z);
}

void LegAng(Motion::BodyState &b,int leg,double r,double s,double k)
{
  b.leg[leg].attr = ATTR_ANGLES;
  set3(b.leg[leg].angles,r,s,k);
}

void HeadPos(Motion::BodyState &b,double x,double y,double z)
{
  b.head.attr = ATTR_POSITION;
  b.head.target.set(x,y,z);
}

void HeadAng(Motion::BodyState &b,double t,double p,double r)
{
  b.head.attr = ATTR_ANGLES;
  set3(b.head.angles,t,p,r);
}


void make_getups()
{
  Motion::BodyStateMotion m[16];
  Motion::BodyState b;
  FILE *out;
  int i,n;


  //==== Get up off of front ====//
  mzero(b);
  b.head.attr = ATTR_ANGLES;
  set3(b.head.angles,0.0,0.0,0.0);

  for(i=0; i<4; i++) b.leg[i].attr = ATTR_ANGLES;
  n = 0;

  m[n].body = b;
  m[n].time = 500;
  n++;

  set3(b.leg[0].angles, 0.0, 0.0, 0.0);
  set3(b.leg[1].angles, 0.0, 0.0, 0.0);
  set3(b.leg[2].angles, 0.0, 0.0, 0.0);
  set3(b.leg[3].angles, 0.0, 0.0, 0.0);
  m[n].body = b;
  m[n].time = 500;
  n++;

  set3(b.leg[0].angles, 0.0, 1.6, 0.0);
  set3(b.leg[1].angles, 0.0, 1.6, 0.0);
  set3(b.leg[2].angles, 1.0, 0.0, 2.5);
  set3(b.leg[3].angles, 1.0, 0.0, 2.5);
  m[n].body = b;
  m[n].time = 500;
  n++;

  set3(b.leg[0].angles, 1.8, 1.6, 0.0);
  set3(b.leg[1].angles, 1.8, 1.6, 0.0);
  set3(b.leg[2].angles,-1.8, 0.0, 2.5);
  set3(b.leg[3].angles,-1.8, 0.0, 2.5);
  m[n].body = b;
  m[n].time = 500;
  n++;

  set3(b.leg[0].angles, 1.8, 0.0, 2.5);
  set3(b.leg[1].angles, 1.8, 0.0, 2.5);
  set3(b.leg[2].angles,-1.8, 0.0, 2.5);
  set3(b.leg[3].angles,-1.8, 0.0, 2.5);
  m[n].body = b;
  m[n].time = 500;
  n++;

  set3(b.leg[0].angles, 1.0, 0.0, 0.5);
  set3(b.leg[1].angles, 1.0, 0.0, 0.5);
  set3(b.leg[2].angles,-1.2, 0.0, 2.5);
  set3(b.leg[3].angles,-1.2, 0.0, 2.5);
  m[n].body = b;
  m[n].time = 500;
  n++;

  out = fopen("gu_front.mot","wb");
  fwrite(&n,sizeof(n),1,out);
  fwrite(m,sizeof(Motion::BodyStateMotion),n,out);
  fclose(out);


  //==== Get up off of back ====//
  mzero(b);
  b.head.attr = ATTR_ANGLES;
  set3(b.head.angles,RAD(-90),0.0,0.0);

  for(i=0; i<4; i++) b.leg[i].attr = ATTR_ANGLES;
  n = 0;

  m[n].body = b;
  m[n].time = 500;
  n++;

  set3(b.leg[0].angles, 0.0, 0.0, 0.0);
  set3(b.leg[1].angles, 0.0, 0.0, 0.0);
  set3(b.leg[2].angles, 0.0, 0.0, 0.0);
  set3(b.leg[3].angles, 0.0, 0.0, 0.0);
  m[n].body = b;
  m[n].time = 500;
  n++;

  set3(b.leg[0].angles, 0.0, 0.0, 0.0);
  set3(b.leg[1].angles, 0.0, 0.0, 0.0);
  set3(b.leg[2].angles,-1.8, 0.0, 2.5);
  set3(b.leg[3].angles,-1.8, 0.0, 2.5);
  m[n].body = b;
  m[n].time = 500;
  n++;

  set3(b.leg[0].angles, 1.0, 0.0, 0.5);
  set3(b.leg[1].angles, 1.0, 0.0, 0.5);
  set3(b.leg[2].angles,-1.2, 0.0, 2.5);
  set3(b.leg[3].angles,-1.2, 0.0, 2.5);
  m[n].body = b;
  m[n].time = 500;
  n++;

  out = fopen("gu_back.mot","wb");
  fwrite(&n,sizeof(n),1,out);
  fwrite(m,sizeof(Motion::BodyStateMotion),n,out);
  fclose(out);

  //==== Get up off of side ====//
  mzero(b);
  b.head.attr = ATTR_ANGLES;
  set3(b.head.angles,0.0,0.0,0.0);

  for(i=0; i<4; i++) b.leg[i].attr = ATTR_ANGLES;
  n = 0;

  m[n].body = b;
  m[n].time = 500;
  n++;

  set3(b.leg[0].angles, 0.0, 0.0, 0.0);
  set3(b.leg[1].angles, 0.0, 0.0, 0.0);
  set3(b.leg[2].angles, 0.0, 0.0, 0.0);
  set3(b.leg[3].angles, 0.0, 0.0, 0.0);
  m[n].body = b;
  m[n].time = 500;
  n++;

  set3(b.leg[0].angles, 2.0, 0.0, 2.5);
  set3(b.leg[1].angles, 2.0, 0.0, 2.5);
  set3(b.leg[2].angles, 2.0, 0.0, 2.5);
  set3(b.leg[3].angles, 2.0, 0.0, 2.5);
  m[n].body = b;
  m[n].time = 300;
  n++;

  set3(b.leg[0].angles, 2.0, 1.6, 2.5);
  set3(b.leg[1].angles, 2.0, 1.6, 2.5);
  set3(b.leg[2].angles, 2.0, 1.6, 2.5);
  set3(b.leg[3].angles, 2.0, 1.6, 2.5);
  m[n].body = b;
  m[n].time = 700;
  n++;

  set3(b.leg[0].angles, 1.0, 0.0, 0.5);
  set3(b.leg[1].angles, 1.0, 0.0, 0.5);
  set3(b.leg[2].angles,-1.2, 0.0, 2.5);
  set3(b.leg[3].angles,-1.2, 0.0, 2.5);
  m[n].body = b;
  m[n].time = 500;
  n++;

  out = fopen("gu_side.mot","wb");
  fwrite(&n,sizeof(n),1,out);
  fwrite(m,sizeof(Motion::BodyStateMotion),n,out);
  fclose(out);
}

void make_kicks()
{
  Motion::BodyStateMotion m[32];
  Motion::BodyState b;
  FILE *out;
  int n;
  double f,s,o;
  int t;

  t = 1;

  //==== Hold Kick ====//
  mzero(b);

  n = 0;
  m[n].body = b;
  m[n].time = 100;
  n++;

  HeadAng(b, RAD(20), RAD(90), 0.0);
  LegAng(b,0, 0.6,-0.1, 1.57);
  LegAng(b,1, 0.6,-0.1, 1.57);
  LegAng(b,2, -.72, 0.2, 1.48);
  LegAng(b,3, -.72, 0.2, 1.48);
  m[n].body = b;
  m[n].time = 300;
  n++;

  HeadAng(b, RAD(20), 0.0, 0.0);
  LegAng(b,0, 0.6,-0.1, 1.57);
  LegAng(b,1, 0.6,-0.1, 1.57);
  LegAng(b,2, -.72, 0.2, 1.48);
  LegAng(b,3, -.72, 0.2, 1.48);
  m[n].body = b;
  m[n].time = 300;
  n++;

  HeadAng(b, RAD(20), RAD(-90), 0.0);
  LegAng(b,0, 0.6,-0.1, 1.57);
  LegAng(b,1, 0.6,-0.1, 1.57);
  LegAng(b,2, -.72, 0.2, 1.48);
  LegAng(b,3, -.72, 0.2, 1.48);
  m[n].body = b;
  m[n].time = 300;
  n++;


  out = fopen("k_hold.mot","wb");
  fwrite(&n,sizeof(n),1,out);
  fwrite(m,sizeof(Motion::BodyStateMotion),n,out);
  fclose(out);

  //==== Grab Kick ====//
  mzero(b);

  n = 0;
  m[n].body = b;
  m[n].time = 500;
  n++;

  BodyPos(b,104,RAD(16));
  HeadAng(b, 0.0, 0.0, 0.0);
  LegAng(b,0, 1.2,-0.2, 0.5);
  LegAng(b,1, 1.2,-0.2, 0.5);
  LegAng(b,2, 1.2, 1.0, 0.5);
  LegAng(b,3, 1.2, 1.0, 0.5);
  m[n].body = b;
  m[n].time = 500;
  n++;

  BodyPos(b,104,RAD(16));
  HeadAng(b, 0.5, 1.5, 0.0);
  // LegAng(b,0, 1.8,-0.2,-0.5);
  // LegAng(b,1, 1.8,-0.2,-0.5);
  LegAng(b,2,-2.5, 1.0, 2.5);
  LegAng(b,3,-2.5, 1.0, 2.5);
  m[n].body = b;
  m[n].time = 500;
  n++;

  BodyPos(b,104,RAD(16));
  // LegAng(b,0, 1.8,-0.2,-0.5);
  // LegAng(b,1, 1.8,-0.2,-0.5);
  LegAng(b,2,-1.0, 0.5, 0.0);
  LegAng(b,3,-1.0, 0.5, 0.0);
  m[n].body = b;
  m[n].time = 100;
  n++;

  m[n].body = b;
  m[n].time = 400;
  n++;

  BodyPos(b,104,RAD(16));
 LegAng(b,0, 1.2, 0.1, 0.5);
  LegAng(b,1, 1.2, 0.1, 0.5);
  // LegAng(b,2, 1.2, 0.0, 0.5);
  // LegAng(b,3, 1.2, 0.0, 0.5);
  m[n].body = b;
  m[n].time = 200;
  n++;

  BodyPos(b,104,RAD(16));
  LegAng(b,0, 1.9, 0.1,-0.5);
  LegAng(b,1, 1.9, 0.1,-0.5);
  // LegAng(b,2, 1.2, 0.0, 0.5);
  // LegAng(b,3, 1.2, 0.0, 0.5);
  m[n].body = b;
  m[n].time = 100;
  n++;

  BodyPos(b,104,RAD(16));
  LegAng(b,0, 1.9,-0.2,-0.5);
  LegAng(b,1, 1.9,-0.2,-0.5);
  LegAng(b,2,-1.0, 0.5, 0.0);
  LegAng(b,3,-1.0, 0.5, 0.0);
  m[n].body = b;
  m[n].time = 800;  // 2.2
  n++;

  BodyPos(b,104,RAD(16));
  LegAng(b,0,-0.5,-0.1, 0.0);
  LegAng(b,1,-0.5,-0.1, 0.0);
  LegAng(b,2,-0.2, 0.5, 0.0);
  LegAng(b,3,-0.2, 0.5, 0.0);
  m[n].body = b;
  m[n].time = 400;
  n++;

  BodyPos(b,104,RAD(16));
  LegAng(b,0,-0.5,-0.1, 0.0);
  LegAng(b,1,-0.5,-0.1, 0.0);
  LegAng(b,2,-0.2, 0.5, 0.0);
  LegAng(b,3,-0.2, 0.5, 0.0);
  m[n].body = b;
  m[n].time = 100;
  n++;

  BodyPos(b,104,RAD(16));
  LegAng(b,0, 1.5, 1.0, 1.5);
  LegAng(b,1, 1.5, 1.0, 1.5);
  LegAng(b,2,-2.0, 0.5, 0.0);
  LegAng(b,3,-2.0, 0.5, 0.0);
  m[n].body = b;
  m[n].time = 500;
  n++;

  BodyPos(b,104,RAD(16));
  LegAng(b,0, 1.0, 0.0, 0.5);
  LegAng(b,1, 1.0, 0.0, 0.5);
  LegAng(b,2,-2.0, 0.0, 2.5);
  LegAng(b,3,-2.0, 0.0, 2.5);
  m[n].body = b;
  m[n].time = 500;
  n++;

  BodyPos(b,104,RAD(16));
  LegAng(b,0, 1.0, 0.0, 0.5);
  LegAng(b,1, 1.0, 0.0, 0.5);
  LegAng(b,2,-1.0, 0.0, 2.5);
  LegAng(b,3,-1.0, 0.0, 2.5);
  m[n].body = b;
  m[n].time = 1000;
  n++;

  m[n].body = b;
  m[n].time = 0;
  n++;

  out = fopen("k_grab.mot","wb");
  fwrite(&n,sizeof(n),1,out);
  fwrite(m,sizeof(Motion::BodyStateMotion),n,out);
  fclose(out);

  //==== Dive Kick ====//

  /*
  mzero(b);

  n = 0;
  m[n].body = b;
  m[n].time = 500;
  n++;

  BodyPos(b,104,RAD(16));
  HeadAng(b, 0.5, 1.5, 0.0);
  LegAng(b,0, 0.2, 0.2, 0.0);
  LegAng(b,1, 0.2, 0.2, 0.0);
  LegAng(b,2,-0.2, 0.5, 0.2);
  LegAng(b,3,-0.2, 0.5, 0.2);
  m[n].body = b;
  m[n].time = 100;
  n++;

  m[n].body = b;
  m[n].time = 100;
  n++;

  BodyPos(b,104,RAD(16));
  HeadAng(b, 0.5, 1.5, 0.0);
  LegAng(b,0, 0.0, 0.2, 0.0);
  LegAng(b,1, 0.0, 0.2, 0.0);
  LegAng(b,2, 0.0, 0.5, 1.5);
  LegAng(b,3, 0.0, 0.5, 1.5);
  m[n].body = b;
  m[n].time = 100;
  n++;

  BodyPos(b,104,RAD(16));
  HeadAng(b, 0.5, 1.5, 0.0);
  LegAng(b,0,-1.2, 0.5, 0.0);
  LegAng(b,1,-1.2, 0.5, 0.0);
  LegAng(b,2, 1.2, 0.5, 1.5);
  LegAng(b,3, 1.2, 0.5, 1.5);
  m[n].body = b;
  m[n].time = 500;
  n++;

  m[n].body = b;
  m[n].time = 500;
  n++;

  BodyPos(b,104,RAD(16));
  HeadAng(b, 0.5, 1.5, 0.0);
  LegAng(b,0, 0.0, 1.5, 0.0);
  LegAng(b,1, 0.0, 1.5, 0.0);
  LegAng(b,2, 0.0, 1.5, 1.5);
  LegAng(b,3, 0.0, 1.5, 1.5);
  m[n].body = b;
  m[n].time = 500;
  n++;

  BodyPos(b,104,RAD(16));
  HeadAng(b, 0.5, 1.5, 0.0);
  LegAng(b,0, 0.0, 1.5, 1.5);
  LegAng(b,1, 0.0, 1.5, 1.5);
  LegAng(b,2, 0.0, 1.5, 1.5);
  LegAng(b,3, 0.0, 1.5, 1.5);
  m[n].body = b;
  m[n].time = 200;
  n++;

  m[n].body = b;
  m[n].time = 500;
  n++;
  */

  mzero(b);

  n = 0;
  m[n].body = b;
  m[n].time = 300;
  n++;

  HeadAng(b, 0.5 , 1.5, 0.0);
  LegAng(b,0, .3, 0.4, 1.6);
  LegAng(b,1, .3, 0.4, 1.6);
  LegAng(b,2, -1.13, .14, 1.74);
  LegAng(b,3, -1.13, .14, 1.74);
  m[n].body = b;
  m[n].time = 200;
  n++;

  HeadAng(b,0.5 , 1.5, 0.0);
  LegAng(b,0, 1.03, 0.09, .9);
  LegAng(b,1, 1.03, 0.09, .9);
  LegAng(b,2, -1.17, .14, 1.74);
  LegAng(b,3, -1.17, .14, 1.74);
  m[n].body = b;
  m[n].time = 200;
  n++;

  HeadAng(b,0.5 , 1.5, 0.0);
  LegAng(b,0, 1.53, -0.11, .72);
  LegAng(b,1, 1.53, -0.11, .72);
  LegAng(b,2, -1.45, .54, 2.35);
  LegAng(b,3, -1.45, .54, 2.35);
  m[n].body = b;
  m[n].time = 200;
  n++;

  HeadAng(b,0.5 , 1.5, 0.0);
  LegAng(b,0, 0.2, 0.4, 1.5);
  LegAng(b,1, 0.2, 0.4, 1.5);
  LegAng(b,2, -1.13, .14, 1.74);
  LegAng(b,3, -1.13, .14, 1.74);
  m[n].body = b;
  m[n].time = 300;
  n++;

  /**********************grab done, dive now**********************/
  // BodyPos(b,104,RAD(16));
  HeadAng(b, 0.5, 1.5, 0.0);
  LegAng(b,0, 0.2, 0.15, 0.0);
  LegAng(b,1, 0.2, 0.15, 0.0);
  LegAng(b,2,-0.2, 0.5, 0.2);
  LegAng(b,3,-0.2, 0.5, 0.2);
  m[n].body = b;
  m[n].time = 100;
  n++;
  
  m[n].body = b;
  HeadAng(b, 0.5, 1.5, 0.0);
  m[n].time = 50;
  n++;
  
  //BodyPos(b,104,RAD(16));
  HeadAng(b, 0.5, 1.5, 0.0);
  LegAng(b,0, 0.0, 0.15, 0.0);
  LegAng(b,1, 0.0, 0.15, 0.0);//.2
  LegAng(b,2, 0.0, 0.5, 1.5);
  LegAng(b,3, 0.0, 0.5, 1.5);
  m[n].body = b;
  m[n].time = 100;
  n++;

  //BodyPos(b,104,RAD(16));
  HeadAng(b, 0.5, 1.5, 0.0);
  LegAng(b,0,-1.2, 0.5, 0.0);
  LegAng(b,1,-1.2, 0.5, 0.0);
  LegAng(b,2, 1.2, 0.5, 1.5);
  LegAng(b,3, 1.2, 0.5, 1.5);
  m[n].body = b;
  m[n].time = 200;//500
  n++;

  m[n].body = b;
  HeadAng(b, 0.5, 1.5, 0.0);
  m[n].time = 100;//500
  n++;

  //BodyPos(b,104,RAD(16));
  HeadAng(b, 0.5, 1.5, 0.0);
  LegAng(b,0, 0.0, 1.5, 0.0);
  LegAng(b,1, 0.0, 1.5, 0.0);
  LegAng(b,2, 0.0, 1.5, 1.5);
  LegAng(b,3, 0.0, 1.5, 1.5);
  m[n].body = b;
  m[n].time = 300;//500
  n++;

  //BodyPos(b,104,RAD(16));
  HeadAng(b, 0.5, 1.5, 0.0);
  LegAng(b,0, 0.0, 1.5, 1.5);
  LegAng(b,1, 0.0, 1.5, 1.5);
  LegAng(b,2, 0.0, 1.5, 1.5);
  LegAng(b,3, 0.0, 1.5, 1.5);
  m[n].body = b;
  m[n].time = 100;//200
  n++;

  m[n].body = b;
  HeadAng(b, 0.5, 1.5, 0.0);
  m[n].time = 500;
  n++;

  out = fopen("k_dive.mot","wb");
  fwrite(&n,sizeof(n),1,out);
  fwrite(m,sizeof(Motion::BodyStateMotion),n,out);
  fclose(out);

  //==== Foreward Kick)====//
  /*  
      mzero(b);

  n = 0;
  m[n].body = b;
  m[n].time = 500;
  n++;

  BodyPos(b,104,RAD(16));
  HeadAng(b,-0.0, 0.0, 0.0);
  LegAng(b,0, 1.3,-0.2,0.5);
  LegAng(b,1, 1.3,-0.2,0.5);
  LegAng(b,2, 1.3, 1.0,0.5);
  LegAng(b,3, 1.3, 1.0,0.5);
  m[n].body = b;
  m[n].time = 100;
  n++;

  BodyPos(b,104,RAD(16));
  LegAng(b,0, 1.3, 0.2,0.5);
  LegAng(b,1, 1.3, 0.2,0.5);
  LegAng(b,2, 1.3, 0.5,0.5);
  LegAng(b,3, 1.3, 0.5,0.5);
  m[n].body = b;
  m[n].time = 200;
  n++;

  BodyPos(b,104,RAD(16));
  HeadAng(b, 0.0, 0.0, 0.0);
  LegAng(b,0, 2.2, 0.2,1.6);
  LegAng(b,1, 2.2, 0.2,1.6);
  LegAng(b,2, 1.3, 0.5,0.5);
  LegAng(b,3, 1.3, 0.5,0.5);
  m[n].body = b;
  m[n].time = 100;
  n++;

  BodyPos(b,104,RAD(16));
  HeadAng(b, 0.0, 0.0, 0.0);
  LegAng(b,0, 2.2,-0.2,1.6);
  LegAng(b,1, 2.2,-0.2,1.6);
  LegAng(b,2, 1.3, 0.5,0.5);
  LegAng(b,3, 1.3, 0.5,0.5);
  m[n].body = b;
  m[n].time = 100;
  n++;

  BodyPos(b,104,RAD(16));
  LegAng(b,0, 1.0,-0.2,1.3);
  LegAng(b,1, 1.0,-0.2,1.3);
  LegAng(b,2, 1.3, 0.5,0.5);
  LegAng(b,3, 1.3, 0.5,0.5);
  m[n].body = b;
  m[n].time = 200;
  n++;

  m[n].body = b;
  m[n].time = 0;
  n++;
*/

 mzero(b);
  n = 0;
  m[n].body = b;
  m[n].time = 100;
  n++;

  BodyPos(b,104,RAD(16));
  HeadAng(b,-0.3, 0.0, 0.0);
  LegAng(b,0, 1.0,-0.2, 1.0);
  LegAng(b,1, 1.0,-0.2, 1.0);
  LegPos(b,2, -2.0,0.5, 0);
  LegPos(b,3, -2.0,0.5,0);
  m[n].body = b;
  m[n].time = 200;
  n++;

  HeadAng(b,-0.5, 0.0, 0.0);
  LegAng(b,0, 1.0,-0.2, 0.5);
  LegAng(b,1, 1.0,-0.2, 0.5);
  LegAng(b,2,-2.0, 0.5, 2.5);
  LegAng(b,3,-2.0, 0.5, 2.5);
  m[n].body = b;
  m[n].time = 200;
  n++;

  LegAng(b,0, 1.0, 0.1, 0.5);
  LegAng(b,1, 1.0, 0.1, 0.5);
  LegAng(b,2,-2.0, 0.5, 2.5);
  LegAng(b,3,-2.0, 0.5, 2.5);
  m[n].body = b;
  m[n].time = 100;
  n++;

  LegAng(b,0, 2.1, 0.1, 1.6);
  LegAng(b,1, 2.1, 0.1, 1.6);
  LegAng(b,2,-2.0, 0.5, 2.5);
  LegAng(b,3,-2.0, 0.5, 2.5);
  m[n].body = b;
  m[n].time = 300;//400
  n++;

  // Bring front legs inward
  HeadAng(b, 0.2, 0.0, 0.0);
  LegAng(b,0, 2.1,-0.3, 1.6);
  LegAng(b,1, 2.1,-0.3, 1.6);
  LegAng(b,2,-2.0, 0.5, 2.5);
  LegAng(b,3,-2.0, 0.5, 2.5);
  m[n].body = b;
  m[n].time = 100;
  n++;

  // Swing forarms downward
  LegAng(b,0, 1.5,-0.3, 1.6);//1.0
  LegAng(b,1, 1.5,-0.3, 1.6);
  LegAng(b,2,-1.0, 0.5, 1.5);
  LegAng(b,3,-1.0, 0.5, 1.5);
  m[n].body = b;
  m[n].time = 100;//300
  n++;

  LegAng(b,0, 1.0,-0.3, 0.0);//1.0
  LegAng(b,1, 1.0,-0.3, 0.0);
  LegAng(b,2,-1.0, 0.5, 1.5);
  LegAng(b,3,-1.0, 0.5, 1.5);
  m[n].body = b;
  m[n].time = 150;//300
  n++;

  //HeadAng(b,-0.0, 0.0, 0.0);
  LegAng(b,0, 1.0,-0.2, 1.0);//1.5
  LegAng(b,1, 1.0,-0.2, 1.0);
  LegAng(b,2,-1.0,-0.1, 1.5);
  LegAng(b,3,-1.0,-0.1, 1.5);
  m[n].body = b;
  m[n].time =250;//300
  n++;

  out = fopen("k_fwd.mot","wb");
  fwrite(&n,sizeof(n),1,out);
  fwrite(m,sizeof(Motion::BodyStateMotion),n,out);
  fclose(out);


  //==== Split Foreward Kick====//
 
  mzero(b);
  n = 0;
  m[n].body = b;
  m[n].time = 100;
  n++;

  BodyPos(b,104,RAD(16));
  HeadAng(b,-0.3, 0.0, 0.0);
  LegAng(b,0, 1.0,-0.2, 1.0);
  LegAng(b,1, 1.0,-0.2, 1.0);
  LegPos(b,2, -100, 75, 0);
  LegPos(b,3, -100,-75,0);
  m[n].body = b;
  m[n].time = 200;
  n++;

   HeadAng(b,-0.5, 0.0, 0.0);
  LegAng(b,0, 1.0,-0.2, 0.5);
  LegAng(b,1, 1.0,-0.2, 0.5);
  LegAng(b,2,-2.0, 0.5, 2.5);
  LegAng(b,3,-2.0, 0.5, 2.5);
  m[n].body = b;
  m[n].time = 200;
  n++;

  LegAng(b,0, 1.0, 0.1, 0.5);
  LegAng(b,1, 1.0, 0.1, 0.5);
  LegAng(b,2,-2.0, 0.5, 2.5);
  LegAng(b,3,-2.0, 0.5, 2.5);
  m[n].body = b;
  m[n].time = 200;
  n++;

  out = fopen("k_fwd1.mot","wb");
  fwrite(&n,sizeof(n),1,out);
  fwrite(m,sizeof(Motion::BodyStateMotion),n,out);
  fclose(out);


  mzero(b);
  n = 0;
  m[n].body = b;
  m[n].time = 100;
  n++;

  LegAng(b,0, 2.0, 0.1, 1.6);
  LegAng(b,1, 2.0, 0.1, 1.6);
  LegAng(b,2,-2.0, 0.5, 2.5);
  LegAng(b,3,-2.0, 0.5, 2.5);
  m[n].body = b;
  m[n].time = 300;//400
  n++;

   HeadAng(b,-0.0, 0.0, 0.0);
  LegAng(b,0, 2.0,-0.3, 1.6);
  LegAng(b,1, 2.0,-0.3, 1.6);
  LegAng(b,2,-2.0, 0.5, 2.5);
  LegAng(b,3,-2.0, 0.5, 2.5);
  m[n].body = b;
  m[n].time = 10;//100
  n++;


  LegAng(b,0, 1.0,-0.2, 1.0);
  LegAng(b,1, 1.0,-0.2, 1.0);
  LegAng(b,2,-1.0, 0.5, 1.5);
  LegAng(b,3,-1.0, 0.5, 1.5);
  m[n].body = b;
  m[n].time = 250;//300
  n++;

  LegAng(b,0, 1.0,-0.2, 1.5);
  LegAng(b,1, 1.0,-0.2, 1.5);
  LegAng(b,2,-1.0,-0.1, 1.5);
  LegAng(b,3,-1.0,-0.1, 1.5);
  m[n].body = b;
  m[n].time =250;//300
  n++;

  out = fopen("k_fwd2.mot","wb");
  fwrite(&n,sizeof(n),1,out);
  fwrite(m,sizeof(Motion::BodyStateMotion),n,out);
  fclose(out);


  //==== Blocking Kick ====//
  mzero(b);

  n = 0;
  m[n].body = b;
  m[n].time = 100;
  n++;

  BodyPos(b,104,RAD(16));
  HeadAng(b, 0.0, 0.0, 0.0);
  LegPos(b,0, 123, 85,0);
  LegPos(b,1, 123,-85,0);
  LegPos(b,2, -80, 75,0);
  LegPos(b,3, -80,-75,0);
  m[n].body = b;
  m[n].time = 500;
  n++;

  BodyPos(b,104,RAD(16));
  HeadAng(b, 0.0, 0.0, 0.0);
  LegAng(b,0, 0.5, 1.0, 0.5);
  LegAng(b,1, 0.5, 1.0, 0.5);
  LegAng(b,2,-2.0, 0.0, 2.5);
  LegAng(b,3,-2.0, 0.0, 2.5);
  m[n].body = b;
  m[n].time = 500;
  n++;

  HeadAng(b,-0.5, 0.0, 0.0);
  LegAng(b,0, 1.0,-0.2, 0.5);
  LegAng(b,1, 1.0,-0.2, 0.5);
  LegAng(b,2, 1.0, 0.0, 2.5);
  LegAng(b,3, 1.0, 0.0, 2.5);
  m[n].body = b;
  m[n].time = 600;
  n++;

  m[n].body = b;
  m[n].time = 100;
  n++;

  LegAng(b,0, 1.0, 0.1, 0.5);
  LegAng(b,1, 1.0, 0.1, 0.5);
  m[n].body = b;
  m[n].time = 200;
  n++;

  LegAng(b,0, 2.0, 0.1, 1.6);
  LegAng(b,1, 2.0, 0.1, 1.6);
  m[n].body = b;
  m[n].time = 300;
  n++;

  HeadAng(b,-0.0, 0.0, 0.0);
  LegAng(b,0, 2.0,-0.2, 1.6);
  LegAng(b,1, 2.0,-0.2, 1.6);
  m[n].body = b;
  m[n].time = 100;
  n++;

  LegAng(b,0, 2.0,-0.2, 1.6);
  LegAng(b,1, 2.0,-0.2, 1.6);
  m[n].body = b;
  m[n].time = 100;
  n++;

  LegAng(b,0, 1.0,-0.2, 1.0);
  LegAng(b,1, 1.0,-0.2, 1.0);
  LegAng(b,2, 0.0, 0.0, 1.5);
  LegAng(b,3, 0.0, 0.0, 1.5);
  m[n].body = b;
  m[n].time = 400;
  n++;

  LegAng(b,0, 1.0,-0.2, 1.5);
  LegAng(b,1, 1.0,-0.2, 1.5);
  LegAng(b,2,-1.0, 0.0, 1.5);
  LegAng(b,3,-1.0, 0.0, 1.5);
  m[n].body = b;
  m[n].time = 400;
  n++;

  /*
  out = fopen("k_grab.mot","wb");
  fwrite(&n,sizeof(n),1,out);
  fwrite(m,sizeof(Motion::BodyStateMotion),n,out);
  fclose(out);
  */

  //==== Bump Kick ====//
  mzero(b);

  t = 1;

  n = 0;
  m[n].body = b;
  m[n].time = 200*t;
  n++;

  f = 10;
  BodyPos(b,90,RAD(16));
  HeadAng(b,0.0, 0.0, 0.0);
  LegPos(b,0, 123+f, 60,0);
  LegPos(b,1, 123+f,-60,0);
  LegPos(b,2, -80+f, 75,0);
  LegPos(b,3, -80+f,-75,0);
  m[n].body = b;
  m[n].time = 200*t;
  n++;

  BodyPos(b,90,RAD(16));
  HeadAng(b,0.0, 0.0, 0.0);
  LegPos(b,0, 153+f, 60,0);
  LegPos(b,1, 153+f,-60,0);
  LegPos(b,2, -80+f, 75,0);
  LegPos(b,3, -80+f,-75,0);
  m[n].body = b;
  m[n].time = 100*t;
  n++;

  f = 70;
  BodyPos(b,90,RAD(0));
  HeadAng(b,0.0, 0.0, 0.0);
  LegPos(b,0, 153-f, 60,0);
  LegPos(b,1, 153-f,-60,0);
  LegPos(b,2, -80-f, 75,0);
  LegPos(b,3, -80-f,-75,0);
  m[n].body = b;
  m[n].time = 200*t;
  n++;

  m[n].body = b;
  m[n].time = 400;
  n++;

  out = fopen("k_bump.mot","wb");
  fwrite(&n,sizeof(n),1,out);
  fwrite(m,sizeof(Motion::BodyStateMotion),n,out);
  fclose(out);


  //==== Punch Kick ====//
  mzero(b);
  BodyPos(b,104,RAD(16));

  n = 0;
  m[n].body = b;
  m[n].time = 500;
  n++;

  f =  0;
  s = 25;

  LegPos(b,0, 124+f, 85+s,0);
  LegPos(b,1, 124+f,-85+s,0);
  LegPos(b,2, -80+f, 75+s,0);
  LegPos(b,3, -80+f,-75+s,0);
  m[n].body = b;
  m[n].time = 500;
  n++;

  LegAng(b,0, 0.0, 0.0, 2.5);
  LegPos(b,1, 124+f,-85+s,0);
  LegPos(b,2, -80-f, 75+s,0);
  LegPos(b,3, -80-f,-75+s,0);
  m[n].body = b;
  m[n].time = 500;
  n++;

  LegAng(b,0, 2.0, 0.0, 1.0);
  LegPos(b,1, 124+f,-85+s,0);
  LegPos(b,2, -80-f, 75+s,0);
  LegPos(b,3, -80-f,-75+s,0);
  m[n].body = b;
  m[n].time = 500;
  n++;

  LegAng(b,0, 2.0, 0.0, 1.0);
  LegPos(b,1, 124+f,-85+s,0);
  LegPos(b,2, -80-f, 75+s,0);
  LegPos(b,3, -80-f,-75+s,0);
  m[n].body = b;
  m[n].time = 100;
  n++;

  LegAng(b,0, 1.0, 0.0, 1.0);
  LegPos(b,1, 124+f,-85+s,0);
  LegPos(b,2, -80-f, 75+s,0);
  LegPos(b,3, -80-f,-75+s,0);
  m[n].body = b;
  m[n].time = 500;
  n++;

  LegPos(b,0, 124, 85,0);
  LegPos(b,1, 124,-85,0);
  LegPos(b,2, -80, 75,0);
  LegPos(b,3, -80,-75,0);
  m[n].body = b;
  m[n].time = 500;
  n++;

  out = fopen("k_punch.mot","wb");
  fwrite(&n,sizeof(n),1,out);
  fwrite(m,sizeof(Motion::BodyStateMotion),n,out);
  fclose(out);


  //==== Side Head Kick ====//

  mzero(b);

  n = 0;
  m[n].body = b;
  m[n].time = 300;
  n++;

  f = -38;
  s =  32;
  o =  15;

  HeadAng(b, 0.0, 0.0, 0.0);
  BodyPos(b,104,RAD(16));
  LegPos(b,0, 123-o, 85,0);
  LegPos(b,1, 123+o,-85,0);
  LegPos(b,2, -80  , 75,0);
  LegPos(b,3, -80  ,-75,0);
  m[n].body = b;
  m[n].time = 300;
  n++;

  HeadAng(b,-1.2,-1.5, 0.0);
  BodyPos(b,95,RAD(16));
  LegPos(b,0, 123-o+f, 85+s,0);
  LegPos(b,1, 123+o+f,-85+s,0);
  LegPos(b,2, -80  +f, 75+s,0);
  LegPos(b,3, -80  +f,-75+s,0);
  m[n].body = b;
  m[n].time = 200;
  n++;

  HeadAng(b,-1.2, 1.5, 0.0);
  BodyPos(b,95,RAD(16));
  LegPos(b,0, 123-o+f, 85-s,0);
  LegPos(b,1, 123+o+f,-85-s,0);
  LegPos(b,2, -80  +f, 75-s,0);
  LegPos(b,3, -80  +f,-75-s,0);
  m[n].body = b;
  m[n].time = 200;
  n++;

  out = fopen("k_head.mot","wb");
  fwrite(&n,sizeof(n),1,out);
  fwrite(m,sizeof(Motion::BodyStateMotion),n,out);
  fclose(out);

  //==== Soft Side Head Kick ====//

  mzero(b);

  n = 0;
  m[n].body = b;
  m[n].time = 300;
  n++;

  f = -38;
  s =  32;
  o =  15;

  HeadAng(b, 0.0, 0.0, 0.0);
  BodyPos(b,104,RAD(16));
  LegPos(b,0, 123-o, 85,0);
  LegPos(b,1, 123+o,-85,0);
  LegPos(b,2, -80  , 75,0);
  LegPos(b,3, -80  ,-75,0);
  m[n].body = b;
  m[n].time = 300;
  n++;

  HeadAng(b,-1.2,-1.5, 0.0);
  BodyPos(b,95,RAD(16));
  LegPos(b,0, 123-o+f, 85+s,0);
  LegPos(b,1, 123+o+f,-85+s,0);
  LegPos(b,2, -80  +f, 75+s,0);
  LegPos(b,3, -80  +f,-75+s,0);
  m[n].body = b;
  m[n].time = 400;
  n++;

  HeadAng(b,-1.2, 1.5, 0.0);
  BodyPos(b,95,RAD(16));
  LegPos(b,0, 123-o+f, 85-s,0);
  LegPos(b,1, 123+o+f,-85-s,0);
  LegPos(b,2, -80  +f, 75-s,0);
  LegPos(b,3, -80  +f,-75-s,0);
  m[n].body = b;
  m[n].time = 200;
  n++;

  out = fopen("k_heads.mot","wb");
  fwrite(&n,sizeof(n),1,out);
  fwrite(m,sizeof(Motion::BodyStateMotion),n,out);
  fclose(out);

  //==== Diagonal Head Kick ====//
  mzero(b);

  n = 0;
  m[n].body = b;
  m[n].time = 300;
  n++;

  f = -38;
  s =  32;
  o =  15;

  HeadAng(b, 0.0, 0.0, 0.0);
  BodyPos(b,104,RAD(16));
  LegPos(b,0, 123-o, 85,0);
  LegPos(b,1, 123+o,-85,0);
  LegPos(b,2, -80  , 75,0);
  LegPos(b,3, -80  ,-75,0);
  m[n].body = b;
  m[n].time = 300;
  n++;

  HeadAng(b,-1.2,-1.5, 0.0);
  BodyPos(b,95,RAD(16));
  LegPos(b,0, 123-o+f, 85+s,0);
  LegPos(b,1, 123+o+f,-85+s,0);
  LegPos(b,2, -80  +f, 75+s,0);
  LegPos(b,3, -80  +f,-75+s,0);
  m[n].body = b;
  m[n].time = 200;
  n++;

  HeadAng(b,-1.2, 1.5, 0.0);
  BodyPos(b,95,RAD(16));
  LegAng(b,0, 0.0, 0.5, 0.0);
  LegPos(b,1, 123+o+f,-85-s,0);
  LegPos(b,2, -80  +f, 75-s,0);
  LegPos(b,3, -80  +f,-75-s,0);
  m[n].body = b;
  m[n].time = 200;
  n++;

  out = fopen("k_diag.mot","wb");
  fwrite(&n,sizeof(n),1,out);
  fwrite(m,sizeof(Motion::BodyStateMotion),n,out);
  fclose(out);



  //==== Warmup ====//
  mzero(b);

  t = 1;
  n = 0;
  m[n].body = b;
  m[n].time = 200;
  n++;

  LegAng(b,0, -0.2, 0.3, 1.7);
  LegAng(b,1, -0.2, 0.3, 1.7);
  LegAng(b,2, -1.1, 0.3, 2.0);
  LegAng(b,3, -1.1, 0.3, 2.0);
  m[n].body = b;
  m[n].time = 500;
  n++;

  for(int i = 0; i < 10; i++){
  //up
  LegAng(b,0, -0.5, 0.2, 1.3);
  LegAng(b,1, -0.5, 0.2, 1.3);
  LegAng(b,2, .7, -0.03, 0.4);
  LegAng(b,3, .7, -0.03, 0.4);
  m[n].body = b;
  m[n].time = 500;
  n++;
  //down
  LegAng(b,0, -0.5, 0.2, 2.3);
  LegAng(b,1, -0.5, 0.2, 2.3);
  LegAng(b,2, 1.0, -0.03, 0.4);
  LegAng(b,3, 1.0, -0.03, 0.4);
  m[n].body = b;
  m[n].time = 500;
  n++;
  }

  out = fopen("warmup.mot","wb");
  fwrite(&n,sizeof(n),1,out);
  fwrite(m,sizeof(Motion::BodyStateMotion),n,out);
  fclose(out);



  //==== Dance ====//
  mzero(b);

  t = 1;
  n = 0;
  m[n].body = b;
  m[n].time = 200;
  n++;

  LegAng(b, 0, -0.253956, -0.131986, 0.470636);
  LegAng(b, 1, -0.257699, -0.139626, 0.478557);
  LegAng(b, 2, -1.160850, -0.168714, 2.277713);
  LegAng(b, 3, -1.167226, -0.162013, 2.273294);
  HeadAng(b, 0.0, 0.0, 0.0);
  m[n].body = b;
  m[n].time = 300;
  n++;
  
  LegAng(b, 0, -0.263956, -0.141986, 0.484838);
  LegAng(b, 1, -0.269413, -0.145443, 0.484187);
  LegAng(b, 2, -1.166392, -0.168714, 2.271955);
  LegAng(b, 3, -1.165705, -0.162013, 2.274890);
  HeadAng(b, 0.0,0.0,0.0);
  m[n].body = b;
  m[n].time = 300;
  n++;
  
  
  LegAng(b, 0, -0.853081, -0.133808, 0.237025);
  LegAng(b, 1, -0.855093, -0.139626, 0.230833);
  LegAng(b, 2, -1.743007, 0.412529, 2.565634);
  LegAng(b, 3, -1.745343, 0.412627, 2.565634);
  HeadAng(b, -0.3, 0.0, 0.0);
  m[n].body = b;
  m[n].time = 300;
  n++;
  
  LegAng(b, 0, -0.848842, -0.133808, 0.231227);
  LegAng(b, 1, -0.849236, -0.139626, 0.230833);
  LegAng(b, 2, -1.747465, 0.418229, 2.565634);
  LegAng(b, 3, -1.744146, 0.415625, 2.565634);
  HeadAng(b, -0.5, 0.0, 0.0);
  m[n].body = b;
  m[n].time = 500;
  n++;
  ///arm stuff

  
  LegAng(b, 0, -1.248842, -0.193808, 0.231227);
  LegAng(b, 1, -1.249236, -0.199626, 0.230833);
  LegAng(b, 2, -0.8518614, 0.215208, 1.7508878);
  LegAng(b, 3, -0.8512491, 0.214532, 1.75005634);
  HeadAng(b, -0.5, 0.0, 0.0);
  m[n].body = b;
  m[n].time = 500;
  n++;

  LegAng(b, 0, 2.003999, -0.005817, 0.007958);
  LegAng(b, 1, 2.005711, -0.003084, 0.005634);
  LegAng(b, 2, -0.8518614, 0.215208, 1.7508878);
  LegAng(b, 3, -0.8512491, 0.214532, 1.75005634);
  HeadAng(b, -0.5, 0.0, 0.0);
  m[n].body = b;
  m[n].time = 500;
  n++;


  LegAng(b, 0, 0.05999, 0.005817, 2.607958);
  LegAng(b, 1, 0.055711, 0.003084, 2.605634);
  LegAng(b, 2, -0.8518614, 0.215208, 1.7508878);
  LegAng(b, 3, -0.8512491, 0.214532, 1.75005634);
  HeadAng(b, -0.5, 0.0, 0.0);
  m[n].body = b;
  m[n].time = 500;
  n++;


  LegAng(b, 0, 2.003999, -0.005817, 0.007958);
  LegAng(b, 1, 2.005711, -0.003084, 0.005634);
  LegAng(b, 2, -0.8518614, 0.215208, 1.7508878);
  LegAng(b, 3, -0.8512491, 0.214532, 1.75005634);
  HeadAng(b, -0.5, 0.0, 0.0);
  m[n].body = b;
  m[n].time = 500;
  n++;

  LegAng(b, 0, 0.0, -0.0, 2.6);
  LegAng(b, 1, 0.0, -0.0, 2.6);
  LegAng(b, 2, -0.85, 0.21, 1.750);
  LegAng(b, 3, -0.85, 0.21, 1.750);
  HeadAng(b, -0.5, 0.0, 0.0);
  m[n].body = b;
  m[n].time = 500;
  n++;
  //end arm stuff
  
  /*
  LegAng(b, 0, 0.435667, -0.196355, 0.945634);
  LegAng(b, 1, 0.433480, -0.191986, 0.941297);
  LegAng(b, 2, -0.25, 0.71, 0.70);
  LegAng(b, 3, -0.25, 0.71, 0.70);
  HeadAng(b, -0.5, 0.0, 0.0);
  m[n].body = b;
  m[n].time = 500;
  n++;

  LegAng(b, 0, 0.435667, -0.196355, 0.945634);
  LegAng(b, 1, 0.433480, -0.191986, 0.941297);
  LegAng(b, 2, -.742035, 0.701128, 0.92545);
  LegAng(b, 3, -.742263, 0.707812, 0.95634);
  HeadAng(b, -0.5, 0.0, 0.0);
  m[n].body = b;
  m[n].time = 500;
  n++;

  LegAng(b, 0, 0.435667, -0.196355, 0.945634);
  LegAng(b, 1, 0.433480, -0.191986, 0.941297);
  LegAng(b, 2, -1.742035, 0.701128, 0.92545);
  LegAng(b, 3, -1.742263, 0.707812, 0.95634);
  HeadAng(b, -0.5, 0.0, 0.0);
  m[n].body = b;
  m[n].time = 500;
  n++;

  LegAng(b, 0, -0.253956, -0.131986, 0.470636);
  LegAng(b, 1, -0.257699, -0.139626, 0.478557);
  LegAng(b, 2, -0.56392, 0.168714, 0.971955);
  LegAng(b, 3, -0.55705, 0.162013, 0.974890);
  HeadAng(b, -0.2, 0.0, 0.0);
  m[n].body = b;
  m[n].time = 500;
  n++;


  LegAng(b, 0, 0.035667, -0.196355, 1.105634);
  LegAng(b, 1, 0.033480, -0.191986, 1.101297);
  LegAng(b, 2, -0.302, 0.168714, 0.971955);
  LegAng(b, 3, -0.305, 0.162013, 0.974890);
  HeadAng(b, -0.0, 0.0, 0.0);
  m[n].body = b;
  m[n].time = 500;
  n++;
  */
  

  out = fopen("dance.mot","wb");
  fwrite(&n,sizeof(n),1,out);
  fwrite(m,sizeof(Motion::BodyStateMotion),n,out);
  fclose(out);
}


void make_walk_param()
{
  Motion::WalkParam wp;
  FILE *out;

  wp.leg[0].neutral.set( 125, 82,0);
  wp.leg[1].neutral.set( 125,-82,0);
  wp.leg[2].neutral.set( -100, 75,0);
  wp.leg[3].neutral.set( -100,-75,0);
  wp.period = 640;
  
  wp.leg[0].lift_vel.set(0,0, 100); 
  wp.leg[1].lift_vel.set(0,0, 100); 
  wp.leg[2].lift_vel.set(0,0, 150);  
  wp.leg[3].lift_vel.set(0,0, 150); 
  
  wp.leg[0].down_vel.set(0,0,-100); 
  wp.leg[1].down_vel.set(0,0,-100); 
  wp.leg[2].down_vel.set(0,0,-100); 
  wp.leg[3].down_vel.set(0,0,-100); 
    
  wp.leg[0].lift_time=0.0000; wp.leg[0].down_time=0.5000;
  wp.leg[1].lift_time=0.5000; wp.leg[1].down_time=1.0000;
  wp.leg[2].lift_time=0.5000; wp.leg[2].down_time=1.0000;
  wp.leg[3].lift_time=0.0000; wp.leg[3].down_time=0.5000;

  wp.body_angle  = RAD(16.0);
  wp.body_height = 98;
  wp.hop  = 0;
  wp.sway = 0;
  wp.front_height = 9.0;
  wp.back_height = 9.0;

  
  out = fopen("walk_d.prm","wb");
  if(out){
    fwrite(&wp,sizeof(wp),1,out);
    fclose(out);
  }
  
  /*********************************/
  //turn for super walk, max at 2.2
  wp.leg[0].neutral.set( 126, 85,0);
  wp.leg[1].neutral.set( 126,-85,0);
  wp.leg[2].neutral.set( -100, 78,0);
  wp.leg[3].neutral.set( -100,-78,0);
  wp.period = 550;
  
  wp.leg[0].lift_vel.set(0,0, 100); 
  wp.leg[1].lift_vel.set(0,0, 100); 
  wp.leg[2].lift_vel.set(0,0, 150);  
  wp.leg[3].lift_vel.set(0,0, 150); 
  
  wp.leg[0].down_vel.set(0,0,-100); 
  wp.leg[1].down_vel.set(0,0,-100); 
  wp.leg[2].down_vel.set(0,0,-100); 
  wp.leg[3].down_vel.set(0,0,-100); 
    
  wp.leg[0].lift_time=0.0000; wp.leg[0].down_time=0.5000;
  wp.leg[1].lift_time=0.5000; wp.leg[1].down_time=1.0000;
  wp.leg[2].lift_time=0.5000; wp.leg[2].down_time=1.0000;
  wp.leg[3].lift_time=0.0000; wp.leg[3].down_time=0.5000;

  wp.body_angle  = RAD(14.0);
  wp.body_height = 101;
  wp.hop  = 0;
  wp.sway = 0;
  wp.front_height = 10.0;
  wp.back_height = 10.0; 

 /*
  wp.leg[0].neutral.set( 127, 84,0);
  wp.leg[1].neutral.set( 127,-84,0);
  wp.leg[2].neutral.set( -100, 77,0);
  wp.leg[3].neutral.set( -100,-77,0);
  wp.period = 500;
  
  wp.leg[0].lift_vel.set(0,0, 100); 
  wp.leg[1].lift_vel.set(0,0, 100); 
  wp.leg[2].lift_vel.set(0,0, 150);  
  wp.leg[3].lift_vel.set(0,0, 150); 
  
  wp.leg[0].down_vel.set(0,0,-100); 
  wp.leg[1].down_vel.set(0,0,-100); 
  wp.leg[2].down_vel.set(0,0,-100); 
  wp.leg[3].down_vel.set(0,0,-100); 
  
  wp.leg[0].lift_time=0.0000; wp.leg[0].down_time=0.5000;
  wp.leg[1].lift_time=0.5000; wp.leg[1].down_time=1.0000;
  wp.leg[2].lift_time=0.5000; wp.leg[2].down_time=1.0000;
  wp.leg[3].lift_time=0.0000; wp.leg[3].down_time=0.5000;
  
  wp.body_angle  = RAD(14.0);
  wp.body_height = 98;
  wp.hop  = 0;
  wp.sway = 0;
  wp.front_height = 100.0;
  wp.back_height = 100.0;
*/
 /*
   //very low body height
  wp.leg[0].neutral.set( 137, 85,0);
  wp.leg[1].neutral.set( 137,-85,0);
  wp.leg[2].neutral.set( -120, 85,0);
  wp.leg[3].neutral.set( -120,-85,0);
  wp.period = 550;

  wp.leg[0].lift_vel.set(0,0, 100);
  wp.leg[1].lift_vel.set(0,0, 100);
  wp.leg[2].lift_vel.set(0,0, 150);
  wp.leg[3].lift_vel.set(0,0, 150);

  wp.leg[0].down_vel.set(0,0,-80);
  wp.leg[1].down_vel.set(0,0,-80);
  wp.leg[2].down_vel.set(0,0,-80);
  wp.leg[3].down_vel.set(0,0,-80);

  wp.leg[0].lift_time=0.0000; wp.leg[0].down_time=0.5000;
  wp.leg[1].lift_time=0.5000; wp.leg[1].down_time=1.0000;
  wp.leg[2].lift_time=0.5000; wp.leg[2].down_time=1.0000;
  wp.leg[3].lift_time=0.0000; wp.leg[3].down_time=0.5000;

  wp.body_angle  = RAD(13.0);
  wp.body_height = 92;
  wp.hop  = 0;
  wp.sway = 0;
  wp.front_height = 4.0;
  wp.back_height = 100.0;
  */
  /*
  //closest to current walk param
  wp.leg[0].neutral.set( 125, 82,0);
  wp.leg[1].neutral.set( 125,-82,0);
  wp.leg[2].neutral.set( -85, 75,0);
  wp.leg[3].neutral.set( -85,-75,0);
  wp.period = 500;
  
  wp.leg[0].lift_vel.set(0,0, 100); 
  wp.leg[1].lift_vel.set(0,0, 100); 
  wp.leg[2].lift_vel.set(0,0, 150);  
  wp.leg[3].lift_vel.set(0,0, 150); 
  
  wp.leg[0].down_vel.set(0,0,-100); 
  wp.leg[1].down_vel.set(0,0,-100); 
  wp.leg[2].down_vel.set(0,0,-100); 
  wp.leg[3].down_vel.set(0,0,-100); 
  
  wp.leg[0].lift_time=0.0000; wp.leg[0].down_time=0.5000;
  wp.leg[1].lift_time=0.5000; wp.leg[1].down_time=1.0000;
  wp.leg[2].lift_time=0.5000; wp.leg[2].down_time=1.0000;
  wp.leg[3].lift_time=0.0000; wp.leg[3].down_time=0.5000;
  
  wp.body_angle  = RAD(14.0);
  wp.body_height = 99;
  wp.hop  = 0;
  wp.sway = 0;
  wp.front_height = 100.0;
  wp.back_height = 100.0;
  */
  /*
  //orig
  wp.leg[0].neutral.set( 120, 85,0);
  wp.leg[1].neutral.set( 120,-85,0);
  wp.leg[2].neutral.set( -85, 77,0);
  wp.leg[3].neutral.set( -85,-77,0);
  wp.period = 550;

  wp.leg[0].lift_vel.set(0,0, 100);
  wp.leg[1].lift_vel.set(0,0, 100);
  wp.leg[2].lift_vel.set(0,0, 200);
  wp.leg[3].lift_vel.set(0,0, 200);

  wp.leg[0].down_vel.set(0,0,-80);
  wp.leg[1].down_vel.set(0,0,-80);
  wp.leg[2].down_vel.set(0,0,-80);
  wp.leg[3].down_vel.set(0,0,-80);

  wp.leg[0].lift_time=0.0000; wp.leg[0].down_time=0.5000;
  wp.leg[1].lift_time=0.5000; wp.leg[1].down_time=1.0000;
  wp.leg[2].lift_time=0.5000; wp.leg[2].down_time=1.0000;
  wp.leg[3].lift_time=0.0000; wp.leg[3].down_time=0.5000;

  wp.body_angle  = RAD(12.0);
  wp.body_height = 105;
  wp.hop  = 0;
  wp.sway = 0;
  wp.front_height = 100.0;
  wp.back_height = 100.0;
  */
  out = fopen("walk_a.prm","wb");
  if(out){
    fwrite(&wp,sizeof(wp),1,out);
    fclose(out);
  }

  /*************************/

  //super walk (<9.5 seconds)
  wp.leg[0].neutral.set( 125, 82,0);
  wp.leg[1].neutral.set( 125,-82,0);
  wp.leg[2].neutral.set( -100, 75,0);
  wp.leg[3].neutral.set( -100,-75,0);
  wp.period = 640;
  
  wp.leg[0].lift_vel.set(0,0, 100); 
  wp.leg[1].lift_vel.set(0,0, 100); 
  wp.leg[2].lift_vel.set(0,0, 150);  
  wp.leg[3].lift_vel.set(0,0, 150); 
  
  wp.leg[0].down_vel.set(0,0,-100); 
  wp.leg[1].down_vel.set(0,0,-100); 
  wp.leg[2].down_vel.set(0,0,-100); 
  wp.leg[3].down_vel.set(0,0,-100); 
    
  wp.leg[0].lift_time=0.0000; wp.leg[0].down_time=0.5000;
  wp.leg[1].lift_time=0.5000; wp.leg[1].down_time=1.0000;
  wp.leg[2].lift_time=0.5000; wp.leg[2].down_time=1.0000;
  wp.leg[3].lift_time=0.0000; wp.leg[3].down_time=0.5000;

  wp.body_angle  = RAD(16.0);
  wp.body_height = 98;
  wp.hop  = 0;
  wp.sway = 0;
  wp.front_height = 100.0;
  wp.back_height = 100.0;

  /*//Fast xy walk with the widest range 
  wp.leg[0].neutral.set( 125, 84,0);
  wp.leg[1].neutral.set( 125,-84,0);
  wp.leg[2].neutral.set( -100, 75,0);
  wp.leg[3].neutral.set( -100,-75,0);
  wp.period = 650;
  
  wp.leg[0].lift_vel.set(0,0, 100); 
  wp.leg[1].lift_vel.set(0,0, 100); 
  wp.leg[2].lift_vel.set(0,0, 200);  
  wp.leg[3].lift_vel.set(0,0, 200); 
  
  wp.leg[0].down_vel.set(0,0,-100); 
  wp.leg[1].down_vel.set(0,0,-100); 
  wp.leg[2].down_vel.set(0,0,-100); 
  wp.leg[3].down_vel.set(0,0,-100); 
    
  wp.leg[0].lift_time=0.0000; wp.leg[0].down_time=0.5000;
  wp.leg[1].lift_time=0.5000; wp.leg[1].down_time=1.0000;
  wp.leg[2].lift_time=0.5000; wp.leg[2].down_time=1.0000;
  wp.leg[3].lift_time=0.0000; wp.leg[3].down_time=0.5000;

  wp.body_angle  = RAD(16.0);
  wp.body_height = 98;
  wp.hop  = 0;
  wp.sway = 0;
  wp.front_height = 100.0;
  wp.back_height = 100.0;
  */

  /*
    //fast v1, fast going straight (10.5 sec for 2 meters) but
    //doesnt interface well with turns, diagonals and backwards walks
  wp.leg[0].neutral.set( 125, 84,0);
  wp.leg[1].neutral.set( 125,-84,0);
  wp.leg[2].neutral.set( -100, 75,0);
  wp.leg[3].neutral.set( -100,-75,0);
  wp.period = 650;
  
  wp.leg[0].lift_vel.set(0,0, 100); 
  wp.leg[1].lift_vel.set(0,0, 100); 
  wp.leg[2].lift_vel.set(0,0, 200);  
  wp.leg[3].lift_vel.set(0,0, 200); 
  
  wp.leg[0].down_vel.set(0,0,-100); 
  wp.leg[1].down_vel.set(0,0,-100); 
  wp.leg[2].down_vel.set(0,0,-100); 
  wp.leg[3].down_vel.set(0,0,-100); 
    
  wp.leg[0].lift_time=0.0000; wp.leg[0].down_time=0.5000;
  wp.leg[1].lift_time=0.5000; wp.leg[1].down_time=1.0000;
  wp.leg[2].lift_time=0.5000; wp.leg[2].down_time=1.0000;
  wp.leg[3].lift_time=0.0000; wp.leg[3].down_time=0.5000;

  wp.body_angle  = RAD(14.0);
  wp.body_height = 97;
  wp.hop  = 0;
  wp.sway = 0;
  wp.front_height = 4.0;
  wp.back_height = 100.0;
  */
  /*
//good
  wp.leg[0].neutral.set( 125, 84,0);
  wp.leg[1].neutral.set( 125,-84,0);
  wp.leg[2].neutral.set( -100, 75,0);
  wp.leg[3].neutral.set( -100,-75,0);
  wp.period = 650;
  
  wp.leg[0].lift_vel.set(0,0, 100); 
  wp.leg[1].lift_vel.set(0,0, 100); 
  wp.leg[2].lift_vel.set(0,0, 200);  
  wp.leg[3].lift_vel.set(0,0, 200); 
  
  wp.leg[0].down_vel.set(0,0,-100); 
  wp.leg[1].down_vel.set(0,0,-100); 
  wp.leg[2].down_vel.set(0,0,-100); 
  wp.leg[3].down_vel.set(0,0,-100); 
  
  wp.leg[0].lift_time=0.0000; wp.leg[0].down_time=0.5000;
  wp.leg[1].lift_time=0.5000; wp.leg[1].down_time=1.0000;
  wp.leg[2].lift_time=0.5000; wp.leg[2].down_time=1.0000;
  wp.leg[3].lift_time=0.0000; wp.leg[3].down_time=0.5000;
  
  wp.body_angle  = RAD(14.0);
  wp.body_height = 97;
  wp.hop  = 0;
  wp.sway = 0;
  wp.front_height = 100.0;
  wp.back_height = 100.0;
  */


  /*
  wp.leg[0].neutral.set( 120, 80,0);
  wp.leg[1].neutral.set( 120,-80,0);
  wp.leg[2].neutral.set( -90, 70,0);
  wp.leg[3].neutral.set( -90,-70,0);
  wp.period = 670;

  wp.leg[0].lift_vel.set(0,0, 100);
  wp.leg[1].lift_vel.set(0,0, 100);
  wp.leg[2].lift_vel.set(0,0, 200);
  wp.leg[3].lift_vel.set(0,0, 200);

  wp.leg[0].down_vel.set(0,0,-100);
  wp.leg[1].down_vel.set(0,0,-100);
  wp.leg[2].down_vel.set(0,0,-100);
  wp.leg[3].down_vel.set(0,0,-100);

  wp.leg[0].lift_time=0.0000; wp.leg[0].down_time=0.5000;
  wp.leg[1].lift_time=0.5000; wp.leg[1].down_time=1.0000;
  wp.leg[2].lift_time=0.5000; wp.leg[2].down_time=1.0000;
  wp.leg[3].lift_time=0.0000; wp.leg[3].down_time=0.5000;

  wp.body_angle  = RAD(12.0);
  wp.body_height = 105;
  wp.hop  = 0;
  wp.sway = 0;
  wp.front_height = 100.0;
  wp.back_height = 100.0;
*/  

  /*  
  wp.leg[0].neutral.set( 114, 90,0);
  wp.leg[1].neutral.set( 114,-90,0);
  wp.leg[2].neutral.set( -83, 75,0);
  wp.leg[3].neutral.set( -83,-75,0);
  wp.period = 550;

  wp.leg[0].lift_vel.set(0,0, 100);
  wp.leg[1].lift_vel.set(0,0, 100);
  wp.leg[2].lift_vel.set(0,0, 200);
  wp.leg[3].lift_vel.set(0,0, 200);

  wp.leg[0].down_vel.set(0,0, -100);
  wp.leg[1].down_vel.set(0,0, -100);
  wp.leg[2].down_vel.set(0,0, -100);
  wp.leg[3].down_vel.set(0,0, -100);

  wp.leg[0].lift_time=0.0000; wp.leg[0].down_time=0.5000;
  wp.leg[1].lift_time=0.5000; wp.leg[1].down_time=1.0000;
  wp.leg[2].lift_time=0.5000; wp.leg[2].down_time=1.0000;
  wp.leg[3].lift_time=0.0000; wp.leg[3].down_time=0.5000;

  wp.body_angle  = RAD(11.0);
  wp.body_height = 98;
  wp.hop  = 0;
  wp.sway = 0;
  wp.front_height = 100.0;
  wp.back_height = 100.0;
  */

  /*
  //Jim's Development trot
  wp.leg[0].neutral.set( 123, 85,0);
  wp.leg[1].neutral.set( 123,-85,0);
  wp.leg[2].neutral.set( -80, 75,0);
  wp.leg[3].neutral.set( -80,-75,0);
  wp.period = 600;

  wp.leg[0].lift_vel.set(0,0, 100);
  wp.leg[1].lift_vel.set(0,0, 100);
  wp.leg[2].lift_vel.set(0,0, 200);
  wp.leg[3].lift_vel.set(0,0, 200);

  wp.leg[0].down_vel.set(0,0, -80);
  wp.leg[1].down_vel.set(0,0, -80);
  wp.leg[2].down_vel.set(0,0,-100);
  wp.leg[3].down_vel.set(0,0,-100);

  wp.leg[0].lift_time=0.0000; wp.leg[0].down_time=0.5000;
  wp.leg[1].lift_time=0.5000; wp.leg[1].down_time=1.0000;
  wp.leg[2].lift_time=0.5000; wp.leg[2].down_time=1.0000;
  wp.leg[3].lift_time=0.0000; wp.leg[3].down_time=0.5000;

  wp.body_angle  = RAD(12.0);
  wp.body_height = 105;
  wp.hop  = 0;
  wp.sway = 0;
  wp.front_height = 100.0;
  wp.back_height = 100.0;
  */

  /*
  // Trot used in 2001 competition
  wp.leg[0].neutral.set( 123, 85,0);
  wp.leg[1].neutral.set( 123,-85,0);
  wp.leg[2].neutral.set( -80, 75,0);
  wp.leg[3].neutral.set( -80,-75,0);
  wp.period = 500;

  wp.leg[0].lift_vel.set(0,0, 100);
  wp.leg[1].lift_vel.set(0,0, 100);
  wp.leg[2].lift_vel.set(0,0, 200);
  wp.leg[3].lift_vel.set(0,0, 200);

  wp.leg[0].down_vel.set(0,0, -80);
  wp.leg[1].down_vel.set(0,0, -80);
  wp.leg[2].down_vel.set(0,0,-100);
  wp.leg[3].down_vel.set(0,0,-100);

  wp.leg[0].lift_time=0.0000; wp.leg[0].down_time=0.5000;
  wp.leg[1].lift_time=0.5000; wp.leg[1].down_time=1.0000;
  wp.leg[2].lift_time=0.5000; wp.leg[2].down_time=1.0000;
  wp.leg[3].lift_time=0.0000; wp.leg[3].down_time=0.5000;

  wp.body_angle  = RAD(16.0);
  wp.body_height = 104;
  wp.hop  = 0;
  wp.sway = 0;
  wp.front_height = 100.0;
  wp.back_height = 100.0;
  */

  /*
  // Development trot
  wp.leg[0].neutral.set( 116, 70,0);
  wp.leg[1].neutral.set( 116,-70,0);
  wp.leg[2].neutral.set( -90, 70,0);
  wp.leg[3].neutral.set( -90,-70,0);
  wp.period = 500;

  wp.leg[0].lift_vel.set(0,0, 150);
  wp.leg[1].lift_vel.set(0,0, 150);
  wp.leg[2].lift_vel.set(0,0, 200);
  wp.leg[3].lift_vel.set(0,0, 200);

  wp.leg[0].down_vel.set(0,0, -80);
  wp.leg[1].down_vel.set(0,0, -80);
  wp.leg[2].down_vel.set(0,0,-100);
  wp.leg[3].down_vel.set(0,0,-100);

  wp.leg[0].lift_time=0.0000; wp.leg[0].down_time=0.5000;
  wp.leg[1].lift_time=0.5000; wp.leg[1].down_time=1.0000;
  wp.leg[2].lift_time=0.5000; wp.leg[2].down_time=1.0000;
  wp.leg[3].lift_time=0.0000; wp.leg[3].down_time=0.5000;

  wp.body_angle  = RAD(14.0);
  wp.body_height = 100;
  wp.hop  = 10;
  wp.sway = 0;
  wp.front_height = 100.0;
  wp.back_height = 100.0;
  */

  /*
  // Current best trot
  wp.leg[0].neutral.set( 123, 85,0);
  wp.leg[1].neutral.set( 123,-85,0);
  wp.leg[2].neutral.set( -80, 75,0);
  wp.leg[3].neutral.set( -80,-75,0);
  wp.period = 600;

  wp.leg[0].lift_vel.set(0,0, 100);
  wp.leg[1].lift_vel.set(0,0, 100);
  wp.leg[2].lift_vel.set(0,0, 200);
  wp.leg[3].lift_vel.set(0,0, 200);

  wp.leg[0].down_vel.set(0,0,-100);
  wp.leg[1].down_vel.set(0,0,-100);
  wp.leg[2].down_vel.set(0,0,-100);
  wp.leg[3].down_vel.set(0,0,-100);

  wp.leg[0].lift_time=0.0000; wp.leg[0].down_time=0.5000;
  wp.leg[1].lift_time=0.5000; wp.leg[1].down_time=1.0000;
  wp.leg[2].lift_time=0.5000; wp.leg[2].down_time=1.0000;
  wp.leg[3].lift_time=0.0000; wp.leg[3].down_time=0.5000;

  wp.body_angle  = RAD(16.0);
  wp.body_height = 104;
  wp.hop  = 0;
  wp.sway = 0;
  wp.front_height = 100.0;
  wp.back_height = 100.0;
  */

  /*
  // Good trot
  wp.leg[0].neutral.set( 125, 85,0);
  wp.leg[1].neutral.set( 125,-85,0);
  wp.leg[2].neutral.set( -80, 75,0);
  wp.leg[3].neutral.set( -80,-75,0);
  wp.period = 600;

  wp.leg[0].lift_vel.set(0,0, 100);
  wp.leg[1].lift_vel.set(0,0, 100);
  wp.leg[2].lift_vel.set(0,0, 150);
  wp.leg[3].lift_vel.set(0,0, 150);

  wp.leg[0].down_vel.set(0,0,-100);
  wp.leg[1].down_vel.set(0,0,-100);
  wp.leg[2].down_vel.set(0,0,-100);
  wp.leg[3].down_vel.set(0,0,-100);

  wp.leg[0].lift_time=0.0000; wp.leg[0].down_time=0.5000;
  wp.leg[1].lift_time=0.5000; wp.leg[1].down_time=1.0000;
  wp.leg[2].lift_time=0.5000; wp.leg[2].down_time=0.8333;
  wp.leg[3].lift_time=0.0000; wp.leg[3].down_time=0.3333;

  wp.body_angle  = RAD(17.5);
  wp.body_height = 102;
  wp.hop  = 4;
  wp.sway = 0;
  wp.front_height = 100.0;
  wp.back_height = 100.0;
  */

  // front_x, front_y, rear_x, rear_y
  // period, lift_vel, down_vel
  // front_air, rear_air, rear_ofs

  /*
  double w;
  w = 75;
  wp.leg[0].neutral.set( 105, w,0);
  wp.leg[1].neutral.set( 105,-w,0);
  wp.leg[2].neutral.set( -70, w,0);
  wp.leg[3].neutral.set( -70,-w,0);
  wp.period = 700;

  wp.leg[0].lift_vel.set(0,0, 100);
  wp.leg[1].lift_vel.set(0,0, 100);
  wp.leg[2].lift_vel.set(0,0, 100);
  wp.leg[3].lift_vel.set(0,0, 100);

  wp.leg[0].down_vel.set(0,0,-100);
  wp.leg[1].down_vel.set(0,0,-100);
  wp.leg[2].down_vel.set(0,0,-100);
  wp.leg[3].down_vel.set(0,0,-100);

  wp.leg[0].lift_time=0.000; wp.leg[0].down_time=0.3333;
  wp.leg[1].lift_time=0.500; wp.leg[1].down_time=0.8333;
  wp.leg[2].lift_time=0.500; wp.leg[2].down_time=0.8333;
  wp.leg[3].lift_time=0.000; wp.leg[3].down_time=0.3333;

  wp.body_angle  = RAD(10.0);
  wp.body_height = 115;
  wp.hop  = 6;
  wp.sway = 0;
  wp.front_height = 100.0;
  wp.back_height = 100.0;
  */

  out = fopen("walk_xy.prm","wb");
  if(out){
    fwrite(&wp,sizeof(wp),1,out);
    fclose(out);
  }


}

void make_motions()
{
  // BodyStateMotion body[16];
}

void test_walks()
{
  Motion::Motion m;
  Motion::MotionCommand cmd;
  double angles[NumPIDJoints];
  double da;
  double dx,dy;
  int i,e;

  mset(angles,0.0,NumPIDJoints);
  mzero(cmd);

  m.init();
  m.init(angles);
  // m.toggle();

  for(da=0.0; da<=0.7; da+=0.15){
    printf("Table: da=%f\n",da);
    for(dx=-230.0; dx<=230.0; dx+=20.0){
      printf("  ");
      for(dy=-230.0; dy<=230.0; dy+=10.0){
	cmd.bound_mode = Motion::BOUND_NONE;
        cmd.motion_cmd = Motion::MOTION_WALK_TROT;
        cmd.vx = dx;
        cmd.vy = dy;
        cmd.va = da;
        m.setCommand(cmd);

        for(i=0; i<600/8*4; i++) m.getAngles(angles);
        KinClearErrors();
        for(i=0; i<600/8*4; i++) m.getAngles(angles);
        e = KinGetTotalErrors();
	
	if(e > 10)
	  printf(".");
	else
	  printf("#");

        //printf("%c",e?'.':'#'); 
	fflush(stdout);
      }
      printf("\n");
    }
  }
}

int main()
{
  Motion::Motion m;
  Motion::MotionCommand cmd;
  double angles[NumPIDJoints];
  int *err;
  int i;
  int s;

  make_walk_param();
  make_getups();
  make_kicks();
  //test_walks();
  //exit(0);

  mset(angles,0.0,NumPIDJoints);
  mzero(cmd);
  m.init();
  m.init(angles);
  //m.toggle();

  cmd.bound_mode = Motion::BOUND_NONE;
  cmd.motion_cmd = Motion::MOTION_WALK_TROT;
  cmd.head_cmd = Motion::HEAD_SCAN_BALL;
  cmd.vx = 200.0; //270/(1.3*cmd.va+1);
  cmd.vy = 0.0; // 140.00/(1.3*cmd.va+1);
  cmd.va = 0.0;

  s = 0;
  m.setCommand(cmd);

  KinClearErrors();
  for(i=0; i<4000*8; i++) m.getAngles(angles);

  /* 
  err = KinGetErrors();
  int *down_err = m.GetDownErrors();
  int *air_err = m.GetAirErrors();

  printf("Errors=[%d %d %d %d]\n",
         err[0],err[1],err[2],err[3]);
  printf("Errors in the Air=   [%d %d %d %d]\n", 
	 air_err[0],air_err[1],air_err[2],air_err[3]);
  printf("Errors on the Ground =[%d %d %d %d]\n", 
	 down_err[0],down_err[1],down_err[2],down_err[3]);
  */  
  
  err = KinGetErrors();
  printf("Errors=[%d %d %d %d]\n",
         err[0],err[1],err[2],err[3]);

  return(0);
}