Numerical Differentiation in for loop

I have solved for the end point positions of an end effector of a robot and I am now looking to find the joint and end point velocities. I have created a for loop to solve for the thetas and endpoint position on the robot.

tt=t0:0.05:tf;
for ii=1:length(tt)
 
end

I ended up getting this but it does not work in the for loop. Rather it does after I take it out of the for loop and put it back in,but I cannot call on any of the instances. I used some form of diff() function but that didn't work either.

thetadot1 = gradient(theta1inv(:)) ./ gradient(tt(:));

I need it to work in the for loop because I am essentially trying to do this.

The theta and theta_dot are changing. I would like to determine endpoint and joint velocity and eventually call on them so I can plot velocity vs time for each.

[David Hill]

Please describe your inputs or provide and example of your inputs and explain what outputs you are trying to get.

[David Fariyike]

I have a 3 arm robot that operates on the XY plane. Essentially the robot's end effector is to rub against a quater circle wall. The first for loop gives the theta of the end effector. The second for loop solves for 3 thetas of the arms.

%% Planar 3R robotinverse kinematics HW03

a1=0.2;
a2=0.15;
a3=0.12;
d1=0.1;
d2=0.1;
d3=0.1;

%% inverse kinematics

%desired end point location and orientation
t0=0;
tf=2;


thetat0=0/360*2*pi;
thetatf=-90/360*2*pi;
delta=0.05;

% velocity at 2*delta and tf=2*delta
dthetadelta=(thetatf-thetat0)/(tf-t0-2*delta);


% position at 2*delta
theta_2delta=thetat0+dthetadelta*delta;


% position at tf-2*delta
theta_tfm2delta=thetatf-dthetadelta*delta;



% generating 4th order poly for 0-2delta, constant velocity for 2deta-(tf-2delta)
% and 4th order for (tf-2delta) to tf 

tt=t0:0.05:tf;

for ii=1:length(tt),
    
if tt(ii)<2*delta, %first 4-th poly
   thetaed(ii)=fourpolytraj(tt(ii),thetat0,theta_2delta,t0,2*delta,0,dthetadelta);
  
elseif tt(ii)>tf-2*delta, %last 4-th poly
    thetaed(ii)=fourpolytraj(tt(ii),theta_tfm2delta,thetatf,tf-2*delta,tf,dthetadelta,0);
   
else thetaed(ii)=thetat0+dthetadelta*(tt(ii)-delta);
 
    %constant velocity
end
end
%{
figure(1)
plot(tt,thetaed)
grid
%}

%determine O3 location for inverse kinematics
for ii=1:length(tt),
    R=.25;
    xed=.1+R*cos((-1*pi*tt)/4);
    yed=.25+R*sin((-1*pi*tt)/4);

x3posd=xed(ii)-a3*cos(thetaed(ii));
y3posd=yed(ii)-a3*sin(thetaed(ii));
p13=sqrt(x3posd^2+y3posd^2);
C2inv=(p13^2-a1^2-a2^2)/2/a1/a2;
C2=C2inv;
theta2invp=atan2(sqrt(1-((p13^2-a1^2-a2^2)/(2*a1*a2))^2),(p13^2-a1^2-a2^2)/(2*a1*a2));
theta2invm=-atan2(sqrt(1-((p13^2-a1^2-a2^2)/(2*a1*a2))^2),(p13^2-a1^2-a2^2)/(2*a1*a2));
theta2inv(ii)=theta2invm;% choose one
S2=sin(theta2inv(ii));
tmp=inv([a1+a2*C2 -a2*S2;a2*S2 a1+a2*C2])*[x3posd;y3posd];

theta1inv(ii)=atan2(tmp(2),tmp(1));
theta3inv(ii)=thetaed(ii)-theta1inv(ii)-theta2inv(ii);




S1=sin(theta1inv(ii));
S12=sin(theta1inv(ii)+theta2inv(ii));
S2=sin(theta2inv(ii));
S123=sin(theta1inv(ii)+theta2inv(ii)+theta3inv(ii));
S23=sin(theta2inv(ii)+theta3inv(ii));
S3=sin(theta3inv(ii));
C1=cos(theta1inv(ii));
C2=cos(theta2inv(ii));
C3=cos(theta3inv(ii));
C12=cos(theta1inv(ii)+theta2inv(ii));
C23=cos(theta2inv(ii)+theta3inv(ii));
C123=cos(theta1inv(ii)+theta2inv(ii)+theta3inv(ii));
    

    
end

theta1dsim=[tt',theta1inv'];
theta2dsim=[tt',theta2inv'];
theta3dsim=[tt',theta3inv'];

I am trying to solve theta_dot 1,2,3 or (change in theta)/(change in time) and endpoint velocity or (change in position)/(change in time).

[David Hill]

Are your inputs arrays? It would be beneficial to buffer and accumulate arrays of inputs at your sample rate. Processing would be much more efficient. I was assuming your inputs were arrays.

[David Fariyike]

Yes my inputs are arrays. For the 2nd for loop the inputs are endpoint data (position along quater circle and theta of end effector obtained from 1st for loop).

for ii=1:length(tt),
    R=.25;
    xed=.1+R*cos((-1*pi*tt)/4);
    yed=.25+R*sin((-1*pi*tt)/4);

I would also like for my differentiation to be an array so that I can call on it and plot all the instances.