Difference between revisions of "Parameters for Industrial Robot"

% Robot dynamics with 3 DOF, this function should be equal to zero at all
% times. e.g. f(t,x=[q,dq],dx=[dq,ddq])=0! This model is only containing
% the inerta Matrix and the torques of the joints. Friction, gravity and coriolis
% effects are incorporated.
 
function diff=f_full_effects(~,y,dy,tau)
 
    dq=zeros(3,1);
    dotq=zeros(3,1);
    ddq=zeros(3,1);
 
    q=y(1:3);
    dq(1:3,1)=y(4:6);
    dotq(1:3,1)=dy(1:3);
    ddq(1:3,1)=dy(4:6);
 
    % intermediate states
 
    cosq=zeros(3,1);
 
    cosq(1,1) =   cos(q(1));
    cosq(2,1) =   cos(q(2));
    cosq(3,1) =   cos(q(3));
 
    sinq=zeros(3,1);
 
    sinq(1,1) =   sin(q(1));
    sinq(2,1) =   sin(q(2));
    sinq(3,1) =   sin(q(3));
 
    cosq2q3 =   cos(q(2) - q(3));
 
    sinq1q2=   sin(q(1) - q(2));
    sinq2q3=   sin(q(2) - q(3));
 
    sin2q2=     sin(2*q(2));
 
    sin2q22q3=  sin(2*q(2) - 2*q(3));
    sin2q2q3=   sin(2*q(2) - q(3));
 
    cos2q2=     cos(2*q(2));
 
    % Modell parameters
 
    b_1 =  2.6491674286195;
    b_2 =  0.0168799610481578;
    b_3 =  1.43009916890323;
    b_4 =  0.53524835732352;
    b_5 =  0.791116620901176;
    b_6 =  0.626240578068518;
    b_7 =  1.57199673626875;
    b_8 =  0.0175195488386143;
    b_9 =  0.0204978721411787;
    b_10=  1.07049671464704;
    b_11=  1.81771647041588;
    b_12=  0.422954084243519;
 
    b=zeros(3,3);       % Inerta Matrix of Robot Modell (3DOF)
 
    b(1,1) =  b_1*(sinq(2)^2) - b_2*sin2q2 + b_3*(cosq(2)^2) + b_4*cosq(3) + b_5*(cosq2q3^2) - b_6*cos(2*q(2) - 2*q(3)) - b_4*cos(2*q(2) - q(3)) - b_7;
    b(2,1) =  b_8*cosq(2) + b_9*cosq2q3;
    b(3,1) = -b_9*cosq2q3;
    b(1,2) =  b_8*cosq(2) + b_9*cosq2q3;
    b(2,2) =  b_10*cosq(3) + b_11;
    b(3,2) = -b_4*cosq(3) - b_12;
    b(1,3) = -b_9*cosq2q3;
    b(2,3) = -b_4*cosq(3) - b_12;
    b(3,3) =  b_12;
 
    g=zeros(3,1);       % gravity torque vector
 
    g_1 =   32.4854155934467;
    g_2 =   13.1269659633593;
    g_3 =   0.41398104470607;
 
    g(1) = 0;
    g(2) =-g_1*sinq(2) - g_2*sinq2q3 + g_3*cosq(2);
    g(3) = g_2*sinq2q3;
 
    f=zeros(3,1);       % friction torque vector
 
    f(1) = 0.186461043875677*atan(100*dq(1));
    f(2) = 0.184489618216264*atan(100*dq(2));
    f(3) = 0.156066608371045*atan(100*dq(3));
 
    c=zeros(3,3);   % coriolis matrix
 
    c_1 =   0.609534129858137;
    c_2 =   0.23068226761793;
    c_3 =   b_4;
    c_4 =   b_2;
    c_5 =   0.26762417866176;
    c_6 =   152.383532464534;
    c_7 =   57.6705669044826;
    c_8 =   133.81208933088;
    c_9 =   4.21999026203945;
    c_10=   66.90604466544;
    c_11=   b_8;
    c_12=   b_9;
    c_13=   1.3381208933088;
 
    c(1,1) = c_1*dq(2)*sin2q2 + c_2*dq(2)*sin2q22q3 + c_3*dq(2)*sin2q2q3 - c_4*dq(2)*cos2q2 - c_5*dq(3)*sinq(3) - c_2*dq(3)*sin2q22q3 - c_5*dq(3)*sin2q2q3;
    c(2,1) = -(-1)*dq(1)*(-c_6*sin2q2 - c_7*sin2q22q3 - c_8*sin2q2q3 + c_9*cos2q2)/250;
    c(3,1) = -(-1)*(-1)*dq(1)*(-c_10*sinq(3) - c_7*sin2q22q3 - c_10*sin2q2q3)/250;
    c(1,2) = -(-1)*(-1)*dq(1)*(-c_6*sin2q2 - c_7*sin2q22q3 - c_8*sin2q2q3 + c_9*cos2q2)/250 + dq(2)*(-c_11*sinq(2) - c_12*sinq2q3) + c_12*dq(3)*sinq2q3;
    c(2,2) = -c_3*dq(3)*sinq(3);
    c(3,2) = c_3*dq(2)*sinq(3);
    c(1,3) = -c_5*dq(1)*sinq(3) - c_2*dq(1)*sin2q22q3 - c_5*dq(1)*sin2q2q3 + c_12*dq(2)*sinq2q3 - c_12*dq(3)*sinq2q3;
    c(2,3) = c_13*(-(-1)*(-2)*dq(2)/5 - (-1)*2*dq(3)/5)*sinq(3);
    c(3,3) = 0; 
 
 
    diff=[dq-dotq;...
        b*ddq+g+c*dq+f-tau'];