MGRobotStanfordArm3DInverseDynamics.html (MotionGenesis input/output).
(1) % MotionGenesis file: MGRobotStanfordArm3DInverseDynamics.txt
(2) % Copyright (c) 2020 Motion Genesis LLC. Only for use with MotionGenesis.
(3) %--------------------------------------------------------------------
(4) NewtonianFrame N % Earth
(5) RigidBody A % Cylindrical base.
(6) RigidBody B % Inner link connected to A by revolute motor.
(7) RigidBody C % Outer link connected to B by revolute motor.
(8) %--------------------------------------------------------------------
(9) Variable TA % Az> measure of motor torque on A from N.
(10) Variable TB % Ax> measure of motor torque on B from A.
(11) Variable TC % Bx> measure of motor torque on C from B.
(12) Specified qA'' % Angle from Nx> to Ax> with +Az> sense.
(13) Specified qB'' % Angle from Ay> to By> with +Ax> sense.
(14) Specified qC'' % Angle from By> to Cy> with +Bx> sense.
(15) Constant LA = 0.4 m % Az> measure of Bo's position from Ao.
(16) Constant LB = 0.3 m % By> measure of Co's position from Bo.
(17) Constant LC = 0.2 m % Cy> measure of Q's position from Co.
(18) Constant g = 9.8 m/s^2 % Earth's gravitational acceleration.
(19) A.SetMassInertia( mA = 3 kg, IAxx = mA*LA^2/12, IAyy = IAxx, IAzz = mA*0.04^2 )
-> (20) IAxx = 0.08333333*mA*LA^2
-> (21) IAyy = IAxx
-> (22) IAzz = 0.0016*mA
(23) B.SetMassInertia( mB = 2 kg, IBxx = mB*LB^2/12, IByy = mB*0.03^2, IBzz = IBxx )
-> (24) IBxx = 0.08333333*mB*LB^2
-> (25) IByy = 0.0009*mB
-> (26) IBzz = IBxx
(27) C.SetMassInertia( mC = 1 kg, ICxx = mC*LC^2/12, ICyy = mC*0.02^2, ICzz = ICxx )
-> (28) ICxx = 0.08333333*mC*LC^2
-> (29) ICyy = 0.0004*mC
-> (30) ICzz = ICxx
(31) %--------------------------------------------------------------------
(32) % Rotational kinematics.
(33) A.RotateZ( N, qA )
-> (34) A_N = [cos(qA), sin(qA), 0; -sin(qA), cos(qA), 0; 0, 0, 1]
-> (35) w_A_N> = qA'*Az>
-> (36) alf_A_N> = qA''*Az>
(37) B.RotateX( A, qB )
-> (38) B_A = [1, 0, 0; 0, cos(qB), sin(qB); 0, -sin(qB), cos(qB)]
-> (39) w_B_A> = qB'*Bx>
-> (40) w_B_N> = qB'*Bx> + sin(qB)*qA'*By> + cos(qB)*qA'*Bz>
-> (41) alf_B_A> = qB''*Bx>
-> (42) alf_B_N> = qB''*Bx> + (cos(qB)*qA'*qB'+sin(qB)*qA'')*By> + (cos(qB)*qA
''-sin(qB)*qA'*qB')*Bz>
(43) C.RotateX( B, qC )
-> (44) C_B = [1, 0, 0; 0, cos(qC), sin(qC); 0, -sin(qC), cos(qC)]
-> (45) w_C_B> = qC'*Cx>
-> (46) w_C_N> = (qB'+qC')*Cx> + sin(qB+qC)*qA'*Cy> + cos(qB+qC)*qA'*Cz>
-> (47) alf_C_B> = qC''*Cx>
-> (48) alf_C_N> = (qB''+qC'')*Cx> + (cos(qC)*(cos(qB)*qA'*qB'+cos(qB)*qA'*qC'+
sin(qB)*qA'')-sin(qC)*(sin(qB)*qA'*qB'+sin(qB)*qA'*qC'-cos(qB)*qA''))*Cy>
+ (-sin(qC)*(cos(qB)*qA'*qB'+cos(qB)*qA'*qC'+sin(qB)*qA'')-cos(qC)*(sin
(qB)*qA'*qB'+sin(qB)*qA'*qC'-cos(qB)*qA''))*Cz>
(49) %--------------------------------------------------------------------
(50) % Translational kinematics.
(51) Ao.Translate( No, 0> )
-> (52) p_No_Ao> = 0>
-> (53) v_Ao_N> = 0>
-> (54) a_Ao_N> = 0>
(55) Acm.Translate( Ao, 0.5*LA*Az> )
-> (56) p_Ao_Acm> = 0.5*LA*Az>
-> (57) v_Acm_N> = 0>
-> (58) a_Acm_N> = 0>
(59) Bo.Translate( Ao, LA*Az> )
-> (60) p_Ao_Bo> = LA*Az>
-> (61) v_Bo_N> = 0>
-> (62) a_Bo_N> = 0>
(63) Bcm.Translate( Bo, 0.5*LB*By> )
-> (64) p_Bo_Bcm> = 0.5*LB*By>
-> (65) v_Bcm_N> = -0.5*LB*cos(qB)*qA'*Bx> + 0.5*LB*qB'*Bz>
-> (66) a_Bcm_N> = 0.5*LB*(2*sin(qB)*qA'*qB'-cos(qB)*qA'')*Bx> - 0.5*LB*(qB'^2+
cos(qB)^2*qA'^2)*By> + 0.5*LB*(sin(qB)*cos(qB)*qA'^2+qB'')*Bz>
(67) Co.Translate( Bo, LB*By> )
-> (68) p_Bo_Co> = LB*By>
-> (69) v_Co_N> = -LB*cos(qB)*qA'*Bx> + LB*qB'*Bz>
-> (70) a_Co_N> = LB*(2*sin(qB)*qA'*qB'-cos(qB)*qA'')*Bx> - LB*(qB'^2+cos(qB)^2
*qA'^2)*By> + LB*(sin(qB)*cos(qB)*qA'^2+qB'')*Bz>
(71) Ccm.Translate( Co, 0.5*LC*Cy> )
-> (72) p_Co_Ccm> = 0.5*LC*Cy>
-> (73) v_Ccm_N> = -0.5*(2*LB*cos(qB)+LC*cos(qB+qC))*qA'*Bx> + LB*qB'*Bz>
+ 0.5*LC*(qB'+qC')*Cz>
-> (74) a_Ccm_N> = (LB*(2*sin(qB)*qA'*qB'-cos(qB)*qA'')+0.5*LC*(sin(qB+qC)*qA'*
(qB'+qC')+sin(qC)*(cos(qB)*qA'*qB'+cos(qB)*qA'*qC'+sin(qB)*qA'')+cos(
qC)*(sin(qB)*qA'*qB'+sin(qB)*qA'*qC'-cos(qB)*qA'')))*Bx> - LB*(qB'^2+
cos(qB)^2*qA'^2)*By> + LB*(sin(qB)*cos(qB)*qA'^2+qB'')*Bz> - 0.5*LC*((
qB'+qC')^2+cos(qB+qC)^2*qA'^2)*Cy> + 0.5*LC*(sin(qB+qC)*cos(qB+qC)*qA'^2
+qB''+qC'')*Cz>
(75) %--------------------------------------------------------------------
(76) % Add relevant forces and torques.
(77) System.AddForceGravity( -g*Az> )
-> (78) Force_Acm> = -mA*g*Az>
-> (79) Force_Bcm> = -mB*g*Az>
-> (80) Force_Ccm> = -mC*g*Az>
(81) A.AddTorque( N, TA*Az> )
-> (82) Torque_A_N> = TA*Az>
(83) B.AddTorque( A, TB*Ax> )
-> (84) Torque_B_A> = TB*Ax>
(85) C.AddTorque( B, TC*Bx> )
-> (86) Torque_C_B> = TC*Bx>
(87) %--------------------------------------------------------------------
(88) % Form equations of motion with MG road-maps (FBDs).
(89) Dynamics[1] = Dot( Az>, System.GetDynamics(Ao) )
-> (90) Dynamics[1] = IAzz*qA'' + IByy*sin(qB)^2*qA'' + 0.25*cos(qB)*(8*IByy*
sin(qB)*qA'*qB'-4*IBzz*(2*sin(qB)*qA'*qB'-cos(qB)*qA'')-mB*LB^2*(2*sin(
qB)*qA'*qB'-cos(qB)*qA'')-2*mC*LB*(2*LB*(2*sin(qB)*qA'*qB'-cos(qB)*qA'')
+LC*(sin(qB+qC)*qA'*(qB'+qC')+sin(qC)*(cos(qB)*qA'*qB'+cos(qB)*qA'*qC'+
sin(qB)*qA'')+cos(qC)*(sin(qB)*qA'*qB'+sin(qB)*qA'*qC'-cos(qB)*qA''))))
+ 0.25*cos(qB+qC)*(4*ICyy*sin(qB+qC)*qA'*(qB'+qC')-4*ICzz*(sin(qC)*(cos
(qB)*qA'*qB'+cos(qB)*qA'*qC'+sin(qB)*qA'')+cos(qC)*(sin(qB)*qA'*qB'+sin
(qB)*qA'*qC'-cos(qB)*qA''))-mC*LC*(2*LB*(2*sin(qB)*qA'*qB'-cos(qB)*qA'')
+LC*(sin(qB+qC)*qA'*(qB'+qC')+sin(qC)*(cos(qB)*qA'*qB'+cos(qB)*qA'*qC'+
sin(qB)*qA'')+cos(qC)*(sin(qB)*qA'*qB'+sin(qB)*qA'*qC'-cos(qB)*qA''))))
- sin(qB+qC)*(ICzz*cos(qB+qC)*qA'*(qB'+qC')-ICyy*(cos(qC)*(cos(qB)*qA'*
qB'+cos(qB)*qA'*qC'+sin(qB)*qA'')-sin(qC)*(sin(qB)*qA'*qB'+sin(qB)*qA'*
qC'-cos(qB)*qA''))) - TA
(91) Dynamics[2] = Dot( Ax>, System(B,C).GetDynamics(Bo) )
-> (92) Dynamics[2] = 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+cos(qB)^2*qA'^2) + IBxx*qB''
+ ICxx*(qB''+qC'') + mC*LB^2*(sin(qB)*cos(qB)*qA'^2+qB'') + 0.25*mB*LB^2
*(sin(qB)*cos(qB)*qA'^2+qB'') + 0.25*mC*LC^2*(sin(qB+qC)*cos(qB+qC)*qA'^2
+qB''+qC'') + 0.5*mC*LB*LC*cos(qC)*(sin(qB)*cos(qB)*qA'^2+2*qB'')
+ 0.5*mC*LB*LC*cos(qC)*(sin(qB+qC)*cos(qB+qC)*qA'^2+qC'') - (IByy-IBzz)
*sin(qB)*cos(qB)*qA'^2 - (ICyy-ICzz)*sin(qB+qC)*cos(qB+qC)*qA'^2 - 0.5*
mC*LB*LC*sin(qC)*((qB'+qC')^2+cos(qB+qC)^2*qA'^2) - TB
(93) Dynamics[3] = Dot( Bx>, C.GetDynamics(Co) )
-> (94) Dynamics[3] = 0.5*mC*g*LC*cos(qB+qC) + 0.5*mC*LB*LC*sin(qC)*(qB'^2+cos(
qB)^2*qA'^2) + ICxx*(qB''+qC'') + 0.5*mC*LB*LC*cos(qC)*(sin(qB)*cos(qB)
*qA'^2+qB'') + 0.25*mC*LC^2*(sin(qB+qC)*cos(qB+qC)*qA'^2+qB''+qC'')
- (ICyy-ICzz)*sin(qB+qC)*cos(qB+qC)*qA'^2 - TC
(95) %--------------------------------------------------------------------
(96) % Optional: Verify dynamics equations with Kane's method.
(97) SetGeneralizedSpeed( qA', qB', qC' )
(98) KaneDynamics = System.GetDynamicsKane()
-> (99) KaneDynamics[1] = 0.5*qA'*(4*(IByy-IBzz)*sin(qB)*cos(qB)*qB'+4*ICyy*sin
(qB+qC)*cos(qB+qC)*(qB'+qC')-mB*LB^2*sin(qB)*cos(qB)*qB'-4*ICzz*sin(qB+
qC)*cos(qB+qC)*(qB'+qC')-mC*(2*LB*cos(qB)+LC*cos(qB+qC))*(2*LB*sin(qB)*
qB'+LC*sin(qB+qC)*(qB'+qC'))) + 0.25*(4*IAzz+4*IBzz+4*ICzz+mB*LB^2+4*(
ICyy-ICzz)*sin(qB+qC)^2+mC*(2*LB*cos(qB)+LC*cos(qB+qC))^2+(4*IByy-4*IB
zz-mB*LB^2)*sin(qB)^2)*qA'' - TA
-> (100) KaneDynamics[2] = 0.5*mB*g*LB*cos(qB) + 0.5*mC*g*(2*LB*cos(qB)+LC*cos(
qB+qC)) + 0.25*mB*LB^2*sin(qB)*cos(qB)*qA'^2 + 0.25*mC*(4*LB^2*sin(qB)
*cos(qB)*qA'^2+LC^2*sin(qB+qC)*cos(qB+qC)*qA'^2+2*LB*LC*sin(qB)*cos(
qB)*cos(qC)*qA'^2+2*LB*LC*sin(qC)*(qB'^2+cos(qB)^2*qA'^2)+2*LB*LC*cos(
qC)*sin(qB+qC)*cos(qB+qC)*qA'^2-2*LB*LC*sin(qC)*((qB'+qC')^2+cos(qB+
qC)^2*qA'^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 - (IB
yy-IBzz)*sin(qB)*cos(qB)*qA'^2 - (ICyy-ICzz)*sin(qB+qC)*cos(qB+qC)*qA'^2
-> (101) KaneDynamics[3] = 0.5*mC*g*LC*cos(qB+qC) + 0.25*mC*LC*(LC*sin(qB+qC)*
cos(qB+qC)*qA'^2+2*LB*sin(qB)*cos(qB)*cos(qC)*qA'^2+2*LB*sin(qC)*(qB'^2
+cos(qB)^2*qA'^2)) + 0.25*(4*ICxx+mC*LC^2)*qC'' + 0.25*(4*ICxx+mC*LC*(
LC+2*LB*cos(qC)))*qB'' - TC - (ICyy-ICzz)*sin(qB+qC)*cos(qB+qC)*qA'^2
(102) ShouldBeZero = Expand( KaneDynamics - Dynamics, 0:2 )
-> (103) ShouldBeZero = [0; 0; 0]
(104) %--------------------------------------------------------------------
(105) % Optional: Use dynamics equations to solve for TA, TB, TC via
(106) % Solve( Dynamics = 0, TA, TB, TC )
(107) % below use: Code Algebraic() ....
(108) % Alternative: Use Code Algebraic( Dynamics = 0, TA, TB, TC ) as shown below.
(109) %--------------------------------------------------------------------
(110) %% To pass values for qA, qA', qA'', etc. to the MATLAB function created below,
(111) %% Type, the following at the MATLAB command prompt:
(112) %% data = {0, 0.1, 0.2, pi/6, 0.3, 0.4, pi/4, 0.5, 0.6};
(113) %% Output = MGRobotStanfordArm3DInverseDynamics( data{:} );
(114) %% TA = Output( 1 )
(115) %% TB = Output( 2 )
(116) %% TC = Output( 3 )
(117) %--------------------------------------------------------------------
(118) % Create MATLAB function for inverse dynamics.
(119) OutputEncode TA N*m, TB N*m, TC N*m
(120) Code Algebraic( Dynamics = 0, TA, TB, TC ) MGRobotStanfordArm3DInverseDynamics.m( qA, qA', qA'', qB, qB', qB'', qC, qC', qC'' )
(121) %--------------------------------------------------------------------
Saved by Motion Genesis LLC. MotionGenesis 6.4 Professional. Command names and syntax: Copyright (c) 2024 Motion Genesis LLC. All rights reserved.