% MotionGenesis file: MGTwoLinkRobotWithCircularSlot.txt
% Problem: Two link robot with end-point in a circular slot.
% Copyright (c) 2023 Motion Genesis LLC.
%--------------------------------------------------------------------
%   Physical objects.
NewtonianFrame  N       % Earth with Nx> from No to Eo, Nz> opposite gravity.
RigidBody       A       % Link connected at Ao to N by a revolute joint.
RigidBody       B       % Link connected at Bo to A by a revolute joint.
Point           BE(B)   % Point of B that slides in the circular slot E.
Point           Eo(N)   % Center of the circular slot E, fixed in N.
RigidFrame      C       % Cx> directed from Co to BE, Cz> = Nz>.
%--------------------------------------------------------------------
%   Mathematical declarations.
Constant  LA = 0.5 m    % Distance between Ao and Bo (length of link A).
Constant  LB = 0.8 m    % Distance between Bo and BE (length of link B).
Constant  LN = 1.0 m    % Nx> measure of Eo's position from No.
Constant  R = 0.2 m     % Radius of circular slot.
Constant  rho = 1 kg/m  % Link density per unit length.
Variable  qA''          % Angle from Nx> to Ax> with sense +Nz>.
Variable  qB''          % Angle from Nx> to Bx> with sense -Nz>.
Variable  theta''       % Angle from Nx> to Cx> with sense +Nz>.
Variable  TA            % Nz> measure of revolute motor torque on A from N.
%--------------------------------------------------------------------
%   Mass and inertia properties.
A.SetMass( mA = rho * LA )
B.SetMass( mB = rho * LB )
A.SetInertia( Acm,  IAxx, IAyy, IAzz = mA*LA^2/12 )
B.SetInertia( Bcm,  IBxx, IByy, IBzz = mB*LB^2/12 )
%--------------------------------------------------------------------
%   Rotational kinematics.
A.RotatePositiveZ( N, qA )
B.RotateNegativeZ( N, qB )
C.RotatePositiveZ( N, theta )
%--------------------------------------------------------------------
%   Translational kinematics.
Ao.Translate(  No,         0> )
Acm.Translate( No, 0.5*LA*Ax> )
Bo.Translate(  Ao,     LA*Ax> )
Bcm.Translate( Bo, 0.5*LB*Bx> )
BE.Translate(  Eo,      R*Cx> )
%--------------------------------------------------------------------
%   Configuration constraints.
Loop> = LN*Nx> + R*Cx> - LB*Bx> - LA*Ax>
ConfigurationConstraint[1] = Dot( Loop>, Nx> )
ConfigurationConstraint[2] = Dot( Loop>, Ny> )
%--------------------------------------------------------------------
%   Optional (now/later) Solve qA', qB' in terms of theta'.
MotionConstraint = Dt( ConfigurationConstraint )
SolveDt( MotionConstraint = 0,  qA', qB' )
%--------------------------------------------------------------------
%   Add relevant contact/distance forces and torques.
A.AddTorque( N, TA*Nz> )     % Revolute motor torque on A from N.
%--------------------------------------------------------------------
%   Kane's dynamic equation for generalized speed theta'.
SetGeneralizedSpeed( theta' )
KaneDynamics = System.GetDynamicsKane()
%--------------------------------------------------------------------
%   Optional: Make Kane's equation easier to read.
massEffective = GetCoefficient( KaneDynamics[1], theta'' )
CentripetalEtc = Exclude( KaneDynamics[1],  theta'', TA )
Dynamics = massEffective * theta'' + GetCoefficient(KaneDynamics[1], TA) * TA + CentripetalEtc
%--------------------------------------------------------------------
%   Feed-forward (computed-torque) control law.
Constant  wn = 1 rad/sec       % Feed-forward control law constant.
Constant  vDesired = 0.2 m/s   % BE's desired speed in circular slot.
v = Dot( BE.GetVelocity(N), Cy> )
FeedForwardControlEqn[1] = Dt(vDesired - v) + wn*(vDesired - v)
%--------------------------------------------------------------------
%   Power, work, and kinetic energy calculations.
systemPower = Dot( A.GetTorque(N), A.GetAngularVelocity(N) )
Variable workDone' = systemPower
Input  workDone = 0 Joules        % Initial value of workDone.
KE = System.GetKineticEnergy()
MechanicalEnergy = KE - workDone
%--------------------------------------------------------------------
%   Set initial values for variables for subsequent ODE command.
Input  theta = 90 deg, theta' = 0 rad/sec
SolveSetInput( ConfigurationConstraint = 0,  qA = 30 deg,  qB = 30 deg )
%--------------------------------------------------------------------
%   List output quantities (e.g., for subsequent ODE command).
Output t sec, qA deg, qB deg, v m/s, TA N*m, MechanicalEnergy Joules
%--------------------------------------------------------------------
%   Set numerical integration parameters and solve ODEs.
Input  tFinal = 16 sec,  tStep = 0.02 sec,  absError = 1.0E-09
ODE( [Dynamics; FeedForwardControlEqn] = 0,  TA, theta'' ) MGTwoLinkRobotWithCircularSlot
%--------------------------------------------------------------------
%   Optional: Plot results.
% Plot MGTwoLinkRobotWithCircularSlot.1[1, 4]
% Plot MGTwoLinkRobotWithCircularSlot.1[1, 5]
%--------------------------------------------------------------------
%   Record input together with responses.
Save  MGTwoLinkRobotWithCircularSlot.html
Quit