create an array of matrixes forn derivates

Hello,

So, I have a cartesian path with 1x1500 robots poses between 2 waypoints (I'm using peter corke toolbox), and I want to derivate that path 2 times in order to achieve velocity and acceleration as well.

My issue comes when I try to create the array that will store velocity values, cause it is generating a 1x1500 cell with 5x 1500 points in each cell. I only want to have a 1x1500 cell with one array inside. This is my code so far:

%% Cart path generator
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;
for i=1:nSamples

    for j=1:nAxis

    XposRot{1,i}=[path(i).n,path(i).o,path(i).a,path(i).t];
    axisAngle{1,i}=rotm2axang(XposRot{1,i}(:,1:3));
    Wxdeltatheta{1,i}=axisAngle{1,i}(1,1:3)*axisAngle{1,i}(1,4);
    Xpos{1,i}=[XposRot{1,i}(:,4);Wxdeltatheta{1,i}'];

    Xvel{1,i}=diff(Xpos{1,i}(:,1))./diff(t);


    end
    
end
end

There is a possibility to manage this data into arrays instead of cells? I did some trials but nothing worked.

Thanks in advance!

Answer 1

[Vatsal]

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!