/* Active dynamic walking of the compass-gait biped Ver.1.0 */
/* Constant torque ratio control  */
/* 2003.1.29 */
/* Fumihiko Asano */

Matrix  H, J, Q_pos, Q_pre, S;
Real    a, b, l, m, mh, gravity, phi, mu, mt, vg2;
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;
	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]];

}    

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.1957, 0.1957, 0.891, 0.598]';

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

}   

Func void diff_eqs(DX, t, X, U)
Real t;
Matrix X, DX, U;
{
	Matrix M, C, g, ddth, dth, th, tau;
	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);

	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;

	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;

	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);
	  
	tau = control(th, dth);

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

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

}

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

	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;
	tau = tau * inv((mu + 1) * dth1 - dth2);

	vg2 = vg;

	return tau;
}