% MotionGenesis file: MGTriplePendulumDynamicsWithControl.txt % Copyright (c) 2009-2018 Motion Genesis LLC. All rights reserved. %-------------------------------------------------------------------- NewtonianFrame N % Ground (Earth). RigidBody A, B, C % Pendulum links. Point CN( C ) % Robot's end-effector. %-------------------------------------------------------------------- Constant L = 4 m % Length of links A, B, C. Constant g = 9.8 m/s^2 % Earth's gravity. Variable qA'', qB'', qC'' % Link angles. Specified TA, TB, TC % Motor torques: A from N, B from A, C from B. %-------------------------------------------------------------------- A.SetMassInertia( m = 0.8 kg, 0, I = m*L^2/12, I ) B.SetMassInertia( m, 0, I, I ) C.SetMassInertia( m, 0, I, I ) %--------------------------------------------------------------- % Rotational kinematics. A.RotateZ( N, qA ) B.RotateZ( N, qB ) C.RotateZ( N, qC ) %-------------------------------------------------------------------- % Translational kinematics. Ao.Translate( No, 0> ) Acm.Translate( Ao, -0.5*L*Ay> ) Bo.Translate( Ao, -L*Ay> ) Bcm.Translate( Bo, 0.5*L*Bx> ) Co.Translate( Bo, L*Bx> ) Ccm.Translate( Co, 0.5*L*Cy> ) CN.Translate( Co, L*Cy> ) %-------------------------------------------------------------------- % Add relevant forces. System.AddForceGravity( -g*Ny> ) %-------------------------------------------------------------------- % Add motor torques. A.AddTorque( N, TA*Az> ) B.AddTorque( A, TB*Bz> ) C.AddTorque( B, TC*Cz> ) %-------------------------------------------------------------------- % Form equations of motion with FBDs (D'Alembert/MG Road-maps). % Alternate: Kane/Lagrange methods with System.GetDynamicsKane(). Dynamics[1] = Dot( Nz>, System(A, B, C).GetDynamics(Ao) ) Dynamics[2] = Dot( Nz>, System(B, C).GetDynamics(Bo) ) Dynamics[3] = Dot( Nz>, C.GetDynamics(Co) ) %-------------------------------------------------------------------- % Integration parameters and initial values. Input tFinal = 9 sec, tStep = 0.02, absError = 1.0E-07 Input qA = -15 deg, qB = 0 deg, qC = 15 deg Input qA' = 0 rad/sec, qB' = 0 rad/sec, qC' = 0 rad/sec %-------------------------------------------------------------------- % Quantities to be output from ODE. Variable x'', y'' SetDt( x = Dot( Nx>, CN.GetPosition(No) ) ) SetDt( y = Dot( Ny>, CN.GetPosition(No) ) ) Output t sec, x m, y m, qA deg, qB deg, qC deg, TA N*m, TB N*m, TC N*m %-------------------------------------------------------------------- % Choose critically-damped control gains. Constant wn = 1.2 rad/sec % Speed of control. kp = wn^2 % Proportional control gain. kd = 2 * wn % Derivative control gain. %-------------------------------------------------------------------- % Joint-control: Provide desired angles and control equations. Specified qADesired'', qBDesired'', qCDesired'' Variable qAError'', qBError'', qCError'' SetDt( qADesired = sin(2*pi*t) ) SetDt( qBDesired = 0 ) SetDt( qCDesired = 0 ) SetDt( qAError = qA - qADesired ) SetDt( qBError = qB - qBDesired ) SetDt( qCError = qC - qCDesired ) ControlConstraint[1] = qAError'' + kd * qAError' + kp * qAError ControlConstraint[2] = qBError'' + kd * qBError' + kp * qBError ControlConstraint[3] = qCError'' + kd * qCError' + kp * qCError %-------------------------------------------------------------------- % Augment dynamics with constraints and simulate. ODE( [Dynamics; ControlConstraint] = 0, qA'', qB'', qC'', TA, TB, TC ) MGTriplePendulumDynamicsJointControl %-------------------------------------------------------------------- % End-effector control: Provide desired motion and control equations. Specified xDesired'', yDesired'' Variable xError'', yError'' SetDt( xDesired = L + L/2*sin(2*pi*t) ) SetDt( yDesired = 0 ) SetDt( qCDesired = 0 ) SetDt( xError = x - xDesired ) SetDt( yError = y - yDesired ) ControlConstraint[1] := xError'' + kd * xError' + kp * xError ControlConstraint[2] := yError'' + kd * yError' + kp * yError ControlConstraint[3] := qCError'' + kd * qCError' + kp * qCError %-------------------------------------------------------------------- % Augment dynamics with constraints and simulate ODE( [Dynamics; ControlConstraint] = 0, qA'', qB'', qC'', TA, TB, TC ) MGTriplePendulumDynamicsEndEffectorControl %-------------------------------------------------------------------- % Save input together with program responses Save MGTriplePendulumDynamicsWithControl.html Quit