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.

X=SimulateRobot(Minv,B,C,G,b,previous_state,vtau,dt)
```%% Bassam Jalgha August 2011
% X=SimulateRobot(Minv,B,C,G,b,previous_step,vtau,dt)
% This function performs the computation of the simulation step for a
% kinematic chain.
% Minv is the inverse of the M matrix
% B C G are computed using ComputeDynamics
% b is an nxn matrix containing the damping ratio at each joint
% previous_state contains the previous state vector (2nx1)
% vtau contains an nx1 vector of torques/forces at each joint.
% dt is the time step.
% http://depotbassam.blogspot.com

function X=SimulateRobot(Minv,B,C,G,b,previous_state,vtau,dt)

NumberOfJoints=length(previous_state)/2;

%assemble joint names
for i=1:NumberOfJoints
j{i}=sym(strcat('j',num2str(i)),'real');
end

%syms j1 j2
mat=cat(2,zeros(NumberOfJoints,NumberOfJoints),eye(NumberOfJoints)); % for 2dof [0 0 1 0;0 0 0 1]

%get the joints speed
Xdot(1:NumberOfJoints,1) = mat*previous_state;

for i =1:NumberOfJoints
Minv=SYM_sub(Minv,j{i},previous_state(i));
B=SYM_sub(B,j{i},previous_state(i));
C=SYM_sub(C,j{i},previous_state(i));
G=SYM_sub(G,j{i},previous_state(i));
end

count=1;

%Bterm, Cterm and bterm are the multipliers of B C and b in the dynamic
%equation

for i=NumberOfJoints+1:length(previous_state)

if(i==length(previous_state))
Bterm(count,1) = previous_state(i)*previous_state(i-1); % to be reconsidered!!!!
else
Bterm(count,1) = previous_state(i)*previous_state(i+1);
end
count=count+1;
end

Cterm = previous_state(NumberOfJoints+1:length(previous_state)).^2;
bterm = previous_state(NumberOfJoints+1:length(previous_state));

Xdot(3:4,1)=Minv*(vtau-B*Bterm-C*Cterm-G-b*bterm);

X=Xdot*dt+previous_state; %Euler```