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.

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

Contact us