/* Robust active dynamic walking based on */
/* new indeterminate equation Ver.1.0	  */
/* Constant torque ratio control	  */
/* External force can be applied	  */
/* 2003.1.31 */
/* Fumihiko Asano */

Matrix  H, J, Q_pos, Q_pre, S;
Real    a, b, l, m, mh, gravity, phi, mu, mt, vg2;
Integer heel_strike, k;
Matrix  dth_save, th_save;
Real    Time_save, Time, Ts, zeta, E0, fe;
Array	XX;

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

	mt = mh + 2 * m;
	mu = (mh * l + m * l + m * a) / (m * b) - 1.0; 

	gravity = 9.81;

	/* Virtual slope */
	phi = 0.02;
	read phi;

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

	Ts = 0.0;

	heel_strike = 0;

	S = [[1, 1][0, -1]];

	zeta = 10.0;
	read zeta;

	E0 = 150.55;

	k = 0;

	fe = 0.0;
//	read fe;

}    

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 = [- 0.21, 0.21, 0.891, 0.598]';

	{T, X, U} = Ode45Hybrid(0.00, t1, dt, x0, diff_eqs, link_eqs, h);
	
	print [[T][X][U]] >> "d1.mat";
	print [XX] >> "d2.mat";

}   

Func void diff_eqs(DX, t, X, U)
Real t;
Matrix X, DX, U;
{
	Matrix M, C, g, ddth, dth, th, tau, Fe;
	Real   dth1, dth2, th1, th2, toe;

	Time = t - Ts;

	th = X(1:2, 1);
	dth = X(3:4, 1);
	tau = U(1:2, 1);

	th1 = th(1, 1);
	th2 = th(2, 1);

	dth1 = dth(1, 1);
	dth2 = dth(2, 1);

	/* External force */ 
	if(k == 5 && Time <= 0.2){

	Fe = [[l * cos(th1) * fe][0]];

	} else{

	Fe = Z(2, 1);

	}

	tau = tau + Fe;

	toe = l * (cos(th1) - cos(th2));

	if(toe <= 0.0 && Time > 0.50 && heel_strike == 0){

	heel_strike = 1;
	dth_save = dth;
	th_save = th;
	Time_save = Time;

	}
         
	/* Robot dynamics */
	M = C = Z(2);
	g = Z(2, 1);

	M(1, 1) = mh * l^2 + m * a^2 + m * l^2;
	M(1, 2) = M(2, 1) = - m * b * l * cos(th1 - th2);
	M(2, 2) = m * b^2;
        
	C(1, 2) = - m * b * l * sin(th1 - th2) * dth2;
	C(2, 1) = m * b * l * sin(th1 - th2) * dth1;

	g(1, 1) = - (mh * l + m * a + m * l) * sin(th1) * gravity;
	g(2, 1) = m * b * sin(th2) * gravity;

	if(heel_strike == 0){

	ddth = - M~ * (C * dth + g - tau);

	} else{

	ddth = dth = Z(2, 1);

	}

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

Func void link_eqs(U, t, X) 
Real t;
Matrix U, X;
{
	Matrix ddth, dth, th, tau;
	Matrix dth_pos, th_pos, control(), M;
	Real   dth1, dth2, th1, th2, alpha_0, energy, xg;

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

	th1 = th(1, 1);
	th2 = th(2, 1);

	dth1 = dth(1, 1);
	dth2 = dth(2, 1);

	if(heel_strike == 1){

	dth = dth_save;
	th = th_save;

	print t;

	th1 = th(1, 1);
	th2 = th(2, 1);

	dth1 = dth(1, 1);
	dth2 = dth(2, 1);
	  
	alpha_0 = (abs(th2) + abs(th1)) / 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^2 + 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^2]];
   
	H = Q_pos~ * Q_pre;
        
	dth = H * dth;
	th = J * th;

	k++;

	XX = [XX,[[k][Time_save][2 * l * sin(alpha_0)]]];

	Time = 0.0;
	Ts = t;

	heel_strike = 0;

	}

	X = [[th][dth]];

	th1 = th(1, 1);
	th2 = th(2, 1);

	dth1 = dth(1, 1);
	dth2 = dth(2, 1);
	  
	xg = (mh * l + m * l + m * a) * sin(th1);
	xg = (xg - m * b * sin(th2)) / mt;

	M = Z(2);
	M(1, 1) = mh * l^2 + m * a^2 + m * l^2;
	M(1, 2) = M(2, 1) = - m * b * l * cos(th1 - th2);
	M(2, 2) = m * b^2;

	energy = ((mh * l + m * l + m * a) * cos(th1) 
	- m * b * cos(th2)) * gravity;

	energy = energy + 0.5 * [dth' * M * dth](1);

	tau = control(th, dth, xg, energy);

	U = [[tau][S~ * tau][xg][energy][vg2]];

}

Func Matrix control(th, dth, xg, ene)
Matrix	th, dth;
Real	xg, ene;
{
	Real	dth1, dth2, th1, th2, vg, Ed;
	Matrix	tau;

	Ed = E0 + mt * gravity * tan(phi) * xg;

	th1 = th(1, 1);
	th2 = th(2, 1);

	dth1 = dth(1, 1);
	dth2 = dth(2, 1);

	vg = (mh * l + m * l + m * a) * cos(th1) * dth1;
	vg = (vg - m * b * cos(th2) * dth2) / mt;

	tau = [[mu + 1][-1]] * (mt * gravity * tan(phi) * vg - 
	zeta * (ene - Ed));
	tau = tau * inv((mu + 1) * dth1 - dth2);

	vg2 = vg;

	return tau;
}