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.

Example_2DOF_Pendulum.m
%% Bassam Jalgha August 2011
% http://depotbassam.blogspot.com
%
% Dynamic Simulation of a 2DOF pendulim
% This script file simulates the dynamics of a 2 DOF pendulum. The purpose
% is to give an exmaple for the use of ComputeDynamics and SimulateRobot
% function. These functions are combined with the 
%"Simple Simple Forward Kinematics library for robotic chains" library  created in 2010
% and that can be found here:
% http://www.mathworks.com/matlabcentral/fileexchange/31170-simple-forward-kinematics-library-for-robotic-chains
% 
%for more info check http://depotbassam.blogspot.com



clc
clear all
close all


syms j1 j2 real; % the active joints need to be always named j1 j2 j3 j4 etc 
syms dj1 dj2 real; % these symbolic variables represent the speed of j1 j2 j3 etc and need to be also declared 
syms tau real; % this variable is used whenever there is torque/force input 

l=4; % the length of the bar

m=[1 0 ; 0 1]; % the mass of 1st and 2nd link for 2 DOF system

b=[1 0; 0 0.4]; % the damping coefficient for the 1st and 2nd link for 2 DOF system

g=[0;0;9.8];

% create a numerical or symbolic DH parameter
DH=[  0        0       0          j1;     %frame1
      0        l       0          j2;
      0        l/2      0          0];     %frame2
  
  
position=[6 6 2];  % the x,y,z position of the base frame with respect to world coordinates
angle=[0 90*pi/180 0];% the x,y,z angles of the base frame with respect to world coordinates

T01=BuildTwb(position,angle); %builds the transformation matrix of the base frame with respect to world coordinates




[M,B,C,G]=ComputeDynamics(DH,T01,m,g); % This function computes the M B C and G matrices of the general dynamics equation in joint space using the Euler-Lagrange method
%M is the joint-space intertia matrix


%initial condition of first joint
teta1 = 129*pi/180; %rad
dteta1= 20*pi/180; %rad/sec

%initial condition of second joint
teta2 = -45*pi/180;
dteta2= -25*pi/180;

vtau=[0;0]; %the vector containing the torques/forces at the joints. If there was any closed loop control it should go into this vector

dt=0.06; %time step
end_time=10; %simulation time


X(:,1)=[teta1;teta2;dteta1;dteta2]; %setting up the initial conditions

Minv=inv(M); % inverting the M matrix.


step=2;

%now we start the simulation computation loop

for i=0:dt:end_time
    disp('Computing Motion [%] : ')
    disp(i/end_time*100);
    X(:,step)=SimulateRobot(Minv,B,C,G,b,X(:,step-1),vtau,dt); %this function performs the dynamic simulation
    step=step+1;
    clc
end

save data X; %saves the mat file


% Displaying the result
joints=[1 1 0]; % the DH parameter frames that are active joints
base=1; %the base frame

counter=1;
for i=1:2:step-1
    clf
    axis([-5 15 -5 15 -5 10]) %set x y z axis size according to the problem
    view(3)
    view([1,0,0])
    grid
    daspect([1,1,1])
    camproj orthographic

    
    real_DH=SYM_sub(DH,j1,X(1,i),j2,X(2,i)); %subs each symbolic joint name with its numerical value
    Tr=RobotPlot(real_DH,joints,base,T01); % from the Fkinematics library
   
    pos(:,counter)=Tr(1:3,4);
    hold on
    plot3(pos(1,1:counter),pos(2,1:counter),pos(3,1:counter),'r');
    
    hold off
    %pause(0.1/2)
    drawnow
    counter=counter+1;
end

    

Contact us