function stick3D(theta) 
% Input: theta: angles of each joint, 1x8 vector
% Output: Plot the legs in 3D

M{1,1} = [0.939692000000000,-0.342021000000000,1.06330000000000e-07,0.133632000000000;0.342020000000000,0.939693000000000,-4.99153000000000e-08,-0.175942000000000;-8.28458000000000e-08,8.32722000000000e-08,1,0.0822827000000000;0,0,0,1];
M{1,2} = [1,-7.79704000000000e-17,-2.10284000000000e-08,-1.56828000000000e-06;7.79704000000000e-17,1,1.86594000000000e-15,-0.606984000000000;2.10284000000000e-08,-1.86594000000000e-15,1,5.96046000000000e-07;0,0,0,1];
M{1,3} = [1,6.18174000000000e-08,4.15020000000000e-07,-1.99659000000000e-06;-4.15020000000000e-07,-7.54979000000000e-08,1,-0.553825000000000;6.18174000000000e-08,-1,-7.54979000000000e-08,6.55651000000000e-07;0,0,0,1];
M{1,4} = [0.939693000000000,0.342020000000000,-1.34265000000000e-07,-0.127991000000000;-0.342020000000000,0.939693000000000,-4.49897000000000e-08,-0.175943000000000;1.10781000000000e-07,8.81979000000000e-08,1,0.0822826000000000;0,0,0,1];
M{1,5} = [1,4.42646000000000e-18,5.01038000000000e-09,1.56828000000000e-06;-4.42646000000000e-18,1,4.71169000000000e-16,-0.630394000000000;-5.01038000000000e-09,-4.71169000000000e-16,1,6.55651000000000e-07;0,0,0,1];
M{1,6} = [1,-1.05770000000000e-07,-1.92253000000000e-07,2.11850000000000e-06;1.92253000000000e-07,-7.54979000000000e-08,1,-0.555133000000000;-1.05770000000000e-07,-1,-7.54979000000000e-08,6.55651000000000e-07;0,0,0,1];

% kinematcis
[p_hip_l, p_knee_l, p_foot_l] = kin3D(M(1:3), theta(1:4));
[p_hip_r, p_knee_r, p_foot_r] = kin3D(M(4:6), theta(5:8));

% visualize
vis3D(p_hip_l, p_knee_l, p_foot_l, p_hip_r, p_knee_r, p_foot_r);

end