function [Fjx,Fjy, Fm] = BioM(aa, ak, aw, as, H, M, mobj)
%initial
g = 9.81;
%object position
x = 0.25;
y = 0.5;
%angles
aground = 180*pi/180;
aankle = aa*pi/180;
aknee = ak*pi/180;
awaist = aw*pi/180;
ashoulder = as*pi/180;
%lengths;
lfeet = (0.152)*H;
hfeet = (0.039)*H;
llegs = (0.285-0.039)*H;
lthighs = 0.245*H;
ltnh = (0.818-0.53)*H;
lua = 0.186*H;
lfah = (0.145+0.108/2)*H;
%mass
mfeet = 0.0145*2*M;
mlegs = 0.0465*2*M;
mthighs = 0.1*2*M;
mtnh = 0.578*M;
mua = 0.028*2*M;
mfah = 0.022*2*M;
msum = mfeet+mlegs+mthighs+mtnh+mua+mfah+mobj;
%Weight matrix
Wmfeet = [0; -mfeet*g; 0];
Wmlegs = [0; -mlegs*g; 0];
Wmthighs = [0; -mthighs*g; 0];
Wmtnh = [0; -mtnh*g; 0];
Wmua = [0; -mua*g; 0];
Wmfah = [0; -mfah*g; 0];
Wmsum = [0; -msum*g; 0];
%Weight
Wfeet = mfeet*g;
Wlegs = mlegs*g;
Wthighs = mthighs*g;
Wtnh = mtnh*g;
Wua = mua*g;
Wfah = mfah*g;
Wsum = msum*g;
%center of gravity
cgfeet = 0.095*H;
cglegs = 0.139*H;
cgthighs = 0.139*H;
%NOTE - the root is defined as the pelvis. Once we pass the hip, swap from distal to proximal.
cgtnh = 0.190*H;
cgua = 0.081*H;
cgfah = 0.099*H;
cgobj = x;
%Normal force
FN = [0; Wsum; 0];
rFN = [-0.5*0.152*H 0 0];
%Location of Normal force on the feet [m]
lN = (Wfeet*cgfeet + Wlegs*cglegs + Wthighs*cgthighs + Wtnh*cgtnh + Wua*cgua + Wfah*cgfah + mobj*g*cgobj)./FN;
%Set up the rotation matrices
Rground = [cos(aground) -sin(aground) 0 0; sin(aground) cos(aground) 0 0; 0 0 1 0; 0 0 0 1];
Rankle = [cos(aankle) -sin(aankle) 0 0; sin(aankle) cos(aankle) 0 0; 0 0 1 0; 0 0 0 1];
Rknee = [cos(aknee) -sin(aknee) 0 0; sin(aknee) cos(aknee) 0 0; 0 0 1 0; 0 0 0 1];
Rwaist = [cos(awaist) -sin(awaist) 0 0; sin(awaist) cos(awaist) 0 0; 0 0 1 0; 0 0 0 1];
Rshoulder = [cos(ashoulder) -sin(ashoulder) 0 0; sin(ashoulder) cos(ashoulder) 0 0; 0 0 1 0; 0 0 0 1];
%Set up the lengths
Lfeet = [1 0 0 lfeet; 0 1 0 -hfeet; 0 0 1 0; 0 0 0 1];
Llegs = [1 0 0 llegs; 0 1 0 0; 0 0 1 0; 0 0 0 1];
Lthighs = [1 0 0 lthighs; 0 1 0 0; 0 0 1 0; 0 0 0 1];
Ltnh = [1 0 0 ltnh; 0 1 0 0; 0 0 1 0; 0 0 0 1];
Lua = [1 0 0 lua; 0 1 0 0; 0 0 1 0; 0 0 0 1];
Lfah = [1 0 0 lfah; 0 1 0 0; 0 0 1 0; 0 0 0 1];
%Create secondary link matrices for the distance to the CG on each link.
Lfeetcg = [1 0 0 cgfeet; 0 1 0 0; 0 0 1 0; 0 0 0 1];
Llegscg = [1 0 0 cglegs; 0 1 0 0; 0 0 1 0; 0 0 0 1];
Lthighscg = [1 0 0 cgthighs; 0 1 0 0; 0 0 1 0; 0 0 0 1];
Ltnhcg = [1 0 0 cgtnh; 0 1 0 0; 0 0 1 0; 0 0 0 1];
Luacg = [1 0 0 cgua; 0 1 0 0; 0 0 1 0; 0 0 0 1];
Lfahcg = [1 0 0 cgfah; 0 1 0 0; 0 0 1 0; 0 0 0 1];
%Displacement of x value for each segment
Tfeet = (Rground * Lfeet);
Tlegs = (Rground*Lfeet*Rankle*Llegs);
Tthighs = (Rground*Lfeet*Rankle*Llegs*Rknee*Lthighs);
Ttnh = (Rground*Lfeet*Rankle*Llegs*Rknee*Lthighs*Rwaist*Ltnh);
Tua = (Rground*Lfeet*Rankle*Llegs*Rknee*Lthighs*Rwaist*Ltnh*Rshoulder*Lua);
Tfah = (Rground*Lfeet*Rankle*Llegs*Rknee*Lthighs*Rwaist*Ltnh*Rshoulder*Lua*Lfah);
Tobj = (Rground*Lfeet*Rankle*Llegs*Rknee*Lthighs*Rwaist*Ltnh*Rshoulder*Lua*Lfah);
%Calculate the x value of displacement for each CG
Tfeetcg = (Rground * Lfeetcg);
Tlegscg = (Rground*Lfeet*Rankle*Llegscg);
Tthighscg = (Rground*Lfeet*Rankle*Llegs*Rknee*Lthighscg);
Ttnhcg = (Rground*Lfeet*Rankle*Llegs*Rknee*Lthighs*Rwaist*Ltnhcg);
Tuacg = (Rground*Lfeet*Rankle*Llegs*Rknee*Lthighs*Rwaist*Ltnh*Rshoulder*Luacg);
Tfahcg = (Rground*Lfeet*Rankle*Llegs*Rknee*Lthighs*Rwaist*Ltnh*Rshoulder*Lua*Lfahcg);
Tobj = (Rground*Lfeet*Rankle*Llegs*Rknee*Lthighs*Rwaist*Ltnh*Rshoulder*Lua*Lfah);
%Define vectors from the origin (where we will take the moments) to the CGs and the object.
rfeetcg = [Tfeetcg(1, 4), Tfeetcg(2, 4), 0];
rlegscg = [Tlegscg(1, 4), Tlegscg(2, 4), 0];
rthighscg = [Tthighscg(1, 4), Tthighscg(2, 4), 0];
rtnhcg = [Ttnhcg(1, 4), Ttnhcg(2, 4), 0];
ruacg = [Tuacg(1, 4), Tuacg(2, 4), 0];
rfahcg = [Tfahcg(1, 4), Tfahcg(2, 4), 0];
robj = [Tobj(1, 4), Tobj(2, 4), 0];
%Define unit vectors
Lerectorspinaeproximal = [1 0 0 0; 0 1 0 0.05*ltnh; 0 0 1 0; 0 0 0 1];
Lerectorspinaedistal = [1 0 0 0.8*ltnh; 0 1 0 0; 0 0 1 0; 0 0 0 1];
Terectorspinaeproximal = (Rground*Lfeet*Rankle*Llegs*Rknee*Lthighs*Rwaist*Lerectorspinaeproximal);
Terectorspinaedistal = (Rground*Lfeet*Rankle*Llegs*Rknee*Lthighs*Rwaist*Lerectorspinaedistal);
%now move to the spine
%Swapped definition of proximal and distal as we move away from the hips
Fmdir = [Terectorspinaedistal(1,4)-Terectorspinaeproximal(1,4) Terectorspinaedistal(2,4)- Terectorspinaeproximal(2,4) 0];
Fmdir = Fmdir/norm(Fmdir);
rFm = [Terectorspinaeproximal(1,4) Terectorspinaeproximal(2,4) 0];
rthighs = [Tthighscg(1,4) Tthighscg(2,4) 0];
msum = (cross(rfeetcg,Wmfeet)+cross(rlegscg,Wmlegs)+cross(rthighscg,Wmthighs)+cross(rFN,FN)); msum = msum(3);
RHS = [0; mfeet*g+mlegs*g+mthighs*g-(M+mobj)*g; -msum];
Fmmom = cross(rFm, Fmdir);
LHS = [1 0 Fmdir(1); 0 1 Fmdir(2); -rthighs(2) rthighs(1) Fmmom(3)];
soln = inv(LHS)*RHS;
Fjx = soln([1]);
Fjy = soln([2]);
Fm = soln([3]);