Chen Huan
StewartOlatform
Embed:
(wiki syntax)
Show/hide line numbers
StewartPlatform.cpp
00001 #include "StewartPlatform.h" 00002 00003 //******************************************** 00004 //功能：计算矩阵乘法 C=A*B 00005 //输入参数：A、B：参加运算的矩阵 00006 //输出参数：C：运算结果 00007 //返回值：计算是否成功 成功返回0 否则返回1 00008 //调用外部函数：无 00009 //作者：陈欢 h-che14@mails.tsinghua.edu.cn 00010 //******************************************** 00011 int MatrixDot(MatrixType* A, MatrixType* B, MatrixType* C) 00012 { 00013 int i, j, k; //循环控制变量 00014 int posA, posB, posC; //矩阵下标索引 00015 00016 //判断异常 00017 if(A->Size[1] != B->Size[0]) //维度不匹配 00018 return 1; 00019 if(A->Size[0] * B->Size[1] > 50) //C矩阵规模太大 00020 return 1; 00021 00022 //计算矩阵乘法 00023 C->Size[0] = A->Size[0]; //得到C的行列数 00024 C->Size[1] = B->Size[1]; 00025 for(i = 1; i <= C->Size[0]; i++) //行循环 00026 { 00027 for(j = 1; j <= C->Size[1]; j++) //列循环 00028 { 00029 posC = f2(i,j,C->Size[1]); 00030 C->Elements[posC] = 0; 00031 for(k = 1; k <= A->Size[1]; k++) //计算乘法结果 00032 { 00033 posA = f2(i,k,A->Size[1]); 00034 posB = f2(k,j,B->Size[1]); 00035 C->Elements[posC] += A->Elements[posA] * B->Elements[posB]; 00036 } 00037 } 00038 } 00039 return 0; 00040 } 00041 00042 //******************************************** 00043 //功能：计算矩阵转置 00044 //输入参数：A：被转置的矩阵 00045 //输出参数：B：转置后的矩阵 00046 //返回值：无 00047 //调用外部函数：无 00048 //作者：陈欢 h-che14@mails.tsinghua.edu.cn 00049 //******************************************** 00050 void MatrixTransposition(MatrixType* A, MatrixType* B) 00051 { 00052 int i, j; 00053 int posA, posB; 00054 00055 B->Size[0] = A->Size[1]; 00056 B->Size[1] = A->Size[0]; 00057 00058 for(i = 1; i <= A->Size[0]; i++) 00059 { 00060 for(j = 1; j <= A->Size[1]; j++) 00061 { 00062 posA = f2(i,j,A->Size[1]); 00063 posB = f2(j,i,B->Size[1]); 00064 B->Elements[posB] = A->Elements[posA]; 00065 } 00066 } 00067 } 00068 00069 //******************************************** 00070 //功能：获得矩阵的子阵 00071 //输入参数：A：原矩阵 StartRow、StartColumn、EndRow、EndColumn：子阵起始元素 子阵终了元素 00072 //输出参数：B：子阵 00073 //返回值：无 00074 //调用外部函数：无 00075 //作者：陈欢 h-che14@mails.tsinghua.edu.cn 00076 //******************************************** 00077 void MatrixSub(MatrixType* A,int StartRow, int StartColumn, int EndRow, int EndColumn, MatrixType* B) 00078 { 00079 int i, j; //循环控制变量 00080 int posA, posB; //矩阵索引 00081 00082 B->Size[0] = EndRow - StartRow + 1; //计算子阵的维度 00083 B->Size[1] = EndColumn - StartColumn + 1; 00084 for(i = 1; i <= B->Size[0]; i++) 00085 { 00086 for(j = 1; j <= B->Size[1]; j++) 00087 { 00088 posA = f2(StartRow + i - 1, StartColumn + j - 1, A->Size[1]); 00089 posB = f2(i, j, B->Size[1]); 00090 B->Elements[posB] = A->Elements[posA]; 00091 } 00092 } 00093 } 00094 00095 //******************************************** 00096 //功能：填充矩阵 将一个矩阵填充到另一个矩阵中 00097 //输入参数：A：被填充的矩阵 Row、Column：矩阵填充的位置 B：要填充到被填充矩阵的矩阵 00098 //输出参数：A：被填充的矩阵 00099 //返回值：0 代表成功 1代表失败 00100 //调用外部函数：无 00101 //作者：陈欢 h-che14@mails.tsinghua.edu.cn 00102 //******************************************** 00103 int MatrixFill(MatrixType* A,int Row, int Column, MatrixType* B) 00104 { 00105 int i, j, posA, posB; 00106 00107 if((Row + B->Size[0] - 1) > A->Size[0]) 00108 { 00109 return 1; 00110 } 00111 else if((Column + B->Size[1] - 1) > A->Size[1]) 00112 { 00113 return 1; 00114 } 00115 else 00116 { 00117 for(i = 1; i <= B->Size[0]; i++) 00118 { 00119 for(j = 1; j <= B->Size[1]; j++) 00120 { 00121 posA = f2(Row + i - 1, Column + j - 1, A->Size[1]); 00122 posB = f2(i, j, B->Size[1]); 00123 A->Elements[posA] = B->Elements[posB]; 00124 } 00125 } 00126 return 0; 00127 } 00128 } 00129 00130 //******************************************** 00131 //功能：指定动平台变换矩阵参数x,y,z,a,b,c，计算动平台上的点A在绝对坐标系下的坐标B A可以是多个点 一行一个点 00132 //输入参数：x,y,z,a,b,c：动平台变换矩阵参数 A：动平台上点的相对坐标 00133 //输出参数：B：点在绝对坐标系下的坐标 00134 //返回值：无 00135 //调用外部函数：无 00136 //作者：陈欢 h-che14@mails.tsinghua.edu.cn 00137 //******************************************** 00138 void Inverse(float x, float y, float z, float a, float b, float c, MatrixType* A, MatrixType* B) 00139 { 00140 int i; 00141 00142 MatrixType T; //原点平移 00143 MatrixType Ra; //俯仰 YAW 00144 MatrixType Rb; //横滚 ROLL 00145 MatrixType Rc; //偏航 PITCH 00146 MatrixType temp1, temp2, temp3; 00147 00148 T.Size[0] = 4; 00149 T.Size[1] = 4; 00150 T.Elements[f2(1,1,T.Size[1])] = 1; 00151 T.Elements[f2(1,2,T.Size[1])] = 0; 00152 T.Elements[f2(1,3,T.Size[1])] = 0; 00153 T.Elements[f2(1,4,T.Size[1])] = x; 00154 T.Elements[f2(2,1,T.Size[1])] = 0; 00155 T.Elements[f2(2,2,T.Size[1])] = 1; 00156 T.Elements[f2(2,3,T.Size[1])] = 0; 00157 T.Elements[f2(2,4,T.Size[1])] = y; 00158 T.Elements[f2(3,1,T.Size[1])] = 0; 00159 T.Elements[f2(3,2,T.Size[1])] = 0; 00160 T.Elements[f2(3,3,T.Size[1])] = 1; 00161 T.Elements[f2(3,4,T.Size[1])] = z; 00162 T.Elements[f2(4,1,T.Size[1])] = 0; 00163 T.Elements[f2(4,2,T.Size[1])] = 0; 00164 T.Elements[f2(4,3,T.Size[1])] = 0; 00165 T.Elements[f2(4,4,T.Size[1])] = 1; 00166 00167 Ra.Size[0] = 4; 00168 Ra.Size[1] = 4; 00169 Ra.Elements[f2(1,1,Ra.Size[1])] = 1; 00170 Ra.Elements[f2(1,2,Ra.Size[1])] = 0; 00171 Ra.Elements[f2(1,3,Ra.Size[1])] = 0; 00172 Ra.Elements[f2(1,4,Ra.Size[1])] = 0; 00173 Ra.Elements[f2(2,1,Ra.Size[1])] = 0; 00174 Ra.Elements[f2(2,2,Ra.Size[1])] = cosd(a); 00175 Ra.Elements[f2(2,3,Ra.Size[1])] = -sind(a); 00176 Ra.Elements[f2(2,4,Ra.Size[1])] = 0; 00177 Ra.Elements[f2(3,1,Ra.Size[1])] = 0; 00178 Ra.Elements[f2(3,2,Ra.Size[1])] = sind(a); 00179 Ra.Elements[f2(3,3,Ra.Size[1])] = cosd(a); 00180 Ra.Elements[f2(3,4,Ra.Size[1])] = 0; 00181 Ra.Elements[f2(4,1,Ra.Size[1])] = 0; 00182 Ra.Elements[f2(4,2,Ra.Size[1])] = 0; 00183 Ra.Elements[f2(4,3,Ra.Size[1])] = 0; 00184 Ra.Elements[f2(4,4,Ra.Size[1])] = 1; 00185 00186 Rb.Size[0] = 4; 00187 Rb.Size[1] = 4; 00188 Rb.Elements[f2(1,1,Rb.Size[1])] = cosd(b); 00189 Rb.Elements[f2(1,2,Rb.Size[1])] = 0; 00190 Rb.Elements[f2(1,3,Rb.Size[1])] = sind(b); 00191 Rb.Elements[f2(1,4,Rb.Size[1])] = 0; 00192 Rb.Elements[f2(2,1,Rb.Size[1])] = 0; 00193 Rb.Elements[f2(2,2,Rb.Size[1])] = 1; 00194 Rb.Elements[f2(2,3,Rb.Size[1])] = 0; 00195 Rb.Elements[f2(2,4,Rb.Size[1])] = 0; 00196 Rb.Elements[f2(3,1,Rb.Size[1])] = -sind(b); 00197 Rb.Elements[f2(3,2,Rb.Size[1])] = 0; 00198 Rb.Elements[f2(3,3,Rb.Size[1])] = cosd(b); 00199 Rb.Elements[f2(3,4,Rb.Size[1])] = 0; 00200 Rb.Elements[f2(4,1,Rb.Size[1])] = 0; 00201 Rb.Elements[f2(4,2,Rb.Size[1])] = 0; 00202 Rb.Elements[f2(4,3,Rb.Size[1])] = 0; 00203 Rb.Elements[f2(4,4,Rb.Size[1])] = 1; 00204 00205 Rc.Size[0] = 4; 00206 Rc.Size[1] = 4; 00207 Rc.Elements[f2(1,1,Rc.Size[1])] = cosd(c); 00208 Rc.Elements[f2(1,2,Rc.Size[1])] = -sind(c); 00209 Rc.Elements[f2(1,3,Rc.Size[1])] = 0; 00210 Rc.Elements[f2(1,4,Rc.Size[1])] = 0; 00211 Rc.Elements[f2(2,1,Rc.Size[1])] = sind(c); 00212 Rc.Elements[f2(2,2,Rc.Size[1])] = cosd(c); 00213 Rc.Elements[f2(2,3,Rc.Size[1])] = 0; 00214 Rc.Elements[f2(2,4,Rc.Size[1])] = 0; 00215 Rc.Elements[f2(3,1,Rc.Size[1])] = 0; 00216 Rc.Elements[f2(3,2,Rc.Size[1])] = 0; 00217 Rc.Elements[f2(3,3,Rc.Size[1])] = 1; 00218 Rc.Elements[f2(3,4,Rc.Size[1])] = 0; 00219 Rc.Elements[f2(4,1,Rc.Size[1])] = 0; 00220 Rc.Elements[f2(4,2,Rc.Size[1])] = 0; 00221 Rc.Elements[f2(4,3,Rc.Size[1])] = 0; 00222 Rc.Elements[f2(4,4,Rc.Size[1])] = 1; 00223 00224 B->Size[0] = A->Size[0]; 00225 B->Size[1] = A->Size[1]; 00226 MatrixDot(&T, &Rc, &temp1); //相当于T * Rc * Rb * Ra 00227 MatrixDot(&temp1, &Rb, &temp2); 00228 MatrixDot(&temp2, &Ra, &temp1); 00229 for(i = 1; i <= B->Size[0]; i++) 00230 { 00231 MatrixSub(A, i, 1, i, A->Size[1], &temp2); 00232 MatrixTransposition(&temp2, &temp3); 00233 MatrixDot(&temp1, &temp3, &temp2); 00234 MatrixTransposition(&temp2, &temp3); 00235 MatrixFill(B, i, 1, &temp3); 00236 } 00237 } 00238 00239 //******************************************** 00240 //功能：计算矩阵行向量所表示的坐标点之间的距离 00241 //输入参数：A, B：要计算距离的矩阵 00242 //输出参数：C：包含距离值信息的列向量 00243 //返回值：无 00244 //调用外部函数：无 00245 //作者：陈欢 h-che14@mails.tsinghua.edu.cn 00246 //******************************************** 00247 int Distance2Point(MatrixType* A, MatrixType* B, MatrixType* C) 00248 { 00249 float distance = 0; 00250 int i = 0, j = 0; 00251 if((A->Size[0] != B->Size[0]) || (A->Size[1] != 4) || (B->Size[1] != 4)) 00252 { 00253 return 1; 00254 } 00255 else 00256 { 00257 C->Size[0] = A->Size[0]; 00258 C->Size[1] = 1; 00259 for(i = 1; i <= A->Size[0]; i++) 00260 { 00261 for(j = 1; j <= A->Size[1]; j++) 00262 { 00263 distance = distance + (A->Elements[f2(i,j,A->Size[1])] - B->Elements[f2(i,j,B->Size[1])]) * (A->Elements[f2(i,j,A->Size[1])] - B->Elements[f2(i,j,B->Size[1])]); 00264 } 00265 C->Elements[f2(i,1,C->Size[1])] = sqrt(distance); 00266 distance = 0; 00267 } 00268 return 0; 00269 } 00270 } 00271 00272 //******************************************** 00273 //功能：解析动感平台 00274 //输入参数：Platform：动感平台数据结构 包含各种输入输出 00275 //输出参数：无 00276 //返回值：无 00277 //调用外部函数：无 00278 //作者：陈欢 h-che14@mails.tsinghua.edu.cn 00279 //******************************************** 00280 void CalStewartPlatform(StewartPlatformType* Platform) 00281 { 00282 int x, y, z, a, b, c; //动平台变换矩阵参数 00283 float xx, yy, zz, r, l, AA, BB, CC, DD, EE, delta, mytheta1, mytheta2, d, e, f, l1, l2, l3, l4; //计算电机角度用的参数 00284 float theta1, theta2, theta3, theta4, theta5, theta6; 00285 float topRadius, topInterval, bottomRadius, bottomInterval, lengthOfSteelWheel, lengthOfCardan, lengthOfBar; 00286 //平台机械尺寸定义 00287 float theta0; //舵盘结构的倾角 00288 MatrixType temp1, temp2; 00289 00290 MatrixType topPlatform; //动平台上的6个参考点 00291 MatrixType Rc; 00292 MatrixType bottomPlatform; //定平台上的6个参考点 00293 MatrixType lengthOfBar1; 00294 00295 topRadius = Platform->topRadius; //平台参数初始化 00296 topInterval = Platform->topInterval; 00297 bottomRadius = Platform->bottomRadius; 00298 bottomInterval = Platform->bottomInterval; 00299 lengthOfSteelWheel = Platform->lengthOfSteelWheel; 00300 lengthOfCardan = Platform->lengthOfCardan; 00301 lengthOfBar = Platform->lengthOfBar; 00302 00303 topPlatform.Size[0] = 6; 00304 topPlatform.Size[1] = 4; 00305 topPlatform.Elements[f2(1,1,topPlatform.Size[1])] = -topInterval / 2; 00306 topPlatform.Elements[f2(1,2,topPlatform.Size[1])] = -topRadius; 00307 topPlatform.Elements[f2(1,3,topPlatform.Size[1])] = 0; 00308 topPlatform.Elements[f2(1,4,topPlatform.Size[1])] = 1; 00309 topPlatform.Elements[f2(2,1,topPlatform.Size[1])] = topInterval / 2; 00310 topPlatform.Elements[f2(2,2,topPlatform.Size[1])] = -topRadius; 00311 topPlatform.Elements[f2(2,3,topPlatform.Size[1])] = 0; 00312 topPlatform.Elements[f2(2,4,topPlatform.Size[1])] = 1; 00313 00314 Rc.Size[0] = 4; 00315 Rc.Size[1] = 4; 00316 Rc.Elements[f2(1,1,Rc.Size[1])] = cosd(120); 00317 Rc.Elements[f2(1,2,Rc.Size[1])] = -sind(120); 00318 Rc.Elements[f2(1,3,Rc.Size[1])] = 0; 00319 Rc.Elements[f2(1,4,Rc.Size[1])] = 0; 00320 Rc.Elements[f2(2,1,Rc.Size[1])] = sind(120); 00321 Rc.Elements[f2(2,2,Rc.Size[1])] = cosd(120); 00322 Rc.Elements[f2(2,3,Rc.Size[1])] = 0; 00323 Rc.Elements[f2(2,4,Rc.Size[1])] = 0; 00324 Rc.Elements[f2(3,1,Rc.Size[1])] = 0; 00325 Rc.Elements[f2(3,2,Rc.Size[1])] = 0; 00326 Rc.Elements[f2(3,3,Rc.Size[1])] = 1; 00327 Rc.Elements[f2(3,4,Rc.Size[1])] = 0; 00328 Rc.Elements[f2(4,1,Rc.Size[1])] = 0; 00329 Rc.Elements[f2(4,2,Rc.Size[1])] = 0; 00330 Rc.Elements[f2(4,3,Rc.Size[1])] = 0; 00331 Rc.Elements[f2(4,4,Rc.Size[1])] = 1; 00332 00333 MatrixSub(&topPlatform, 1, 1, 1, 4, &temp1); //等效于topPlatform(3,:) = (Rc * topPlatform(1, :)')'; 00334 MatrixTransposition(&temp1, &temp2); 00335 MatrixDot(&Rc, &temp2, &temp1); 00336 MatrixTransposition(&temp1, &temp2); 00337 MatrixFill(&topPlatform, 3, 1, &temp2); 00338 MatrixSub(&topPlatform, 2, 1, 2, 4, &temp1); //等效于topPlatform(4,:) = (Rc * topPlatform(2, :)')'; 00339 MatrixTransposition(&temp1, &temp2); 00340 MatrixDot(&Rc, &temp2, &temp1); 00341 MatrixTransposition(&temp1, &temp2); 00342 MatrixFill(&topPlatform, 4, 1, &temp2); 00343 MatrixSub(&topPlatform, 3, 1, 3, 4, &temp1); //等效于topPlatform(5,:) = (Rc * topPlatform(3, :)')'; 00344 MatrixTransposition(&temp1, &temp2); 00345 MatrixDot(&Rc, &temp2, &temp1); 00346 MatrixTransposition(&temp1, &temp2); 00347 MatrixFill(&topPlatform, 5, 1, &temp2); 00348 MatrixSub(&topPlatform, 4, 1, 4, 4, &temp1); //等效于topPlatform(5,:) = (Rc * topPlatform(3, :)')'; 00349 MatrixTransposition(&temp1, &temp2); 00350 MatrixDot(&Rc, &temp2, &temp1); 00351 MatrixTransposition(&temp1, &temp2); 00352 MatrixFill(&topPlatform, 6, 1, &temp2); 00353 00354 x = Platform->x; 00355 y = Platform->y; 00356 z = Platform->z; 00357 a = Platform->a; 00358 b = Platform->b; 00359 c = Platform->c; 00360 Inverse(x, y, z, a, b, c, &topPlatform, &temp1); //计算出动平台参考点的实际位置 00361 MatrixFill(&topPlatform, 1, 1, &temp1); 00362 00363 bottomPlatform.Size[0] = 6; 00364 bottomPlatform.Size[1] = 4; 00365 bottomPlatform.Elements[f2(1,1,bottomPlatform.Size[1])] = -bottomInterval / 2; 00366 bottomPlatform.Elements[f2(1,2,bottomPlatform.Size[1])] = -bottomRadius; 00367 bottomPlatform.Elements[f2(1,3,bottomPlatform.Size[1])] = 0; 00368 bottomPlatform.Elements[f2(1,4,bottomPlatform.Size[1])] = 1; 00369 bottomPlatform.Elements[f2(2,1,bottomPlatform.Size[1])] = bottomInterval / 2; 00370 bottomPlatform.Elements[f2(2,2,bottomPlatform.Size[1])] = -bottomRadius; 00371 bottomPlatform.Elements[f2(2,3,bottomPlatform.Size[1])] = 0; 00372 bottomPlatform.Elements[f2(2,4,bottomPlatform.Size[1])] = 1; 00373 00374 MatrixSub(&bottomPlatform, 1, 1, 1, 4, &temp1); //等效于bottomPlatform(3,:) = (Rc * bottomPlatform(1, :)')'; 00375 MatrixTransposition(&temp1, &temp2); 00376 MatrixDot(&Rc, &temp2, &temp1); 00377 MatrixTransposition(&temp1, &temp2); 00378 MatrixFill(&bottomPlatform, 3, 1, &temp2); 00379 MatrixSub(&bottomPlatform, 2, 1, 2, 4, &temp1); //等效于bottomPlatform(4,:) = (Rc * bottomPlatform(2, :)')'; 00380 MatrixTransposition(&temp1, &temp2); 00381 MatrixDot(&Rc, &temp2, &temp1); 00382 MatrixTransposition(&temp1, &temp2); 00383 MatrixFill(&bottomPlatform, 4, 1, &temp2); 00384 MatrixSub(&bottomPlatform, 3, 1, 3, 4, &temp1); //等效于bottomPlatform(5,:) = (Rc * bottomPlatform(3, :)')'; 00385 MatrixTransposition(&temp1, &temp2); 00386 MatrixDot(&Rc, &temp2, &temp1); 00387 MatrixTransposition(&temp1, &temp2); 00388 MatrixFill(&bottomPlatform, 5, 1, &temp2); 00389 MatrixSub(&bottomPlatform, 4, 1, 4, 4, &temp1); //等效于bottomPlatform(6,:) = (Rc * bottomPlatform(3, :)')'; 00390 MatrixTransposition(&temp1, &temp2); 00391 MatrixDot(&Rc, &temp2, &temp1); 00392 MatrixTransposition(&temp1, &temp2); 00393 MatrixFill(&bottomPlatform, 6, 1, &temp2); 00394 00395 Distance2Point(&topPlatform, &bottomPlatform, &lengthOfBar1); 00396 00397 Platform->BarLength[0] = lengthOfBar1.Elements[f2(1,1,lengthOfBar1.Size[1])]; //赋值计算出的杆长 00398 Platform->BarLength[1] = lengthOfBar1.Elements[f2(2,1,lengthOfBar1.Size[1])]; 00399 Platform->BarLength[2] = lengthOfBar1.Elements[f2(3,1,lengthOfBar1.Size[1])]; 00400 Platform->BarLength[3] = lengthOfBar1.Elements[f2(4,1,lengthOfBar1.Size[1])]; 00401 Platform->BarLength[4] = lengthOfBar1.Elements[f2(5,1,lengthOfBar1.Size[1])]; 00402 Platform->BarLength[5] = lengthOfBar1.Elements[f2(6,1,lengthOfBar1.Size[1])]; 00403 00404 //计算角度 00405 r = sqrt(lengthOfCardan * lengthOfCardan + lengthOfSteelWheel * lengthOfSteelWheel); 00406 l = lengthOfBar; 00407 //点1 00408 xx = topPlatform.Elements[f2(1,1,topPlatform.Size[1])] - bottomPlatform.Elements[f2(1,1,bottomPlatform.Size[1])]; 00409 yy = topPlatform.Elements[f2(1,2,topPlatform.Size[1])] - bottomPlatform.Elements[f2(1,2,bottomPlatform.Size[1])]; 00410 zz = topPlatform.Elements[f2(1,3,topPlatform.Size[1])] - bottomPlatform.Elements[f2(1,3,bottomPlatform.Size[1])]; 00411 AA = -(l * l - xx * xx - yy * yy - r * r - zz * zz) / (2 * r * zz); 00412 BB = 2 * r * xx / (2 * r * zz); 00413 CC = BB * BB + 1; 00414 DD = 2 * AA * BB; 00415 EE = AA * AA - 1; 00416 delta = DD * DD - 4 * CC * EE; 00417 mytheta1 = acos((-DD + sqrt(delta)) / 2 / CC); 00418 mytheta2 = acos((-DD - sqrt(delta)) / 2 / CC); 00419 d = (xx + r * cos(mytheta1)) * (xx + r * cos(mytheta1)); 00420 e = yy * yy; 00421 f = (zz - r * sin(mytheta1)) * (zz - r * sin(mytheta1)); 00422 l1 = sqrt(d + e + f); 00423 d = (xx + r * cos(mytheta1)) * (xx + r * cos(mytheta1)); 00424 e = yy * yy; 00425 f = (zz - r * sin(-mytheta1)) * (zz - r * sin(-mytheta1)); 00426 l2 = sqrt(d + e + f); 00427 d = (xx + r * cos(mytheta2)) * (xx + r * cos(mytheta2)); 00428 e = yy * yy; 00429 f = (zz - r * sin(mytheta2)) * (zz - r * sin(mytheta2)); 00430 l3 = sqrt(d + e + f); 00431 d = (xx + r * cos(mytheta2)) * (xx + r * cos(mytheta2)); 00432 e = yy * yy; 00433 f = (zz - r * sin(-mytheta2)) * (zz - r * sin(-mytheta2)); 00434 l4 = sqrt(d + e + f); 00435 mytheta1 = mytheta1 / 3.1415926 * 180; 00436 mytheta2 = mytheta2 / 3.1415926 * 180; 00437 if(abs(l1 - l) <= 0.0001) 00438 { 00439 theta1 = mytheta1; 00440 } 00441 else if(abs(l2 - l) <= 0.0001) 00442 { 00443 theta1 = -mytheta1; 00444 } 00445 else if(abs(l3 - l) <= 0.0001) 00446 { 00447 theta1 = mytheta2; 00448 } 00449 else 00450 { 00451 theta1 = -mytheta2; 00452 } 00453 //点2 00454 xx = topPlatform.Elements[f2(2,1,topPlatform.Size[1])] - bottomPlatform.Elements[f2(2,1,bottomPlatform.Size[1])]; 00455 yy = topPlatform.Elements[f2(2,2,topPlatform.Size[1])] - bottomPlatform.Elements[f2(2,2,bottomPlatform.Size[1])]; 00456 zz = topPlatform.Elements[f2(2,3,topPlatform.Size[1])] - bottomPlatform.Elements[f2(2,3,bottomPlatform.Size[1])]; 00457 AA = -(l * l - xx * xx - yy * yy - r * r - zz * zz) / (2 * r * zz); 00458 BB = -2 * r * xx / (2 * r * zz); 00459 CC = BB * BB + 1; 00460 DD = 2 * AA * BB; 00461 EE = AA * AA - 1; 00462 delta = DD * DD - 4 * CC * EE; 00463 mytheta1 = acos((-DD + sqrt(delta)) / 2 / CC); 00464 mytheta2 = acos((-DD - sqrt(delta)) / 2 / CC); 00465 d = (xx - r * cos(mytheta1)) * (xx - r * cos(mytheta1)); 00466 e = yy * yy; 00467 f = (zz - r * sin(mytheta1)) * (zz - r * sin(mytheta1)); 00468 l1 = sqrt(d + e + f); 00469 d = (xx - r * cos(mytheta1)) * (xx - r * cos(mytheta1)); 00470 e = yy * yy; 00471 f = (zz - r * sin(-mytheta1)) * (zz - r * sin(-mytheta1)); 00472 l2 = sqrt(d + e + f); 00473 d = (xx - r * cos(mytheta2)) * (xx - r * cos(mytheta2)); 00474 e = yy * yy; 00475 f = (zz - r * sin(mytheta2)) * (zz - r * sin(mytheta2)); 00476 l3 = sqrt(d + e + f); 00477 d = (xx - r * cos(mytheta2)) * (xx - r * cos(mytheta2)); 00478 e = yy * yy; 00479 f = (zz - r * sin(-mytheta2)) * (zz - r * sin(-mytheta2)); 00480 l4 = sqrt(d + e + f); 00481 mytheta1 = mytheta1 / 3.1415926 * 180; 00482 mytheta2 = mytheta2 / 3.1415926 * 180; 00483 if(abs(l1 - l) <= 0.0001) 00484 { 00485 theta2 = mytheta1; 00486 } 00487 else if(abs(l2 - l) <= 0.0001) 00488 { 00489 theta2 = -mytheta1; 00490 } 00491 else if(abs(l4 - l) <= 0.0001) 00492 { 00493 theta2 = mytheta2; 00494 } 00495 else 00496 { 00497 theta2 = -mytheta2; 00498 } 00499 //点3 00500 MatrixSub(&topPlatform, 3, 1, 3, 4, &temp1); //等效于bottomPlatform(6,:) = (Rc * Rc * bottomPlatform(3, :)')'; 00501 MatrixTransposition(&temp1, &temp2); 00502 MatrixDot(&Rc, &temp2, &temp1); 00503 MatrixDot(&Rc, &temp1, &temp2); 00504 MatrixTransposition(&temp2, &temp1); 00505 xx = temp1.Elements[f2(1,1,temp1.Size[1])] - bottomPlatform.Elements[f2(1,1,bottomPlatform.Size[1])]; 00506 yy = temp1.Elements[f2(1,2,temp1.Size[1])] - bottomPlatform.Elements[f2(1,2,bottomPlatform.Size[1])]; 00507 zz = temp1.Elements[f2(1,3,temp1.Size[1])] - bottomPlatform.Elements[f2(1,3,bottomPlatform.Size[1])]; 00508 AA = -(l * l - xx * xx - yy * yy - r * r - zz * zz) / (2 * r * zz); 00509 BB = 2 * r * xx / (2 * r * zz); 00510 CC = BB * BB + 1; 00511 DD = 2 * AA * BB; 00512 EE = AA * AA - 1; 00513 delta = DD * DD - 4 * CC * EE; 00514 mytheta1 = acos((-DD + sqrt(delta)) / 2 / CC); 00515 mytheta2 = acos((-DD - sqrt(delta)) / 2 / CC); 00516 d = (xx + r * cos(mytheta1)) * (xx + r * cos(mytheta1)); 00517 e = yy * yy; 00518 f = (zz - r * sin(mytheta1)) * (zz - r * sin(mytheta1)); 00519 l1 = sqrt(d + e + f); 00520 d = (xx + r * cos(mytheta1)) * (xx + r * cos(mytheta1)); 00521 e = yy * yy; 00522 f = (zz - r * sin(-mytheta1)) * (zz - r * sin(-mytheta1)); 00523 l2 = sqrt(d + e + f); 00524 d = (xx + r * cos(mytheta2)) * (xx + r * cos(mytheta2)); 00525 e = yy * yy; 00526 f = (zz - r * sin(mytheta2)) * (zz - r * sin(mytheta2)); 00527 l3 = sqrt(d + e + f); 00528 d = (xx + r * cos(mytheta2)) * (xx + r * cos(mytheta2)); 00529 e = yy * yy; 00530 f = (zz - r * sin(-mytheta2)) * (zz - r * sin(-mytheta2)); 00531 l4 = sqrt(d + e + f); 00532 mytheta1 = mytheta1 / 3.1415926 * 180; 00533 mytheta2 = mytheta2 / 3.1415926 * 180; 00534 if(abs(l1 - l) <= 0.0001) 00535 { 00536 theta3 = mytheta1; 00537 } 00538 else if(abs(l2 - l) <= 0.0001) 00539 { 00540 theta3 = -mytheta1; 00541 } 00542 else if(abs(l4 - l) <= 0.0001) 00543 { 00544 theta3 = mytheta2; 00545 } 00546 else 00547 { 00548 theta3 = -mytheta2; 00549 } 00550 //点4 00551 MatrixSub(&topPlatform, 4, 1, 4, 4, &temp1); //等效于bottomPlatform(6,:) = (Rc * Rc * bottomPlatform(3, :)')'; 00552 MatrixTransposition(&temp1, &temp2); 00553 MatrixDot(&Rc, &temp2, &temp1); 00554 MatrixDot(&Rc, &temp1, &temp2); 00555 MatrixTransposition(&temp2, &temp1); 00556 xx = temp1.Elements[f2(1,1,temp1.Size[1])] - bottomPlatform.Elements[f2(2,1,bottomPlatform.Size[1])]; 00557 yy = temp1.Elements[f2(1,2,temp1.Size[1])] - bottomPlatform.Elements[f2(2,2,bottomPlatform.Size[1])]; 00558 zz = temp1.Elements[f2(1,3,temp1.Size[1])] - bottomPlatform.Elements[f2(2,3,bottomPlatform.Size[1])]; 00559 AA = -(l * l - xx * xx - yy * yy - r * r - zz * zz) / (2 * r * zz); 00560 BB = -2 * r * xx / (2 * r * zz); 00561 CC = BB * BB + 1; 00562 DD = 2 * AA * BB; 00563 EE = AA * AA - 1; 00564 delta = DD * DD - 4 * CC * EE; 00565 mytheta1 = acos((-DD + sqrt(delta)) / 2 / CC); 00566 mytheta2 = acos((-DD - sqrt(delta)) / 2 / CC); 00567 d = (xx - r * cos(mytheta1)) * (xx - r * cos(mytheta1)); 00568 e = yy * yy; 00569 f = (zz - r * sin(mytheta1)) * (zz - r * sin(mytheta1)); 00570 l1 = sqrt(d + e + f); 00571 d = (xx - r * cos(mytheta1)) * (xx - r * cos(mytheta1)); 00572 e = yy * yy; 00573 f = (zz - r * sin(-mytheta1)) * (zz - r * sin(-mytheta1)); 00574 l2 = sqrt(d + e + f); 00575 d = (xx - r * cos(mytheta2)) * (xx - r * cos(mytheta2)); 00576 e = yy * yy; 00577 f = (zz - r * sin(mytheta2)) * (zz - r * sin(mytheta2)); 00578 l3 = sqrt(d + e + f); 00579 d = (xx - r * cos(mytheta2)) * (xx - r * cos(mytheta2)); 00580 e = yy * yy; 00581 f = (zz - r * sin(-mytheta2)) * (zz - r * sin(-mytheta2)); 00582 l4 = sqrt(d + e + f); 00583 mytheta1 = mytheta1 / 3.1415926 * 180; 00584 mytheta2 = mytheta2 / 3.1415926 * 180; 00585 if(abs(l1 - l) <= 0.0001) 00586 { 00587 theta4 = mytheta1; 00588 } 00589 else if(abs(l2 - l) <= 0.0001) 00590 { 00591 theta4 = -mytheta1; 00592 } 00593 else if(abs(l4 - l) <= 0.0001) 00594 { 00595 theta4 = mytheta2; 00596 } 00597 else 00598 { 00599 theta4 = -mytheta2; 00600 } 00601 //点5 00602 MatrixSub(&topPlatform, 5, 1, 5, 4, &temp1); //等效于bottomPlatform(6,:) = (Rc * Rc * bottomPlatform(3, :)')'; 00603 MatrixTransposition(&temp1, &temp2); 00604 MatrixDot(&Rc, &temp2, &temp1); 00605 MatrixTransposition(&temp1, &temp2); 00606 xx = temp2.Elements[f2(1,1,temp2.Size[1])] - bottomPlatform.Elements[f2(1,1,bottomPlatform.Size[1])]; 00607 yy = temp2.Elements[f2(1,2,temp2.Size[1])] - bottomPlatform.Elements[f2(1,2,bottomPlatform.Size[1])]; 00608 zz = temp2.Elements[f2(1,3,temp2.Size[1])] - bottomPlatform.Elements[f2(1,3,bottomPlatform.Size[1])]; 00609 AA = -(l * l - xx * xx - yy * yy - r * r - zz * zz) / (2 * r * zz); 00610 BB = 2 * r * xx / (2 * r * zz); 00611 CC = BB * BB + 1; 00612 DD = 2 * AA * BB; 00613 EE = AA * AA - 1; 00614 delta = DD * DD - 4 * CC * EE; 00615 mytheta1 = acos((-DD + sqrt(delta)) / 2 / CC); 00616 mytheta2 = acos((-DD - sqrt(delta)) / 2 / CC); 00617 d = (xx + r * cos(mytheta1)) * (xx + r * cos(mytheta1)); 00618 e = yy * yy; 00619 f = (zz - r * sin(mytheta1)) * (zz - r * sin(mytheta1)); 00620 l1 = sqrt(d + e + f); 00621 d = (xx + r * cos(mytheta1)) * (xx + r * cos(mytheta1)); 00622 e = yy * yy; 00623 f = (zz - r * sin(-mytheta1)) * (zz - r * sin(-mytheta1)); 00624 l2 = sqrt(d + e + f); 00625 d = (xx + r * cos(mytheta2)) * (xx + r * cos(mytheta2)); 00626 e = yy * yy; 00627 f = (zz - r * sin(mytheta2)) * (zz - r * sin(mytheta2)); 00628 l3 = sqrt(d + e + f); 00629 d = (xx + r * cos(mytheta2)) * (xx + r * cos(mytheta2)); 00630 e = yy * yy; 00631 f = (zz - r * sin(-mytheta2)) * (zz - r * sin(-mytheta2)); 00632 l4 = sqrt(d + e + f); 00633 mytheta1 = mytheta1 / 3.1415926 * 180; 00634 mytheta2 = mytheta2 / 3.1415926 * 180; 00635 if(abs(l1 - l) <= 0.0001) 00636 { 00637 theta5 = mytheta1; 00638 } 00639 else if(abs(l2 - l) <= 0.0001) 00640 { 00641 theta5 = -mytheta1; 00642 } 00643 else if(abs(l4 - l) <= 0.0001) 00644 { 00645 theta5 = mytheta2; 00646 } 00647 else 00648 { 00649 theta5 = -mytheta2; 00650 } 00651 //点6 00652 MatrixSub(&topPlatform, 6, 1, 6, 4, &temp1); //等效于bottomPlatform(6,:) = (Rc * Rc * bottomPlatform(3, :)')'; 00653 MatrixTransposition(&temp1, &temp2); 00654 MatrixDot(&Rc, &temp2, &temp1); 00655 MatrixTransposition(&temp1, &temp2); 00656 xx = temp2.Elements[f2(1,1,temp2.Size[1])] - bottomPlatform.Elements[f2(2,1,bottomPlatform.Size[1])]; 00657 yy = temp2.Elements[f2(1,2,temp2.Size[1])] - bottomPlatform.Elements[f2(2,2,bottomPlatform.Size[1])]; 00658 zz = temp2.Elements[f2(1,3,temp2.Size[1])] - bottomPlatform.Elements[f2(2,3,bottomPlatform.Size[1])]; 00659 AA = -(l * l - xx * xx - yy * yy - r * r - zz * zz) / (2 * r * zz); 00660 BB = -2 * r * xx / (2 * r * zz); 00661 CC = BB * BB + 1; 00662 DD = 2 * AA * BB; 00663 EE = AA * AA - 1; 00664 delta = DD * DD - 4 * CC * EE; 00665 mytheta1 = acos((-DD + sqrt(delta)) / 2 / CC); 00666 mytheta2 = acos((-DD - sqrt(delta)) / 2 / CC); 00667 d = (xx - r * cos(mytheta1)) * (xx - r * cos(mytheta1)); 00668 e = yy * yy; 00669 f = (zz - r * sin(mytheta1)) * (zz - r * sin(mytheta1)); 00670 l1 = sqrt(d + e + f); 00671 d = (xx - r * cos(mytheta1)) * (xx - r * cos(mytheta1)); 00672 e = yy * yy; 00673 f = (zz - r * sin(-mytheta1)) * (zz - r * sin(-mytheta1)); 00674 l2 = sqrt(d + e + f); 00675 d = (xx - r * cos(mytheta2)) * (xx - r * cos(mytheta2)); 00676 e = yy * yy; 00677 f = (zz - r * sin(mytheta2)) * (zz - r * sin(mytheta2)); 00678 l3 = sqrt(d + e + f); 00679 d = (xx - r * cos(mytheta2)) * (xx - r * cos(mytheta2)); 00680 e = yy * yy; 00681 f = (zz - r * sin(-mytheta2)) * (zz - r * sin(-mytheta2)); 00682 l4 = sqrt(d + e + f); 00683 mytheta1 = mytheta1 / 3.1415926 * 180; 00684 mytheta2 = mytheta2 / 3.1415926 * 180; 00685 if(abs(l1 - l) <= 0.0001) 00686 { 00687 theta6 = mytheta1; 00688 } 00689 else if(abs(l2 - l) <= 0.0001) 00690 { 00691 theta6 = -mytheta1; 00692 } 00693 else if(abs(l4 - l) <= 0.0001) 00694 { 00695 theta6 = mytheta2; 00696 } 00697 else 00698 { 00699 theta6 = -mytheta2; 00700 } 00701 Platform->theta[0] = theta1; 00702 Platform->theta[1] = theta2; 00703 Platform->theta[2] = theta3; 00704 Platform->theta[3] = theta4; 00705 Platform->theta[4] = theta5; 00706 Platform->theta[5] = theta6; 00707 theta0 = atan(lengthOfCardan / lengthOfSteelWheel) / 3.1415926 * 180; 00708 Platform->theta_servo[0] = theta1 - theta0; 00709 Platform->theta_servo[1] = theta2 - theta0; 00710 Platform->theta_servo[2] = theta3 - theta0; 00711 Platform->theta_servo[3] = theta4 - theta0; 00712 Platform->theta_servo[4] = theta5 - theta0; 00713 Platform->theta_servo[5] = theta6 - theta0; 00714 } 00715 00716 00717 00718 //******************************************** 00719 //功能：角度制的三角函数 余弦 00720 //输入参数：angle：角度 00721 //输出参数：无 00722 //返回值：三角函数值 00723 //调用外部函数：无 00724 //作者：陈欢 h-che14@mails.tsinghua.edu.cn 00725 //******************************************** 00726 float cosd(float angle) 00727 { 00728 return cos(angle/180*3.1415926); 00729 } 00730 00731 //******************************************** 00732 //功能：角度制的三角函数 正弦 00733 //输入参数：angle：角度 00734 //输出参数：无 00735 //返回值：三角函数值 00736 //调用外部函数：无 00737 //作者：陈欢 h-che14@mails.tsinghua.edu.cn 00738 //******************************************** 00739 float sind(float angle) 00740 { 00741 return sin(angle/180*3.1415926); 00742 } 00743 00744 00745 //******************************************** 00746 //功能：在命令行打印矩阵 00747 //输入参数：A：要打印的矩阵 00748 //输出参数：无 00749 //返回值：无 00750 //调用外部函数：无 00751 //作者：陈欢 h-che14@mails.tsinghua.edu.cn 00752 //******************************************** 00753 void PrintMatrix(MatrixType* A) 00754 { 00755 int i, j; 00756 int pos2 = 0; 00757 printf("\r\nThe Matrix is:\r\n"); 00758 for(i = 1; i <= A->Size[0]; i++) 00759 { 00760 for(j = 1; j <= A->Size[1]; j++) 00761 { 00762 pos2 = f2(i,j,A->Size[1]); 00763 printf(" %f ", A->Elements[pos2]); 00764 } 00765 printf("\r\n"); 00766 } 00767 } 00768
Generated on Wed Jul 20 2022 23:55:24 by
1.7.2