/* A real iterative program in CH for RCCC mechanism created by H. H. Cheng and S. Thompson The algorithm is described in Uicker Jr., J. J., Denavit, J., Hartenberg, R. S. "An Iterative Method for the Displacement Analysis of Spatial Mechanisms", Journal of Applied Mechanics, June 1964, pp. 309-314. */ #include /* PI is defined in */ #define D2R PI/180 #define R2D 180/PI main(){ int i, j, k, n=6, m=9, theta1; float alpha_a[8], theta_s[8], epsilon = 0.00001 ,MaxError= 1000000, error; array float v[m],BB[m][6],d[6],Bi[4][4],B1[4][4],Ai[4][4],Q[4][4]; void a_i(float alpha,a,theta,s;array float A[:][:]) { /*(5)*/ A[0][0] = 1; A[0][1] = 0; A[0][2] = 0; A[0][3] = 0; A[1][0] = a*cos(theta) A[1][1] = cos(theta); A[1][2] = -sin(theta)*cos(alpha); A[1][3] = sin(theta)*sin(alpha) A[2][0] = a*sin(theta) A[2][1] = sin(theta); A[2][2] = cos(theta)*cos(alpha); A[2][3] = -cos(theta)*sin(alpha); A[3][0] = s; A[3][1] = 0;A[3][2] = sin(alpha); A[3][3] = cos(alpha); } alpha_a[0]=30*D2R; alpha_a[1]=2; alpha_a[2]=(55*D2R); alpha_a[3]=4; alpha_a[4]=45*D2R; alpha_a[5]=3; alpha_a[6]=(60*D2R); alpha_a[7]=5; /* link parameters */ theta_s[0]=0; theta_s[1]=0; theta_s[2]=(100*D2R); theta_s[3]=0; theta_s[4]=100*D2R; theta_s[5]=0; theta_s[6]=(100*D2R); theta_s[7]=0; /* initial values */ printf(" theta1 s1 theta2 s2 " "theta3 s3 theta4 s4\n"); /* print heading */ for(theta1=0;theta1<=360;theta1+=90) { /* 90 deg step for theta1 */ theta_s[0] = theta1*D2R; /* change deg to rad */ do{ for (k=0,i=2;i<=(n+1);i++;) { Q=0; if (i%2==0){ /* Q used for rotational displacements */ /*(13)*/ Q[1][2] = -1; Q[2][1] = +1; k++; } else Q[3][0] = 1; /* Q used for translational displacements */ /*(46)*/ for (Bi=0,j=0;j<=3;j++) Bi[j][j] = 1; /* initialize to I matrix */ for (j=0;j<=k*2-2;j+=2,Bi*=Ai) a_i(alpha_a[j],alpha_a[j+1],theta_s[j],theta_s[j+1],Ai); /*(18a)&(48)*/ for (Bi*=Q,j=k*2;j<=n;j+=2,Bi*=Ai) a_i(alpha_a[j],alpha_a[j+1],theta_s[j],theta_s[j+1],Ai); /*(18a)&(48)*/ BB[0][i-2] = Bi[1][0]; BB[1][i-2] = Bi[2][0]; BB[2][i-2] = Bi[3][0]; /*(39)*/ BB[3][i-2] = Bi[1][1]; BB[4][i-2] = Bi[2][1]; BB[5][i-2] = Bi[3][1]; /*(39)*/ BB[6][i-2] = Bi[2][2]; BB[7][i-2] = Bi[3][2]; BB[8][i-2] = Bi[3][3]; /*(40)*/ } a_i(alpha_a[0],alpha_a[1],theta_s[0],theta_s[1],B1) /*(17a)*/ for(i=2;i<=n;i+=2;B1*=Ai) a_i(alpha_a[i],alpha_a[i+1],theta_s[i],theta_s[i+1],Ai); /*(17a)*/ v[0]= -B1[1][0]; v[1]=-B1[2][0]; v[2]= -B1[3][0]; /*(39)*/ v[3]=1-B1[1][1]; v[4]=-B1[2][1]; v[5]= -B1[3][1]; /*(39)*/ v[6]=1-B1[2][2]; v[7]=-B1[3][2]; v[8]=1-B1[3][3]; /*(40)*/ d = linearsolver(transpose(BB)*BB, transpose(BB)*v); /*(41)*/ for (error=0,i=0;i<=n-1;i++) error += abs(d[i]); /*(52)*/ for (i=2;i<=n+1;i++) theta_s[i] += d[i-2]; /* update Theta and S */ /*(7) */ }while ( error >= epsilon && error < MaxError); if(error >= MaxError) /* diverged */ printf("Error: the iterative algorithm diverges at theta1 = %d \n", theta1); else /* converged, print results */ printf("%8.2f %8.3f %8.3f %8.3f %8.3f %8.3f %8.3f %8.3f\n", theta_s[0]*R2D,theta_s[1],theta_s[2]*R2D,theta_s[3],theta_s[4]*R2D, theta_s[5],theta_s[6]*R2D,theta_s[7]); } } /**** The output from the above program theta1 s1 theta2 s2 theta3 s3 theta4 s4 0.00 0.000 149.679 -0.210 45.556 -2.693 144.209 -0.115 90.00 0.000 54.512 -3.171 92.715 -1.513 81.114 -2.114 180.00 0.000 -59.094 -0.301 142.648 -1.814 83.700 -0.173 270.00 0.000 -157.692 1.136 92.714 -1.513 148.494 -0.515 360.00 0.000 -210.320 -0.210 45.556 -2.693 144.209 -0.115 ****/