MGTriplePendulumDynamicsWithControl.html  (MotionGenesis input/output).
   (1) % MotionGenesis file: MGTriplePendulumDynamicsWithControl.txt
   (2) % Copyright (c) 2009-2018 Motion Genesis LLC.  All rights reserved.
   (3) %--------------------------------------------------------------------
   (4) NewtonianFrame  N              % Ground (Earth).
   (5) RigidBody       A, B, C        % Pendulum links.
   (6) Point           CN( C )        % Robot's end-effector.
   (7) %--------------------------------------------------------------------
   (8) Constant   L = 4 m             % Length of links A, B, C.
   (9) Constant   g = 9.8 m/s^2       % Earth's gravity.
   (10) Variable   qA'',  qB'',  qC''  % Link angles.
   (11) Specified  TA,  TB,  TC        % Motor torques: A from N, B from A, C from B.
   (12) %--------------------------------------------------------------------
   (13) A.SetMassInertia( m = 0.8 kg,  0,  I = m*L^2/12,  I  )
-> (14) I = 0.08333333*m*L^2

   (15) B.SetMassInertia( m,  0,  I,  I  )
   (16) C.SetMassInertia( m,  0,  I,  I  )
   (17) %---------------------------------------------------------------
   (18) %   Rotational kinematics.
   (19) A.RotateZ( N,  qA )
-> (20) A_N = [cos(qA), sin(qA), 0;  -sin(qA), cos(qA), 0;  0, 0, 1]
-> (21) w_A_N> = qA'*Az>
-> (22) alf_A_N> = qA''*Az>

   (23) B.RotateZ( N,  qB )
-> (24) B_N = [cos(qB), sin(qB), 0;  -sin(qB), cos(qB), 0;  0, 0, 1]
-> (25) w_B_N> = qB'*Bz>
-> (26) alf_B_N> = qB''*Bz>

   (27) C.RotateZ( N,  qC )
-> (28) C_N = [cos(qC), sin(qC), 0;  -sin(qC), cos(qC), 0;  0, 0, 1]
-> (29) w_C_N> = qC'*Cz>
-> (30) alf_C_N> = qC''*Cz>

   (31) %--------------------------------------------------------------------
   (32) %   Translational kinematics.
   (33) Ao.Translate(  No,          0> )
-> (34) p_No_Ao> = 0>
-> (35) v_Ao_N> = 0>
-> (36) a_Ao_N> = 0>

   (37) Acm.Translate( Ao,  -0.5*L*Ay> )
-> (38) p_Ao_Acm> = -0.5*L*Ay>
-> (39) v_Acm_N> = 0.5*L*qA'*Ax>
-> (40) a_Acm_N> = 0.5*L*qA''*Ax> + 0.5*L*qA'^2*Ay>

   (41) Bo.Translate(  Ao,      -L*Ay> )
-> (42) p_Ao_Bo> = -L*Ay>
-> (43) v_Bo_N> = L*qA'*Ax>
-> (44) a_Bo_N> = L*qA''*Ax> + L*qA'^2*Ay>

   (45) Bcm.Translate( Bo,   0.5*L*Bx> )
-> (46) p_Bo_Bcm> = 0.5*L*Bx>
-> (47) v_Bcm_N> = L*qA'*Ax> + 0.5*L*qB'*By>
-> (48) a_Bcm_N> = L*qA''*Ax> + L*qA'^2*Ay> - 0.5*L*qB'^2*Bx> + 0.5*L*qB''*By>

   (49) Co.Translate(  Bo,       L*Bx> )
-> (50) p_Bo_Co> = L*Bx>
-> (51) v_Co_N> = L*qA'*Ax> + L*qB'*By>
-> (52) a_Co_N> = L*qA''*Ax> + L*qA'^2*Ay> - L*qB'^2*Bx> + L*qB''*By>

   (53) Ccm.Translate( Co,   0.5*L*Cy> )
-> (54) p_Co_Ccm> = 0.5*L*Cy>
-> (55) v_Ccm_N> = L*qA'*Ax> + L*qB'*By> - 0.5*L*qC'*Cx>
-> (56) a_Ccm_N> = L*qA''*Ax> + L*qA'^2*Ay> - L*qB'^2*Bx> + L*qB''*By> - 0.5*L*
        qC''*Cx> - 0.5*L*qC'^2*Cy>

   (57) CN.Translate(  Co,       L*Cy> )
-> (58) p_Co_CN> = L*Cy>
-> (59) v_CN_N> = L*qA'*Ax> + L*qB'*By> - L*qC'*Cx>
-> (60) a_CN_N> = L*qA''*Ax> + L*qA'^2*Ay> - L*qB'^2*Bx> + L*qB''*By> - L*qC''*Cx> - L*qC'^2*Cy>

   (61) %--------------------------------------------------------------------
   (62) %   Add relevant forces.
   (63) System.AddForceGravity( -g*Ny> )
-> (64) Force_Acm> = -m*g*Ny>
-> (65) Force_Bcm> = -m*g*Ny>
-> (66) Force_Ccm> = -m*g*Ny>

   (67) %--------------------------------------------------------------------
   (68) %   Add motor torques.
   (69) A.AddTorque( N,  TA*Az> )
-> (70) Torque_A_N> = TA*Az>

   (71) B.AddTorque( A,  TB*Bz> )
-> (72) Torque_B_A> = TB*Bz>

   (73) C.AddTorque( B,  TC*Cz> )
-> (74) Torque_C_B> = TC*Cz>

   (75) %--------------------------------------------------------------------
   (76) %   Form equations of motion with FBDs (D'Alembert/MG Road-maps).
   (77) %   Alternate: Kane/Lagrange methods with System.GetDynamicsKane().
   (78) Dynamics[1] = Dot( Nz>,  System(A, B, C).GetDynamics(Ao) )
-> (79) Dynamics[1] = 0.5*m*L^2*sin(qA-qC)*qA'^2 + 0.5*m*L^2*cos(qB-qC)*qB'^2
        + 1.5*m*L^2*cos(qA-qB)*qA'^2 + I*qA'' + I*qB'' + I*qC'' + 0.25*m*L^2*qC''
        + 1.25*m*L^2*qB'' + 2.25*m*L^2*qA'' + 0.5*m*L^2*sin(qB-qC)*qB'' + 0.5*m
        *L^2*sin(qB-qC)*qC'' + 1.5*m*L^2*sin(qA-qB)*qA'' + 1.5*m*L^2*sin(qA-qB)*qB''
        - TA - 0.5*m*g*L*(sin(qC)-5*sin(qA)-3*cos(qB)) - 1.5*m*L^2*cos(qA-qB)*qB'^2
        - 0.5*m*L^2*sin(qA-qC)*qC'^2 - 0.5*m*L^2*cos(qB-qC)*qC'^2 - 0.5*m*L^2*
        cos(qA-qC)*qA'' - 0.5*m*L^2*cos(qA-qC)*qC''

   (80) Dynamics[2] = Dot( Nz>,     System(B, C).GetDynamics(Bo) )
-> (81) Dynamics[2] = 0.5*m*L^2*cos(qB-qC)*qB'^2 + I*qB'' + I*qC'' + 0.25*m*L^2*qC''
        + 1.25*m*L^2*qB'' + 0.5*m*L^2*sin(qB-qC)*qB'' + 0.5*m*L^2*sin(qB-qC)*qC''
        + 0.5*m*L^2*(sin(qA-qC)*qA'^2+3*cos(qA-qB)*qA'^2+3*sin(qA-qB)*qA''-cos(
        qA-qC)*qA'') - TB - 0.5*m*g*L*(sin(qC)-3*cos(qB)) - 0.5*m*L^2*cos(qB-
        qC)*qC'^2

   (82) Dynamics[3] = Dot( Nz>,                C.GetDynamics(Co) )
-> (83) Dynamics[3] = I*qC'' + 0.25*m*L^2*qC'' + 0.5*m*L^2*(cos(qB-qC)*qB'^2+
        sin(qB-qC)*qB'') + 0.5*m*L^2*(sin(qA-qC)*qA'^2-cos(qA-qC)*qA'') - TC
        - 0.5*m*g*L*sin(qC)

   (84) %--------------------------------------------------------------------
   (85) %   Integration parameters and initial values.
   (86) Input  tFinal = 9 sec,  tStep = 0.02,  absError = 1.0E-07
   (87) Input  qA = -15 deg,      qB =  0 deg,      qC = 15 deg
   (88) Input  qA' =  0 rad/sec,  qB' = 0 rad/sec,  qC' = 0 rad/sec
   (89) %--------------------------------------------------------------------
   (90) %   Quantities to be output from ODE.
   (91) Variable   x'',  y''
   (92) SetDt( x = Dot( Nx>,  CN.GetPosition(No) ) )
-> (93) x = -L*(sin(qC)-sin(qA)-cos(qB))
-> (94) x' = L*(cos(qA)*qA'-sin(qB)*qB'-cos(qC)*qC')
-> (95) x'' = L*(sin(qC)*qC'^2+cos(qA)*qA''-sin(qA)*qA'^2-cos(qB)*qB'^2-sin(qB)
        *qB''-cos(qC)*qC'')

   (96) SetDt( y = Dot( Ny>,  CN.GetPosition(No) ) )
-> (97) y = -L*(cos(qA)-sin(qB)-cos(qC))
-> (98) y' = -L*(sin(qC)*qC'-sin(qA)*qA'-cos(qB)*qB')
-> (99) y'' = -L*(sin(qB)*qB'^2+cos(qC)*qC'^2+sin(qC)*qC''-cos(qA)*qA'^2-sin(
        qA)*qA''-cos(qB)*qB'')

   (100) Output  t sec,  x m, y m,  qA deg, qB deg, qC deg,  TA N*m, TB N*m, TC N*m
   (101) %--------------------------------------------------------------------
   (102) %   Choose critically-damped control gains.
   (103) Constant  wn = 1.2 rad/sec         % Speed of control.
   (104) kp = wn^2                          % Proportional control gain.
-> (105) kp = wn^2

   (106) kd = 2 * wn                        % Derivative control gain.
-> (107) kd = 2*wn

   (108) %--------------------------------------------------------------------
   (109) %   Joint-control: Provide desired angles and control equations.
   (110) Specified  qADesired'',  qBDesired'',  qCDesired''
   (111) Variable   qAError'',    qBError'',    qCError''
   (112) SetDt( qADesired = sin(2*pi*t) )
-> (113) qADesired = sin(6.283185*t)
-> (114) qADesired' = 6.283185*cos(6.283185*t)
-> (115) qADesired'' = -39.47842*sin(6.283185*t)

   (116) SetDt( qBDesired = 0 )
-> (117) qBDesired = 0
-> (118) qBDesired' = 0
-> (119) qBDesired'' = 0

   (120) SetDt( qCDesired = 0 )
-> (121) qCDesired = 0
-> (122) qCDesired' = 0
-> (123) qCDesired'' = 0

   (124) SetDt( qAError = qA - qADesired )
-> (125) qAError = qA - qADesired
-> (126) qAError' = qA' - qADesired'
-> (127) qAError'' = qA'' - qADesired''

   (128) SetDt( qBError = qB - qBDesired )
-> (129) qBError = qB - qBDesired
-> (130) qBError' = qB' - qBDesired'
-> (131) qBError'' = qB'' - qBDesired''

   (132) SetDt( qCError = qC - qCDesired )
-> (133) qCError = qC - qCDesired
-> (134) qCError' = qC' - qCDesired'
-> (135) qCError'' = qC'' - qCDesired''

   (136) ControlConstraint[1] = qAError'' + kd * qAError' + kp * qAError
-> (137) ControlConstraint[1] = kp*qAError + kd*qAError' + qAError''

   (138) ControlConstraint[2] = qBError'' + kd * qBError' + kp * qBError
-> (139) ControlConstraint[2] = kp*qBError + kd*qBError' + qBError''

   (140) ControlConstraint[3] = qCError'' + kd * qCError' + kp * qCError
-> (141) ControlConstraint[3] = kp*qCError + kd*qCError' + qCError''

   (142) %--------------------------------------------------------------------
   (143) %   Augment dynamics with constraints and simulate.
   (144) ODE( [Dynamics; ControlConstraint] = 0,  qA'', qB'', qC'', TA, TB, TC ) MGTriplePendulumDynamicsJointControl

   (145) %--------------------------------------------------------------------
   (146) %   End-effector control: Provide desired motion and control equations.
   (147) Specified  xDesired'',  yDesired''
   (148) Variable   xError'',    yError''
   (149) SetDt( xDesired = L + L/2*sin(2*pi*t) )
-> (150) xDesired = 0.5*L*(2+sin(6.283185*t))
-> (151) xDesired' = 3.141593*L*cos(6.283185*t)
-> (152) xDesired'' = -19.73921*L*sin(6.283185*t)

   (153) SetDt( yDesired =  0 )
-> (154) yDesired = 0
-> (155) yDesired' = 0
-> (156) yDesired'' = 0

   (157) SetDt( qCDesired = 0 )
-> (158) qCDesired = 0
-> (159) qCDesired' = 0
-> (160) qCDesired'' = 0

   (161) SetDt( xError = x - xDesired )
-> (162) xError = x - xDesired
-> (163) xError' = x' - xDesired'
-> (164) xError'' = x'' - xDesired''

   (165) SetDt( yError = y - yDesired )
-> (166) yError = y - yDesired
-> (167) yError' = y' - yDesired'
-> (168) yError'' = y'' - yDesired''

   (169) ControlConstraint[1] := xError'' + kd * xError' + kp * xError
-> (170) ControlConstraint[1] = kp*xError + kd*xError' + xError''

   (171) ControlConstraint[2] := yError'' + kd * yError' + kp * yError
-> (172) ControlConstraint[2] = kp*yError + kd*yError' + yError''

   (173) ControlConstraint[3] := qCError'' + kd * qCError' + kp * qCError
-> (174) ControlConstraint[3] = kp*qCError + kd*qCError' + qCError''

   (175) %--------------------------------------------------------------------
   (176) %       Augment dynamics with constraints and simulate
   (177) ODE( [Dynamics; ControlConstraint] = 0,  qA'', qB'', qC'', TA, TB, TC ) MGTriplePendulumDynamicsEndEffectorControl

   (178) %--------------------------------------------------------------------
   (179) %       Save input together with program responses
Saved by Motion Genesis LLC.   Command names and syntax: Copyright (c) 2009-2018 Motion Genesis LLC. All rights reserved.