/* Virtual 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, gam;
Integer heel_strike;
Matrix  dth_save, th_save;
Real    Time_save, Time, Ts;

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;

     /* Virtual slope */
     gam = 0.02;

     J = [[0 1][1 0]];

     Ts = 0.0;

     /* 遊脚の接地のフラグです */
     heel_strike = 0;
}    

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 = 1.00;
     read t1;
     x0 = [1.00, 0.80, -0.20, 0.20]';

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

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

}   

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

     Time = t - Ts;

     dth = X(1:2, 1);
     th = X(3:4, 1);
     tau = U(1:2, 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) - cos(th_sw));

     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 - tau);

     } 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, tau;
     Matrix dth_pos, th_pos, input;
     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;
	  th = J * th;

	  Time = 0.0;
	  Ts = t;

	  /* フラグのリセットです */
	  heel_strike = 0;

     }

     X = [[dth][th]];

     tau = [[(mh * l + m * l + m * a) * cos(th_st)]
           [- m * b * cos(th_sw)]] * gravity * tan(gam);

     U = [[tau][tau(1, 1) + tau(2, 1)][- tau(2, 1)]];
}