/* Active dynamic walking with knees based on	*/
/* new indeterminate equation Ver.1.0		*/
/* Knee-joint PID control */
/* 2003.1.30 */
/* Fumihiko Asano */

Matrix	J, Jr, Ji;
Real 	gravity;
Real 	phi;
Integer count, sw;
Integer i, j, k, heel_strike, motion_stop;
Real	m_1, m_2, m_3, m_H, mt;
Real	a_1, a_2, a_3;
Real	l_1, l_2, l_3;
Real	b_1, b_2, b_3;
Real	Time_save, Time;
Matrix	dth_save, th_save;
Matrix	dth_d, th_d, S, S3, tau_save;
Real    Ts, Te, mu, ss, stime, T1, aa, Kp, Kd, Ki;
Real    Kp, Kd, Ki, omega, pole, sum_y, x_d, eta, zmp;
Array	XX;

Func void init_params()
{

	/* Parameters for the robot */
	m_1 = 5.00;
	m_2 = 3.00;
	m_3 = m_1 - m_2;
	m_H = 10.00;
	mt = m_H + m_1 + m_2 + m_3;

	l_1 = 1.00;
	l_2 = 0.50;
	l_3 = 0.50;
	a_2 = 0.20;
	a_3 = 0.20;
	a_1 = (m_2 * (l_3 + a_2) + m_3 * a_3) / m_1;
	b_1 = l_1 - a_1;
	b_2 = l_2 - a_2;
	b_3 = l_3 - a_3;

	gravity = 9.81;
    
	/* Virtual gravity field */
	phi = 0.025;

	heel_strike = 0;
    
	i = j = k = 0;

	/* Timer reset */
	Time_save = 0.00;
	Time = 0.00;
	motion_stop = 0;

	/* Matrix definitions */
	J = [[0 1][1 0]];
	Jr = [0, -1, 1];
	Ji = [0, -1, 1];
	S3 = [0, -1, 1]';

	/* Steady initial condition */
	th_d = [[-0.224]
		[0.224]
		[0.224]];
	dth_d = [[0.92]
		[0.56]
		[0.56]];

	Ts = Te = 0.0;

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

	ss = 0.0;

	T1 = 0.75;
	omega = PI / T1;
	aa = 0.40;

	/* PID gain */
	pole = 10.0;
	Kd = 3 * pole;
	Kp = 3 * pole^2;
	Ki = pole^3;

	sum_y = 0.0;

	/* ZMP setting */
	x_d = 0.022;

	tau_save = Z(3, 1); 
}

Func void main()
{
	Matrix	x0;
	Array	T, X, U;
	Real	dt, h, t0, t1;
	void	diff_eqs(), link_eqs(), init_params();
    
	init_params();
    
	/* Function parameters definition */
	dt = 1.0e-3;
	h = 1.0e-4;
	t0 = 0.0;
	t1 = 5;
	stime = dt;
	read t1;

	/* Initial condition */
	x0 = [[th_d][dth_d]];

	{T, X, U} = Ode45Hybrid(t0, 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	ddth, dth, th, h, tau, M, C, g;
	Real	dth_1, th_1, dth_2, th_2, dth_3, th_3, toe;

	Time = t - Time_save;

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

	th_1 = th(1, 1);
	th_2 = th(2, 1);
	th_3 = th(3, 1);

	dth_1 = dth(1, 1);
	dth_2 = dth(2, 1);
	dth_3 = dth(3, 1);
    
	M(1, 1) = m_1 * a_1^2 + (m_H + m_2 + m_3) * l_1^2;
	M(1, 2) = - (m_2 * b_2 * l_1 + m_3 * l_1 * l_2) * cos(th_1 - th_2);
	M(1, 3) = - m_3 * b_3 * l_1 * cos(th_1 - th_3);
	M(2, 1) = - (m_2 * b_2 * l_1 + m_3 * l_1 * l_2) * cos(th_1 - th_2);
	M(2, 2) = m_2 * b_2^2 + m_3 * l_2^2;
	M(2, 3) = m_3 * b_3 * l_2 * cos(th_2 - th_3);
	M(3, 1) = - m_3 * b_3 * l_1 * cos(th_1 - th_3);
	M(3, 2) = m_3 * b_3 * l_2 * cos(th_2 - th_3);
	M(3, 3) = m_3 * b_3^2;
	
	C(1, 1) = C(2, 2) = C(3, 3) = 0.00;
	C(1, 2) = - (m_2 * b_2 * l_1 + m_3 * l_1 * l_2) * 
	sin(th_1 - th_2) * dth_2;
	C(1, 3) = - m_3 * b_3 * l_1 * sin(th_1 - th_3) * dth_3;
	C(2, 1) = (m_2 * b_2 * l_1 + m_3 * l_1 * l_2) * 
	sin(th_1 - th_2) * dth_1;
	C(2, 3) = m_3 * b_3 * l_2 * sin(th_2 - th_3) * dth_3;
	C(3, 1) = m_3 * b_3 * l_1 * sin(th_1 - th_3) * dth_1;
	C(3, 2) = - m_3 * b_3 * l_2 * sin(th_2 - th_3) * dth_2;

	g(1, 1) = (- (m_1 * a_1 + m_2 * l_1 + m_3 * l_1 + m_H * l_1) * 
	sin(th_1)) * gravity;
	g(2, 1) = ((m_2 * b_2 + m_3 * l_2) * sin(th_2)) * gravity;
	g(3, 1) = (m_3 * b_3 * sin(th_3)) * gravity;
	
	toe = l_1 * cos(th_1) - l_2 * cos(th_2) - l_3 * cos(th_3);

	if(toe <= 0.0 && heel_strike == 0 && motion_stop == 0 && ss > 0.5){

	/* Heel-strike */
	dth_save = dth;
	th_save = th;

	/* Flug set */
	motion_stop = 1;
	heel_strike = 1;

	Te = t - Ts;

	} 
	
	h = C * dth + g - tau;

	if(motion_stop == 1){

	ddth = dth = Z(3, 1);

	} else{	

	ddth = - inv(M) * h;

	}

	DX = [[dth][ddth]];

}

Func void link_eqs(U, t, X)
Real	t;
Matrix	U, X;
{
	Matrix	M, C, g, h;
	Matrix	dth, th, Q_pre, Q_pos, H, tau;
	Real	dth_1, th_1, dth_2, th_2, dth_3, th_3, u1, u2, u3;
	Matrix	Xi, dth_pos, input, u, control();
	Real	alpha, xg, energy(), ene;
    
	th = X(1:3, 1);
	dth = X(4:6, 1);

	th_1 = th(1, 1);
	th_2 = th(2, 1);
	th_3 = th(3, 1);
	
	dth_1 = dth(1, 1);
	dth_2 = dth(2, 1);
	dth_3 = dth(3, 1);

	if(motion_stop == 1){

	print t;

	}

	if(heel_strike == 1 && motion_stop == 1){

	print "heel_strike!\n";

	/* HEEL-STRIKE */	    

	dth = dth_save;
	th = th_save;

	th_1 = th(1, 1);
	th_2 = th(2, 1);
	th_3 = th(3, 1);
	
	dth_1 = dth(1, 1);
	dth_2 = dth(2, 1);
	dth_3 = dth(3, 1);

	alpha = (abs(th_1) + abs(th_3)) / 2.0;
	
	Q_pre = [[(m_H * l_1^2 + 2 * m_1 * a_1 * l_1) * cos(2 * alpha) 
	- m_1 * a_1 * b_1, - m_1 * a_1 * b_1]
		  [- m_1 * a_1 * b_1, 0]];  
	
	Q_pos = [[m_H * l_1^2 + m_1 * a_1^2 
	+ m_1 * l_1 * (l_1 - b_1 * cos(2 * alpha)), 
	m_1 * b_1 * (b_1 - l_1 * cos(2 * alpha))]
		  [- m_1 * b_1 * l_1 * cos(2 * alpha), m_1 * b_1^2]];
	
	H = Q_pos~ * Q_pre;
	
	dth_pos = H * [dth_1, dth_3]';
	
	th = [th_3, th_1, th_1]';
	dth = [dth_pos(1, 1), dth_pos(2, 1), dth_pos(2, 1)]';
	
	heel_strike = 0;
	motion_stop = 0;

	Time_save = t;

	k++;

	Ts = t;

	X = [[th][dth]];

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

	ss = 0.0;
	sw = 0;
	
	sum_y = 0.0;
	
    }
    
	th_1 = th(1, 1);
	th_2 = th(2, 1);
	th_3 = th(3, 1);
	
	dth_1 = dth(1, 1);
	dth_2 = dth(2, 1);
	dth_3 = dth(3, 1);

	M(1, 1) = m_1 * a_1^2 + (m_H + m_2 + m_3) * l_1^2;
	M(1, 2) = - (m_2 * b_2 * l_1 + m_3 * l_1 * l_2) * cos(th_1 - th_2);
	M(1, 3) = - m_3 * b_3 * l_1 * cos(th_1 - th_3);
	M(2, 1) = - (m_2 * b_2 * l_1 + m_3 * l_1 * l_2) * cos(th_1 - th_2);
	M(2, 2) = m_2 * b_2^2 + m_3 * l_2^2;
	M(2, 3) = m_3 * b_3 * l_2 * cos(th_2 - th_3);
	M(3, 1) = - m_3 * b_3 * l_1 * cos(th_1 - th_3);
	M(3, 2) = m_3 * b_3 * l_2 * cos(th_2 - th_3);
	M(3, 3) = m_3 * b_3^2;

	C(1, 1) = C(2, 2) = C(3, 3) = 0.00;
	C(1, 2) = - (m_2 * b_2 * l_1 + m_3 * l_1 * l_2) * 
	sin(th_1 - th_2) * dth_2;
	C(1, 3) = - m_3 * b_3 * l_1 * sin(th_1 - th_3) * dth_3;
	C(2, 1) = (m_2 * b_2 * l_1 + m_3 * l_1 * l_2) * 
	sin(th_1 - th_2) * dth_1;
	C(2, 3) = m_3 * b_3 * l_2 * sin(th_2 - th_3) * dth_3;
	C(3, 1) = m_3 * b_3 * l_1 * sin(th_1 - th_3) * dth_1;
	C(3, 2) = - m_3 * b_3 * l_2 * sin(th_2 - th_3) * dth_2;

	g(1, 1) = (- (m_1 * a_1 + m_2 * l_1 + m_3 * l_1 + m_H * l_1) * 
	sin(th_1)) * gravity;
	g(2, 1) = ((m_2 * b_2 + m_3 * l_2) * sin(th_2)) * gravity;
	g(3, 1) = (m_3 * b_3 * sin(th_3)) * gravity;
	
	h = C * dth + g;

	xg = (m_H * l_1 + m_1 * a_1 + m_2 * l_1 + m_3 * l_1) *
	sin(th_1) - (m_2 * b_2 + m_3 * l_2) * sin(th_2) - m_3 * b_3 *
	sin(th_3);
	xg = xg * inv(mt);

	tau = control(th, dth, ss, M, h, xg);

	ene = energy(th, dth, M);

	u = inv(S) * tau;

	U = [[tau][u][xg][ene][ss][S3' * th][zmp]];

	if(k == 3){

	i++;

	if(t == 0 || i == 50){

	XX = [XX, [[0][0]]];
	XX = [XX, [[l_1 * sin(th_1)][l_1 * cos(th_1)]]];
	XX = [XX, [[l_1 * sin(th_1) - l_2 * sin(th_2)]
	[l_1 * cos(th_1) - l_2 * cos(th_2)]]];
	XX = [XX, [[l_1 * sin(th_1) - l_2 * sin(th_2) - l_3 * sin(th_3)]
	[l_1 * cos(th_1) - l_2 * cos(th_2) - l_3 * cos(th_3)]]];

	i = 0;	

	}

	}

	ss = ss + stime;
	
}

Func Matrix control(th, dth, ss, M, h, xg)
Matrix	th, dth, M, h;
Real	ss, xg;
{
	Matrix	tau, u, ddth, p_c, p_s, u_save;
	Real	th_1, th_2, th_3, dth_1, dth_2, dth_3, vg;
	Real	yy, dyy, yy_d, dyy_d, ddyy_d, SIN, COS, ddy, Rn;

	th_1 = th(1, 1);
	th_2 = th(2, 1);
	th_3 = th(3, 1);
	
	dth_1 = dth(1, 1);
	dth_2 = dth(2, 1);
	dth_3 = dth(3, 1);

	tau = Z(3, 1);

	/* --- Reaction force --- */
	p_c = [[(m_1 * a_1 + m_2 * l_1 + m_3 * l_1 + m_H * l_1) * cos(th_1)]
	[- (m_2 * b_2 + m_3 * l_2) * cos(th_2)]
	[- m_3 * b_3 * cos(th_3)]];
	p_s = [[(m_1 * a_1 + m_2 * l_1 + m_3 * l_1 + m_H * l_1) * sin(th_1)]
	[- (m_2 * b_2 + m_3 * l_2) * sin(th_2)]
	[- m_3 * b_3 * sin(th_3)]];

	ddth = - inv(M) * (h - tau_save);
	ddy = - [p_c' * [dth_1^2, dth_2^2, dth_3^2]' + p_s' * ddth](1);
	Rn = ddy + mt * gravity;

	/* --- Knee torque --- */
	yy = [S3' * th](1);
	dyy = [S3' * dth](1);

	if(ss <= T1){

        SIN = sin(omega * ss);
        COS = cos(omega * ss);
        yy_d = aa * SIN^3;
        dyy_d = 3 * aa * omega * SIN^2 * COS;
        ddyy_d = 3 * aa * omega^2 * SIN * (2 * COS^2 - SIN^2);

	} else{

	yy_d = 0.0;
	dyy_d = 0.0;
        ddyy_d = 0.0;

	}

	u(3, 1) = Kd * (dyy_d - dyy) + Kp * (yy_d - yy) + Ki * sum_y;

	sum_y = sum_y + (yy_d - yy) * stime;

	/* --- Indeterminate equation --- */
	vg = (m_H * l_1 + m_1 * a_1 + m_2 * l_1 + m_3 * l_1) *
	cos(th_1) * dth_1 - (m_2 * b_2 + m_3 * l_2) * cos(th_2) * dth_2 - m_3
	* b_3 * cos(th_3) * dth_3;
	vg = vg * inv(mt);

	if(xg <= 0.10){

	u(1, 1) = Rn * x_d;
        u(2, 1) = mt * gravity * tan(phi) * vg - u(1, 1) * dth_1 -
        u(3, 1) * (dth_3 - dth_2);
        u(2, 1) = u(2, 1) * inv(dth_1 - dth_2);

	} else{

	if(sw == 0){

	sw = 1;

	u_save = inv(S) * tau_save;

	eta = mt * gravity * tan(phi) * vg - u_save(1, 1) * dth_1 - 
	u_save(3, 1) * (dth_3 - dth_2);
	eta = eta * inv((dth_1 - dth_2)^2);

	print eta;

	}

	u(2, 1) = eta * (dth_1 - dth_2);
	u(1, 1) = mt * gravity * tan(phi) * vg - u(3, 1) * 
	(dth_3 - dth_2);
	u(1, 1) = (u(1, 1) - (dth_1 - dth_2) * u(2, 1)) * inv(dth_1);

	}

	zmp = - u(1, 1) * inv(Rn);

	tau = S * u;

	tau_save = tau;

	return	tau;

}

Func Real energy(th, dth, M)
Matrix	th, dth, M;
{
	Real	th_1, th_2, th_3, ene;

	th_1 = th(1, 1);
	th_2 = th(2, 1);
	th_3 = th(3, 1);
	
	ene = (m_H * l_1 + m_1 * a_1 + m_2 * l_1 + m_3 * l_1) *
	cos(th_1) - (m_2 * b_2 + m_3 * l_2) * cos(th_2) - 
	m_3 * b_3 * cos(th_3);
	ene = ene * gravity;
	ene = ene + 0.5 * [dth' * M * dth](1);
	
	return ene;
}