Code covered by the BSD License  

Highlights from
Dynamics Simulator for Kinematic Chains

image thumbnail

Dynamics Simulator for Kinematic Chains

by

 

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);

Contact us