clc;clear;close;
t= [0,5,7,8,10,15,18];
q= [3.31 -3.01 -1.07 4.48 1.52 ,5.53, 10.54, 15.55 ,20.56, 25.57, 30.52;-2.38 -3.53 5.81 2.97 -1.25, -1.25 ,-1.25 ,-1.25 ,-1.25 ,-1.25, -1.25;7.14 10.89 6.72 4.54 5.81 5.81 5.81 5.81 5.81 5.81 5.81]';
rpy=[0 0 -1.11 2.11 2.46 2.46 2.46 2.46 2.46 2.46 2.46;0 0.90 0.42 -0.33 -0.77 -0.77 -0.77 -0.77 -0.77 -0.77 -0.77;0 0 -0.69 0 -0.87 -0.87 -0.87 -0.87 -0.87 -0.87 -0.87;]'
n=size(q,1)-1;
p=4;
NodeVector=[0, 0, 0, 0, 0, 2.5, 6, 7.5, 9, 12.5, 16.5, 18, 18, 18, 18, 18];
v0=2;
vn=-3;
a0=0;
an=0;
Base = zeros(n+1, 1)';
for a=1:size(t,2)
for b=p+1:size(NodeVector,2)-p
if t(a)>=NodeVector(b) && t(a)<NodeVector(b+1)||(a==size(t,2)&&t(a)==NodeVector(b))
i=b;
end
end
u=t(a);
if u==18
i=i-1;
end
[B,D]= MyBaseFun(i, p , u, NodeVector);
Base(a,1:size(B,2))=B';
end
i=5;
u=t(1);
der=3;
[B,D]= MyBaseFun(i, p , u, NodeVector);
[Ders]=MyDersBasisFuns( i,u,p,B,D,der,NodeVector);
Ders=[Ders,zeros(size(Ders,1),size(Base,2)-size(Ders,2))];
i=11;
u=t(size(t,2));
der=3;
[B,D]= MyBaseFun(i, p , u, NodeVector);
[Ders2]=MyDersBasisFuns( i,u,p,B,D,der,NodeVector);
A=[Base(1,:);Ders(2,:);Ders(3,:);Base(2:size(Base,1)-1,:);Ders2(3,:);Ders2(2,:);Base(size(Base,1),:)];
Point=inv(A)*q;
Point=Point';
n=size(Point,2)-1;
d=1;
for u = min(t) :0.1: max(t)
for k=p+1:size(NodeVector,2)-p
if u>=NodeVector(k) && u<NodeVector(k+1)||(u==max(t))
i=k;
end
end
if u==18
i=i-1;
end
[B,D]= MyBaseFun(i, p , u, NodeVector);
[Ders]=MyDersBasisFuns( i,u,p,B,D,der,NodeVector);
Ders=[Ders,zeros(size(Ders,1),size(Base,2)-size(Ders,2))];
Base2(1:size(B,2),1)=B';
Base2=[Base2;zeros(size(Base,2)-size(Base2,1),1)];
Ptemp = Point * Base2;
Vtemp=Point*Ders(2,:)';
Atemp=Point*Ders(3,:)';
Jtemp=Point*Ders(4,:)';
ut(d)=u;
px(d)=Ptemp(1);
py(d)=Ptemp(2);
pz(d)=Ptemp(3);
d=d+1;
end
figure('name','BSpine位移曲线');
hold on
plot3(q(:,1)',q(:,2)',q(:,3)');
plot3(q(:,1)',q(:,2)',q(:,3)','o');
plot3(px,py,pz);
%%%%%%%%%%%%%%%%%%%%%%姿态插值
Base2=0;
Point=0;
t= [0,5,7,8,10,15,18];
q= [3.31 -3.01 -1.07 4.48 1.52 ,5.53, 10.54, 15.55 ,20.56, 25.57, 30.52;-2.38 -3.53 5.81 2.97 -1.25, -1.25 ,-1.25 ,-1.25 ,-1.25 ,-1.25, -1.25;7.14 10.89 6.72 4.54 5.81 5.81 5.81 5.81 5.81 5.81 5.81]';
rpy=[0 0 -1.11 2.11 2.46 2.46 2.46 2.46 2.46 2.46 2.46;0 0.90 0.42 -0.33 -0.77 -0.77 -0.77 -0.77 -0.77 -0.77 -0.77;0 0 -0.69 0 -0.87 -0.87 -0.87 -0.87 -0.87 -0.87 -0.87;]'
q=rpy;
n=size(q,1)-1;
p=4;
NodeVector=[0, 0, 0, 0, 0, 2.5, 6, 7.5, 9, 12.5, 16.5, 18, 18, 18, 18, 18];
v0=2;
vn=-3;
a0=0;
an=0;
Base = zeros(n+1, 1)';
for a=1:size(t,2)
for b=p+1:size(NodeVector,2)-p
if t(a)>=NodeVector(b) && t(a)<NodeVector(b+1)||(a==size(t,2)&&t(a)==NodeVector(b))
i=b;
end
end
u=t(a);
if u==18
i=i-1;
end
[B,D]= MyBaseFun(i, p , u, NodeVector);
Base(a,1:size(B,2))=B';
end
i=5;
u=t(1);
der=3;
[B,D]= MyBaseFun(i, p , u, NodeVector);
[Ders]=MyDersBasisFuns( i,u,p,B,D,der,NodeVector);
Ders=[Ders,zeros(size(Ders,1),size(Base,2)-size(Ders,2))];
i=11;
u=t(size(t,2));
der=3;
[B,D]= MyBaseFun(i, p , u, NodeVector);
[Ders2]=MyDersBasisFuns( i,u,p,B,D,der,NodeVector);
A=[Base(1,:);Ders(2,:);Ders(3,:);Base(2:size(Base,1)-1,:);Ders2(3,:);Ders2(2,:);Base(size(Base,1),:)];
Point=inv(A)*q;
Point=Point';
n=size(Point,2)-1;
d=1;
for u = min(t) :0.1: max(t)
for k=p+1:size(NodeVector,2)-p
if u>=NodeVector(k) && u<NodeVector(k+1)||(u==max(t))
i=k;
end
end
if u==18
i=i-1;
end
[B,D]= MyBaseFun(i, p , u, NodeVector);
[Ders]=MyDersBasisFuns( i,u,p,B,D,der,NodeVector);
Ders=[Ders,zeros(size(Ders,1),size(Base,2)-size(Ders,2))];
Base2(1:size(B,2),1)=B';
Base2=[Base2;zeros(size(Base,2)-size(Base2,1),1)];
Ptemp = Point * Base2;
Vtemp=Point*Ders(2,:)';
Atemp=Point*Ders(3,:)';
Jtemp=Point*Ders(4,:)';
ut(d)=u;
qx(d)=Ptemp(1);
qy(d)=Ptemp(2);
qz(d)=Ptemp(3);
R0=angle2dcm(Ptemp(1),Ptemp(2),Ptemp(3),'XYZ');
Pt=[px(d);py(d);pz(d)];
T=[R0,Pt];
T=[T;0 0 0 1];
PaintCoor( T );
d=d+1;
end
xlabel('X轴');
ylabel('Y轴');
zlabel('Z轴');