Info

This question is closed. Reopen it to edit or answer.

My code of Spline Cubic gives an error for velocity profil

3 views (last 30 days)
I have an error when i plot the curve of velocity with 05 knots, this is my code:
clear all ;close all; clc;
%%--------------- Data---------------------------------:
h=[2.5, 2.5, 2, 1];
% q=[ 3, -2, -5, 0, 6, 12, 8];
q(1)=3; q(3)=-2; q(5)=8;
dqm=-3; ddqm=0;
dq1=2; ddq1=0;
m=5; % Number of knots
%%Calcul of matrix A and a vector B:
for i=1:m-2
for j=1:m-2
A(i,j)=0;
end
end
A(1,1)=3*h(1)+2*h(2)+(h(1)^2/h(2));
A(1,2)=h(2);
A(2,1)=h(2)-((h(1)^2)/h(2));
A(2,2)=2*(h(2)+h(3));
% m=10;
if m>4
A(2,3)=h(3);
A(m-3,m-4)=h(m-3);
A(m-3,m-3)=2*(h(m-2)+h(m-3));
A(m-3,m-2)=h(m-2)-((h(m-1))^2/h(m-2));
A(m-2,m-2)=3*h(m-1)+2*h(m-2)+(((h(m-1))^2)/(h(m-2)));
A(m-2,m-3)=h(m-2);
else
end
for i=4:m-3
A(i-1,i-2)=h(i-1);
A(i-1,i-1)=2*(h(i-1)+h(i));
A(i-1,i)=h(i)
end
% Calcul du vecteur B:
B(1)=6*((q(3)/h(2))+(q(1)/h(1)))-6*((1/h(1))+(1/h(2)))*(q(1)+(h(1)*dq1)+(((h(1)^2)/3)*ddq1))-(h(1)*ddq1);
B(2)=(6/h(2))*(q(1)+(h(1)*dq1)+((((h(1))^2)/3)*ddq1))+((6*q(4))/h(3))-6*((1/h(2))+(1/h(3)))*q(3);
% m=10;
if m>4
B(m-3)=(6/h(m-2))*(q(m)-(h(m-1)*dqm)+((((h(m-1))^2)/3)*ddqm))-6*((1/h(m-2))+(1/h(m-3)))*q(m-2)+((6*q(m-3))/h(m-3));
B(m-2)=-6*((1/h(m-1))+(1/h(m-2)))*(q(m)-(h(m-1)*dqm)+((((h(m-1))^2)/3)*ddqm))+6*((q(m)/h(m-1))+(q(m-2)/h(m-2)))-(h(m-1)*ddqm);
% B(7)
% B(8)
else
end
for i=4:m-3
B(i-1)=6*(((q(i+1)-q(i))/h(i))-((q(i)-q(i-1))/h(i-1)));
% disp('%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%')
end
B=B';
A;
ddQi=inv(A)*B;
ddq=[0; ddQi; 0];
% disp('%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%')
q2=q(1)+(h(1)*dq1)+((((h(1))^2)/3)*ddq(1))+(((h(1))^2/6)*ddq(2));
q4=q(5)-(h(4)*dqm)+((((h(4))^2)/3)*ddqm)+(((h(4)^2)/6)*ddq(4));
q_i=[q(1) q2 q(3) q4 q(5)];
ddQ_ti=ddq';
t_i1=0; i=1; pas=0; t=0;
for j=1:length(h)
pas=pas+h(j);
end
for z=1:length(h)
% display('*********new trajectory (z)*********')
z;
t_i1=t_i1+h(z);
% pas=h(z)/2
pas=h(z)/10;
ti=t;
for t=t:pas:t_i1
% display('*********new pices of trajectory (i)*********')
i;
t_i1;
t;
ti;
h(z);
ddQ_ti(z);
ddQ_ti(z+1);
q_i(z);
q_i(z+1);
interval=h(z);
temps(i)=t;
qt(i)=((ddQ_ti(z)/(6.*h(z))).*(t_i1-t).^3)...
+((ddQ_ti(z+1)/(6.*h(z))).*(t-ti).^3)...
+(((q_i(z+1)/h(z))-((h(z).*ddQ_ti(z+1))/6)).*(t-ti))...
+(((q_i(z)/h(z))-((h(z).*ddQ_ti(z))/6)).*(t_i1-t));
dqt(i)=(((-ddQ_ti(z))/(2*h(z))).*((t_i1-t).^2))...
+((ddQ_ti(z+1)/(2.*h(z))).*((t-ti).^2))...
+((q_i(z+1)/h(z))-((h(z).*ddQ_ti(z+1))/6))...
-((q_i(z)/h(z))-((h(z).*ddQ_ti(z))/6));
ddqt(i)=((((t_i1-t)/h(z)).*ddQ_ti(z))...
+(((t-ti)/h(z)).*ddQ_ti(z+1)));
i=i+1;
end
% display('***********************')
end
figure
[temps',qt',dqt',ddqt',dddqt']
plot(temps,qt,'b','LineWidth',2); grid on; hold on;
plot(temps,dqt,'r','LineWidth',2); grid on; hold on;
plot(temps,ddqt,'g','LineWidth',2); grid on; hold on;
Can you help me to correct the profil of velocity Thank tou

Answers (0)

This question is closed.

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!