% MotionGenesis file: MGTemplateAdvanced.txt
% Copyright (c) 2009-2026 Motion Genesis LLC. All rights reserved.
%-------------------------------------------------------------------------------
%   Default settings and unit system for input/output conversions.
SetDigits( 5 )                   % Number of digits displayed for numbers.
SetAutoEpsilon( 1.0E-14 )        % Rounds to nearest integer.
SetAutoZee( OFF )                % Turn ON for efficient calculations or large problems.
SetLineWidth( 120 )              % Line width for files: .all, .html, .for, .c, .m, etc.
SetUnitSystem( kg, meter, sec )  % Default units for input/output conversions.
%-------------------------------------------------------------------------------
%   Physical objects.
NewtonianFrame  N        % Example: Earth
RigidBody       A        % Example: Upper arm.
RigidFrame      B        % Comment.
Particle        Q        % Comment.
Point           P( B )   % Point P is a point of B.
System       S( A, B )   % System S consists of objects A and B.
%-------------------------------------------------------------------------------
%   Mathematical declarations.
Variable   qA''           % Angle qA and its 1st and 2nd derivatives.
Variable   q{1:3}'        % q1, q2, q3, q1', q2', q3'.
Variable   Fx, Fy         % Reaction forces.
Constant   g = 9.8 m/s^2  % Earth's gravitational acceleration.
Specified  TA             % Torque on A from N.
SetGeneralizedSpeed( x', theta', vx, vy, wz )
%-------------------------------------------------------------------------------
%   Quantities to be left explicit when SetAutoZee(ON).
SetNoZeeSymbol( Fx, Fy )
%-------------------------------------------------------------------------------
%   Mass and inertia properties.
A.SetMass( mA = 2 kg )
Q.SetMass( mQ = 3 kg )
A.SetInertia( Acm,  IAxx = 0.2 kg*m^2,  IAyy = 0.3 kg*m^2,  IAzz = 0.4 kg*m^2 )
B.SetInertia( Bcm,  IBxx, IByy, IBzz,  IBxy, IByz, IBzx )
%-------------------------------------------------------------------------------
%   Rotational kinematics.
A.RotateX( N, qA )
B.RotateNegativeY( A, qB )
C.SetAngularVelocity( N, someVector> )
%-------------------------------------------------------------------------------
%   Translational kinematics.
Q.SetPosition( No, positionVector> )
Q.SetPositionVelocity( No, positionVector> ) % Sets position, velocity.
Q.Translate( No, positionVector> )           % Sets position, velocity, acceleration.
Q.SetVelocityAcceleration( N, velVector> )   % Differentiates to find acceleration.
%-------------------------------------------------------------------------------
%   Rare alternative: Set just velocity (or acceleration).
Q.SetVelocity( N, someVector> )
Q.SetVelocity( N, No )     % Differentiates Q's position vector from No.
Q.SetVelocity( N, Bo, B )  % Both Q and Bo must be fixed on B.
Q.SetVelocity( N, Bo, B, coincidentPoint )
%-------------------------------------------------------------------------------
%   Kinematical differential equations (if any).
%   Also try HELP SetRotationMatrixODE().
q1' = ( w1*cos(q3) - w2*sin(q3) ) / cos(q2)
q2' = w1*sin(q3) + w2*cos(q3)
%-------------------------------------------------------------------------------
%   Configuration constraints (if any).
LoopConstraint[1] = Dot( Loop>,  Nx> )
LoopConstraint[2] = Dot( Loop>,  Ny> )
%-------------------------------------------------------------------------------
%   Motion constraints (if any).
MotionConstraint[1] = Dot( Q.GetVelocity(N), Ax> )   % E.g., rolling at point Q.
MotionConstraint[2] = Dot( Q.GetVelocity(N), Ay> )
SolveDt( MotionConstraint = 0,  varName1, varName2 )
%-------------------------------------------------------------------------------
%   Add relevant contact and distance forces.
System.AddForceGravity( -g*Ny> )            % Add gravity force to each massive object.
Q.AddForce( someVector> )                   % External force.
Q.AddForce( P, actionReactionForce> )       % Law of action/reaction.
%-------------------------------------------------------------------------------
%   Add relevant torques.
A.AddTorque( SomeVector> )                  % External torque.
B.AddTorque( A, actionReactionTorque> )     % Law of action/reaction.
%-------------------------------------------------------------------------------
%   Translational dynamic equations (F = m*a).
%   For static analysis, use  B.GetStatics()  or  System.GetStatics().
Dynamics[1] = Dot(  B.GetDynamics(),  Nx>  )
Dynamics[2] = Dot(  System(A,B).GetDynamics(),  Ny>  )
%-------------------------------------------------------------------------------
%   Rotational dynamic equations (M = DH/Dt + ...).
%   For static analysis, use  B.GetStatics( aboutPoint ).
Dynamics[3] = Dot(  B.GetDynamics(Bcm),  Bz>  )
%-------------------------------------------------------------------------------
%   Alternative: Form dynamic equations with Kane's method.
%   For static analysis, use  System.GetStaticsKane().
SetGeneralizedSpeed( x', theta', vx, vy, wz )
DynamicsKane = System.GetDynamicsKane()
%-------------------------------------------------------------------------------
%   Optional: Solve algebraic equations for list of unknowns.
%   Efficient alternative: Delay this until the ODE command below.
Solve( Dynamics = 0,   x'', y'', Fx, Fy )
%-------------------------------------------------------------------------------
%   Power calculations (if needed).
power = System.GetPower()                  % Power of all forces/torques on the system.
powerF1 = P1.GetPowerResultantForce()      % Power due to P1.GetResultantForce().
powerF2 = P1.GetPowerResultantForce(P2)    % Power due to P1.GetResultantForce(P2).
powerT1 = B1.GetPowerResultantTorque()     % Power due to B1.GetResultantTorque().
powerT2 = B2.GetPowerResultantTorque( B1 ) % Power due to B2.GetResultantTorque( B1 ).
%-------------------------------------------------------------------------------
%   Work done by forces and torques (if needed).
Variable workDone' = powerP1 + powerP2 + ...   % Work done by forces & torques.
Input  workDone = 0 Joules                     % Initial value of workDone.
%-------------------------------------------------------------------------------
%   Potential, kinetic, and mechanical energy calculations (if needed).
KE = System.GetKineticEnergy()
PEgravity = System.GetForceGravityPotentialEnergy( -g*Ny>, No )
MechanicalEnergy = KE + PEgravity - workDone
%-------------------------------------------------------------------------------
%   Momentum calculations (if needed).
H> = System.GetAngularMomentum( aboutPoint )
L> = EvaluateToNumber( System.GetLinearMomentum() )
P =  System.GetGeneralizedMomentum()
%-------------------------------------------------------------------------------
%   Other calculations (e.g., for output).
xQ = Dot( Q.GetPosition(No),  Nx> )
%-------------------------------------------------------------------------------
%   Expressions for specified quantities (if needed).
%   For example, if t < 0,  TA = cos(t)  else  TA = exp(t)
TA = IsNegative(t)*cos(t) + IsPositiveOr0(t) * exp(t)
%-------------------------------------------------------------------------------
%   Set initial values for variables.
%   If needed, solve constraints to determine initial values, e.g.,
%   SolveSetInput( LoopConstraint = 0,  qA = qAguess deg, qB = qBguess deg )
Input  qA = 30 deg,  qA' = 0 rad/sec
Input  qB = 90 deg,  qB' = 0 rad/sec
%-------------------------------------------------------------------------------
%   List output quantities.
Output      t sec,  x meters,  Fx Newton,  Fy Newton
OutputPlot  t sec,  qA degrees  qB degrees
%-------------------------------------------------------------------------------
%   Output quantities for animation with Animake.
Animate( N, No, A, B, C )
%-------------------------------------------------------------------------------
%   Set numerical integration parameters.
Input  tFinal = 8 sec,  tStep = 0.02 sec,  absError = 1.0E-7
%-------------------------------------------------------------------------------
%   Solve ODEs and write Output to the data file someFilename.1
%   Alternative: Auto-generate efficient MATLAB, C, or Fortran code with:
%   ODE( Dynamics = 0,  listOfVariablesToSolve )  Filename.m
ODE()  Filename
%-------------------------------------------------------------------------------
%   Plot columns 2, 3 vs. column 1 using data in the file Filename.1
Plot Filename.1 [1, 2, 3]   % Plot x and Fx vs time t.
Pause
%-------------------------------------------------------------------------------
%   Set new initial values, solve ODEs, and plot again.
Input  qA := 45 deg,  qA' := 10 rad/sec
ODE()  FilenameB
Plot Filename.1 [1, 2, 3]   FilenameB.1 [ 1, 2, 3]
%-------------------------------------------------------------------------------
%   Numerical analysis.
Answer = EvaluateToNumber( Fx,  q1 = 1, q2 = 2 )

%*******************************************************************************
%   LINEARIZATION AND CONTROL SYSTEM / STABILITY ANALYSIS
%-------------------------------------------------------------------------------
%   Perturbation and nominal solution parameters.
Variable  du{5}'       % Perturbations of u1, ... u5.
Variable  dq{3}'       % Perturbations of q1, ... q3.
Variable  nq{3}'       % Nominal solutions for q1, ... q3.
Variable  nTA          % Nominal solution for TA.
Constant  Omega        % Nominal solution for u4.
%-------------------------------------------------------------------------------
%   Determine nominal solution for TA.
Nominal = Evaluate( Zero,  q1=nq1, q1'=0, u1=0, u1'=0, TA=nTA )
Solve( Nominal = 0,  nTA )
%-------------------------------------------------------------------------------
%   Linearize kinematical equations about nominal solution.
dq1' = Taylor(q1', 1, q1=0:dq1, q2=0:dq2, ...)
%-------------------------------------------------------------------------------
%   Linearize equations of motion about nominal solution.
Perturb  = Taylor(Zero, 1, q1=nq1:dq1, u1=0:du1, u1'=0:du1' )
Solve( Perturb = 0,  du{1:5}' )
%-------------------------------------------------------------------------------
%   Form, x, x', u, A, B, matrices in equation x'=A*x + B*u.
Xm = [ dq1;  du1;   ... ]
Xp = [ dq1'; du1';  ... ]
um = [dTCA, dTCB]
Am = GetCoefficient( Xp, Transpose(Xm) )  % Related: D( Xp, Transpose(Xm) )
Bm = GetCoefficient( Xp, uM )             % Related: D( Xp, um )
%*******************************************************************************

%-------------------------------------------------------------------------------
%   Record input together with responses.
Save  someFilename.html
Quit

