multibody dynamics

%23%20%2F%2F%2F%20script%0A%23%20requires-python%20%3D%20%22%3E%3D3.11%22%0A%23%20dependencies%20%3D%20%5B%22marimo%3E%3D0.23.9%22%2C%20%22numpy%3D%3D2.4.3%22%2C%20%22matplotlib%3D%3D3.10.8%22%2C%20%22sympy%3D%3D1.14.0%22%2C%20%22urllib3%3D%3D2.6.3%22%2C%20%22ipython%3D%3D9.12.0%22%2C%20%22invkin2_2d%20%40%20..%2Fpublic%2Fwheels%2Finvkin2_2d-0.0.0%252Bmarimo.91f95b2124b9-py3-none-any.whl%22%2C%20%22minjerk%20%40%20..%2Fpublic%2Fwheels%2Fminjerk-0.0.0%252Bmarimo.3864ea8a8b69-py3-none-any.whl%22%2C%20%22notebook_resources%20%40%20..%2Fpublic%2Fwheels%2Fnotebook_resources-0.0.0%252Bmarimo.9f9afe92b910-py3-none-any.whl%22%5D%0A%23%20%0A%23%20%5Btool.marimo.export%5D%0A%23%20lock_kind%20%3D%20%22observed%22%0A%23%20%2F%2F%2F%0A%0Aimport%20marimo%0A%0A__generated_with%20%3D%20%220.25.2%22%0Aapp%20%3D%20marimo.App()%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20%23%20Multibody%20dynamics%20of%20one%20and%20two-link%20systems%0A%0A%20%20%20%20%3E%20Marcos%20Duarte%2C%20Renato%20Naville%20Watanabe%3Cbr%3E%0A%20%20%20%20%3E%20%5BLaboratory%20of%20Biomechanics%20and%20Motor%20Control%5D(https%3A%2F%2Fbmclab.pesquisa.ufabc.edu.br%2F)%3Cbr%3E%0A%20%20%20%20%3E%20Federal%20University%20of%20ABC%2C%20Brazil%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20%23%23%20How%20to%20use%20this%20guide%0A%0A%20%20%20%20Follow%20the%20one-link%20derivation%20before%20the%20two-link%20model.%20We%20will%20connect%20the%20Jacobian%2C%20joint%20and%20segment%20angles%2C%20interaction%20torques%2C%20inverse%20dynamics%20and%20forward%20dynamics.%20Use%20the%20slow%2C%20fast%20and%20horizontal-plane%20simulations%20as%20experiments%3A%20predict%20which%20terms%20disappear%20before%20running%20them.%0A%0A%20%20%20%20Try%20the%20distributed%20**Exercises**%20before%20opening%20their%20collapsed%20worked%20solutions.%20Then%20use%20the%20**Checkpoint%20questions**%20and%20attempt%20the%20**Problems**%20independently%3B%20their%20panels%20contain%20final%20answers%20only.%20Predict%20the%20effect%20of%20a%20parameter%20change%20before%20running%20it%2C%20and%20check%20units%20and%20limiting%20cases.%0A%0A%20%20%20%20**Opening%20challenge.**%20Move%20your%20forearm%20quickly%20while%20trying%20to%20keep%20your%20upper%20arm%20still.%20Why%20might%20the%20shoulder%20need%20a%20torque%20even%20when%20its%20angle%20barely%20changes%3F%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20%23%23%20Python%20setup%0A%0A%20%20%20%20The%20dependency%20declaration%20at%20the%20top%20of%20this%20file%20lists%20the%20packages%20used%20by%20this%20guide.%20Marimo%20runs%20cells%20in%20dependency%20order.%20Run%20the%20notebook%20in%20an%20editor%20to%20use%20its%20displays%20and%20controls%3B%20the%20computational%20examples%20below%20explain%20their%20inputs%20before%20using%20them.%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20The%20human%20body%20is%20composed%20of%20multiple%20interconnected%20segments%20(which%20can%20be%20modeled%20as%20rigid%20or%20flexible)%20and%20each%20segment%20may%20have%20translational%20and%20rotational%20movement.%20The%20part%20of%20mechanics%20for%20the%20study%20of%20movement%20and%20forces%20of%20interconnected%20bodies%20is%20called%20%5Bmultibody%20system%5D(https%3A%2F%2Fen.wikipedia.org%2Fwiki%2FMultibody_system)%20or%20multibody%20dynamics.%0A%0A%20%20%20%20There%20are%20different%20approaches%20to%20deduce%20the%20kinematics%20and%20dynamics%20of%20such%20bodies%2C%20the%20most%20common%20are%20the%20%5BNewton-Euler%5D(https%3A%2F%2Fen.wikipedia.org%2Fwiki%2FNewton%25E2%2580%2593Euler_equations)%20and%20the%20%5BLangrangian%5D(https%3A%2F%2Fen.wikipedia.org%2Fwiki%2FLagrangian_mechanics)%20formalisms.%20The%20Newton-Euler%20formalism%20is%20based%20on%20the%20well%20known%20Newton-Euler%20equations.%20The%20Langrangian%20formalism%20uses%20the%20%5Bprinciple%20of%20least%20action%5D(https%3A%2F%2Fen.wikipedia.org%2Fwiki%2FPrinciple_of_least_action)%20and%20describes%20the%20movement%20based%20on%20%5Bgeneralized%20coordinates%5D(https%3A%2F%2Fen.wikipedia.org%2Fwiki%2FGeneralized_coordinates)%2C%20a%20set%20of%20parameters%20(typically%2C%20a%20convenient%20minimal%20set)%20to%20describe%20the%20configuration%20of%20the%20system%20taking%20into%20account%20its%20constraints.%20For%20a%20system%20with%20multiple%20bodies%20and%20several%20constraints%2C%20e.g.%2C%20the%20human%20body%2C%20it%20is%20easier%20to%20describe%20the%20dynamics%20of%20such%20system%20using%20the%20Langrangian%20formalism.%0A%0A%20%20%20%20Zajac%20and%20Gordon%20(1989)%20and%20Zajac%20(1993)%20offer%20excellent%20discussions%20about%20applying%20multibody%20system%20concepts%20to%20understanding%20human%20body%20movement.%0A%0A%20%20%20%20Next%2C%20we%20will%20study%20two%20simple%20problems%20of%20multibody%20systems%20in%20the%20context%20of%20biomechanics%20which%20we%20can%20handle%20well%20using%20the%20Newton-Euler%20approach.%3Cbr%3E%0A%20%20%20%20First%20a%20planar%20one-link%20system%20(which%20is%20not%20a%20multibody)%2C%20which%20can%20represent%20the%20movement%20of%20one%20limb%20of%20the%20body%20or%20the%20entire%20body%20as%20a%20single%20inverted%20pendulum.%3Cbr%3E%0A%20%20%20%20Second%2C%20a%20planar%20two-link%20system%2C%20which%20can%20represent%20the%20movement%20of%20two%20segments%20of%20the%20body%2C%20e.g.%2C%20upper%20arm%20and%20forearm.%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20%23%23%20Newton-Euler%20equations%0A%0A%20%20%20%20For%20a%20two-dimensional%20movement%20in%20the%20%24XY%24%20plane%2C%20the%20Newton-Euler%20equations%20are%3A%0A%0A%20%20%20%20%24%24%0A%20%20%20%20%5Cbegin%7Baligned%7D%0A%20%20%20%20%5Cleft%5C%7B%20%5Cbegin%7Barray%7D%7Bl%20l%7D%0A%20%20%20%20%5Csum%20F_X%20%26%3D%26%20m%20%5Cddot%7Bx%7D_%7Bcm%7D%20%5C%5C%0A%20%20%20%20%5C%5C%0A%20%20%20%20%5Csum%20F_Y%20%26%3D%26%20m%20%5Cddot%7By%7D_%7Bcm%7D%20%5C%5C%0A%20%20%20%20%5C%5C%0A%20%20%20%20%5Csum%20M_Z%20%26%3D%26%20I_%7Bcm%7D%20%5Cddot%7B%5Calpha%7D_Z%0A%20%20%20%20%5Cend%7Barray%7D%20%5Cright.%0A%20%20%20%20%5Cend%7Baligned%7D%0A%20%20%20%20%24%24%0A%0A%20%20%20%20Where%20the%20movement%20is%20described%20around%20the%20body%20center%20of%20mass%20(%24cm%24).%3Cbr%3E%0A%20%20%20%20%24(F_X%2C%5C%2CF_Y)%24%20and%20%24M_Z%24%20are%2C%20respectively%2C%20the%20forces%20and%20moment%20of%20forces%20(torques)%20acting%20on%20the%20body.%3Cbr%3E%0A%20%20%20%20%24(%5Cddot%7Bx%7D_%7Bcm%7D%2C%5C%2C%5Cddot%7By%7D_%7Bcm%7D)%24%20and%20%24%5Cddot%7B%5Calpha%7D_Z%24%20are%2C%20respectively%2C%20the%20linear%20and%20angular%20accelerations.%3Cbr%3E%0A%20%20%20%20%24I_%7Bcm%7D%24%20is%20the%20body%20moment%20of%20inertia%20around%20the%20body%20center%20of%20mass%20at%20the%20%24Z%24%20axis.%0A%0A%20%20%20%20Let's%20use%20Sympy%20to%20derive%20some%20of%20the%20characteristics%20of%20the%20systems.%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_()%3A%0A%20%20%20%20import%20sympy%20as%20sym%0A%20%20%20%20from%20sympy%20import%20Symbol%2C%20symbols%2C%20cos%2C%20sin%2C%20Matrix%2C%20simplify%0A%20%20%20%20from%20sympy.vector%20import%20CoordSys3D%0A%20%20%20%20from%20sympy.physics.mechanics%20import%20dynamicsymbols%2C%20mlatex%2C%20init_vprinting%0A%20%20%20%20init_vprinting()%0A%20%20%20%20from%20IPython.display%20import%20display%2C%20Math%0A%0A%20%20%20%20eq%20%3D%20lambda%20lhs%2C%20rhs%3A%20display(Math(lhs%20%2B%20'%3D'%20%2B%20mlatex(rhs)))%0A%20%20%20%20eq%20%3D%20lambda%20lhs%2C%20rhs%3A%20display(Math(r'%5Cbegin%7Barray%7D%7Bl%20l%7D'%20%2B%20lhs%20%2B%0A%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20'%26%3D%26'%20%2B%20mlatex(rhs)%20%2B%20r'%5Cend%7Barray%7D'))%0A%20%20%20%20return%20(%0A%20%20%20%20%20%20%20%20CoordSys3D%2C%0A%20%20%20%20%20%20%20%20Math%2C%0A%20%20%20%20%20%20%20%20Matrix%2C%0A%20%20%20%20%20%20%20%20Symbol%2C%0A%20%20%20%20%20%20%20%20cos%2C%0A%20%20%20%20%20%20%20%20display%2C%0A%20%20%20%20%20%20%20%20dynamicsymbols%2C%0A%20%20%20%20%20%20%20%20eq%2C%0A%20%20%20%20%20%20%20%20mlatex%2C%0A%20%20%20%20%20%20%20%20simplify%2C%0A%20%20%20%20%20%20%20%20sin%2C%0A%20%20%20%20%20%20%20%20symbols%2C%0A%20%20%20%20)%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20%23%23%20One-link%20system%0A%0A%20%20%20%20Let's%20study%20the%20dynamics%20of%20a%20planar%20inverted%20pendulum%20as%20a%20model%20for%20the%20movement%20of%20a%20human%20body%20segment%20with%20an%20external%20force%20acting%20on%20the%20segment%20(see%20Figure%201).%0A%0A%20%20%20%20%3Cfigure%20aria-describedby%3D%22figure-caption-1%22%3E%3Cimg%20src%3D%22https%3A%2F%2Fgithub.com%2FBMClab%2FBMC%2Fblob%2Fmaster%2Fimages%2Finvpend1.png%3Fraw%3D1%22%20alt%3D%22Inverted%20pendulum%22%2F%3E%3C%2Ffigure%3E%0A%0A%20%20%20%20%3Cspan%20id%3D%22figure-caption-1%22%20class%3D%22figure-caption%22%20style%3D%22display%3Ablock%3Btext-align%3Acenter%3Bfont-style%3Aitalic%22%3EFigure%201.%20Planar%20inverted%20pendulum%20(one%20link%20attached%20to%20a%20fixed%20body%20by%20a%20hinge%20joint%20in%20a%20plane)%20with%20a%20joint%20actuators%20and%20corresponding%20free%20body%20diagram%20as%20a%20model%20of%20a%20human%20body%20segment.%20See%20text%20for%20notation%20convention.%3C%2Fspan%3E%0A%0A%20%20%20%20The%20following%20notation%20convention%20will%20be%20used%20for%20this%20problem%3A%0A%0A%20%20%20%20-%20%24L%24%20is%20the%20length%20of%20the%20segment.%3Cbr%3E%0A%20%20%20%20-%20%24d%24%20is%20the%20distance%20from%20the%20joint%20of%20the%20segment%20to%20its%20center%20of%20mass%20position.%3Cbr%3E%0A%20%20%20%20-%20%24m%24%20is%20the%20mass%20of%20the%20segment.%0A%20%20%20%20-%20%24g%24%20is%20the%20gravitational%20acceleration%20(%2B).%3Cbr%3E%0A%20%20%20%20-%20%24%5Calpha%24%20is%20the%20angular%20position%20of%20the%20joint%20w.r.t.%20horizontal%2C%20%24%5Cddot%7B%5Calpha_i%7D%24%20is%20the%20corresponding%20angular%20acceleration.%3Cbr%3E%0A%20%20%20%20-%20%24I%24%20is%20the%20moment%20of%20inertia%20of%20the%20segment%20around%20its%20center%20of%20mass%20position.%3Cbr%3E%0A%20%20%20%20-%20%24F_%7Br%7D%24%20is%20the%20joint%20reaction%20force.%3Cbr%3E%0A%20%20%20%20-%20%24F_%7Be%7D%24%20is%20the%20external%20force%20acting%20on%20the%20segment.%0A%20%20%20%20-%20%24T%24%20is%20the%20joint%20moment%20of%20force%20(torque).%0A%0A%20%20%20%20In%20the%20case%20of%20a%20human%20body%20segment%2C%20muscles%20responsible%20for%20the%20movement%20of%20the%20segment%20are%20represented%20as%20a%20single%20pair%20of%20antagonistic%20joint%20actuators%20(e.g.%2C%20flexors%20and%20extensors).%20We%20will%20consider%20that%20all%20joint%20torques%20are%20generated%20only%20by%20these%20muscles%20(we%20will%20disregard%20the%20torques%20generated%20by%20ligaments%20and%20other%20tissues)%20and%20the%20total%20or%20net%20joint%20torque%20will%20be%20the%20sum%20of%20the%20torques%20generated%20by%20the%20two%20muscles%3A%0A%0A%20%20%20%20%24%24%0A%20%20%20%20T%20%5Cquad%3D%5Cquad%20T_%7Bnet%7D%20%5Cquad%3D%5Cquad%20T_%7Bextension%7D%20-%20T_%7Bflexion%7D%0A%20%20%20%20%24%24%0A%0A%20%20%20%20Where%20we%20considered%20the%20extensor%20torque%20as%20positive%20(counter-clockwise).%20In%20what%20follows%2C%20we%20will%20determine%20only%20the%20net%20torque%20and%20we%20will%20be%20unable%20to%20decompose%20the%20net%20torque%20in%20its%20components.%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20**Exercise%202%20%E2%80%94%20Distinguish%20joint%20and%20segment%20angles.**%20The%20first%20segment%20is%20at%20%2430%5E%5Ccirc%24%20to%20horizontal%20and%20the%20second%20joint%20angle%20relative%20to%20it%20is%20%2460%5E%5Ccirc%24.%20What%20is%20the%20second%20segment%20angle%3F%20If%20their%20rates%20are%202%20and%20%E2%88%921%20rad%2Fs%2C%20what%20is%20its%20angular%20velocity%3F%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20%2F%2F%2F%20details%20%7C%20Solution%20to%20Exercise%202%0A%0A%20%20%20%20%24%5Ctheta_2%3D%5Calpha_1%2B%5Calpha_2%3D90%5E%5Ccirc%24%20and%20%24%5Cdot%5Ctheta_2%3D2-1%3D1%24%20rad%2Fs.%20Adding%20angles%20and%20rates%20is%20specific%20to%20this%20planar%20convention%3B%20mixing%20joint%20and%20segment%20variables%20changes%20the%20equations.%0A%20%20%20%20%2F%2F%2F%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20%23%23%23%20Kinetics%0A%0A%20%20%20%20From%20the%20free%20body%20diagram%2C%20the%20Newton-Euler%20equations%20for%20the%20planar%20inverted%20pendulum%20are%20(writing%20the%20equation%20for%20the%20torques%20around%20the%20center%20of%20mass)%3A%0A%0A%20%20%20%20%24%24%0A%20%20%20%20%5Cbegin%7Barray%7D%7Bl%20l%7D%0A%20%20%20%20F_%7Br%2Cx%7D%20%2B%20F_%7Be%2Cx%7D%20%26%20%3D%20%26%20m%5Cddot%7Bx%7D%20%5C%5C%0A%20%20%20%20%5C%5C%0A%20%20%20%20F_%7Br%2Cy%7D%20-%20mg%20%2B%20F_%7Be%2Cy%7D%20%26%20%3D%20%26%20m%5Cddot%7By%7D%20%5C%5C%0A%20%20%20%20%5C%5C%0A%20%20%20%20T%20%2B%20dF_%7Br%2Cx%7D%5Csin%5Calpha%20-%20dF_%7Br%2Cy%7D%5Ccos%5Calpha%20-%20(L-d)F_%7Be%2Cx%7D%5Csin%5Calpha%20%2B%20(L-d)F_%7Be%2Cy%7D%5Ccos%5Calpha%20%26%20%3D%20%26%20I%5Cddot%7B%5Calpha%7D%0A%20%20%20%20%5Cend%7Barray%7D%0A%20%20%20%20%24%24%0A%0A%20%20%20%20However%2C%20manually%20placing%20the%20terms%20in%20the%20Newton-Euler%20equations%20as%20we%20did%20above%20where%20we%20calculated%20the%20signs%20of%20the%20cross%20products%20is%20error%20prone.%20We%20can%20avoid%20this%20manual%20placing%20by%20treating%20the%20quantities%20as%20vectors%20and%20express%20them%20in%20matricial%20form%3A%0A%0A%20%20%20%20%24%24%0A%20%20%20%20%5Cbegin%7Barray%7D%7Bl%20l%7D%0A%20%20%20%20%5Cvec%7B%5Cmathbf%7BF%7D%7D_r%20%2B%20%5Cvec%7B%5Cmathbf%7BF%7D%7D_g%20%2B%20%5Cvec%7B%5Cmathbf%7BF%7D%7D_e%20%26%3D%26%20m%5Cddot%7B%5Cvec%7B%5Cmathbf%7Br%7D%7D%7D%20%5C%5C%0A%20%20%20%20%5C%5C%0A%20%20%20%20%5Cvec%7B%5Cmathbf%7BT%7D%7D%20%2B%20%5Cvec%7B%5Cmathbf%7Br%7D%7D_%7Bcm%2Cj%7D%20%5Ctimes%20%5Cvec%7B%5Cmathbf%7BF%7D%7D_r%20%2B%20%5Cvec%7B%5Cmathbf%7Br%7D%7D_%7Bcm%2Ce%7D%20%5Ctimes%20%5Cvec%7B%5Cmathbf%7BF%7D%7D_e%20%26%3D%26%20I%5Cddot%7B%5Cvec%7B%5Cmathbf%7B%5Calpha%7D%7D%7D%0A%20%20%20%20%5Cend%7Barray%7D%0A%20%20%20%20%24%24%0A%0A%20%20%20%20Where%3A%0A%0A%20%20%20%20%24%24%0A%20%20%20%20%5Cbegin%7Barray%7D%7Bl%20l%7D%0A%20%20%20%20%5Cbegin%7Bbmatrix%7D%20F_%7Brx%7D%20%5C%5C%20F_%7Bry%7D%20%5C%5C%200%20%5Cend%7Bbmatrix%7D%20%20%2B%20%5Cbegin%7Bbmatrix%7D%200%20%5C%5C%20-g%20%5C%5C%200%20%5Cend%7Bbmatrix%7D%20%20%2B%20%5Cbegin%7Bbmatrix%7D%20F_%7Bex%7D%20%5C%5C%20F_%7Bey%7D%20%5C%5C%200%20%5Cend%7Bbmatrix%7D%20%26%3D%26%20m%5Cbegin%7Bbmatrix%7D%20%5Cddot%7Bx%7D%20%5C%5C%20%5Cddot%7By%7D%20%5C%5C%200%20%5Cend%7Bbmatrix%7D%20%2C%20%5Cquad%20%5Cbegin%7Bbmatrix%7D%20%5Chat%7Bi%7D%20%5C%5C%20%5Chat%7Bj%7D%20%5C%5C%20%5Chat%7Bk%7D%20%5Cend%7Bbmatrix%7D%0A%20%20%20%20%5C%5C%0A%20%20%20%20%5Cbegin%7Bbmatrix%7D%200%20%5C%5C%200%20%5C%5C%20T_z%20%5Cend%7Bbmatrix%7D%20%20%2B%20%5Cbegin%7Bbmatrix%7D%20-d%5Ccos%5Calpha%20%5C%5C%20-d%5Csin%5Calpha%20%5C%5C%200%20%5Cend%7Bbmatrix%7D%20%20%5Ctimes%20%5Cbegin%7Bbmatrix%7D%20F_%7Brx%7D%20%5C%5C%20F_%7Bry%7D%20%5C%5C%200%20%5Cend%7Bbmatrix%7D%20%2B%20%5Cbegin%7Bbmatrix%7D%20(L-d)%5Ccos%5Calpha%20%5C%5C%20(L-d)%5Csin%5Calpha%20%5C%5C%200%20%5Cend%7Bbmatrix%7D%20%20%5Ctimes%20%5Cbegin%7Bbmatrix%7D%20F_%7Bex%7D%20%5C%5C%20F_%7Bey%7D%20%5C%5C%200%20%5Cend%7Bbmatrix%7D%20%26%3D%26%20I_z%5Cbegin%7Bbmatrix%7D%200%20%5C%5C%200%20%5C%5C%20%5Cddot%7B%5Calpha%7D%20%5Cend%7Bbmatrix%7D%20%2C%20%5Cquad%20%5Cbegin%7Bbmatrix%7D%20%5Chat%7Bi%7D%20%5C%5C%20%5Chat%7Bj%7D%20%5C%5C%20%5Chat%7Bk%7D%20%5Cend%7Bbmatrix%7D%0A%20%20%20%20%5Cend%7Barray%7D%0A%20%20%20%20%24%24%0A%0A%20%20%20%20Note%20that%20%24%5Ctimes%24%20represents%20the%20cross%20product%2C%20not%20matrix%20multiplication.%20Then%2C%20both%20in%20symbolic%20or%20numeric%20manipulation%20we%20would%20use%20the%20cross%20product%20function%20to%20perform%20part%20of%20the%20calculations.%3Cbr%3E%0A%20%20%20%20There%20are%20different%20computational%20tools%20that%20can%20be%20used%20for%20the%20formulation%20of%20the%20equations%20of%20motion.%20For%20instance%2C%20Sympy%20has%20a%20module%2C%20%5BClassical%20Mechanics%5D(http%3A%2F%2Fdocs.sympy.org%2Fdev%2Fmodules%2Fphysics%2Fmechanics%2F)%2C%20and%20see%20%5Bthis%20list%5D(http%3A%2F%2Freal.uwaterloo.ca%2F~mbody%2F%23Software)%20for%20other%20software.%3Cbr%3E%0A%20%20%20%20Let's%20continue%20with%20the%20explicit%20manual%20formulation%20of%20the%20equations%20for%20now.%0A%0A%20%20%20%20We%20can%20rewrite%20the%20equation%20for%20the%20moments%20of%20force%20in%20a%20form%20that%20doesn't%20explicitly%20involve%20the%20joint%20reaction%20force%20expressing%20the%20moments%20of%20force%20around%20the%20joint%20center%3A%0A%0A%20%20%20%20%24%24%0A%20%20%20%20T%20-%20mgd%5Ccos%5Calpha%20-%20LF_%7Be%2Cx%7D%5Csin%5Calpha%20%2B%20LF_%7Be%2Cy%7D%5Ccos%5Calpha%20%5Cquad%3D%5Cquad%20I_o%5Cddot%7B%5Calpha%7D%0A%20%20%20%20%24%24%0A%0A%20%20%20%20Where%20%24I_o%24%20is%20the%20moment%20of%20inertia%20around%20the%20joint%2C%20%24I_o%3DI_%7Bcm%7D%2Bmd%5E2%24%2C%20using%20the%20parallel%20axis%20theorem.%0A%0A%20%20%20%20The%20torque%20due%20to%20the%20joint%20reaction%20force%20does%20not%20appear%20on%20this%20equation%3B%20this%20torque%20is%20null%20because%20by%20the%20definition%20the%20reaction%20force%20acts%20on%20the%20joint.%20If%20we%20want%20to%20determine%20the%20joint%20torque%20and%20we%20know%20the%20kinematics%2C%20we%20perform%20inverse%20dynamics%3A%0A%0A%20%20%20%20%24%24%0A%20%20%20%20T%20%5Cquad%3D%5Cquad%20I_o%5Cddot%7B%5Calpha%7D%20%2B%20mgd%20%5Ccos%20%5Calpha%20%2B%20LF_%7Be%2Cx%7D%5Csin%5Calpha%20-%20LF_%7Be%2Cy%7D%5Ccos%5Calpha%0A%20%20%20%20%24%24%0A%0A%20%20%20%20If%20we%20want%20to%20determine%20the%20kinematics%20and%20we%20know%20the%20joint%20torque%2C%20we%20perform%20direct%20dynamics%3A%0A%0A%20%20%20%20%24%24%0A%20%20%20%20%5Cddot%7B%5Calpha%7D%20%5Cquad%3D%5Cquad%20I_o%5E%7B-1%7D%5BT%20-%20mgd%20%5Ccos%20%5Calpha%20-%20LF_%7Be%2Cx%7D%5Csin%5Calpha%20%2B%20LF_%7Be%2Cy%7D%5Ccos%5Calpha%20%5D%0A%20%20%20%20%24%24%0A%0A%20%20%20%20The%20expression%20above%20is%20a%20second-order%20differential%20equation%20which%20typically%20is%20solved%20numerically.%20So%2C%20unless%20we%20are%20explicitly%20interested%20in%20estimating%20the%20joint%20reaction%20forces%2C%20we%20don't%20need%20to%20use%20them%20for%20calculating%20the%20joint%20torque%20or%20simulate%20movement.%20Anyway%2C%20let's%20look%20at%20the%20kinematics%20of%20this%20problem%20to%20introduce%20some%20important%20concepts%20which%20will%20be%20needed%20later.%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20%23%23%23%20Kinematics%0A%0A%20%20%20%20A%20single%20planar%20inverted%20pendulum%20has%20one%20degree%20of%20freedom%2C%20the%20rotation%20movement%20of%20the%20segment%20around%20the%20pin%20joint.%20In%20this%20case%2C%20if%20the%20angular%20position%20%24%5Calpha(t)%24%20is%20known%2C%20the%20coordinates%20%24x(t)%24%20and%20%24y(t)%24%20of%20the%20center%20of%20mass%20and%20their%20derivatives%20can%20be%20readily%20determined%20(a%20process%20referred%20as%20%5Bforward%20kinematics)%5D(kinematic_chain.py%23forward-and-inverse-kinematics)%3A%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(Symbol%2C%20dynamicsymbols%2C%20symbols)%3A%0A%20%20%20%20t%20%3D%20Symbol('t')%0A%20%20%20%20d%2C%20L%20%3D%20symbols('d%20L'%2C%20positive%3DTrue)%0A%20%20%20%20a%20%3D%20dynamicsymbols('alpha')%0A%20%20%20%20return%20L%2C%20a%2C%20d%2C%20t%0A%0A%0A%40app.cell%0Adef%20_(a%2C%20cos%2C%20d%2C%20eq%2C%20sin%2C%20t)%3A%0A%20%20%20%20x%2C%20y%20%3D%20d*cos(a)%2C%20d*sin(a)%0A%20%20%20%20xd%2C%20yd%20%3D%20x.diff(t)%2C%20y.diff(t)%0A%20%20%20%20xdd%2C%20ydd%20%3D%20xd.diff(t)%2C%20yd.diff(t)%0A%0A%20%20%20%20eq(r'x'%2C%20x)%0A%20%20%20%20eq(r'%5Cdot%7Bx%7D'%2C%20xd)%0A%20%20%20%20eq(r'%5Cddot%7Bx%7D'%2C%20xdd)%0A%20%20%20%20eq(r'y'%2C%20y)%0A%20%20%20%20eq(r'%5Cdot%7By%7D'%2C%20yd)%0A%20%20%20%20eq(r'%5Cddot%7By%7D'%2C%20ydd)%0A%20%20%20%20return%20x%2C%20xdd%2C%20y%2C%20ydd%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20The%20terms%20in%20%24%5Cddot%7Bx%7D%24%20and%20%24%5Cddot%7By%7D%24%20proportional%20to%20%24%5Cdot%7B%5Calpha%7D%5E2%24%20are%20components%20of%20the%20centripetal%20acceleration%20on%20the%20body.%20As%20the%20name%20suggests%2C%20the%20%5Bcentripetal%5D(https%3A%2F%2Fen.wikipedia.org%2Fwiki%2FCentripetal_force)%20acceleration%20is%20always%20directed%20to%20the%20center%20(towards%20the%20joint)%20when%20the%20segment%20is%20rotating.%20See%20the%20notebook%20%5BKinematic%20chain%5D(https%3A%2F%2Fmolab.marimo.io%2Fgithub%2FBMClab%2FBMC%2Fblob%2Fmaster%2Fnotebooks%2Fkinematic_chain.py)%20for%20more%20on%20that.%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20We%20could%20also%20use%20the%20methods%20of%20the%20Sympy%20physics%2Fmechanics%20module%20and%20explicitly%20create%20a%20coordinate%20system%20in%203-D%20space%2C%20which%20will%20employ%20the%20versors%20%24%5Chat%7B%5Cmathbf%7Bi%7D%7D%2C%20%5Chat%7B%5Cmathbf%7Bj%7D%7D%2C%20%5Chat%7B%5Cmathbf%7Bk%7D%7D%24%20for%20representing%20the%20vector%20components%3A%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(CoordSys3D%2C%20a%2C%20cos%2C%20d%2C%20eq%2C%20sin%2C%20t)%3A%0A%20%20%20%20G%20%3D%20CoordSys3D('')%0A%20%20%20%20r%20%3D%20d*cos(a)*G.i%20%2B%20d*sin(a)*G.j%20%2B%200*G.k%0A%20%20%20%20rd%20%3D%20r.diff(t)%0A%20%20%20%20rdd%20%3D%20r.diff(t%2C%202)%0A%0A%20%20%20%20eq(r'%5Cvec%7B%5Cmathbf%7Br%7D%7D'%2C%20r)%0A%20%20%20%20eq(r'%5Cdot%7B%5Cvec%7B%5Cmathbf%7Br%7D%7D%7D'%2C%20rd)%0A%20%20%20%20eq(r'%5Cddot%7B%5Cvec%7B%5Cmathbf%7Br%7D%7D%7D'%2C%20rdd)%0A%20%20%20%20return%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20But%20for%20now%2C%20let's%20continue%20writing%20the%20components%20ourselves.%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20As%20an%20exercise%2C%20let's%20go%20back%20to%20the%20Newton-Euler%20equation%20for%20the%20sum%20of%20torques%20around%20the%20center%20of%20mass%20where%20the%20torques%20due%20to%20the%20joint%20reaction%20forces%20are%20explicit.%3Cbr%3E%0A%20%20%20%20From%20the%20equation%20for%20the%20the%20sum%20of%20forces%2C%20hence%20we%20have%20expressions%20for%20the%20linear%20accelerations%2C%20we%20can%20isolate%20the%20reaction%20forces%20and%20substitute%20them%20on%20the%20equation%20for%20the%20torques.%20With%20a%20little%20help%20from%20Sympy%3A%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(a%2C%20symbols%2C%20t)%3A%0A%20%20%20%20m%2C%20I%2C%20g%20%3D%20symbols('m%20I%20g'%2C%20positive%3DTrue)%0A%20%20%20%20Fex%2C%20Fey%20%3D%20symbols('F_ex%20F_ey')%0A%20%20%20%20add%20%3D%20a.diff(t%2C%202)%0A%20%20%20%20return%20Fex%2C%20Fey%2C%20I%2C%20add%2C%20g%2C%20m%0A%0A%0A%40app.cell%0Adef%20_(Fex%2C%20Fey%2C%20eq%2C%20g%2C%20m%2C%20xdd%2C%20ydd)%3A%0A%20%20%20%20Frx%20%3D%20m*xdd%20-%20Fex%0A%20%20%20%20Fry%20%3D%20m*ydd%20%2B%20m*g%20-%20Fey%0A%20%20%20%20eq(r'F_%7Brx%7D'%2C%20Frx)%0A%20%20%20%20eq(r'F_%7Bry%7D'%2C%20Fry)%0A%20%20%20%20return%20Frx%2C%20Fry%0A%0A%0A%40app.cell%0Adef%20_(Fex%2C%20Fey%2C%20Frx%2C%20Fry%2C%20I%2C%20L%2C%20a%2C%20add%2C%20cos%2C%20d%2C%20eq%2C%20sin)%3A%0A%20%20%20%20T%20%3D%20I*add%20-%20d*sin(a)*Frx%20%2B%20d*cos(a)*Fry%20%2B%20(L-d)*sin(a)*Fex%20-%20(L-d)*cos(a)*Fey%0A%20%20%20%20eq(r'T'%2C%20T)%0A%20%20%20%20return%20(T%2C)%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20This%20equation%20for%20the%20torques%20around%20the%20center%20of%20mass%20of%20only%20one%20rotating%20segment%20seems%20too%20complicated.%20The%20equation%20we%20derived%20before%20for%20the%20torques%20around%20the%20joint%20was%20much%20simpler.%20However%2C%20if%20we%20look%20at%20the%20terms%20on%20this%20last%20equation%2C%20we%20can%20simplify%20most%20of%20them.%20Let's%20use%20Sympy%20to%20simplify%20this%20equation%3A%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(T%2C%20eq%2C%20simplify)%3A%0A%20%20%20%20T_1%20%3D%20simplify(T)%0A%20%20%20%20eq('T'%2C%20T_1)%0A%20%20%20%20return%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20And%20we%20are%20back%20to%20the%20more%20simple%20equation%20we've%20seen%20before.%20The%20first%20two%20terms%20on%20the%20right%20side%20are%20the%20torque%20due%20to%20the%20external%20force%2C%20the%20third%20and%20fourth%20are%20the%20moment%20of%20inertia%20around%20the%20joint%20(use%20the%20theorem%20of%20parallel%20axis)%20times%20the%20acceleration%2C%20and%20the%20last%20term%20is%20the%20gravitational%20torque.%0A%0A%20%20%20%20But%20what%20happened%20with%20all%20the%20other%20terms%20in%20the%20equation%3F%0A%0A%20%20%20%20First%2C%20the%20terms%20proportional%20to%20the%20angular%20acceleration%20were%20just%20components%20from%20each%20direction%20of%20the%20'inertial'%20torque%20that%20when%20summed%20resulted%20in%20%24md%5E2%5Cddot%7B%5Calpha%7D%24.%0A%20%20%20%20Second%2C%20the%20terms%20proportional%20to%20%24%5Cdot%7B%5Calpha%7D%5E2%24%20are%20components%20of%20the%20torque%20due%20to%20the%20centripetal%20force%20(acceleration).%20But%20the%20centripetal%20force%20passes%20through%20the%20joint%20as%20well%20as%20through%20the%20center%20of%20mass%2C%20i.e.%2C%20it%20has%20zero%20lever%20arm%20and%20this%20torque%20should%20be%20zero.%20Indeed%2C%20when%20summed%20these%20terms%20are%20canceled%20out.%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20%23%23%23%20The%20Jacobian%20matrix%0A%0A%20%20%20%20Another%20way%20to%20deduce%20the%20velocity%20and%20acceleration%20of%20a%20point%20at%20the%20rotating%20link%20is%20to%20use%20the%20%5BJacobian%20matrix%5D(https%3A%2F%2Fen.wikipedia.org%2Fwiki%2FJacobian_matrix_and_determinant)%20(see%20%5BKinematic%20chain%5D(https%3A%2F%2Fmolab.marimo.io%2Fgithub%2FBMClab%2FBMC%2Fblob%2Fmaster%2Fnotebooks%2Fkinematic_chain.py)).%3Cbr%3E%0A%20%20%20%20Remember%20that%20in%20the%20context%20of%20kinematic%20chains%2C%20the%20Jacobian%20relates%20changes%20in%20the%20joint%20space%20to%20changes%20in%20the%20Cartesian%20space.%20The%20Jacobian%20is%20a%20matrix%20of%20all%20first-order%20partial%20derivatives%20of%20the%20linear%20position%20vector%20of%20the%20endpoint%20with%20respect%20to%20the%20angular%20position%20vector.%3Cbr%3E%0A%20%20%20%20For%20the%20center%20of%20mass%20of%20the%20planar%20one-link%20system%2C%20this%20means%20that%20the%20Jacobian%20matrix%20is%3A%0A%0A%20%20%20%20%24%24%0A%20%20%20%20%5Cmathbf%7BJ%7D%20%5Cquad%3D%5Cquad%0A%20%20%20%20%5Cbegin%7Bbmatrix%7D%0A%20%20%20%20%5Cdfrac%7B%5Cpartial%20x%7D%7B%5Cpartial%20%5Calpha%7D%20%5C%5C%0A%20%20%20%20%5Cdfrac%7B%5Cpartial%20y%7D%7B%5Cpartial%20%5Calpha%7D%20%5C%5C%0A%20%20%20%20%5Cend%7Bbmatrix%7D%0A%20%20%20%20%24%24%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(Matrix%2C%20a%2C%20eq%2C%20x%2C%20y)%3A%0A%20%20%20%20r_1%20%3D%20Matrix(%5Bx%2C%20y%5D)%0A%20%20%20%20J%20%3D%20r_1.diff(a)%0A%20%20%20%20eq('%5C%5Cmathbf%7BJ%7D'%2C%20J)%0A%20%20%20%20return%20(r_1%2C)%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20And%20Sympy%20has%20a%20function%20to%20calculate%20the%20Jacobian%3A%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(a%2C%20eq%2C%20r_1)%3A%0A%20%20%20%20J_1%20%3D%20r_1.jacobian(%5Ba%5D)%0A%20%20%20%20eq('%5C%5Cmathbf%7BJ%7D'%2C%20J_1)%0A%20%20%20%20return%20(J_1%2C)%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20The%20linear%20velocity%20of%20a%20point%20in%20the%20link%20will%20be%20given%20by%20the%20product%20between%20the%20Jacobian%20of%20the%20kinematic%20link%20and%20its%20angular%20velocity%3A%0A%0A%20%20%20%20%24%24%0A%20%20%20%20%5Cvec%7B%5Cmathbf%7Bv%7D%7D%20%5Cquad%3D%5Cquad%20%5Cmathbf%7BJ%7D%20%5Cdot%7B%5Cvec%7B%5Calpha%7D%7D%0A%20%20%20%20%24%24%0A%0A%20%20%20%20Using%20Sympy%2C%20the%20linear%20velocity%20of%20the%20center%20of%20mass%20is%3A%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(J_1%2C%20a%2C%20eq%2C%20t)%3A%0A%20%20%20%20vel%20%3D%20J_1%20*%20a.diff(t)%0A%20%20%20%20eq('%5C%5Cbegin%7Bbmatrix%7D%20%5C%5Cdot%7Bx%7D%20%5C%5C%5C%5C%20%5C%5Cdot%7By%7D%20%5C%5Cend%7Bbmatrix%7D'%2C%20vel)%0A%20%20%20%20return%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20And%20the%20linear%20acceleration%20of%20a%20point%20in%20the%20link%20will%20be%20given%20by%20the%20derivative%20of%20this%20last%20expression%3A%0A%0A%20%20%20%20%24%24%0A%20%20%20%20%5Cvec%7B%5Cmathbf%7Ba%7D%7D%20%5Cquad%3D%5Cquad%20%5Cdot%7B%5Cmathbf%7BJ%7D%7D%20%5Cdot%7B%5Cvec%7B%5Calpha%7D%7D%20%2B%20%5Cmathbf%7BJ%7D%20%5Cddot%7B%5Cvec%7B%5Calpha%7D%7D%0A%20%20%20%20%24%24%0A%0A%20%20%20%20And%20using%20Sympy%20again%2C%20the%20linear%20acceleration%20of%20the%20center%20of%20mass%20is%3A%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(J_1%2C%20a%2C%20eq%2C%20t)%3A%0A%20%20%20%20acc%20%3D%20(J_1%20*%20a.diff(t)).diff(t)%0A%20%20%20%20eq('%5C%5Cbegin%7Bbmatrix%7D%20%5C%5Cddot%7Bx%7D%20%5C%5C%5C%5C%20%5C%5Cddot%7By%7D%20%5C%5Cend%7Bbmatrix%7D'%2C%20acc)%0A%20%20%20%20return%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20Same%20expressions%20as%20before.%0A%0A%20%20%20%20We%20can%20also%20use%20the%20Jacobian%20matrix%20to%20calculate%20the%20torque%20due%20to%20a%20force%20on%20the%20link%3A%0A%0A%20%20%20%20%24%24%0A%20%20%20%20T%20%5Cquad%3D%5Cquad%20%5Cmathbf%7BJ%7D%5ET%20%5Cbegin%7Bbmatrix%7D%20F_%7Bex%7D%20%5C%5C%20F_%7Bey%7D%20%5Cend%7Bbmatrix%7D%0A%20%20%20%20%24%24%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(Fex%2C%20Fey%2C%20J_1%2C%20Matrix%2C%20eq)%3A%0A%20%20%20%20Te%20%3D%20J_1.T%20*%20Matrix((Fex%2C%20Fey))%0A%20%20%20%20eq('T_e'%2C%20Te%5B0%5D)%0A%20%20%20%20return%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20Where%20in%20this%20case%20we%20considered%20that%20the%20force%20was%20applied%20to%20the%20center%20of%20mass%2C%20just%20because%20we%20already%20had%20the%20Jacobian%20calculated%20at%20that%20position.%0A%0A%20%20%20%20We%20could%20simulate%20the%20movement%20of%20this%20one-link%20system%20for%20a%20typical%20human%20movement%20to%20understand%20the%20magnitude%20of%20these%20physical%20quantities.%3Cbr%3E%0A%20%20%20%20The%20reader%20is%20invited%20to%20that%20now.%20We%20will%20perform%20this%20simulation%20for%20a%20two-link%20system%20next%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20**Exercise%201%20%E2%80%94%20Read%20a%20Jacobian%20column.**%20For%20a%20planar%20link%20of%20length%20%24L%24%20with%20endpoint%20%24(L%5Ccos%20q%2CL%5Csin%20q)%24%2C%20find%20its%20endpoint%20velocity%20for%20angular%20velocity%20%24%5Cdot%20q%24.%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20%2F%2F%2F%20details%20%7C%20Solution%20to%20Exercise%201%0A%0A%20%20%20%20Differentiate%20with%20respect%20to%20%24q%24%3A%20%24J%3D(-L%5Csin%20q%2CL%5Ccos%20q)%5ET%24%2C%20so%20%24%5Cmathbf%20v%3DJ%5Cdot%20q%24.%20At%20%24q%3D0%24%2C%20the%20velocity%20is%20vertical.%20The%20Jacobian%20maps%20a%20joint%20rate%20to%20a%20Cartesian%20velocity%3B%20it%20is%20not%20itself%20a%20rotation%20matrix.%0A%20%20%20%20%2F%2F%2F%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20%23%23%20Two-link%20system%0A%0A%20%20%20%20Let's%20study%20the%20dynamics%20of%20a%20planar%20double%20inverted%20pendulum%20(see%20Figure%202)%20as%20a%20model%20of%20two%20interconnected%20segments%20in%20the%20human%20body%20with%20an%20external%20force%20acting%20on%20the%20distal%20segment.%20Once%20again%2C%20we%20will%20consider%20that%20there%20are%20muscles%20around%20each%20joint%20and%20they%20generate%20torques.%0A%0A%20%20%20%20%3Cfigure%20aria-describedby%3D%22figure-caption-2%22%3E%3Cimg%20src%3D%22https%3A%2F%2Fgithub.com%2FBMClab%2FBMC%2Fblob%2Fmaster%2Fimages%2Finvpend2.png%3Fraw%3D1%22%20alt%3D%22Double%20inverted%20pendulum%22%2F%3E%3C%2Ffigure%3E%0A%0A%20%20%20%20%3Cspan%20id%3D%22figure-caption-2%22%20class%3D%22figure-caption%22%20style%3D%22display%3Ablock%3Btext-align%3Acenter%3Bfont-style%3Aitalic%22%3EFigure%202.%20Planar%20double%20inverted%20pendulum%20connected%20by%20hinge%20joints%20with%20joint%20actuators%20and%20corresponding%20free%20body%20diagrams.%20See%20text%20for%20notation%20convention.%3C%2Fspan%3E%0A%0A%20%20%20%20The%20following%20notation%20convention%20will%20be%20used%20for%20this%20problem%3A%3Cbr%3E%0A%20%20%20%20-%20Subscript%20%24i%24%20runs%201%20or%202%20meaning%20first%20(most%20proximal)%20or%20second%20joint%20when%20referring%20to%20angles%2C%20joint%20moments%2C%20or%20joint%20reaction%20forces%2C%20or%20meaning%20first%20or%20second%20segment%20when%20referring%20to%20everything%20else.%3Cbr%3E%0A%20%20%20%20-%20%24L_i%24%20is%20the%20length%20of%20segment%20%24i%24.%3Cbr%3E%0A%20%20%20%20-%20%24d_i%24%20is%20the%20distance%20from%20the%20proximal%20joint%20of%20segment%20%24i%24%20to%20its%20center%20of%20mass%20position.%3Cbr%3E%0A%20%20%20%20-%20%24m_i%24%20is%20the%20mass%20of%20segment%20%24i%24.%0A%20%20%20%20-%20%24g%24%20is%20the%20gravitational%20acceleration%20(%2B).%3Cbr%3E%0A%20%20%20%20-%20%24%5Calpha_i%24%20is%20the%20angular%20position%20of%20joint%20%24i%24%20in%20the%20joint%20space%2C%20%24%5Cddot%7B%5Calpha_i%7D%24%20is%20the%20corresponding%20angular%20acceleration.%0A%20%20%20%20-%20%24%5Ctheta_i%24%20is%20the%20angular%20position%20of%20joint%20%24i%24%20in%20the%20segmental%20space%20w.r.t.%20horizontal%2C%20%24%5Ctheta_1%3D%5Calpha_1%24%20and%20%24%5Ctheta_2%3D%5Calpha_1%2B%5Calpha_2%24.%3Cbr%3E%0A%20%20%20%20-%20%24I_i%24%20is%20the%20moment%20of%20inertia%20of%20segment%20%24i%24%20around%20its%20center%20of%20mass%20position.%3Cbr%3E%0A%20%20%20%20-%20%24F_%7Bri%7D%24%20is%20the%20reaction%20force%20at%20joint%20%24i%24.%3Cbr%3E%0A%20%20%20%20-%20%24F_%7Be%7D%24%20is%20the%20external%20force%20acting%20on%20the%20distal%20segment.%0A%20%20%20%20-%20%24T_i%24%20is%20the%20moment%20of%20force%20(torque)%20at%20joint%20%24i%24.%0A%0A%20%20%20%20Hence%20we%20know%20we%20will%20need%20the%20linear%20accelerations%20for%20solving%20the%20Newton-Euler%20equations%2C%20let's%20deduce%20them%20first.%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(Symbol%2C%20dynamicsymbols%2C%20symbols)%3A%0A%20%20%20%20t_1%20%3D%20Symbol('t')%0A%20%20%20%20(d1%2C%20d2%2C%20L1%2C%20L2)%20%3D%20symbols('d1%2C%20d2%2C%20L_1%20L_2'%2C%20positive%3DTrue)%0A%20%20%20%20(a1%2C%20a2)%20%3D%20dynamicsymbols('alpha1%20alpha2')%0A%20%20%20%20(a1d%2C%20a2d)%20%3D%20(a1.diff(t_1)%2C%20a2.diff(t_1))%0A%20%20%20%20(a1dd%2C%20a2dd)%20%3D%20(a1.diff(t_1%2C%202)%2C%20a2.diff(t_1%2C%202))%0A%20%20%20%20return%20L1%2C%20L2%2C%20a1%2C%20a1d%2C%20a1dd%2C%20a2%2C%20a2d%2C%20a2dd%2C%20d1%2C%20d2%2C%20t_1%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20%23%23%23%20Kinematics%0A%0A%20%20%20%20Once%20again%2C%20if%20the%20angular%20positions%20%24%5Calpha_1(t)%24%20and%20%24%5Calpha_2(t)%24%20are%20known%2C%20the%20coordinates%20%24(x_1(t)%2C%20y_1(t))%24%20and%20%24(x_2(t)%2C%20y_2(t))%24%20and%20their%20derivatives%20can%20be%20readily%20determined%20(by%20forward%20kinematics)%3A%0A%0A%20%20%20%20%23%23%23%23%20Link%201%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(a1%2C%20cos%2C%20d1%2C%20eq%2C%20sin%2C%20t_1)%3A%0A%20%20%20%20(x1%2C%20y1)%20%3D%20(d1%20*%20cos(a1)%2C%20d1%20*%20sin(a1))%0A%20%20%20%20(x1d%2C%20y1d)%20%3D%20(x1.diff(t_1)%2C%20y1.diff(t_1))%0A%20%20%20%20(x1dd%2C%20y1dd)%20%3D%20(x1d.diff(t_1)%2C%20y1d.diff(t_1))%0A%20%20%20%20eq('x_1'%2C%20x1)%0A%20%20%20%20eq('%5C%5Cdot%7Bx_1%7D'%2C%20x1d)%0A%20%20%20%20eq('%5C%5Cddot%7Bx_1%7D'%2C%20x1dd)%0A%20%20%20%20eq('y_1'%2C%20y1)%0A%20%20%20%20eq('%5C%5Cdot%7By_1%7D'%2C%20y1d)%0A%20%20%20%20eq('%5C%5Cddot%7By_1%7D'%2C%20y1dd)%0A%20%20%20%20return%20x1dd%2C%20y1dd%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20%23%23%23%23%20Link%202%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(L1%2C%20a1%2C%20a2%2C%20cos%2C%20d2%2C%20eq%2C%20sin%2C%20t_1)%3A%0A%20%20%20%20(x2%2C%20y2)%20%3D%20(L1%20*%20cos(a1)%20%2B%20d2%20*%20cos(a1%20%2B%20a2)%2C%20L1%20*%20sin(a1)%20%2B%20d2%20*%20sin(a1%20%2B%20a2))%0A%20%20%20%20(x2d%2C%20y2d)%20%3D%20(x2.diff(t_1)%2C%20y2.diff(t_1))%0A%20%20%20%20(x2dd%2C%20y2dd)%20%3D%20(x2d.diff(t_1)%2C%20y2d.diff(t_1))%0A%20%20%20%20eq('x_2'%2C%20x2)%0A%20%20%20%20eq('%5C%5Cdot%7Bx_2%7D'%2C%20x2d)%0A%20%20%20%20eq('%5C%5Cddot%7Bx_2%7D'%2C%20x2dd)%0A%20%20%20%20eq('y_2'%2C%20y2)%0A%20%20%20%20eq('%5C%5Cdot%7By_2%7D'%2C%20y2d)%0A%20%20%20%20eq('%5C%5Cddot%7By_2%7D'%2C%20y2dd)%0A%20%20%20%20return%20x2%2C%20x2dd%2C%20y2%2C%20y2dd%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20Inspecting%20the%20equations%20above%2C%20we%20see%20a%20new%20kind%20of%20acceleration%2C%20proportional%20to%20%24%5Cdot%7B%5Calpha_1%7D%5Cdot%7B%5Calpha_2%7D%24.%20This%20acceleration%20is%20due%20to%20the%20%5BCoriolis%20effect%5D(https%3A%2F%2Fen.wikipedia.org%2Fwiki%2FCoriolis_effect)%20and%20is%20%20present%20only%20when%20there%20are%20movement%20in%20the%20two%20joints.%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20%23%23%23%20Jacobian%20matrix%20for%20the%20two-link%20system%0A%0A%20%20%20%20The%20Jacobian%20matrix%20for%20the%20two-link%20system%20w.r.t.%20the%20center%20of%20mass%20of%20the%20second%20link%20is%3A%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(Matrix%2C%20a1%2C%20a2%2C%20eq%2C%20x2%2C%20y2)%3A%0A%20%20%20%20r2%20%3D%20Matrix(%5B%5Bx2%2C%20y2%5D%5D)%0A%20%20%20%20J2%20%3D%20r2.jacobian(%5Ba1%2C%20a2%5D)%0A%20%20%20%20eq(r'%5Cmathbf%7BJ%7D'%2C%20J2)%0A%20%20%20%20return%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20%23%23%23%20Kinetics%0A%0A%20%20%20%20From%20the%20free%20body%20diagrams%2C%20the%20Newton-Euler%20equations%20for%20links%201%20and%202%20of%20the%20planar%20double%20inverted%20pendulum%20are%3A%0A%0A%20%20%20%20%24%24%0A%20%20%20%20%5Cbegin%7Barray%7D%7Bl%20l%7D%0A%20%20%20%20F_%7Br2x%7D%20%2B%20F_%7Be%2Cx%7D%20%26%3D%26%20m_2%5Cddot%7Bx%7D_%7B2%7D%20%5C%5C%0A%20%20%20%20%5C%5C%0A%20%20%20%20F_%7Br2y%7D%20-%20m_2g%20%2B%20F_%7Be%2Cy%7D%20%26%3D%26%20m_2%5Cddot%7By%7D_%7B2%7D%20%5C%5C%0A%20%20%20%20%5C%5C%0A%20%20%20%20T_2%20%2B%20d_2F_%7Br2x%7D%5Csin(%5Calpha_1%2B%5Calpha_2)%20-%20d_2F_%7Br2y%7D%5Ccos(%5Calpha_1%2B%5Calpha_2)%20-%20(L_2-d_2)F_%7Be%2Cx%7D%5Csin(%5Calpha_1%2B%5Calpha_2)%20-%20(L_2-d_2)F_%7Be%2Cy%7D%5Ccos(%5Calpha_1%2B%5Calpha_2)%20%26%3D%26%20I_%7B2%7D(%5Cddot%7B%5Calpha%7D_1%2B%5Cddot%7B%5Calpha%7D_2)%20%5C%5C%0A%20%20%20%20%5C%5C%0A%20%20%20%20F_%7Br1x%7D%20-%20F_%7Br2x%7D%20%26%3D%26%20m_1%5Cddot%7Bx%7D_%7B1%7D%20%5C%5C%0A%20%20%20%20%5C%5C%0A%20%20%20%20F_%7Br1y%7D%20-%20F_%7Br2y%7D%20-%20m_1g%20%26%3D%26%20m_1%5Cddot%7By%7D_%7B1%7D%20%5C%5C%0A%20%20%20%20%5C%5C%0A%20%20%20%20T_1%20-%20T_2%20%2B%20d_1F_%7Br1x%7D%5Csin%5Calpha_1%20-%20d_1F_%7Br1y%7D%5Ccos%5Calpha_1%20%2B%20(L_1-d_1)F_%7Br2x%7D%5Csin%5Calpha_1%20-%20(L_1-d_1)F_%7Br2y%7D%5Ccos%5Calpha_1%20%26%3D%26%20I_%7B1%7D%5Cddot%7B%5Calpha%7D_1%0A%20%20%20%20%5Cend%7Barray%7D%0A%20%20%20%20%24%24%0A%0A%20%20%20%20If%20we%20want%20to%20determine%20the%20joint%20torques%20and%20we%20know%20the%20kinematics%20of%20the%20links%2C%20the%20inverse%20dynamics%20approach%2C%20we%20isolate%20the%20joint%20torques%20in%20the%20equations%20above%2C%20start%20solving%20for%20link%202%20and%20then%20link%201.%20To%20determine%20the%20kinematics%20knowing%20the%20joint%20torques%2C%20the%20direct%20dynamics%20approach%2C%20we%20isolate%20the%20joint%20angular%20accelerations%20in%20the%20equations%20above%20and%20solve%20the%20ordinary%20differential%20equations.%0A%0A%20%20%20%20Let's%20express%20the%20equations%20for%20the%20torques%20substituting%20the%20terms%20we%20know%3A%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(symbols)%3A%0A%20%20%20%20(m1%2C%20m2%2C%20I1%2C%20I2%2C%20g_1)%20%3D%20symbols('m_1%2C%20m_2%2C%20I_1%20I_2%20g'%2C%20positive%3DTrue)%0A%20%20%20%20(Fex_1%2C%20Fey_1)%20%3D%20symbols('F_ex%20F_ey')%0A%20%20%20%20return%20Fex_1%2C%20Fey_1%2C%20I1%2C%20I2%2C%20g_1%2C%20m1%2C%20m2%0A%0A%0A%40app.cell%0Adef%20_(%0A%20%20%20%20Fex_1%2C%0A%20%20%20%20Fey_1%2C%0A%20%20%20%20I1%2C%0A%20%20%20%20I2%2C%0A%20%20%20%20L1%2C%0A%20%20%20%20L2%2C%0A%20%20%20%20a1%2C%0A%20%20%20%20a1dd%2C%0A%20%20%20%20a2%2C%0A%20%20%20%20a2dd%2C%0A%20%20%20%20cos%2C%0A%20%20%20%20d1%2C%0A%20%20%20%20d2%2C%0A%20%20%20%20g_1%2C%0A%20%20%20%20m1%2C%0A%20%20%20%20m2%2C%0A%20%20%20%20simplify%2C%0A%20%20%20%20sin%2C%0A%20%20%20%20x1dd%2C%0A%20%20%20%20x2dd%2C%0A%20%20%20%20y1dd%2C%0A%20%20%20%20y2dd%2C%0A)%3A%0A%20%20%20%20Fr2x%20%3D%20m2%20*%20x2dd%20-%20Fex_1%0A%20%20%20%20Fr2y%20%3D%20m2%20*%20y2dd%20%2B%20m2%20*%20g_1%20-%20Fey_1%0A%20%20%20%20T2%20%3D%20I2%20*%20(a1dd%20%2B%20a2dd)%20-%20d2%20*%20Fr2x%20*%20sin(a1%20%2B%20a2)%20%2B%20d2%20*%20Fr2y%20*%20cos(a1%20%2B%20a2)%20%2B%20(L2%20-%20d2)%20*%20Fex_1%20*%20sin(a1%20%2B%20a2)%20-%20(L2%20-%20d2)%20*%20Fey_1%20*%20cos(a1%20%2B%20a2)%0A%20%20%20%20T2%20%3D%20simplify(T2)%0A%20%20%20%20Fr1x%20%3D%20m1%20*%20x1dd%20%2B%20Fr2x%0A%20%20%20%20Fr1y%20%3D%20m1%20*%20y1dd%20%2B%20Fr2y%20%2B%20m1%20*%20g_1%0A%20%20%20%20T1%20%3D%20I1%20*%20a1dd%20%2B%20T2%20-%20d1%20*%20Fr1x%20*%20sin(a1)%20%2B%20d1%20*%20Fr1y%20*%20cos(a1)%20-%20(L1%20-%20d1)%20*%20Fr2x%20*%20sin(a1)%20%2B%20(L1%20-%20d1)%20*%20Fr2y%20*%20cos(a1)%0A%20%20%20%20T1%20%3D%20simplify(T1)%0A%20%20%20%20return%20T1%2C%20T2%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20The%20expressions%20for%20the%20joint%20moments%20of%20force%20(torques)%20are%3A%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(T1%2C%20T2%2C%20eq)%3A%0A%20%20%20%20eq(r'T_1'%2C%20T1)%0A%20%20%20%20eq(r'T_2'%2C%20T2)%0A%20%20%20%20return%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20There%20is%20an%20elegant%20form%20to%20display%20the%20equations%20for%20the%20torques%20using%20generalized%20coordinates%2C%20%24q%3D%5B%5Calpha_1%2C%20%5Calpha_2%5D%5ET%24%20and%20grouping%20the%20terms%20proportional%20to%20common%20quantities%20in%20matrices%2C%20see%20for%20example%2C%20Craig%20(2005%2C%20page%20180)%2C%20Pandy%20(2001)%2C%20and%20Zatsiorsky%20(2002%2C%20page%20383)%3A%0A%0A%20%20%20%20%24%24%0A%20%20%20%20%5Ctau%20%5Cquad%3D%5Cquad%20M(q)%5Cddot%7Bq%7D%20%2B%20C(q%2C%5Cdot%7Bq%7D)%20%2B%20G(q)%20%2B%20E(q%2C%5Cdot%7Bq%7D)%0A%20%20%20%20%24%24%0A%0A%20%20%20%20Where%2C%20for%20this%20two-link%20system%3A%3Cbr%3E%0A%20%20%20%20-%20%24%5Ctau%24%20is%20a%20matrix%20(2x1)%20of%20joint%20torques%3B%3Cbr%3E%0A%20%20%20%20-%20%24M%24%20is%20the%20mass%20or%20inertia%20matrix%20(2x2)%3B%3Cbr%3E%0A%20%20%20%20-%20%24%5Cddot%7Bq%7D%24%20is%20a%20matrix%20(2x1)%20of%20angular%20accelerations%3B%3Cbr%3E%0A%20%20%20%20-%20%24C%24%20is%20a%20matrix%20(2x1)%20of%20%5Bcentipetal%5D(https%3A%2F%2Fen.wikipedia.org%2Fwiki%2FCentripetal_force)%20and%20%5BCoriolis%5D(https%3A%2F%2Fen.wikipedia.org%2Fwiki%2FCoriolis_effect)%20torques%3B%3Cbr%3E%0A%20%20%20%20-%20%24G%24%20is%20a%20matrix%20(2x1)%20of%20%20gravitational%20torques%3B%3Cbr%3E%0A%20%20%20%20-%20%24E%24%20is%20a%20matrix%20(2x1)%20of%20external%20torques.%0A%0A%20%20%20%20Let's%20use%20Sympy%20to%20display%20the%20equations%20for%20the%20torques%20in%20this%20new%20form%3A%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(T1%2C%20T2%2C%20a1%2C%20a1d%2C%20a1dd%2C%20a2%2C%20a2d%2C%20a2dd%2C%20dynamicsymbols%2C%20t_1)%3A%0A%20%20%20%20(T1_1%2C%20T2_1)%20%3D%20(T1.expand()%2C%20T2.expand())%0A%20%20%20%20(q1%2C%20q2)%20%3D%20dynamicsymbols('q_1%20q_2')%0A%20%20%20%20(q1d%2C%20q2d)%20%3D%20(q1.diff(t_1)%2C%20q2.diff(t_1))%0A%20%20%20%20(q1dd%2C%20q2dd)%20%3D%20(q1.diff(t_1%2C%202)%2C%20q2.diff(t_1%2C%202))%0A%20%20%20%20T1_1%20%3D%20T1_1.subs(%7Ba1%3A%20q1%2C%20a2%3A%20q2%2C%20a1d%3A%20q1d%2C%20a2d%3A%20q2d%2C%20a1dd%3A%20q1dd%2C%20a2dd%3A%20q2dd%7D)%0A%20%20%20%20T2_1%20%3D%20T2_1.subs(%7Ba1%3A%20q1%2C%20a2%3A%20q2%2C%20a1d%3A%20q1d%2C%20a2d%3A%20q2d%2C%20a1dd%3A%20q1dd%2C%20a2dd%3A%20q2dd%7D)%0A%20%20%20%20return%20T1_1%2C%20T2_1%2C%20q1%2C%20q1d%2C%20q1dd%2C%20q2%2C%20q2d%2C%20q2dd%0A%0A%0A%40app.cell%0Adef%20_(%0A%20%20%20%20Fex_1%2C%0A%20%20%20%20Fey_1%2C%0A%20%20%20%20Math%2C%0A%20%20%20%20Matrix%2C%0A%20%20%20%20T1_1%2C%0A%20%20%20%20T2_1%2C%0A%20%20%20%20display%2C%0A%20%20%20%20g_1%2C%0A%20%20%20%20mlatex%2C%0A%20%20%20%20q1d%2C%0A%20%20%20%20q1dd%2C%0A%20%20%20%20q2d%2C%0A%20%20%20%20q2dd%2C%0A%20%20%20%20simplify%2C%0A)%3A%0A%20%20%20%20M%20%3D%20Matrix(((simplify(T1_1.coeff(q1dd))%2C%20simplify(T1_1.coeff(q2dd)))%2C%20(simplify(T2_1.coeff(q1dd))%2C%20simplify(T2_1.coeff(q2dd)))))%0A%20%20%20%20C%20%3D%20Matrix((simplify(T1_1.coeff(q1d%20**%202)%20*%20q1d%20**%202%20%2B%20T1_1.coeff(q2d%20**%202)%20*%20q2d%20**%202%20%2B%20T1_1.coeff(q1d%20*%20q2d)%20*%20q1d%20*%20q2d)%2C%20simplify(T2_1.coeff(q1d%20**%202)%20*%20q1d%20**%202%20%2B%20T2_1.coeff(q2d%20**%202)%20*%20q2d%20**%202%20%2B%20T2_1.coeff(q1d%20*%20q2d)%20*%20q1d%20*%20q2d)))%0A%20%20%20%20G_1%20%3D%20Matrix((simplify(T1_1.coeff(g_1)%20*%20g_1)%2C%20simplify(T2_1.coeff(g_1)%20*%20g_1)))%0A%20%20%20%20E%20%3D%20Matrix((simplify(T1_1.coeff(Fex_1)%20*%20Fex_1%20%2B%20T1_1.coeff(Fey_1)%20*%20Fey_1)%2C%20simplify(T2_1.coeff(Fex_1)%20*%20Fex_1%20%2B%20T2_1.coeff(Fey_1)%20*%20Fey_1)))%0A%20%20%20%20display(Math('%5C%5Cbegin%7Beqnarray%7D%5C%5Ctau%26%5C%5Cquad%3D%5C%5Cquad%26%5C%5Cbegin%7Bbmatrix%7D%5C%5Ctau_1%5C%5C%5C%5C%20%5C%5Ctau_2%5C%5C%5C%5C%20%5C%5Cend%7Bbmatrix%7D%20%5C%5C%5C%5C'%20%2B%20'M(q)%26%5C%5Cquad%3D%5C%5Cquad%26'%20%2B%20mlatex(M)%20%2B%20'%5C%5C%5C%5C'%20%2B%20'%5C%5Cddot%7Bq%7D%26%5C%5Cquad%3D%5C%5Cquad%26'%20%2B%20mlatex(Matrix((q1dd%2C%20q2dd)))%20%2B%20'%5C%5C%5C%5C'%20%2B%20'C(q%2C%5C%5Cdot%7Bq%7D)%26%5C%5Cquad%3D%5C%5Cquad%26'%20%2B%20mlatex(C)%20%2B%20'%5C%5C%5C%5C'%20%2B%20'G(q)%26%5C%5Cquad%3D%5C%5Cquad%26'%20%2B%20mlatex(G_1)%20%2B%20'%5C%5C%5C%5C'%20%2B%20'E(q%2C%5C%5Cdot%7Bq%7D)%26%5C%5Cquad%3D%5C%5Cquad%26'%20%2B%20mlatex(E)%20%2B%20'%5C%5Cend%7Beqnarray%7D'))%0A%20%20%20%20return%20C%2C%20E%2C%20G_1%2C%20M%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20With%20this%20convention%2C%20to%20perform%20inverse%20dynamics%20we%20would%20calculate%3A%0A%0A%20%20%20%20%24%24%0A%20%20%20%20%5Ctau%20%5Cquad%3D%5Cquad%20M(q)%5Cddot%7Bq%7D%20%2B%20C(q%2C%5Cdot%7Bq%7D)%20%2B%20G(q)%20%2B%20E(q%2C%5Cdot%7Bq%7D)%0A%20%20%20%20%24%24%0A%0A%20%20%20%20And%20for%20direct%20dynamics%20we%20would%20solve%20the%20differential%20equation%3A%0A%0A%20%20%20%20%24%24%0A%20%20%20%20%5Cddot%7Bq%7D%20%5Cquad%3D%5Cquad%20M(q)%5E%7B-1%7D%20%5Cleft%5B%5Ctau%20-%20C(q%2C%5Cdot%7Bq%7D)%20-%20G(q)%20-%20E(q%2C%5Cdot%7Bq%7D)%20%5Cright%5D%0A%20%20%20%20%24%24%0A%0A%20%20%20%20The%20advantage%20of%20calculating%20analytically%20the%20derivatives%20of%20the%20position%20vector%20as%20function%20of%20the%20joint%20angles%20and%20using%20the%20notation%20above%20is%20that%20each%20term%20that%20contributes%20to%20each%20joint%20torque%20or%20acceleration%20can%20be%20easily%20identified.%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20%23%23%23%23%20Coupling%20(or%20interaction)%20effects%0A%0A%20%20%20%20The%20two%20terms%20off%20the%20main%20diagonal%20in%20the%20inertia%20matrix%20(which%20are%20the%20same)%20and%20the%20centripetal%20and%20Coriolis%20terms%20represent%20the%20effects%20of%20the%20movement%20(nonzero%20velocity)%20of%20one%20joint%20over%20the%20other.%20These%20torques%20are%20referred%20as%20coupling%20or%20interaction%20effects%20(see%20for%20example%20Hollerbach%20and%20Flash%20(1982)%20for%20an%20application%20of%20this%20concept%20in%20the%20study%20of%20the%20motor%20control%20of%20the%20upper%20limb%20movement).%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20%23%23%23%23%20Planar%20double%20pendulum%0A%0A%20%20%20%20Using%20the%20same%20equations%20above%2C%20one%20can%20represent%20a%20planar%20double%20pendulum%20(hanging%20from%20the%20top%2C%20not%20inverted)%20considering%20the%20angles%20%24%5Calpha_1%24%20and%20%24%5Calpha_2%24%20negative%2C%20e.g.%2C%20at%20%24%5Calpha_1%3D-90%5Eo%24%20and%20%24%5Calpha_2%3D0%24%20the%20pendulum%20is%20hanging%20vertical.%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20%23%23%23%23%20WARNING%3A%20%24F_r%24%20is%20not%20the%20actual%20joint%20reaction%20force!%0A%0A%20%20%20%20For%20these%20two%20examples%2C%20in%20the%20Newton-Euler%20equations%20based%20on%20the%20free%20body%20diagrams%20we%20represented%20the%20consequences%20of%20all%20possible%20muscle%20forces%20on%20a%20joint%20as%20a%20net%20muscle%20torque%20and%20all%20forces%20acting%20on%20a%20joint%20as%20a%20resultant%20joint%20reaction%20force.%20That%20is%2C%20all%20forces%20between%20segments%20were%20represented%20as%20a%20resultant%20force%20that%20doesn't%20generate%20torque%20and%20a%20force%20couple%20(or%20free%20moment)%20that%20only%20generates%20torque.%20This%20is%20an%20important%20principle%20in%20mechanics%20of%20rigid%20bodies%2C%20see%20for%20example%20%5Bthis%20text%5D(https%3A%2F%2Fmolab.marimo.io%2Fgithub%2FBMClab%2FBMC%2Fblob%2Fmaster%2Fnotebooks%2Ffree_body_diagram.py).%20However%2C%20this%20principle%20creates%20the%20unrealistic%20notion%20that%20the%20sum%20of%20forces%20is%20applied%20directly%20on%20the%20joint%20(which%20has%20no%20further%20implication%20for%20a%20rigid%20body)%2C%20but%20it%20is%20inaccurate%20for%20the%20understanding%20of%20the%20local%20effects%20on%20the%20joint.%20So%2C%20if%20we%20are%20trying%20to%20understand%20the%20stress%20on%20the%20joint%20or%20mechanisms%20of%20joint%20injury%2C%20the%20forces%20acting%20on%20the%20joint%20and%20on%20the%20rest%20of%20the%20segment%20must%20be%20considered%20individually.%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20%23%23%23%23%20Determination%20of%20muscle%20force%0A%0A%20%20%20%20The%20torque%20%24T%24%20exerted%20by%20a%20muscle%20is%20given%20by%20the%20product%20between%20the%20muscle-tendon%20moment%20arm%20%24r%24%20and%20its%20force%20%24F%24.%20For%20the%20human%20body%2C%20there%20is%20more%20than%20one%20muscle%20crossing%20a%20joint%20and%20several%20joints.%20In%20such%20case%2C%20the%20torques%20due%20to%20the%20muscles%20are%20expressed%20in%20the%20following%20matrix%20form%20considering%20%24n%24%20joints%20and%20%24m%24%20muscles%3A%0A%0A%20%20%20%20%24%24%0A%20%20%20%20%5Cbegin%7Baligned%7D%0A%20%20%20%20%5Cbegin%7Bbmatrix%7D%20T_1%20%5C%5C%20%5Cvdots%20%5C%5C%20T_n%20%5Cend%7Bbmatrix%7D%20%26%5Cquad%3D%5Cquad%26%20%5Cbegin%7Bbmatrix%7D%20r_%7B11%7D%20%26%20%5Ccdots%20%26%20r_%7B1m%7D%20%5C%5C%20%5Cvdots%20%26%20%5Cddots%20%26%20%5Cvdots%20%5C%5C%20r_%7Bn1%7D%20%26%20%5Ccdots%20%26%20r_%7Bnm%7D%20%5Cend%7Bbmatrix%7D%20%5Cbegin%7Bbmatrix%7D%20F_1%20%5C%5C%20%5Cvdots%20%5C%5C%20F_m%20%5Cend%7Bbmatrix%7D%0A%20%20%20%20%5Cend%7Baligned%7D%0A%20%20%20%20%24%24%0A%0A%20%20%20%20Where%20%24r_%7Bnm%7D%24%20is%20the%20moment%20arm%20about%20joint%20%24n%24%20of%20the%20muscle%20%24m%24.%3Cbr%3E%0A%20%20%20%20In%20the%20example%20of%20the%20two-link%20system%2C%20we%20sketched%20two%20uniarticular%20muscles%20for%20each%20of%20the%20two%20joints%2C%20consequently%3A%0A%0A%20%20%20%20%24%24%0A%20%20%20%20%5Cbegin%7Baligned%7D%0A%20%20%20%20%5Cbegin%7Bbmatrix%7D%20T_1%20%5C%5C%20T_2%20%5Cend%7Bbmatrix%7D%20%26%5Cquad%3D%5Cquad%26%20%5Cbegin%7Bbmatrix%7D%20r_%7B1%2Cext%7D%20%26%20-r_%7B1%2Cflex%7D%20%26%200%20%26%200%20%5C%5C%200%20%26%200%20%26%20r_%7B1%2Cext%7D%20%26%20-r_%7B1%2Cflex%7D%20%5Cend%7Bbmatrix%7D%20%5Cbegin%7Bbmatrix%7D%20F_%7B1%2Cext%7D%20%5C%5C%20F_%7B1%2Cflex%7D%20%5C%5C%20F_%7B2%2Cext%7D%20%5C%5C%20F_%7B2%2Cflex%7D%20%5Cend%7Bbmatrix%7D%0A%20%20%20%20%5Cend%7Baligned%7D%0A%20%20%20%20%24%24%0A%0A%20%20%20%20Note%20the%20opposite%20signs%20for%20the%20moment%20arms%20of%20the%20extension%20and%20flexion%20muscles%20hence%20they%20generate%20opposite%20torques.%20We%20could%20have%20represented%20the%20opposite%20signs%20in%20the%20muscle%20forces%20instead%20of%20in%20the%20moment%20arms.%0A%0A%20%20%20%20The%20moment%20arm%20of%20a%20muscle%20varies%20with%20the%20motion%20of%20the%20joints%20it%20crosses.%20In%20this%20case%2C%20using%20the%20%5Bvirtual%20work%20principle%5D(https%3A%2F%2Fen.wikipedia.org%2Fwiki%2FVirtual_work)%20the%20moment%20arm%20can%20be%20given%20by%20(Sherman%20et%20al.%2C%202013%3B%20Nigg%20and%20Herzog%2C%202006%2C%20page%20634)%3A%0A%0A%20%20%20%20%24%24%0A%20%20%20%20r(q)%20%5Cquad%3D%5Cquad%20%5Cdfrac%7B%5Cpartial%20L_%7BMT%7D(q)%7D%7B%5Cpartial%20q%7D%0A%20%20%20%20%24%24%0A%0A%20%20%20%20Where%20%24L_%7BMT%7D(q)%24%20is%20the%20length%20of%20the%20muscle-tendon%20unit%20expressed%20as%20a%20function%20of%20angle%20%24q%24.%0A%0A%20%20%20%20For%20the%20simulation%20of%20human%20movement%2C%20muscles%20can%20be%20modeled%20as%20%5BHill-type%20muscles%5D(https%3A%2F%2Fmolab.marimo.io%2Fgithub%2FBMClab%2FBMC%2Fblob%2Fmaster%2Fnotebooks%2Fmuscle_simulation.py)%2C%20the%20torques%20they%20generate%20are%20given%20by%20the%20matrix%20above%2C%20and%20this%20matrix%20is%20entered%20in%20the%20ODE%20for%20a%20multibody%20system%20dynamics%20we%20deduced%20before%3A%0A%0A%20%20%20%20%24%24%0A%20%20%20%20%5Cddot%7Bq%7D%20%5Cquad%3D%5Cquad%20M(q)%5E%7B-1%7D%20%5Cleft%5BR_%7BMT%7D(q)F_%7BMT%7D(a%2CL_%7BMT%7D%2C%5Cdot%7BL%7D_%7BMT%7D)%20-%20C(q%2C%5Cdot%7Bq%7D)%20-%20G(q)%20-%20E(q%2C%5Cdot%7Bq%7D)%20%5Cright%5D%0A%20%20%20%20%24%24%0A%0A%20%20%20%20Where%20%24R_%7BMT%7D%24%20and%20%24F_%7BMT%7D%24%20are%20matrices%20for%20the%20moment%20arms%20and%20muscle-tendon%20forces%2C%20respectively.%0A%20%20%20%20This%20ODE%20is%20then%20solved%20numerically%20given%20initial%20values%3B%20but%20this%20problem%20is%20far%20from%20trivial%20for%20a%20simulation%20with%20several%20segments%20and%20muscles.%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20**Exercise%203%20%E2%80%94%20Couple%20the%20joints.**%20An%20inertia%20matrix%20is%20%24%5Cbegin%7Bbmatrix%7D3%261%5C%5C1%261%5Cend%7Bbmatrix%7D%24%20and%20angular%20acceleration%20is%20%24(0%2C2)%24%20rad%2Fs%C2%B2.%20Ignore%20velocity%2C%20gravity%20and%20external-load%20terms.%20What%20joint%20torques%20are%20required%3F%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20%2F%2F%2F%20details%20%7C%20Solution%20to%20Exercise%203%0A%0A%20%20%20%20%24%5Ctau%3DM%5Cddot%20q%3D(2%2C2)%24%20N%20m.%20The%20first%20joint%20needs%20torque%20even%20though%20its%20acceleration%20is%20zero%2C%20because%20the%20distal%20segment%20accelerates%20and%20interacts%20with%20the%20proximal%20one.%0A%20%20%20%20%2F%2F%2F%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20%23%23%20Numerical%20simulation%20of%20inverse%20dynamics%0A%0A%20%20%20%20Let's%20simulate%20a%20voluntary%20movement%20of%20the%20upper%20limb%20using%20the%20planar%20two-link%20system%20as%20a%20model%20in%20order%20to%20visualize%20the%20contribution%20of%20each%20torque%20term.%0A%0A%20%20%20%20We%20will%20ignore%20the%20muscle%20dynamics%20and%20we%20will%20calculate%20the%20joint%20torques%20necessary%20to%20move%20the%20upper%20limb%20from%20one%20point%20to%20another%20under%20the%20assumption%20that%20the%20movement%20is%20performed%20with%20the%20smoothest%20trajectory%20possible.%20I.e.%2C%20the%20movement%20is%20performed%20with%20a%20%5Bminimum-jerk%20trajectory%5D(https%3A%2F%2Fmolab.marimo.io%2Fgithub%2FBMClab%2FBMC%2Fblob%2Fmaster%2Fnotebooks%2Fminimum_jerk_hypothesis.py)%2C%20a%20hypothesis%20about%20control%20of%20voluntary%20movements%20proposed%20by%20Flash%20and%20Hogan%20(1985).%0A%0A%20%20%20%20Once%20we%20determine%20the%20desired%20trajectory%2C%20we%20can%20calculate%20the%20velocity%20and%20acceleration%20of%20the%20segments%20and%20combine%20with%20anthropometric%20measures%20to%20calculate%20the%20joint%20torques%20necessary%20to%20move%20the%20segments.%20This%20means%20we%20will%20perform%20inverse%20dynamics.%0A%0A%20%20%20%20Let's%20simulate%20a%20slow%20(4%20s)%20and%20a%20fast%20(0.5%20s)%20movement%20of%20the%20upper%20limb%20starting%20at%20the%20anatomical%20neutral%20position%20(upper%20limb%20at%20the%20side%20of%20the%20trunk)%20and%20ending%20with%20the%20upper%20arm%20forward%20at%20horizontal%20and%20elbow%20flexed%20at%2090%20degrees.%0A%0A%20%20%20%20First%2C%20let's%20import%20the%20necessary%20Python%20libraries%20and%20customize%20the%20environment%3A%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_()%3A%0A%20%20%20%20try%3A%0A%20%20%20%20%20%20%20%20from%20notebook_resources%20import%20repo_file%0A%20%20%20%20except%20ModuleNotFoundError%20as%20_error%3A%0A%20%20%20%20%20%20%20%20if%20_error.name%20!%3D%20%22notebook_resources%22%3A%0A%20%20%20%20%20%20%20%20%20%20%20%20raise%0A%20%20%20%20%20%20%20%20raise%20ModuleNotFoundError(%0A%20%20%20%20%20%20%20%20%20%20%20%20%22This%20notebook%20needs%20notebook_resources.py.%20Download%20it%20from%3A%5Cn%22%0A%20%20%20%20%20%20%20%20%20%20%20%20%22https%3A%2F%2Fraw.githubusercontent.com%2FBMClab%2FBMC%2Fmaster%2Ffunctions%2Fnotebook_resources.py%5Cn%22%0A%20%20%20%20%20%20%20%20%20%20%20%20%22Save%20it%20in%20the%20same%20folder%20as%20this%20notebook%2C%20then%20rerun%20this%20cell.%22%2C%0A%20%20%20%20%20%20%20%20%20%20%20%20name%3D%22notebook_resources%22%2C%0A%20%20%20%20%20%20%20%20)%20from%20None%0A%20%20%20%20return%20(repo_file%2C)%0A%0A%0A%40app.cell%0Adef%20_(repo_file)%3A%0A%20%20%20%20import%20numpy%20as%20np%0A%20%20%20%20%23%20'%25matplotlib%20inline'%20command%20supported%20automatically%20in%20marimo%0A%20%20%20%20import%20matplotlib%0A%20%20%20%20import%20matplotlib.pyplot%20as%20plt%0A%20%20%20%20matplotlib.rcParams%5B'lines.linewidth'%5D%20%3D%203%0A%20%20%20%20matplotlib.rcParams%5B'font.size'%5D%20%3D%2013%0A%20%20%20%20matplotlib.rcParams%5B'lines.markersize'%5D%20%3D%205%0A%20%20%20%20matplotlib.rc('axes'%2C%20grid%3DTrue%2C%20labelsize%3D14%2C%20titlesize%3D16%2C%20ymargin%3D0.05)%0A%20%20%20%20matplotlib.rc('legend'%2C%20numpoints%3D1%2C%20fontsize%3D11)%0A%20%20%20%20import%20sys%0A%20%20%20%20_functions%20%3D%20%5Brepo_file(f'functions%2F%7B_m%7D.py')%20for%20_m%20in%20('minjerk'%2C%20'invkin2_2d')%5D%0A%20%20%20%20sys.path.insert(0%2C%20str(_functions%5B0%5D.parent))%20%20%23%20add%20to%20pythonpath%0A%20%20%20%20return%20np%2C%20plt%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20Let's%20take%20the%20anthropometric%20data%20from%20Dempster's%20model%20(see%20%5BBody%20segment%20parameters%5D(https%3A%2F%2Fmolab.marimo.io%2Fgithub%2FBMClab%2FBMC%2Fblob%2Fmaster%2Fnotebooks%2Fbody_segment_parameters.py))%3A%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_()%3A%0A%20%20%20%20height%2C%20mass%20%3D%201.70%2C%20%20%20%20%20%20%20%20%20%20%20%20%20%20%2070%20%20%23%20m%2C%20kg%0A%20%20%20%20L1n%2C%20L2n%20%20%20%20%20%3D%200.188*height%2C%20%20%20%20%20%20%200.253*height%0A%20%20%20%20d1n%2C%20d2n%20%20%20%20%20%3D%200.436*L1n%2C%20%20%20%20%20%20%20%20%20%200.682*L2n%0A%20%20%20%20m1n%2C%20m2n%20%20%20%20%20%3D%200.0280*mass%2C%20%20%20%20%20%20%20%200.0220*mass%0A%20%20%20%20rg1n%2C%20rg2n%20%20%20%3D%200.322%2C%20%20%20%20%20%20%20%20%20%20%20%20%20%200.468%0A%20%20%20%20I1n%2C%20I2n%20%20%20%20%20%3D%20m1n*(rg1n*L1n)**2%2C%20%20m2n*(rg2n*L2n)**2%0A%20%20%20%20return%20I1n%2C%20I2n%2C%20L1n%2C%20L2n%2C%20d1n%2C%20d2n%2C%20m1n%2C%20m2n%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20Considering%20these%20lengths%2C%20the%20initial%20and%20final%20positions%20of%20the%20endpoint%20(finger%20tip)%20for%20the%20simulated%20movement%20will%20be%3A%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(L1n%2C%20L2n)%3A%0A%20%20%20%20xi%2C%20yi%20%3D%200%2C%20-L1n-L2n%0A%20%20%20%20xf%2C%20yf%20%3D%20L1n%2C%20L2n%0A%20%20%20%20gn%20%3D%209.81%20%20%23%20gravity%20acceleration%20m%2Fs2%0A%20%20%20%20return%20gn%2C%20xf%2C%20xi%2C%20yf%2C%20yi%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20%23%23%23%20Slow%20movement%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_()%3A%0A%20%20%20%20duration%20%3D%204%20%20%23%20seconds%0A%20%20%20%20return%20(duration%2C)%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20The%20endpoint%20minimum%20jerk%20trajectory%20will%20be%20(see%20%5BKinematic%20chain%20in%20a%20plane%20(2D)%5D(https%3A%2F%2Fmolab.marimo.io%2Fgithub%2FBMClab%2FBMC%2Fblob%2Fmaster%2Fnotebooks%2Fkinematic_chain.py))%3A%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_()%3A%0A%20%20%20%20from%20minjerk%20import%20minjerk%0A%0A%20%20%20%20return%20(minjerk%2C)%0A%0A%0A%40app.cell%0Adef%20_(duration%2C%20minjerk%2C%20xf%2C%20xi%2C%20yf%2C%20yi)%3A%0A%20%20%20%20(time%2C%20rlin%2C%20_vlin%2C%20_alin%2C%20_jlin)%20%3D%20minjerk(%5Bxi%2C%20yi%5D%2C%20%5Bxf%2C%20yf%5D%2C%20duration%3Dduration)%0A%20%20%20%20return%20rlin%2C%20time%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20Let's%20find%20the%20joint%20angles%20to%20produce%20this%20minimum-jerk%20trajectory%20(inverse%20kinematics)%3A%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_()%3A%0A%20%20%20%20from%20invkin2_2d%20import%20invkin%0A%0A%20%20%20%20return%20(invkin%2C)%0A%0A%0A%40app.cell%0Adef%20_(L1n%2C%20L2n%2C%20invkin%2C%20rlin%2C%20time)%3A%0A%20%20%20%20rang%20%3D%20invkin(time%2C%20rlin%2C%20L1%3DL1n%2C%20L2%3DL2n)%0A%20%20%20%20return%20(rang%2C)%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20For%20the%20joint%20torques%2C%20we%20need%20to%20calculate%20the%20angular%20velocity%20and%20acceleration.%20Let's%20do%20that%20using%20numerical%20differentiation%3A%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(duration%2C%20np%2C%20plt%2C%20rang%2C%20time)%3A%0A%20%20%20%20def%20diff_c(ang%2C%20duration)%3A%0A%20%20%20%20%20%20%20%20%22%22%22Numerical%20differentiations%20using%20the%20central%20difference%20for%20the%20angular%20data.%0A%20%20%20%20%20%20%20%20%22%22%22%0A%20%20%20%20%20%20%20%20%23%20central%20difference%20(f(x%2Bh)-f(x-h))%2F(2*h)%0A%20%20%20%20%20%20%20%20dt%20%3D%20duration%2F(ang.shape%5B0%5D-1)%0A%20%20%20%20%20%20%20%20vang%20%3D%20np.empty_like(rang)%0A%20%20%20%20%20%20%20%20aang%20%3D%20np.empty_like(rang)%0A%20%20%20%20%20%20%20%20vang%5B%3A%2C%200%5D%20%3D%20np.gradient(rang%5B%3A%2C%200%5D%2C%20dt)%0A%20%20%20%20%20%20%20%20vang%5B%3A%2C%201%5D%20%3D%20np.gradient(rang%5B%3A%2C%201%5D%2C%20dt)%0A%20%20%20%20%20%20%20%20aang%5B%3A%2C%200%5D%20%3D%20np.gradient(vang%5B%3A%2C%200%5D%2C%20dt)%0A%20%20%20%20%20%20%20%20aang%5B%3A%2C%201%5D%20%3D%20np.gradient(vang%5B%3A%2C%201%5D%2C%20dt)%0A%0A%20%20%20%20%20%20%20%20_%2C%20ax%20%3D%20plt.subplots(1%2C%203%2C%20sharex%3DTrue%2C%20figsize%3D(10%2C%203))%0A%20%20%20%20%20%20%20%20ax%5B0%5D.plot(time%2C%20rang*180%2Fnp.pi)%0A%20%20%20%20%20%20%20%20ax%5B0%5D.legend(%5B'Ang%201'%2C%20'Ang%202'%5D%2C%20framealpha%3D.5%2C%20loc%3D'best')%0A%20%20%20%20%20%20%20%20ax%5B1%5D.plot(time%2C%20vang*180%2Fnp.pi)%0A%20%20%20%20%20%20%20%20ax%5B2%5D.plot(time%2C%20aang*180%2Fnp.pi)%0A%20%20%20%20%20%20%20%20ylabel%20%3D%20%5Br'Displacement%20%5B%24%5Cmathrm%7B%5Eo%7D%24%5D'%2C%20r'Velocity%20%5B%24%5Cmathrm%7B%5Eo%2Fs%7D%24%5D'%2C%0A%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20r'Acceleration%20%5B%24%5Cmathrm%7B%5Eo%2Fs%5E2%7D%24%5D'%5D%0A%20%20%20%20%20%20%20%20for%20i%2C%20axi%20in%20enumerate(ax)%3A%0A%20%20%20%20%20%20%20%20%20%20%20%20axi.set_xlabel('Time%20%5B%24s%24%5D')%0A%20%20%20%20%20%20%20%20%20%20%20%20axi.set_ylabel(ylabel%5Bi%5D)%0A%20%20%20%20%20%20%20%20%20%20%20%20axi.xaxis.set_major_locator(plt.MaxNLocator(4))%0A%20%20%20%20%20%20%20%20%20%20%20%20axi.yaxis.set_major_locator(plt.MaxNLocator(4))%0A%20%20%20%20%20%20%20%20plt.tight_layout()%0A%20%20%20%20%20%20%20%20plt.show()%0A%0A%20%20%20%20%20%20%20%20return%20vang%2C%20aang%0A%0A%20%20%20%20vang%2C%20aang%20%3D%20diff_c(rang%2C%20duration)%0A%20%20%20%20return%20aang%2C%20diff_c%2C%20vang%0A%0A%0A%40app.cell%0Adef%20_(Fex_1%2C%20Fey_1%2C%20I1%2C%20I2%2C%20L1%2C%20L2%2C%20d1%2C%20d2%2C%20g_1%2C%20m1%2C%20m2%2C%20plt%2C%20t_1)%3A%0A%20%20%20%20def%20dyna(time%2C%20L1n%2C%20L2n%2C%20d1n%2C%20d2n%2C%20m1n%2C%20m2n%2C%20gn%2C%20I1n%2C%20I2n%2C%20q1%2C%20q2%2C%20rang%2C%20vang%2C%20aang%2C%20Fexn%2C%20Feyn%2C%20M%2C%20C%2C%20G%2C%20E)%3A%0A%20%20%20%20%20%20%20%20%22%22%22Numerical%20calculation%20and%20plot%20for%20the%20torques%20of%20a%20planar%20two-link%20system.%0A%20%20%20%20%20%20%20%20%22%22%22%0A%20%20%20%20%20%20%20%20from%20sympy%20import%20lambdify%2C%20symbols%0A%20%20%20%20%20%20%20%20Mfun%20%3D%20lambdify((I1%2C%20I2%2C%20L1%2C%20L2%2C%20d1%2C%20d2%2C%20m1%2C%20m2%2C%20q1%2C%20q2)%2C%20M.tolist()%2C%20'numpy')%20%20%23%20nested%20list%3A%20elements%20are%20arrays%20and%20constants%0A%20%20%20%20%20%20%20%20Mn%20%3D%20Mfun(I1n%2C%20I2n%2C%20L1n%2C%20L2n%2C%20d1n%2C%20d2n%2C%20m1n%2C%20m2n%2C%20rang%5B%3A%2C%200%5D%2C%20rang%5B%3A%2C%201%5D)%0A%20%20%20%20%20%20%20%20_M00%20%3D%20Mn%5B0%5D%5B0%5D%20*%20aang%5B%3A%2C%200%5D%0A%20%20%20%20%20%20%20%20_M01%20%3D%20Mn%5B0%5D%5B1%5D%20*%20aang%5B%3A%2C%201%5D%0A%20%20%20%20%20%20%20%20_M10%20%3D%20Mn%5B1%5D%5B0%5D%20*%20aang%5B%3A%2C%200%5D%0A%20%20%20%20%20%20%20%20_M11%20%3D%20Mn%5B1%5D%5B1%5D%20*%20aang%5B%3A%2C%201%5D%0A%20%20%20%20%20%20%20%20(Q1d%2C%20Q2d)%20%3D%20symbols('Q1d%20Q2d')%0A%20%20%20%20%20%20%20%20dicti%20%3D%20%7Bq1.diff(t_1%2C%201)%3A%20Q1d%2C%20q2.diff(t_1%2C%201)%3A%20Q2d%7D%0A%20%20%20%20%20%20%20%20C0fun%20%3D%20lambdify((L1%2C%20d2%2C%20m2%2C%20q2%2C%20Q1d%2C%20Q2d)%2C%20C%5B0%5D.subs(dicti)%2C%20'numpy')%0A%20%20%20%20%20%20%20%20_C0%20%3D%20C0fun(L1n%2C%20d2n%2C%20m2n%2C%20rang%5B%3A%2C%201%5D%2C%20vang%5B%3A%2C%200%5D%2C%20vang%5B%3A%2C%201%5D)%0A%20%20%20%20%20%20%20%20C1fun%20%3D%20lambdify((L1%2C%20d2%2C%20m2%2C%20q2%2C%20Q1d%2C%20Q2d)%2C%20C%5B1%5D.subs(dicti)%2C%20'numpy')%0A%20%20%20%20%20%20%20%20_C1%20%3D%20C1fun(L1n%2C%20d2n%2C%20m2n%2C%20rang%5B%3A%2C%201%5D%2C%20vang%5B%3A%2C%200%5D%2C%20vang%5B%3A%2C%201%5D)%0A%20%20%20%20%20%20%20%20G0fun%20%3D%20lambdify((L1%2C%20d1%2C%20d2%2C%20m1%2C%20m2%2C%20g_1%2C%20q1%2C%20q2)%2C%20G%5B0%5D%2C%20'numpy')%0A%20%20%20%20%20%20%20%20_G0%20%3D%20G0fun(L1n%2C%20d1n%2C%20d2n%2C%20m1n%2C%20m2n%2C%20gn%2C%20rang%5B%3A%2C%200%5D%2C%20rang%5B%3A%2C%201%5D)%0A%20%20%20%20%20%20%20%20G1fun%20%3D%20lambdify((L1%2C%20d1%2C%20d2%2C%20m1%2C%20m2%2C%20g_1%2C%20q1%2C%20q2)%2C%20G%5B1%5D%2C%20'numpy')%0A%20%20%20%20%20%20%20%20_G1%20%3D%20G1fun(L1n%2C%20d1n%2C%20d2n%2C%20m1n%2C%20m2n%2C%20gn%2C%20rang%5B%3A%2C%200%5D%2C%20rang%5B%3A%2C%201%5D)%0A%20%20%20%20%20%20%20%20E0fun%20%3D%20lambdify((L1%2C%20L2%2C%20q1%2C%20q2%2C%20Fex_1%2C%20Fey_1)%2C%20E%5B0%5D%2C%20'numpy')%0A%20%20%20%20%20%20%20%20_E0%20%3D%20E0fun(L1n%2C%20L2n%2C%20rang%5B%3A%2C%200%5D%2C%20rang%5B%3A%2C%201%5D%2C%200%2C%200)%0A%20%20%20%20%20%20%20%20E1fun%20%3D%20lambdify((L1%2C%20L2%2C%20q1%2C%20q2%2C%20Fex_1%2C%20Fey_1)%2C%20E%5B1%5D%2C%20'numpy')%0A%20%20%20%20%20%20%20%20_E1%20%3D%20E1fun(L1n%2C%20L2n%2C%20rang%5B%3A%2C%200%5D%2C%20rang%5B%3A%2C%201%5D%2C%20Fexn%2C%20Feyn)%0A%20%20%20%20%20%20%20%20(fig%2C%20ax)%20%3D%20plt.subplots(1%2C%202%2C%20sharex%3DTrue%2C%20squeeze%3DTrue%2C%20figsize%3D(10%2C%204))%0A%20%20%20%20%20%20%20%20ax%5B0%5D.plot(time%2C%20_M00%20%2B%20_M01)%0A%20%20%20%20%20%20%20%20ax%5B0%5D.plot(time%2C%20_C0)%0A%20%20%20%20%20%20%20%20ax%5B0%5D.plot(time%2C%20_G0)%0A%20%20%20%20%20%20%20%20ax%5B0%5D.plot(time%2C%20_E0)%0A%20%20%20%20%20%20%20%20ax%5B0%5D.set_ylabel('Torque%20%5BNm%5D')%0A%20%20%20%20%20%20%20%20ax%5B0%5D.set_title('Joint%201')%0A%20%20%20%20%20%20%20%20ax%5B1%5D.plot(time%2C%20_M10%20%2B%20_M11%2C%20label%3D'Mass%2FInertia')%0A%20%20%20%20%20%20%20%20ax%5B1%5D.plot(time%2C%20_C1%2C%20label%3D'Centripetal%2FCoriolis%20%20%20%20%20%20%20')%0A%20%20%20%20%20%20%20%20ax%5B1%5D.plot(time%2C%20_G1%2C%20label%3D'Gravitational')%0A%20%20%20%20%20%20%20%20ax%5B1%5D.plot(time%2C%20_E1%2C%20label%3D'External')%0A%20%20%20%20%20%20%20%20ax%5B1%5D.set_title('Joint%202')%0A%20%20%20%20%20%20%20%20fig.legend(framealpha%3D0.5%2C%20bbox_to_anchor%3D(1.15%2C%200.95)%2C%20fontsize%3D12)%0A%20%20%20%20%20%20%20%20for%20(i%2C%20axi)%20in%20enumerate(ax)%3A%0A%20%20%20%20%20%20%20%20%20%20%20%20axi.set_xlabel('Time%20%5B%24s%24%5D')%0A%20%20%20%20%20%20%20%20%20%20%20%20axi.xaxis.set_major_locator(plt.MaxNLocator(4))%0A%20%20%20%20%20%20%20%20%20%20%20%20axi.yaxis.set_major_locator(plt.MaxNLocator(4))%0A%20%20%20%20%20%20%20%20plt.show()%0A%20%20%20%20%20%20%20%20return%20(_M00%2C%20_M01%2C%20_M10%2C%20_M11%2C%20_C0%2C%20_C1%2C%20_G0%2C%20_G1%2C%20_E0%2C%20_E1)%0A%0A%20%20%20%20return%20(dyna%2C)%0A%0A%0A%40app.cell%0Adef%20_(%0A%20%20%20%20C%2C%0A%20%20%20%20E%2C%0A%20%20%20%20G_1%2C%0A%20%20%20%20I1n%2C%0A%20%20%20%20I2n%2C%0A%20%20%20%20L1n%2C%0A%20%20%20%20L2n%2C%0A%20%20%20%20M%2C%0A%20%20%20%20aang%2C%0A%20%20%20%20d1n%2C%0A%20%20%20%20d2n%2C%0A%20%20%20%20dyna%2C%0A%20%20%20%20gn%2C%0A%20%20%20%20m1n%2C%0A%20%20%20%20m2n%2C%0A%20%20%20%20q1%2C%0A%20%20%20%20q2%2C%0A%20%20%20%20rang%2C%0A%20%20%20%20time%2C%0A%20%20%20%20vang%2C%0A)%3A%0A%20%20%20%20(Fexn%2C%20Feyn)%20%3D%20(0%2C%200)%0A%20%20%20%20(_M00%2C%20_M01%2C%20_M10%2C%20_M11%2C%20_C0%2C%20_C1%2C%20_G0%2C%20_G1%2C%20_E0%2C%20_E1)%20%3D%20dyna(time%2C%20L1n%2C%20L2n%2C%20d1n%2C%20d2n%2C%20m1n%2C%20m2n%2C%20gn%2C%20I1n%2C%20I2n%2C%20q1%2C%20q2%2C%20rang%2C%20vang%2C%20aang%2C%20Fexn%2C%20Feyn%2C%20M%2C%20C%2C%20G_1%2C%20E)%0A%20%20%20%20T1a%20%3D%20_M00%20%2B%20_M01%20%2B%20_C0%20%2B%20_G0%20%2B%20_E0%0A%20%20%20%20T2a%20%3D%20_M10%20%2B%20_M11%20%2B%20_C1%20%2B%20_G1%20%2B%20_E1%0A%20%20%20%20return%20Fexn%2C%20Feyn%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20The%20joint%20torques%20essentially%20compensate%20the%20gravitational%20torque.%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20%23%23%23%20Fast%20movement%0A%0A%20%20%20%20Let's%20see%20what%20is%20changed%20for%20a%20fast%20movement%3A%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(%0A%20%20%20%20C%2C%0A%20%20%20%20E%2C%0A%20%20%20%20Fexn%2C%0A%20%20%20%20Feyn%2C%0A%20%20%20%20G_1%2C%0A%20%20%20%20I1n%2C%0A%20%20%20%20I2n%2C%0A%20%20%20%20L1n%2C%0A%20%20%20%20L2n%2C%0A%20%20%20%20M%2C%0A%20%20%20%20d1n%2C%0A%20%20%20%20d2n%2C%0A%20%20%20%20diff_c%2C%0A%20%20%20%20dyna%2C%0A%20%20%20%20gn%2C%0A%20%20%20%20invkin%2C%0A%20%20%20%20m1n%2C%0A%20%20%20%20m2n%2C%0A%20%20%20%20minjerk%2C%0A%20%20%20%20q1%2C%0A%20%20%20%20q2%2C%0A%20%20%20%20xf%2C%0A%20%20%20%20xi%2C%0A%20%20%20%20yf%2C%0A%20%20%20%20yi%2C%0A)%3A%0A%20%20%20%20duration_1%20%3D%200.5%0A%20%20%20%20(time_1%2C%20rlin_1%2C%20_vlin%2C%20_alin%2C%20_jlin)%20%3D%20minjerk(%5Bxi%2C%20yi%5D%2C%20%5Bxf%2C%20yf%5D%2C%20duration%3Dduration_1)%0A%20%20%20%20rang_1%20%3D%20invkin(time_1%2C%20rlin_1%2C%20L1%3DL1n%2C%20L2%3DL2n)%0A%20%20%20%20(vang_1%2C%20aang_1)%20%3D%20diff_c(rang_1%2C%20duration_1)%0A%20%20%20%20(_M00%2C%20_M01%2C%20_M10%2C%20_M11%2C%20_C0%2C%20_C1%2C%20_G0%2C%20_G1%2C%20_E0%2C%20_E1)%20%3D%20dyna(time_1%2C%20L1n%2C%20L2n%2C%20d1n%2C%20d2n%2C%20m1n%2C%20m2n%2C%20gn%2C%20I1n%2C%20I2n%2C%20q1%2C%20q2%2C%20rang_1%2C%20vang_1%2C%20aang_1%2C%20Fexn%2C%20Feyn%2C%20M%2C%20C%2C%20G_1%2C%20E)%0A%20%20%20%20_T1b%20%3D%20_M00%20%2B%20_M01%20%2B%20_C0%20%2B%20_G0%20%2B%20_E0%0A%20%20%20%20_T2b%20%3D%20_M10%20%2B%20_M11%20%2B%20_C1%20%2B%20_G1%20%2B%20_E1%0A%20%20%20%20return%20aang_1%2C%20rang_1%2C%20time_1%2C%20vang_1%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20The%20interaction%20torques%20are%20larger%20than%20the%20gravitational%20torques%20for%20most%20part%20of%20the%20movement.%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20%23%23%23%20Fast%20movement%20in%20the%20horizontal%20plane%0A%0A%20%20%20%20Let's%20simulate%20a%20fast%20movement%20in%20the%20horizontal%20plane%3A%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(%0A%20%20%20%20C%2C%0A%20%20%20%20E%2C%0A%20%20%20%20Fexn%2C%0A%20%20%20%20Feyn%2C%0A%20%20%20%20G_1%2C%0A%20%20%20%20I1n%2C%0A%20%20%20%20I2n%2C%0A%20%20%20%20L1n%2C%0A%20%20%20%20L2n%2C%0A%20%20%20%20M%2C%0A%20%20%20%20aang_1%2C%0A%20%20%20%20d1n%2C%0A%20%20%20%20d2n%2C%0A%20%20%20%20dyna%2C%0A%20%20%20%20m1n%2C%0A%20%20%20%20m2n%2C%0A%20%20%20%20q1%2C%0A%20%20%20%20q2%2C%0A%20%20%20%20rang_1%2C%0A%20%20%20%20time_1%2C%0A%20%20%20%20vang_1%2C%0A)%3A%0A%20%20%20%20gn_1%20%3D%200%0A%20%20%20%20(_M00%2C%20_M01%2C%20_M10%2C%20_M11%2C%20_C0%2C%20_C1%2C%20_G0%2C%20_G1%2C%20_E0%2C%20_E1)%20%3D%20dyna(time_1%2C%20L1n%2C%20L2n%2C%20d1n%2C%20d2n%2C%20m1n%2C%20m2n%2C%20gn_1%2C%20I1n%2C%20I2n%2C%20q1%2C%20q2%2C%20rang_1%2C%20vang_1%2C%20aang_1%2C%20Fexn%2C%20Feyn%2C%20M%2C%20C%2C%20G_1%2C%20E)%0A%20%20%20%20_T1b%20%3D%20_M00%20%2B%20_M01%20%2B%20_C0%20%2B%20_G0%20%2B%20_E0%0A%20%20%20%20_T2b%20%3D%20_M10%20%2B%20_M11%20%2B%20_C1%20%2B%20_G1%20%2B%20_E1%0A%20%20%20%20return%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20**Exercise%204%20%E2%80%94%20Scale%20movement%20time.**%20Keep%20a%20joint-angle%20path%20unchanged%20but%20execute%20it%20twice%20as%20fast.%20How%20do%20the%20velocity%2C%20acceleration%2C%20velocity-quadratic%20and%20gravity%20terms%20scale%3F%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20%2F%2F%2F%20details%20%7C%20Solution%20to%20Exercise%204%0A%0A%20%20%20%20Rates%20double%20and%20accelerations%20quadruple.%20Inertial%20terms%20and%20Coriolis%2Fcentrifugal%20terms%20therefore%20scale%20by%20four%2C%20while%20gravity%20at%20a%20given%20posture%20is%20unchanged.%20This%20predicts%20the%20comparison%20between%20slow%20and%20fast%20inverse%20dynamics.%0A%20%20%20%20%2F%2F%2F%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20%23%23%20Numerical%20simulation%20of%20direct%20dynamics%0A%0A%20%20%20%20Remember%20that%20for%20direct%20dynamics%20we%20want%20to%20solve%20the%20following%20differential%20equation%3A%0A%0A%20%20%20%20%24%24%0A%20%20%20%20%5Cddot%7Bq%7D%20%5Cquad%3D%5Cquad%20M(q)%5E%7B-1%7D%20%5Cleft%5B%5Ctau%20-%20C(q%2C%5Cdot%7Bq%7D)%20-%20G(q)%20-%20E(q%2C%5Cdot%7Bq%7D)%20%5Cright%5D%0A%20%20%20%20%24%24%0A%0A%20%20%20%20Let's%20use%20the%20Euler%20method%20for%20solving%20this%20equation%20numerically.%0A%0A%20%20%20%20First%2C%20transforming%20the%20equation%20above%20into%20a%20system%20of%20two%20first-order%20ODE%3A%0A%0A%20%20%20%20%24%24%0A%20%20%20%20%5Cleft%5C%7B%0A%20%20%20%20%5Cbegin%7Barray%7D%7Bl%20l%7D%0A%20%20%20%20%5Cdfrac%7B%5Cmathrm%7Bd%7D%20q%7D%7B%5Cmathrm%7Bd%7Dt%7D%20%26%3D%26%20%5Cdot%7Bq%7D%2C%20%5Cquad%20%26q(t_0)%20%3D%20q_0%0A%20%20%20%20%5C%5C%0A%20%20%20%20%5Cdfrac%7B%5Cmathrm%7Bd%7D%20%5Cdot%7Bq%7D%7D%7B%5Cmathrm%7Bd%7Dt%7D%20%26%3D%26%20M(q)%5E%7B-1%7D%20%5Cleft%5B%5Ctau%20-%20C(q%2C%5Cdot%7Bq%7D)%20-%20G(q)%20-%20E(q%2C%5Cdot%7Bq%7D)%20%5Cright%5D%2C%20%5Cquad%20%26%5Cdot%7Bq%7D(t_0)%20%3D%20%5Cdot%7Bq%7D_0%0A%20%20%20%20%5Cend%7Barray%7D%0A%20%20%20%20%5Cright.%0A%20%20%20%20%24%24%0A%0A%20%20%20%20Second%2C%20we%20would%20write%20a%20function%20for%20the%20calculation%20of%20the%20system%20states%20and%20another%20function%20for%20the%20Euler%20method.%0A%0A%20%20%20%20Third%2C%20now%20joint%20torques%20are%20the%20input%20to%20the%20system%3B%20just%20to%20cast%20out%20nines%2C%20let's%20choose%20as%20input%20torques%20the%20output%20torques%20of%20the%20inverse%20dynamics%20solution%20we%20calculated%20before.%0A%0A%20%20%20%20Fourth%2C%20plot%20everything.%0A%0A%20%20%20%20Easy%20peasy.%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_()%3A%0A%20%20%20%20import%20marimo%20as%20mo%0A%0A%20%20%20%20return%20(mo%2C)%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20%23%23%20Checkpoint%20questions%0A%0A%20%20%20%201.%20What%20is%20known%20in%20inverse%20dynamics%2C%20and%20what%20is%20known%20in%20forward%20dynamics%3F%0A%20%20%20%202.%20Why%20can%20a%20computed%20net%20joint%20torque%20not%20uniquely%20identify%20every%20muscle%20force%3F%0A%20%20%20%203.%20Which%20terms%20vanish%20in%20an%20ideal%20horizontal-plane%20model%3F%0A%20%20%20%204.%20Why%20should%20a%20forward%20simulation%20reproduce%20a%20trajectory%20driven%20by%20its%20inverse-dynamics%20torque%3F%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20%2F%2F%2F%20details%20%7C%20Checkpoint%20answers%0A%0A%20%20%20%201.%20Inverse%3A%20motion%20and%20external%20loads%3B%20forward%3A%20forces%2Ftorques%20and%20initial%20state.%0A%20%20%20%202.%20Many%20muscles%20can%20generate%20the%20same%20net%20moment.%0A%20%20%20%203.%20Gravity%20moments%20about%20vertical%20joint%20axes%20vanish%3B%20interaction%20terms%20remain.%0A%20%20%20%204.%20They%20solve%20the%20same%20equations%3B%20disagreement%20beyond%20numerical%20error%20signals%20inconsistent%20models%2C%20signs%20or%20inputs.%0A%20%20%20%20%2F%2F%2F%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20%23%23%20Problems%0A%0A%20%20%20%201.%20Derive%20the%20equations%20of%20motion%20for%20a%20single%20pendulum%20(not%20inverted).%3Cbr%3E%0A%20%20%20%202.%20Derive%20the%20equations%20of%20motion%20for%20a%20double%20pendulum%20(not%20inverted).%3Cbr%3E%0A%20%20%20%203.%20For%20the%20one-link%20system%2C%20simulate%20a%20typical%20trajectory%20to%20calculate%20the%20joint%20torque%20(i.e.%2C%20perform%20inverse%20dynamics).%0A%20%20%20%204.%20For%20the%20one-link%20system%2C%20simulate%20a%20typical%20joint%20torque%20to%20calculate%20the%20trajectory%20(i.e.%2C%20perform%20direct%20dynamics).%0A%20%20%20%205.%20Consider%20the%20double%20pendulum%20moving%20in%20the%20horizontal%20plane%20and%20with%20no%20external%20force.%20Find%20out%20the%20type%20of%20movement%20and%20which%20torque%20terms%20are%20changed%20when%3A%3Cbr%3E%0A%20%20%20%20%20%20a)%20%24%5Cdot%7B%5Calpha%7D_1%3D0%5Eo%24%3Cbr%3E%0A%20%20%20%20%20%20b)%20%24%5Calpha_2%3D0%5Eo%24%3Cbr%3E%0A%20%20%20%20%20%20c)%20%24%5Cdot%7B%5Calpha%7D_2%3D0%5Eo%24%3Cbr%3E%0A%20%20%20%20%20%20d)%20%242%5Calpha_1%2B%5Calpha_2%3D180%5Eo%24%20(hint%3A%20a%20two-link%20system%20with%20this%20configuration%20is%20called%20polar%20manipulator)%0A%20%20%20%206.%20Derive%20the%20equations%20of%20motion%20and%20the%20torque%20terms%20using%20angles%20in%20the%20segmental%20space%20%24(%5Ctheta_1%2C%5C%2C%5Ctheta_2)%24.%3Cbr%3E%0A%20%20%20%207.%20Run%20the%20numerical%20simulations%20for%20the%20torques%20with%20different%20parameters.%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20%2F%2F%2F%20details%20%7C%20Final%20answers%20to%20original%20Problems%0A%0A%20%20%20%201.%20For%20a%20rigid%20link%20with%20center%20distance%20%24r%24%20and%20pivot%20inertia%20%24I_O%24%2C%20angle%20from%20downward%20vertical%3A%20%24I_O%5Cddot%5Ctheta%2Bmgr%5Csin%5Ctheta%3D%5Ctau%24.%0A%0A%20%20%20%202.%20For%20two%20uniform%20rods%20with%20absolute%20downward-vertical%20angles%2C%20%24M_%7B11%7D%3Dm_1L_1%5E2%2F3%2Bm_2L_1%5E2%24%2C%20%24M_%7B22%7D%3Dm_2L_2%5E2%2F3%24%2C%20%24M_%7B12%7D%3DM_%7B21%7D%3Dm_2L_1L_2%5Ccos(%5Ctheta_1-%5Ctheta_2)%2F2%24.%20The%20velocity-quadratic%20terms%20on%20the%20left%20are%20%24(h%5Cdot%5Ctheta_2%5E2%2C-h%5Cdot%5Ctheta_1%5E2)%5ET%24%2C%20with%20%24h%3Dm_2L_1L_2%5Csin(%5Ctheta_1-%5Ctheta_2)%2F2%24%3B%20gravity%20terms%20are%20%24((m_1%2F2%2Bm_2)gL_1%5Csin%5Ctheta_1%2Cm_2gL_2%5Csin%5Ctheta_2%2F2)%5ET%24.%20Applied%20generalized%20torques%20are%20%24(%5Ctau_1-%5Ctau_2%2C%5Ctau_2)%5ET%24.%0A%0A%20%20%20%203.%20For%20any%20selected%20one-link%20trajectory%2C%20%24%5Ctau(t)%3DI_O%5Cddot%5Ctheta(t)%2Bmgr%5Csin%5Ctheta(t)%24%20in%20the%20convention%20of%20answer%201.%20Different%20prescribed%20trajectories%20give%20different%20torque%20histories.%0A%0A%20%20%20%204.%20The%20forward%20result%20solves%20%24%5Cddot%5Ctheta%3D%5B%5Ctau(t)-mgr%5Csin%5Ctheta%5D%2FI_O%24%20with%20two%20stated%20initial%20conditions.%20The%20inverse%20torque%20from%20answer%203%20reproduces%20its%20trajectory%20up%20to%20integration%20error%20when%20the%20initial%20conditions%20match.%0A%0A%20%20%20%205.%20In%20joint%20coordinates%2C%20velocity%20terms%20are%20proportional%20to%20%24-b%5Csin%5Calpha_2(2%5Cdot%5Calpha_1%5Cdot%5Calpha_2%2B%5Cdot%5Calpha_2%5E2)%24%20and%20%24b%5Csin%5Calpha_2%5Cdot%5Calpha_1%5E2%24.%20(a)%20First%20joint%20stationary%3A%20only%20the%20%24%5Cdot%5Calpha_2%5E2%24%20term%20remains.%20(b)%20Straight%20links%3A%20both%20velocity%20terms%20vanish%20instantaneously.%20(c)%20Fixed%20elbow%20angle%3A%20the%20second%20term%20remains%20when%20%24%5Cdot%5Calpha_1%5Cne0%24.%20(d)%20%242%5Calpha_1%2B%5Calpha_2%3D%5Cpi%24%20gives%20a%20straight%20radial%20endpoint%20path%20for%20equal%20link%20lengths%3B%20it%20implies%20%24%5Cdot%5Calpha_2%3D-2%5Cdot%5Calpha_1%24%20and%20makes%20the%20first%20velocity%20term%20zero.%20Unequal%20link%20lengths%20do%20not%20generally%20produce%20that%20radial%20path.%0A%0A%20%20%20%206.%20With%20%24q%3D(%5Calpha_1%2C%5Calpha_2)%5ET%3DA%5Ctheta%24%2C%20%24A%3D%5Cbegin%7Bbmatrix%7D1%260%5C%5C-1%261%5Cend%7Bbmatrix%7D%24%2C%20the%20segment-coordinate%20equation%20is%20%24M_%5Ctheta%3DA%5ETM_qA%24%2C%20%24h_%5Ctheta%3DA%5ETh_q%24%2C%20%24g_%5Ctheta%3DA%5ETg_q%24%20and%20%24%5Ctau_%5Ctheta%3DA%5ET%5Ctau_q%24.%0A%0A%20%20%20%207.%20Numerical%20curves%20depend%20on%20the%20chosen%20parameters.%20For%20the%20same%20path%20traversed%20twice%20as%20fast%2C%20inertial%20and%20quadratic-velocity%20torques%20scale%20by%20four%3B%20gravity%20torque%20at%20each%20posture%20is%20unchanged.%0A%20%20%20%20%2F%2F%2F%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20%23%23%20Go%20deeper%0A%0A%20%20%20%20Use%20%5BKineticsFundamentalConcepts%5D(https%3A%2F%2Fmolab.marimo.io%2Fgithub%2FBMClab%2FBMC%2Fblob%2Fmaster%2Fnotebooks%2Fkinetics_fundamental_concepts.py)%20to%20connect%20this%20guide%20with%20the%20surrounding%20mechanics%20and%20modeling%20material.%20Return%20to%20the%20assumptions%20behind%20each%20equation%20when%20applying%20it%20to%20a%20new%20experiment.%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20%23%23%23%20Video%20lectures%20on%20the%20Internet%0A%0A%20%20%20%20%5BMIT%20OpenCourseWare%5D(https%3A%2F%2Focw.mit.edu%2Fcourses%2F2-003sc-engineering-dynamics-fall-2011%2Fpages%2Flagrange-equations%2F)%20%E2%80%94%20Lagrange%20equations%3A%20follow%20the%20energy-to-equation%20method%20in%20the%20lecture%20and%20worked%20examples.%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%0A%20%20%20%20%23%23%20References%0A%0A%20%20%20%20-%20Craig%20JJ%20(2005)%20%5BIntroduction%20to%20Robotics%3A%20Mechanics%20and%20Control%5D(https%3A%2F%2Fbooks.google.com.br%2Fbooks%3Fid%3DMqMeAQAAIAAJ).%203rd%20Edition.%20Prentice%20Hall.%0A%0A%20%20%20%20-%20Flash%20T%2C%20Hogan%20N%20(1985)%20%5BThe%20coordination%20of%20arm%20movements%3A%20an%20experimentally%20confirmed%20mathematical%20model%5D(https%3A%2F%2Fdoi.org%2F10.1523%2FJNEUROSCI.05-07-01688.1985).%20Journal%20of%20Neuroscience%2C%205%2C%201688-1703.%0A%0A%20%20%20%20-%20Hollerbach%20JM%2C%20Flash%20T%20(1982)%20%5BDynamic%20interactions%20between%20limb%20segments%20during%20planar%20arm%20movement%5D(http%3A%2F%2Flink.springer.com%2Farticle%2F10.1007%252FBF00353957).%20Biological%20Cybernetics%2C%2044%2C%2067-77.%0A%0A%20%20%20%20-%20Nigg%20BM%20and%20Herzog%20W%20(2006)%20%5BBiomechanics%20of%20the%20Musculo-skeletal%20System%5D(https%3A%2F%2Fbooks.google.com.br%2Fbooks%3Fid%3DhOIeAQAAIAAJ%26dq%3Deditions%3AISBN0470017678).%203rd%20Edition.%20Wiley.%0A%0A%20%20%20%20-%20Pandy%20MG%20(2001)%20%5BComputer%20modeling%20and%20simulation%5D(https%3A%2F%2Fdrive.google.com%2Fopen%3Fid%3D0BxbW72zV7WmUbXZBR2VRMnF5UTA%26authuser%3D0).%20Annu.%20Rev.%20Biomed.%20Eng.%2C%203%2C%20245%E2%80%9373.%0A%0A%20%20%20%20-%20Sherman%20MA%2C%20Seth%20A%2C%20Delp%20SL%20(2013)%20%5BWhat%20is%20a%20moment%20arm%3F%20Calculating%20muscle%20effectiveness%20in%20biomechanical%20models%20using%20generalized%20coordinates%5D(http%3A%2F%2Fsimtk-confluence.stanford.edu%3A8080%2Fdownload%2Fattachments%2F3376330%2FShermanSethDelp-2013-WhatIsMuscleMomentArm-Final2-DETC2013-13633.pdf%3Fversion%3D1%26modificationDate%3D1369103515834)%20in%20Proc.%20ASME%20Int.%20Design%20Engineering%20Technical%20Conferences%20(IDETC)%2C%20Portland%2C%20OR%2C%20USA.%0A%0A%20%20%20%20-%20Zajac%20FE%20(1993)%20%5BMuscle%20coordination%20of%20movement%3A%20a%20perspective%5D(http%3A%2F%2Fe.guigon.free.fr%2Frsc%2Farticle%2FZajac93.pdf).%20J%20Biomech.%2C%2026%2C%20Suppl%201%3A109-24.%0A%0A%20%20%20%20-%20Zajac%20FE%2C%20Gordon%20ME%20(1989)%20%5BDetermining%20muscle's%20force%20and%20action%20in%20multi-articular%20movement%5D(https%3A%2F%2Fdrive.google.com%2Fopen%3Fid%3D0BxbW72zV7WmUcC1zSGpEOUxhWXM%26authuser%3D0).%20Exercise%20and%20Sport%20Sciences%20Reviews%2C%2017%2C%20187-230.%0A%0A%20%20%20%20-%20Zatsiorsky%20VM%20(2002)%20%5BKinetics%20of%20human%20motion%5D(https%3A%2F%2Fbooks.google.com.br%2Fbooks%3Fid%3Dwp3zt7oF8a0C).%20Human%20Kinetics.%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0Aif%20__name__%20%3D%3D%20%22__main__%22%3A%0A%20%20%20%20app.run()%0A