part6

0001 clear;
0002 
0003 % initial conditions for required parameters
0004 
0005 L1 = 3;
0006 L2 = 3 * sqrt(2);
0007 L3 = L1;
0008 
0009 p1 = 5;
0010 p2 = 4;
0011 p3 = 3;
0012 
0013 x1 = 5;
0014 x2 = 0;
0015 y2 = 6;
0016 
0017 gamma = pi/4;
0018 
0019 theta = -pi:0.1:pi;
0020 
0021 vals = zeros(length(theta));
0022 
0023 for i = 1:length(theta)
0024     vals(i) = f2DStewart(theta(i), gamma, L2, L3, p1, p2, p3, x1, x2, y2);
0025 end
0026 
0027 plot(theta, vals);
0028 
0029 interval1 = [1.258 1.358];
0030 interval2 = [1.758 1.858];
0031 
0032 g = @(x) f2DStewart(x, gamma, L2, L3, p1, p2, p3, x1, x2, y2);
0033 
0034 zero1 = bisect(g, interval1(1), interval1(2), 0.5e-8)
0035 zero2 = bisect(g, interval2(1), interval2(2), 0.5e-8)
0036 
0037 [ val1 u1_1 v1_1 ] = f2DStewart(zero1, gamma, L2, L3, p1, p2, p3, x1, x2, y2);
0038 [ val2 u1_2 v1_2 ] = f2DStewart(zero2, gamma, L2, L3, p1, p2, p3, x1, x2, y2);
0039 
0040 plotcnt = 1;
0041 
0042 tol = 0.5E-8;
0043 
0044 % assert that the strut lengths are correct
0045 
0046 theta = zero1; u1 = u1_1; v1 = v1_1;
0047 
0048 A2 = L3 * cos(theta) - x1;
0049 A3 = L2 * ( cos(theta) * cos(gamma) - sin(theta) * sin(gamma) ) - x2;
0050 
0051 B2 = L3 * sin(theta);
0052 B3 = L2 * ( cos(theta) * sin(gamma) + sin(theta) * cos(gamma) ) - y2;
0053 
0054 p1v = sqrt( u1^2 + v1^2 );
0055 p2v = sqrt( (u1 + A2)^2 + (v1 + B2)^2 );
0056 p3v = sqrt( (u1 + A3)^2 + (v1 + B3)^2 );
0057 
0058 assert( p1 - p1v < tol );
0059 assert( p2 - p2v < tol );
0060 assert( p3 - p3v < tol );
0061 
0062 % plot pose 1
0063 
0064 u2 = u1 + L2 * cos( theta + gamma );
0065 v2 = v1 + L2 * sin( theta + gamma );
0066 
0067 u3 = u1 + L3 * cos( theta );
0068 v3 = v1 + L3 * sin( theta );
0069 
0070 figure(plotcnt); plotcnt = plotcnt + 1;
0071 
0072 plot( [u1 u2 u3 u1], [v1 v2 v3 v1], 'r' ); hold on
0073 plot( [ 0 x1 x2], [ 0  0 y2], 'bo' ); hold on
0074 plot( [ 0 u1 ], [ 0 v1 ], 'b-', [ x2 u2 ], [ y2  v2 ], 'b-', [ x1 u3 ], [ 0 v3 ], 'b-' );
0075 
0076 % assert that the strut lengths are correct
0077 
0078 theta = zero2; u1 = u1_2; v1 = v1_2;
0079 
0080 A2 = L3 * cos(theta) - x1;
0081 A3 = L2 * ( cos(theta) * cos(gamma) - sin(theta) * sin(gamma) ) - x2;
0082 
0083 B2 = L3 * sin(theta);
0084 B3 = L2 * ( cos(theta) * sin(gamma) + sin(theta) * cos(gamma) ) - y2;
0085 
0086 p1v = sqrt( u1^2 + v1^2 );
0087 p2v = sqrt( (u1 + A2)^2 + (v1 + B2)^2 );
0088 p3v = sqrt( (u1 + A3)^2 + (v1 + B3)^2 );
0089 
0090 assert( p1 - p1v < tol );
0091 assert( p2 - p2v < tol );
0092 assert( p3 - p3v < tol );
0093 
0094 % plot pose 2
0095 
0096 u2 = u1 + L2 * cos( theta + gamma );
0097 v2 = v1 + L2 * sin( theta + gamma );
0098 
0099 u3 = u1 + L3 * cos( theta );
0100 v3 = v1 + L3 * sin( theta );
0101 
0102 figure(plotcnt); plotcnt = plotcnt + 1;
0103 
0104 plot( [u1 u2 u3 u1], [v1 v2 v3 v1], 'r' ); hold on
0105 plot( [ 0 x1 x2], [ 0  0 y2], 'bo' ); hold on
0106 plot( [ 0 u1 ], [ 0 v1 ], 'b-', [ x2 u2 ], [ y2  v2 ], 'b-', [ x1 u3 ], [ 0 v3 ], 'b-' );