/* Passive Dynamic Walking Ver.1.0 */

/* 2000.6.1 */

/* Fumihiko Asano */

Matrix  M, N, g, H, J, Q_pos, Q_pre;
Real    a, b, l, m, mh, gravity;
Real    gam;
Matrix  th_d, dth_d;
Integer heel_strike;
Matrix  dth_save, th_save;
Real    Time_save, Ts, Te, Time;

Func void init()
{
     /* Numerical settings of the robot */
     a = 0.50;
     l = 1.00;
     b = l - a;
     m = 5.0;
     mh = 10.0;
     gravity = 9.81;

     /* Slope */
     gam = 0.05;
     
     J = [[0 1][1 0]];

     /* counter reset */
     heel_strike = 0;
     Ts = Te = 0.0;

     /* INITIAL CONDITIONS */
     dth_d = [[1.20]
	      [0.50]];

     th_d = [[-0.30 + gam]
	     [0.30 + gam]];

}    

Func void main()
{
     Matrix x0;
     Array  T, X, U;
     Real   dt, h, t0, t1;
     void   diff_eqs(), link_eqs(), init();

     init();

     h = 1.0e-4;
     dt = 1.0e-3;
     t0 = 0.00;
     t1 = 3.00;
     read t1;
     x0 = [[dth_d][th_d]];

     {T, X, U} = Ode45Hybrid(0.00, t1, dt, x0, diff_eqs, link_eqs, h);

     print [[T][X][U]] >> "pdw.mat";

}   

Func void diff_eqs(DX, t, X, U)
     Real t;
     Matrix X, DX, U;
{
     Matrix ddth, dth, th;
     Real   dth_st, dth_sw, th_st, th_sw, toe;

     Time = t - Ts;

     dth = X(1:2, 1);
     th = X(3:4, 1);
    
     dth_st = dth(1, 1);
     dth_sw = dth(2, 1);

     th_st = th(1, 1);
     th_sw = th(2, 1);

     toe = l * (cos(th_st - gam) - cos(th_sw - gam));

     if(toe <= 0.0 && Time > 0.50 && heel_strike == 0){
	  /* 遊脚の接地を判定します */
	  heel_strike = 1;
      
	  dth_save = dth;
	  th_save = th;
	  Time_save = Time;
    
     }
        
     M = [[mh * l^2 + m * a^2 + m * l^2, - m * b * l * cos(th_st - th_sw)]
	  [- m * b * l * cos(th_st - th_sw), m * b * b]];
        
     N = [[0, - m * b * l * sin(th_st - th_sw) * dth_sw]
	  [m * b * l * sin(th_st - th_sw) * dth_st, 0]];

     g = [[-(mh * l + m * a + m * l) * sin(th_st)]
	  [m * b * sin(th_sw)]] * gravity;

     if(heel_strike == 0){

	  ddth = - M‾ * (N * dth + g);

     } else{

	  ddth = dth = Z(2, 1);

     }

     DX = [[ddth][dth]];
}

Func void link_eqs(U, t, X) 
     Real t;
     Matrix U, X;
{
     Matrix ddth, dth, th;
     Matrix dth_pos, th_pos;
     Real   dth_st, dth_sw, th_st, th_sw, alpha_0;

     dth = X(1:2, 1);
     th = X(3:4, 1);
    
     dth_st = dth(1, 1);
     dth_sw = dth(2, 1);

     th_st = th(1, 1);
     th_sw = th(2, 1);

     if(heel_strike == 1){

	  dth = dth_save;
	  th = th_save;

	  print t;

	  dth_st = dth(1, 1);
	  dth_sw = dth(2, 1);
	  
	  th_st = th(1, 1);
	  th_sw = th(2, 1);

	  alpha_0 = (abs(th_sw) + abs(th_st)) / 2.0;
      
	  Q_pre = [[(mh * l^2 + 2 * m * a * l) * cos(2 * alpha_0) - m * a * b, - m *
 a * b]
		   [- m * a * b, 0]];
    
	  Q_pos = [[mh * l^2 + m * a * a + m * l * (l - b * cos(2 * alpha_0)), m * b * (b - l * cos(2 * alpha_0))]
		   [- m * b * l * cos(2 * alpha_0), m * b * b]];
   
	  H = Q_pos‾ * Q_pre;
        
	  dth = H * [dth_st, dth_sw]';
	  th = J * [th_st, th_sw]';

	  Time = 0.0;
	  Ts = t;
	  heel_strike = 0;

     }

     X = [[dth][th]];

     U = [0];
}