MGTwoLinkRobotWithCircularSlot.html (MotionGenesis input/output).
(1) % MotionGenesis file: MGTwoLinkRobotWithCircularSlot.txt
(2) % Problem: Two link robot with end-point in a circular slot.
(3) % Copyright (c) 2023 Motion Genesis LLC.
(4) %--------------------------------------------------------------------
(5) % Physical objects.
(6) NewtonianFrame N % Earth with Nx> from No to Eo, Nz> opposite gravity.
(7) RigidBody A % Link connected at Ao to N by a revolute joint.
(8) RigidBody B % Link connected at Bo to A by a revolute joint.
(9) Point BE(B) % Point of B that slides in the circular slot E.
(10) Point Eo(N) % Center of the circular slot E, fixed in N.
(11) RigidFrame C % Cx> directed from Co to BE, Cz> = Nz>.
(12) %--------------------------------------------------------------------
(13) % Mathematical declarations.
(14) Constant LA = 0.5 m % Distance between Ao and Bo (length of link A).
(15) Constant LB = 0.8 m % Distance between Bo and BE (length of link B).
(16) Constant LN = 1.0 m % Nx> measure of Eo's position from No.
(17) Constant R = 0.2 m % Radius of circular slot.
(18) Constant rho = 1 kg/m % Link density per unit length.
(19) Variable qA'' % Angle from Nx> to Ax> with sense +Nz>.
(20) Variable qB'' % Angle from Nx> to Bx> with sense -Nz>.
(21) Variable theta'' % Angle from Nx> to Cx> with sense +Nz>.
(22) Variable TA % Nz> measure of revolute motor torque on A from N.
(23) %--------------------------------------------------------------------
(24) % Mass and inertia properties.
(25) A.SetMass( mA = rho * LA )
-> (26) mA = LA*rho
(27) B.SetMass( mB = rho * LB )
-> (28) mB = LB*rho
(29) A.SetInertia( Acm, IAxx, IAyy, IAzz = mA*LA^2/12 )
-> (30) IAzz = 0.08333333*mA*LA^2
(31) B.SetInertia( Bcm, IBxx, IByy, IBzz = mB*LB^2/12 )
-> (32) IBzz = 0.08333333*mB*LB^2
(33) %--------------------------------------------------------------------
(34) % Rotational kinematics.
(35) A.RotatePositiveZ( N, qA )
-> (36) A_N = [cos(qA), sin(qA), 0; -sin(qA), cos(qA), 0; 0, 0, 1]
-> (37) w_A_N> = qA'*Az>
-> (38) alf_A_N> = qA''*Az>
(39) B.RotateNegativeZ( N, qB )
-> (40) B_N = [cos(qB), -sin(qB), 0; sin(qB), cos(qB), 0; 0, 0, 1]
-> (41) w_B_N> = -qB'*Bz>
-> (42) alf_B_N> = -qB''*Bz>
(43) C.RotatePositiveZ( N, theta )
-> (44) C_N = [cos(theta), sin(theta), 0; -sin(theta), cos(theta), 0; 0, 0, 1]
-> (45) w_C_N> = theta'*Cz>
-> (46) alf_C_N> = theta''*Cz>
(47) %--------------------------------------------------------------------
(48) % Translational kinematics.
(49) Ao.Translate( No, 0> )
-> (50) p_No_Ao> = 0>
-> (51) v_Ao_N> = 0>
-> (52) a_Ao_N> = 0>
(53) Acm.Translate( No, 0.5*LA*Ax> )
-> (54) p_No_Acm> = 0.5*LA*Ax>
-> (55) v_Acm_N> = 0.5*LA*qA'*Ay>
-> (56) a_Acm_N> = -0.5*LA*qA'^2*Ax> + 0.5*LA*qA''*Ay>
(57) Bo.Translate( Ao, LA*Ax> )
-> (58) p_Ao_Bo> = LA*Ax>
-> (59) v_Bo_N> = LA*qA'*Ay>
-> (60) a_Bo_N> = -LA*qA'^2*Ax> + LA*qA''*Ay>
(61) Bcm.Translate( Bo, 0.5*LB*Bx> )
-> (62) p_Bo_Bcm> = 0.5*LB*Bx>
-> (63) v_Bcm_N> = LA*qA'*Ay> - 0.5*LB*qB'*By>
-> (64) a_Bcm_N> = -LA*qA'^2*Ax> + LA*qA''*Ay> - 0.5*LB*qB'^2*Bx> - 0.5*LB*qB''*By>
(65) BE.Translate( Eo, R*Cx> )
-> (66) p_Eo_BE> = R*Cx>
-> (67) v_BE_N> = R*theta'*Cy>
-> (68) a_BE_N> = -R*theta'^2*Cx> + R*theta''*Cy>
(69) %--------------------------------------------------------------------
(70) % Configuration constraints.
(71) Loop> = LN*Nx> + R*Cx> - LB*Bx> - LA*Ax>
-> (72) Loop> = -LA*Ax> - LB*Bx> + R*Cx> + LN*Nx>
(73) ConfigurationConstraint[1] = Dot( Loop>, Nx> )
-> (74) ConfigurationConstraint[1] = LN + R*cos(theta) - LA*cos(qA) - LB*cos(qB)
(75) ConfigurationConstraint[2] = Dot( Loop>, Ny> )
-> (76) ConfigurationConstraint[2] = LB*sin(qB) + R*sin(theta) - LA*sin(qA)
(77) %--------------------------------------------------------------------
(78) % Optional (now/later) Solve qA', qB' in terms of theta'.
(79) MotionConstraint = Dt( ConfigurationConstraint )
-> (80) MotionConstraint[1] = LA*sin(qA)*qA' + LB*sin(qB)*qB' - R*sin(theta)*theta'
-> (81) MotionConstraint[2] = LB*cos(qB)*qB' + R*cos(theta)*theta' - LA*cos(qA)*qA'
(82) SolveDt( MotionConstraint = 0, qA', qB' )
-> (83) qA' = R*sin(qB+theta)*theta'/(LA*sin(qA+qB))
-> (84) qB' = -R*sin(qA-theta)*theta'/(LB*sin(qA+qB))
-> (85) qA'' = -(LB*qB'^2+cos(qB)*(LA*cos(qA)*qA'^2-R*(cos(theta)*theta'^2+sin(
theta)*theta''))-sin(qB)*(LA*sin(qA)*qA'^2-R*(sin(theta)*theta'^2-cos(
theta)*theta'')))/(LA*sin(qA+qB))
-> (86) qB'' = (sin(qA)*(LB*sin(qB)*qB'^2+R*(sin(theta)*theta'^2-cos(theta)*th
eta''))-LA*qA'^2-cos(qA)*(LB*cos(qB)*qB'^2-R*(cos(theta)*theta'^2+sin(
theta)*theta'')))/(LB*sin(qA+qB))
(87) %--------------------------------------------------------------------
(88) % Add relevant contact/distance forces and torques.
(89) A.AddTorque( N, TA*Nz> ) % Revolute motor torque on A from N.
-> (90) Torque_A_N> = TA*Nz>
(91) %--------------------------------------------------------------------
(92) % Kane's dynamic equation for generalized speed theta'.
(93) SetGeneralizedSpeed( theta' )
(94) KaneDynamics = System.GetDynamicsKane()
-> (95) KaneDynamics[1] = 0.25*R*(4*IBzz*sin(qA-theta)*(LA*qA'^2-sin(qA)*(R*sin
(theta)*theta'^2+LB*sin(qB)*qB'^2)-cos(qA)*(R*cos(theta)*theta'^2-LB*
cos(qB)*qB'^2))/(LB^2*sin(qA+qB)^2)-4*TA*sin(qB+theta)/(LA*sin(qA+qB))-
4*IAzz*sin(qB+theta)*(LB*qB'^2+sin(qB)*(R*sin(theta)*theta'^2-LA*sin(
qA)*qA'^2)-cos(qB)*(R*cos(theta)*theta'^2-LA*cos(qA)*qA'^2))/(LA^2*sin(
qA+qB)^2)-mB*(2*LA*sin(qA-theta)*qA'^2-2*LB*sin(qB+theta)*qB'^2-(2*(sin
(qB+theta)*(LA*qA'^2-sin(qA)*(R*sin(theta)*theta'^2+LB*sin(qB)*qB'^2)-
cos(qA)*(R*cos(theta)*theta'^2-LB*cos(qB)*qB'^2))-sin(qA-theta)*(LB*qB'^2
+sin(qB)*(R*sin(theta)*theta'^2-LA*sin(qA)*qA'^2)-cos(qB)*(R*cos(theta)
*theta'^2-LA*cos(qA)*qA'^2)))/tan(qA+qB)-(4*sin(qB+theta)*(LB*qB'^2+sin
(qB)*(R*sin(theta)*theta'^2-LA*sin(qA)*qA'^2)-cos(qB)*(R*cos(theta)*theta'^2
-LA*cos(qA)*qA'^2))-sin(qA-theta)*(LA*qA'^2-sin(qA)*(R*sin(theta)*theta'^2
+LB*sin(qB)*qB'^2)-cos(qA)*(R*cos(theta)*theta'^2-LB*cos(qB)*qB'^2)))/
sin(qA+qB))/sin(qA+qB))-(mA*sin(qB+theta)*(LB*qB'^2+sin(qB)*(R*sin(the
ta)*theta'^2-LA*sin(qA)*qA'^2)-cos(qB)*(R*cos(theta)*theta'^2-LA*cos(
qA)*qA'^2))-R*(4*IBzz*sin(qA-theta)^2/LB^2+(mA+4*IAzz/LA^2)*sin(qB+the
ta)^2+mB*(sin(qA-theta)^2+4*sin(qB+theta)^2+4*sin(qB+theta)*cos(qA+qB)*
sin(qA-theta)))*theta'')/sin(qA+qB)^2)
(96) %--------------------------------------------------------------------
(97) % Optional: Make Kane's equation easier to read.
(98) massEffective = GetCoefficient( KaneDynamics[1], theta'' )
-> (99) massEffective = 0.25*R^2*(4*IBzz*sin(qA-theta)^2/LB^2+(mA+4*IAzz/LA^2)*
sin(qB+theta)^2+mB*(sin(qA-theta)^2+4*sin(qB+theta)^2+4*sin(qB+theta)*
cos(qA+qB)*sin(qA-theta)))/sin(qA+qB)^2
(100) CentripetalEtc = Exclude( KaneDynamics[1], theta'', TA )
-> (101) CentripetalEtc = 0.25*R*(4*IBzz*sin(qA-theta)*(LA*qA'^2-sin(qA)*(R*sin
(theta)*theta'^2+LB*sin(qB)*qB'^2)-cos(qA)*(R*cos(theta)*theta'^2-LB*
cos(qB)*qB'^2))/(LB^2*sin(qA+qB)^2)-mA*sin(qB+theta)*(LB*qB'^2+sin(qB)
*(R*sin(theta)*theta'^2-LA*sin(qA)*qA'^2)-cos(qB)*(R*cos(theta)*theta'^2
-LA*cos(qA)*qA'^2))/sin(qA+qB)^2-4*IAzz*sin(qB+theta)*(LB*qB'^2+sin(
qB)*(R*sin(theta)*theta'^2-LA*sin(qA)*qA'^2)-cos(qB)*(R*cos(theta)*theta'^2
-LA*cos(qA)*qA'^2))/(LA^2*sin(qA+qB)^2)-mB*(2*LA*sin(qA-theta)*qA'^2-2
*LB*sin(qB+theta)*qB'^2-(2*(sin(qB+theta)*(LA*qA'^2-sin(qA)*(R*sin(th
eta)*theta'^2+LB*sin(qB)*qB'^2)-cos(qA)*(R*cos(theta)*theta'^2-LB*cos(
qB)*qB'^2))-sin(qA-theta)*(LB*qB'^2+sin(qB)*(R*sin(theta)*theta'^2-LA*
sin(qA)*qA'^2)-cos(qB)*(R*cos(theta)*theta'^2-LA*cos(qA)*qA'^2)))/tan(
qA+qB)-(4*sin(qB+theta)*(LB*qB'^2+sin(qB)*(R*sin(theta)*theta'^2-LA*
sin(qA)*qA'^2)-cos(qB)*(R*cos(theta)*theta'^2-LA*cos(qA)*qA'^2))-sin(
qA-theta)*(LA*qA'^2-sin(qA)*(R*sin(theta)*theta'^2+LB*sin(qB)*qB'^2)-
cos(qA)*(R*cos(theta)*theta'^2-LB*cos(qB)*qB'^2)))/sin(qA+qB))/sin(qA+
qB)))
(102) Dynamics = massEffective * theta'' + GetCoefficient(KaneDynamics[1], TA) * TA + CentripetalEtc
-> (103) Dynamics = CentripetalEtc + massEffective*theta'' - R*TA*sin(qB+theta)
/(LA*sin(qA+qB))
(104) %--------------------------------------------------------------------
(105) % Feed-forward (computed-torque) control law.
(106) Constant wn = 1 rad/sec % Feed-forward control law constant.
(107) Constant vDesired = 0.2 m/s % BE's desired speed in circular slot.
(108) v = Dot( BE.GetVelocity(N), Cy> )
-> (109) v = R*theta'
(110) FeedForwardControlEqn[1] = Dt(vDesired - v) + wn*(vDesired - v)
-> (111) FeedForwardControlEqn[1] = wn*(vDesired-v) - R*theta''
(112) %--------------------------------------------------------------------
(113) % Power, work, and kinetic energy calculations.
(114) systemPower = Dot( A.GetTorque(N), A.GetAngularVelocity(N) )
-> (115) systemPower = TA*qA'
(116) Variable workDone' = systemPower
-> (117) workDone' = systemPower
(118) Input workDone = 0 Joules % Initial value of workDone.
(119) KE = System.GetKineticEnergy()
-> (120) KE = 0.5*IAzz*qA'^2 + 0.5*IBzz*qB'^2 + 0.125*mA*LA^2*qA'^2 + 0.125*mB*
(LB^2*qB'^2+4*LA^2*qA'^2-4*LA*LB*cos(qA+qB)*qA'*qB')
(121) MechanicalEnergy = KE - workDone
-> (122) MechanicalEnergy = KE - workDone
(123) %--------------------------------------------------------------------
(124) % Set initial values for variables for subsequent ODE command.
(125) Input theta = 90 deg, theta' = 0 rad/sec
(126) SolveSetInput( ConfigurationConstraint = 0, qA = 30 deg, qB = 30 deg )
-> % INPUT has been assigned as follows:
-> % qA 61.7133939114504 deg
-> % qB 17.47968321368927 deg
(127) %--------------------------------------------------------------------
(128) % List output quantities (e.g., for subsequent ODE command).
(129) Output t sec, qA deg, qB deg, v m/s, TA N*m, MechanicalEnergy Joules
(130) %--------------------------------------------------------------------
(131) % Set numerical integration parameters and solve ODEs.
(132) Input tFinal = 16 sec, tStep = 0.02 sec, absError = 1.0E-09
(133) ODE( [Dynamics; FeedForwardControlEqn] = 0, TA, theta'' ) MGTwoLinkRobotWithCircularSlot
(134) %--------------------------------------------------------------------
(135) % Optional: Plot results.
(136) % Plot MGTwoLinkRobotWithCircularSlot.1[1, 4]
(137) % Plot MGTwoLinkRobotWithCircularSlot.1[1, 5]
(138) %--------------------------------------------------------------------
(139) % Record input together with responses.
Saved by Motion Genesis LLC. MotionGenesis 6.4 Professional. Command names and syntax: Copyright (c) 2024 Motion Genesis LLC. All rights reserved.