Chen Huan
/
StewartOlatform
StewartOlatform
StewartPlatform.cpp
- Committer:
- heroistired
- Date:
- 2017-10-11
- Revision:
- 0:2b80f11eb1d3
File content as of revision 0:2b80f11eb1d3:
#include "StewartPlatform.h" //******************************************** //功能:计算矩阵乘法 C=A*B //输入参数:A、B:参加运算的矩阵 //输出参数:C:运算结果 //返回值:计算是否成功 成功返回0 否则返回1 //调用外部函数:无 //作者:陈欢 h-che14@mails.tsinghua.edu.cn //******************************************** int MatrixDot(MatrixType* A, MatrixType* B, MatrixType* C) { int i, j, k; //循环控制变量 int posA, posB, posC; //矩阵下标索引 //判断异常 if(A->Size[1] != B->Size[0]) //维度不匹配 return 1; if(A->Size[0] * B->Size[1] > 50) //C矩阵规模太大 return 1; //计算矩阵乘法 C->Size[0] = A->Size[0]; //得到C的行列数 C->Size[1] = B->Size[1]; for(i = 1; i <= C->Size[0]; i++) //行循环 { for(j = 1; j <= C->Size[1]; j++) //列循环 { posC = f2(i,j,C->Size[1]); C->Elements[posC] = 0; for(k = 1; k <= A->Size[1]; k++) //计算乘法结果 { posA = f2(i,k,A->Size[1]); posB = f2(k,j,B->Size[1]); C->Elements[posC] += A->Elements[posA] * B->Elements[posB]; } } } return 0; } //******************************************** //功能:计算矩阵转置 //输入参数:A:被转置的矩阵 //输出参数:B:转置后的矩阵 //返回值:无 //调用外部函数:无 //作者:陈欢 h-che14@mails.tsinghua.edu.cn //******************************************** void MatrixTransposition(MatrixType* A, MatrixType* B) { int i, j; int posA, posB; B->Size[0] = A->Size[1]; B->Size[1] = A->Size[0]; for(i = 1; i <= A->Size[0]; i++) { for(j = 1; j <= A->Size[1]; j++) { posA = f2(i,j,A->Size[1]); posB = f2(j,i,B->Size[1]); B->Elements[posB] = A->Elements[posA]; } } } //******************************************** //功能:获得矩阵的子阵 //输入参数:A:原矩阵 StartRow、StartColumn、EndRow、EndColumn:子阵起始元素 子阵终了元素 //输出参数:B:子阵 //返回值:无 //调用外部函数:无 //作者:陈欢 h-che14@mails.tsinghua.edu.cn //******************************************** void MatrixSub(MatrixType* A,int StartRow, int StartColumn, int EndRow, int EndColumn, MatrixType* B) { int i, j; //循环控制变量 int posA, posB; //矩阵索引 B->Size[0] = EndRow - StartRow + 1; //计算子阵的维度 B->Size[1] = EndColumn - StartColumn + 1; for(i = 1; i <= B->Size[0]; i++) { for(j = 1; j <= B->Size[1]; j++) { posA = f2(StartRow + i - 1, StartColumn + j - 1, A->Size[1]); posB = f2(i, j, B->Size[1]); B->Elements[posB] = A->Elements[posA]; } } } //******************************************** //功能:填充矩阵 将一个矩阵填充到另一个矩阵中 //输入参数:A:被填充的矩阵 Row、Column:矩阵填充的位置 B:要填充到被填充矩阵的矩阵 //输出参数:A:被填充的矩阵 //返回值:0 代表成功 1代表失败 //调用外部函数:无 //作者:陈欢 h-che14@mails.tsinghua.edu.cn //******************************************** int MatrixFill(MatrixType* A,int Row, int Column, MatrixType* B) { int i, j, posA, posB; if((Row + B->Size[0] - 1) > A->Size[0]) { return 1; } else if((Column + B->Size[1] - 1) > A->Size[1]) { return 1; } else { for(i = 1; i <= B->Size[0]; i++) { for(j = 1; j <= B->Size[1]; j++) { posA = f2(Row + i - 1, Column + j - 1, A->Size[1]); posB = f2(i, j, B->Size[1]); A->Elements[posA] = B->Elements[posB]; } } return 0; } } //******************************************** //功能:指定动平台变换矩阵参数x,y,z,a,b,c,计算动平台上的点A在绝对坐标系下的坐标B A可以是多个点 一行一个点 //输入参数:x,y,z,a,b,c:动平台变换矩阵参数 A:动平台上点的相对坐标 //输出参数:B:点在绝对坐标系下的坐标 //返回值:无 //调用外部函数:无 //作者:陈欢 h-che14@mails.tsinghua.edu.cn //******************************************** void Inverse(float x, float y, float z, float a, float b, float c, MatrixType* A, MatrixType* B) { int i; MatrixType T; //原点平移 MatrixType Ra; //俯仰 YAW MatrixType Rb; //横滚 ROLL MatrixType Rc; //偏航 PITCH MatrixType temp1, temp2, temp3; T.Size[0] = 4; T.Size[1] = 4; T.Elements[f2(1,1,T.Size[1])] = 1; T.Elements[f2(1,2,T.Size[1])] = 0; T.Elements[f2(1,3,T.Size[1])] = 0; T.Elements[f2(1,4,T.Size[1])] = x; T.Elements[f2(2,1,T.Size[1])] = 0; T.Elements[f2(2,2,T.Size[1])] = 1; T.Elements[f2(2,3,T.Size[1])] = 0; T.Elements[f2(2,4,T.Size[1])] = y; T.Elements[f2(3,1,T.Size[1])] = 0; T.Elements[f2(3,2,T.Size[1])] = 0; T.Elements[f2(3,3,T.Size[1])] = 1; T.Elements[f2(3,4,T.Size[1])] = z; T.Elements[f2(4,1,T.Size[1])] = 0; T.Elements[f2(4,2,T.Size[1])] = 0; T.Elements[f2(4,3,T.Size[1])] = 0; T.Elements[f2(4,4,T.Size[1])] = 1; Ra.Size[0] = 4; Ra.Size[1] = 4; Ra.Elements[f2(1,1,Ra.Size[1])] = 1; Ra.Elements[f2(1,2,Ra.Size[1])] = 0; Ra.Elements[f2(1,3,Ra.Size[1])] = 0; Ra.Elements[f2(1,4,Ra.Size[1])] = 0; Ra.Elements[f2(2,1,Ra.Size[1])] = 0; Ra.Elements[f2(2,2,Ra.Size[1])] = cosd(a); Ra.Elements[f2(2,3,Ra.Size[1])] = -sind(a); Ra.Elements[f2(2,4,Ra.Size[1])] = 0; Ra.Elements[f2(3,1,Ra.Size[1])] = 0; Ra.Elements[f2(3,2,Ra.Size[1])] = sind(a); Ra.Elements[f2(3,3,Ra.Size[1])] = cosd(a); Ra.Elements[f2(3,4,Ra.Size[1])] = 0; Ra.Elements[f2(4,1,Ra.Size[1])] = 0; Ra.Elements[f2(4,2,Ra.Size[1])] = 0; Ra.Elements[f2(4,3,Ra.Size[1])] = 0; Ra.Elements[f2(4,4,Ra.Size[1])] = 1; Rb.Size[0] = 4; Rb.Size[1] = 4; Rb.Elements[f2(1,1,Rb.Size[1])] = cosd(b); Rb.Elements[f2(1,2,Rb.Size[1])] = 0; Rb.Elements[f2(1,3,Rb.Size[1])] = sind(b); Rb.Elements[f2(1,4,Rb.Size[1])] = 0; Rb.Elements[f2(2,1,Rb.Size[1])] = 0; Rb.Elements[f2(2,2,Rb.Size[1])] = 1; Rb.Elements[f2(2,3,Rb.Size[1])] = 0; Rb.Elements[f2(2,4,Rb.Size[1])] = 0; Rb.Elements[f2(3,1,Rb.Size[1])] = -sind(b); Rb.Elements[f2(3,2,Rb.Size[1])] = 0; Rb.Elements[f2(3,3,Rb.Size[1])] = cosd(b); Rb.Elements[f2(3,4,Rb.Size[1])] = 0; Rb.Elements[f2(4,1,Rb.Size[1])] = 0; Rb.Elements[f2(4,2,Rb.Size[1])] = 0; Rb.Elements[f2(4,3,Rb.Size[1])] = 0; Rb.Elements[f2(4,4,Rb.Size[1])] = 1; Rc.Size[0] = 4; Rc.Size[1] = 4; Rc.Elements[f2(1,1,Rc.Size[1])] = cosd(c); Rc.Elements[f2(1,2,Rc.Size[1])] = -sind(c); Rc.Elements[f2(1,3,Rc.Size[1])] = 0; Rc.Elements[f2(1,4,Rc.Size[1])] = 0; Rc.Elements[f2(2,1,Rc.Size[1])] = sind(c); Rc.Elements[f2(2,2,Rc.Size[1])] = cosd(c); Rc.Elements[f2(2,3,Rc.Size[1])] = 0; Rc.Elements[f2(2,4,Rc.Size[1])] = 0; Rc.Elements[f2(3,1,Rc.Size[1])] = 0; Rc.Elements[f2(3,2,Rc.Size[1])] = 0; Rc.Elements[f2(3,3,Rc.Size[1])] = 1; Rc.Elements[f2(3,4,Rc.Size[1])] = 0; Rc.Elements[f2(4,1,Rc.Size[1])] = 0; Rc.Elements[f2(4,2,Rc.Size[1])] = 0; Rc.Elements[f2(4,3,Rc.Size[1])] = 0; Rc.Elements[f2(4,4,Rc.Size[1])] = 1; B->Size[0] = A->Size[0]; B->Size[1] = A->Size[1]; MatrixDot(&T, &Rc, &temp1); //相当于T * Rc * Rb * Ra MatrixDot(&temp1, &Rb, &temp2); MatrixDot(&temp2, &Ra, &temp1); for(i = 1; i <= B->Size[0]; i++) { MatrixSub(A, i, 1, i, A->Size[1], &temp2); MatrixTransposition(&temp2, &temp3); MatrixDot(&temp1, &temp3, &temp2); MatrixTransposition(&temp2, &temp3); MatrixFill(B, i, 1, &temp3); } } //******************************************** //功能:计算矩阵行向量所表示的坐标点之间的距离 //输入参数:A, B:要计算距离的矩阵 //输出参数:C:包含距离值信息的列向量 //返回值:无 //调用外部函数:无 //作者:陈欢 h-che14@mails.tsinghua.edu.cn //******************************************** int Distance2Point(MatrixType* A, MatrixType* B, MatrixType* C) { float distance = 0; int i = 0, j = 0; if((A->Size[0] != B->Size[0]) || (A->Size[1] != 4) || (B->Size[1] != 4)) { return 1; } else { C->Size[0] = A->Size[0]; C->Size[1] = 1; for(i = 1; i <= A->Size[0]; i++) { for(j = 1; j <= A->Size[1]; j++) { 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])]); } C->Elements[f2(i,1,C->Size[1])] = sqrt(distance); distance = 0; } return 0; } } //******************************************** //功能:解析动感平台 //输入参数:Platform:动感平台数据结构 包含各种输入输出 //输出参数:无 //返回值:无 //调用外部函数:无 //作者:陈欢 h-che14@mails.tsinghua.edu.cn //******************************************** void CalStewartPlatform(StewartPlatformType* Platform) { int x, y, z, a, b, c; //动平台变换矩阵参数 float xx, yy, zz, r, l, AA, BB, CC, DD, EE, delta, mytheta1, mytheta2, d, e, f, l1, l2, l3, l4; //计算电机角度用的参数 float theta1, theta2, theta3, theta4, theta5, theta6; float topRadius, topInterval, bottomRadius, bottomInterval, lengthOfSteelWheel, lengthOfCardan, lengthOfBar; //平台机械尺寸定义 float theta0; //舵盘结构的倾角 MatrixType temp1, temp2; MatrixType topPlatform; //动平台上的6个参考点 MatrixType Rc; MatrixType bottomPlatform; //定平台上的6个参考点 MatrixType lengthOfBar1; topRadius = Platform->topRadius; //平台参数初始化 topInterval = Platform->topInterval; bottomRadius = Platform->bottomRadius; bottomInterval = Platform->bottomInterval; lengthOfSteelWheel = Platform->lengthOfSteelWheel; lengthOfCardan = Platform->lengthOfCardan; lengthOfBar = Platform->lengthOfBar; topPlatform.Size[0] = 6; topPlatform.Size[1] = 4; topPlatform.Elements[f2(1,1,topPlatform.Size[1])] = -topInterval / 2; topPlatform.Elements[f2(1,2,topPlatform.Size[1])] = -topRadius; topPlatform.Elements[f2(1,3,topPlatform.Size[1])] = 0; topPlatform.Elements[f2(1,4,topPlatform.Size[1])] = 1; topPlatform.Elements[f2(2,1,topPlatform.Size[1])] = topInterval / 2; topPlatform.Elements[f2(2,2,topPlatform.Size[1])] = -topRadius; topPlatform.Elements[f2(2,3,topPlatform.Size[1])] = 0; topPlatform.Elements[f2(2,4,topPlatform.Size[1])] = 1; Rc.Size[0] = 4; Rc.Size[1] = 4; Rc.Elements[f2(1,1,Rc.Size[1])] = cosd(120); Rc.Elements[f2(1,2,Rc.Size[1])] = -sind(120); Rc.Elements[f2(1,3,Rc.Size[1])] = 0; Rc.Elements[f2(1,4,Rc.Size[1])] = 0; Rc.Elements[f2(2,1,Rc.Size[1])] = sind(120); Rc.Elements[f2(2,2,Rc.Size[1])] = cosd(120); Rc.Elements[f2(2,3,Rc.Size[1])] = 0; Rc.Elements[f2(2,4,Rc.Size[1])] = 0; Rc.Elements[f2(3,1,Rc.Size[1])] = 0; Rc.Elements[f2(3,2,Rc.Size[1])] = 0; Rc.Elements[f2(3,3,Rc.Size[1])] = 1; Rc.Elements[f2(3,4,Rc.Size[1])] = 0; Rc.Elements[f2(4,1,Rc.Size[1])] = 0; Rc.Elements[f2(4,2,Rc.Size[1])] = 0; Rc.Elements[f2(4,3,Rc.Size[1])] = 0; Rc.Elements[f2(4,4,Rc.Size[1])] = 1; MatrixSub(&topPlatform, 1, 1, 1, 4, &temp1); //等效于topPlatform(3,:) = (Rc * topPlatform(1, :)')'; MatrixTransposition(&temp1, &temp2); MatrixDot(&Rc, &temp2, &temp1); MatrixTransposition(&temp1, &temp2); MatrixFill(&topPlatform, 3, 1, &temp2); MatrixSub(&topPlatform, 2, 1, 2, 4, &temp1); //等效于topPlatform(4,:) = (Rc * topPlatform(2, :)')'; MatrixTransposition(&temp1, &temp2); MatrixDot(&Rc, &temp2, &temp1); MatrixTransposition(&temp1, &temp2); MatrixFill(&topPlatform, 4, 1, &temp2); MatrixSub(&topPlatform, 3, 1, 3, 4, &temp1); //等效于topPlatform(5,:) = (Rc * topPlatform(3, :)')'; MatrixTransposition(&temp1, &temp2); MatrixDot(&Rc, &temp2, &temp1); MatrixTransposition(&temp1, &temp2); MatrixFill(&topPlatform, 5, 1, &temp2); MatrixSub(&topPlatform, 4, 1, 4, 4, &temp1); //等效于topPlatform(5,:) = (Rc * topPlatform(3, :)')'; MatrixTransposition(&temp1, &temp2); MatrixDot(&Rc, &temp2, &temp1); MatrixTransposition(&temp1, &temp2); MatrixFill(&topPlatform, 6, 1, &temp2); x = Platform->x; y = Platform->y; z = Platform->z; a = Platform->a; b = Platform->b; c = Platform->c; Inverse(x, y, z, a, b, c, &topPlatform, &temp1); //计算出动平台参考点的实际位置 MatrixFill(&topPlatform, 1, 1, &temp1); bottomPlatform.Size[0] = 6; bottomPlatform.Size[1] = 4; bottomPlatform.Elements[f2(1,1,bottomPlatform.Size[1])] = -bottomInterval / 2; bottomPlatform.Elements[f2(1,2,bottomPlatform.Size[1])] = -bottomRadius; bottomPlatform.Elements[f2(1,3,bottomPlatform.Size[1])] = 0; bottomPlatform.Elements[f2(1,4,bottomPlatform.Size[1])] = 1; bottomPlatform.Elements[f2(2,1,bottomPlatform.Size[1])] = bottomInterval / 2; bottomPlatform.Elements[f2(2,2,bottomPlatform.Size[1])] = -bottomRadius; bottomPlatform.Elements[f2(2,3,bottomPlatform.Size[1])] = 0; bottomPlatform.Elements[f2(2,4,bottomPlatform.Size[1])] = 1; MatrixSub(&bottomPlatform, 1, 1, 1, 4, &temp1); //等效于bottomPlatform(3,:) = (Rc * bottomPlatform(1, :)')'; MatrixTransposition(&temp1, &temp2); MatrixDot(&Rc, &temp2, &temp1); MatrixTransposition(&temp1, &temp2); MatrixFill(&bottomPlatform, 3, 1, &temp2); MatrixSub(&bottomPlatform, 2, 1, 2, 4, &temp1); //等效于bottomPlatform(4,:) = (Rc * bottomPlatform(2, :)')'; MatrixTransposition(&temp1, &temp2); MatrixDot(&Rc, &temp2, &temp1); MatrixTransposition(&temp1, &temp2); MatrixFill(&bottomPlatform, 4, 1, &temp2); MatrixSub(&bottomPlatform, 3, 1, 3, 4, &temp1); //等效于bottomPlatform(5,:) = (Rc * bottomPlatform(3, :)')'; MatrixTransposition(&temp1, &temp2); MatrixDot(&Rc, &temp2, &temp1); MatrixTransposition(&temp1, &temp2); MatrixFill(&bottomPlatform, 5, 1, &temp2); MatrixSub(&bottomPlatform, 4, 1, 4, 4, &temp1); //等效于bottomPlatform(6,:) = (Rc * bottomPlatform(3, :)')'; MatrixTransposition(&temp1, &temp2); MatrixDot(&Rc, &temp2, &temp1); MatrixTransposition(&temp1, &temp2); MatrixFill(&bottomPlatform, 6, 1, &temp2); Distance2Point(&topPlatform, &bottomPlatform, &lengthOfBar1); Platform->BarLength[0] = lengthOfBar1.Elements[f2(1,1,lengthOfBar1.Size[1])]; //赋值计算出的杆长 Platform->BarLength[1] = lengthOfBar1.Elements[f2(2,1,lengthOfBar1.Size[1])]; Platform->BarLength[2] = lengthOfBar1.Elements[f2(3,1,lengthOfBar1.Size[1])]; Platform->BarLength[3] = lengthOfBar1.Elements[f2(4,1,lengthOfBar1.Size[1])]; Platform->BarLength[4] = lengthOfBar1.Elements[f2(5,1,lengthOfBar1.Size[1])]; Platform->BarLength[5] = lengthOfBar1.Elements[f2(6,1,lengthOfBar1.Size[1])]; //计算角度 r = sqrt(lengthOfCardan * lengthOfCardan + lengthOfSteelWheel * lengthOfSteelWheel); l = lengthOfBar; //点1 xx = topPlatform.Elements[f2(1,1,topPlatform.Size[1])] - bottomPlatform.Elements[f2(1,1,bottomPlatform.Size[1])]; yy = topPlatform.Elements[f2(1,2,topPlatform.Size[1])] - bottomPlatform.Elements[f2(1,2,bottomPlatform.Size[1])]; zz = topPlatform.Elements[f2(1,3,topPlatform.Size[1])] - bottomPlatform.Elements[f2(1,3,bottomPlatform.Size[1])]; AA = -(l * l - xx * xx - yy * yy - r * r - zz * zz) / (2 * r * zz); BB = 2 * r * xx / (2 * r * zz); CC = BB * BB + 1; DD = 2 * AA * BB; EE = AA * AA - 1; delta = DD * DD - 4 * CC * EE; mytheta1 = acos((-DD + sqrt(delta)) / 2 / CC); mytheta2 = acos((-DD - sqrt(delta)) / 2 / CC); d = (xx + r * cos(mytheta1)) * (xx + r * cos(mytheta1)); e = yy * yy; f = (zz - r * sin(mytheta1)) * (zz - r * sin(mytheta1)); l1 = sqrt(d + e + f); d = (xx + r * cos(mytheta1)) * (xx + r * cos(mytheta1)); e = yy * yy; f = (zz - r * sin(-mytheta1)) * (zz - r * sin(-mytheta1)); l2 = sqrt(d + e + f); d = (xx + r * cos(mytheta2)) * (xx + r * cos(mytheta2)); e = yy * yy; f = (zz - r * sin(mytheta2)) * (zz - r * sin(mytheta2)); l3 = sqrt(d + e + f); d = (xx + r * cos(mytheta2)) * (xx + r * cos(mytheta2)); e = yy * yy; f = (zz - r * sin(-mytheta2)) * (zz - r * sin(-mytheta2)); l4 = sqrt(d + e + f); mytheta1 = mytheta1 / 3.1415926 * 180; mytheta2 = mytheta2 / 3.1415926 * 180; if(abs(l1 - l) <= 0.0001) { theta1 = mytheta1; } else if(abs(l2 - l) <= 0.0001) { theta1 = -mytheta1; } else if(abs(l3 - l) <= 0.0001) { theta1 = mytheta2; } else { theta1 = -mytheta2; } //点2 xx = topPlatform.Elements[f2(2,1,topPlatform.Size[1])] - bottomPlatform.Elements[f2(2,1,bottomPlatform.Size[1])]; yy = topPlatform.Elements[f2(2,2,topPlatform.Size[1])] - bottomPlatform.Elements[f2(2,2,bottomPlatform.Size[1])]; zz = topPlatform.Elements[f2(2,3,topPlatform.Size[1])] - bottomPlatform.Elements[f2(2,3,bottomPlatform.Size[1])]; AA = -(l * l - xx * xx - yy * yy - r * r - zz * zz) / (2 * r * zz); BB = -2 * r * xx / (2 * r * zz); CC = BB * BB + 1; DD = 2 * AA * BB; EE = AA * AA - 1; delta = DD * DD - 4 * CC * EE; mytheta1 = acos((-DD + sqrt(delta)) / 2 / CC); mytheta2 = acos((-DD - sqrt(delta)) / 2 / CC); d = (xx - r * cos(mytheta1)) * (xx - r * cos(mytheta1)); e = yy * yy; f = (zz - r * sin(mytheta1)) * (zz - r * sin(mytheta1)); l1 = sqrt(d + e + f); d = (xx - r * cos(mytheta1)) * (xx - r * cos(mytheta1)); e = yy * yy; f = (zz - r * sin(-mytheta1)) * (zz - r * sin(-mytheta1)); l2 = sqrt(d + e + f); d = (xx - r * cos(mytheta2)) * (xx - r * cos(mytheta2)); e = yy * yy; f = (zz - r * sin(mytheta2)) * (zz - r * sin(mytheta2)); l3 = sqrt(d + e + f); d = (xx - r * cos(mytheta2)) * (xx - r * cos(mytheta2)); e = yy * yy; f = (zz - r * sin(-mytheta2)) * (zz - r * sin(-mytheta2)); l4 = sqrt(d + e + f); mytheta1 = mytheta1 / 3.1415926 * 180; mytheta2 = mytheta2 / 3.1415926 * 180; if(abs(l1 - l) <= 0.0001) { theta2 = mytheta1; } else if(abs(l2 - l) <= 0.0001) { theta2 = -mytheta1; } else if(abs(l4 - l) <= 0.0001) { theta2 = mytheta2; } else { theta2 = -mytheta2; } //点3 MatrixSub(&topPlatform, 3, 1, 3, 4, &temp1); //等效于bottomPlatform(6,:) = (Rc * Rc * bottomPlatform(3, :)')'; MatrixTransposition(&temp1, &temp2); MatrixDot(&Rc, &temp2, &temp1); MatrixDot(&Rc, &temp1, &temp2); MatrixTransposition(&temp2, &temp1); xx = temp1.Elements[f2(1,1,temp1.Size[1])] - bottomPlatform.Elements[f2(1,1,bottomPlatform.Size[1])]; yy = temp1.Elements[f2(1,2,temp1.Size[1])] - bottomPlatform.Elements[f2(1,2,bottomPlatform.Size[1])]; zz = temp1.Elements[f2(1,3,temp1.Size[1])] - bottomPlatform.Elements[f2(1,3,bottomPlatform.Size[1])]; AA = -(l * l - xx * xx - yy * yy - r * r - zz * zz) / (2 * r * zz); BB = 2 * r * xx / (2 * r * zz); CC = BB * BB + 1; DD = 2 * AA * BB; EE = AA * AA - 1; delta = DD * DD - 4 * CC * EE; mytheta1 = acos((-DD + sqrt(delta)) / 2 / CC); mytheta2 = acos((-DD - sqrt(delta)) / 2 / CC); d = (xx + r * cos(mytheta1)) * (xx + r * cos(mytheta1)); e = yy * yy; f = (zz - r * sin(mytheta1)) * (zz - r * sin(mytheta1)); l1 = sqrt(d + e + f); d = (xx + r * cos(mytheta1)) * (xx + r * cos(mytheta1)); e = yy * yy; f = (zz - r * sin(-mytheta1)) * (zz - r * sin(-mytheta1)); l2 = sqrt(d + e + f); d = (xx + r * cos(mytheta2)) * (xx + r * cos(mytheta2)); e = yy * yy; f = (zz - r * sin(mytheta2)) * (zz - r * sin(mytheta2)); l3 = sqrt(d + e + f); d = (xx + r * cos(mytheta2)) * (xx + r * cos(mytheta2)); e = yy * yy; f = (zz - r * sin(-mytheta2)) * (zz - r * sin(-mytheta2)); l4 = sqrt(d + e + f); mytheta1 = mytheta1 / 3.1415926 * 180; mytheta2 = mytheta2 / 3.1415926 * 180; if(abs(l1 - l) <= 0.0001) { theta3 = mytheta1; } else if(abs(l2 - l) <= 0.0001) { theta3 = -mytheta1; } else if(abs(l4 - l) <= 0.0001) { theta3 = mytheta2; } else { theta3 = -mytheta2; } //点4 MatrixSub(&topPlatform, 4, 1, 4, 4, &temp1); //等效于bottomPlatform(6,:) = (Rc * Rc * bottomPlatform(3, :)')'; MatrixTransposition(&temp1, &temp2); MatrixDot(&Rc, &temp2, &temp1); MatrixDot(&Rc, &temp1, &temp2); MatrixTransposition(&temp2, &temp1); xx = temp1.Elements[f2(1,1,temp1.Size[1])] - bottomPlatform.Elements[f2(2,1,bottomPlatform.Size[1])]; yy = temp1.Elements[f2(1,2,temp1.Size[1])] - bottomPlatform.Elements[f2(2,2,bottomPlatform.Size[1])]; zz = temp1.Elements[f2(1,3,temp1.Size[1])] - bottomPlatform.Elements[f2(2,3,bottomPlatform.Size[1])]; AA = -(l * l - xx * xx - yy * yy - r * r - zz * zz) / (2 * r * zz); BB = -2 * r * xx / (2 * r * zz); CC = BB * BB + 1; DD = 2 * AA * BB; EE = AA * AA - 1; delta = DD * DD - 4 * CC * EE; mytheta1 = acos((-DD + sqrt(delta)) / 2 / CC); mytheta2 = acos((-DD - sqrt(delta)) / 2 / CC); d = (xx - r * cos(mytheta1)) * (xx - r * cos(mytheta1)); e = yy * yy; f = (zz - r * sin(mytheta1)) * (zz - r * sin(mytheta1)); l1 = sqrt(d + e + f); d = (xx - r * cos(mytheta1)) * (xx - r * cos(mytheta1)); e = yy * yy; f = (zz - r * sin(-mytheta1)) * (zz - r * sin(-mytheta1)); l2 = sqrt(d + e + f); d = (xx - r * cos(mytheta2)) * (xx - r * cos(mytheta2)); e = yy * yy; f = (zz - r * sin(mytheta2)) * (zz - r * sin(mytheta2)); l3 = sqrt(d + e + f); d = (xx - r * cos(mytheta2)) * (xx - r * cos(mytheta2)); e = yy * yy; f = (zz - r * sin(-mytheta2)) * (zz - r * sin(-mytheta2)); l4 = sqrt(d + e + f); mytheta1 = mytheta1 / 3.1415926 * 180; mytheta2 = mytheta2 / 3.1415926 * 180; if(abs(l1 - l) <= 0.0001) { theta4 = mytheta1; } else if(abs(l2 - l) <= 0.0001) { theta4 = -mytheta1; } else if(abs(l4 - l) <= 0.0001) { theta4 = mytheta2; } else { theta4 = -mytheta2; } //点5 MatrixSub(&topPlatform, 5, 1, 5, 4, &temp1); //等效于bottomPlatform(6,:) = (Rc * Rc * bottomPlatform(3, :)')'; MatrixTransposition(&temp1, &temp2); MatrixDot(&Rc, &temp2, &temp1); MatrixTransposition(&temp1, &temp2); xx = temp2.Elements[f2(1,1,temp2.Size[1])] - bottomPlatform.Elements[f2(1,1,bottomPlatform.Size[1])]; yy = temp2.Elements[f2(1,2,temp2.Size[1])] - bottomPlatform.Elements[f2(1,2,bottomPlatform.Size[1])]; zz = temp2.Elements[f2(1,3,temp2.Size[1])] - bottomPlatform.Elements[f2(1,3,bottomPlatform.Size[1])]; AA = -(l * l - xx * xx - yy * yy - r * r - zz * zz) / (2 * r * zz); BB = 2 * r * xx / (2 * r * zz); CC = BB * BB + 1; DD = 2 * AA * BB; EE = AA * AA - 1; delta = DD * DD - 4 * CC * EE; mytheta1 = acos((-DD + sqrt(delta)) / 2 / CC); mytheta2 = acos((-DD - sqrt(delta)) / 2 / CC); d = (xx + r * cos(mytheta1)) * (xx + r * cos(mytheta1)); e = yy * yy; f = (zz - r * sin(mytheta1)) * (zz - r * sin(mytheta1)); l1 = sqrt(d + e + f); d = (xx + r * cos(mytheta1)) * (xx + r * cos(mytheta1)); e = yy * yy; f = (zz - r * sin(-mytheta1)) * (zz - r * sin(-mytheta1)); l2 = sqrt(d + e + f); d = (xx + r * cos(mytheta2)) * (xx + r * cos(mytheta2)); e = yy * yy; f = (zz - r * sin(mytheta2)) * (zz - r * sin(mytheta2)); l3 = sqrt(d + e + f); d = (xx + r * cos(mytheta2)) * (xx + r * cos(mytheta2)); e = yy * yy; f = (zz - r * sin(-mytheta2)) * (zz - r * sin(-mytheta2)); l4 = sqrt(d + e + f); mytheta1 = mytheta1 / 3.1415926 * 180; mytheta2 = mytheta2 / 3.1415926 * 180; if(abs(l1 - l) <= 0.0001) { theta5 = mytheta1; } else if(abs(l2 - l) <= 0.0001) { theta5 = -mytheta1; } else if(abs(l4 - l) <= 0.0001) { theta5 = mytheta2; } else { theta5 = -mytheta2; } //点6 MatrixSub(&topPlatform, 6, 1, 6, 4, &temp1); //等效于bottomPlatform(6,:) = (Rc * Rc * bottomPlatform(3, :)')'; MatrixTransposition(&temp1, &temp2); MatrixDot(&Rc, &temp2, &temp1); MatrixTransposition(&temp1, &temp2); xx = temp2.Elements[f2(1,1,temp2.Size[1])] - bottomPlatform.Elements[f2(2,1,bottomPlatform.Size[1])]; yy = temp2.Elements[f2(1,2,temp2.Size[1])] - bottomPlatform.Elements[f2(2,2,bottomPlatform.Size[1])]; zz = temp2.Elements[f2(1,3,temp2.Size[1])] - bottomPlatform.Elements[f2(2,3,bottomPlatform.Size[1])]; AA = -(l * l - xx * xx - yy * yy - r * r - zz * zz) / (2 * r * zz); BB = -2 * r * xx / (2 * r * zz); CC = BB * BB + 1; DD = 2 * AA * BB; EE = AA * AA - 1; delta = DD * DD - 4 * CC * EE; mytheta1 = acos((-DD + sqrt(delta)) / 2 / CC); mytheta2 = acos((-DD - sqrt(delta)) / 2 / CC); d = (xx - r * cos(mytheta1)) * (xx - r * cos(mytheta1)); e = yy * yy; f = (zz - r * sin(mytheta1)) * (zz - r * sin(mytheta1)); l1 = sqrt(d + e + f); d = (xx - r * cos(mytheta1)) * (xx - r * cos(mytheta1)); e = yy * yy; f = (zz - r * sin(-mytheta1)) * (zz - r * sin(-mytheta1)); l2 = sqrt(d + e + f); d = (xx - r * cos(mytheta2)) * (xx - r * cos(mytheta2)); e = yy * yy; f = (zz - r * sin(mytheta2)) * (zz - r * sin(mytheta2)); l3 = sqrt(d + e + f); d = (xx - r * cos(mytheta2)) * (xx - r * cos(mytheta2)); e = yy * yy; f = (zz - r * sin(-mytheta2)) * (zz - r * sin(-mytheta2)); l4 = sqrt(d + e + f); mytheta1 = mytheta1 / 3.1415926 * 180; mytheta2 = mytheta2 / 3.1415926 * 180; if(abs(l1 - l) <= 0.0001) { theta6 = mytheta1; } else if(abs(l2 - l) <= 0.0001) { theta6 = -mytheta1; } else if(abs(l4 - l) <= 0.0001) { theta6 = mytheta2; } else { theta6 = -mytheta2; } Platform->theta[0] = theta1; Platform->theta[1] = theta2; Platform->theta[2] = theta3; Platform->theta[3] = theta4; Platform->theta[4] = theta5; Platform->theta[5] = theta6; theta0 = atan(lengthOfCardan / lengthOfSteelWheel) / 3.1415926 * 180; Platform->theta_servo[0] = theta1 - theta0; Platform->theta_servo[1] = theta2 - theta0; Platform->theta_servo[2] = theta3 - theta0; Platform->theta_servo[3] = theta4 - theta0; Platform->theta_servo[4] = theta5 - theta0; Platform->theta_servo[5] = theta6 - theta0; } //******************************************** //功能:角度制的三角函数 余弦 //输入参数:angle:角度 //输出参数:无 //返回值:三角函数值 //调用外部函数:无 //作者:陈欢 h-che14@mails.tsinghua.edu.cn //******************************************** float cosd(float angle) { return cos(angle/180*3.1415926); } //******************************************** //功能:角度制的三角函数 正弦 //输入参数:angle:角度 //输出参数:无 //返回值:三角函数值 //调用外部函数:无 //作者:陈欢 h-che14@mails.tsinghua.edu.cn //******************************************** float sind(float angle) { return sin(angle/180*3.1415926); } //******************************************** //功能:在命令行打印矩阵 //输入参数:A:要打印的矩阵 //输出参数:无 //返回值:无 //调用外部函数:无 //作者:陈欢 h-che14@mails.tsinghua.edu.cn //******************************************** void PrintMatrix(MatrixType* A) { int i, j; int pos2 = 0; printf("\r\nThe Matrix is:\r\n"); for(i = 1; i <= A->Size[0]; i++) { for(j = 1; j <= A->Size[1]; j++) { pos2 = f2(i,j,A->Size[1]); printf(" %f ", A->Elements[pos2]); } printf("\r\n"); } }