[Robotica] Pianificazione traiettorie spazio operativo

[cyrus2]

Buonasera, devo realizzare una funzione per la pianificazione di traiettorie del manipolatore nello spazio operativo.I dati in ingresso sono la posizione iniziale e finale e il tempo finale, le velocità iniziali e finali devono essere nulle e anche le accelerazioni iniziali e finali, l'ascissa curvilinea deve avere una legge oraria con profilo trapezoidale e devo utilizzare il segmento come primitiva geometrica per la posizione. Questa è la funzione:
function funzione(pi,pf, tf)

vc=6;

tc=(-norm(pf-pi)+vc*tf)/vc;

ac=vc/tc;

t=[0:0.01:tf];
for i=1:length(t)
if t(i)>=0 && t(i)<= tc
s(i)=ac*t(i)^2/2;
s2(i)=ac*t(i);
end
if t(i)> tc && t(i) <=tf-tc
s(i)=ac*tc*(t(i)-tc/2);
s2(i)=ac*tc;
end
if t(i)> tf-tc && t(i) <= tf
s(i)=(-ac*(tf-t(i))^2/2)+norm(pf-pi);
s2(i)=(ac*(tf-t(i)));
end

end

px= pi(1)+s*(pf(1)-pi(1))/norm(pf-pi);
py= pi(2)+s*(pf(2)-pi(2))/norm(pf-pi);
pz= pi(3)+s*(pf(3)-pi(3))/norm(pf-pi);
figure;
plot(t,s);
figure;
plot(t,s2);
figure;
plot3(px,py,pz);

end
Il problema è che, come si può osservare dai grafici non riesco ad ottenere la legge oraria corretta, ovvero non presenta il profilo trapezoidale, non capisco dove possa essere il problema. Spero possiate darmi alcune indicazioni.

[Quinzio]

Sei sicuro che venga rispettata la condizione $t_f > ("norm"(p_f-p_i))/v_c$. ?
Non e' che $t_c$ viene negativo ?

[cyrus2]

Era proprio quello l'errore, infatti tc veniva meno di tf/2 e quindi non mi trovavo con la legge orario. Dovevo proprio aggiungere la condizione norm(pf-pi)/tf< tc < 2*norm(pf-pi)/tf. Il grafico tridimensionale plot3(px,py,pz) è un segmento che unisce le posizioni inziali e finali, spero sia corretto.
Questo invece è la funzione per la pianificazione dell'orientamento:
function [Rit,Re]= point_to_point(Ri,Rf,tf, vc)

Rif=Ri'*Rf;

angolof=acos((Rif(1,1)+Rif(2,2)+Rif(3,3)-1)/2);

r=[Rif(3,2)-Rif(2,3);Rif(1,3)-Rif(3,1);Rif(2,1)-Rif(1,2)]*1/(2*sin(angolof));

if abs(vc)= 2*angolof/tf
disp("inserire valori corretti");

else


tc=(-angolof+vc*tf)/vc;

ac=vc/tc;

t=[0:0.05:tf];
for i=1:length(t)
if t(i)>=0 && t(i)<= tc
angolo(i)=ac*t(i)^2/2;
angolo2(i)=ac*t(i);
end
if t(i)> tc && t(i) <=tf-tc
angolo(i)=ac*tc*(t(i)-tc/2);
angolo2(i)=ac*tc;
end
if t(i)> tf-tc && t(i) <= tf
angolo(i)=(-ac*(t(i)-tf)^2/2)+angolof;
angolo2(i)=(ac*(tf-t(i)));
end

end

Rit=[r(1)^2*(1-cos(angolo))+cos(angolo) r(1)*r(2)*(1-cos(angolo))-r(3)*sin(angolo) r(1)*r(3)*(1-cos(angolo))+r(2)*sin(angolo);
r(1)*r(2)*(1-cos(angolo))+r(3)*sin(angolo) r(2)^2*(1-cos(angolo))+cos(angolo) r(2)*r(3)*(1-cos(angolo))-r(1)*sin(angolo);
r(1)*r(3)*(1-cos(angolo))-r(2)*sin(angolo) r(2)*r(3)*(1-cos(angolo))+r(1)*sin(angolo) r(3)^2*(1-cos(angolo))+cos(angolo);];


Re=Ri*Rit;
end
end
L'unica cosa che non ho capito è come rappresentare graficamente la traiettoria per l'orientamento.