% MotionGenesis file:  MGRobotStanfordArm3DInverseDynamics.txt
% Copyright (c) 2020 Motion Genesis LLC.  Only for use with MotionGenesis.
%--------------------------------------------------------------------
NewtonianFrame  N            % Earth
RigidBody       A            % Cylindrical base.
RigidBody       B            % Inner link connected to A by revolute motor.
RigidBody       C            % Outer link connected to B by revolute motor.

%--------------------------------------------------------------------
Variable   TA                % Az> measure of motor torque on A from N.
Variable   TB                % Ax> measure of motor torque on B from A.
Variable   TC                % Bx> measure of motor torque on C from B.
Specified  qA''              % Angle from Nx> to Ax> with +Az> sense.
Specified  qB''              % Angle from Ay> to By> with +Ax> sense.
Specified  qC''              % Angle from By> to Cy> with +Bx> sense.

Constant   LA = 0.4 m        % Az> measure of Bo's position from Ao.
Constant   LB = 0.3 m        % By> measure of Co's position from Bo.
Constant   LC = 0.2 m        % Cy> measure of Q's  position from Co.
Constant    g = 9.8 m/s^2    % Earth's gravitational acceleration.

A.SetMassInertia( mA = 3 kg, IAxx = mA*LA^2/12, IAyy = IAxx,      IAzz = mA*0.04^2 )
B.SetMassInertia( mB = 2 kg, IBxx = mB*LB^2/12, IByy = mB*0.03^2, IBzz = IBxx  )
C.SetMassInertia( mC = 1 kg, ICxx = mC*LC^2/12, ICyy = mC*0.02^2, ICzz = ICxx  )

%--------------------------------------------------------------------
%   Rotational kinematics.
A.RotateZ( N, qA )
B.RotateX( A, qB )
C.RotateX( B, qC )

%--------------------------------------------------------------------
%   Translational kinematics.
Ao.Translate(  No,         0> )
Acm.Translate( Ao, 0.5*LA*Az> )
Bo.Translate(  Ao,     LA*Az> )
Bcm.Translate( Bo, 0.5*LB*By> )
Co.Translate(  Bo,     LB*By> )
Ccm.Translate( Co, 0.5*LC*Cy> )

%--------------------------------------------------------------------
%   Add relevant forces and torques.
System.AddForceGravity( -g*Az> )
A.AddTorque( N,  TA*Az> )
B.AddTorque( A,  TB*Ax> )
C.AddTorque( B,  TC*Bx> )

%--------------------------------------------------------------------
%   Form equations of motion with MG road-maps (FBDs).
Dynamics[1] = Dot(  Az>,       System.GetDynamics(Ao)  )
Dynamics[2] = Dot(  Ax>,  System(B,C).GetDynamics(Bo)  )
Dynamics[3] = Dot(  Bx>,            C.GetDynamics(Co)  )

%--------------------------------------------------------------------
%   Optional: Verify dynamics equations with Kane's method.
SetGeneralizedSpeed(  qA',  qB',  qC'  )
KaneDynamics = System.GetDynamicsKane()
ShouldBeZero = Expand( KaneDynamics - Dynamics,  0:2 )

%--------------------------------------------------------------------
%    Optional: Use dynamics equations to solve for TA, TB, TC via
%              Solve( Dynamics = 0,  TA, TB, TC )
%   below use: Code Algebraic() ....
% Alternative: Use Code Algebraic( Dynamics = 0,  TA, TB, TC ) as shown below.

%--------------------------------------------------------------------
%%  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 )

%--------------------------------------------------------------------
%   Create MATLAB function for inverse dynamics.
OutputEncode  TA N*m,  TB N*m,  TC N*m
Code Algebraic( Dynamics = 0,  TA, TB, TC )  MGRobotStanfordArm3DInverseDynamics.m( qA, qA', qA'',  qB, qB', qB'',  qC, qC', qC'' )

%--------------------------------------------------------------------
Save MGRobotStanfordArm3DInverseDynamics.html
if( IsSimplifyZero(ShouldBeZero) == true ) { Quit }