from
Task Space control of RPRP manipulator
by Sumit Tripathi
Forward and inverse kinematics of RPRP manipulator with task space control to trace trajectories.
|
| [flag_set]=set_dimensions()
|
%Sumit Tripathi
function [flag_set]=set_dimensions()
global theta1_g d1_g theta2_g d2_g;
global x_ee_g y_ee_g phi_g;
global pF_jr2_g pF_pL1_prism_g pF_pL2_prism_g;
global jr2_g pL1_prism_g pL2_prism_g;
global x1_g y1_g r_g len_g wd_g;
global Link1_g Link2_g joint2_g;
global pF_EE_RPRP_g pEE_RPRP_g EE_g;
while(theta1_g>360)
theta1_g=theta1_g-360;
end
while(theta2_g>360)
theta2_g= theta2_g-360;
end
while(theta1_g<(-360))
theta1_g=theta1_g+360;
end
while(theta2_g<(-360))
theta2_g= theta2_g+360;
end
if((d1_g<=15)&(d1_g>=6)&(d2_g<=15)&(d2_g>=6)&(theta1_g<=360)&(theta1_g>=-360)&(theta2_g<=360)&(theta2_g>=-360))
flag_set=1;
wd=wd_g;
xc=0;yc=0;
plunger1=d1_g-5;
xend_pL1_plunger=xc+ 5 +plunger1;
xep=xend_pL1_plunger;
wdc=wd_g/4;
ypL1_prism=[yc-.5 yc-.5 yc-wd yc-wd yc-wd+.5 yc-wd+.5 yc-wd yc-wd yc-wdc yc-wdc yc+wdc yc+wdc yc+wd yc+wd yc+wd-.5 yc+wd-.5 yc+wd yc+wd yc+.5 yc+.5];
xpL1_prism=[xc xc+.5 xc+.5 xc+1 xc+1 xc+4.5 xc+4.5 xc+5 xc+5 xc+xep xc+xep xc+5 xc+5 xc+4.5 xc+4.5 xc+1 xc+1 xc+.5 xc+.5 xc];
pL1_prism_g=[xpL1_prism' ypL1_prism' zeros(size(xpL1_prism))' ones(size(xpL1_prism))']'; % "rotaion joint" # 1 matrix
plunger2=d2_g-5;
xend_pL2_plunger=xc + 5 +plunger2;
xep=xend_pL2_plunger;
wdc=wd_g/4;
ypL2_prism=[yc-.5 yc-.5 yc-wd yc-wd yc-wd+.5 yc-wd+.5 yc-wd yc-wd yc-wdc yc-wdc yc+wdc yc+wdc yc+wd yc+wd yc+wd-.5 yc+wd-.5 yc+wd yc+wd yc+.5 yc+.5];
xpL2_prism=[xc xc+.5 xc+.5 xc+1 xc+1 xc+4.5 xc+4.5 xc+5 xc+5 xc+xep xc+xep xc+5 xc+5 xc+4.5 xc+4.5 xc+1 xc+1 xc+.5 xc+.5 xc];
pL2_prism_g=[xpL2_prism' ypL2_prism' zeros(size(xpL2_prism))' ones(size(xpL2_prism))']'; % "prismatic joint" # 2 matrix
%Calculations to draw end effector
wd=len_g/2;
yc=0;xc=0;
wdc=wd_g/2;
yEE_RPRP=[yc+wd yc+wd-wdc yc+wd-wdc yc-wd+wdc yc-wd+wdc yc-wd];
xEE_RPRP=[xc xc+len_g xc+len_g/2 xc+len_g/2 xc+len_g xc];
pEE_RPRP_g=[xEE_RPRP' yEE_RPRP' zeros(size(xEE_RPRP))' ones(size(xEE_RPRP))']';
else
flag_set=0;
end
|
|
Contact us at files@mathworks.com