% Inverse Dynamics for trajectory of 2R robot



function trajdyn(Tseg)
Tseg

%clear

clf


% polynomial for smooth motion

ps = [6 -15 10 0 0 0];

psd = polyder(ps);

psdd = polyder(psd);



% plot the smooth polynomial

tstar = linspace(0,1);

s = polyval(ps,tstar);

sd = polyval(psd,tstar);

sdd = polyval(psdd,tstar)/4;



figure(1)

whitebg('white')

plot(tstar,s,'b',tstar,sd,'r',tstar,sdd,'g')

axis([-.1 1.1 -2 2])

title('Trajectory Polynomial')

legend('Position','Velocity','Acceleration/4 ')

xlabel('t_star')

ylabel('s')





% start and finish angles in degrees and radians

tstartd = [30 -60]'

tendd = [80 -30]'

tstart = tstartd*pi/180;

tend = tendd*pi/180;



% timing parameters

%Tseg = 1;             % total segment time

dt = 0.05;           % real time interval between points

dtstar = dt/Tseg;   % normalized time interval



% initialize some storage arrays

torq = [];

torqi = [];

torqc = [];

torqg = [];

time = [];



% compute the torques

for t = 0:dtstar:1



  TH = tstart + polyval(ps,t)*(tend - tstart);

  THD = polyval(psd,t)*(tend - tstart)/Tseg;

  THDD = polyval(psdd,t)*(tend - tstart)/Tseg^2;



  % call the inverse dynamics.  This returns a column vector for each

  % component (total, inertia torques, coriolis and centripetal, gravity)



  [tau taui tauc taug] = idyn2r(TH,THD,THDD);



  % store the torque and time values for plotting

  torq = [torq tau];

  torqi = [torqi taui];

  torqc = [torqc tauc];

  torqg = [torqg taug];

  time = [time t*Tseg];   % real time



end



% plot the torques



for j=1:2     % j = joint number

figure(1+j)

  plot(time, torq(j,:),'k')

  hold on

  plot(time, torqi(j,:),'r')

  plot(time, torqc(j,:),'b')

  plot(time, torqg(j,:),'g')

  if j==1 

    title('Joint #1 Torque')

  else

    title('Joint #2 Torque')

  end

  legend(' Total ',' Inertia ',' C & C ',' Gravity ')

  xlabel('Time (sec)')

  ylabel('Torque (N.m)')

end

