Code covered by the BSD License  

Highlights from
Task Space control of RPRP manipulator

image thumbnail
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