7_dofPositionanalysis

%---------------- POSITION ANALYSIS ----------------------------

%------- 1. MOBILE PLATFORM ----------------
 

syms s Nl Nr theta real
l=1; step=s;
x0=0; y0=0; %%% starting from global origin
R=(l/2)*(Nl+Nr)/(Nr-Nl);
omega_delt_t=(Nr-Nl)*step/l;
ICRx=x0-R*sin(theta);
ICRy=y0+R*cos(theta);

Rotz=[cos(omega_delt_t) -sin(omega_delt_t) 0;
 sin(omega_delt_t) cos(omega_delt_t) 0;
 0 0 1];
transform_pre=Rotz*[x0-ICRx; y0-ICRy; 0]+[ICRx; ICRy; omega_delt_t];

H=[Rotz(1,1) Rotz(1,2) Rotz(1,3) transform_pre(1,1);
 Rotz(2,1) Rotz(2,2) Rotz(2,3) transform_pre(2,1);
 Rotz(3,1) Rotz(3,2) Rotz(3,3) transform_pre(3,1);
 0 0 0 1 ];
 
h=simplify(H);

% % % The Mobile platform can be considered as a manipulator having two virtual joints ; first is having a revolute joint at the centere of the axle of driving wheel's and another a prismatic joint , on top of the the revolute joint , having a pure translation along the platform's heading direction measure from the center of the axel of driving wheel.

% % % The above two virtual joint can be seperated as a pure rotation transformation matrix and a pure translation transformation matrix given as Trot and Ttrns.

 T_rot=[h(1,1) h(1,2) h(1,3) 0;
 h(2,1) h(2,2) h(2,3) 0;
 h(3,1) h(3,2) h(3,3) 0;
 0 0 0 1];
 T_trns=[1 0 0 h(1,4);
         0 1 0 h(2,4);
         0 0 1 h(3,4);
         0 0 0 1 ];
         
%%%% Where h=T_rot*T_trns
 H12=T_rot*T_trns;
 H12_symbolic=matlabFunction(H12);
 
% % % Jacobian for the moving platform having 2 virtual joints j1(revolute) and j2(prismatic);
 J1=[cross(T_rot(1:3,1:3)*[0; 0; 1],T_trns(1:3,4));
 T_rot(1:3,1:3)*[0; 0; 1]];
 J2=[T_rot(1:3,1:3)*[0; 0; 1];
 0; 0; 0];
 J_platform=[J1 J2];
J_p_symb=matlabFunction(J_platform);


%------- 1. MANIPULATOR ARM ----------------


syms theta1 theta2 theta3 theta4 theta5 t a d alpha real

%%% Manipulator arm parameters 
link1=1;link2=1;link3=1;link4=1;link5=1;

% syms alpha1 alpha2 alpha3 alpha4 alpha5
alpha1=pi/2; alpha2=0;%-pi/2;
alpha3=-pi/2; alpha4=pi/2; alpha5=0;

% syms a1 a2 a3 a4 a5 real
% syms d1 d2 d3 d4 d5 real
a1=0;a2=0.1;a3=0;a4=0;a5=-0.5;
d1=0.1;d2=0;d3=0;d4=0.2;d5=0;
H_g=[cos(t) -sin(t)*cos(alpha) sin(t)*sin(alpha) a*cos(t);
     sin(t) cos(t)*cos(alpha) -cos(t)*sin(alpha) a*sin(t);
     0 sin(alpha) cos(alpha) d;
     0 0 0 1];
     
H=matlabFunction(H_g); %%% func(a,alpha,d,theta)
H_m_1=H(a1,alpha1,d1,theta1); %%% w r t mobile platform
H_1_2=H(a2,alpha2,d2,theta2);
H_2_3=H(a3,alpha3,d3,theta3);
H_3_4=H(a4,alpha4,d4,theta4+pi);
H_4_5=H(a5,alpha5,d5,theta5);

% % % % Transformation matrix of 5 DOF manipulator w.r.t mobile robot's frame
% % % % Forward kinematics for the manipulator
Scroll to Top