Code covered by the BSD License

# Dynamics Simulator for Kinematic Chains

### Bassam Jalgha (view profile)

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
%

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

%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

```