part4

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 = p1;
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.0 -0.6];
0030 interval2 = [-0.5 0.5];
0031 interval3 = [1.0 1.5];
0032 interval4 = [1.5 2.5];
0033 
0034 g = @(x) f2DStewart(x, gamma, L2, L3, p1, p2, p3, x1, x2, y2);
0035 
0036 zero1 = bisect(g, interval1(1), interval1(2), 0.5e-8)
0037 zero2 = bisect(g, interval2(1), interval2(2), 0.5e-8)
0038 zero3 = bisect(g, interval3(1), interval3(2), 0.5e-8)
0039 zero4 = bisect(g, interval4(1), interval4(2), 0.5e-8)
0040 
0041 [ val1 u1_1 v1_1 ] = f2DStewart(zero1, gamma, L2, L3, p1, p2, p3, x1, x2, y2);
0042 [ val2 u1_2 v1_2 ] = f2DStewart(zero2, gamma, L2, L3, p1, p2, p3, x1, x2, y2);
0043 [ val3 u1_3 v1_3 ] = f2DStewart(zero3, gamma, L2, L3, p1, p2, p3, x1, x2, y2);
0044 [ val4 u1_4 v1_4 ] = f2DStewart(zero4, gamma, L2, L3, p1, p2, p3, x1, x2, y2);
0045 
0046 plotcnt = 1;
0047 
0048 tol = 0.5E-8;
0049 
0050 % assert that the strut lengths are correct
0051 
0052 theta = zero1; u1 = u1_1; v1 = v1_1;
0053 
0054 A2 = L3 * cos(theta) - x1;
0055 A3 = L2 * ( cos(theta) * cos(gamma) - sin(theta) * sin(gamma) ) - x2;
0056 
0057 B2 = L3 * sin(theta);
0058 B3 = L2 * ( cos(theta) * sin(gamma) + sin(theta) * cos(gamma) ) - y2;
0059 
0060 p1v = sqrt( u1^2 + v1^2 );
0061 p2v = sqrt( (u1 + A2)^2 + (v1 + B2)^2 );
0062 p3v = sqrt( (u1 + A3)^2 + (v1 + B3)^2 );
0063 
0064 assert( p1 - p1v < tol );
0065 assert( p2 - p2v < tol );
0066 assert( p3 - p3v < tol );
0067 
0068 % plot pose 1
0069 
0070 u2 = u1 + L2 * cos( theta + gamma );
0071 v2 = v1 + L2 * sin( theta + gamma );
0072 
0073 u3 = u1 + L3 * cos( theta );
0074 v3 = v1 + L3 * sin( theta );
0075 
0076 figure(plotcnt); plotcnt = plotcnt + 1;
0077 
0078 plot( [u1 u2 u3 u1], [v1 v2 v3 v1], 'r' ); hold on
0079 plot( [ 0 x1 x2], [ 0  0 y2], 'bo' ); hold on
0080 plot( [ 0 u1 ], [ 0 v1 ], 'b-', [ x2 u2 ], [ y2  v2 ], 'b-', [ x1 u3 ], [ 0 v3 ], 'b-' );
0081 
0082 % assert that the strut lengths are correct
0083 
0084 theta = zero2; u1 = u1_2; v1 = v1_2;
0085 
0086 A2 = L3 * cos(theta) - x1;
0087 A3 = L2 * ( cos(theta) * cos(gamma) - sin(theta) * sin(gamma) ) - x2;
0088 
0089 B2 = L3 * sin(theta);
0090 B3 = L2 * ( cos(theta) * sin(gamma) + sin(theta) * cos(gamma) ) - y2;
0091 
0092 p1v = sqrt( u1^2 + v1^2 );
0093 p2v = sqrt( (u1 + A2)^2 + (v1 + B2)^2 );
0094 p3v = sqrt( (u1 + A3)^2 + (v1 + B3)^2 );
0095 
0096 assert( p1 - p1v < tol );
0097 assert( p2 - p2v < tol );
0098 assert( p3 - p3v < tol );
0099 
0100 % plot pose 2
0101 
0102 u2 = u1 + L2 * cos( theta + gamma );
0103 v2 = v1 + L2 * sin( theta + gamma );
0104 
0105 u3 = u1 + L3 * cos( theta );
0106 v3 = v1 + L3 * sin( theta );
0107 
0108 figure(plotcnt); plotcnt = plotcnt + 1;
0109 
0110 plot( [u1 u2 u3 u1], [v1 v2 v3 v1], 'r' ); hold on
0111 plot( [ 0 x1 x2], [ 0  0 y2], 'bo' ); hold on
0112 plot( [ 0 u1 ], [ 0 v1 ], 'b-', [ x2 u2 ], [ y2  v2 ], 'b-', [ x1 u3 ], [ 0 v3 ], 'b-' );
0113 
0114 % assert that the strut lengths are correct
0115 
0116 theta = zero3; u1 = u1_3; v1 = v1_3;
0117 
0118 A2 = L3 * cos(theta) - x1;
0119 A3 = L2 * ( cos(theta) * cos(gamma) - sin(theta) * sin(gamma) ) - x2;
0120 
0121 B2 = L3 * sin(theta);
0122 B3 = L2 * ( cos(theta) * sin(gamma) + sin(theta) * cos(gamma) ) - y2;
0123 
0124 p1v = sqrt( u1^2 + v1^2 );
0125 p2v = sqrt( (u1 + A2)^2 + (v1 + B2)^2 );
0126 p3v = sqrt( (u1 + A3)^2 + (v1 + B3)^2 );
0127 
0128 assert( p1 - p1v < tol );
0129 assert( p2 - p2v < tol );
0130 assert( p3 - p3v < tol );
0131 
0132 % plot pose 3
0133 
0134 u2 = u1 + L2 * cos( theta + gamma );
0135 v2 = v1 + L2 * sin( theta + gamma );
0136 
0137 u3 = u1 + L3 * cos( theta );
0138 v3 = v1 + L3 * sin( theta );
0139 
0140 figure(plotcnt); plotcnt = plotcnt + 1;
0141 
0142 plot( [u1 u2 u3 u1], [v1 v2 v3 v1], 'r' ); hold on
0143 plot( [ 0 x1 x2], [ 0  0 y2], 'bo' ); hold on
0144 plot( [ 0 u1 ], [ 0 v1 ], 'b-', [ x2 u2 ], [ y2  v2 ], 'b-', [ x1 u3 ], [ 0 v3 ], 'b-' );
0145 
0146 % assert that the strut lengths are correct
0147 
0148 theta = zero4; u1 = u1_4; v1 = v1_4;
0149 
0150 A2 = L3 * cos(theta) - x1;
0151 A3 = L2 * ( cos(theta) * cos(gamma) - sin(theta) * sin(gamma) ) - x2;
0152 
0153 B2 = L3 * sin(theta);
0154 B3 = L2 * ( cos(theta) * sin(gamma) + sin(theta) * cos(gamma) ) - y2;
0155 
0156 p1v = sqrt( u1^2 + v1^2 );
0157 p2v = sqrt( (u1 + A2)^2 + (v1 + B2)^2 );
0158 p3v = sqrt( (u1 + A3)^2 + (v1 + B3)^2 );
0159 
0160 assert( p1 - p1v < tol );
0161 assert( p2 - p2v < tol );
0162 assert( p3 - p3v < tol );
0163 
0164 % plot pose 4
0165 
0166 u2 = u1 + L2 * cos( theta + gamma );
0167 v2 = v1 + L2 * sin( theta + gamma );
0168 
0169 u3 = u1 + L3 * cos( theta );
0170 v3 = v1 + L3 * sin( theta );
0171 
0172 figure(plotcnt); plotcnt = plotcnt + 1;
0173 
0174 plot( [u1 u2 u3 u1], [v1 v2 v3 v1], 'r' ); hold on
0175 plot( [ 0 x1 x2], [ 0  0 y2], 'bo' ); hold on
0176 plot( [ 0 u1 ], [ 0 v1 ], 'b-', [ x2 u2 ], [ y2  v2 ], 'b-', [ x1 u3 ], [ 0 v3 ], 'b-' );