clear
%下平台中各支点的位置，世界坐标系
%基本的设计参数
a1=[-52.0945,295.4423,0]';
a2=[52.0945,295.4423,0]';
a3=[281.9078,-102.6060,0]';
a4=[229.8133,-192.8363,0]';
a5=[-229.8133,-192.8363,0]';
a6=[-281.9078,-102.6060,0]';
%上平台中各支点的位置，世界坐标系
%基本的设计参数
b1=[-114.9067,96.4181,600]';
b2=[114.9067,96.4181,600]';
b3=[140.9539,51.3030,600]';
b4=[26.0472,-147.7212,600]';
b5=[-26.0472,-147.7212,600]';
b6=[-140.9539,51.3030,600]';
%上平台在下平台的投影
%请注意是投影不是b点在下平台坐标系中的坐标
%这样做是为了计算y在世界坐标系中的单位向量
bm1=[-114.9067,96.4181,0]';
bm2=[114.9067,96.4181,0]';
bm3=[140.9539,51.3030,0]';
bm4=[26.0472,-147.7212,0]';
bm5=[-26.0472,-147.7212,0]';
bm6=[-140.9539,51.3030,0]';
%计算各条退的长度，一个很重要的初始条件
leglen1=norm(b1-a1);
leglen2=norm(b2-a2);
leglen3=norm(b3-a3);
leglen4=norm(b4-a4);
leglen5=norm(b5-a5);
leglen6=norm(b6-a6);
%计算各条腿的单位向量
%计算刚体x轴在世界坐标系中的单位向量
x1=(b1-a1);x1=x1/norm(x1);
x2=(b2-a2);x2=x2/norm(x2);
x3=(b3-a3);x3=x3/norm(x3);
x4=(b4-a4);x4=x4/norm(x4);
x5=(b5-a5);x5=x5/norm(x5);
x6=(b6-a6);x6=x6/norm(x6);
%计算各条腿在下平面投影的向量
%为了计算刚体y轴在世界坐标系中的单位向量
xm1=(bm1-a1);
xm2=(bm2-a2);
xm3=(bm3-a3);
xm4=(bm4-a4);
xm5=(bm5-a5);
xm6=(bm6-a6);
%计算刚体y轴在世界坐标系中的单位向量
y1=cross(xm1,x1);y1=y1/norm(y1);
y2=cross(xm2,x2);y2=y2/norm(y2);
y3=cross(xm3,x3);y3=y3/norm(y3);
y4=cross(xm4,x4);y4=y4/norm(y4);
y5=cross(xm5,x5);y5=y5/norm(y5);
y6=cross(xm6,x6);y6=y6/norm(y6);
%计算刚体z轴在世界坐标系中的单位向量
z1=cross(x1,y1);z1=z1/norm(z1);
z2=cross(x2,y2);z2=z2/norm(z2);
z3=cross(x3,y3);z3=z3/norm(z3);
z4=cross(x4,y4);z4=z4/norm(z4);
z5=cross(x5,y5);z5=z5/norm(z5);
z6=cross(x6,y6);z6=z6/norm(z6);
%合成旋转矩阵
%最重要的结果
trans1=[x1,y1,z1];
trans2=[x2,y2,z2];
trans3=[x3,y3,z3];
trans4=[x4,y4,z4];
trans5=[x5,y5,z5];
trans6=[x6,y6,z6];
评论20
最新资源