%Objective is to find angular variation and curvature of manipulator bending section wrt forces

applied (on account of pneumatic pressure ) in actuators ( force vector being tau (section 1) ,
tau2(section2) in the main file ) it is to be taken as input variable. The output is obtained as plots
between linear and angular variations v/s time.

% Further Independent tranformation is done from configuration space to work space.

% Origin is made fixed at ( 90,0.07,89.94 )
% After transformation tip parameters are obtained for bending w.r.t y axis

q0=[0.0338;0.0338;0.0338; 0; 0; 0; 0; 0; 0; 0; 0; 0];
t=linspace(0,1,100);
prompt = 'Enter value of pressure in kpa in first tube of first section ';
p1=input(prompt);
prompt = 'Enter value of pressure in kpa in second tube of first section ';
p2=input(prompt);
pr=[p1;p2];
atmpr=101.325; % atmospheric pressure
for1=(1/9)*((pi/4)*(0.05005^2))*((pr(1)-atmpr)*1000); % effective force in first bellow of first
section w.r.t pressure in kpa
for2=(1/9)*((pi/4)*(0.05005^2))*((pr(2)-atmpr)*1000); % effective force in second bellow of
first section w.r.t pressure in kpa
tau=[for1 for2 for1 for2 for1 for2]'; % forces vector

x='now pass tau matrix in function robotino_appl file';

display(x);
[t,q]= ode45(@robotino_appl,t,q0); % using ode45 solver for integrating second derivatives as
passed through function robtino_appl through f

plot(t, q(:,1:3)); %plot between change in curvature for each module resp. wrt time
plot(t, q(:,4:6)); %plot between change in angles for each module resp.wrt time
length=q(100,1)+q(100,2)+q(100,3); % summation of curvatures of three modules of section
angle=(q(100,4))+(q(100,5))+q(100,6); % summation of angular variation of three modules

z0=[q(100,1);q(100,2);q(100,3);q(100,4);q(100,5);q(100,6);q(100,7);q(100,8);q(100,9);q(100,10
);q(100,11);q(100,12)];
x2='now pass first six values Z0 matrix values in function robotino_appl2 for initial values of
S0';
display(x2);
t1=linspace(0,1,100);
prompt = 'Enter value of pressure in kpa in first tube of the second section ';
p12=input(prompt);
prompt = 'Enter value of pressure in kpa in second tube of second section ';
p22=input(prompt);
pr2=[p12;p22];
for1=(1/9)*((pi/4)*(0.05005^2))*((pr2(1)-atmpr)*1000); % effective force in first bellow of
second section w.r.t pressure in kpa -101.325
for2=(1/9)*((pi/4)*(0.05005^2))*((pr2(2)-atmpr)*1000); % effective force in second bellow of
second section w.r.t pressure in kpa
tau2=[for1 for2 for1 for2 for1 for2]'; % forces vector
x2='now pass tau matrix in function robotino_appl2 file';
display(x2);
% final values of q inserted in z0 i.e computation of final vlaues of
% angles and curvatures for second section's modules of robotino arm
[t1,z]= ode45(@robotino_appl2,t1,z0); % using ode45 solver for integrating second derivatives
as passed through function robtino_test through g
%plot(t1, z(:,1:3)); %plot between change in curvature for each module resp. wrt time
plot(t1, z(:,4:6)); %plot between change in angles for each module resp. wrt time
length2=z(100,1)+z(100,2)+z(100,3); % summation of curvatures of second section
angle2=z(100,4)+z(100,5)+z(100,6); % summation of angular variation of three modulesof
second section
%independent transformation from arc parameters to position orientation
%base is at origin (manipulator inclined at 48 degrees wrt base)
theta0=(pi*(90-48)/180); % initial inclination
trans1=[cos(theta0),0,sin(theta0),(((2.7/100)/theta0)*(1-cos(theta0))); % first tranformation
matrix
0,1,0,0;
-sin(theta0),0,cos(theta0),(((2.7/100)/theta0)*(sin(theta0)));
0,0,0,1];
origin=[90,0.07,89.94,1]';
orient1=trans1*origin; %coordinates after first transformation
trans2=[cos(angle),0,sin(angle),(length/angle)*(1-cos(angle)); % second tranformation
matrix
0,1,0,0;
-sin(angle),0,cos(angle),(length/angle)*(sin(angle));
0,0,0,1];
orient2=trans2*orient1; %coordinates after second transformation
trans3=[cos(angle2),0,sin(angle2),(length2/angle2)*(1-cos(angle2)); %third tranformation
matrix
0,1,0,0;
-sin(angle2),0,cos(angle2),(length2/angle2)*(sin(angle2));
0,0,0,1];
orient3=trans3*orient2; %coordinates after third transformation
trans4=[cos(0),0,sin(0),(0.28+3.5+0.2+0.2+2.67)*(1-cos(angle2)); %fourth tranformation matrix
0,1,0,0;
-sin(0),0,cos(0),(0.28+3.5+0.2+0.2+2.67)*(sin(angle2));
0,0,0,1];
orient4=trans4*orient3; %coordinates after fourth transformation
disp(orient4);
disp(angle2); %total angular deflections
function f= robotino_appl(t,q)
% q[1,2,3]= curvatures of three modules of section 1
% q[4,5,6]= angular variation of arm
% q[7,8,9]= variation of curvatures w.r.t time
% q[10,11,12]= angular variation w.r.t time
% f[7,8,9]= second derivative of curvatures w.r.t time
% f[10,11,12]= second derivative of angular variation w.r.t time
% generalized coordinates
%q= [ q(1);q(2);q(3);q(4);q(5);q(6);q(7);q(8);q(9); q(10);q(11);q(12)];
% the equation to be solved is fcoeff*tau=Dmat(q)*(dq^2/dt^2)+cmat(q)*(dq/dt)-gmat
so=[0.0338;0.0338;0.0338; 0 ;0; 0]; % initial curvature and angles of section's three lumped
elements
dtip=0.0464;dbase=0.0537;d1=0.0488;d2=.0514; % tip dia, base dia , dia at lump1 end , dia at
lump2 end resp in meters
seclength= 0.1014; % section length in meters
k1=57.9489; % spring coefficient of modules
k2=57.9489;
mperl=1.643; % mass per unit length of actuator bellow in kg/m
mfull=mperl*seclength;
m1=mfull/3;
tau=[11.5789466;7.5567144;11.5789466;7.5567144;11.5789466;7.5567144];
k=[k1 k2 k1 k2 k1 k2]'; % stiffness coefficient of bellows
l1=d1*2; l2=d2*2;l3=dtip*2;
m= [m1 m1 m1 0 0 0 ]'; %first three column values represents the m1,m2,m3 (masses)
repsepctively for 3 lumped modules
i1=(m(1)*(l1^2))/12;
i2=(m(2)*(l2^2))/12;
i3=(m(3)*(l3^2))/12;
i= [i1 i2 i3 0 0 0 ]'; %first three column values represents the i1,i2,i3 (intertias)
repsepctively for 3 lumped modules
g=9.8; %gravitational constant
c1=2*((k(1)*m(1))^0.5);
c2=2*((k(2)*m(1))^0.5);
c3=2*((k(3)*m(2))^0.5);
c4=2*((k(4)*m(2))^0.5);
c5=2*((k(5)*m(3))^0.5);
c6=2*((k(6)*m(3))^0.5);
c=[c1 c2 c3 c4 c5 c6]'; %damping coefficients of bellows
fcoeff = [1 1 cos(q(4)) cos(q(4)) cos(q(4)+q(5)) cos(q(4)+q(5));
0 0 1 1 cos(q(5)) cos(q(5));
0 0 0 0 1 1;
0.5 -0.5 0.5 -0.5 0.5+q(2)*sin(q(5)) -0.5+q(2)*sin(q(5));
0 0 0.5 -0.5 0.5 -0.5;
0 0 0 0 0.5 -0.5];

D1 = [m(1)+m(2)+m(3), m(2)*cos(q(4))+ m(3)*cos(q(4)), m(3)*cos(q(4)+q(5)), -

m(2)*q(2)*sin(q(4))-m(3)*q(2)*sin(q(4))-m(3)*q(3)*sin(q(4)+q(5)),-m(3)*q(3)*sin(q(4)+q(5)),
0];
D2 = [ m(2)*cos(q(4))+ m(3)*cos(q(4)), m(2)+m(3), m(3)*cos(q(5)), -m(3)*q(3)*sin(q(5)), -
m(3)*q(3)*sin(q(5)), 0];
D3= [ m(3)*cos(q(4)+q(5)), m(3)*cos(q(5)), m(3), m(3)*q(2)*sin(q(5)), 0, 0 ];
D4= [ -m(2)*q(2)*sin(q(4))-m(3)*q(2)*sin(q(4))-m(3)*q(3)*sin(q(4)+q(5)), -
m(3)*q(3)*sin(q(5)),m(3)*q(2)*sin(q(5)),
m(2)*(q(2)^2)+i(1)+i(2)+i(3)+m(3)*(q(2)^2)+m(3)*(q(3)^2)+2*m(3)*q(3)*cos(q(5))*q(2),
i(2)+m(3)*(q(3)^2)+i(3)+m(3)*q(3)*cos(q(5))*q(2), i(3)];
D5= [ -m(3)*q(3)*sin(q(4)+q(5)), -m(3)*q(3)*sin(q(5)), 0,
i(2)+m(3)*(q(3)^2)+i(3)+m(3)*q(3)*cos(q(5))*q(2), i(2)+m(3)*(q(3)^2)+i(3), i(3)];
D6=[0, 0, 0, i(3), i(3), i(3)];
Dmat=[D1;D2;D3;D4;D5;D6];
c12=[c(1)+c(2), -2*m(2)*sin(q(4))*q(10)-2*m(3)*sin(q(4))*q(10), -
2*m(3)*sin(q(4)+q(5))*(q(10)+q(11)),...
-m(2)*q(2)*cos(q(4))*q(10)+(.5)*(c(1)-c(2))-m(3)*q(2)*cos(q(4))*q(10)-
m(3)*q(3)*cos(q(4)+q(5))*q(10),...
-2*m(3)*q(3)*cos(q(4)+q(5))*q(10)-m(3)*q(3)*cos(q(4)+q(5))*q(11), 0];
c22=[ 0 , c(3)+c(4), -2*m(3)*sin(q(5))*(q(10)+q(11)),...
-m(2)*q(2)*q(10)+(.5)*(c(3)-c(4))-m(3)*q(2)*q(10)-m(3)*q(3)*cos(q(5))*q(10),...
-2*m(3)*q(3)*cos(q(5))*q(10)-m(3)*q(3)*cos(q(5))*q(11), 0];
c32=[ 0 , 2*m(3)*sin(q(5))*q(10), c(5)+c(6), m(3)*q(2)*cos(q(5))*q(10)-m(3)*q(3)*q(10),...
-2*m(3)*q(3)*q(10)-m(3)*q(3)*q(11), 0.5*(c(5)-c(6))];
c42=[ 0.5*(c(1)-c(2)), 2*m(3)*q(3)*cos(q(5))*q(10)-2*m(3)*q(2)*q(10)+2*m(2)*q(2)*q(10),...
2*m(3)*q(3)*(q(10)+q(11))-2*m(3)*q(2)*cos(q(5))*(q(10)+q(11)),...
2*m(3)*q(3)*q(2)*sin(q(5))*(q(11))+((l1^2)/4)*(c(1)+c(2)),
m(3)*q(3)*q(2)*sin(q(5))*(q(11)), 0];
c52=[ 0 , 0.5*(c(3)-c(4))+2*m(3)*q(3)*cos(q(5))*q(10), 2*m(3)*q(3)*(q(10)+q(11)),...
m(3)*q(3)*q(2)*sin(q(5))*(q(10)),((l2^2)/4)*(c(3)+c(4)), 0];
c62=[ 0 ,0 ,(1/2)*(c(5)-c(6)), 0 , 0 , ((l3^2)/4)*(c(5)+c(6))];
cmat=[c12;c22;c32;c42;c52;c62];

gmat=[-m(1)*g-m(2)*g+k(1)*(q(1)+(.5*q(1))-so(1))+k(2)*(q(1)-(.5*q(4))-so(1))-m(3)*g;
-m(2)*g*cos(q(4))+k(3)*(q(2)+(.5*q(5))-so(2))+k(4)*(q(2)-(.5*q(5))-so(2))-
m(3)*g*cos(q(4));
-m(3)*g*cos(q(4)+q(5))+k(5)*(q(3)+(.5*q(6))-so(3))+k(6)*(q(3)-(.5*q(6))-so(3));
m(2)*q(2)*g*sin(q(4))+m(3)*q(3)*g*sin(q(4)+q(5))+
m(3)*q(2)*g*sin(q(4))+k(1)*(q(1)+(.5*q(4))-so(1))*(.5)+k(2)*(q(1)-(.5*q(4))-so(1))*(-.5);
m(3)*q(3)*g*sin(q(4)+q(5))+k(3)*(q(2)+(.5*q(5))-so(2))*(.5)+k(4)*(q(2)-(.5*q(5))-so(1))*(-
.5);
k(5)*(q(3)+(.5*q(6))-so(3))*(.5)+k(6)*(q(3)-(.5*q(6))-so(3))*(-.5)];
 f(1,1)= q(7);
f(2,1)= q(8);
f(3,1)= q(9);
f(4,1)= q(10);
f(5,1)= q(11);
f(6,1)= q(12);

sol1=fcoeff*tau;
sol2=cmat*q(1:6,1);
mul=sol1-sol2-gmat;
mul2=inv(Dmat);
f(7:12,1)=mul2*(mul);

end
function g= robotino_appl2(t1,z) % function for section 2
% z[1,2,3]= curvatures of three modules of section 2
% z[4,5,6]= angular variation of arm
% z[7,8,9]= variation of curvatures w.r.t time
% z[10,11,12]= angular variation w.r.t time
% g[7,8,9]= second derivative of curvatures w.r.t time
% g[10,11,12]= second derivative of angular variation w.r.t time
% generalized coordinates
%z= [ z(1);z(2);z(3);z(4);z(5);z(6);z(7);z(8);z(9); z(10);z(11);z(12)];
so2=[0.169936002 0.263255588 0.104940744 -0.278467027 -0.122900614 -0.083456358]';
%initial curvature and angles of 2nd section's lumped elements
dtip2=0.0391;dbase2=0.0464;d12=0.04153;d22=.043965; % tip dia , base dia , dia at lump1 end
, dia at lump2 end resp in meters
seclength2= 0.1046; % section length in meters
% spring coefficient of modules
k12=57.9489;
k22=57.9489;
mperl2=1.643; % mass per unit length of actuator bellow in kg/m
mfull2=mperl2*seclength2;
m12=mfull2/3;
k2=[k12 k22 k12 k22 k12 k22]'; % stiffness coefficient of bellows
l12=d12*2; l22=d22*2;l32=dtip2*2;
m2= [m12 m12 m12 0 0 0 ]'; %first three column values represents the m1,m2,m3
(masses) repsepctively for 3 lumped modules
i12=(m2(1)*(l12^2))/12;
i22=(m2(2)*(l22^2))/12;
i32=(m2(3)*(l32^2))/12;
i2= [i12 i22 i32 0 0 0 ]'; %first three column values represents the i12,i22,i32 (intertias)
repsepctively for 3 lumped modules
g=9.8; %gravitational constant
tau2=[9.598944;6.798794;9.598944;6.798794;9.598944;6.798794];
c12=2*((k2(1)*m2(1))^0.5);
c22=2*((k2(2)*m2(1))^0.5);
c32=2*((k2(3)*m2(2))^0.5);
c42=2*((k2(4)*m2(2))^0.5);
c52=2*((k2(5)*m2(3))^0.5);
c62=2*((k2(6)*m2(3))^0.5);
c2=[c12 c22 c32 c42 c52 c62]'; %damping coefficients of bellows
fcoeff2 = [1 1 cos(z(4)) cos(z(4)) cos(z(4)+z(5)) cos(z(4)+z(5));
0 0 1 1 cos(z(5)) cos(z(5));
0 0 0 0 1 1;
0.5 -0.5 0.5 -0.5 0.5+z(2)*sin(z(5)) -0.5+z(2)*sin(z(5));
0 0 0.5 -0.5 0.5 -0.5;
0 0 0 0 0.5 -0.5];

D12 = [m2(1)+m2(2)+m2(3), m2(2)*cos(z(4))+ m2(3)*cos(z(4)), m2(3)*cos(z(4)+z(5)), -

m2(2)*z(2)*sin(z(4))-m2(3)*z(2)*sin(z(4))-m2(3)*z(3)*sin(z(4)+z(5)),-
m2(3)*z(3)*sin(z(4)+z(5)), 0];
D22 = [ m2(2)*cos(z(4))+ m2(3)*cos(z(4)), m2(2)+m2(3), m2(3)*cos(z(5)), -
m2(3)*z(3)*sin(z(5)), -m2(3)*z(3)*sin(z(5)), 0];
D32= [ m2(3)*cos(z(4)+z(5)), m2(3)*cos(z(5)), m2(3), m2(3)*z(2)*sin(z(5)), 0, 0 ];
D42= [ -m2(2)*z(2)*sin(z(4))-m2(3)*z(2)*sin(z(4))-m2(3)*z(3)*sin(z(4)+z(5)), -
m2(3)*z(3)*sin(z(5)),m2(3)*z(2)*sin(z(5)),
m2(2)*(z(2)^2)+i2(1)+i2(2)+i2(3)+m2(3)*(z(2)^2)+m2(3)*(z(3)^2)+2*m2(3)*z(3)*cos(z(5))*z(
2), i2(2)+m2(3)*(z(3)^2)+i2(3)+m2(3)*z(3)*cos(z(5))*z(2), i2(3)];
D52= [ -m2(3)*z(3)*sin(z(4)+z(5)), -m2(3)*z(3)*sin(z(5)), 0,
i2(2)+m2(3)*(z(3)^2)+i2(3)+m2(3)*z(3)*cos(z(5))*z(2), i2(2)+m2(3)*(z(3)^2)+i2(3), i2(3)];
D62=[0, 0, 0, i2(3), i2(3), i2(3)];
Dmat2=[D12;D22;D32;D42;D52;D62];
c122=[c2(1)+c2(2), -2*m2(2)*sin(z(4))*z(10)-2*m2(3)*sin(z(4))*z(10), -
2*m2(3)*sin(z(4)+z(5))*(z(10)+z(11)),...
-m2(2)*z(2)*cos(z(4))*z(10)+(.5)*(c2(1)-c2(2))-m2(3)*z(2)*cos(z(4))*z(10)-
m2(3)*z(3)*cos(z(4)+z(5))*z(10),...
-2*m2(3)*z(3)*cos(z(4)+z(5))*z(10)-m2(3)*z(3)*cos(z(4)+z(5))*z(11), 0];
c222=[ 0 , c2(3)+c2(4), -2*m2(3)*sin(z(5))*(z(10)+z(11)),...
-m2(2)*z(2)*z(10)+(.5)*(c2(3)-c2(4))-m2(3)*z(2)*z(10)-m2(3)*z(3)*cos(z(5))*z(10),...
-2*m2(3)*z(3)*cos(z(5))*z(10)-m2(3)*z(3)*cos(z(5))*z(11), 0];
c322=[ 0 , 2*m2(3)*sin(z(5))*z(10), c2(5)+c2(6), m2(3)*z(2)*cos(z(5))*z(10)-
m2(3)*z(3)*z(10),...
-2*m2(3)*z(3)*z(10)-m2(3)*z(3)*z(11), 0.5*(c2(5)-c2(6))];
c422=[ 0.5*(c2(1)-c2(2)), 2*m2(3)*z(3)*cos(z(5))*z(10)-
2*m2(3)*z(2)*z(10)+2*m2(2)*z(2)*z(10),...
2*m2(3)*z(3)*(z(10)+z(11))-2*m2(3)*z(2)*cos(z(5))*(z(10)+z(11)),...
2*m2(3)*z(3)*z(2)*sin(z(5))*(z(11))+((l12^2)/4)*(c2(1)+c2(2)),
m2(3)*z(3)*z(2)*sin(z(5))*(z(11)), 0];
c522=[ 0 , 0.5*(c2(3)-c2(4))+2*m2(3)*z(3)*cos(z(5))*z(10), 2*m2(3)*z(3)*(z(10)+z(11)),...
m2(3)*z(3)*z(2)*sin(z(5))*(z(10)),((l22^2)/4)*(c2(3)+c2(4)), 0];
c622=[ 0 ,0 ,(1/2)*(c2(5)-c2(6)), 0 , 0 , ((l32^2)/4)*(c2(5)+c2(6))];
cmat2=[c122;c222;c322;c422;c522;c622];

gmat2=[-m2(1)*g-m2(2)*g+k2(1)*(z(1)+(.5*z(1))-so2(1))+k2(2)*(z(1)-(.5*z(4))-so2(1))-
m2(3)*g;
-m2(2)*g*cos(z(4))+k2(3)*(z(2)+(.5*z(5))-so2(2))+k2(4)*(z(2)-(.5*z(5))-so2(2))-
m2(3)*g*cos(z(4));
-m2(3)*g*cos(z(4)+z(5))+k2(5)*(z(3)+(.5*z(6))-so2(3))+k2(6)*(z(3)-(.5*z(6))-so2(3));
m2(2)*z(2)*g*sin(z(4))+m2(3)*z(3)*g*sin(z(4)+z(5))+
m2(3)*z(2)*g*sin(z(4))+k2(1)*(z(1)+(.5*z(4))-so2(1))*(.5)+k2(2)*(z(1)-(.5*z(4))-so2(1))*(-.5);
m2(3)*z(3)*g*sin(z(4)+z(5))+k2(3)*(z(2)+(.5*z(5))-so2(2))*(.5)+k2(4)*(z(2)-(.5*z(5))-
so2(1))*(-.5);
k2(5)*(z(3)+(.5*z(6))-so2(3))*(.5)+k2(6)*(z(3)-(.5*z(6))-so2(3))*(-.5)];
 g(1,1)= z(7);
g(2,1)= z(8);
g(3,1)= z(9);
g(4,1)= z(10);
g(5,1)= z(11);
g(6,1)= z(12);

sol12=fcoeff2*tau2;
sol22=cmat2*z(1:6,1);
mul12=sol12-sol22-gmat2;
mul22=inv(Dmat2);
g(7:12,1)=mul22*(mul12);

end