MGRobotStanfordArm2DFeedForwardControl.html (MotionGenesis input/output).
(1) % MotionGenesis file: MGRobotStanfordArm2DFeedForwardControl.txt
(2) % Copyright (c) 2020 Motion Genesis LLC. Only for use with MotionGenesis.
(3) %--------------------------------------------------------------------
(4) NewtonianFrame A % Cylindrical base (rigidly connected to Earth).
(5) RigidBody B % Inner link connected to A by revolute motor.
(6) RigidBody C % Outer link connected to B by revolute motor.
(7) %--------------------------------------------------------------------
(8) Variable TB % Ax> measure of motor torque on B from A.
(9) Variable TC % Bx> measure of motor torque on C from B.
(10) Specified qB'' % Angle from Ay> to By> with +Ax> sense.
(11) Specified qC'' % Angle from By> to Cy> with +Bx> sense.
(12) Constant LA = 0.4 m % Az> measure of Bo's position from Ao.
(13) Constant LB = 0.3 m % By> measure of Co's position from Bo.
(14) Constant LC = 0.2 m % Cy> measure of Q's position from Co.
(15) Constant g = 9.8 m/s^2 % Earth's gravitational acceleration.
(16) B.SetMassInertia( mB = 4 kg, IBxx = mB*LB^2/6, IByy, IBzz )
-> (17) IBxx = 0.1666667*mB*LB^2
(18) C.SetMassInertia( mC = 2 kg, ICxx = mC*LC^2/6, ICyy, ICzz )
-> (19) ICxx = 0.1666667*mC*LC^2
(20) %--------------------------------------------------------------------
(21) % Rotational kinematics.
(22) B.RotateX( A, qB )
-> (23) B_A = [1, 0, 0; 0, cos(qB), sin(qB); 0, -sin(qB), cos(qB)]
-> (24) w_B_A> = qB'*Bx>
-> (25) alf_B_A> = qB''*Bx>
(26) C.RotateX( B, qC )
-> (27) C_B = [1, 0, 0; 0, cos(qC), sin(qC); 0, -sin(qC), cos(qC)]
-> (28) w_C_B> = qC'*Cx>
-> (29) w_C_A> = (qB'+qC')*Cx>
-> (30) alf_C_B> = qC''*Cx>
-> (31) alf_C_A> = (qB''+qC'')*Cx>
(32) %--------------------------------------------------------------------
(33) % Translational kinematics.
(34) Bo.Translate( Ao, 0> )
-> (35) p_Ao_Bo> = 0>
-> (36) v_Bo_A> = 0>
-> (37) a_Bo_A> = 0>
(38) Bcm.Translate( Bo, 0.5*LB*By> )
-> (39) p_Bo_Bcm> = 0.5*LB*By>
-> (40) v_Bcm_A> = 0.5*LB*qB'*Bz>
-> (41) a_Bcm_A> = -0.5*LB*qB'^2*By> + 0.5*LB*qB''*Bz>
(42) Co.Translate( Bo, LB*By> )
-> (43) p_Bo_Co> = LB*By>
-> (44) v_Co_A> = LB*qB'*Bz>
-> (45) a_Co_A> = -LB*qB'^2*By> + LB*qB''*Bz>
(46) Ccm.Translate( Co, 0.5*LC*Cy> )
-> (47) p_Co_Ccm> = 0.5*LC*Cy>
-> (48) v_Ccm_A> = LB*qB'*Bz> + 0.5*LC*(qB'+qC')*Cz>
-> (49) a_Ccm_A> = -LB*qB'^2*By> + LB*qB''*Bz> - 0.5*LC*(qB'+qC')^2*Cy> + 0.5*
LC*(qB''+qC'')*Cz>
(50) %--------------------------------------------------------------------
(51) % Add relevant forces and torques.
(52) System.AddForceGravity( -g*Az> )
-> (53) Force_Bcm> = -mB*g*Az>
-> (54) Force_Ccm> = -mC*g*Az>
(55) B.AddTorque( A, TB*Ax> )
-> (56) Torque_B_A> = TB*Ax>
(57) C.AddTorque( B, TC*Bx> )
-> (58) Torque_C_B> = TC*Bx>
(59) %--------------------------------------------------------------------
(60) % Form equations of motion with MG road-maps (FBDs).
(61) Dynamics[1] = Dot( Ax>, System.GetDynamics(Bo) )
-> (62) Dynamics[1] = mC*g*LB*cos(qB) + 0.5*mB*g*LB*cos(qB) + 0.5*mC*g*LC*cos(
qB+qC) + 0.5*mC*LB*LC*sin(qC)*qB'^2 + IBxx*qB'' + ICxx*(qB''+qC'')
+ mC*LB^2*qB'' + 0.25*mB*LB^2*qB'' + mC*LB*LC*cos(qC)*qB'' + 0.25*mC*LC^2
*(qB''+qC'') + 0.5*mC*LB*LC*cos(qC)*qC'' - 0.5*mC*LB*LC*sin(qC)*(qB'+
qC')^2 - TB
(63) Dynamics[2] = Dot( Bx>, C.GetDynamics(Co) )
-> (64) Dynamics[2] = 0.5*mC*g*LC*cos(qB+qC) + 0.5*mC*LB*LC*sin(qC)*qB'^2
+ ICxx*(qB''+qC'') + 0.25*mC*LC^2*(qB''+qC'') + 0.5*mC*LB*LC*cos(qC)*qB''
- TC
(65) %--------------------------------------------------------------------
(66) % Verify dynamics equations with Kane's method.
(67) SetGeneralizedSpeed( qB', qC' )
(68) KaneDynamics = System.GetDynamicsKane()
-> (69) KaneDynamics[1] = 0.5*mB*g*LB*cos(qB) + 0.5*mC*g*(2*LB*cos(qB)+LC*cos(
qB+qC)) + 0.5*mC*LB*LC*sin(qC)*(qB'^2-(qB'+qC')^2) + 0.25*(4*ICxx+mC*
LC*(LC+2*LB*cos(qC)))*qC'' + 0.25*(4*IBxx+4*ICxx+mB*LB^2+mC*(LC^2+4*LB^2
+4*LB*LC*cos(qC)))*qB'' - TB
-> (70) KaneDynamics[2] = 0.5*mC*g*LC*cos(qB+qC) + 0.5*mC*LB*LC*sin(qC)*qB'^2
+ 0.25*(4*ICxx+mC*LC^2)*qC'' + 0.25*(4*ICxx+mC*LC*(LC+2*LB*cos(qC)))*qB''
- TC
(71) IsDynamicsVerified = IsSimplifyEqual( KaneDynamics, Dynamics )
-> (72) IsDynamicsVerified = true
(73) %--------------------------------------------------------------------
(74) % Use dynamics equations to solve for TB and TC.
(75) Solve( KaneDynamics = 0, TB, TC )
-> (76) TB = 0.5*mB*g*LB*cos(qB) + 0.5*mC*g*(2*LB*cos(qB)+LC*cos(qB+qC)) + 0.5*
mC*LB*LC*sin(qC)*(qB'^2-(qB'+qC')^2) + 0.25*(4*ICxx+mC*LC*(LC+2*LB*cos(
qC)))*qC'' + 0.25*(4*IBxx+4*ICxx+mB*LB^2+mC*(LC^2+4*LB^2+4*LB*LC*cos(
qC)))*qB''
-> (77) TC = 0.5*mC*g*LC*cos(qB+qC) + 0.5*mC*LB*LC*sin(qC)*qB'^2 + 0.25*(4*ICxx
+mC*LC^2)*qC'' + 0.25*(4*ICxx+mC*LC*(LC+2*LB*cos(qC)))*qB''
(78) %--------------------------------------------------------------------
(79) % Create desired trajectory (for now, just a constant).
(80) Specified qBDesired''
(81) Specified qCDesired''
(82) SetDt( qBDesired = pi/3 )
-> (83) qBDesired = 1.047198
-> (84) qBDesired' = 0
-> (85) qBDesired'' = 0
(86) SetDt( qCDesired = pi/6 * sin(2*pi*t) )
-> (87) qCDesired = 0.5235988*sin(6.283185*t)
-> (88) qCDesired' = 3.289868*cos(6.283185*t)
-> (89) qCDesired'' = -20.67085*sin(6.283185*t)
(90) %--------------------------------------------------------------------
(91) % Create equation governing error (for feed-forward control).
(92) Constant wn = 2.0 rad/sec
(93) Error[1] = qB - qBDesired
-> (94) Error[1] = qB - qBDesired
(95) Error[2] = qC - qCDesired
-> (96) Error[2] = qC - qCDesired
(97) ControlEqn = DtDt(Error) + 2*wn*Dt(Error) + wn^2*Error
-> (98) ControlEqn[1] = wn^2*(qB-qBDesired) + qB'' - qBDesired'' - 2*wn*(qBDes
ired'-qB')
-> (99) ControlEqn[2] = wn^2*(qC-qCDesired) + qC'' - qCDesired'' - 2*wn*(qCDes
ired'-qC')
(100) %--------------------------------------------------------------------
(101) % Integration parameters and initial values for variables.
(102) Input tFinal = 4 sec, tStep = 0.01 sec, absError = 1.0E-08
(103) Input qB = 0 deg, qB' = 0 rad/sec
(104) Input qC = -60 deg, qC' = 0 rad/sec
(105) %--------------------------------------------------------------------
(106) % List output quantities and simulate robot with feed-forward controller.
(107) OutputPlot t sec, qB degrees, qBDesired degrees, qC degrees, qCDesired degrees
(108) Output t sec, TB N*m, TC N*m
(109) ODE( ControlEqn = 0, qB'', qC'' ) MGRobotStanfordArm2DFeedForwardControl
(110) %--------------------------------------------------------------------
Saved by Motion Genesis LLC. MotionGenesis 6.4 Professional. Command names and syntax: Copyright (c) 2024 Motion Genesis LLC. All rights reserved.