/* Virtual Passive Dynamic Walking with Knees Ver.1.01 */

/* 2000.6.1 */
/* 2000.8.4 */

/* Fumihiko Asano */

Matrix	M, C, g;
Matrix	J, Jr, Ji;
Real 	gravity;
Real 	phi;
Integer i, j, k, knee_lock, heel_strike, motion_stop;
Real	m_1, m_2, m_3, m_H;
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	lambda;
Matrix	dth_d, th_d;
Integer	count;
Real    Ts, Te;

Func void init_params()
{

	/* Parameters for the robot */
	m_1 = 5.00;
	m_2 = 3.50;
	m_3 = m_1 - m_2;
	m_H = 10.00;
	l_1 = 1.00;
	l_2 = 0.50;
	l_3 = 0.50;
	a_2 = 0.150;
	a_3 = 0.250;
	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.050;

	knee_lock = 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];

	/* Steady initial condition */
	/* phi = 0.05 [rad] */
	dth_d = [[1.07713136E+00]
		[3.86374372E-01]
		[3.86374372E-01]];
	th_d = [[-2.68994192E-01]
		[2.69005282E-01]
		[2.69005282E-01]];
	
	lambda = [0.0];

	Ts = Te = 0.0;
}

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 = 3;
	read t1;

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

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

	print [[T][X][U]] >> "knee.mat";
}

Func void diff_eqs(DX, t, X, U)
Real	t;
Matrix	X, DX, U;
{
	Matrix	ddth, dth, th, h, tau;
	Real	dth_1, th_1, dth_2, th_2, dth_3, th_3, toe;

	Time = t - Time_save;

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

	dth_1 = dth(1, 1);
	dth_2 = dth(2, 1);
	dth_3 = dth(3, 1);

	th_1 = th(1, 1);
	th_2 = th(2, 1);
	th_3 = th(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;
	
	if(Time >= 0.40 && th_3 <= th_2 && th_3 < 0.0 && knee_lock == 0){

	/* Knee-lock */

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

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

	print Time;
	} 
	
	toe = l_1 * cos(th_1) - l_2 * cos(th_2) - l_3 * cos(th_3);

	if(toe <= 0.0 && knee_lock == 1 && heel_strike == 0 && motion_stop == 0){

	/* 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(knee_lock == 0){

		lambda = [0.0];

	} else{

		lambda = - inv(Jr * M~ * Jr') * Jr * M~ * h;


	}

	if(motion_stop == 1){

	ddth = dth = Z(3, 1);

	} else{	

	ddth = M~ * (- h - Jr' * lambda);

	}

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

Func void link_eqs(U, t, X)
Real	t;
Matrix	U, X;
{
	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;
	Real	alpha;
    
	dth = X(1:3, 1);
	th = X(4:6, 1);

	dth_1 = dth(1, 1);
	dth_2 = dth(2, 1);
	dth_3 = dth(3, 1);

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

	print t;

	}

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

	/* KNEE-STRIKE */	    

	dth = dth_save;
	th = th_save;

	print "knee_strike!\n";
	
	/* Flug reset */

	motion_stop = 0;

	Xi = Ji * M~ * Ji';

	dth = (I(3) - M~ * Ji' * Xi~ * Ji) * dth;
	
	X = [[dth][th]];

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

    }

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

	print "heel_strike!\n";

	/* HEEL-STRIKE */	    

	dth = dth_save;
	th = th_save;

	dth_1 = dth(1, 1);
	dth_2 = dth(2, 1);
	dth_3 = dth(3, 1);

	th_1 = th(1, 1);
	th_2 = th(2, 1);
	th_3 = th(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]';
	
	dth = [dth_pos(1, 1), dth_pos(2, 1), dth_pos(2, 1)]';
	th = [th_3, th_1, th_1]';
	
	knee_lock = 0;
	heel_strike = 0;
	motion_stop = 0;

	Time_save = t;

	k++;

	Ts = t;

    }
    
    tau = [[(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)]] * gravity * tan(phi);

    u1 = tau(1, 1) + tau(2, 1) + tau(3, 1);
    u2 = - tau(2, 1) - tau(3, 1);
    u3 = tau(3, 1);

    if(knee_lock == 1){

      u3 = 0.0;

    }

    tau = [[1, 1, 0][0, -1, -1][0, 0, 1]] * [u1, u2, u3]';

    X = [[dth][th]];

    U = [[tau][u1][u2][u3]];

}