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

%--------------------------------------------------------------------
Variable   TB                % Ax> measure of motor torque on B from A.
Variable   TC                % Bx> measure of motor torque on C from B.
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.

B.SetMassInertia( mB = 4 kg, IBxx = mB*LB^2/6, IByy, IBzz )
C.SetMassInertia( mC = 2 kg, ICxx = mC*LC^2/6, ICyy, ICzz )

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

%--------------------------------------------------------------------
%   Translational kinematics.
Bo.Translate(  Ao,         0> )
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> )
B.AddTorque( A,  TB*Ax> )
C.AddTorque( B,  TC*Bx> )

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

%--------------------------------------------------------------------
%   Verify dynamics equations with Kane's method.
SetGeneralizedSpeed(  qB',  qC'  )
KaneDynamics = System.GetDynamicsKane()
IsDynamicsVerified = IsSimplifyEqual( KaneDynamics, Dynamics )

%--------------------------------------------------------------------
%   Use dynamics equations to solve for TB and TC.
Solve( KaneDynamics = 0,  TB, TC )

%--------------------------------------------------------------------
%   Create desired trajectory (for now, just a constant).
Specified  qBDesired''
Specified  qCDesired''
SetDt( qBDesired = pi/3 )
SetDt( qCDesired = pi/6 * sin(2*pi*t) )

%--------------------------------------------------------------------
%   Create equation governing error (for feed-forward control).
Constant wn = 2.0 rad/sec
Error[1] = qB - qBDesired
Error[2] = qC - qCDesired
ControlEqn = DtDt(Error) + 2*wn*Dt(Error) + wn^2*Error

%--------------------------------------------------------------------
%   Integration parameters and initial values for variables.
Input  tFinal = 4 sec,  tStep = 0.01 sec,  absError = 1.0E-08
Input  qB =   0 deg,  qB' = 0 rad/sec
Input  qC = -60 deg,  qC' = 0 rad/sec

%--------------------------------------------------------------------
%   List output quantities and simulate robot with feed-forward controller.
OutputPlot  t sec,  qB degrees, qBDesired degrees,  qC degrees,  qCDesired degrees
Output      t sec,  TB N*m,  TC N*m
ODE( ControlEqn = 0,  qB'', qC'' )  MGRobotStanfordArm2DFeedForwardControl

%--------------------------------------------------------------------
Save MGRobotStanfordArm2DFeedForwardControl.html
if( IsDynamicsVerified == true ) { Quit }

%--------------------------------------------------------------------
%   Optional: Create MATLAB function solely for feed-forward control.
EncodeOutput  TB N*m,  TC N*m
Code Algebraic()  MGRobotStanfordArm2DFeedForwardControl.m( qB, qB', qB'',  qC, qC', qC'' )