/* Virtual Passive Dynamic Walking with Knees Ver.1.01 */
/* Reducing Virtual Gravity (RVG) Control */
/* Output Zeroing Control for Knee-joint */
/* Output Zeroing Control for Hip-joint */
/* For RSJ2002 */

/* 2000.6.1 */
/* 2000.8.4 */
/* 2002.6.12 */
/* 2002.6.13 */
/* 2002.6.14 */
/* 2002.6.17 */
/* 2002.6.24 */
/* 2002.7.1 */

/* Fumihiko Asano */

Matrix	M, C, g, Q_pre, Q_pos, H;
Matrix	J, Jr, Ji;
Real 	gravity, alpha;
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, S;
Integer	count;
Real    Ts, Te;
Real	x_d, nu, u3d, u2d, Kp, Kd, pole, Ki, stime;
Real	th_2d, dth_2d, th_3d, dth_3d;
Real	omega_1, omega_2, int_y_1, int_y_2, y_1, y_2, dy_1, dy_2;
Real	ztime_1, ztime_2, aa_1, aa_2, aa_20, pp, ztime_20, DTH;
Matrix	c1, c2, c3, u_save;
Array	XXX, SAVE, X4;
Real	A5, A4, A3, A2, A1, A0, TT, eps, dth0, th0;

Func void init_params()
{
	Matrix AA, xx;
	Real zt;

	count = 0;	
	
	/* Parameters for the robot */
	m_1 = 5.00;
	m_2 = 3.00;
	read m_2;

	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.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;
    
	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 */
	th_d = [- th0, th0, th0]';
	
	dth_d = [[dth0 + eps]
		[dth0 + eps]
		[dth0 + eps]];

	SAVE = [SAVE,[dth_d(1, 1)]];

	alpha = (abs(th_d(1, 1)) + abs(th_d(2, 1))) / 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]];
	
	dth_d = [[1, 0][0, 1][0, 1]] * inv(Q_pos) * Q_pre * dth_d(1:2, 1);

	lambda = [0.0];

	Ts = Te = 0.0;

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

	c1 = S(1:3, 1);
	c2 = S(1:3, 2);
	c3 = S(1:3, 3);

	/* PID control */
	Kd = 2.0 * pole;
	Kp = pole^2;
	Ki = 0;
	
	/* Knee joint trajectory */
	ztime_1 = 0.70;
	ztime_2 = 0.70;
	ztime_1 = 0.65;
	ztime_2 = 0.65;

	omega_1 = PI / ztime_1;
	omega_2 = 0.50 * PI / ztime_1;

	aa_1 = 0.40;
	aa_2 = abs(th_d(1, 1)) * 2.0;

	zt = ztime_2;

	AA = [[0, 0, 0, 0, 0, 1]
	[0, 0, 0, 0, 1, 0] 
	[0, 0, 0, 2, 0, 0] 
	[zt^5, zt^4, zt^3, zt^2, zt, 1]
	[5 * zt^4, 4 * zt^3, 3 * zt^2, 2 * zt, 1, 0]
	[20 * zt^3, 12 * zt^2, 6 * zt, 2, 0, 0]];

//	xx = [- aa_2, 0.5, 5.0, aa_2, 0, 0]';
	xx = [- aa_2, 0.5, 5.0, aa_2, 0, 0]';
	read xx;

	AA = inv(AA) * xx;

	A5 = AA(1, 1);
	A4 = AA(2, 1);
	A3 = AA(3, 1);
	A2 = AA(4, 1);
	A1 = AA(5, 1);
	A0 = AA(6, 1);

	u_save = Z(3, 1);
}

Func void main()
{
	Matrix	x0;
	Array	T, X, U;
	Real	dt, h, t0, t1, a1, a2, b1, b2, a10, b10;
	Integer	ii;
	void	diff_eqs(), link_eqs(), init_params();
	
	/* ZMP position */
	x_d = 0.035;

	pole = 300.0;
	dth0 = 1.0;
	dth0 = 1.33279;
	dth0 = 1.2;
	dth0 = 1.1;
//	dth0 = 1.3;
	th0 = 0.25;
	th0 = 0.24;

	/* Function parameters definition */
	init_params();
	dt = 1.0e-3;
	h = 1.0e-4;
	t0 = 0.0;
	t1 = 1.0;
	read t1;
	stime = dt;

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

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

	SAVE = [SAVE,[DTH]];

	print [[T][X][U]] >> "d1.mat";
	print [XXX] >> "d3.mat";
	print [X4] >> "d4.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;
	
	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 
	&& Time >= ztime_1){

	/* 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, tau;
	Real	dth_1, th_1, dth_2, th_2, dth_3, th_3, u1, u2, u3;
	Matrix	Xi, dth_pos, input, rvg(), p_c;
    
	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(heel_strike == 1 && motion_stop == 1){

	print "heel_strike!\n";

	/* HEEL-STRIKE */	    

	dth = dth_save;
	th = th_save;

	DTH = inv(3.0) * (abs(dth(1, 1) + abs(dth(2, 1) + abs(dth(3, 1)))));
//	DTH = abs(dth(1, 1));
	print DTH;

	X4 = [X4,[[k][DTH]]];

	k++;

	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;

	int_y_1 = 0.0;
	int_y_2 = 0.0;
	u_save = Z(3, 1);

    }
    
	Time = t - Time_save;

    tau = rvg(th, dth);

    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][inv(S) * tau] // 11, 12, 13
	[c3' * th]	// 14
	[c2' * th]	// 15
	[y_1]
	[y_2]
	[dy_1]
	[dy_2]
	[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)]
	[th_3d]
	[th_2d]];	// 15

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

	XXX = [XXX,[[0][0]]];
	XXX = [XXX,[[l_1 * sin(th_1)][l_1 * cos(th_1)]]];
	XXX = [XXX,[[l_1 * sin(th_1) - l_2 * sin(th_2)]
	[l_1 * cos(th_1) - l_2 * cos(th_2)]]];
	XXX = [XXX,[[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)]]];

	count = 0;

	}

	count++;

	u_save = inv(S) * tau;
}

Func Matrix rvg(th, dth)
Matrix	th, dth;
{
	Matrix	h, tau, XX, u, p_c;
	Real	dth_1, th_1, dth_2, th_2, dth_3, th_3, Rn, ddth_3d, ddth_2d;
	Real	RN(), S1, S2, C1, C2;
	Real	v_1, v_2;

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

	h = C * dth + g;

	Rn = RN(th, dth);

	XX = [[inv(Rn), 0, 0][0, 1, 0][0, 0, 1]];

	/* Output Zeroing Control for Knee-joint */

	if(Time <= ztime_1){

	S1 = sin(omega_1 * Time);
	C1 = cos(omega_1 * Time);

	/* Knee-joint */
	th_3d = aa_1 * S1^3;
	dth_3d = 3 * aa_1 * omega_1 * S1^2 * C1;
	ddth_3d = 3 * aa_1 * omega_1^2 * S1 * (2 * C1^2 - S1^2);

	} else{

	/* Knee-joint */
	th_3d = 0;
	dth_3d = 0;
	ddth_3d = 0;

	}

	if(Time <= (ztime_2 - pp)){

	TT = Time + pp;

	/* Hip-joint */
	th_2d = A5 * TT^5 + A4 * TT^4 + A3 * TT^3 + A2 * TT^2 
	+ A1 * TT + A0;
	dth_2d = 5 * A5 * TT^4 + 4 * A4 * TT^3 + 3 * A3 * TT^2 
	+ 2 * A2 * TT + A1;
	ddth_2d = 20 * A5 * TT^3 + 12 * A4 * TT^2 + 6 * A3 * TT^1 
	+ 2 * A2;

	} else{

	/* Hip-joint */
	th_2d = aa_2;
	dth_2d = 0;
	ddth_2d = 0;

	}

	/* Knee-joint Control */
	y_1 = [c3' * th](1) - th_3d;
	dy_1 = [c3' * dth](1) - dth_3d;
	v_1 = - Kd * dy_1 - Kp * y_1;
	u3d = [inv(c3' * inv(M) * c3) * ([v_1] 
	+ c3' * inv(M) * h + [ddth_3d])](1);

	/* Knee-joint Control */
	y_2 = [c2' * th](1) - th_2d;
	dy_2 = [c2' * dth](1) - dth_2d;
	v_2 = - Kd * dy_2 - Kp * y_2;
	u2d = [inv(c2' * inv(M) * c2) * ([v_2] 
	+ c2' * inv(M) * h + [ddth_2d])](1);

	u = inv(XX) * [x_d, u2d, u3d]';

	return S * u;
}

Func Real RN(th, dth)
Matrix	th, dth;
{
	Matrix	ddth, p_c, p_s, ddy, h;
	Real	dth_1, th_1, dth_2, th_2, dth_3, th_3;

	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);
    
	h = C * dth + g;

	ddth = inv(M) * (S * u_save - h);

	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)]];
		
	ddy = - p_c' * [dth_1^2, dth_2^2, dth_3^2]' - p_s' * ddth;
	ddy = ddy * inv(m_H + 2.0 * m_1);
	
	return (m_H + 2.0 * m_1) * (ddy(1) + gravity);

}