Code covered by the BSD License

# Dynamics Simulator for Kinematic Chains

This is a library that performs a dynamics simulation of a robotic arm.

[M B C G]=ComputeDynamics(DH,T01,m,g)
%% Bassam Jalgha August 2011
% COMPUTE DYNAMICS FUNCTION
% This function computes the M B C and G matrices of the joint-space dynamic
% equation of a kinematic chain.
%
% [M B C G]=ComputeDynamics(DH,T01,m,g)
% Where DH is a matrix containing the DH parameters where each variable
% joint is a symbolic variable named 'jx' where x is 1,2,3,4 etc. (according to the order of joints)
%
% T01 is the location of the base frame wrt the world coordinates computed
% using T01=BuildTwb(position,angle); of the Fkinematics library
%
% m is a nxn matrix containing the masses of the links, and g is the
% gravity vector
%
%tau=M\ddot{\theta}+B\dot{\theta}+C\dot{\theta}+G
%where M is the joint-space intertia matrix
%B and C are the coriolis and centrifugal matrices
%G is the gravity vector
%
%The dynamic equation is found using the Euler-Langrange method
%(lagrangian)
% P.S. The mass of each link is assumed to be a point mass on the lower edge of each
% link, second moment of inertia is ignored. (to be improved)
%
% http://depotbassam.blogspot.com

function [M B C G]=ComputeDynamics(DH,T01,m,g)

symbolic=symvar(DH);

joint =0;

syms dummy

for i=1:length(symbolic)
name=char(symbolic(i));
if(name(1)=='j')
joint=joint+1;
j{i}=sym(strcat('j',num2str(joint)),'real');
jdot{i}=sym(strcat('dj',num2str(joint)),'real');
jddot{i}=sym(strcat('d2j',num2str(joint)),'real'); %% changed to cell because it works with fulldiff
% tau{i}=sym(strcat('tau',num2str(joint))); %% changed to cell because it works with fulldiff

end
end
L=0;
for n=1:size(DH,1)-1

T1n=FKinematics(DH,1,n+1);
T0n=T01*T1n;

pos=T0n(1:3,4);
vel=fulldiff(pos,j); % finds the derivative of the position with respect to t

k=0.5*m(n,n)*(vel'*vel); %the kinetic energy % need to think abt MASSSSSSSSS
k=vpa(k);
k=expand(k);
c = coeffs(k);

%I always need to round else I am ending up with extermely small
%numbers that become huge numbers upon inversion!!!!

x = 3; % decimal places to round off
small=c(round(c*10^x)==0); % works like find
%dynamics1
for i=1:length(small)
k = sym(strrep(char(k),char(small(i)),'0'));
end
k=simple(k);
k=simple(k);

u=m(n,n)*g'*pos; % the potential energy

%The lagrangian
L=L+(k-u);
end

for n=1:size(DH,1)-1

dL_dj=diff(L,jdot{n});
ddL_dtdj=fulldiff(dL_dj,j);
dL_j=diff(L,j{n});

dynamics{n}=ddL_dtdj-dL_j;%Solving for the dynamics equation
dynamics{n}=vpa(dynamics{n}); %makes variale precision instead of fractions
dynamics{n}=expand(dynamics{n});

end

%Now we need to assemble the M B C and G matricies from the dynamics
%equation

for x=1:size(DH,1)-1 % loop in the equations
column=0;
dyna_new=dynamics{x};
for y=1:size(DH,1)-1 % loop in the terms

bla=coeffs(dyna_new,jddot{y});
if(size(bla,2)==1)
M(x,y)=sym(0);
else
M(x,y)=bla(2);
end

dyna_new=bla(1);

new=subs(dyna_new,jdot{y}^2,dummy,0);
bla=coeffs(new,dummy);
if(size(bla,2)==1)
C(x,y)=sym(0);
else
C(x,y)=bla(2);
end

dyna_new=bla(1);

for z=1:size(DH,1)-1
if y==z
continue
end
new=subs(dyna_new,jdot{y},dummy/jdot{z},0);
bla=coeffs(new,dummy);
column=column+1;
if(size(bla,2)==1)
B(x,column)=sym(0);
else
B(x,column)=bla(2);
end
dyna_new=bla(1);
end
end
G(x,1)=dyna_new;
end

M=simple(M);
B=simple(B);
C=simple(C);
G=simple(G);