function [SolutionToAlgebraicEquations,Output] = MGRobotStanfordArm3DInverseDynamics( qA, qADt, qADDt, qB, qBDt, qBDDt, qC, qCDt, qCDDt )
if( nargin ~= 9 ) error( 'MGRobotStanfordArm3DInverseDynamics expects 9 (not %d) command line arguments.', nargin ),  end
%===========================================================================
% File: MGRobotStanfordArm3DInverseDynamics.m created Apr 29 2020 by MotionGenesis 5.9.
% Portions copyright (c) 2009-2020 Motion Genesis LLC. are licensed under
% the 3-clause BSD license.  https://opensource.org/licenses/BSD-3-Clause.
% This copyright notice must appear in all copies and distributions.
% MotionGenesis Professional Licensee: Motion Genesis LLC.
%===========================================================================
%   To pass values for qA, qA', qA'', etc. to the MATLAB function created below,
%   Type, the following at the MATLAB command prompt:
%   data = {0, 0.1, 0.2,  pi/6, 0.3, 0.4,  pi/4, 0.5, 0.6};
%   Output = MGRobotStanfordArm3DInverseDynamics( data{:} );
%   TA = Output( 1 )
%   TB = Output( 2 )
%   TC = Output( 3 )
%===========================================================================


%-------------------------------+--------------------------+-------------------+-----------------
% Quantity                      | Value                    | Units             | Description
%-------------------------------|--------------------------|-------------------|-----------------
g                               =  9.8;                    % m/s^2               Constant
LB                              =  0.3;                    % m                   Constant
LC                              =  0.2;                    % m                   Constant
mA                              =  3;                      % kg                  Constant
mB                              =  2;                      % kg                  Constant
mC                              =  1;                      % kg                  Constant
%-------------------------------+--------------------------+-------------------+-----------------



%===========================================================================
IAzz = 0.0016*mA;
IBxx = 0.08333333333333333*mB*LB^2;
IByy = 0.0009*mB;
IBzz = IBxx;
ICxx = 0.08333333333333333*mC*LC^2;
ICyy = 0.0004*mC;
ICzz = ICxx;

COEF = zeros( 3, 3 );
COEF(1,1) = -1;
COEF(2,2) = -1;
COEF(3,3) = -1;
RHS = zeros( 1, 3 );
RHS(1) = sin(qB+qC)*(ICzz*cos(qB+qC)*qADt*(qBDt+qCDt)-ICyy*(cos(qC)*(cos(qB)*qADt*qBDt+cos(qB)*qADt*qCDt+sin(qB)*qADDt)-sin(qC)*(  ...
sin(qB)*qADt*qBDt+sin(qB)*qADt*qCDt-cos(qB)*qADDt))) - IAzz*qADDt - IByy*sin(qB)^2*qADDt - 0.25*cos(qB)*(8*IByy*sin(qB)*qADt*qBDt-  ...
4*IBzz*(2*sin(qB)*qADt*qBDt-cos(qB)*qADDt)-mB*LB^2*(2*sin(qB)*qADt*qBDt-cos(qB)*qADDt)-2*mC*LB*(2*LB*(2*sin(qB)*qADt*qBDt-cos(qB)*  ...
qADDt)+LC*(sin(qB+qC)*qADt*(qBDt+qCDt)+sin(qC)*(cos(qB)*qADt*qBDt+cos(qB)*qADt*qCDt+sin(qB)*qADDt)+cos(qC)*(sin(qB)*qADt*qBDt+sin(  ...
qB)*qADt*qCDt-cos(qB)*qADDt)))) - 0.25*cos(qB+qC)*(4*ICyy*sin(qB+qC)*qADt*(qBDt+qCDt)-4*ICzz*(sin(qC)*(cos(qB)*qADt*qBDt+cos(qB)*  ...
qADt*qCDt+sin(qB)*qADDt)+cos(qC)*(sin(qB)*qADt*qBDt+sin(qB)*qADt*qCDt-cos(qB)*qADDt))-mC*LC*(2*LB*(2*sin(qB)*qADt*qBDt-cos(qB)*  ...
qADDt)+LC*(sin(qB+qC)*qADt*(qBDt+qCDt)+sin(qC)*(cos(qB)*qADt*qBDt+cos(qB)*qADt*qCDt+sin(qB)*qADDt)+cos(qC)*(sin(qB)*qADt*qBDt+sin(  ...
qB)*qADt*qCDt-cos(qB)*qADDt))));
RHS(2) = (IByy-IBzz)*sin(qB)*cos(qB)*qADt^2 + (ICyy-ICzz)*sin(qB+qC)*cos(qB+qC)*qADt^2 + 0.5*mC*LB*LC*sin(qC)*((qBDt+qCDt)^2+cos(  ...
qB+qC)^2*qADt^2) - mC*g*LB*cos(qB) - 0.5*mB*g*LB*cos(qB) - 0.5*mC*g*LC*cos(qB+qC) - 0.5*mC*LB*LC*sin(qC)*(qBDt^2+cos(qB)^2*qADt^2)  ...
- IBxx*qBDDt - ICxx*(qBDDt+qCDDt) - mC*LB^2*(sin(qB)*cos(qB)*qADt^2+qBDDt) - 0.25*mB*LB^2*(sin(qB)*cos(qB)*qADt^2+qBDDt)  ...
- 0.25*mC*LC^2*(sin(qB+qC)*cos(qB+qC)*qADt^2+qBDDt+qCDDt) - 0.5*mC*LB*LC*cos(qC)*(sin(qB)*cos(qB)*qADt^2+2*qBDDt) - 0.5*mC*LB*LC*  ...
cos(qC)*(sin(qB+qC)*cos(qB+qC)*qADt^2+qCDDt);
RHS(3) = (ICyy-ICzz)*sin(qB+qC)*cos(qB+qC)*qADt^2 - 0.5*mC*g*LC*cos(qB+qC) - 0.5*mC*LB*LC*sin(qC)*(qBDt^2+cos(qB)^2*qADt^2)  ...
- ICxx*(qBDDt+qCDDt) - 0.5*mC*LB*LC*cos(qC)*(sin(qB)*cos(qB)*qADt^2+qBDDt) - 0.25*mC*LC^2*(sin(qB+qC)*cos(qB+qC)*qADt^2+qBDDt+  ...
qCDDt);
SolutionToAlgebraicEquations = COEF \ transpose(RHS);

% Update variables after uncoupling equations
TA = SolutionToAlgebraicEquations(1);
TB = SolutionToAlgebraicEquations(2);
TC = SolutionToAlgebraicEquations(3);



%===========================================================================
Output = zeros( 1, 3 );

Output(1) = TA;
Output(2) = TB;
Output(3) = TC;

%============================================================
end    % End of function MGRobotStanfordArm3DInverseDynamics
%============================================================
