Code covered by the BSD License  

Highlights from
Dynamics Simulator for Kinematic Chains

image thumbnail

Dynamics Simulator for Kinematic Chains



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

%% Bassam Jalgha August 2011
% 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:
%for more info check

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


% 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.


%now we start the simulation computation loop

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

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

for i=1:2:step-1
    axis([-5 15 -5 15 -5 10]) %set x y z axis size according to the problem
    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
    hold on
    hold off


Contact us