Code covered by the BSD License

Modeling Flexible Bodies in SimMechanics

Dallas Kennedy (view profile)

08 May 2006 (Updated )

Technical paper and examples on modeling flexibility in SimMechanics.

Editor's Notes:

fea_to_statespace( modes , frequencies , damping , inputScaling , outputScaling )
```function [ ssObjectPVA , ssObjectP , A , B , C , D , C1 , D1 , C2 ] = fea_to_statespace( modes , frequencies , damping , inputScaling , outputScaling )
%FEA_TO_STATESPACE Data structures for using FEA results in state-space formalism
%
%  [ ssObject , A , B , C , D , C1 , D1 , C2 ] = fea_to_statespace( modes , frequencies , damping , inputScaling , outputScaling )
% ssObject implements the state-space system defined by the A, B, C, D matrices:
% dx/dt = A*x + B*v
% y     = C*x + D*v
%
% where v=Fu; that is, the matrix F selects to which nodes the input u
% corresponds. The output y in this formulation is a column vector of length
% 3*length(find(outputScaling)) consisting of all the positions, then all the
% velocities, and then all the accelerations of the nodes given by find(
% outputScaling ):
%
%    y = [ p1 ; p2 ; p3 ; ...
%          v1 ; v2 ; v3 ; ...
%          a1 ; a2 ; a3 ; ...
%          ]

% The state variables x correspond to the mode variables eta: x=[ eta ;
% d(eta)/dt].

% Copyright 2006, The MathWorks, Inc.

doNew = true;

% Get sizes
%

[ numNodes               , numModes               ] = size( modes );
[ inputScaling_numNodes  , numInputs              ] = size( inputScaling );
[ numOutputs             , outputScaling_numNodes ] = size( outputScaling );

[ frequencies_numModes ] = prod( size( frequencies ) );
[ damping_numModes     ] = prod( size( damping ) );

%
% Assert size consistency
%
if ...
( numNodes ~= inputScaling_numNodes  ) || ...
( numNodes ~= outputScaling_numNodes ) || ...
( numModes ~= frequencies_numModes   ) || ...
( numModes ~= damping_numModes       )

error( 'Inconsistent number of modes or nodes. Check matrices.' );
end

%
% Form matrices
%

Omega = diag( frequencies.^2 );
Beta  = diag( frequencies .*  damping );
Gamma  = diag( frequencies .*  damping );
Sigma = modes' * inputScaling ;
Theta = outputScaling * modes ;

A = [ zeros( [ numModes , numModes ] )     eye( [ numModes , numModes ] )
-Omega                               -Beta                           ];

B = [ zeros( [ numModes , numInputs ] )
Sigma                              ];

Cp = [ Theta       zeros( [ numOutputs , numModes ] )   ];

Dp = zeros( [ numOutputs , numInputs ] ) ;
ssObjectP = ss( A , B ,Cp ,Dp );

% new
zeroOutputModes  = zeros( [ numOutputs numModes ] );
zeroOutputInputs = zeros( [ numOutputs numInputs] );

Cpva = [ Theta            zeroOutputModes
zeroOutputModes  Theta
-Theta*Omega     -Theta*Gamma           ];
Dpva = [ zeroOutputInputs
zeroOutputInputs
Theta * Sigma ];
ssObjectPVA = ss( A , B , Cpva , Dpva ); % zeroing D

if doNew
C = -Cpva; D= Dpva; %%% zeroing this to avoid double counting
else
C=Cp;D=Dp;
end

%{

C1 = [ eyeModes   zeroModes
zeroModes  eyeModes
zeroModes  zeroModes ];

zeroModeInputs = zeros( [ numModes numInputs] );
eyeInputs      = eye( numInputs );
D1 = [ zeroModeInputs
zeroModeInputs
eyeInputs ];

C2 = Theta * ...
[ eyeModes   zeroModes   zeroModes
zeroModes  eyeModes    zeroModes
-Omega     -Gamma      Sigma ];

%}

%    ssObject = ss( A , B , C , D );

```