part5

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 = 7.01;
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 = [-0.7416 -0.5416];
0030 interval2 = [-0.5416 -0.2416];
0031 interval3 = [-0.2416 0.2584];
0032 interval4 = [0.2584 0.7584];
0033 interval5 = [0.7584 1.158];
0034 interval6 = [2.458 2.558];
0035 
0036 g = @(x) f2DStewart(x, gamma, L2, L3, p1, p2, p3, x1, x2, y2);
0037 
0038 zero1 = bisect(g, interval1(1), interval1(2), 0.5e-8)
0039 zero2 = bisect(g, interval2(1), interval2(2), 0.5e-8)
0040 zero3 = bisect(g, interval3(1), interval3(2), 0.5e-8)
0041 zero4 = bisect(g, interval4(1), interval4(2), 0.5e-8)
0042 zero5 = bisect(g, interval5(1), interval5(2), 0.5e-8)
0043 zero6 = bisect(g, interval6(1), interval6(2), 0.5e-8)
0044 
0045 [ val1 u1_1 v1_1 ] = f2DStewart(zero1, gamma, L2, L3, p1, p2, p3, x1, x2, y2);
0046 [ val2 u1_2 v1_2 ] = f2DStewart(zero2, gamma, L2, L3, p1, p2, p3, x1, x2, y2);
0047 [ val3 u1_3 v1_3 ] = f2DStewart(zero3, gamma, L2, L3, p1, p2, p3, x1, x2, y2);
0048 [ val4 u1_4 v1_4 ] = f2DStewart(zero4, gamma, L2, L3, p1, p2, p3, x1, x2, y2);
0049 [ val5 u1_5 v1_5 ] = f2DStewart(zero4, gamma, L2, L3, p1, p2, p3, x1, x2, y2);
0050 [ val6 u1_6 v1_6 ] = f2DStewart(zero4, gamma, L2, L3, p1, p2, p3, x1, x2, y2);
0051 
0052 plotcnt = 1;
0053 
0054 tol = 0.5E-8;
0055 
0056 % assert that the strut lengths are correct
0057 
0058 theta = zero1; u1 = u1_1; v1 = v1_1;
0059 
0060 A2 = L3 * cos(theta) - x1;
0061 A3 = L2 * ( cos(theta) * cos(gamma) - sin(theta) * sin(gamma) ) - x2;
0062 
0063 B2 = L3 * sin(theta);
0064 B3 = L2 * ( cos(theta) * sin(gamma) + sin(theta) * cos(gamma) ) - y2;
0065 
0066 p1v = sqrt( u1^2 + v1^2 );
0067 p2v = sqrt( (u1 + A2)^2 + (v1 + B2)^2 );
0068 p3v = sqrt( (u1 + A3)^2 + (v1 + B3)^2 );
0069 
0070 assert( p1 - p1v < tol );
0071 assert( p2 - p2v < tol );
0072 assert( p3 - p3v < tol );
0073 
0074 % plot pose 1
0075 
0076 u2 = u1 + L2 * cos( theta + gamma );
0077 v2 = v1 + L2 * sin( theta + gamma );
0078 
0079 u3 = u1 + L3 * cos( theta );
0080 v3 = v1 + L3 * sin( theta );
0081 
0082 figure(plotcnt); plotcnt = plotcnt + 1;
0083 
0084 plot( [u1 u2 u3 u1], [v1 v2 v3 v1], 'r' ); hold on
0085 plot( [ 0 x1 x2], [ 0  0 y2], 'bo' ); hold on
0086 plot( [ 0 u1 ], [ 0 v1 ], 'b-', [ x2 u2 ], [ y2  v2 ], 'b-', [ x1 u3 ], [ 0 v3 ], 'b-' );
0087 
0088 % assert that the strut lengths are correct
0089 
0090 theta = zero2; u1 = u1_2; v1 = v1_2;
0091 
0092 A2 = L3 * cos(theta) - x1;
0093 A3 = L2 * ( cos(theta) * cos(gamma) - sin(theta) * sin(gamma) ) - x2;
0094 
0095 B2 = L3 * sin(theta);
0096 B3 = L2 * ( cos(theta) * sin(gamma) + sin(theta) * cos(gamma) ) - y2;
0097 
0098 p1v = sqrt( u1^2 + v1^2 );
0099 p2v = sqrt( (u1 + A2)^2 + (v1 + B2)^2 );
0100 p3v = sqrt( (u1 + A3)^2 + (v1 + B3)^2 );
0101 
0102 assert( p1 - p1v < tol );
0103 assert( p2 - p2v < tol );
0104 assert( p3 - p3v < tol );
0105 
0106 % plot pose 2
0107 
0108 u2 = u1 + L2 * cos( theta + gamma );
0109 v2 = v1 + L2 * sin( theta + gamma );
0110 
0111 u3 = u1 + L3 * cos( theta );
0112 v3 = v1 + L3 * sin( theta );
0113 
0114 figure(plotcnt); plotcnt = plotcnt + 1;
0115 
0116 plot( [u1 u2 u3 u1], [v1 v2 v3 v1], 'r' ); hold on
0117 plot( [ 0 x1 x2], [ 0  0 y2], 'bo' ); hold on
0118 plot( [ 0 u1 ], [ 0 v1 ], 'b-', [ x2 u2 ], [ y2  v2 ], 'b-', [ x1 u3 ], [ 0 v3 ], 'b-' );
0119 
0120 % assert that the strut lengths are correct
0121 
0122 theta = zero3; u1 = u1_3; v1 = v1_3;
0123 
0124 A2 = L3 * cos(theta) - x1;
0125 A3 = L2 * ( cos(theta) * cos(gamma) - sin(theta) * sin(gamma) ) - x2;
0126 
0127 B2 = L3 * sin(theta);
0128 B3 = L2 * ( cos(theta) * sin(gamma) + sin(theta) * cos(gamma) ) - y2;
0129 
0130 p1v = sqrt( u1^2 + v1^2 );
0131 p2v = sqrt( (u1 + A2)^2 + (v1 + B2)^2 );
0132 p3v = sqrt( (u1 + A3)^2 + (v1 + B3)^2 );
0133 
0134 assert( p1 - p1v < tol );
0135 assert( p2 - p2v < tol );
0136 assert( p3 - p3v < tol );
0137 
0138 % plot pose 3
0139 
0140 u2 = u1 + L2 * cos( theta + gamma );
0141 v2 = v1 + L2 * sin( theta + gamma );
0142 
0143 u3 = u1 + L3 * cos( theta );
0144 v3 = v1 + L3 * sin( theta );
0145 
0146 figure(plotcnt); plotcnt = plotcnt + 1;
0147 
0148 plot( [u1 u2 u3 u1], [v1 v2 v3 v1], 'r' ); hold on
0149 plot( [ 0 x1 x2], [ 0  0 y2], 'bo' ); hold on
0150 plot( [ 0 u1 ], [ 0 v1 ], 'b-', [ x2 u2 ], [ y2  v2 ], 'b-', [ x1 u3 ], [ 0 v3 ], 'b-' );
0151 
0152 % assert that the strut lengths are correct
0153 
0154 theta = zero4; u1 = u1_4; v1 = v1_4;
0155 
0156 A2 = L3 * cos(theta) - x1;
0157 A3 = L2 * ( cos(theta) * cos(gamma) - sin(theta) * sin(gamma) ) - x2;
0158 
0159 B2 = L3 * sin(theta);
0160 B3 = L2 * ( cos(theta) * sin(gamma) + sin(theta) * cos(gamma) ) - y2;
0161 
0162 p1v = sqrt( u1^2 + v1^2 );
0163 p2v = sqrt( (u1 + A2)^2 + (v1 + B2)^2 );
0164 p3v = sqrt( (u1 + A3)^2 + (v1 + B3)^2 );
0165 
0166 assert( p1 - p1v < tol );
0167 assert( p2 - p2v < tol );
0168 assert( p3 - p3v < tol );
0169 
0170 % plot pose 4
0171 
0172 u2 = u1 + L2 * cos( theta + gamma );
0173 v2 = v1 + L2 * sin( theta + gamma );
0174 
0175 u3 = u1 + L3 * cos( theta );
0176 v3 = v1 + L3 * sin( theta );
0177 
0178 figure(plotcnt); plotcnt = plotcnt + 1;
0179 
0180 plot( [u1 u2 u3 u1], [v1 v2 v3 v1], 'r' ); hold on
0181 plot( [ 0 x1 x2], [ 0  0 y2], 'bo' ); hold on
0182 plot( [ 0 u1 ], [ 0 v1 ], 'b-', [ x2 u2 ], [ y2  v2 ], 'b-', [ x1 u3 ], [ 0 v3 ], 'b-' );
0183 
0184 % assert that the strut lengths are correct
0185 
0186 theta = zero5; u1 = u1_5; v1 = v1_5;
0187 
0188 A2 = L3 * cos(theta) - x1;
0189 A3 = L2 * ( cos(theta) * cos(gamma) - sin(theta) * sin(gamma) ) - x2;
0190 
0191 B2 = L3 * sin(theta);
0192 B3 = L2 * ( cos(theta) * sin(gamma) + sin(theta) * cos(gamma) ) - y2;
0193 
0194 p1v = sqrt( u1^2 + v1^2 );
0195 p2v = sqrt( (u1 + A2)^2 + (v1 + B2)^2 );
0196 p3v = sqrt( (u1 + A3)^2 + (v1 + B3)^2 );
0197 
0198 assert( p1 - p1v < tol );
0199 assert( p2 - p2v < tol );
0200 assert( p3 - p3v < tol );
0201 
0202 % plot pose 5
0203 
0204 u2 = u1 + L2 * cos( theta + gamma );
0205 v2 = v1 + L2 * sin( theta + gamma );
0206 
0207 u3 = u1 + L3 * cos( theta );
0208 v3 = v1 + L3 * sin( theta );
0209 
0210 figure(plotcnt); plotcnt = plotcnt + 1;
0211 
0212 plot( [u1 u2 u3 u1], [v1 v2 v3 v1], 'r' ); hold on
0213 plot( [ 0 x1 x2], [ 0  0 y2], 'bo' ); hold on
0214 plot( [ 0 u1 ], [ 0 v1 ], 'b-', [ x2 u2 ], [ y2  v2 ], 'b-', [ x1 u3 ], [ 0 v3 ], 'b-' );
0215 
0216 % assert that the strut lengths are correct
0217 
0218 theta = zero6; u1 = u1_6; v1 = v1_6;
0219 
0220 A2 = L3 * cos(theta) - x1;
0221 A3 = L2 * ( cos(theta) * cos(gamma) - sin(theta) * sin(gamma) ) - x2;
0222 
0223 B2 = L3 * sin(theta);
0224 B3 = L2 * ( cos(theta) * sin(gamma) + sin(theta) * cos(gamma) ) - y2;
0225 
0226 p1v = sqrt( u1^2 + v1^2 );
0227 p2v = sqrt( (u1 + A2)^2 + (v1 + B2)^2 );
0228 p3v = sqrt( (u1 + A3)^2 + (v1 + B3)^2 );
0229 
0230 assert( p1 - p1v < tol );
0231 assert( p2 - p2v < tol );
0232 assert( p3 - p3v < tol );
0233 
0234 % plot pose 6
0235 
0236 u2 = u1 + L2 * cos( theta + gamma );
0237 v2 = v1 + L2 * sin( theta + gamma );
0238 
0239 u3 = u1 + L3 * cos( theta );
0240 v3 = v1 + L3 * sin( theta );
0241 
0242 figure(plotcnt); plotcnt = plotcnt + 1;
0243 
0244 plot( [u1 u2 u3 u1], [v1 v2 v3 v1], 'r' ); hold on
0245 plot( [ 0 x1 x2], [ 0  0 y2], 'bo' ); hold on
0246 plot( [ 0 u1 ], [ 0 v1 ], 'b-', [ x2 u2 ], [ y2  v2 ], 'b-', [ x1 u3 ], [ 0 v3 ], 'b-' );
0247