Code_7dofKinematicAnalysis

% ------------------------------------------ KINEMATIC ANALYSIS ----------------------------------------------

% ----------------------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);

%%
% ----------------------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

H_m_5=H_m_1*H_1_2*H_2_3*H_3_4*H_4_5;
H_m_2=H_m_1*H_1_2;
H_m_3=H_m_1*H_1_2*H_2_3;
H_m_4=H_m_1*H_1_2*H_2_3*H_3_4;
our_pos=H_m_5(1:3,4);

Manipulatorpose=matlabFunction(our_pos);
homepose=Manipulatorpose(0.1,0.1,0.1,0.1,0.1)
Scroll to Top