B样条的姿态插值RPY插值

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轴');