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.

Tr=RobotPlot(DH,varargin)
```%% Bassam Jalgha Nov 2010
% RobotPlot
% This function plots the robotic arm manipulator
%
% Example Tr=RobotPlot(DH)
% where DH is the numerical (not symbolic) Denavit-Hatenberg matrix defined according to p71 of
% Craig book "Introduction to robotics"
% Tr is the resulting transformation matrix of the last frame with respect to the first frame.
%
%
%
% Example: Tr=RobotPlot(DH,joints,base,Twb)
% joints is the active joints represented in the DH matrix. base is the
% base frame of the manipulator. Twb is the transformation matrix of the
% base frame with respect to the world coordinates.

function Tr=RobotPlot(DH,varargin)

active_joint=zeros(1,size(DH,1));

Twb=[1 0 0 0; 0 1 0 0; 0 0 1 0; 0 0 0 1];
base=1;

if(isempty(varargin))
active_joint=ones(1,size(DH,1));
else
active_joint=varargin{1};
active_joint=[active_joint zeros(1,size(DH,1)-length(active_joint))]; %zero padding if there is missing joints

if(size(varargin,2)==2)
base=varargin{2};
end
if(size(varargin,2)==3)
base=varargin{2};
Twb=varargin{3};
end
end

hold on
magnitude=max(max(DH(:,2:3))); %this will set the scale for the initial frame and the joints axis display

%% plotting the initial frame

init_frame=[magnitude/10 magnitude/10 magnitude/10 1 ; 0 0 0 1];
plot3(init_frame(:,1),[0;0],[0;0],'r','LineWidth',3)
plot3([0;0],init_frame(:,2),[0;0],'g','LineWidth',3)
plot3([0;0],[0;0],init_frame(:,3),'b','LineWidth',3)

%% plotting the first joint in the z direction (DH rule)

init_joint=[0 0 0 1 ;0 0 magnitude/5 1];

%Ts1=FKinematics(DH,base,1);
%Ts1=eye(4);
for i=1:size(DH,1)
Tr=Twb*FKinematics(DH,1,i);
position=Tr*[0 0 0 1]';
x(1,i)=position(1);
y(1,i)=position(2);
z(1,i)=position(3);
if(active_joint(i))
result=Tr*init_joint';
joint=result';
plot3(joint(:,1),joint(:,2),joint(:,3),'m:','LineWidth',3)
%plot3(x(1,i),y(1,i),z(1,i),'ok','MarkerFaceColor','k')
end

end

plot3(x,y,z,'LineWidth',2)
%plot3(x(base),y(base),z(base),'go','LineWidth',2);

plot3(x(1,size(DH,1)),y(1,size(DH,1)),z(1,size(DH,1)),'ok','MarkerFaceColor','k')

hold off

end```