Hi,
I understand that you need to calculate the velocity of a robot along a Cartesian path. To address the issue, the velocity calculation should be moved outside the inner loop. Below is the modified code:
clc;
close all;
clear all;
%% define variables
% time increment
deltaT=1/1499;
%% path
% starting pose
Tinit=SE3(0.0,0,0)*SE3.Rx(0);
% final pose
Tfinal=SE3(0.5,0.3,0.2)*SE3.Rx(pi/2*180/pi)*SE3.Ry(pi/2*180/pi);
t=[0:1/1499:1];
dt=diff(t);
ta = ((t(1:end-1)+t(2:end))/2);
dta=diff(ta);
path=ctraj(Tinit,Tfinal,t);
nAxis=6;
nSamples=1500;
Xpos = cell(1,nSamples);
for i=1:nSamples
XposRot=[path(i).n,path(i).o,path(i).a,path(i).t];
axisAngle=rotm2axang(XposRot(:,1:3));
Wxdeltatheta=axisAngle(1,1:3)*axisAngle(1,4);
Xpos{1,i}=[XposRot(:,4);Wxdeltatheta'];
end
Xvel = cell(1,nSamples-1);
for i=1:nSamples-1
Xvel{1,i}=diff(Xpos{1,i})./dt(i);
End
I hope this helps!
