function F = consnt7(x)
m=1.669e-3;
jx=5.743e-14;
E=2.1e11;
O=12.5e-3;
jz=1.149e-13;
G=8e10;
L=3e-3;
g=9.81;
N=1;
T1=5;
T2=0;
T3=0;
xd = sym('xd', [1 3*N], 'real');
r(:,:,1)=[1*O;0;0;];
r(:,:,2)=[(-1/2)*O;(sqrt(3))*O/2;0];
r(:,:,3)=[(-1/2)*O;-(sqrt(3))*O/2;0];
for j=2:N+1
for i=j-1
pl(:,:,j)=L*[(cos(x(3*i-2))*(1-cos(x(3*i-1))))/x(3*i-1) (sin(x(3*i-2))*(1-cos(x(3*i-1))))/x(3*i-1) sin(x(3*i-1))/x(3*i-1)]';
Rl(:,:,j)=[cos(x(3*i-2))*cos(x(3*i-1))*cos(x(3*i))-sin(x(3*i-2))*sin(x(3*i)) -cos(x(3*i-2))*cos(x(3*i-1))*sin(x(3*i))-sin(x(3*i-2))*cos(x(3*i)) cos(x(3*i-2))*sin(x(3*i-1));sin(x(3*i-2))*cos(x(3*i-1))*cos(x(3*i))+cos(x(3*i-2))*sin(x(3*i)) -sin(x(3*i-2))*cos(x(3*i-1))*sin(x(3*i))+cos(x(3*i-2))*cos(x(3*i)) sin(x(3*i-2))*sin(x(3*i-1));-sin(x(3*i-1))*cos(x(3*i)) sin(x(3*i-1))*sin(x(3*i)) cos(x(3*i-1))];
p(:,:,2)=pl(:,:,2);
end
end
R(:,:,2)=Rl(:,:,2);
R(:,:,1)=[1 0 0;0 1 0;0 0 1];
for i=3:N+1
R(:,:,i)=R(:,:,i-1)*Rl(:,:,i);
p(:,:,i)=p(:,:,i-1)+R(:,:,i-1)*pl(:,:,i);
end
for j=2:N+1
for i=j-1
wl(:,:,j)=[-sin(x(3*i-2))*xd(3*i-1)+cos(x(3*i-2))*sin(x(3*i-1))*xd(3*i);cos(x(3*i-2))*xd(3*i-1)+sin(x(3*i-2))*sin(x(3*i-1))*xd(3*i);xd(3*i-2)+cos(x(3*i-1))*xd(3*i)];
w(:,:,2)=wl(:,:,2);
end
end
for i=3:N+1
w(:,:,i)= w(:,:,i-1)+R(:,:,i-1)*wl(:,:,i);
end
for i=2:N+1
wc(:,:,i)=collect(w(:,:,i),[xd(1),xd(2),xd(3)]);%,xd(4),xd(5),xd(6),xd(7),xd(8),xd(9),xd(10),xd(11),xd(12)]);% ,xd(13),xd(14),xd(15),xd(16),xd(17),xd(18),xd(19),xd(20),xd(21),xd(22),xd(23),xd(24)]);
end
for i=2:N+1
for j=1:size(w(:,:,i),1)
[wtemp,vtemp]=coeffs(wc(j,:,i),[xd(1),xd(2),xd(3)]);%,xd(4),xd(5),xd(6),xd(7),xd(8),xd(9),xd(10),xd(11),xd(12)]);%,xd(13),xd(14),xd(15),xd(16),xd(17),xd(18),xd(19),xd(20),xd(21),xd(22),xd(23),xd(24)]);
[~,idx]=ismember(vtemp,[xd(1),xd(2),xd(3)]);%,xd(4),xd(5),xd(6),xd(7),xd(8),xd(9),xd(10),xd(11),xd(12)]);%,xd(13),xd(14),xd(15),xd(16),xd(17),xd(18),xd(19),xd(20),xd(21),xd(22),xd(23),xd(24)]);
wk(j,idx,i)=wtemp;
end
end
for j=2:N+1
for i=j-1
pd(:,:,j)=L*[(-sin(x(3*i-2))*((1-cos(x(3*i-1)))/x(3*i-1))*xd(3*i-2))+(cos(x(3*i-2))*((sin(x(3*i-1))*x(3*i-1))-(1-cos(x(3*i-1)))/(x(3*i-1)^2))*xd(3*i-1));(cos(x(3*i-2))*((1-cos(x(3*i-1)))/x(3*i-1))*xd(3*i-2))+(sin(x(3*i-2))*((sin(x(3*i-1))*x(3*i-1))-(1-cos(x(3*i-1)))/(x(3*i-1)^2))*xd(3*i-1));(((cos(x(3*i-1))*x(3*i-1))-sin(x(3*i-1)))/(x(3*i-1)^2))*xd(3*i-1)];
v(:,:,2)=pd(:,:,2);
end
end
for i=3:N+1
v(:,:,i)=v(:,:,i-1)+cross(w(:,:,i-1),R(:,:,i-1)*pl(:,:,i))+R(:,:,i-1)*pd(:,:,i);
end
for i=2:N+1
vc(:,:,i)=collect(v(:,:,i),[xd(1),xd(2),xd(3)]);%),xd(4),xd(5),xd(6),xd(7),xd(8),xd(9),xd(10),xd(11),xd(12)]);%,xd(13),xd(14),xd(15),xd(16),xd(17),xd(18),xd(19),xd(20),xd(21),xd(22),xd(23),xd(24)]);
end
for i=2:N+1
for j=1:size(v(:,:,i),1)
[vwtemp,vvtemp]=coeffs(vc(j,:,i),[xd(1),xd(2),xd(3)]);%),xd(4),xd(5),xd(6),xd(7),xd(8),xd(9),xd(10),xd(11),xd(12)]);%,xd(13),xd(14),xd(15),xd(16),xd(17),xd(18),xd(19),xd(20),xd(21),xd(22),xd(23),xd(24)]);
[~,idx]=ismember(vvtemp,[xd(1),xd(2),xd(3)]);%,xd(4),xd(5),xd(6),xd(7),xd(8),xd(9),xd(10),xd(11),xd(12)]);%,xd(13),xd(14),xd(15),xd(16),xd(17),xd(18),xd(19),xd(20),xd(21),xd(22),xd(23),xd(24)]);
vk(j,idx,i)=vwtemp;
end
vk(:,3*(i-1),i)=[0;0;0];
end
for j=N+1
for i=j-1
Mb(:,:,j)=E*jx*((x(3*i-1))/L)*R(:,:,j-1)*[-sin(x(3*i-2));cos(x(3*i-2));0];
Mt(:,:,j)=-G*jz*(x(3*i-2)+x(3*i))*R(:,1,i)/L;
Me(:,:,j)=Mt(:,:,i)-Mb(:,:,i);
end
end
for j=2:N
for i=j-1
Mb(:,:,j)=E*jx*((x(3*i-1))/L)*R(:,:,j-1)*[-sin(x(3*i-2));cos(x(3*i-2));0];
end
end
for j=2:N
for i=j-1
Mt(:,:,j)=-G*jz*(x(3*i-2)+x(3*i))*R(:,3,j)/L;
end
end
for j=2:N
for i=j
Mtt(:,:,j)=G*jz*(x(3*i-2)+x(3*i))*R(:,3,j)/L;
end
end
for j=N+1
Mtt(:,:,j)=[0;0;0];
end
for i=2:N+1
MT(:,:,i)=Mt(:,:,i)+Mtt(:,:,i);
end
for i=2:N
Me(:,:,i)=Mb(:,:,i+1)-Mb(:,:,i)+MT(:,:,i);
end
for i=2:N+1 Fg(:,:,i)=-m*g*[1;0;0];
end
for i=2
ph(:,1,i)=[pl(:,:,i)+R(:,:,i)*r(:,:,1)-r(:,:,1)];
ph(:,2,i)=[pl(:,:,i)+R(:,:,i)*r(:,:,2)-r(:,:,2)];
ph(:,3,i)=[pl(:,:,i)+R(:,:,i)*r(:,:,3)-r(:,:,3)];
end
for i=3:N+1
ph(:,1,i)=[R(:,:,i-1)*pl(:,:,i)+R(:,:,i)*r(:,:,1)-R(:,:,i-1)*r(:,:,1)];
ph(:,2,i)=[R(:,:,i-1)*pl(:,:,i)+R(:,:,i)*r(:,:,2)-R(:,:,i-1)*r(:,:,2)];
ph(:,3,i)=[R(:,:,i-1)*pl(:,:,i)+R(:,:,i)*r(:,:,3)-R(:,:,i-1)*r(:,:,3)];
end
for i=2:N+1
ap(:,:,i)=[sqrt(ph(1,1,i)^2+ph(2,1,i)^2+ph(3,1,i)^2);sqrt(ph(1,2,i)^2+ph(2,2,i)^2+ph(3,2,i)^2);sqrt(ph(1,3,i)^2+ph(2,3,i)^2+ph(3,3,i)^2)];
f(:,:,i)=[ph(:,1,i)/ap(1,1,i) ph(:,2,i)/ap(2,1,i) ph(:,3,i)/ap(3,1,i)];
end
for i=N+1
Fc(:,:,i)=[-T1*f(:,1,i) -T2*f(:,2,i) -T3*f(:,3,i)];
end
for i=2:N
Fc(:,:,i)=[T1*f(:,1,i+1) T2*f(:,2,i+1) T3*f(:,3,i+1)]+[-T1*f(:,1,i)-T2*f(:,2,i) -T3*f(:,3,i)];
end
for i=2:N+1
fa(:,:,i)=Fc(:,1,i)+Fc(:,2,i)+Fc(:,3,i);
end
for i=2:N+1
Ma(:,:,i)=cross(r(:,:,1),Fc(:,1,i))+cross(r(:,:,2),Fc(:,2,i))+cross(r(:,:,3),Fc(:,3,i));
end
for i=2:N+1
feq(:,:,i)=fa(:,:,i) +Fg(:,:,i);
Meq(:,:,i)=Ma(:,:,i)+Me(:,:,i);
end
for j=1:(3*N)
for i=2:N+1
F(j,:,:)= dot(Meq(:,:,i),wk(:,j,i))+dot(feq(:,:,i),vk(:,j,i));
end
end