%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%3E%3D2.0%22%2C%20%22matplotlib%3E%3D3.9%22%2C%20%22sympy%3E%3D1.13%22%5D%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(width%3D%22medium%22)%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%20Kinematic%20chain%20in%20a%20plane%20(2D)%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%20Your%20muscles%20do%20not%20move%20your%20hand.%20They%20rotate%20joints%3A%20the%20shoulder%2C%20the%20elbow%2C%20the%20wrist.%20The%20hand%20goes%20wherever%20the%20combination%20of%20those%20rotations%20happens%20to%20put%20it.%20And%20yet%20almost%20every%20task%20you%20perform%20is%20defined%20by%20where%20the%20hand%20must%20go%2C%20not%20by%20what%20the%20joints%20must%20do.%20Reaching%20for%20a%20cup%2C%20the%20nervous%20system%20has%20to%20translate%20one%20description%20into%20the%20other%2C%20continuously%2C%20in%20both%20directions.%0A%0A%20%20%20%20The%20mathematical%20model%20behind%20that%20translation%20is%20the%20**kinematic%20chain**%3A%20rigid%20links%20connected%20by%20joints.%20This%20notebook%20builds%20it%20up%20from%20one%20link%20to%20two%2C%20in%20a%20plane%2C%20using%20only%20trigonometry%20and%20calculus%2C%20with%20%5BSymPy%5D(http%3A%2F%2Fsympy.org%2F)%20doing%20the%20algebra.%20It%20covers%20how%20to%20go%20from%20joint%20angles%20to%20the%20position%20of%20the%20endpoint%20and%20back%2C%20how%20joint%20velocities%20become%20endpoint%20velocities%20through%20a%20matrix%20called%20the%20Jacobian%2C%20and%20why%20the%20endpoint%20accelerates%20in%20ways%20that%20no%20single%20joint%20does.%0A%0A%20%20%20%20Read%20it%20in%20order%20and%20run%20each%20cell%20as%20you%20reach%20it.%20Where%20you%20find%20a%20**Challenge**%20or%20a%20set%20of%20**Guiding%20questions**%2C%20stop%20and%20answer%20on%20a%20scratchpad%20before%20moving%20on.%20Several%20of%20them%20ask%20you%20to%20predict%20a%20result%20*before*%20the%20code%20prints%20it.%20Make%20the%20prediction%3B%20a%20wrong%20guess%20that%20the%20code%20then%20corrects%20teaches%20more%20than%20a%20right%20one.%0A%0A%20%20%20%20Along%20the%20way%20there%20are%20also%20**Exercises**%2C%20each%20followed%20by%20a%20collapsed%20solution.%20Try%20the%20exercise%20first%2C%20and%20open%20the%20solution%20only%20to%20check%20your%20answer.%0A%0A%20%20%20%20You%20will%20need%20vectors%20from%20%5BScalar%20and%20vector%5D(https%3A%2F%2Fmolab.marimo.io%2Fgithub%2FBMClab%2FBMC%2Fblob%2Fmaster%2Fnotebooks%2Fscalar_vector.py)%2C%20derivatives%20from%20%5BKinematics%20of%20a%20particle%5D(https%3A%2F%2Fmolab.marimo.io%2Fgithub%2FBMClab%2FBMC%2Fblob%2Fmaster%2Fnotebooks%2Fkinematics_particle.py)%2C%20and%20the%20tangential%20and%20normal%20accelerations%20from%20%5BPath%20frame%5D(https%3A%2F%2Fmolab.marimo.io%2Fgithub%2FBMClab%2FBMC%2Fblob%2Fmaster%2Fnotebooks%2Fpath_frame.py).%0A%0A%20%20%20%20**Challenge%200.**%20Put%20your%20hand%20flat%20on%20a%20table%20and%20slide%20your%20fingertip%20along%20a%20straight%20line%2C%20from%20close%20to%20your%20chest%20to%20almost%20arm's%20length%2C%20without%20moving%20your%20trunk.%20Pay%20attention%20to%20your%20shoulder%20and%20your%20elbow.%20Does%20each%20of%20them%20rotate%20in%20one%20direction%20only%3F%20At%20a%20constant%20rate%3F%20Write%20down%20your%20answers%3B%20the%20checkpoint%20questions%20ask%20you%20to%20come%20back%20to%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%20%23%23%20Reaching%20in%20a%20straight%20line%0A%0A%20%20%20%20When%20people%20move%20the%20hand%20from%20one%20point%20to%20another%20on%20a%20table%2C%20the%20hand%20travels%20along%20a%20nearly%20straight%20path%2C%20and%20its%20speed%20rises%20and%20falls%20in%20a%20smooth%2C%20bell-shaped%20curve%20(Morasso%2C%201981).%20Flash%20and%20Hogan%20(1985)%20showed%20that%20these%20paths%20are%20well%20described%20by%20the%20*minimum-jerk*%20trajectory%2C%20the%20smoothest%20possible%20movement%20of%20the%20hand%20between%20the%20two%20points%20(see%20%5BThe%20minimum-jerk%20hypothesis%5D(https%3A%2F%2Fmolab.marimo.io%2Fgithub%2FBMClab%2FBMC%2Fblob%2Fmaster%2Fnotebooks%2Fminimum_jerk_hypothesis.py)).%0A%0A%20%20%20%20A%20straight%2C%20smooth%20path%20is%20simple%20to%20describe%20for%20the%20hand.%20It%20is%20not%20obviously%20simple%20for%20the%20joints.%20The%20shoulder%20and%20the%20elbow%20are%20hinges%3B%20each%20of%20them%20can%20only%20swing%20the%20parts%20beyond%20it%20around%20a%20circle.%20Somehow%2C%20two%20circular%20motions%20must%20add%20up%20to%20a%20straight%20line.%0A%0A%20%20%20%20By%20the%20end%20of%20this%20notebook%20you%20will%20compute%20exactly%20what%20the%20shoulder%20and%20elbow%20have%20to%20do%20for%20such%20a%20reach%2C%20and%20see%20that%20the%20simplicity%20of%20the%20hand's%20movement%20is%20not%20shared%20by%20the%20joints.%0A%0A%20%20%20%20**Guiding%20questions%200.**%0A%0A%20%20%20%201.%20For%20the%20hand%20to%20move%20in%20a%20straight%20line%2C%20must%20both%20joints%20rotate%20in%20the%20same%20direction%3F%20Must%20they%20rotate%20at%20constant%20angular%20velocities%3F%0A%20%20%20%202.%20An%20arm%20with%20two%20joints%20moving%20in%20a%20plane%20has%20two%20angles%20to%20set%2C%20and%20the%20hand%20has%20two%20coordinates.%20For%20a%20given%20point%20on%20the%20table%2C%20how%20many%20different%20arm%20configurations%20put%20the%20hand%20there%3F%0A%20%20%20%203.%20Suppose%20the%20elbow%20is%20locked%20and%20the%20shoulder%20rotates%20at%20a%20constant%20angular%20velocity.%20The%20joints%20do%20not%20accelerate.%20Does%20the%20hand%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%20SymPy%20derives%20the%20expressions%2C%20NumPy%20evaluates%20them%20and%20Matplotlib%20draws%20the%20plots.%20SymPy%20prints%20derivatives%20with%20respect%20to%20time%20as%20%24%5Cfrac%7Bd%7D%7Bdt%7D%5Ctheta(t)%24%3B%20the%20function%20%60show%60%2C%20defined%20below%2C%20prints%20them%20instead%20in%20%5BNewton's%20notation%5D(https%3A%2F%2Fen.wikipedia.org%2Fwiki%2FNotation_for_differentiation)%2C%20%24%5Cdot%7B%5Ctheta%7D%24%2C%20which%20is%20much%20easier%20to%20read.%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%20numpy%20as%20np%0A%20%20%20%20import%20matplotlib%0A%20%20%20%20import%20matplotlib.pyplot%20as%20plt%0A%20%20%20%20import%20sympy%20as%20sym%0A%20%20%20%20from%20sympy.vector%20import%20CoordSys3D%0A%20%20%20%20from%20sympy.physics.mechanics%20import%20dynamicsymbols%0A%0A%20%20%20%20matplotlib.rc(%22axes%22%2C%20labelsize%3D12%2C%20titlesize%3D13)%0A%20%20%20%20matplotlib.rc(%22xtick%22%2C%20labelsize%3D10)%0A%20%20%20%20matplotlib.rc(%22ytick%22%2C%20labelsize%3D10)%0A%20%20%20%20matplotlib.rc(%22legend%22%2C%20fontsize%3D10)%0A%20%20%20%20return%20CoordSys3D%2C%20dynamicsymbols%2C%20np%2C%20plt%2C%20sym%0A%0A%0A%40app.function%0Adef%20show(expr%2C%20lhs%3D%22%22)%3A%0A%20%20%20%20%22%22%22Display%20a%20SymPy%20expression%2C%20with%20time%20derivatives%20in%20Newton's%20notation.%22%22%22%0A%20%20%20%20import%20marimo%20as%20mo%0A%20%20%20%20from%20sympy.physics.vector.printing%20import%20vlatex%0A%0A%20%20%20%20return%20mo.md(rf%22%24%24%20%7Blhs%7D%20%7Bvlatex(expr)%7D%20%24%24%22)%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%20What%20a%20kinematic%20chain%20is%0A%0A%20%20%20%20%3Cfigure%3E%3Ca%20href%3D%22https%3A%2F%2Fen.wikipedia.org%2Fwiki%2FKinematic_chain%22%3E%3Cimg%20align%3D%22right%22%20src%3D%22https%3A%2F%2Fupload.wikimedia.org%2Fwikipedia%2Fcommons%2F2%2F2c%2FModele_cinematique_corps_humain.svg%22%20width%3D%22240%22%20style%3D%22margin%3A%200%2010px%200%2010px%22%20alt%3D%22human%20body%20modelled%20as%20a%20kinematic%20chain%22%2F%3E%3C%2Fa%3E%3C%2Ffigure%3E%0A%0A%20%20%20%20**A%20kinematic%20chain%20is%20an%20assembly%20of%20rigid%20bodies%20(links)%20connected%20by%20joints%3B%20it%20is%20the%20mathematical%20model%20for%20a%20mechanical%20system%2C%20which%20in%20turn%20can%20represent%20a%20biological%20system%20such%20as%20the%20human%20body**%20(%5BWikipedia%5D(https%3A%2F%2Fen.wikipedia.org%2Fwiki%2FKinematic_chain)).%0A%0A%20%20%20%20The%20term%20chain%20refers%20to%20the%20fact%20that%20the%20links%20are%20constrained%20by%20their%20connections%20to%20other%20links%2C%20typically%20by%20a%20hinge%20joint%2C%20also%20called%20a%20pin%20joint%20or%20revolute%20joint.%20As%20a%20consequence%20of%20this%20constraint%2C%20each%20link%20of%20a%20kinematic%20chain%20in%20a%20plane%20rotates%20about%20its%20joint%3A%20the%20motion%20of%20a%20planar%20chain%20is%20built%20out%20of%20circular%20motions%20of%20rigid%20objects.%0A%0A%20%20%20%20Chapter%2016%20of%20Ruina%20and%20Pratap's%20book%20and%20chapter%202%20of%20Rade's%20book%20are%20a%20good%20formal%20introduction%20to%20the%20circular%20motion%20of%20a%20rigid%20object%2C%20and%20sections%202.7%20and%202.8%20of%20Rade's%20book%20cover%20kinematic%20chains%20specifically.%20In%20this%20notebook%2C%20however%2C%20we%20do%20not%20use%20the%20formalism%20introduced%20in%20those%20chapters%2C%20the%20rotating%20reference%20frame%20and%20the%20rotation%20matrix%3B%20those%20are%20covered%20in%20%5BRigid-body%20transformations%20in%202D%5D(https%3A%2F%2Fmolab.marimo.io%2Fgithub%2FBMClab%2FBMC%2Fblob%2Fmaster%2Fnotebooks%2Ftransformation_2d.py).%20Here%2C%20we%20describe%20the%20kinematics%20of%20a%20chain%20in%20a%20Cartesian%20coordinate%20system%20using%20only%20trigonometry%20and%20calculus.%20This%20approach%20is%20simpler%20and%20more%20intuitive%2C%20but%20it%20gets%20too%20complicated%20for%20a%20chain%20with%20many%20links%20or%20in%20three%20dimensions%3B%20for%20such%20problems%2C%20rigid-body%20transformations%20are%20the%20recommended%20tool%20(see%2C%20for%20example%2C%20Siciliano%20et%20al.%20(2009)).%0A%0A%20%20%20%20We%20derive%20the%20kinematic%20properties%20of%20the%20chains%20algebraically%20with%20%5BSymPy%5D(http%3A%2F%2Fsympy.org%2F)%2C%20a%20Python%20library%20for%20symbolic%20mathematics.%20SymPy%20has%20a%20%5Bmechanics%20module%5D(https%3A%2F%2Fdocs.sympy.org%2Flatest%2Fmodules%2Fphysics%2Fmechanics%2Findex.html)%20built%20for%20deriving%20the%20equations%20of%20motion%20of%20multibody%20systems%2C%20but%20we%20will%20derive%20most%20of%20it%20ourselves%2C%20to%20understand%20the%20details.%0A%0A%20%20%20%20%3Cp%20style%3D%22text-align%3A%20right%3B%22%3E%3Ci%3EFigure.%20The%20human%20body%20modelled%20as%20a%20kinematic%20chain%20(%3Ca%20href%3D%22https%3A%2F%2Fen.wikipedia.org%2Fwiki%2FKinematic_chain%22%3Eimage%20from%20Wikipedia%3C%2Fa%3E).%3C%2Fi%3E%3C%2Fp%3E%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%20Properties%20of%20kinematic%20chains%0A%0A%20%20%20%20The%20**base**%20of%20a%20kinematic%20chain%20is%20the%20extremity%20(origin)%20of%20the%20chain%20that%20is%20typically%20considered%20attached%20to%20the%20ground%2C%20to%20the%20body%2C%20or%20otherwise%20fixed.%20The%20**endpoint**%20is%20the%20other%20extremity%2C%20which%20typically%20can%20move.%20In%20robotics%2C%20the%20term%20**end-effector**%20is%20used%2C%20and%20it%20usually%20refers%20to%20the%20last%20link%20of%20the%20chain.%0A%0A%20%20%20%20In%20topological%20terms%2C%20a%20kinematic%20chain%20is%20called%20**open**%20when%20there%20is%20only%20one%20sequence%20of%20links%20connecting%20its%20two%20ends.%20Otherwise%20it%20is%20called%20**closed**%2C%20and%20a%20sequence%20of%20links%20forms%20a%20loop.%20A%20kinematic%20chain%20can%20be%20**serial**%2C%20**parallel**%2C%20or%20a%20**mix**%20of%20both.%20In%20a%20serial%20chain%20the%20links%20are%20connected%20one%20after%20the%20other%3B%20a%20serial%20chain%20is%20an%20open%20chain.%20Otherwise%2C%20the%20chain%20is%20parallel%2C%20or%20branched%20(for%20example%2C%20the%20hand%20and%20fingers).%0A%0A%20%20%20%20Although%20this%20definition%20is%20clear%20and%20classic%20in%20mechanics%2C%20it%20is%20not%20the%20one%20used%20by%20health%20professionals%20(clinicians%20and%20athletic%20trainers)%20to%20describe%20human%20movement.%20They%20call%20a%20set%20of%20joints%20and%20segments%20a%20closed%20or%20open%20kinematic%20(or%20kinetic)%20chain%20simply%20according%20to%20whether%20the%20distal%20segment%2C%20typically%20the%20foot%20or%20the%20hand%2C%20is%20fixed%20(closed%20chain)%20or%20not%20(open%20chain).%20The%20two%20definitions%20sometimes%20classify%20the%20same%20movement%20differently.%20A%20person%20standing%20on%20one%20leg%2C%20for%20example%2C%20is%20an%20open%20kinematic%20chain%20in%20mechanics%2C%20but%20a%20closed%20one%20in%20the%20clinical%20sense.%20In%20this%20text%20we%20stay%20consistent%20with%20mechanics%2C%20but%20keep%20the%20difference%20in%20mind%20when%20talking%20with%20clinicians%20and%20athletic%20trainers.%0A%0A%20%20%20%20Another%20important%20property%20of%20a%20kinematic%20chain%20is%20its%20number%20of%20**%3Ca%20href%3D%22https%3A%2F%2Fen.wikipedia.org%2Fwiki%2FDegrees_of_freedom_(mechanics)%22%3Edegrees%20of%20freedom%20(DOF)%3C%2Fa%3E**.%20In%20mechanics%2C%20the%20degrees%20of%20freedom%20of%20a%20system%20are%20the%20number%20of%20independent%20parameters%20needed%20to%20define%20its%20configuration.%20A%20particle%20in%203D%20space%20has%20three%20DOFs%2C%20because%20we%20need%20three%20coordinates%20to%20specify%20its%20position.%20A%20rigid%20body%20in%203D%20space%20has%20six%20DOFs%3A%20three%20coordinates%20of%20one%20of%20its%20points%20to%20specify%20its%20position%2C%20and%20three%20angles%20to%20specify%20its%20orientation.%20A%20link%20attached%20to%20a%20fixed%20body%20by%20a%20hinge%20joint%20in%20a%20plane%20needs%20only%20one%20angle%20to%20define%20its%20configuration%2C%20so%20it%20has%20one%20DOF.%20A%20kinematic%20chain%20with%20two%20links%20in%20a%20plane%20has%20two%20DOFs%2C%20and%20so%20on.%0A%0A%20%20%20%20The%20**mobility**%20of%20a%20kinematic%20chain%20is%20its%20total%20number%20of%20degrees%20of%20freedom.%20The%20**redundancy**%20of%20a%20kinematic%20chain%20is%20its%20mobility%20minus%20the%20number%20of%20degrees%20of%20freedom%20of%20the%20endpoint.%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%20The%20kinematics%20of%20a%20one-link%20system%0A%0A%20%20%20%20Let's%20start%20with%20a%20system%20made%20of%20one%20planar%20hinge%20joint%20and%20one%20link.%20Technically%20it%20is%20not%20a%20chain%2C%20but%20it%20introduces%2C%20one%20at%20a%20time%2C%20all%20the%20concepts%20we%20will%20need.%0A%0A%20%20%20%20%3Cfigure%20aria-describedby%3D%22figure-caption-1%22%3E%3Ccenter%3E%3Cimg%20src%3D%22https%3A%2F%2Fgithub.com%2FBMClab%2FBMC%2Fblob%2Fmaster%2Fimages%2Fonelink.gif%3Fraw%3D1%22%20width%3D350%20alt%3D%22one%20link%20attached%20to%20a%20fixed%20body%20by%20a%20hinge%20joint%22%2F%3E%3C%2Fcenter%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.%20One%20link%20attached%20to%20a%20fixed%20body%20by%20a%20hinge%20joint%20in%20a%20plane.%3C%2Fspan%3E%0A%0A%20%20%20%20We%20need%20a%20Cartesian%20coordinate%20system%20and%20the%20symbols%20for%20time%2C%20%24t%24%2C%20the%20length%20of%20the%20link%2C%20%24%5Cell%24%2C%20and%20the%20joint%20angle%2C%20%24%5Ctheta%24%2C%20which%20is%20a%20function%20of%20time%3A%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(CoordSys3D%2C%20dynamicsymbols%2C%20sym)%3A%0A%20%20%20%20G%20%3D%20CoordSys3D(%22%22)%0A%20%20%20%20t%20%3D%20sym.Symbol(%22t%22)%0A%20%20%20%20ell%20%3D%20sym.Symbol(%22ell%22%2C%20real%3DTrue%2C%20positive%3DTrue)%0A%20%20%20%20theta%20%3D%20dynamicsymbols(%22theta%22%2C%20real%3DTrue)%20%20%23%20or%20sym.Function(%22theta%22)(t)%0A%20%20%20%20return%20G%2C%20ell%2C%20t%2C%20theta%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%20Using%20trigonometry%2C%20the%20position%20of%20the%20endpoint%20in%20terms%20of%20the%20joint%20angle%20and%20the%20length%20of%20the%20link%20is%3A%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(G%2C%20ell%2C%20sym%2C%20theta)%3A%0A%20%20%20%20r_p%20%3D%20ell%20*%20sym.cos(theta)%20*%20G.i%20%2B%20ell%20*%20sym.sin(theta)%20*%20G.j%20%2B%200%20*%20G.k%0A%0A%20%20%20%20show(r_p%2C%20r%22%5Coverrightarrow%7B%5Cmathbf%7Br%7D%7D_P%20%3D%22)%0A%20%20%20%20return%20(r_p%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%20With%20the%20components%3A%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(G%2C%20mo%2C%20r_p)%3A%0A%20%20%20%20_components%20%3D%20r_p.components%0A%0A%20%20%20%20mo.vstack(%5Bshow(_components%5BG.i%5D%2C%20%22x_P%20%3D%22)%2C%20show(_components%5BG.j%5D%2C%20%22y_P%20%3D%22)%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%20%23%23%23%20Forward%20and%20inverse%20kinematics%0A%0A%20%20%20%20Computing%20the%20configuration%20of%20a%20link%20or%20a%20chain%2C%20including%20the%20location%20of%20the%20endpoint%2C%20from%20the%20joint%20parameters%20(joint%20angles%20and%20link%20lengths)%2C%20as%20we%20have%20just%20done%2C%20is%20called%20%5B**forward%20or%20direct%20kinematics**%5D(https%3A%2F%2Fen.wikipedia.org%2Fwiki%2FForward_kinematics).%0A%0A%20%20%20%20If%20the%20coordinates%20of%20the%20endpoint%20are%20known%2C%20for%20example%20because%20they%20were%20measured%20with%20a%20motion%20capture%20system%2C%20and%20we%20want%20the%20joint%20angle(s)%2C%20the%20process%20is%20called%20%5B**inverse%20kinematics**%5D(https%3A%2F%2Fen.wikipedia.org%2Fwiki%2FInverse_kinematics).%20For%20the%20one-link%20system%3A%0A%0A%20%20%20%20%24%24%0A%20%20%20%20%5Ctheta%20%3D%20%5Carctan%5Cleft(%5Cfrac%7By_P%7D%7Bx_P%7D%5Cright)%0A%20%20%20%20%24%24%0A%0A%20%20%20%20with%20one%20caveat%20that%20matters%20in%20code.%20The%20ratio%20%24y_P%2Fx_P%24%20is%20the%20same%20for%20a%20point%20and%20for%20its%20mirror%20image%20through%20the%20joint%2C%20so%20the%20plain%20arctangent%20cannot%20tell%20%2445%5Eo%24%20from%20%24-135%5Eo%24.%20The%20two-argument%20function%20%60numpy.arctan2(y%2C%20x)%60%20uses%20the%20signs%20of%20both%20coordinates%20and%20returns%20the%20correct%20angle%20in%20all%20four%20quadrants.%20We%20will%20meet%20this%20again%2C%20with%20consequences%2C%20in%20the%20two-link%20chain.%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%20Matrix%20representation%20of%20the%20kinematics%0A%0A%20%20%20%20The%20algebra%20is%20easier%20in%20matrix%20form%20(and%20let's%20drop%20the%20explicit%20dependence%20on%20%24t%24%20when%20printing)%3A%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(G%2C%20r_p%2C%20sym)%3A%0A%20%20%20%20r%20%3D%20sym.Matrix((r_p.dot(G.i)%2C%20r_p.dot(G.j)))%0A%0A%20%20%20%20show(r%2C%20r%22%5Coverrightarrow%7B%5Cmathbf%7Br%7D%7D_P%20%3D%22)%0A%20%20%20%20return%20(r%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%20matrix%20form%20simplifies%20things%2C%20but%20we%20lose%20some%20of%20SymPy's%20methods%20for%20vectors%3B%20for%20instance%2C%20the%20vector%20%60r_p%60%20has%20a%20method%20%60magnitude%60%2C%20and%20the%20matrix%20%60r%60%20does%20not.%20If%20you%20prefer%2C%20you%20can%20keep%20the%20vector%20representation%20and%20switch%20to%20the%20matrix%20representation%20only%20to%20display%20a%20variable%3A%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(G%2C%20r_p)%3A%0A%20%20%20%20show(r_p.to_matrix(G)%2C%20r%22%5Coverrightarrow%7B%5Cmathbf%7Br%7D%7D_P%20%3D%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%20third%20element%20of%20this%20matrix%20is%20the%20%24%5Chat%7B%5Cmathbf%7Bk%7D%7D%24%20component%2C%20which%20is%20zero%20because%20the%20movement%20is%20planar.%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%20Differential%20kinematics%0A%0A%20%20%20%20Differential%20kinematics%20gives%20the%20relationship%20between%20the%20angular%20velocities%20of%20the%20joints%20and%20the%20linear%20velocity%20of%20the%20endpoint.%20This%20mapping%20is%20described%20by%20a%20matrix%2C%20the%20%5B**Jacobian%20matrix**%5D(https%3A%2F%2Fen.wikipedia.org%2Fwiki%2FJacobian_matrix_and_determinant)%2C%20which%20depends%20on%20the%20configuration%20of%20the%20chain%20and%20is%20of%20great%20use%20in%20the%20study%20of%20kinematic%20chains.%20First%2C%20let's%20derive%20the%20velocity%20of%20the%20endpoint%20without%20the%20Jacobian%3B%20then%20we%20will%20see%20how%20to%20calculate%20it%20with%20the%20Jacobian.%0A%0A%20%20%20%20The%20velocity%20of%20the%20endpoint%20is%20the%20first-order%20derivative%20of%20its%20position%20vector%2C%20and%20the%20derivative%20of%20a%20vector%20is%20obtained%20by%20differentiating%20each%20of%20its%20components%3A%0A%0A%20%20%20%20%24%24%0A%20%20%20%20%5Cfrac%7B%5Cmathrm%7Bd%7D%5Coverrightarrow%7B%5Cmathbf%7Br%7D%7D%7D%7B%5Cmathrm%7Bd%7Dt%7D%20%3D%0A%20%20%20%20%5Cbegin%7Bbmatrix%7D%0A%20%20%20%20%5Cdfrac%7B%5Cmathrm%7Bd%7Dx_P%7D%7B%5Cmathrm%7Bd%7Dt%7D%20%5C%5C%5B2ex%5D%0A%20%20%20%20%5Cdfrac%7B%5Cmathrm%7Bd%7Dy_P%7D%7B%5Cmathrm%7Bd%7Dt%7D%0A%20%20%20%20%5Cend%7Bbmatrix%7D%0A%20%20%20%20%24%24%0A%0A%20%20%20%20The%20derivative%20is%20with%20respect%20to%20time%2C%20but%20%24x_P%24%20and%20%24y_P%24%20depend%20explicitly%20on%20%24%5Ctheta%24%2C%20and%20it%20is%20%24%5Ctheta%24%20that%20depends%20on%20%24t%24%3A%20%24x_P%24%20and%20%24y_P%24%20depend%20on%20%24t%24%20only%20implicitly.%20To%20calculate%20this%20type%20of%20derivative%20we%20use%20the%20%5Bchain%20rule%5D(https%3A%2F%2Fen.wikipedia.org%2Fwiki%2FChain_rule).%0A%0A%20%20%20%20%3E%20**%5BChain%20rule%5D(https%3A%2F%2Fen.wikipedia.org%2Fwiki%2FChain_rule).**%20For%20a%20variable%20%24f%24%20that%20is%20a%20function%20of%20a%20variable%20%24g%24%2C%20which%20in%20turn%20is%20a%20function%20of%20%24t%24%2C%20%24f(g(t))%24%20or%20%24(f%5Ccirc%20g)(t)%24%2C%20the%20derivative%20of%20%24f%24%20with%20respect%20to%20%24t%24%20is%2C%20in%20%5BLagrange's%20notation%5D(https%3A%2F%2Fen.wikipedia.org%2Fwiki%2FNotation_for_differentiation)%3A%0A%20%20%20%20%3E%0A%20%20%20%20%3E%20%24%24(f%5Ccirc%20g)%5E%7B'%7D(t)%20%3D%20f'(g(t))%20%5Ccdot%20g'(t)%24%24%0A%20%20%20%20%3E%0A%20%20%20%20%3E%20or%2C%20in%20%5BLeibniz's%20notation%5D(https%3A%2F%2Fen.wikipedia.org%2Fwiki%2FNotation_for_differentiation)%3A%0A%20%20%20%20%3E%0A%20%20%20%20%3E%20%24%24%5Cfrac%7B%5Cmathrm%7Bd%7Df%7D%7B%5Cmathrm%7Bd%7Dt%7D%20%3D%20%5Cfrac%7B%5Cmathrm%7Bd%7Df%7D%7B%5Cmathrm%7Bd%7Dg%7D%20%5Ccdot%20%5Cfrac%7B%5Cmathrm%7Bd%7Dg%7D%7B%5Cmathrm%7Bd%7Dt%7D%24%24%0A%20%20%20%20%3E%0A%20%20%20%20%3E%20If%20%24f%24%20is%20a%20function%20of%20two%20variables%20that%20are%20both%20functions%20of%20%24t%24%2C%20%24f(x(t)%2Cy(t))%24%2C%20the%20chain%20rule%20becomes%3A%0A%20%20%20%20%3E%0A%20%20%20%20%3E%20%24%24%5Cfrac%7B%5Cmathrm%7Bd%7Df%7D%7B%5Cmathrm%7Bd%7Dt%7D%20%3D%20%5Cfrac%7B%5Cpartial%20f%7D%7B%5Cpartial%20x%7D%20%5Ccdot%20%5Cfrac%7B%5Cmathrm%7Bd%7Dx%7D%7B%5Cmathrm%7Bd%7Dt%7D%20%2B%20%5Cfrac%7B%5Cpartial%20f%7D%7B%5Cpartial%20y%7D%20%5Ccdot%20%5Cfrac%7B%5Cmathrm%7Bd%7Dy%7D%7B%5Cmathrm%7Bd%7Dt%7D%24%24%0A%20%20%20%20%3E%0A%20%20%20%20%3E%20where%20%24%5Cmathrm%7Bd%7Df%2F%5Cmathrm%7Bd%7Dt%24%20is%20the%20%5Btotal%20derivative%5D(https%3A%2F%2Fen.wikipedia.org%2Fwiki%2FTotal_derivative)%20and%20%24%5Cpartial%20f%20%2F%20%5Cpartial%20x%24%20is%20a%20%5Bpartial%20derivative%5D(https%3A%2F%2Fen.wikipedia.org%2Fwiki%2FPartial_derivative).%0A%20%20%20%20%3E%0A%20%20%20%20%3E%20**%5BProduct%20rule%5D(https%3A%2F%2Fen.wikipedia.org%2Fwiki%2FProduct_rule).**%20The%20derivative%20of%20the%20product%20of%20two%20functions%20is%3A%0A%20%20%20%20%3E%0A%20%20%20%20%3E%20%24%24%20(f%20%5Ccdot%20g)'%20%3D%20f'%20%5Ccdot%20g%20%2B%20f%20%5Ccdot%20g'%20%24%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%23%23%23%20Linear%20velocity%20of%20the%20endpoint%0A%0A%20%20%20%20For%20the%20planar%20one-link%20system%2C%20the%20linear%20velocity%20of%20the%20endpoint%20is%3A%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(r%2C%20t)%3A%0A%20%20%20%20v%20%3D%20r.diff(t)%0A%0A%20%20%20%20show(v%2C%20r%22%5Coverrightarrow%7B%5Cmathbf%7Bv%7D%7D%20%3D%22)%0A%20%20%20%20return%20(v%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%24%5Cdot%7B%5Ctheta%7D%24%20is%20the%20angular%20velocity%20of%20the%20joint.%20It%20is%20an%20unknown%20function%20of%20time%2C%20which%20is%20why%20SymPy%20leaves%20it%20as%20a%20derivative%20instead%20of%20evaluating%20it.%20The%20magnitude%2C%20or%20%5BEuclidean%20norm%5D(https%3A%2F%2Fen.wikipedia.org%2Fwiki%2FVector_norm)%2C%20of%20the%20vector%20%24%5Coverrightarrow%7B%5Cmathbf%7Bv%7D%7D%24%20is%3A%0A%0A%20%20%20%20%24%24%0A%20%20%20%20%7C%7C%5Coverrightarrow%7B%5Cmathbf%7Bv%7D%7D%7C%7C%3D%5Csqrt%7Bv_x%5E2%2Bv_y%5E2%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_(sym%2C%20v)%3A%0A%20%20%20%20show(sym.simplify(sym.sqrt(v%5B0%5D%20**%202%20%2B%20v%5B1%5D%20**%202))%2C%20r%22%7C%7C%5Coverrightarrow%7B%5Cmathbf%7Bv%7D%7D%7C%7C%20%3D%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%20That%20is%20%24%5Cell%7C%5Cdot%7B%5Ctheta%7D%7C%24%3A%20the%20speed%20of%20the%20endpoint%20is%20the%20length%20of%20the%20link%20times%20the%20magnitude%20of%20the%20angular%20velocity%20(SymPy%20does%20not%20know%20the%20sign%20of%20%24%5Cdot%7B%5Ctheta%7D%24%2C%20so%20it%20keeps%20the%20square%20root%20of%20its%20square).%20We%20could%20have%20used%20SymPy's%20%60norm%60%20method%2C%20but%20the%20output%20does%20not%20simplify%20as%20nicely%3A%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(sym%2C%20v)%3A%0A%20%20%20%20show(sym.simplify(v.norm())%2C%20r%22%7C%7C%5Coverrightarrow%7B%5Cmathbf%7Bv%7D%7D%7C%7C%20%3D%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%20direction%20of%20%24%5Coverrightarrow%7B%5Cmathbf%7Bv%7D%7D%24%20is%20tangent%20to%20the%20circular%20trajectory%20of%20the%20endpoint%2C%20as%20the%20figure%20below%20shows%2C%20together%20with%20the%20components%20of%20the%20velocity.%0A%0A%20%20%20%20%3Cfigure%20aria-describedby%3D%22figure-caption-2%22%3E%3Ccenter%3E%3Cimg%20src%3D%22https%3A%2F%2Fgithub.com%2FBMClab%2FBMC%2Fblob%2Fmaster%2Fimages%2Fonelink_vel.gif%3Fraw%3D1%22%20width%3D350%20alt%3D%22endpoint%20velocity%20of%20one%20link%22%2F%3E%3C%2Fcenter%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.%20Endpoint%20velocity%20of%20one%20link%20attached%20to%20a%20fixed%20body%20by%20a%20hinge%20joint%20in%20a%20plane.%3C%2Fspan%3E%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%20Linear%20acceleration%20of%20the%20endpoint%0A%0A%20%20%20%20The%20acceleration%20of%20the%20endpoint%20is%20the%20second-order%20derivative%20of%20its%20position%2C%20or%20the%20first-order%20derivative%20of%20its%20velocity.%20Using%20the%20chain%20and%20product%20rules%2C%20the%20linear%20acceleration%20of%20the%20endpoint%20is%3A%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(t%2C%20v)%3A%0A%20%20%20%20acc%20%3D%20v.diff(t%2C%201)%0A%0A%20%20%20%20show(acc%2C%20r%22%5Coverrightarrow%7B%5Cmathbf%7Ba%7D%7D%20%3D%22)%0A%20%20%20%20return%20(acc%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%20expression%20has%20two%20types%20of%20terms%20in%20each%20direction%3A%20one%20proportional%20to%20the%20angular%20acceleration%2C%20%24%5Cddot%7B%5Ctheta%7D%24%2C%20and%20one%20proportional%20to%20the%20square%20of%20the%20angular%20velocity%2C%20%24%5Cdot%7B%5Ctheta%7D%5E%7B2%7D%24.%0A%0A%20%20%20%20%23%23%23%23%20Tangential%20acceleration%0A%0A%20%20%20%20The%20term%20proportional%20to%20the%20angular%20acceleration%2C%20%24a_t%24%2C%20is%20always%20tangent%20to%20the%20trajectory%20of%20the%20endpoint%20(see%20the%20figure%20below)%2C%20and%20its%20magnitude%20is%3A%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(acc%2C%20sym%2C%20t%2C%20theta)%3A%0A%20%20%20%20_A%20%3D%20theta.diff(t%2C%202)%0A%0A%20%20%20%20show(sym.simplify(sym.sqrt(sym.expand(acc%5B0%5D).coeff(_A)%20**%202%20%2B%20sym.expand(acc%5B1%5D).coeff(_A)%20**%202))%20*%20_A%2C%20%22a_t%20%3D%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%20Centripetal%20acceleration%0A%0A%20%20%20%20The%20term%20proportional%20to%20the%20square%20of%20the%20angular%20velocity%2C%20%24a_c%24%2C%20always%20points%20to%20the%20joint%2C%20the%20centre%20of%20the%20circular%20motion%20(see%20the%20figure%20below)%2C%20which%20is%20why%20it%20is%20called%20the%20%5Bcentripetal%20acceleration%5D(https%3A%2F%2Fen.wikipedia.org%2Fwiki%2FCentripetal_acceleration%23Tangential_and_centripetal_acceleration).%20Its%20magnitude%20is%3A%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(acc%2C%20sym%2C%20t%2C%20theta)%3A%0A%20%20%20%20_A%20%3D%20theta.diff(t)%20**%202%0A%0A%20%20%20%20show(sym.simplify(sym.sqrt(sym.expand(acc%5B0%5D).coeff(_A)%20**%202%20%2B%20sym.expand(acc%5B1%5D).coeff(_A)%20**%202))%20*%20_A%2C%20%22a_c%20%3D%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%20So%20the%20endpoint%20accelerates%20even%20when%20the%20angular%20acceleration%20is%20zero%3A%20the%20magnitude%20of%20its%20velocity%20is%20then%20constant%2C%20but%20its%20direction%20keeps%20changing%2C%20and%20the%20centripetal%20acceleration%20is%20what%20changes%20it.%20These%20are%20the%20tangential%20and%20normal%20accelerations%20of%20%5BPath%20frame%5D(https%3A%2F%2Fmolab.marimo.io%2Fgithub%2FBMClab%2FBMC%2Fblob%2Fmaster%2Fnotebooks%2Fpath_frame.py)%2C%20for%20a%20path%20whose%20radius%20of%20curvature%20is%20always%20%24%5Cell%24.%0A%0A%20%20%20%20%3Cfigure%20aria-describedby%3D%22figure-caption-3%22%3E%3Ccenter%3E%3Cimg%20src%3D%22https%3A%2F%2Fgithub.com%2FBMClab%2FBMC%2Fblob%2Fmaster%2Fimages%2Fonelink_acc.gif%3Fraw%3D1%22%20width%3D350%20alt%3D%22tangential%20and%20centripetal%20acceleration%20of%20one%20link%22%2F%3E%3C%2Fcenter%3E%3C%2Ffigure%3E%0A%0A%20%20%20%20%3Cspan%20id%3D%22figure-caption-3%22%20class%3D%22figure-caption%22%20style%3D%22display%3Ablock%3Btext-align%3Acenter%3Bfont-style%3Aitalic%22%3EFigure.%20Endpoint%20tangential%20and%20centripetal%20acceleration%20terms%20of%20one%20link%20attached%20to%20a%20fixed%20body%20by%20a%20hinge%20joint%20in%20a%20plane.%3C%2Fspan%3E%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%20Simulation%0A%0A%20%20%20%20Let's%20put%20numbers%20on%20these%20expressions.%20Consider%20a%20link%20of%20length%20%24%5Cell%20%3D%201%5C%3Am%24%20that%20rotates%20from%20%24%5Ctheta_i%20%3D%200%5Eo%24%20to%20%24%5Ctheta_f%20%3D%2090%5Eo%24%20in%20%241%5C%3As%24%2C%20following%20a%20%5Bminimum-jerk%20trajectory%5D(https%3A%2F%2Fmolab.marimo.io%2Fgithub%2FBMClab%2FBMC%2Fblob%2Fmaster%2Fnotebooks%2Fminimum_jerk_hypothesis.py)%2C%20the%20smooth%20movement%20we%20met%20in%20the%20opening%20section%2C%20here%20applied%20to%20the%20joint%20angle.%0A%0A%20%20%20%20**Before%20you%20run%20the%20next%20cells**%2C%20predict%3A%20at%20the%20middle%20of%20the%20movement%2C%20the%20angular%20velocity%20is%20at%20its%20peak%20and%20the%20angular%20acceleration%20is%20zero.%20What%20is%20the%20acceleration%20of%20the%20endpoint%20at%20that%20instant%3F%0A%0A%20%20%20%20First%2C%20the%20angle%20and%20its%20derivatives%2C%20and%20the%20endpoint%20expressions%20turned%20into%20numerical%20functions%20with%20%60lambdify%60%3A%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(acc%2C%20ell%2C%20np%2C%20r%2C%20sym%2C%20t%2C%20theta%2C%20v)%3A%0A%20%20%20%20theta_i%2C%20theta_f%2C%20duration%20%3D%200%2C%20np.pi%20%2F%202%2C%201%0A%20%20%20%20ts%20%3D%20np.linspace(0%2C%201%2C%20101)%0A%20%20%20%20mjt%20%3D%20theta_i%20%2B%20(theta_f%20-%20theta_i)%20*%20(10%20*%20(t%20%2F%20duration)%20**%203%20-%2015%20*%20(t%20%2F%20duration)%20**%204%20%2B%206%20*%20(t%20%2F%20duration)%20**%205)%0A%0A%20%20%20%20ang%20%3D%20sym.lambdify(t%2C%20mjt%2C%20%22numpy%22)(ts)%0A%20%20%20%20vang%20%3D%20sym.lambdify(t%2C%20mjt.diff(t%2C%201)%2C%20%22numpy%22)(ts)%0A%20%20%20%20aang%20%3D%20sym.lambdify(t%2C%20mjt.diff(t%2C%202)%2C%20%22numpy%22)(ts)%0A%20%20%20%20jang%20%3D%20sym.lambdify(t%2C%20mjt.diff(t%2C%203)%2C%20%22numpy%22)(ts)%0A%0A%20%20%20%20_b%2C%20_c%2C%20_d%2C%20_e%20%3D%20sym.symbols(%22b%20c%20d%20e%22)%0A%20%20%20%20_subs%20%3D%20%7Bell%3A%201%2C%20theta%3A%20_b%2C%20theta.diff(t%2C%201)%3A%20_c%2C%20theta.diff(t%2C%202)%3A%20_d%2C%20theta.diff(t%2C%203)%3A%20_e%7D%0A%0A%20%20%20%20_r%20%3D%20r.subs(_subs)%0A%20%20%20%20rxfu%20%3D%20sym.lambdify(_b%2C%20_r%5B0%5D%2C%20modules%3D%22numpy%22)%0A%20%20%20%20ryfu%20%3D%20sym.lambdify(_b%2C%20_r%5B1%5D%2C%20modules%3D%22numpy%22)%0A%0A%20%20%20%20_v%20%3D%20v.subs(_subs)%0A%20%20%20%20vxfu%20%3D%20sym.lambdify((_b%2C%20_c)%2C%20_v%5B0%5D%2C%20modules%3D%22numpy%22)%0A%20%20%20%20vyfu%20%3D%20sym.lambdify((_b%2C%20_c)%2C%20_v%5B1%5D%2C%20modules%3D%22numpy%22)%0A%0A%20%20%20%20_acc%20%3D%20acc.subs(_subs)%0A%20%20%20%20axfu%20%3D%20sym.lambdify((_b%2C%20_c%2C%20_d)%2C%20_acc%5B0%5D%2C%20modules%3D%22numpy%22)%0A%20%20%20%20ayfu%20%3D%20sym.lambdify((_b%2C%20_c%2C%20_d)%2C%20_acc%5B1%5D%2C%20modules%3D%22numpy%22)%0A%0A%20%20%20%20_jerk%20%3D%20r.diff(t%2C%203).subs(_subs)%0A%20%20%20%20jxfu%20%3D%20sym.lambdify((_b%2C%20_c%2C%20_d%2C%20_e)%2C%20_jerk%5B0%5D%2C%20modules%3D%22numpy%22)%0A%20%20%20%20jyfu%20%3D%20sym.lambdify((_b%2C%20_c%2C%20_d%2C%20_e)%2C%20_jerk%5B1%5D%2C%20modules%3D%22numpy%22)%0A%20%20%20%20return%20(%0A%20%20%20%20%20%20%20%20aang%2C%0A%20%20%20%20%20%20%20%20ang%2C%0A%20%20%20%20%20%20%20%20axfu%2C%0A%20%20%20%20%20%20%20%20ayfu%2C%0A%20%20%20%20%20%20%20%20jang%2C%0A%20%20%20%20%20%20%20%20jxfu%2C%0A%20%20%20%20%20%20%20%20jyfu%2C%0A%20%20%20%20%20%20%20%20rxfu%2C%0A%20%20%20%20%20%20%20%20ryfu%2C%0A%20%20%20%20%20%20%20%20ts%2C%0A%20%20%20%20%20%20%20%20vang%2C%0A%20%20%20%20%20%20%20%20vxfu%2C%0A%20%20%20%20%20%20%20%20vyfu%2C%0A%20%20%20%20)%0A%0A%0A%40app.cell%0Adef%20_(%0A%20%20%20%20aang%2C%0A%20%20%20%20ang%2C%0A%20%20%20%20axfu%2C%0A%20%20%20%20ayfu%2C%0A%20%20%20%20jang%2C%0A%20%20%20%20jxfu%2C%0A%20%20%20%20jyfu%2C%0A%20%20%20%20np%2C%0A%20%20%20%20plt%2C%0A%20%20%20%20rxfu%2C%0A%20%20%20%20ryfu%2C%0A%20%20%20%20ts%2C%0A%20%20%20%20vang%2C%0A%20%20%20%20vxfu%2C%0A%20%20%20%20vyfu%2C%0A)%3A%0A%20%20%20%20_fig%2C%20_hax%20%3D%20plt.subplots(2%2C%204%2C%20sharex%3DTrue%2C%20figsize%3D(14%2C%207)%2C%20layout%3D%22constrained%22)%0A%0A%20%20%20%20_hax%5B0%2C%200%5D.plot(ts%2C%20np.degrees(ang)%2C%20linewidth%3D3)%0A%20%20%20%20_hax%5B0%2C%200%5D.set_title(%22Angular%20displacement%20%5B%24%5Eo%24%5D%22)%0A%20%20%20%20_hax%5B0%2C%200%5D.set_ylabel(%22Joint%22)%0A%20%20%20%20_hax%5B0%2C%201%5D.plot(ts%2C%20np.degrees(vang)%2C%20linewidth%3D3)%0A%20%20%20%20_hax%5B0%2C%201%5D.set_title(%22Angular%20velocity%20%5B%24%5Eo%2Fs%24%5D%22)%0A%20%20%20%20_hax%5B0%2C%202%5D.plot(ts%2C%20np.degrees(aang)%2C%20linewidth%3D3)%0A%20%20%20%20_hax%5B0%2C%202%5D.set_title(%22Angular%20acceleration%20%5B%24%5Eo%2Fs%5E2%24%5D%22)%0A%20%20%20%20_hax%5B0%2C%203%5D.plot(ts%2C%20np.degrees(jang)%2C%20linewidth%3D3)%0A%20%20%20%20_hax%5B0%2C%203%5D.set_title(%22Angular%20jerk%20%5B%24%5Eo%2Fs%5E3%24%5D%22)%0A%0A%20%20%20%20_hax%5B1%2C%200%5D.plot(ts%2C%20rxfu(ang)%2C%20%22r%22%2C%20linewidth%3D3%2C%20label%3D%22x%22)%0A%20%20%20%20_hax%5B1%2C%200%5D.plot(ts%2C%20ryfu(ang)%2C%20%22k%22%2C%20linewidth%3D3%2C%20label%3D%22y%22)%0A%20%20%20%20_hax%5B1%2C%200%5D.set_title(%22Linear%20displacement%20%5B%24m%24%5D%22)%0A%20%20%20%20_hax%5B1%2C%200%5D.legend(loc%3D%22best%22).get_frame().set_alpha(0.8)%0A%20%20%20%20_hax%5B1%2C%200%5D.set_ylabel(%22Endpoint%22)%0A%20%20%20%20_hax%5B1%2C%201%5D.plot(ts%2C%20vxfu(ang%2C%20vang)%2C%20%22r%22%2C%20linewidth%3D3)%0A%20%20%20%20_hax%5B1%2C%201%5D.plot(ts%2C%20vyfu(ang%2C%20vang)%2C%20%22k%22%2C%20linewidth%3D3)%0A%20%20%20%20_hax%5B1%2C%201%5D.set_title(%22Linear%20velocity%20%5B%24m%2Fs%24%5D%22)%0A%20%20%20%20_hax%5B1%2C%202%5D.plot(ts%2C%20axfu(ang%2C%20vang%2C%20aang)%2C%20%22r%22%2C%20linewidth%3D3)%0A%20%20%20%20_hax%5B1%2C%202%5D.plot(ts%2C%20ayfu(ang%2C%20vang%2C%20aang)%2C%20%22k%22%2C%20linewidth%3D3)%0A%20%20%20%20_hax%5B1%2C%202%5D.set_title(%22Linear%20acceleration%20%5B%24m%2Fs%5E2%24%5D%22)%0A%20%20%20%20_hax%5B1%2C%203%5D.plot(ts%2C%20jxfu(ang%2C%20vang%2C%20aang%2C%20jang)%2C%20%22r%22%2C%20linewidth%3D3)%0A%20%20%20%20_hax%5B1%2C%203%5D.plot(ts%2C%20jyfu(ang%2C%20vang%2C%20aang%2C%20jang)%2C%20%22k%22%2C%20linewidth%3D3)%0A%20%20%20%20_hax%5B1%2C%203%5D.set_title(%22Linear%20jerk%20%5B%24m%2Fs%5E3%24%5D%22)%0A%0A%20%20%20%20_fig.suptitle(%22Minimum-jerk%20trajectory%20kinematics%20of%20a%20one-link%20system%22%2C%20fontsize%3D18)%0A%20%20%20%20for%20_i%2C%20_ax%20in%20enumerate(_hax.flat)%3A%0A%20%20%20%20%20%20%20%20_ax.locator_params(nbins%3D5)%0A%20%20%20%20%20%20%20%20_ax.grid(True)%0A%20%20%20%20%20%20%20%20if%20_i%20%3E%203%3A%0A%20%20%20%20%20%20%20%20%20%20%20%20_ax.set_xlabel(%22Time%20%5Bs%5D%22)%0A%20%20%20%20plt.show()%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%20joint%20angle%20follows%20the%20familiar%20S-shaped%20curve%2C%20and%20its%20velocity%20the%20bell-shaped%20one.%20The%20endpoint%2C%20though%2C%20has%20an%20acceleration%20that%20does%20not%20look%20like%20the%20angular%20acceleration%20at%20all.%20Using%20the%20two%20expressions%20we%20derived%2C%20%24a_t%20%3D%20%5Cell%5Cddot%7B%5Ctheta%7D%24%20and%20%24a_c%20%3D%20%5Cell%5Cdot%7B%5Ctheta%7D%5E2%24%2C%20let's%20look%20at%20a%20few%20instants%3A%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(aang%2C%20np%2C%20ts%2C%20vang)%3A%0A%20%20%20%20_ell%20%3D%201.0%20%20%23%20m%0A%20%20%20%20print(%22%20%20%20%20t%20%20%20%20angular%20vel.%20%20%20angular%20acc.%20%20%20%20%20a_t%20%20%20%20%20%20a_c%22)%0A%20%20%20%20for%20_time%20in%20%5B0%2C%200.21%2C%200.5%2C%200.79%2C%201%5D%3A%0A%20%20%20%20%20%20%20%20_i%20%3D%20np.argmin(np.abs(ts%20-%20_time))%0A%20%20%20%20%20%20%20%20print(%0A%20%20%20%20%20%20%20%20%20%20%20%20f%22%7Bts%5B_i%5D%3A5.2f%7D%20s%20%7Bnp.degrees(vang%5B_i%5D)%3A8.1f%7D%20deg%2Fs%20%7Bnp.degrees(aang%5B_i%5D)%3A8.1f%7D%20deg%2Fs2%22%0A%20%20%20%20%20%20%20%20%20%20%20%20f%22%20%7B_ell%20*%20aang%5B_i%5D%3A7.2f%7D%20%7B_ell%20*%20vang%5B_i%5D%20**%202%3A7.2f%7D%20m%2Fs2%22%0A%20%20%20%20%20%20%20%20)%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%20At%20the%20middle%20of%20the%20movement%20the%20angular%20acceleration%20is%20zero%2C%20but%20the%20endpoint%20is%20accelerating%20at%208.67%20m%2Fs%C2%B2%2C%20all%20of%20it%20centripetal%2C%20pointing%20at%20the%20joint.%20That%20is%20nearly%20as%20much%20as%20the%20peak%20tangential%20acceleration%2C%209.07%20m%2Fs%C2%B2%2C%20which%20occurs%20about%200.21%20s%20into%20the%20movement.%20Even%20a%20perfectly%20smooth%20joint%20rotation%20produces%20an%20endpoint%20acceleration%20with%20two%20parts%20that%20peak%20at%20different%20instants%20and%20point%20in%20different%20directions.%0A%0A%20%20%20%20**Challenge%201.**%20Halve%20the%20amplitude%20of%20the%20movement%2C%20to%20%2445%5Eo%24%20in%20the%20same%201%20s.%20Before%20running%20it%2C%20predict%20by%20what%20factor%20the%20peak%20tangential%20acceleration%20changes%2C%20and%20by%20what%20factor%20the%20peak%20centripetal%20acceleration%20changes.%20Then%20check.%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.**%20With%20the%20elbow%20resting%20on%20a%20table%20and%20the%20upper%20arm%20still%2C%20the%20forearm%20and%20hand%20form%20a%20one-link%20system%20about%20the%20elbow%2C%200.35%20m%20long%20from%20the%20elbow%20to%20the%20centre%20of%20the%20hand.%20The%20forearm%20rotates%20at%20a%20constant%20%24180%5Eo%2Fs%24.%20What%20are%20the%20speed%20of%20the%20hand%20and%20its%20tangential%20and%20centripetal%20accelerations%3F%20Which%20way%20does%20the%20acceleration%20point%3F%0A%0A%20%20%20%20%2F%2F%2F%20details%20%7C%20Solution%20to%20Exercise%201%0A%20%20%20%20With%20%24%5Cell%20%3D%200.35%24%20m%20and%20%24%5Cdot%7B%5Ctheta%7D%20%3D%20180%5Eo%2Fs%20%3D%20%5Cpi%24%20rad%2Fs%20(the%20angles%20in%20these%20formulas%20must%20be%20in%20radians)%3A%0A%0A%20%20%20%20%24%24%0A%20%20%20%20%7C%7C%5Coverrightarrow%7B%5Cmathbf%7Bv%7D%7D%7C%7C%20%3D%20%5Cell%7C%5Cdot%7B%5Ctheta%7D%7C%20%3D%200.35%5C%2C%5Cpi%20%3D%201.10%5C%3B%5Cmathrm%7Bm%2Fs%7D%0A%20%20%20%20%24%24%0A%0A%20%20%20%20The%20angular%20velocity%20is%20constant%2C%20so%20%24a_t%20%3D%20%5Cell%5Cddot%7B%5Ctheta%7D%20%3D%200%24%2C%20and%0A%0A%20%20%20%20%24%24%0A%20%20%20%20a_c%20%3D%20%5Cell%5Cdot%7B%5Ctheta%7D%5E2%20%3D%200.35%5C%2C%5Cpi%5E2%20%3D%203.45%5C%3B%5Cmathrm%7Bm%2Fs%5E2%7D%0A%20%20%20%20%24%24%0A%0A%20%20%20%20All%20of%20the%20acceleration%20is%20centripetal%3A%20it%20points%20from%20the%20hand%20to%20the%20elbow%2C%20and%20it%20is%20about%20a%20third%20of%20the%20gravitational%20acceleration%2C%20although%20the%20forearm%20turns%20at%20a%20perfectly%20steady%20rate.%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%20Jacobian%20matrix%0A%0A%20%20%20%20%3E%20The%20**%5BJacobian%20matrix%5D(https%3A%2F%2Fen.wikipedia.org%2Fwiki%2FJacobian_matrix_and_determinant)**%20is%20the%20matrix%20of%20all%20first-order%20partial%20derivatives%20of%20a%20vector-valued%20function%20%24F%24%3A%0A%20%20%20%20%3E%0A%20%20%20%20%3E%20%24%24%0A%20%20%20%20%3E%20F(q_1%2C...q_n)%20%3D%20%5Cbegin%7Bbmatrix%7DF_%7B1%7D(q_1%2C...q_n)%5C%5C%0A%20%20%20%20%3E%20%5Cvdots%5C%5C%0A%20%20%20%20%3E%20F_%7Bm%7D(q_1%2C...q_n)%5C%5C%0A%20%20%20%20%3E%20%5Cend%7Bbmatrix%7D%0A%20%20%20%20%3E%20%24%24%0A%20%20%20%20%3E%0A%20%20%20%20%3E%20In%20general%20form%2C%20the%20Jacobian%20matrix%20of%20the%20function%20%24F%24%20is%3A%0A%20%20%20%20%3E%0A%20%20%20%20%3E%20%24%24%0A%20%20%20%20%3E%20%5Cmathbf%7BJ%7D%3D%0A%20%20%20%20%3E%20%5Cbegin%7Bbmatrix%7D%0A%20%20%20%20%3E%20%5Cdfrac%7B%5Cpartial%20F_%7B1%7D%7D%7B%5Cpartial%20q_%7B1%7D%7D%20%26%20...%20%26%20%5Cdfrac%7B%5Cpartial%20F_%7B1%7D%7D%7B%5Cpartial%20q_%7Bn%7D%7D%20%5C%5C%0A%20%20%20%20%3E%20%5Cvdots%20%20%26%20%5Cddots%20%20%26%20%5Cvdots%20%5C%5C%0A%20%20%20%20%3E%20%5Cdfrac%7B%5Cpartial%20F_%7Bm%7D%7D%7B%5Cpartial%20q_%7B1%7D%7D%20%26%20...%20%26%20%5Cdfrac%7B%5Cpartial%20F_%7Bm%7D%7D%7B%5Cpartial%20q_%7Bn%7D%7D%20%5C%5C%0A%20%20%20%20%3E%20%5Cend%7Bbmatrix%7D%0A%20%20%20%20%3E%20%24%24%0A%0A%20%20%20%20%23%23%23%20Derivative%20of%20a%20vector-valued%20function%20using%20the%20Jacobian%20matrix%0A%0A%20%20%20%20%3E%20The%20time%20derivative%20of%20a%20vector-valued%20function%20%24F%24%20can%20be%20computed%20with%20the%20Jacobian%20matrix%3A%0A%20%20%20%20%3E%0A%20%20%20%20%3E%20%24%24%0A%20%20%20%20%3E%20%5Cfrac%7BdF%7D%7Bdt%7D%20%3D%20%5Cmathbf%7BJ%7D%20%5Ccdot%20%5Cbegin%7Bbmatrix%7D%5Cdfrac%7Bd%20q_1%7D%7Bdt%7D%5C%5C%0A%20%20%20%20%3E%20%5Cvdots%5C%5C%0A%20%20%20%20%3E%20%5Cdfrac%7Bd%20q_n%7D%7Bdt%7D%5C%5C%0A%20%20%20%20%3E%20%5Cend%7Bbmatrix%7D%0A%20%20%20%20%3E%20%24%24%0A%0A%20%20%20%20%23%23%23%20Jacobian%20matrix%20in%20the%20context%20of%20kinematic%20chains%0A%0A%20%20%20%20For%20a%20kinematic%20chain%2C%20the%20Jacobian%20is%20the%20matrix%20of%20all%20first-order%20partial%20derivatives%20of%20the%20linear%20position%20vector%20of%20the%20endpoint%20with%20respect%20to%20the%20vector%20of%20joint%20angles.%20It%20relates%20differential%20changes%20in%20the%20joint%20angles%20to%20the%20resulting%20differential%20changes%20in%20the%20position%20of%20the%20endpoint.%20The%20function%20%24F_%7Bi%7D%24%20is%20the%20expression%20for%20the%20endpoint%20position%20in%20each%20of%20its%20%24m%24%20coordinates%2C%20and%20the%20variable%20%24q_%7Bi%7D%24%20is%20the%20angle%20of%20each%20of%20the%20%24n%24%20joints.%0A%0A%20%20%20%20%23%23%23%23%20Jacobian%20matrix%20of%20a%20one-link%20chain%0A%0A%20%20%20%20For%20the%20planar%20one-link%20system%2C%20the%20Jacobian%20of%20the%20position%20vector%20of%20the%20endpoint%2C%20%24%5Coverrightarrow%7B%5Cmathbf%7Br%7D%7D_P%24%2C%20with%20respect%20to%20the%20joint%20angle%2C%20%24q_1%3D%5Ctheta%24%2C%20is%0A%0A%20%20%20%20%24%24%0A%20%20%20%20%5Cmathbf%7BJ%7D%3D%0A%20%20%20%20%5Cbegin%7Bbmatrix%7D%0A%20%20%20%20%5Cdfrac%7B%5Cpartial%20x_P%7D%7B%5Cpartial%20%5Ctheta%7D%20%5C%5C%5B2ex%5D%0A%20%20%20%20%5Cdfrac%7B%5Cpartial%20y_P%7D%7B%5Cpartial%20%5Ctheta%7D%0A%20%20%20%20%5Cend%7Bbmatrix%7D%0A%20%20%20%20%24%24%0A%0A%20%20%20%20which%20evaluates%20to%3A%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(r%2C%20theta)%3A%0A%20%20%20%20show(r.diff(theta)%2C%20r%22%5Cmathbf%7BJ%7D%20%3D%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%20SymPy%20also%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_(r%2C%20theta)%3A%0A%20%20%20%20J%20%3D%20r.jacobian(%5Btheta%5D)%0A%0A%20%20%20%20show(J%2C%20r%22%5Cmathbf%7BJ%7D%20%3D%22)%0A%20%20%20%20return%20(J%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%20We%20can%20recalculate%20the%20kinematic%20expressions%20with%20the%20Jacobian%2C%20which%20can%20simplify%20the%20derivation.%20The%20linear%20velocity%20of%20the%20endpoint%20is%20the%20product%20of%20the%20Jacobian%20and%20the%20angular%20velocity%2C%0A%0A%20%20%20%20%24%24%0A%20%20%20%20%5Coverrightarrow%7B%5Cmathbf%7Bv%7D%7D%20%3D%20%5Cmathbf%7BJ%7D%20%5Ccdot%20%5Cdot%7B%5Ctheta%7D%0A%20%20%20%20%24%24%0A%0A%20%20%20%20where%3A%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(t%2C%20theta)%3A%0A%20%20%20%20omega%20%3D%20theta.diff(t)%0A%0A%20%20%20%20show(omega%2C%20r%22%5Comega%20%3D%22)%0A%20%20%20%20return%20(omega%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%20angular%20velocity%20is%20also%20a%20vector.%20Its%20direction%20is%20perpendicular%20to%20the%20plane%20of%20rotation%2C%20and%20by%20the%20%5Bright-hand%20rule%5D(https%3A%2F%2Fen.wikipedia.org%2Fwiki%2FRight-hand_rule)%20it%20has%20the%20direction%20of%20the%20versor%20%24%5Chat%7B%5Cmathbf%7Bk%7D%7D%24%2C%20coming%20out%20of%20the%20screen%20(or%20paper)%2C%20when%20%24%5Cdot%7B%5Ctheta%7D%20%3E%200%24.%20Then%3A%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(J%2C%20omega)%3A%0A%20%20%20%20show(J%20*%20omega%2C%20r%22%5Coverrightarrow%7B%5Cmathbf%7Bv%7D%7D%20%3D%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%20And%20the%20linear%20acceleration%20of%20the%20endpoint%20is%20the%20derivative%20of%20this%20product%2C%20with%20the%20product%20rule%3A%0A%0A%20%20%20%20%24%24%0A%20%20%20%20%5Coverrightarrow%7B%5Cmathbf%7Ba%7D%7D%20%3D%20%5Cdot%7B%5Cmathbf%7BJ%7D%7D%20%5Ccdot%20%5Coverrightarrow%7B%5Cmathbf%7B%5Comega%7D%7D%20%2B%20%5Cmathbf%7BJ%7D%20%5Ccdot%20%5Cdot%7B%5Coverrightarrow%7B%5Cmathbf%7B%5Comega%7D%7D%7D%0A%20%20%20%20%24%24%0A%0A%20%20%20%20Let's%20calculate%20this%20derivative%2C%20and%20check%20that%20it%20is%20the%20same%20as%20the%20second%20derivative%20of%20the%20position%20we%20calculated%20before%3A%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(J%2C%20acc%2C%20mo%2C%20omega%2C%20sym%2C%20t)%3A%0A%20%20%20%20accJ%20%3D%20J.diff(t)%20*%20omega%20%2B%20J%20*%20omega.diff(t)%0A%0A%20%20%20%20mo.vstack(%0A%20%20%20%20%20%20%20%20%5B%0A%20%20%20%20%20%20%20%20%20%20%20%20show(accJ%2C%20r%22%5Coverrightarrow%7B%5Cmathbf%7Ba%7D%7D%20%3D%22)%2C%0A%20%20%20%20%20%20%20%20%20%20%20%20mo.md(f%22Same%20as%20the%20direct%20derivative%3A%20%60%7Bsym.simplify(accJ%20-%20acc)%20%3D%3D%20sym.zeros(2%2C%201)%7D%60%22)%2C%0A%20%20%20%20%20%20%20%20%5D%0A%20%20%20%20)%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%20For%20one%20link%2C%20the%20Jacobian%20is%20not%20much%20of%20a%20shortcut.%20Its%20value%20appears%20with%20two%20or%20more%20links%2C%20where%20it%20organizes%20the%20bookkeeping%20of%20many%20joints%20at%20once%2C%20and%20where%20its%20properties%20start%20to%20say%20something%20about%20the%20movement%20itself.%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.**%20Show%20that%2C%20for%20the%20one-link%20system%2C%20the%20Jacobian%20%24%5Cmathbf%7BJ%7D%24%20is%20always%20perpendicular%20to%20the%20position%20vector%20%24%5Coverrightarrow%7B%5Cmathbf%7Br%7D%7D_P%24%20and%20has%20the%20same%20length%20as%20the%20link.%20What%20does%20that%20say%20about%20the%20direction%20and%20the%20magnitude%20of%20the%20velocity%20of%20the%20endpoint%3F%0A%0A%20%20%20%20%2F%2F%2F%20details%20%7C%20Solution%20to%20Exercise%202%0A%20%20%20%20With%20%24%5Cmathbf%7BJ%7D%20%3D%20%5B-%5Cell%5Csin%5Ctheta%2C%5C%3B%20%5Cell%5Ccos%5Ctheta%5D%5ET%24%20and%20%24%5Coverrightarrow%7B%5Cmathbf%7Br%7D%7D_P%20%3D%20%5B%5Cell%5Ccos%5Ctheta%2C%5C%3B%20%5Cell%5Csin%5Ctheta%5D%5ET%24%3A%0A%0A%20%20%20%20%24%24%0A%20%20%20%20%5Cmathbf%7BJ%7D%5Ccdot%5Coverrightarrow%7B%5Cmathbf%7Br%7D%7D_P%20%3D%20-%5Cell%5E2%5Csin%5Ctheta%5Ccos%5Ctheta%20%2B%20%5Cell%5E2%5Ccos%5Ctheta%5Csin%5Ctheta%20%3D%200%0A%20%20%20%20%24%24%0A%0A%20%20%20%20%24%24%0A%20%20%20%20%7C%7C%5Cmathbf%7BJ%7D%7C%7C%20%3D%20%5Csqrt%7B%5Cell%5E2%5Csin%5E2%5Ctheta%20%2B%20%5Cell%5E2%5Ccos%5E2%5Ctheta%7D%20%3D%20%5Cell%0A%20%20%20%20%24%24%0A%0A%20%20%20%20Since%20%24%5Coverrightarrow%7B%5Cmathbf%7Bv%7D%7D%20%3D%20%5Cmathbf%7BJ%7D%5C%2C%5Cdot%7B%5Ctheta%7D%24%2C%20the%20velocity%20is%20perpendicular%20to%20the%20link%2C%20that%20is%2C%20tangent%20to%20the%20circle%20the%20endpoint%20travels%20on%2C%20and%20its%20magnitude%20is%20%24%5Cell%7C%5Cdot%7B%5Ctheta%7D%7C%24%2C%20as%20we%20found%20by%20differentiating.%20The%20Jacobian%20is%20the%20velocity%20of%20the%20endpoint%20for%20an%20angular%20velocity%20of%201%20rad%2Fs.%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%20The%20kinematics%20of%20a%20two-link%20chain%0A%0A%20%20%20%20Now%20let's%20look%20at%20a%20planar%20kinematic%20chain%20with%20two%20links%2C%20as%20shown%20below.%20The%20derivation%20follows%20the%20same%20steps%20as%20for%20one%20link.%0A%0A%20%20%20%20%3Cfigure%20aria-describedby%3D%22figure-caption-4%22%3E%3Ccenter%3E%3Cimg%20src%3D%22https%3A%2F%2Fgithub.com%2FBMClab%2FBMC%2Fblob%2Fmaster%2Fimages%2Ftwolinks.gif%3Fraw%3D1%22%20width%3D400%20alt%3D%22two-link%20chain%22%2F%3E%3C%2Fcenter%3E%3C%2Ffigure%3E%0A%0A%20%20%20%20%3Cspan%20id%3D%22figure-caption-4%22%20class%3D%22figure-caption%22%20style%3D%22display%3Ablock%3Btext-align%3Acenter%3Bfont-style%3Aitalic%22%3EFigure.%20Kinematics%20of%20a%20two-link%20chain%20with%20hinge%20joints%20in%20a%20plane.%3C%2Fspan%3E%0A%0A%20%20%20%20We%20reuse%20the%20coordinate%20system%20and%20the%20time%20symbol%2C%20and%20define%20the%20link%20lengths%20%24%5Cell_1%2C%20%5Cell_2%24%20and%20the%20joint%20angles%20%24%5Ctheta_1%2C%20%5Ctheta_2%24%2C%20functions%20of%20time%3A%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(dynamicsymbols%2C%20sym)%3A%0A%20%20%20%20ell1%2C%20ell2%20%3D%20sym.symbols(%22ell_1%20ell_2%22%2C%20positive%3DTrue)%0A%20%20%20%20theta1%2C%20theta2%20%3D%20dynamicsymbols(%22theta1%20theta2%22)%0A%20%20%20%20return%20ell1%2C%20ell2%2C%20theta1%2C%20theta2%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%20position%20of%20the%20endpoint%20in%20terms%20of%20the%20joint%20angles%20and%20link%20lengths%20is%3A%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(G%2C%20ell1%2C%20ell2%2C%20sym%2C%20theta1%2C%20theta2)%3A%0A%20%20%20%20r2_p%20%3D%20(ell1%20*%20sym.cos(theta1)%20%2B%20ell2%20*%20sym.cos(theta1%20%2B%20theta2))%20*%20G.i%20%2B%20(%0A%20%20%20%20%20%20%20%20ell1%20*%20sym.sin(theta1)%20%2B%20ell2%20*%20sym.sin(theta1%20%2B%20theta2)%0A%20%20%20%20)%20*%20G.j%0A%0A%20%20%20%20show(r2_p%2C%20r%22%5Coverrightarrow%7B%5Cmathbf%7Br%7D%7D_P%20%3D%22)%0A%20%20%20%20return%20(r2_p%2C)%0A%0A%0A%40app.cell%0Adef%20_(G%2C%20mo%2C%20r2_p)%3A%0A%20%20%20%20_components%20%3D%20r2_p.components%0A%0A%20%20%20%20mo.vstack(%5Bshow(_components%5BG.i%5D%2C%20%22x_P%20%3D%22)%2C%20show(_components%5BG.j%5D%2C%20%22y_P%20%3D%22)%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%20And%20in%20matrix%20form%3A%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(G%2C%20r2_p%2C%20sym)%3A%0A%20%20%20%20r2%20%3D%20sym.Matrix((r2_p.dot(G.i)%2C%20r2_p.dot(G.j)))%0A%0A%20%20%20%20show(r2%2C%20r%22%5Coverrightarrow%7B%5Cmathbf%7Br%7D%7D_P%20%3D%22)%0A%20%20%20%20return%20(r2%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%23%20Joint%20and%20segment%20angles%0A%0A%20%20%20%20Note%20that%20%24%5Ctheta_2%24%20is%20a%20joint%20angle%2C%20measured%20in%20what%20is%20called%20the%20**joint%20space**%3A%20it%20is%20the%20angle%20of%20the%20second%20link%20relative%20to%20the%20first.%20The%20angle%20of%20the%20second%20link%20with%20respect%20to%20the%20horizontal%20is%20%24%5Ctheta_1%2B%5Ctheta_2%24%2C%20an%20angle%20in%20the%20**segment%20space**.%20Joint%20and%20segment%20angles%20are%20also%20called%20relative%20and%20absolute%20angles%2C%20respectively.%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%20Inverse%20kinematics%0A%0A%20%20%20%20Using%20the%20%5Blaw%20of%20cosines%5D(https%3A%2F%2Fen.wikipedia.org%2Fwiki%2FLaw_of_cosines)%20in%20the%20triangle%20formed%20by%20the%20two%20links%20and%20the%20line%20from%20the%20base%20to%20the%20endpoint%2C%20the%20angle%20%24%5Ctheta_2%24%20in%20terms%20of%20the%20endpoint%20position%20is%20given%20by%0A%0A%20%20%20%20%24%24%0A%20%20%20%20x_P%5E2%20%2B%20y_P%5E2%20%3D%20%5Cell_1%5E2%2B%5Cell_2%5E2%20-%202%5Cell_1%20%5Cell_2%20%5Ccos(%5Cpi-%5Ctheta_2)%0A%20%20%20%20%24%24%0A%0A%20%20%20%20%24%24%0A%20%20%20%20%5Ctheta_2%20%3D%20%5Carccos%5Cleft(%5Cfrac%7Bx_P%5E2%20%2B%20y_P%5E2%20-%20%5Cell_1%5E2%20-%20%5Cell_2%5E2%7D%7B2%5Cell_1%20%5Cell_2%7D%5Cright)%0A%20%20%20%20%24%24%0A%0A%20%20%20%20To%20find%20the%20angle%20%24%5Ctheta_1%24%2C%20look%20at%20the%20triangle%20in%20red%20in%20the%20figure%20below.%20Its%20angle%20%24%5Cphi%24%20is%0A%0A%20%20%20%20%24%24%0A%20%20%20%20%5Cphi%20%3D%20%5Carctan%5Cleft(%5Cfrac%7B%5Cell_2%20%5Csin(%5Ctheta_2)%7D%7B%5Cell_1%20%2B%20%5Cell_2%20%5Ccos(%5Ctheta_2)%7D%5Cright)%0A%20%20%20%20%24%24%0A%0A%20%20%20%20and%20the%20angle%20of%20its%20hypotenuse%20with%20the%20horizontal%20is%0A%0A%20%20%20%20%24%24%0A%20%20%20%20%5Ctheta_1%20%2B%20%5Cphi%20%3D%20%5Carctan%5Cleft(%5Cfrac%7By_P%7D%7Bx_P%7D%5Cright)%0A%20%20%20%20%24%24%0A%0A%20%20%20%20So%20the%20angle%20%24%5Ctheta_1%24%20is%3A%0A%0A%20%20%20%20%24%24%0A%20%20%20%20%5Ctheta_1%20%3D%20%5Carctan%5Cleft(%5Cfrac%7By_P%7D%7Bx_P%7D%5Cright)%20-%20%5Carctan%5Cleft(%5Cfrac%7B%5Cell_2%20%5Csin(%5Ctheta_2)%7D%7B%5Cell_1%2B%5Cell_2%20%5Ccos(%5Ctheta_2)%7D%5Cright)%0A%20%20%20%20%24%24%0A%0A%20%20%20%20There%20are%20two%20possible%20sets%20of%20angles%20%24(%5Ctheta_1%2C%20%5Ctheta_2)%24%20for%20the%20same%20endpoint%20coordinates%20%24(x_P%2C%20y_P)%24.%20The%20figure%20below%20shows%20in%20orange%20the%20other%20configuration%20of%20the%20chain%20with%20the%20same%20endpoint.%20The%20other%20solution%20is%20%24%5Ctheta_2'%3D2%5Cpi%20-%20%5Ctheta_2%24%20(equivalently%2C%20%24-%5Ctheta_2%24)%3B%20since%20%24%5Csin(%5Ctheta_2')%3D-%5Csin(%5Ctheta_%7B2%7D)%24%2C%20the%20second%20arctangent%20in%20the%20last%20equation%20changes%20sign.%20Even%20a%20simple%20two-link%20chain%20already%20has%20more%20than%20one%20joint%20configuration%20for%20the%20same%20endpoint%20position%2C%20and%20the%20problem%20gets%20much%20worse%20for%20chains%20with%20more%20links%20(more%20degrees%20of%20freedom).%0A%0A%20%20%20%20%3Cfigure%20aria-describedby%3D%22figure-caption-5%22%3E%3Ccenter%3E%3Cimg%20src%3D%22https%3A%2F%2Fgithub.com%2FBMClab%2FBMC%2Fblob%2Fmaster%2Fimages%2Ftwolinks_ik.gif%3Fraw%3D1%22%20width%3D350%20alt%3D%22two%20solutions%20of%20the%20inverse%20kinematics%22%2F%3E%3C%2Fcenter%3E%3C%2Ffigure%3E%0A%0A%20%20%20%20%3Cspan%20id%3D%22figure-caption-5%22%20class%3D%22figure-caption%22%20style%3D%22display%3Ablock%3Btext-align%3Acenter%3Bfont-style%3Aitalic%22%3EFigure.%20Indetermination%20in%20the%20inverse%20kinematics%20of%20a%20two-link%20chain%20with%20hinge%20joints%20in%20a%20plane%3A%20two%20configurations%20reach%20the%20same%20endpoint.%3C%2Fspan%3E%0A%0A%20%20%20%20Let's%20write%20the%20inverse%20kinematics%20as%20a%20function%2C%20together%20with%20the%20forward%20kinematics%20to%20check%20it.%20As%20announced%20in%20the%20one-link%20section%2C%20the%20code%20uses%20%60arctan2%60%20for%20both%20arctangents%2C%20so%20that%20the%20angles%20come%20out%20right%20in%20every%20quadrant%3A%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.function%0Adef%20forward_kinematics(theta1%2C%20theta2%2C%20l1%2C%20l2)%3A%0A%20%20%20%20%22%22%22Endpoint%20position%20(x%2C%20y)%20of%20a%20planar%20two-link%20chain%20with%20joint%20angles%20in%20radians.%22%22%22%0A%20%20%20%20import%20numpy%20as%20np%0A%0A%20%20%20%20x%20%3D%20l1%20*%20np.cos(theta1)%20%2B%20l2%20*%20np.cos(theta1%20%2B%20theta2)%0A%20%20%20%20y%20%3D%20l1%20*%20np.sin(theta1)%20%2B%20l2%20*%20np.sin(theta1%20%2B%20theta2)%0A%20%20%20%20return%20x%2C%20y%0A%0A%0A%40app.function%0Adef%20inverse_kinematics(x%2C%20y%2C%20l1%2C%20l2%2C%20elbow%3D1)%3A%0A%20%20%20%20%22%22%22Joint%20angles%20%5Brad%5D%20of%20a%20planar%20two-link%20chain%20with%20its%20endpoint%20at%20(x%2C%20y).%0A%0A%20%20%20%20%60elbow%3D1%60%20returns%20the%20solution%20with%20theta2%20%3E%3D%200%20and%20%60elbow%3D-1%60%20the%20mirror%0A%20%20%20%20solution%20with%20theta2%20%3C%3D%200.%20Points%20out%20of%20reach%20give%20NaN.%0A%20%20%20%20%22%22%22%0A%20%20%20%20import%20numpy%20as%20np%0A%0A%20%20%20%20cos_theta2%20%3D%20(np.square(x)%20%2B%20np.square(y)%20-%20l1**2%20-%20l2**2)%20%2F%20(2%20*%20l1%20*%20l2)%0A%20%20%20%20with%20np.errstate(invalid%3D%22ignore%22)%3A%0A%20%20%20%20%20%20%20%20theta2%20%3D%20elbow%20*%20np.arccos(cos_theta2)%0A%20%20%20%20theta1%20%3D%20np.arctan2(y%2C%20x)%20-%20np.arctan2(l2%20*%20np.sin(theta2)%2C%20l1%20%2B%20l2%20*%20np.cos(theta2))%0A%20%20%20%20return%20theta1%2C%20theta2%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%20a%20chain%20with%20%24%5Cell_1%20%3D%20%5Cell_2%20%3D%200.5%24%20m%2C%20let's%20find%20both%20solutions%20for%20three%20endpoint%20positions%2C%20the%20first%20two%20of%20which%20will%20be%20the%20start%20and%20the%20end%20of%20the%20reach%20simulated%20later%2C%20and%20check%20each%20of%20them%20with%20the%20forward%20kinematics%3A%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(np)%3A%0A%20%20%20%20_l1%20%3D%20_l2%20%3D%200.5%20%20%23%20m%0A%20%20%20%20for%20_x%2C%20_y%20in%20%5B(-0.5%2C%200.5)%2C%20(0.0%2C%200.5%20*%20np.sqrt(2))%2C%20(0.6%2C%200.3)%5D%3A%0A%20%20%20%20%20%20%20%20print(f%22Endpoint%20(%7B_x%3A.3f%7D%2C%20%7B_y%3A.3f%7D)%20m%3A%22)%0A%20%20%20%20%20%20%20%20for%20_elbow%20in%20%5B1%2C%20-1%5D%3A%0A%20%20%20%20%20%20%20%20%20%20%20%20_th1%2C%20_th2%20%3D%20inverse_kinematics(_x%2C%20_y%2C%20_l1%2C%20_l2%2C%20elbow%3D_elbow)%0A%20%20%20%20%20%20%20%20%20%20%20%20_xf%2C%20_yf%20%3D%20forward_kinematics(_th1%2C%20_th2%2C%20_l1%2C%20_l2)%0A%20%20%20%20%20%20%20%20%20%20%20%20print(%0A%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20f%22%20%20%20theta1%20%3D%20%7Bnp.degrees(_th1)%3A7.2f%7D%2C%20theta2%20%3D%20%7Bnp.degrees(_th2)%3A7.2f%7D%20degrees%22%0A%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20f%22%20%20-%3E%20forward%20kinematics%3A%20(%7Bround(_xf%2C%203)%20%2B%200.0%3A.3f%7D%2C%20%7Bround(_yf%2C%203)%20%2B%200.0%3A.3f%7D)%20m%22%0A%20%20%20%20%20%20%20%20%20%20%20%20)%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%20Every%20pair%20of%20angles%20puts%20the%20endpoint%20back%20where%20it%20should%20be.%20The%20two%20solutions%20for%20each%20point%20are%20mirror%20images%20of%20each%20other%20across%20the%20line%20from%20the%20base%20to%20the%20endpoint%2C%20with%20%24%5Ctheta_2%24%20of%20opposite%20signs.%0A%0A%20%20%20%20**Before%20you%20run%20the%20next%20cell**%2C%20predict%20what%20goes%20wrong%20with%20the%20first%20point%2C%20%24(-0.5%2C%200.5)%24%2C%20if%20%24%5Ctheta_1%24%20is%20computed%20with%20the%20plain%20arctangent%2C%20%24%5Carctan(y_P%2Fx_P)%24%2C%20as%20the%20equations%20above%20are%20written.%20And%20what%20does%20%60inverse_kinematics%60%20return%20for%20the%20point%20%24(1.2%2C%200)%24%3F%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(np)%3A%0A%20%20%20%20_l1%20%3D%20_l2%20%3D%200.5%20%20%23%20m%0A%20%20%20%20_x%2C%20_y%20%3D%20-0.5%2C%200.5%0A%20%20%20%20_th2%20%3D%20np.arccos((_x**2%20%2B%20_y**2%20-%20_l1**2%20-%20_l2**2)%20%2F%20(2%20*%20_l1%20*%20_l2))%0A%20%20%20%20_th1%20%3D%20np.arctan(_y%20%2F%20_x)%20-%20np.arctan(_l2%20*%20np.sin(_th2)%20%2F%20(_l1%20%2B%20_l2%20*%20np.cos(_th2)))%0A%20%20%20%20_xf%2C%20_yf%20%3D%20forward_kinematics(_th1%2C%20_th2%2C%20_l1%2C%20_l2)%0A%0A%20%20%20%20print(f%22arctan(y%2Fx)%20%3D%20%7Bnp.degrees(np.arctan(_y%20%2F%20_x))%3A.1f%7D%20degrees%2C%20arctan2(y%2C%20x)%20%3D%20%7Bnp.degrees(np.arctan2(_y%2C%20_x))%3A.1f%7D%20degrees%22)%0A%20%20%20%20print(f%22With%20arctan%3A%20theta1%20%3D%20%7Bnp.degrees(_th1)%3A.1f%7D%20degrees%2C%20and%20the%20endpoint%20lands%20at%20(%7B_xf%3A.2f%7D%2C%20%7B_yf%3A.2f%7D)%20m%22)%0A%20%20%20%20print(%22%5CnOut%20of%20reach%3A%22%2C%20inverse_kinematics(1.2%2C%200%2C%20_l1%2C%20_l2))%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%20plain%20arctangent%20sees%20%24y%2Fx%20%3D%20-1%24%20and%20answers%20%24-45%5Eo%24%2C%20the%20direction%20of%20the%20opposite%20quadrant%2C%20and%20the%20chain%20ends%20up%20with%20its%20endpoint%20at%20%24(0.5%2C%20-0.5)%24%2C%20the%20mirror%20image%20of%20the%20target%20through%20the%20base.%20And%20a%20point%20farther%20from%20the%20base%20than%20%24%5Cell_1%20%2B%20%5Cell_2%24%20has%20no%20solution%20at%20all%3A%20the%20argument%20of%20the%20arccosine%20is%20larger%20than%201%2C%20and%20the%20function%20returns%20%60NaN%60%20(not%20a%20number).%20The%20**workspace**%20of%20the%20chain%2C%20the%20set%20of%20points%20the%20endpoint%20can%20reach%2C%20is%20the%20ring%20between%20the%20radii%20%24%7C%5Cell_1%20-%20%5Cell_2%7C%24%20and%20%24%5Cell_1%20%2B%20%5Cell_2%24.%0A%0A%20%20%20%20Use%20the%20sliders%20below%20to%20set%20the%20two%20joint%20angles%20of%20the%20chain.%20The%20other%20configuration%20that%20reaches%20the%20same%20endpoint%20is%20drawn%20dashed%2C%20in%20orange.%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(mo)%3A%0A%20%20%20%20theta1_slider%20%3D%20mo.ui.slider(-180%2C%20180%2C%20value%3D40%2C%20step%3D1%2C%20label%3Dr%22%24%5Ctheta_1%24%20%5Bdegrees%5D%22)%0A%20%20%20%20theta2_slider%20%3D%20mo.ui.slider(-180%2C%20180%2C%20value%3D70%2C%20step%3D1%2C%20label%3Dr%22%24%5Ctheta_2%24%20%5Bdegrees%5D%22)%0A%0A%20%20%20%20mo.hstack(%5Btheta1_slider%2C%20theta2_slider%5D%2C%20justify%3D%22start%22)%0A%20%20%20%20return%20theta1_slider%2C%20theta2_slider%0A%0A%0A%40app.cell%0Adef%20_(np%2C%20plt%2C%20theta1_slider%2C%20theta2_slider)%3A%0A%20%20%20%20_l1%20%3D%20_l2%20%3D%200.5%20%20%23%20m%0A%20%20%20%20_th1%2C%20_th2%20%3D%20np.radians(theta1_slider.value)%2C%20np.radians(theta2_slider.value)%0A%20%20%20%20_x%2C%20_y%20%3D%20forward_kinematics(_th1%2C%20_th2%2C%20_l1%2C%20_l2)%0A%20%20%20%20_alt1%2C%20_alt2%20%3D%20inverse_kinematics(_x%2C%20_y%2C%20_l1%2C%20_l2%2C%20elbow%3D-1%20if%20_th2%20%3E%200%20else%201)%0A%0A%20%20%20%20_%2C%20_ax%20%3D%20plt.subplots(figsize%3D(6%2C%206))%0A%20%20%20%20_circle%20%3D%20np.linspace(0%2C%202%20*%20np.pi%2C%20200)%0A%20%20%20%20_ax.plot((_l1%20%2B%20_l2)%20*%20np.cos(_circle)%2C%20(_l1%20%2B%20_l2)%20*%20np.sin(_circle)%2C%20%22%3A%22%2C%20color%3D%220.6%22%2C%20label%3D%22edge%20of%20the%20workspace%22)%0A%20%20%20%20for%20_a1%2C%20_a2%2C%20_style%2C%20_label%20in%20%5B(_alt1%2C%20_alt2%2C%20%22--%22%2C%20%22other%20solution%22)%2C%20(_th1%2C%20_th2%2C%20%22-%22%2C%20%22chain%22)%5D%3A%0A%20%20%20%20%20%20%20%20_elbow_xy%20%3D%20(_l1%20*%20np.cos(_a1)%2C%20_l1%20*%20np.sin(_a1))%0A%20%20%20%20%20%20%20%20_color%20%3D%20%22tab%3Aorange%22%20if%20_style%20%3D%3D%20%22--%22%20else%20%22tab%3Ablue%22%0A%20%20%20%20%20%20%20%20_ax.plot(%5B0%2C%20_elbow_xy%5B0%5D%2C%20_x%5D%2C%20%5B0%2C%20_elbow_xy%5B1%5D%2C%20_y%5D%2C%20_style%2C%20color%3D_color%2C%20linewidth%3D4%2C%20marker%3D%22o%22%2C%20markersize%3D8%2C%20label%3D_label)%0A%20%20%20%20_ax.plot(0%2C%200%2C%20%22ks%22%2C%20markersize%3D12)%0A%20%20%20%20_ax.set_aspect(%22equal%22)%0A%20%20%20%20_ax.set_xlim(-1.1%2C%201.1)%0A%20%20%20%20_ax.set_ylim(-1.1%2C%201.1)%0A%20%20%20%20_ax.set_xlabel(%22x%20%5Bm%5D%22)%0A%20%20%20%20_ax.set_ylabel(%22y%20%5Bm%5D%22)%0A%20%20%20%20_ax.set_title(f%22Endpoint%20at%20(%7B_x%3A.2f%7D%2C%20%7B_y%3A.2f%7D)%20m%3B%20%20det(J)%20%3D%20%7B_l1%20*%20_l2%20*%20np.sin(_th2)%3A.3f%7D%20m%24%5E2%24%22)%0A%20%20%20%20_ax.legend(loc%3D%22lower%20left%22)%0A%20%20%20%20_ax.grid(True)%0A%20%20%20%20plt.tight_layout()%0A%20%20%20%20plt.show()%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(The%20determinant%20of%20the%20Jacobian%2C%20shown%20in%20the%20title%2C%20will%20make%20sense%20in%20a%20few%20sections.)%0A%0A%20%20%20%20**Challenge%202.**%20Find%20joint%20angles%20for%20which%20the%20two%20configurations%20coincide.%20Where%20in%20the%20workspace%20is%20the%20endpoint%20then%3F%20Explain%2C%20with%20the%20law%20of%20cosines%2C%20why%20there%20is%20only%20one%20solution%20there.%0A%0A%20%20%20%20**Exercise%203.**%20For%20the%20chain%20with%20%24%5Cell_1%20%3D%20%5Cell_2%20%3D%200.5%24%20m%2C%20find%20by%20hand%2C%20with%20the%20law%20of%20cosines%20and%20a%20sketch%2C%20the%20two%20pairs%20of%20joint%20angles%20that%20put%20the%20endpoint%20at%20%24(0.5%2C%200.5)%24%20m.%20Then%20check%20your%20answer%20with%20%60inverse_kinematics%60.%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_(mo%2C%20np)%3A%0A%20%20%20%20_rows%20%3D%20%5B%5D%0A%20%20%20%20for%20_elbow%20in%20%5B1%2C%20-1%5D%3A%0A%20%20%20%20%20%20%20%20_th1%2C%20_th2%20%3D%20inverse_kinematics(0.5%2C%200.5%2C%200.5%2C%200.5%2C%20elbow%3D_elbow)%0A%20%20%20%20%20%20%20%20_x%2C%20_y%20%3D%20forward_kinematics(_th1%2C%20_th2%2C%200.5%2C%200.5)%0A%20%20%20%20%20%20%20%20_rows.append(%0A%20%20%20%20%20%20%20%20%20%20%20%20f%22%7C%20%7B_elbow%3A%2Bd%7D%20%7C%20%7Bround(float(np.degrees(_th1))%2C%201)%20%2B%200.0%7D%20%7C%20%7Bround(float(np.degrees(_th2))%2C%201)%20%2B%200.0%7D%22%0A%20%20%20%20%20%20%20%20%20%20%20%20f%22%20%7C%20(%7B_x%3A.3f%7D%2C%20%7B_y%3A.3f%7D)%20%7C%22%0A%20%20%20%20%20%20%20%20)%0A%0A%20%20%20%20mo.accordion(%0A%20%20%20%20%20%20%20%20%7B%0A%20%20%20%20%20%20%20%20%20%20%20%20%22Solution%20to%20Exercise%203%22%3A%20mo.md(%0A%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20r%22%22%22%0A%20%20%20%20The%20endpoint%20is%20at%20a%20distance%20%24%5Csqrt%7B0.5%5E2%20%2B%200.5%5E2%7D%20%3D%20%5Csqrt%7B0.5%7D%24%20m%20from%20the%20base%2C%20so%20%24x_P%5E2%20%2B%20y_P%5E2%20%3D%200.5%24%20and%0A%0A%20%20%20%20%24%24%0A%20%20%20%20%5Ccos%5Ctheta_2%20%3D%20%5Cfrac%7B0.5%20-%200.25%20-%200.25%7D%7B2%20%5Ccdot%200.5%20%5Ccdot%200.5%7D%20%3D%200%20%5Cquad%5CLongrightarrow%5Cquad%20%5Ctheta_2%20%3D%20%5Cpm%2090%5Eo%0A%20%20%20%20%24%24%0A%0A%20%20%20%20The%20line%20from%20the%20base%20to%20the%20endpoint%20is%20at%20%2445%5Eo%24.%20With%20%24%5Ctheta_2%20%3D%2090%5Eo%24%2C%20the%20triangle%20formed%20by%20the%20two%20links%20is%20right-angled%20and%20isosceles%2C%20so%20%24%5Cphi%20%3D%2045%5Eo%24%20and%20%24%5Ctheta_1%20%3D%2045%5Eo%20-%2045%5Eo%20%3D%200%5Eo%24.%20With%20%24%5Ctheta_2%20%3D%20-90%5Eo%24%2C%20%24%5Cphi%20%3D%20-45%5Eo%24%20and%20%24%5Ctheta_1%20%3D%2090%5Eo%24.%20In%20words%3A%20the%20first%20link%20along%20%24x%24%20and%20the%20second%20link%20pointing%20straight%20up%2C%20or%20the%20first%20link%20pointing%20straight%20up%20and%20the%20second%20along%20%24x%24.%0A%0A%20%20%20%20%60inverse_kinematics%60%20agrees%2C%20and%20the%20forward%20kinematics%20puts%20both%20back%20on%20the%20target%3A%0A%0A%20%20%20%20%7C%20elbow%20%7C%20%24%5Ctheta_1%24%20%5Bdegrees%5D%20%7C%20%24%5Ctheta_2%24%20%5Bdegrees%5D%20%7C%20forward%20kinematics%20%5Bm%5D%20%7C%0A%20%20%20%20%7C%3A-----%3A%7C%3A--------------------%3A%7C%3A--------------------%3A%7C%3A----------------------%3A%7C%0A%20%20%20%20%22%22%22%0A%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%2B%20%22%5Cn%22.join(%22%20%20%20%20%22%20%2B%20_row%20for%20_row%20in%20_rows)%0A%20%20%20%20%20%20%20%20%20%20%20%20)%0A%20%20%20%20%20%20%20%20%7D%0A%20%20%20%20)%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%20Example%3A%20an%20arm%20reaching%20across%20a%20table%0A%0A%20%20%20%20Let's%20use%20the%20two-link%20chain%20for%20a%20real%20arm.%20Seen%20from%20above%2C%20with%20the%20shoulder%20at%20the%20origin%2C%20%24x%24%20pointing%20to%20the%20right%20and%20%24y%24%20pointing%20forward%2C%20a%20right%20arm%20reaching%20over%20a%20table%20is%20a%20planar%20two-link%20chain%3A%20the%20upper%20arm%2C%20from%20the%20shoulder%20to%20the%20elbow%2C%20about%20%24%5Cell_1%20%3D%200.30%24%20m%20long%20in%20an%20adult%2C%20and%20the%20forearm%20with%20the%20hand%2C%20from%20the%20elbow%20to%20the%20centre%20of%20the%20hand%2C%20about%20%24%5Cell_2%20%3D%200.33%24%20m.%20The%20hand%20is%20to%20reach%20a%20cup%20at%20%24(0.15%2C%200.40)%24%20m.%0A%0A%20%20%20%20**Before%20you%20run%20the%20next%20cell**%2C%20predict%3A%20the%20inverse%20kinematics%20has%20two%20solutions.%20Can%20your%20right%20arm%20adopt%20both%20of%20them%3F%20Try%20it.%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(np)%3A%0A%20%20%20%20L1_arm%2C%20L2_arm%20%3D%200.30%2C%200.33%20%20%23%20m%2C%20upper%20arm%2C%20and%20forearm%20with%20hand%0A%20%20%20%20cup%20%3D%20(0.15%2C%200.40)%20%20%23%20m%0A%0A%20%20%20%20print(f%22Distance%20from%20the%20shoulder%20to%20the%20cup%3A%20%7Bnp.hypot(*cup)%3A.3f%7D%20m%20(maximum%20reach%20%7BL1_arm%20%2B%20L2_arm%3A.2f%7D%20m)%22)%0A%20%20%20%20for%20_elbow%20in%20%5B1%2C%20-1%5D%3A%0A%20%20%20%20%20%20%20%20_th1%2C%20_th2%20%3D%20inverse_kinematics(*cup%2C%20L1_arm%2C%20L2_arm%2C%20elbow%3D_elbow)%0A%20%20%20%20%20%20%20%20print(%0A%20%20%20%20%20%20%20%20%20%20%20%20f%22theta1%20%3D%20%7Bnp.degrees(_th1)%3A6.1f%7D%2C%20theta2%20%3D%20%7Bnp.degrees(_th2)%3A6.1f%7D%20degrees%2C%22%0A%20%20%20%20%20%20%20%20%20%20%20%20f%22%20elbow%20at%20(%7BL1_arm%20*%20np.cos(_th1)%3A.3f%7D%2C%20%7BL1_arm%20*%20np.sin(_th1)%3A.3f%7D)%20m%22%0A%20%20%20%20%20%20%20%20)%0A%20%20%20%20return%20L1_arm%2C%20L2_arm%2C%20cup%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%20Both%20solutions%20put%20the%20hand%20on%20the%20cup%2C%200.427%20m%20from%20the%20shoulder.%20In%20the%20first%2C%20the%20elbow%20is%20flexed%20by%20%2494.8%5Eo%24%20and%20sits%20out%20to%20the%20right%20of%20the%20shoulder%2C%20at%20%24(0.283%2C%200.098)%24%20m%3A%20the%20posture%20you%20would%20adopt.%20In%20the%20second%2C%20%24%5Ctheta_2%20%3D%20-94.8%5Eo%24%2C%20and%20the%20forearm%20would%20have%20to%20bend%20the%20other%20way%20by%20the%20same%20amount%2C%20with%20the%20elbow%20in%20front%20of%20the%20chest.%0A%0A%20%20%20%20Seen%20from%20above%2C%20flexing%20the%20right%20elbow%20turns%20the%20forearm%20counterclockwise%2C%20so%20%24%5Ctheta_2%20%3E%200%24%20is%20flexion%20and%20%24%5Ctheta_2%20%3C%200%24%20is%20hyperextension%2C%20which%20the%20elbow%20allows%20by%20a%20few%20degrees%20at%20most%20in%20most%20adults.%20The%20mathematics%20has%20two%20answers%3B%20the%20anatomy%20has%20one.%20Joint%20limits%20are%20how%20a%20model%20of%20the%20body%20discards%20the%20solutions%20the%20body%20cannot%20use.%0A%0A%20%20%20%20**Exercise%204.**%20For%20this%20arm%2C%20with%20no%20joint%20limits%2C%20what%20are%20the%20inner%20and%20outer%20radii%20of%20the%20workspace%3F%20Can%20the%20hand%20reach%20the%20points%20%24(0.45%2C%200.45)%24%20m%20and%20%24(0.40%2C%200.40)%24%20m%3F%20For%20the%20point%20it%20can%20reach%2C%20by%20how%20much%20is%20the%20elbow%20flexed%3F%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_(L1_arm%2C%20L2_arm%2C%20mo%2C%20np)%3A%0A%20%20%20%20_inner%2C%20_outer%20%3D%20abs(L1_arm%20-%20L2_arm)%2C%20L1_arm%20%2B%20L2_arm%0A%20%20%20%20_far%2C%20_near%20%3D%20(0.45%2C%200.45)%2C%20(0.40%2C%200.40)%0A%20%20%20%20_th1_far%2C%20_th2_far%20%3D%20inverse_kinematics(*_far%2C%20L1_arm%2C%20L2_arm)%0A%20%20%20%20_th1%2C%20_th2%20%3D%20inverse_kinematics(*_near%2C%20L1_arm%2C%20L2_arm)%0A%0A%20%20%20%20mo.accordion(%0A%20%20%20%20%20%20%20%20%7B%0A%20%20%20%20%20%20%20%20%20%20%20%20%22Solution%20to%20Exercise%204%22%3A%20mo.md(%0A%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20rf%22%22%22%0A%20%20%20%20The%20workspace%20is%20the%20ring%20between%20%24%7C%5Cell_1%20-%20%5Cell_2%7C%20%3D%20%7B_inner%3A.2f%7D%24%20m%20and%20%24%5Cell_1%20%2B%20%5Cell_2%20%3D%20%7B_outer%3A.2f%7D%24%20m%20from%20the%20shoulder.%0A%0A%20%20%20%20-%20The%20point%20%24(0.45%2C%200.45)%24%20m%20is%20%7Bnp.hypot(*_far)%3A.3f%7D%20m%20from%20the%20shoulder%2C%20just%20out%20of%20reach%3B%20%60inverse_kinematics%60%20returns%20%60%7B_th2_far%7D%60%20for%20it.%0A%20%20%20%20-%20The%20point%20%24(0.40%2C%200.40)%24%20m%20is%20%7Bnp.hypot(*_near)%3A.3f%7D%20m%20away%2C%20inside%20the%20ring.%20The%20elbow%20is%20flexed%20by%20%24%5Ctheta_2%20%3D%20%7Bnp.degrees(_th2)%3A.1f%7D%5Eo%24%2C%20with%20%24%5Ctheta_1%20%3D%20%7Bnp.degrees(_th1)%3A.1f%7D%5Eo%24.%0A%0A%20%20%20%20The%20inner%20radius%20is%20tiny%20because%20the%20two%20links%20have%20nearly%20the%20same%20length%3A%20with%20the%20elbow%20folded%20all%20the%20way%2C%20the%20hand%20would%20come%20within%20%7B100%20*%20_inner%3A.0f%7D%20cm%20of%20the%20shoulder.%20The%20real%20elbow%20stops%20long%20before%20that%3B%20the%20example%20on%20the%20workspace%20of%20a%20real%20arm%2C%20later%20in%20this%20notebook%2C%20adds%20the%20joint%20limits.%0A%20%20%20%20%22%22%22%0A%20%20%20%20%20%20%20%20%20%20%20%20)%0A%20%20%20%20%20%20%20%20%7D%0A%20%20%20%20)%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%20Differential%20kinematics%20of%20the%20two-link%20chain%0A%0A%20%20%20%20The%20linear%20velocity%20of%20the%20endpoint%20is%3A%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(r2%2C%20t)%3A%0A%20%20%20%20vel2%20%3D%20r2.diff(t)%0A%0A%20%20%20%20show(vel2%2C%20r%22%5Coverrightarrow%7B%5Cmathbf%7Bv%7D%7D%20%3D%22)%0A%20%20%20%20return%20(vel2%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%20the%20endpoint%20is%20the%20sum%20of%20two%20velocities%3A%20the%20velocity%20of%20the%20endpoint%20relative%20to%20joint%202%2C%20for%20instance%20%24%5Cell_2%5Ccos(%5Ctheta_1%20%2B%20%5Ctheta_2)(%5Cdot%7B%5Ctheta%7D_1%20%2B%20%5Cdot%7B%5Ctheta%7D_2)%24%20in%20the%20%24y%24%20direction%2C%20plus%20the%20velocity%20of%20joint%202%20relative%20to%20joint%201%2C%20for%20instance%20%24%5Cell_1%5Ccos(%5Ctheta_1)%5Cdot%7B%5Ctheta%7D_1%24%2C%20the%20term%20we%20already%20saw%20for%20the%20one-link%20system.%20In%20classical%20mechanics%20this%20is%20known%20as%20%5Brelative%20velocity%5D(https%3A%2F%2Fen.wikipedia.org%2Fwiki%2FRelative_velocity).%20Note%20that%20the%20second%20link%20turns%20at%20%24%5Cdot%7B%5Ctheta%7D_1%20%2B%20%5Cdot%7B%5Ctheta%7D_2%24%2C%20the%20rate%20of%20change%20of%20its%20segment%20angle%2C%20not%20at%20%24%5Cdot%7B%5Ctheta%7D_2%24.%0A%0A%20%20%20%20The%20linear%20acceleration%20of%20the%20endpoint%20is%3A%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(r2%2C%20t)%3A%0A%20%20%20%20acc2%20%3D%20r2.diff(t%2C%202)%0A%0A%20%20%20%20show(acc2%2C%20r%22%5Coverrightarrow%7B%5Cmathbf%7Ba%7D%7D%20%3D%22)%0A%20%20%20%20return%20(acc2%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%20We%20can%20separate%20this%20expression%20into%20three%20types%20of%20terms%3A%20those%20proportional%20to%20%24%5Cddot%7B%5Ctheta%7D%24%20and%20to%20%24%5Cdot%7B%5Ctheta%7D%5E2%24%2C%20as%20for%20the%20one-link%20system%2C%20and%20a%20new%20term%2C%20proportional%20to%20%24%5Cdot%7B%5Ctheta%7D_1%5Cdot%7B%5Ctheta%7D_2%24%3A%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(acc2%2C%20mo%2C%20sym%2C%20t%2C%20theta1%2C%20theta2)%3A%0A%20%20%20%20_acc%20%3D%20acc2.expand()%0A%0A%20%20%20%20_A%2C%20_B%20%3D%20theta1.diff(t%2C%202)%2C%20theta2.diff(t%2C%202)%0A%20%20%20%20tg%20%3D%20_A%20*%20sym.Matrix((_acc%5B0%5D.coeff(_A)%2C%20_acc%5B1%5D.coeff(_A)))%20%2B%20_B%20*%20sym.Matrix((_acc%5B0%5D.coeff(_B)%2C%20_acc%5B1%5D.coeff(_B)))%0A%0A%20%20%20%20_A%2C%20_B%20%3D%20theta1.diff(t)%20**%202%2C%20theta2.diff(t)%20**%202%0A%20%20%20%20ct%20%3D%20_A%20*%20sym.Matrix((_acc%5B0%5D.coeff(_A)%2C%20_acc%5B1%5D.coeff(_A)))%20%2B%20_B%20*%20sym.Matrix((_acc%5B0%5D.coeff(_B)%2C%20_acc%5B1%5D.coeff(_B)))%0A%0A%20%20%20%20_A%20%3D%20theta1.diff(t)%20*%20theta2.diff(t)%0A%20%20%20%20co%20%3D%20_A%20*%20sym.Matrix((_acc%5B0%5D.coeff(_A)%2C%20_acc%5B1%5D.coeff(_A)))%0A%0A%20%20%20%20mo.vstack(%0A%20%20%20%20%20%20%20%20%5B%0A%20%20%20%20%20%20%20%20%20%20%20%20show(tg%2C%20r%22%5Ctext%7BTangential%3A%7D%20%5Cquad%22)%2C%0A%20%20%20%20%20%20%20%20%20%20%20%20show(ct%2C%20r%22%5Ctext%7BCentripetal%3A%7D%20%5Cquad%22)%2C%0A%20%20%20%20%20%20%20%20%20%20%20%20show(co%2C%20r%22%5Ctext%7BCoriolis%3A%7D%20%5Cquad%22)%2C%0A%20%20%20%20%20%20%20%20%20%20%20%20mo.md(f%22The%20three%20terms%20add%20up%20to%20the%20acceleration%3A%20%60%7Bsym.simplify(tg%20%2B%20ct%20%2B%20co%20-%20_acc)%20%3D%3D%20sym.zeros(2%2C%201)%7D%60%22)%2C%0A%20%20%20%20%20%20%20%20%5D%0A%20%20%20%20)%0A%20%20%20%20return%20co%2C%20ct%2C%20tg%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%20new%20term%20is%20the%20%5BCoriolis%20acceleration%5D(https%3A%2F%2Fen.wikipedia.org%2Fwiki%2FCoriolis_effect).%20It%20appears%20only%20when%20both%20joints%20rotate%20at%20the%20same%20time%3A%20the%20endpoint%20is%20then%20moving%2C%20at%20the%20speed%20%24%5Cell_2%5Cdot%7B%5Ctheta%7D_2%24%2C%20relative%20to%20the%20first%20link%2C%20while%20the%20first%20link%20itself%20is%20rotating%2C%20at%20%24%5Cdot%7B%5Ctheta%7D_1%24.%20Its%20magnitude%20is%20%242%5Cell_2%5Cdot%7B%5Ctheta%7D_1%5Cdot%7B%5Ctheta%7D_2%24%2C%20and%20it%20points%20along%20the%20second%20link%2C%20perpendicular%20to%20the%20velocity%20of%20the%20endpoint%20relative%20to%20joint%202%2C%20%24%5Coverrightarrow%7B%5Cmathbf%7Bv%7D%7D%20-%20%5Coverrightarrow%7B%5Cmathbf%7Bv%7D%7D_%7Bjoint2%7D%24%3A%20it%20deflects%20the%20endpoint's%20motion%20sideways%20to%20that%20relative%20velocity.%0A%0A%20%20%20%20**Guiding%20questions%201.**%0A%0A%20%20%20%201.%20Go%20back%20to%20Guiding%20question%200.3.%20With%20the%20elbow%20locked%20(%24%5Cdot%7B%5Ctheta%7D_2%20%3D%20%5Cddot%7B%5Ctheta%7D_2%20%3D%200%24)%20and%20the%20shoulder%20rotating%20at%20a%20constant%20rate%2C%20which%20of%20the%20three%20terms%20survive%3F%20Where%20do%20they%20point%3F%0A%20%20%20%202.%20With%20the%20shoulder%20fixed%20and%20only%20the%20elbow%20moving%2C%20the%20chain%20is%20a%20one-link%20system.%20Check%20that%20the%20three%20terms%20reduce%20to%20the%20ones%20we%20found%20for%20one%20link.%0A%20%20%20%203.%20Why%20is%20the%20Coriolis%20term%20zero%20when%20either%20joint%20is%20still%2C%20however%20fast%20the%20other%20one%20moves%3F%0A%0A%20%20%20%20**Exercise%205.**%20Both%20joints%20of%20the%20chain%20with%20%24%5Cell_1%20%3D%20%5Cell_2%20%3D%200.5%24%20m%20rotate%20at%20constant%20rates%2C%20%24%5Cdot%7B%5Ctheta%7D_1%20%3D%202%24%20rad%2Fs%20and%20%24%5Cdot%7B%5Ctheta%7D_2%20%3D%203%24%20rad%2Fs.%20At%20the%20instant%20when%20%24%5Ctheta_1%20%3D%200%5Eo%24%20and%20%24%5Ctheta_2%20%3D%2090%5Eo%24%2C%20calculate%20the%20tangential%2C%20centripetal%20and%20Coriolis%20terms%20and%20the%20total%20acceleration%20of%20the%20endpoint.%20Then%20explain%20the%20total%20without%20using%20the%20three%20terms.%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_(co%2C%20ct%2C%20ell1%2C%20ell2%2C%20mo%2C%20np%2C%20sym%2C%20t%2C%20tg%2C%20theta1%2C%20theta2)%3A%0A%20%20%20%20_w1%2C%20_w2%2C%20_l1%2C%20_l2%20%3D%202%2C%203%2C%200.5%2C%200.5%0A%20%20%20%20_values%20%3D%20%5B%20%20%23%20higher%20derivatives%20first%2C%20so%20they%20are%20not%20caught%20by%20the%20lower%20ones%0A%20%20%20%20%20%20%20%20%7Btheta1.diff(t%2C%202)%3A%200%2C%20theta2.diff(t%2C%202)%3A%200%7D%2C%0A%20%20%20%20%20%20%20%20%7Btheta1.diff(t)%3A%20_w1%2C%20theta2.diff(t)%3A%20_w2%7D%2C%0A%20%20%20%20%20%20%20%20%7Btheta1%3A%200%2C%20theta2%3A%20sym.pi%20%2F%202%2C%20ell1%3A%20_l1%2C%20ell2%3A%20_l2%7D%2C%0A%20%20%20%20%5D%0A%0A%20%20%20%20def%20_evaluate(term)%3A%0A%20%20%20%20%20%20%20%20%22%22%22Evaluate%20an%20acceleration%20term%20at%20the%20instant%20of%20Exercise%205.%22%22%22%0A%20%20%20%20%20%20%20%20for%20_v%20in%20_values%3A%0A%20%20%20%20%20%20%20%20%20%20%20%20term%20%3D%20term.subs(_v)%0A%20%20%20%20%20%20%20%20return%20np.round(np.array(term%2C%20dtype%3Dfloat).ravel()%2C%206)%20%2B%200.0%0A%0A%20%20%20%20def%20_fmt(vector)%3A%0A%20%20%20%20%20%20%20%20return%20f%22(%7Bvector%5B0%5D%3A.1f%7D%2C%20%7Bvector%5B1%5D%3A.1f%7D)%22%0A%0A%20%20%20%20_tg%2C%20_ct%2C%20_co%20%3D%20_evaluate(tg)%2C%20_evaluate(ct)%2C%20_evaluate(co)%0A%20%20%20%20_total%20%3D%20_tg%20%2B%20_ct%20%2B%20_co%0A%20%20%20%20_text%20%3D%20r%22%22%22%0A%20%20%20%20Nothing%20accelerates%20angularly%2C%20so%20the%20tangential%20term%20is%20%40TG%40%20m%2Fs%C2%B2.%20With%20%24%5Ctheta_1%20%3D%200%5Eo%24%20the%20first%20link%20lies%20along%20%24x%24%2C%20and%20with%20%24%5Ctheta_1%20%2B%20%5Ctheta_2%20%3D%2090%5Eo%24%20the%20second%20link%20points%20along%20%24y%24%2C%20so%20the%20expressions%20above%20give%3A%0A%0A%20%20%20%20-%20centripetal%3A%20%24-%5Cell_1%5Cdot%7B%5Ctheta%7D_1%5E2%5C%2C%5Chat%7B%5Cmathbf%7Bi%7D%7D%20-%20%5Cell_2(%5Cdot%7B%5Ctheta%7D_1%5E2%20%2B%20%5Cdot%7B%5Ctheta%7D_2%5E2)%5C%2C%5Chat%7B%5Cmathbf%7Bj%7D%7D%24%20%3D%20%40CT%40%20m%2Fs%C2%B2%3B%0A%20%20%20%20-%20Coriolis%3A%20%24-2%5Cell_2%5Cdot%7B%5Ctheta%7D_1%5Cdot%7B%5Ctheta%7D_2%5C%2C%5Chat%7B%5Cmathbf%7Bj%7D%7D%24%20%3D%20%40CO%40%20m%2Fs%C2%B2%3B%0A%20%20%20%20-%20total%3A%20%40TOT%40%20m%2Fs%C2%B2%2C%20with%20a%20magnitude%20of%20%40MAG%40%20m%2Fs%C2%B2.%0A%0A%20%20%20%20Without%20the%20three%20terms%3A%20the%20elbow%20goes%20round%20the%20shoulder%20at%202%20rad%2Fs%2C%20with%20a%20centripetal%20acceleration%20%24%5Cell_1%5Cdot%7B%5Ctheta%7D_1%5E2%24%20%3D%20%40A1%40%20m%2Fs%C2%B2%20towards%20the%20shoulder%2C%20and%20the%20second%20link%20turns%20at%20%24%5Cdot%7B%5Ctheta%7D_1%20%2B%20%5Cdot%7B%5Ctheta%7D_2%20%3D%205%24%20rad%2Fs%2C%20which%20gives%20the%20endpoint%20%24%5Cell_2(%5Cdot%7B%5Ctheta%7D_1%20%2B%20%5Cdot%7B%5Ctheta%7D_2)%5E2%24%20%3D%20%40A2%40%20m%2Fs%C2%B2%20towards%20the%20elbow.%20Expanding%20%24(%5Cdot%7B%5Ctheta%7D_1%20%2B%20%5Cdot%7B%5Ctheta%7D_2)%5E2%24%20gives%20%24%5Cdot%7B%5Ctheta%7D_1%5E2%20%2B%20%5Cdot%7B%5Ctheta%7D_2%5E2%24%2C%20which%20this%20notebook%20counts%20as%20centripetal%2C%20plus%20%242%5Cdot%7B%5Ctheta%7D_1%5Cdot%7B%5Ctheta%7D_2%24%2C%20the%20Coriolis%20term.%20The%20split%20into%20three%20terms%20is%20bookkeeping%3B%20the%20total%20is%20not.%0A%20%20%20%20%22%22%22%0A%20%20%20%20for%20_key%2C%20_value%20in%20%7B%0A%20%20%20%20%20%20%20%20%22%40TG%40%22%3A%20_fmt(_tg)%2C%0A%20%20%20%20%20%20%20%20%22%40CT%40%22%3A%20_fmt(_ct)%2C%0A%20%20%20%20%20%20%20%20%22%40CO%40%22%3A%20_fmt(_co)%2C%0A%20%20%20%20%20%20%20%20%22%40TOT%40%22%3A%20_fmt(_total)%2C%0A%20%20%20%20%20%20%20%20%22%40MAG%40%22%3A%20f%22%7Bnp.linalg.norm(_total)%3A.2f%7D%22%2C%0A%20%20%20%20%20%20%20%20%22%40A1%40%22%3A%20f%22%7B_l1%20*%20_w1**2%3A.1f%7D%22%2C%0A%20%20%20%20%20%20%20%20%22%40A2%40%22%3A%20f%22%7B_l2%20*%20(_w1%20%2B%20_w2)%20**%202%3A.1f%7D%22%2C%0A%20%20%20%20%7D.items()%3A%0A%20%20%20%20%20%20%20%20_text%20%3D%20_text.replace(_key%2C%20_value)%0A%0A%20%20%20%20mo.accordion(%7B%22Solution%20to%20Exercise%205%22%3A%20mo.md(_text)%7D)%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%20of%20the%20two-link%20chain%0A%0A%20%20%20%20Let's%20now%20derive%20the%20Jacobian%20of%20the%20planar%20two-link%20chain%3A%0A%0A%20%20%20%20%24%24%0A%20%20%20%20%5Cmathbf%7BJ%7D%20%3D%0A%20%20%20%20%5Cbegin%7Bbmatrix%7D%0A%20%20%20%20%5Cdfrac%7B%5Cpartial%20x_P%7D%7B%5Cpartial%20%5Ctheta_%7B1%7D%7D%20%26%20%5Cdfrac%7B%5Cpartial%20x_P%7D%7B%5Cpartial%20%5Ctheta_%7B2%7D%7D%20%5C%5C%5B2ex%5D%0A%20%20%20%20%5Cdfrac%7B%5Cpartial%20y_P%7D%7B%5Cpartial%20%5Ctheta_%7B1%7D%7D%20%26%20%5Cdfrac%7B%5Cpartial%20y_P%7D%7B%5Cpartial%20%5Ctheta_%7B2%7D%7D%0A%20%20%20%20%5Cend%7Bbmatrix%7D%0A%20%20%20%20%24%24%0A%0A%20%20%20%20We%20could%20build%20it%20by%20hand%3A%0A%0A%20%20%20%20%60%60%60python%0A%20%20%20%20J2%20%3D%20sym.Matrix(%5B%5Br2%5B0%5D.diff(theta1)%2C%20r2%5B0%5D.diff(theta2)%5D%2C%20%5Br2%5B1%5D.diff(theta1)%2C%20r2%5B1%5D.diff(theta2)%5D%5D)%0A%20%20%20%20%60%60%60%0A%0A%20%20%20%20but%20SymPy's%20%60jacobian%60%20method%20is%20shorter%3A%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(r2%2C%20theta1%2C%20theta2)%3A%0A%20%20%20%20J2%20%3D%20r2.jacobian(%5Btheta1%2C%20theta2%5D)%0A%0A%20%20%20%20show(J2%2C%20r%22%5Cmathbf%7BJ%7D%20%3D%22)%0A%20%20%20%20return%20(J2%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%20Each%20column%20of%20the%20Jacobian%20is%20the%20velocity%20the%20endpoint%20would%20have%20if%20only%20one%20joint%20rotated%2C%20at%201%20rad%2Fs.%20Using%20the%20Jacobian%2C%20the%20linear%20velocity%20of%20the%20endpoint%20is%0A%0A%20%20%20%20%24%24%0A%20%20%20%20%5Coverrightarrow%7B%5Cmathbf%7Bv%7D%7D_J%20%3D%20%5Cmathbf%7BJ%7D%20%5Ccdot%20%5Cbegin%7Bbmatrix%7D%5Cdot%7B%5Ctheta%7D_1%20%5C%5C%0A%20%20%20%20%5Cdot%7B%5Ctheta%7D_2%0A%20%20%20%20%5Cend%7Bbmatrix%7D%0A%20%20%20%20%24%24%0A%0A%20%20%20%20where%3A%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(sym%2C%20t%2C%20theta1%2C%20theta2)%3A%0A%20%20%20%20omega2%20%3D%20sym.Matrix((theta1%2C%20theta2)).diff(t)%0A%0A%20%20%20%20show(omega2%2C%20r%22%5Coverrightarrow%7B%5Cmathbf%7B%5Comega%7D%7D%20%3D%22)%0A%20%20%20%20return%20(omega2%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%20Then%3A%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(J2%2C%20omega2)%3A%0A%20%20%20%20vel2J%20%3D%20J2%20*%20omega2%0A%0A%20%20%20%20show(vel2J%2C%20r%22%5Coverrightarrow%7B%5Cmathbf%7Bv%7D%7D_J%20%3D%22)%0A%20%20%20%20return%20(vel2J%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%20expression%20is%20the%20same%20as%20the%20first%20derivative%20of%20the%20endpoint%20position%2C%20which%20we%20can%20check%20by%20comparing%20the%20two%20expressions%20with%20SymPy%3A%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(vel2%2C%20vel2J)%3A%0A%20%20%20%20vel2.expand()%20%3D%3D%20vel2J.expand()%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%20Once%20again%2C%20the%20linear%20acceleration%20of%20the%20endpoint%20is%20the%20derivative%20of%20the%20product%20of%20the%20Jacobian%20and%20the%20angular%20velocity%3A%0A%0A%20%20%20%20%24%24%0A%20%20%20%20%5Coverrightarrow%7B%5Cmathbf%7Ba%7D%7D%20%3D%20%5Cdot%7B%5Cmathbf%7BJ%7D%7D%20%5Ccdot%20%5Coverrightarrow%7B%5Cmathbf%7B%5Comega%7D%7D%20%2B%20%5Cmathbf%7BJ%7D%20%5Ccdot%20%5Cdot%7B%5Coverrightarrow%7B%5Cmathbf%7B%5Comega%7D%7D%7D%0A%20%20%20%20%24%24%0A%0A%20%20%20%20Let's%20calculate%20this%20derivative%3A%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(J2%2C%20omega2%2C%20t)%3A%0A%20%20%20%20acc2J%20%3D%20J2.diff(t)%20*%20omega2%20%2B%20J2%20*%20omega2.diff(t)%0A%0A%20%20%20%20show(acc2J%2C%20r%22%5Coverrightarrow%7B%5Cmathbf%7Ba%7D%7D%20%3D%22)%0A%20%20%20%20return%20(acc2J%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%20once%20again%2C%20it%20is%20the%20same%20as%20the%20second%20derivative%20of%20the%20endpoint%20position%3A%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(acc2%2C%20acc2J)%3A%0A%20%20%20%20acc2.expand()%20%3D%3D%20acc2J.expand()%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%20Example%3A%20what%20each%20joint%20contributes%20to%20the%20velocity%20of%20the%20hand%0A%0A%20%20%20%20Each%20column%20of%20the%20Jacobian%20is%20the%20velocity%20the%20endpoint%20would%20have%20if%20only%20that%20joint%20rotated%2C%20at%201%20rad%2Fs.%20The%20velocity%20of%20the%20endpoint%20is%20the%20sum%20of%20the%20two%20columns%2C%20each%20multiplied%20by%20the%20angular%20velocity%20of%20its%20joint%2C%20and%20since%20the%20columns%20are%20vectors%2C%20the%20two%20contributions%20add%20as%20vectors.%0A%0A%20%20%20%20Take%20the%20chain%20with%20%24%5Cell_1%3D%5Cell_2%3D0.5%24%20m%20at%20%24%5Ctheta_1%20%3D%2045%5Eo%24%20and%20%24%5Ctheta_2%20%3D%2090%5Eo%24%2C%20with%20both%20joints%20rotating%20at%201%20rad%2Fs.%0A%0A%20%20%20%20**Before%20you%20run%20the%20next%20cell**%2C%20predict%3A%20is%20the%20endpoint%20faster%20than%20it%20would%20be%20with%20only%20the%20shoulder%20rotating%3F%20Faster%20than%20the%20sum%20of%20the%20two%20speeds%3F%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(J2%2C%20ell1%2C%20ell2%2C%20np%2C%20plt%2C%20sym%2C%20theta1%2C%20theta2)%3A%0A%20%20%20%20_l1%20%3D%20_l2%20%3D%200.5%20%20%23%20m%0A%20%20%20%20_th1%2C%20_th2%20%3D%20np.radians(45)%2C%20np.radians(90)%0A%20%20%20%20_w%20%3D%20np.array(%5B1.0%2C%201.0%5D)%20%20%23%20rad%2Fs%0A%20%20%20%20_J%20%3D%20np.array(sym.lambdify((theta1%2C%20theta2%2C%20ell1%2C%20ell2)%2C%20J2%2C%20%22numpy%22)(_th1%2C%20_th2%2C%20_l1%2C%20_l2)%2C%20dtype%3Dfloat)%0A%0A%20%20%20%20_shoulder%2C%20_elbow%20%3D%20_J%5B%3A%2C%200%5D%20*%20_w%5B0%5D%2C%20_J%5B%3A%2C%201%5D%20*%20_w%5B1%5D%20%20%23%20contribution%20of%20each%20joint%0A%20%20%20%20_v%20%3D%20_shoulder%20%2B%20_elbow%0A%20%20%20%20_p%20%3D%20np.array(forward_kinematics(_th1%2C%20_th2%2C%20_l1%2C%20_l2))%0A%20%20%20%20_e%20%3D%20np.array(%5B_l1%20*%20np.cos(_th1)%2C%20_l1%20*%20np.sin(_th1)%5D)%0A%20%20%20%20_angle%20%3D%20np.degrees(np.arccos(_shoulder%20%40%20_elbow%20%2F%20(np.linalg.norm(_shoulder)%20*%20np.linalg.norm(_elbow))))%0A%0A%20%20%20%20print(f%22Shoulder%20alone%3A%20%7Bnp.linalg.norm(_shoulder)%3A.3f%7D%20m%2Fs%2C%20elbow%20alone%3A%20%7Bnp.linalg.norm(_elbow)%3A.3f%7D%20m%2Fs%2C%20both%3A%20%7Bnp.linalg.norm(_v)%3A.3f%7D%20m%2Fs%22)%0A%20%20%20%20print(f%22Angle%20between%20the%20two%20contributions%3A%20%7B_angle%3A.1f%7D%20degrees%22)%0A%20%20%20%20print(f%22Distance%20from%20the%20shoulder%20to%20the%20endpoint%3A%20%7Bnp.linalg.norm(_p)%3A.3f%7D%20m%22)%0A%0A%20%20%20%20_%2C%20_ax%20%3D%20plt.subplots(figsize%3D(6.5%2C%205))%0A%20%20%20%20_ax.plot(%5B0%2C%20_e%5B0%5D%2C%20_p%5B0%5D%5D%2C%20%5B0%2C%20_e%5B1%5D%2C%20_p%5B1%5D%5D%2C%20%22-o%22%2C%20color%3D%22tab%3Ablue%22%2C%20linewidth%3D4%2C%20markersize%3D8%2C%20label%3D%22chain%22)%0A%20%20%20%20_ax.plot(%5B0%2C%20_p%5B0%5D%5D%2C%20%5B0%2C%20_p%5B1%5D%5D%2C%20%22%3A%22%2C%20color%3D%220.5%22%2C%20label%3D%22shoulder%20to%20endpoint%22)%0A%20%20%20%20_ax.plot(0%2C%200%2C%20%22ks%22%2C%20markersize%3D12)%0A%20%20%20%20for%20_vec%2C%20_color%2C%20_label%20in%20%5B(_shoulder%2C%20%22tab%3Ared%22%2C%20%22shoulder%20only%22)%2C%20(_elbow%2C%20%22tab%3Agreen%22%2C%20%22elbow%20only%22)%2C%20(_v%2C%20%22k%22%2C%20%22both%20joints%22)%5D%3A%0A%20%20%20%20%20%20%20%20_ax.quiver(*_p%2C%20*_vec%2C%20angles%3D%22xy%22%2C%20scale_units%3D%22xy%22%2C%20scale%3D2%2C%20color%3D_color%2C%20width%3D0.012%2C%20label%3D_label)%0A%20%20%20%20_ax.set_aspect(%22equal%22)%0A%20%20%20%20_ax.set_xlim(-0.75%2C%200.55)%0A%20%20%20%20_ax.set_ylim(-0.1%2C%200.95)%0A%20%20%20%20_ax.set_xlabel(%22x%20%5Bm%5D%22)%0A%20%20%20%20_ax.set_ylabel(%22y%20%5Bm%5D%22)%0A%20%20%20%20_ax.set_title(%22Endpoint%20velocity%2C%201%20rad%2Fs%20at%20each%20joint%5Cn(arrows%20drawn%20at%200.5%20m%20per%20m%2Fs)%22)%0A%20%20%20%20_ax.legend(loc%3D%22upper%20right%22%2C%20fontsize%3D9)%0A%20%20%20%20_ax.grid(True)%0A%20%20%20%20plt.tight_layout()%0A%20%20%20%20plt.show()%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%20Alone%2C%20the%20shoulder%20would%20move%20the%20endpoint%20at%200.707%20m%2Fs%20and%20the%20elbow%20at%200.5%20m%2Fs%3B%20together%20they%20move%20it%20at%201.118%20m%2Fs%2C%20more%20than%20either%20alone%20and%20less%20than%20their%20sum%2C%20because%20the%20two%20contributions%20are%20%2445%5Eo%24%20apart.%20Look%20at%20the%20directions%20of%20the%20arrows.%20The%20contribution%20of%20the%20shoulder%20is%20perpendicular%20to%20the%20dotted%20line%20from%20the%20shoulder%20to%20the%20endpoint%2C%20and%20its%20magnitude%20for%201%20rad%2Fs%2C%200.707%20m%2Fs%2C%20is%20the%20length%20of%20that%20line.%20The%20contribution%20of%20the%20elbow%20is%20perpendicular%20to%20the%20second%20link%2C%20with%20magnitude%20%24%5Cell_2%24.%20Each%20joint%20swings%20everything%20beyond%20it%20on%20a%20circle%20around%20itself%3A%20the%20rule%20%24%5Coverrightarrow%7B%5Cmathbf%7Bv%7D%7D%20%3D%20%5Coverrightarrow%7B%5Cboldsymbol%7B%5Comega%7D%7D%20%5Ctimes%20%5Coverrightarrow%7B%5Cmathbf%7Br%7D%7D%24%20of%20%5BKinematics%20of%20a%20rigid%20body%5D(https%3A%2F%2Fmolab.marimo.io%2Fgithub%2FBMClab%2FBMC%2Fblob%2Fmaster%2Fnotebooks%2Fkinematics_of_rigid_body.py)%2C%20applied%20once%20per%20joint.%0A%0A%20%20%20%20**Exercise%206.**%20Same%20chain%2C%20now%20at%20%24%5Ctheta_1%20%3D%2030%5Eo%24%20and%20%24%5Ctheta_2%20%3D%2060%5Eo%24%2C%20with%20%24%5Cdot%7B%5Ctheta%7D_1%20%3D%201%24%20rad%2Fs%20and%20%24%5Cdot%7B%5Ctheta%7D_2%20%3D%20-1%24%20rad%2Fs.%20Calculate%20the%20velocity%20of%20the%20endpoint%20with%20the%20Jacobian.%20Its%20magnitude%20comes%20out%20as%20a%20round%20number.%20Why%3F%20(Hint%3A%20what%20happens%20to%20the%20segment%20angle%20of%20the%20second%20link%3F)%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell(hide_code%3DTrue)%0Adef%20_(J2%2C%20ell1%2C%20ell2%2C%20mo%2C%20np%2C%20sym%2C%20theta1%2C%20theta2)%3A%0A%20%20%20%20_J%20%3D%20np.array(%0A%20%20%20%20%20%20%20%20sym.lambdify((theta1%2C%20theta2%2C%20ell1%2C%20ell2)%2C%20J2%2C%20%22numpy%22)(np.radians(30)%2C%20np.radians(60)%2C%200.5%2C%200.5)%2C%20dtype%3Dfloat%0A%20%20%20%20)%0A%20%20%20%20_J%20%3D%20np.round(_J%2C%206)%20%2B%200.0%0A%20%20%20%20_v%20%3D%20_J%20%40%20np.array(%5B1.0%2C%20-1.0%5D)%0A%20%20%20%20_text%20%3D%20r%22%22%22%0A%20%20%20%20At%20%24%5Ctheta_1%20%3D%2030%5Eo%24%20and%20%24%5Ctheta_2%20%3D%2060%5Eo%24%20the%20Jacobian%20is%20(in%20metres%20per%20radian)%0A%0A%20%20%20%20%24%24%0A%20%20%20%20%5Cmathbf%7BJ%7D%20%3D%20%5Cbegin%7Bbmatrix%7D%20%40J00%40%20%26%20%40J01%40%20%5C%5C%20%40J10%40%20%26%20%40J11%40%20%5Cend%7Bbmatrix%7D%0A%20%20%20%20%24%24%0A%0A%20%20%20%20and%20the%20velocity%20of%20the%20endpoint%20is%20%24%5Cmathbf%7BJ%7D%5C%2C%5B1%2C%5C%3B%20-1%5D%5ET%24%20%3D%20%40V%40%20m%2Fs%2C%20with%20a%20magnitude%20of%20%40SPEED%40%20m%2Fs%2C%20pointing%20at%20%40DIR%40%C2%B0%20from%20the%20%24x%24%20axis.%0A%0A%20%20%20%20The%20round%20number%20has%20a%20simple%20reason.%20With%20%24%5Cdot%7B%5Ctheta%7D_2%20%3D%20-%5Cdot%7B%5Ctheta%7D_1%24%2C%20the%20segment%20angle%20of%20the%20second%20link%2C%20%24%5Ctheta_1%20%2B%20%5Ctheta_2%24%2C%20does%20not%20change%3A%20the%20second%20link%20translates%20without%20rotating%2C%20and%20all%20its%20points%2C%20the%20endpoint%20included%2C%20move%20with%20the%20elbow.%20The%20elbow%20goes%20round%20the%20shoulder%20at%20%24%5Cell_1%5Cdot%7B%5Ctheta%7D_1%20%3D%200.5%24%20m%2Fs%2C%20perpendicular%20to%20the%20first%20link%2C%20which%20is%20at%20%2430%5Eo%24%3B%20hence%20the%20direction%20%2430%5Eo%20%2B%2090%5Eo%20%3D%20120%5Eo%24.%0A%20%20%20%20%22%22%22%0A%20%20%20%20for%20_key%2C%20_value%20in%20%7B%0A%20%20%20%20%20%20%20%20%22%40J00%40%22%3A%20f%22%7B_J%5B0%2C%200%5D%3A.3f%7D%22%2C%0A%20%20%20%20%20%20%20%20%22%40J01%40%22%3A%20f%22%7B_J%5B0%2C%201%5D%3A.3f%7D%22%2C%0A%20%20%20%20%20%20%20%20%22%40J10%40%22%3A%20f%22%7B_J%5B1%2C%200%5D%3A.3f%7D%22%2C%0A%20%20%20%20%20%20%20%20%22%40J11%40%22%3A%20f%22%7B_J%5B1%2C%201%5D%3A.3f%7D%22%2C%0A%20%20%20%20%20%20%20%20%22%40V%40%22%3A%20f%22(%7B_v%5B0%5D%3A.3f%7D%2C%20%7B_v%5B1%5D%3A.3f%7D)%22%2C%0A%20%20%20%20%20%20%20%20%22%40SPEED%40%22%3A%20f%22%7Bnp.linalg.norm(_v)%3A.3f%7D%22%2C%0A%20%20%20%20%20%20%20%20%22%40DIR%40%22%3A%20f%22%7Bnp.degrees(np.arctan2(_v%5B1%5D%2C%20_v%5B0%5D))%3A.1f%7D%22%2C%0A%20%20%20%20%7D.items()%3A%0A%20%20%20%20%20%20%20%20_text%20%3D%20_text.replace(_key%2C%20_value)%0A%0A%20%20%20%20mo.accordion(%7B%22Solution%20to%20Exercise%206%22%3A%20mo.md(_text)%7D)%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%20When%20the%20Jacobian%20cannot%20be%20inverted%0A%0A%20%20%20%20The%20Jacobian%20turns%20joint%20velocities%20into%20an%20endpoint%20velocity.%20Going%20the%20other%20way%2C%20from%20a%20desired%20endpoint%20velocity%20to%20the%20joint%20velocities%20that%20produce%20it%2C%20means%20solving%20%24%5Cmathbf%7BJ%7D%5C%2C%5Coverrightarrow%7B%5Cmathbf%7B%5Comega%7D%7D%20%3D%20%5Coverrightarrow%7B%5Cmathbf%7Bv%7D%7D%24%20for%20%24%5Coverrightarrow%7B%5Cmathbf%7B%5Comega%7D%7D%24%2C%20which%20is%20possible%20only%20where%20the%20matrix%20can%20be%20inverted%2C%20that%20is%2C%20where%20its%20determinant%20is%20not%20zero%3A%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(J2%2C%20sym)%3A%0A%20%20%20%20show(sym.simplify(J2.det())%2C%20r%22%5Cdet(%5Cmathbf%7BJ%7D)%20%3D%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%20determinant%20depends%20only%20on%20the%20elbow%20angle%2C%20and%20it%20is%20zero%20when%20%24%5Csin%5Ctheta_2%20%3D%200%24%3A%20with%20the%20chain%20fully%20stretched%20(%24%5Ctheta_2%20%3D%200%24)%20or%20fully%20folded%20(%24%5Ctheta_2%20%3D%20%5Cpm%20180%5Eo%24).%20Such%20configurations%20are%20called%20**singular**.%0A%0A%20%20%20%20**Before%20you%20run%20the%20next%20cell**%2C%20think%20of%20your%20own%20arm%20almost%20fully%20stretched.%20Which%20is%20harder%3A%20to%20move%20your%20hand%20a%20little%20further%20away%20from%20your%20shoulder%2C%20or%20sideways%3F%20Then%20let's%20compute%20the%20joint%20velocities%20needed%20to%20move%20the%20endpoint%20at%201%20m%2Fs%2C%20either%20radially%20(away%20from%20the%20base)%20or%20tangentially%20(perpendicular%20to%20that)%2C%20for%20decreasing%20elbow%20angles%3A%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(J2%2C%20ell1%2C%20ell2%2C%20np%2C%20sym%2C%20theta1%2C%20theta2)%3A%0A%20%20%20%20_l1%20%3D%20_l2%20%3D%200.5%20%20%23%20m%0A%20%20%20%20_J%20%3D%20sym.lambdify((theta1%2C%20theta2%2C%20ell1%2C%20ell2)%2C%20J2%2C%20%22numpy%22)%0A%0A%20%20%20%20print(%22Elbow%20%20%20%20%20reach%20%20%20%20%20joint%20velocities%20%5Bdeg%2Fs%5D%20for%201%20m%2Fs%20of%20endpoint%20velocity%22)%0A%20%20%20%20print(%22angle%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20radially%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20tangentially%22)%0A%20%20%20%20for%20_deg%20in%20%5B90%2C%2030%2C%2010%2C%202%5D%3A%0A%20%20%20%20%20%20%20%20_th1%2C%20_th2%20%3D%20np.radians(30)%2C%20np.radians(_deg)%0A%20%20%20%20%20%20%20%20_Jn%20%3D%20np.array(_J(_th1%2C%20_th2%2C%20_l1%2C%20_l2)%2C%20dtype%3Dfloat)%0A%20%20%20%20%20%20%20%20_p%20%3D%20np.array(forward_kinematics(_th1%2C%20_th2%2C%20_l1%2C%20_l2))%0A%20%20%20%20%20%20%20%20_radial%20%3D%20_p%20%2F%20np.linalg.norm(_p)%0A%20%20%20%20%20%20%20%20_tangential%20%3D%20np.array(%5B-_radial%5B1%5D%2C%20_radial%5B0%5D%5D)%0A%20%20%20%20%20%20%20%20_w_radial%20%3D%20np.degrees(np.linalg.solve(_Jn%2C%20_radial)).round(1)%20%2B%200.0%0A%20%20%20%20%20%20%20%20_w_tangential%20%3D%20np.degrees(np.linalg.solve(_Jn%2C%20_tangential)).round(1)%20%2B%200.0%0A%20%20%20%20%20%20%20%20print(%0A%20%20%20%20%20%20%20%20%20%20%20%20f%22%7B_deg%3A4d%7D%20deg%20%20%7B100%20*%20np.linalg.norm(_p)%20%2F%20(_l1%20%2B%20_l2)%3A6.2f%7D%25%22%0A%20%20%20%20%20%20%20%20%20%20%20%20f%22%20%20%20(%7B_w_radial%5B0%5D%3A7.1f%7D%2C%20%7B_w_radial%5B1%5D%3A7.1f%7D)%20%20%20%20%20%20(%7B_w_tangential%5B0%5D%3A5.1f%7D%2C%20%7B_w_tangential%5B1%5D%3A5.1f%7D)%22%0A%20%20%20%20%20%20%20%20)%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%20Moving%20the%20endpoint%20sideways%20always%20costs%20about%20the%20same%3A%20between%2057%20and%2081%20%C2%B0%2Fs%20at%20the%20shoulder%2C%20and%20nothing%20at%20the%20elbow%2C%20because%20rotating%20the%20shoulder%20alone%20swings%20the%20endpoint%20perpendicular%20to%20the%20line%20from%20the%20base.%20Moving%20it%20radially%20costs%20more%20and%20more%20as%20the%20chain%20straightens%3A%20162%20%C2%B0%2Fs%20at%20the%20elbow%20with%20the%20elbow%20at%2090%C2%B0%2C%201315%20%C2%B0%2Fs%20at%2010%C2%B0%2C%20and%206566%20%C2%B0%2Fs%20at%202%C2%B0%2C%20when%20the%20endpoint%20is%20at%2099.98%25%20of%20its%20maximum%20reach.%20At%20%24%5Ctheta_2%20%3D%200%24%20no%20finite%20joint%20velocity%20will%20do%3B%20the%20endpoint%20simply%20cannot%20move%20radially.%20Close%20to%20a%20singular%20configuration%2C%20small%20endpoint%20movements%20in%20some%20directions%20require%20enormous%20joint%20movements%2C%20and%20small%20errors%20in%20the%20endpoint%20position%20become%20large%20errors%20in%20the%20joint%20angles%20calculated%20by%20inverse%20kinematics.%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%20Example%3A%20the%20workspace%20of%20a%20real%20arm%0A%0A%20%20%20%20The%20ring%20of%20Exercise%204%20is%20the%20workspace%20of%20an%20idealized%20chain%20whose%20joints%20turn%20all%20the%20way%20round.%20Real%20joints%20do%20not.%20For%20illustration%2C%20let's%20give%20the%20arm%20of%20the%20cup%20example%20approximate%20joint%20limits%3A%20shoulder%20angles%20between%20%24%5Ctheta_1%20%3D%20-30%5Eo%24%20(the%20upper%20arm%20pointing%20out%20to%20the%20right%20and%20slightly%20backwards)%20and%20%24135%5Eo%24%20(across%20the%20front%20of%20the%20chest)%2C%20and%20elbow%20angles%20between%20%240%5Eo%24%20(straight)%20and%20%24145%5Eo%24%2C%20roughly%20the%20normal%20range%20of%20elbow%20flexion.%0A%0A%20%20%20%20The%20region%20the%20hand%20can%20reach%20can%20be%20mapped%20forwards%2C%20by%20sweeping%20the%20joint%20angles%20through%20their%20ranges%2C%20or%20backwards%2C%20by%20asking%20%60inverse_kinematics%60%20whether%20each%20point%20of%20the%20plane%20has%20a%20solution%20within%20the%20limits.%20The%20code%20below%20does%20it%20backwards%2C%20for%20400%20000%20random%20points%2C%20which%20also%20gives%20the%20area%20of%20the%20region%2C%20and%20tests%20four%20targets%3A%20the%20cup%2C%20a%20point%20on%20the%20left%2C%20a%20point%20behind%20the%20shoulder%2C%20and%20a%20point%20just%20in%20front%20of%20the%20shoulder.%0A%0A%20%20%20%20**Before%20you%20run%20the%20next%20cell**%2C%20predict%3A%20what%20fraction%20of%20the%20ideal%20ring%20can%20the%20real%20arm%20reach%3F%20A%20quarter%3F%20A%20half%3F%20Most%20of%20it%3F%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(L1_arm%2C%20L2_arm%2C%20cup%2C%20np%2C%20plt)%3A%0A%20%20%20%20joint_limits%20%3D%20np.radians(%5B%5B-30%2C%20135%5D%2C%20%5B0%2C%20145%5D%5D)%20%20%23%20shoulder%20and%20elbow%20%5Brad%5D%0A%0A%20%20%20%20def%20_reachable(x%2C%20y)%3A%0A%20%20%20%20%20%20%20%20%22%22%22True%20where%20the%20hand%20reaches%20(x%2C%20y)%20with%20the%20joints%20within%20their%20limits.%22%22%22%0A%20%20%20%20%20%20%20%20_th1%2C%20_th2%20%3D%20inverse_kinematics(x%2C%20y%2C%20L1_arm%2C%20L2_arm%2C%20elbow%3D1)%20%20%23%20elbow%20flexion%3A%20theta2%20%3E%3D%200%0A%20%20%20%20%20%20%20%20_th1%20%3D%20(_th1%20%2B%20np.pi)%20%25%20(2%20*%20np.pi)%20-%20np.pi%20%20%23%20shoulder%20angle%20in%20%5B-180%2C%20180)%20degrees%0A%20%20%20%20%20%20%20%20with%20np.errstate(invalid%3D%22ignore%22)%3A%0A%20%20%20%20%20%20%20%20%20%20%20%20return%20(_th2%20%3C%3D%20joint_limits%5B1%2C%201%5D)%20%26%20(_th1%20%3E%3D%20joint_limits%5B0%2C%200%5D)%20%26%20(_th1%20%3C%3D%20joint_limits%5B0%2C%201%5D)%0A%0A%20%20%20%20_R%20%3D%20L1_arm%20%2B%20L2_arm%0A%20%20%20%20_pts%20%3D%20np.random.default_rng(0).uniform(-_R%2C%20_R%2C%20(400_000%2C%202))%0A%20%20%20%20_ok%20%3D%20_reachable(_pts%5B%3A%2C%200%5D%2C%20_pts%5B%3A%2C%201%5D)%0A%20%20%20%20workspace_area%20%3D%20_ok.mean()%20*%20(2%20*%20_R)%20**%202%0A%20%20%20%20_ring%20%3D%20np.pi%20*%20((L1_arm%20%2B%20L2_arm)%20**%202%20-%20(L1_arm%20-%20L2_arm)%20**%202)%0A%20%20%20%20print(%0A%20%20%20%20%20%20%20%20f%22Area%20reached%20within%20the%20joint%20limits%3A%20%7Bworkspace_area%3A.3f%7D%20m2%2C%22%0A%20%20%20%20%20%20%20%20f%22%20of%20an%20ideal%20ring%20of%20%7B_ring%3A.3f%7D%20m2%20(%7B100%20*%20workspace_area%20%2F%20_ring%3A.0f%7D%25)%22%0A%20%20%20%20)%0A%0A%20%20%20%20_targets%20%3D%20%7B%22cup%22%3A%20cup%2C%20%22left%22%3A%20(-0.30%2C%200.20)%2C%20%22behind%22%3A%20(0.0%2C%20-0.40)%2C%20%22close%22%3A%20(0.10%2C%200.05)%7D%0A%20%20%20%20for%20_name%2C%20(_x%2C%20_y)%20in%20_targets.items()%3A%0A%20%20%20%20%20%20%20%20print(f%22%7B_name%3A7s%7D%20(%7B_x%3A5.2f%7D%2C%20%7B_y%3A5.2f%7D)%20m%3A%20%7B'reachable'%20if%20_reachable(_x%2C%20_y)%20else%20'out%20of%20reach'%7D%22)%0A%0A%20%20%20%20_%2C%20_ax%20%3D%20plt.subplots(figsize%3D(6.5%2C%206))%0A%20%20%20%20_sample%20%3D%20_pts%5B_ok%5D%5B%3A30_000%5D%0A%20%20%20%20_ax.plot(_sample%5B%3A%2C%200%5D%2C%20_sample%5B%3A%2C%201%5D%2C%20%22.%22%2C%20color%3D%22tab%3Ablue%22%2C%20markersize%3D1%2C%20alpha%3D0.3)%0A%20%20%20%20_ax.plot(%5B%5D%2C%20%5B%5D%2C%20%22s%22%2C%20color%3D%22tab%3Ablue%22%2C%20alpha%3D0.4%2C%20label%3D%22reachable%20within%20the%20limits%22)%0A%20%20%20%20_circle%20%3D%20np.linspace(0%2C%202%20*%20np.pi%2C%20200)%0A%20%20%20%20for%20_radius%20in%20%5BL1_arm%20%2B%20L2_arm%2C%20abs(L1_arm%20-%20L2_arm)%5D%3A%0A%20%20%20%20%20%20%20%20_ax.plot(_radius%20*%20np.cos(_circle)%2C%20_radius%20*%20np.sin(_circle)%2C%20%22--%22%2C%20color%3D%220.5%22)%0A%20%20%20%20_ax.plot(%5B%5D%2C%20%5B%5D%2C%20%22--%22%2C%20color%3D%220.5%22%2C%20label%3D%22ideal%20ring%22)%0A%20%20%20%20_ax.plot(0%2C%200%2C%20%22ks%22%2C%20markersize%3D10%2C%20label%3D%22shoulder%22)%0A%20%20%20%20for%20_name%2C%20(_x%2C%20_y)%20in%20_targets.items()%3A%0A%20%20%20%20%20%20%20%20_in%20%3D%20bool(_reachable(_x%2C%20_y))%0A%20%20%20%20%20%20%20%20_ax.plot(_x%2C%20_y%2C%20%22o%22%20if%20_in%20else%20%22X%22%2C%20color%3D%22tab%3Agreen%22%20if%20_in%20else%20%22tab%3Ared%22%2C%20markersize%3D11)%0A%20%20%20%20%20%20%20%20_ax.annotate(_name%2C%20(_x%2C%20_y)%2C%20textcoords%3D%22offset%20points%22%2C%20xytext%3D(8%2C%206)%2C%20fontsize%3D11)%0A%20%20%20%20_ax.set_aspect(%22equal%22)%0A%20%20%20%20_ax.set_xlim(-0.7%2C%200.7)%0A%20%20%20%20_ax.set_ylim(-0.7%2C%200.7)%0A%20%20%20%20_ax.set_xlabel(%22x%20%5Bm%5D%20(to%20the%20right)%22)%0A%20%20%20%20_ax.set_ylabel(%22y%20%5Bm%5D%20(forward)%22)%0A%20%20%20%20_ax.set_title(%22Workspace%20of%20a%20right%20arm%20seen%20from%20above%22)%0A%20%20%20%20_ax.legend(loc%3D%22lower%20left%22%2C%20fontsize%3D9)%0A%20%20%20%20_ax.grid(True)%0A%20%20%20%20plt.tight_layout()%0A%20%20%20%20plt.show()%0A%20%20%20%20return%20joint_limits%2C%20workspace_area%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%20real%20arm%20reaches%200.519%20m%C2%B2%2C%2042%25%20of%20the%20ideal%20ring.%20The%20cup%20and%20the%20point%20on%20the%20left%20are%20within%20reach.%20The%20point%20behind%20the%20shoulder%20would%20need%20a%20shoulder%20angle%20of%20%24-144%5Eo%24%2C%20far%20beyond%20the%20limit%2C%20and%20the%20point%20just%20in%20front%20of%20the%20shoulder%20would%20need%20the%20elbow%20flexed%20by%20%24160%5Eo%24.%20What%20the%20limits%20remove%20is%20the%20region%20behind%20the%20body%20and%20a%20hole%20around%20the%20shoulder%20much%20larger%20than%20the%203%20cm%20of%20Exercise%204%3A%20the%20elbow%20cannot%20fold%20the%20hand%20back%20onto%20the%20shoulder.%0A%0A%20%20%20%20There%20is%20a%20neat%20way%20to%20check%20the%20area%2C%20and%20it%20gives%20the%20determinant%20of%20the%20Jacobian%20a%20second%20meaning.%20The%20forward%20kinematics%20maps%20a%20small%20patch%20of%20joint%20space%2C%20%24d%5Ctheta_1%5C%2Cd%5Ctheta_2%24%2C%20to%20a%20small%20patch%20of%20the%20plane%20with%20area%20%24%7C%5Cdet%5Cmathbf%7BJ%7D%7C%5C%2Cd%5Ctheta_1%5C%2Cd%5Ctheta_2%24%3A%20the%20determinant%20is%20the%20factor%20by%20which%20the%20chain%20stretches%20areas.%20Within%20these%20limits%20each%20point%20is%20reached%20by%20a%20single%20posture%2C%20so%20the%20area%20of%20the%20workspace%20is%20the%20integral%20of%20%24%7C%5Cdet%5Cmathbf%7BJ%7D%7C%20%3D%20%5Cell_1%5Cell_2%7C%5Csin%5Ctheta_2%7C%24%20over%20the%20joint%20ranges%3A%0A%0A%20%20%20%20%24%24%0A%20%20%20%20A%20%3D%20%5Cell_1%5Cell_2%20%5Cint_%7B-30%5Eo%7D%5E%7B135%5Eo%7D%20d%5Ctheta_1%20%5Cint_%7B0%7D%5E%7B145%5Eo%7D%20%5Csin%5Ctheta_2%5C%2C%20d%5Ctheta_2%20%3D%20%5Cell_1%5Cell_2%5C%2C%5CDelta%5Ctheta_1%5C%2C(1%20-%20%5Ccos%20145%5Eo)%0A%20%20%20%20%24%24%0A%0A%20%20%20%20with%20%24%5CDelta%5Ctheta_1%24%20in%20radians%3A%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(L1_arm%2C%20L2_arm%2C%20joint_limits%2C%20np%2C%20workspace_area)%3A%0A%20%20%20%20_formula%20%3D%20L1_arm%20*%20L2_arm%20*%20np.ptp(joint_limits%5B0%5D)%20*%20(np.cos(joint_limits%5B1%2C%200%5D)%20-%20np.cos(joint_limits%5B1%2C%201%5D))%0A%0A%20%20%20%20print(f%22Integral%20of%20%7Cdet%20J%7C%3A%20%7B_formula%3A.4f%7D%20m2%3B%20%20random%20points%3A%20%7Bworkspace_area%3A.4f%7D%20m2%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%20two%20agree%20to%20within%20the%20random%20error%20of%20the%20400%20000%20points.%20The%20formula%20also%20explains%20where%20the%20area%20comes%20from%3A%20elbow%20angles%20near%20%240%5Eo%24%20or%20%24180%5Eo%24%2C%20where%20%24%5Csin%5Ctheta_2%24%20is%20small%2C%20add%20little%20area.%20It%20is%20the%20singularity%20of%20the%20previous%20section%20seen%20from%20another%20side%3A%20where%20the%20chain%20cannot%20move%20the%20endpoint%20in%20every%20direction%2C%20it%20also%20covers%20little%20ground.%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%20Simulation%3A%20reaching%20in%20a%20straight%20line%0A%0A%20%20%20%20Back%20to%20the%20question%20of%20the%20opening%20section.%20Consider%20a%20chain%20with%20%24%5Cell_1%3D%5Cell_2%3D0.5%5C%3Am%24%20whose%20endpoint%20moves%20in%20a%20straight%20line%2C%20from%20%24(-0.5%2C%200.5)%24%20m%20to%20%24(0%2C%200.5%5Csqrt%7B2%7D)%24%20m%20in%201%20s%2C%20following%20a%20%5Bminimum-jerk%20trajectory%5D(https%3A%2F%2Fmolab.marimo.io%2Fgithub%2FBMClab%2FBMC%2Fblob%2Fmaster%2Fnotebooks%2Fminimum_jerk_hypothesis.py).%20We%20found%20the%20joint%20angles%20at%20the%20two%20ends%20of%20this%20path%20above%3A%20the%20chain%20starts%20at%20%24%5Ctheta_1%20%3D%20%5Ctheta_2%20%3D%2090%5Eo%24%20and%20ends%20at%20%24%5Ctheta_1%20%3D%2045%5Eo%24%2C%20%24%5Ctheta_2%20%3D%2090%5Eo%24.%0A%0A%20%20%20%20**Before%20you%20run%20the%20next%20cells**%2C%20predict%3A%20the%20elbow%20angle%20is%20the%20same%20at%20the%20start%20and%20at%20the%20end.%20Does%20it%20stay%20constant%20during%20the%20movement%3F%0A%0A%20%20%20%20First%2C%20the%20minimum-jerk%20trajectory%20of%20the%20endpoint%2C%20as%20functions%20of%20time%3A%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(np%2C%20sym%2C%20t)%3A%0A%20%20%20%20_p0%2C%20_pf%2C%20_d%20%3D%20sym.symbols(%22p0%20pf%20d%22)%0A%0A%20%20%20%20%23%20minimum-jerk%20kinematics%0A%20%20%20%20_mjt%20%3D%20_p0%20%2B%20(_pf%20-%20_p0)%20*%20(10%20*%20(t%20%2F%20_d)%20**%203%20-%2015%20*%20(t%20%2F%20_d)%20**%204%20%2B%206%20*%20(t%20%2F%20_d)%20**%205)%0A%20%20%20%20_rfu%20%3D%20sym.lambdify((t%2C%20_p0%2C%20_pf%2C%20_d)%2C%20_mjt%2C%20%22numpy%22)%0A%20%20%20%20_vfu%20%3D%20sym.lambdify((t%2C%20_p0%2C%20_pf%2C%20_d)%2C%20sym.diff(_mjt%2C%20t%2C%201)%2C%20%22numpy%22)%0A%20%20%20%20_afu%20%3D%20sym.lambdify((t%2C%20_p0%2C%20_pf%2C%20_d)%2C%20sym.diff(_mjt%2C%20t%2C%202)%2C%20%22numpy%22)%0A%20%20%20%20_jfu%20%3D%20sym.lambdify((t%2C%20_p0%2C%20_pf%2C%20_d)%2C%20sym.diff(_mjt%2C%20t%2C%203)%2C%20%22numpy%22)%0A%0A%20%20%20%20%23%20values%0A%20%20%20%20D%2C%20L1%2C%20L2%20%3D%201%2C%200.5%2C%200.5%0A%20%20%20%20p0%2C%20pf%20%3D%20%5B-0.5%2C%200.5%5D%2C%20%5B0%2C%200.5%20*%20np.sqrt(2)%5D%20%20%23%20initial%20and%20final%20endpoint%20positions%0A%20%20%20%20ts2%20%3D%20np.linspace(0%2C%201%2C%20101)%0A%0A%20%20%20%20%23%20endpoint%20kinematics%0A%20%20%20%20x%20%3D%20_rfu(ts2%2C%20p0%5B0%5D%2C%20pf%5B0%5D%2C%20D)%0A%20%20%20%20y%20%3D%20_rfu(ts2%2C%20p0%5B1%5D%2C%20pf%5B1%5D%2C%20D)%0A%20%20%20%20vx%20%3D%20_vfu(ts2%2C%20p0%5B0%5D%2C%20pf%5B0%5D%2C%20D)%0A%20%20%20%20vy%20%3D%20_vfu(ts2%2C%20p0%5B1%5D%2C%20pf%5B1%5D%2C%20D)%0A%20%20%20%20ax%20%3D%20_afu(ts2%2C%20p0%5B0%5D%2C%20pf%5B0%5D%2C%20D)%0A%20%20%20%20ay%20%3D%20_afu(ts2%2C%20p0%5B1%5D%2C%20pf%5B1%5D%2C%20D)%0A%20%20%20%20jx%20%3D%20_jfu(ts2%2C%20p0%5B0%5D%2C%20pf%5B0%5D%2C%20D)%0A%20%20%20%20jy%20%3D%20_jfu(ts2%2C%20p0%5B1%5D%2C%20pf%5B1%5D%2C%20D)%0A%0A%20%20%20%20print(f%22Path%20length%3A%20%7Bnp.hypot(pf%5B0%5D%20-%20p0%5B0%5D%2C%20pf%5B1%5D%20-%20p0%5B1%5D)%3A.3f%7D%20m%2C%20peak%20speed%3A%20%7Bnp.hypot(vx%2C%20vy).max()%3A.2f%7D%20m%2Fs%22)%0A%20%20%20%20return%20L1%2C%20L2%2C%20ax%2C%20ay%2C%20jx%2C%20jy%2C%20ts2%2C%20vx%2C%20vy%2C%20x%2C%20y%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%20Now%20the%20inverse%20kinematics.%20The%20joint%20angles%20are%20written%20in%20terms%20of%20the%20endpoint%20coordinates%20%24r_x(t)%24%20and%20%24r_y(t)%24%2C%20and%20their%20derivatives%20with%20respect%20to%20time%20are%20found%20symbolically%2C%20with%20the%20chain%20rule%20doing%20the%20work.%20Each%20derivative%20is%20then%20evaluated%20at%20every%20instant%20with%20the%20endpoint%20position%2C%20velocity%2C%20acceleration%20and%20jerk%20computed%20above%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%20L1%2C%0A%20%20%20%20L2%2C%0A%20%20%20%20ax%2C%0A%20%20%20%20ay%2C%0A%20%20%20%20dynamicsymbols%2C%0A%20%20%20%20ell1%2C%0A%20%20%20%20ell2%2C%0A%20%20%20%20jx%2C%0A%20%20%20%20jy%2C%0A%20%20%20%20np%2C%0A%20%20%20%20sym%2C%0A%20%20%20%20t%2C%0A%20%20%20%20ts2%2C%0A%20%20%20%20vx%2C%0A%20%20%20%20vy%2C%0A%20%20%20%20x%2C%0A%20%20%20%20y%2C%0A)%3A%0A%20%20%20%20rx%2C%20ry%20%3D%20dynamicsymbols(%22rx%20ry%22%2C%20real%3DTrue)%20%20%23%20or%20sym.Function(%22rx%22)(t)%0A%0A%20%20%20%20%23%20inverse%20kinematics%0A%20%20%20%20theta2_ik%20%3D%20sym.acos((rx**2%20%2B%20ry**2%20-%20ell1**2%20-%20ell2**2)%20%2F%20(2%20*%20ell1%20*%20ell2))%0A%20%20%20%20theta1_ik%20%3D%20sym.atan2(ry%2C%20rx)%20-%20sym.atan2(ell2%20*%20sym.sin(theta2_ik)%2C%20ell1%20%2B%20ell2%20*%20sym.cos(theta2_ik))%0A%0A%20%20%20%20_X%2C%20_Y%2C%20_Xd%2C%20_Yd%2C%20_Xdd%2C%20_Ydd%2C%20_Xddd%2C%20_Yddd%20%3D%20sym.symbols(%22X%20Y%20Xd%20Yd%20Xdd%20Ydd%20Xddd%20Yddd%22)%0A%20%20%20%20_subs%20%3D%20%7B%0A%20%20%20%20%20%20%20%20rx%3A%20_X%2C%20ry%3A%20_Y%2C%0A%20%20%20%20%20%20%20%20rx.diff(t%2C%201)%3A%20_Xd%2C%20ry.diff(t%2C%201)%3A%20_Yd%2C%0A%20%20%20%20%20%20%20%20rx.diff(t%2C%202)%3A%20_Xdd%2C%20ry.diff(t%2C%202)%3A%20_Ydd%2C%0A%20%20%20%20%20%20%20%20rx.diff(t%2C%203)%3A%20_Xddd%2C%20ry.diff(t%2C%203)%3A%20_Yddd%2C%0A%20%20%20%20%7D%0A%20%20%20%20_args%20%3D%20(_X%2C%20_Y%2C%20_Xd%2C%20_Yd%2C%20_Xdd%2C%20_Ydd%2C%20_Xddd%2C%20_Yddd%2C%20ell1%2C%20ell2)%0A%20%20%20%20_vals%20%3D%20(x%2C%20y%2C%20vx%2C%20vy%2C%20ax%2C%20ay%2C%20jx%2C%20jy%2C%20L1%2C%20L2)%0A%0A%20%20%20%20def%20_evaluate(expr)%3A%0A%20%20%20%20%20%20%20%20%22%22%22Evaluate%20a%20symbolic%20expression%20of%20the%20endpoint%20kinematics%20at%20every%20instant.%22%22%22%0A%20%20%20%20%20%20%20%20_f%20%3D%20sym.lambdify(_args%2C%20expr.subs(_subs)%2C%20%22numpy%22)%0A%20%20%20%20%20%20%20%20return%20np.broadcast_to(_f(*_vals)%2C%20ts2.shape).astype(float)%0A%0A%20%20%20%20ang1%2C%20ang2%20%3D%20_evaluate(theta1_ik)%2C%20_evaluate(theta2_ik)%0A%20%20%20%20vang1%2C%20vang2%20%3D%20_evaluate(theta1_ik.diff(t%2C%201))%2C%20_evaluate(theta2_ik.diff(t%2C%201))%0A%20%20%20%20aang1%2C%20aang2%20%3D%20_evaluate(theta1_ik.diff(t%2C%202))%2C%20_evaluate(theta2_ik.diff(t%2C%202))%0A%20%20%20%20jang1%2C%20jang2%20%3D%20_evaluate(theta1_ik.diff(t%2C%203))%2C%20_evaluate(theta2_ik.diff(t%2C%203))%0A%20%20%20%20return%20aang1%2C%20aang2%2C%20ang1%2C%20ang2%2C%20jang1%2C%20jang2%2C%20vang1%2C%20vang2%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%20plots%20of%20the%20trajectories%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%20aang1%2C%0A%20%20%20%20aang2%2C%0A%20%20%20%20ang1%2C%0A%20%20%20%20ang2%2C%0A%20%20%20%20ax%2C%0A%20%20%20%20ay%2C%0A%20%20%20%20jang1%2C%0A%20%20%20%20jang2%2C%0A%20%20%20%20jx%2C%0A%20%20%20%20jy%2C%0A%20%20%20%20np%2C%0A%20%20%20%20plt%2C%0A%20%20%20%20ts2%2C%0A%20%20%20%20vang1%2C%0A%20%20%20%20vang2%2C%0A%20%20%20%20vx%2C%0A%20%20%20%20vy%2C%0A%20%20%20%20x%2C%0A%20%20%20%20y%2C%0A)%3A%0A%20%20%20%20_fig%2C%20_hax%20%3D%20plt.subplots(2%2C%204%2C%20sharex%3DTrue%2C%20figsize%3D(14%2C%207)%2C%20layout%3D%22constrained%22)%0A%0A%20%20%20%20_hax%5B0%2C%200%5D.plot(ts2%2C%20x%2C%20%22r%22%2C%20linewidth%3D3%2C%20label%3D%22x%22)%0A%20%20%20%20_hax%5B0%2C%200%5D.plot(ts2%2C%20y%2C%20%22k%22%2C%20linewidth%3D3%2C%20label%3D%22y%22)%0A%20%20%20%20_hax%5B0%2C%200%5D.set_title(%22Linear%20displacement%20%5B%24m%24%5D%22)%0A%20%20%20%20_hax%5B0%2C%200%5D.legend(loc%3D%22best%22).get_frame().set_alpha(0.8)%0A%20%20%20%20_hax%5B0%2C%200%5D.set_ylabel(%22Endpoint%22)%0A%20%20%20%20_hax%5B0%2C%201%5D.plot(ts2%2C%20vx%2C%20%22r%22%2C%20linewidth%3D3)%0A%20%20%20%20_hax%5B0%2C%201%5D.plot(ts2%2C%20vy%2C%20%22k%22%2C%20linewidth%3D3)%0A%20%20%20%20_hax%5B0%2C%201%5D.set_title(%22Linear%20velocity%20%5B%24m%2Fs%24%5D%22)%0A%20%20%20%20_hax%5B0%2C%202%5D.plot(ts2%2C%20ax%2C%20%22r%22%2C%20linewidth%3D3)%0A%20%20%20%20_hax%5B0%2C%202%5D.plot(ts2%2C%20ay%2C%20%22k%22%2C%20linewidth%3D3)%0A%20%20%20%20_hax%5B0%2C%202%5D.set_title(%22Linear%20acceleration%20%5B%24m%2Fs%5E2%24%5D%22)%0A%20%20%20%20_hax%5B0%2C%203%5D.plot(ts2%2C%20jx%2C%20%22r%22%2C%20linewidth%3D3)%0A%20%20%20%20_hax%5B0%2C%203%5D.plot(ts2%2C%20jy%2C%20%22k%22%2C%20linewidth%3D3)%0A%20%20%20%20_hax%5B0%2C%203%5D.set_title(%22Linear%20jerk%20%5B%24m%2Fs%5E3%24%5D%22)%0A%0A%20%20%20%20_hax%5B1%2C%200%5D.plot(ts2%2C%20np.degrees(ang1)%2C%20%22b%22%2C%20linewidth%3D3%2C%20label%3D%22joint%201%22)%0A%20%20%20%20_hax%5B1%2C%200%5D.plot(ts2%2C%20np.degrees(ang2)%2C%20%22g%22%2C%20linewidth%3D3%2C%20label%3D%22joint%202%22)%0A%20%20%20%20_hax%5B1%2C%200%5D.set_title(%22Angular%20displacement%20%5B%24%5Eo%24%5D%22)%0A%20%20%20%20_hax%5B1%2C%200%5D.legend(loc%3D%22best%22).get_frame().set_alpha(0.8)%0A%20%20%20%20_hax%5B1%2C%200%5D.set_ylabel(%22Joint%22)%0A%20%20%20%20_hax%5B1%2C%201%5D.plot(ts2%2C%20np.degrees(vang1)%2C%20%22b%22%2C%20linewidth%3D3)%0A%20%20%20%20_hax%5B1%2C%201%5D.plot(ts2%2C%20np.degrees(vang2)%2C%20%22g%22%2C%20linewidth%3D3)%0A%20%20%20%20_hax%5B1%2C%201%5D.set_title(%22Angular%20velocity%20%5B%24%5Eo%2Fs%24%5D%22)%0A%20%20%20%20_hax%5B1%2C%202%5D.plot(ts2%2C%20np.degrees(aang1)%2C%20%22b%22%2C%20linewidth%3D3)%0A%20%20%20%20_hax%5B1%2C%202%5D.plot(ts2%2C%20np.degrees(aang2)%2C%20%22g%22%2C%20linewidth%3D3)%0A%20%20%20%20_hax%5B1%2C%202%5D.set_title(%22Angular%20acceleration%20%5B%24%5Eo%2Fs%5E2%24%5D%22)%0A%20%20%20%20_hax%5B1%2C%203%5D.plot(ts2%2C%20np.degrees(jang1)%2C%20%22b%22%2C%20linewidth%3D3)%0A%20%20%20%20_hax%5B1%2C%203%5D.plot(ts2%2C%20np.degrees(jang2)%2C%20%22g%22%2C%20linewidth%3D3)%0A%20%20%20%20_hax%5B1%2C%203%5D.set_title(%22Angular%20jerk%20%5B%24%5Eo%2Fs%5E3%24%5D%22)%0A%0A%20%20%20%20_fig.suptitle(%22Minimum-jerk%20trajectory%20kinematics%20of%20a%20two-link%20chain%22%2C%20fontsize%3D18)%0A%20%20%20%20for%20_i%2C%20_ax%20in%20enumerate(_hax.flat)%3A%0A%20%20%20%20%20%20%20%20_ax.locator_params(nbins%3D5)%0A%20%20%20%20%20%20%20%20_ax.grid(True)%0A%20%20%20%20%20%20%20%20if%20_i%20%3E%203%3A%0A%20%20%20%20%20%20%20%20%20%20%20%20_ax.set_xlabel(%22Time%20%5B%24s%24%5D%22)%0A%20%20%20%20plt.show()%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(ang1%2C%20ang2%2C%20np%2C%20plt%2C%20x%2C%20y)%3A%0A%20%20%20%20_fig%2C%20(_ax1%2C%20_ax2)%20%3D%20plt.subplots(1%2C%202%2C%20figsize%3D(14%2C%204.5)%2C%20layout%3D%22constrained%22)%0A%0A%20%20%20%20_ax1.plot(x%2C%20y%2C%20%22r%22%2C%20linewidth%3D3)%0A%20%20%20%20_ax1.set_xlabel(%22Displacement%20in%20x%20%5B%24m%24%5D%22)%0A%20%20%20%20_ax1.set_ylabel(%22Displacement%20in%20y%20%5B%24m%24%5D%22)%0A%20%20%20%20_ax1.set_title(%22Endpoint%20space%22)%0A%20%20%20%20_ax1.axis(%22equal%22)%0A%20%20%20%20_ax1.grid(True)%0A%0A%20%20%20%20_ax2.plot(np.degrees(ang1)%2C%20np.degrees(ang2)%2C%20%22b%22%2C%20linewidth%3D3)%0A%20%20%20%20_ax2.set_xlabel(%22Displacement%20in%20joint%201%20%5B%24%5Eo%24%5D%22)%0A%20%20%20%20_ax2.set_ylabel(%22Displacement%20in%20joint%202%20%5B%24%5Eo%24%5D%22)%0A%20%20%20%20_ax2.set_title(%22Joint%20space%22)%0A%20%20%20%20_ax2.axis(%22equal%22)%0A%20%20%20%20_ax2.grid(True)%0A%20%20%20%20plt.show()%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(ang1%2C%20ang2%2C%20np%2C%20ts2%2C%20vang1%2C%20vang2)%3A%0A%20%20%20%20print(f%22Joint%201%3A%20from%20%7Bnp.degrees(ang1%5B0%5D)%3A.1f%7D%20to%20%7Bnp.degrees(ang1%5B-1%5D)%3A.1f%7D%20degrees%2C%20peak%20velocity%20%7Bnp.degrees(np.abs(vang1).max())%3A.1f%7D%20deg%2Fs%22)%0A%20%20%20%20print(%0A%20%20%20%20%20%20%20%20f%22Joint%202%3A%20from%20%7Bnp.degrees(ang2%5B0%5D)%3A.1f%7D%20to%20%7Bnp.degrees(ang2%5B-1%5D)%3A.1f%7D%20degrees%2C%22%0A%20%20%20%20%20%20%20%20f%22%20maximum%20%7Bnp.degrees(ang2.max())%3A.1f%7D%20degrees%20at%20t%20%3D%20%7Bts2%5Bang2.argmax()%5D%3A.2f%7D%20s%2C%22%0A%20%20%20%20%20%20%20%20f%22%20peak%20velocity%20%7Bnp.degrees(np.abs(vang2).max())%3A.1f%7D%20deg%2Fs%22%0A%20%20%20%20)%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%20hand%20moves%20in%20a%20straight%20line%20with%20a%20single%2C%20symmetric%2C%20bell-shaped%20speed%20profile.%20The%20joints%20do%20not.%20The%20shoulder%20rotates%20by%20%2445%5Eo%24%20in%20one%20direction%2C%20with%20a%20peak%20velocity%20of%20about%2093%20%C2%B0%2Fs.%20The%20elbow%20starts%20and%20ends%20at%20%2490%5Eo%24%2C%20yet%20it%20does%20not%20stay%20there%3A%20it%20flexes%20by%20more%20than%208%C2%B0%20and%20then%20extends%20back%2C%20with%20a%20peak%20velocity%20of%20about%2030%20%C2%B0%2Fs.%20In%20joint%20space%2C%20the%20straight%20line%20of%20the%20hand%20becomes%20a%20curve.%20Simple%20at%20the%20hand%2C%20complicated%20at%20the%20joints%3B%20this%20is%20the%20observation%20that%20led%20Morasso%20(1981)%20and%20Flash%20and%20Hogan%20(1985)%20to%20propose%20that%20the%20nervous%20system%20plans%20reaching%20movements%20in%20terms%20of%20the%20hand%2C%20not%20the%20joints.%0A%0A%20%20%20%20The%20joint%20velocities%20above%20came%20from%20differentiating%20the%20inverse%20kinematics%20symbolically.%20The%20Jacobian%20offers%20an%20independent%20check%3A%20at%20every%20instant%2C%20the%20joint%20velocities%20must%20solve%20%24%5Cmathbf%7BJ%7D%5C%2C%5Coverrightarrow%7B%5Cmathbf%7B%5Comega%7D%7D%20%3D%20%5Coverrightarrow%7B%5Cmathbf%7Bv%7D%7D%24%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%20J2%2C%0A%20%20%20%20L1%2C%0A%20%20%20%20L2%2C%0A%20%20%20%20ang1%2C%0A%20%20%20%20ang2%2C%0A%20%20%20%20ell1%2C%0A%20%20%20%20ell2%2C%0A%20%20%20%20np%2C%0A%20%20%20%20sym%2C%0A%20%20%20%20theta1%2C%0A%20%20%20%20theta2%2C%0A%20%20%20%20vang1%2C%0A%20%20%20%20vang2%2C%0A%20%20%20%20vx%2C%0A%20%20%20%20vy%2C%0A)%3A%0A%20%20%20%20_J%20%3D%20sym.lambdify((theta1%2C%20theta2%2C%20ell1%2C%20ell2)%2C%20J2%2C%20%22numpy%22)%0A%20%20%20%20_omega%20%3D%20np.array(%0A%20%20%20%20%20%20%20%20%5Bnp.linalg.solve(np.array(_J(_a1%2C%20_a2%2C%20L1%2C%20L2)%2C%20dtype%3Dfloat)%2C%20%5B_vx%2C%20_vy%5D)%20for%20_a1%2C%20_a2%2C%20_vx%2C%20_vy%20in%20zip(ang1%2C%20ang2%2C%20vx%2C%20vy)%5D%0A%20%20%20%20)%0A%0A%20%20%20%20print(f%22Largest%20difference%3A%20%7Bnp.degrees(np.abs(_omega%20-%20np.column_stack((vang1%2C%20vang2))).max())%3A.1e%7D%20deg%2Fs%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%20two%20methods%20agree%20to%20rounding%20error.%20The%20Jacobian%20route%20is%20also%20much%20cheaper%3A%20one%20small%20linear%20system%20per%20instant%2C%20instead%20of%20differentiating%20the%20inverse%20kinematics%20expressions.%0A%0A%20%20%20%20**Challenge%203.**%20Repeat%20the%20simulation%20with%20the%20other%20solution%20of%20the%20inverse%20kinematics%2C%20the%20one%20with%20%24%5Ctheta_2%20%3C%200%24.%20Plot%20the%20joint%20angles%20and%20joint%20velocities%2C%20and%20compare%20them%20with%20the%20solution%20above.%20Does%20the%20hand%20path%20change%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%23%20Each%20type%20of%20acceleration%20of%20the%20endpoint%20in%20the%20simulated%20reach%0A%0A%20%20%20%20Let's%20evaluate%20the%20tangential%2C%20centripetal%20and%20Coriolis%20terms%20with%20the%20joint%20angles%20and%20their%20derivatives%20from%20the%20simulation%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%20L1%2C%0A%20%20%20%20L2%2C%0A%20%20%20%20aang1%2C%0A%20%20%20%20aang2%2C%0A%20%20%20%20ang1%2C%0A%20%20%20%20ang2%2C%0A%20%20%20%20co%2C%0A%20%20%20%20ct%2C%0A%20%20%20%20ell1%2C%0A%20%20%20%20ell2%2C%0A%20%20%20%20np%2C%0A%20%20%20%20sym%2C%0A%20%20%20%20t%2C%0A%20%20%20%20tg%2C%0A%20%20%20%20theta1%2C%0A%20%20%20%20theta2%2C%0A%20%20%20%20ts2%2C%0A%20%20%20%20vang1%2C%0A%20%20%20%20vang2%2C%0A)%3A%0A%20%20%20%20_A1%2C%20_A2%2C%20_A1d%2C%20_A2d%2C%20_A1dd%2C%20_A2dd%20%3D%20sym.symbols(%22A1%20A2%20A1d%20A2d%20A1dd%20A2dd%22)%0A%20%20%20%20_subs%20%3D%20%7B%0A%20%20%20%20%20%20%20%20theta1%3A%20_A1%2C%20theta2%3A%20_A2%2C%0A%20%20%20%20%20%20%20%20theta1.diff(t%2C%201)%3A%20_A1d%2C%20theta2.diff(t%2C%201)%3A%20_A2d%2C%0A%20%20%20%20%20%20%20%20theta1.diff(t%2C%202)%3A%20_A1dd%2C%20theta2.diff(t%2C%202)%3A%20_A2dd%2C%0A%20%20%20%20%20%20%20%20ell1%3A%20L1%2C%20ell2%3A%20L2%2C%0A%20%20%20%20%7D%0A%0A%20%20%20%20def%20_evaluate(term)%3A%0A%20%20%20%20%20%20%20%20%22%22%22Evaluate%20the%20x%20and%20y%20components%20of%20an%20acceleration%20term%20at%20every%20instant.%22%22%22%0A%20%20%20%20%20%20%20%20_comps%20%3D%20%5Bsym.lambdify((_A1%2C%20_A2%2C%20_A1d%2C%20_A2d%2C%20_A1dd%2C%20_A2dd)%2C%20_c.subs(_subs)%2C%20%22numpy%22)%20for%20_c%20in%20term%5D%0A%20%20%20%20%20%20%20%20return%20np.column_stack(%5Bnp.broadcast_to(_f(ang1%2C%20ang2%2C%20vang1%2C%20vang2%2C%20aang1%2C%20aang2)%2C%20ts2.shape)%20for%20_f%20in%20_comps%5D)%0A%0A%20%20%20%20tg2n%20%3D%20_evaluate(tg)%20%20%23%20tangential%20acceleration%0A%20%20%20%20ct2n%20%3D%20_evaluate(ct)%20%20%23%20centripetal%20acceleration%0A%20%20%20%20co2n%20%3D%20_evaluate(co)%20%20%23%20Coriolis%20acceleration%0A%20%20%20%20acc_tot%20%3D%20tg2n%20%2B%20ct2n%20%2B%20co2n%20%20%23%20total%20acceleration%0A%20%20%20%20return%20acc_tot%2C%20co2n%2C%20ct2n%2C%20tg2n%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%20total%20has%20to%20be%20the%20acceleration%20of%20the%20minimum-jerk%20trajectory%20we%20started%20from.%20Let's%20check%20it%2C%20and%20compare%20the%20sizes%20of%20the%20three%20terms%3A%0A%20%20%20%20%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(acc_tot%2C%20ax%2C%20ay%2C%20co2n%2C%20ct2n%2C%20np%2C%20tg2n%2C%20ts2)%3A%0A%20%20%20%20print(f%22Largest%20difference%20between%20the%20sum%20of%20the%20terms%20and%20the%20minimum-jerk%20acceleration%3A%20%7Bnp.abs(acc_tot%20-%20np.column_stack((ax%2C%20ay))).max()%3A.1e%7D%20m%2Fs2%5Cn%22)%0A%20%20%20%20for%20_name%2C%20_term%20in%20%5B(%22Tangential%22%2C%20tg2n)%2C%20(%22Centripetal%22%2C%20ct2n)%2C%20(%22Coriolis%22%2C%20co2n)%2C%20(%22Total%22%2C%20acc_tot)%5D%3A%0A%20%20%20%20%20%20%20%20_mag%20%3D%20np.linalg.norm(_term%2C%20axis%3D1)%0A%20%20%20%20%20%20%20%20print(f%22%7B_name%3A12s%7D%20peak%20magnitude%20%7B_mag.max()%3A.2f%7D%20m%2Fs2%20at%20t%20%3D%20%7Bts2%5B_mag.argmax()%5D%3A.2f%7D%20s%22)%0A%20%20%20%20_i%20%3D%20np.linalg.norm(tg2n%2C%20axis%3D1).argmax()%0A%20%20%20%20print(f%22%5CnAt%20t%20%3D%20%7Bts2%5B_i%5D%3A.2f%7D%20s%3A%20tangential%20%7Bnp.linalg.norm(tg2n%5B_i%5D)%3A.2f%7D%20m%2Fs2%2C%20total%20%7Bnp.linalg.norm(acc_tot%5B_i%5D)%3A.2f%7D%20m%2Fs2%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(acc_tot%2C%20co2n%2C%20ct2n%2C%20plt%2C%20tg2n%2C%20ts2)%3A%0A%20%20%20%20_fig%2C%20_hax%20%3D%20plt.subplots(1%2C%203%2C%20sharex%3DTrue%2C%20sharey%3DTrue%2C%20figsize%3D(12%2C%205)%2C%20layout%3D%22constrained%22)%0A%20%20%20%20for%20_ax%2C%20_term%2C%20_title%20in%20zip(_hax%2C%20%5Btg2n%2C%20ct2n%2C%20co2n%5D%2C%20%5B%22Tangential%22%2C%20%22Centripetal%22%2C%20%22Coriolis%22%5D)%3A%0A%20%20%20%20%20%20%20%20_ax.plot(ts2%2C%20acc_tot%5B%3A%2C%200%5D%2C%20color%3D(1%2C%200%2C%200%2C%200.3)%2C%20linewidth%3D5%2C%20label%3D%22x%20total%22)%0A%20%20%20%20%20%20%20%20_ax.plot(ts2%2C%20acc_tot%5B%3A%2C%201%5D%2C%20color%3D(0%2C%200%2C%200%2C%200.3)%2C%20linewidth%3D5%2C%20label%3D%22y%20total%22)%0A%20%20%20%20%20%20%20%20_ax.plot(ts2%2C%20_term%5B%3A%2C%200%5D%2C%20%22r%22%2C%20linewidth%3D2%2C%20label%3D%22x%22)%0A%20%20%20%20%20%20%20%20_ax.plot(ts2%2C%20_term%5B%3A%2C%201%5D%2C%20%22k%22%2C%20linewidth%3D2%2C%20label%3D%22y%22)%0A%20%20%20%20%20%20%20%20_ax.set_title(_title)%0A%20%20%20%20%20%20%20%20_ax.set_xlabel(%22Time%20%5B%24s%24%5D%22)%0A%20%20%20%20%20%20%20%20_ax.locator_params(nbins%3D5)%0A%20%20%20%20%20%20%20%20_ax.grid(True)%0A%20%20%20%20_hax%5B0%5D.set_ylabel(%22Endpoint%20acceleration%20%5B%24m%2Fs%5E2%24%5D%22)%0A%20%20%20%20_hax%5B1%5D.legend(loc%3D%22best%22).get_frame().set_alpha(0.8)%0A%20%20%20%20_fig.suptitle(%22Acceleration%20terms%20for%20the%20minimum-jerk%20trajectory%20of%20a%20two-link%20chain%22%2C%20fontsize%3D16)%0A%20%20%20%20plt.show()%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%20three%20terms%20add%20up%20to%20the%20minimum-jerk%20acceleration%20to%20within%20%2410%5E%7B-15%7D%5C%3B%5Cmathrm%7Bm%2Fs%5E2%7D%24.%20The%20tangential%20term%2C%20the%20one%20produced%20by%20the%20angular%20accelerations%20of%20the%20joints%2C%20is%20the%20largest%2C%20but%20it%20is%20not%20the%20whole%20story.%20The%20centripetal%20term%20reaches%20more%20than%20half%20the%20peak%20of%20the%20total%2C%20and%20the%20Coriolis%20term%20almost%20a%20quarter.%20And%20at%20%24t%20%3D%200.77%24%20s%20the%20tangential%20term%20alone%20is%20larger%20than%20the%20total%20acceleration%3A%20the%20velocity-dependent%20terms%20partly%20cancel%20it.%20To%20produce%20a%20simple%20hand%20acceleration%2C%20the%20joints%20must%20produce%20angular%20accelerations%20that%20compensate%20for%20the%20centripetal%20and%20Coriolis%20accelerations%20the%20movement%20itself%20generates.%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%20Checkpoint%20questions%0A%0A%20%20%20%20Pause%20here%20before%20the%20problems.%0A%0A%20%20%20%201.%20Go%20back%20to%20your%20answers%20to%20Challenge%200.%20Did%20your%20shoulder%20and%20elbow%20each%20rotate%20in%20one%20direction%20only%2C%20at%20a%20constant%20rate%3F%20Compare%20with%20the%20simulated%20reach.%0A%20%20%20%202.%20A%20person%20stands%20on%20one%20leg.%20Is%20the%20supporting%20leg%20an%20open%20or%20a%20closed%20kinematic%20chain%20in%20mechanics%3F%20And%20in%20the%20clinical%20sense%3F%0A%20%20%20%203.%20Why%20can%20the%20endpoint%20of%20a%20chain%20accelerate%20when%20no%20joint%20has%20an%20angular%20acceleration%3F%0A%20%20%20%204.%20Why%20does%20the%20Coriolis%20term%20need%20both%20joints%20to%20rotate%2C%20and%20what%20is%20it%20proportional%20to%3F%0A%20%20%20%205.%20Near%20full%20extension%2C%20why%20is%20it%20hard%20to%20move%20the%20hand%20radially%2C%20away%20from%20the%20shoulder%3F%20What%20does%20the%20determinant%20of%20the%20Jacobian%20say%20about%20it%3F%0A%20%20%20%206.%20A%20planar%20arm%20with%20a%20shoulder%2C%20an%20elbow%20and%20a%20wrist%20moves%20a%20fingertip%20in%20a%20plane.%20What%20is%20its%20mobility%2C%20and%20its%20redundancy%3F%20What%20does%20that%20mean%20for%20the%20inverse%20kinematics%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%20Problems%0A%0A%20%20%20%201.%20Calculate%20the%20Jacobian%20matrix%20of%20the%20following%20function%3A%0A%0A%20%20%20%20%20%20%20%20%24%24%0A%20%20%20%20%20%20%20%20f(x%2C%20y)%20%3D%20%5Cbegin%7Bbmatrix%7D%0A%20%20%20%20%20%20%20%20x%5E2%20y%20%5C%5C%0A%20%20%20%20%20%20%20%205%20x%20%2B%20%5Csin%20y%20%5Cend%7Bbmatrix%7D%0A%20%20%20%20%20%20%20%20%24%24%0A%0A%20%20%20%202.%20For%20the%20two-link%20chain%2C%20calculate%20and%20interpret%20the%20Jacobian%20and%20the%20expressions%20for%20the%20position%2C%20velocity%20and%20acceleration%20of%20the%20endpoint%20in%20the%20following%20cases%3A%3Cbr%3E%0A%20%20%20%20%20%20%20a)%20the%20first%20joint%20(the%20joint%20at%20the%20base)%20is%20fixed%20at%20%240%5Eo%24%3B%3Cbr%3E%0A%20%20%20%20%20%20%20b)%20the%20second%20joint%20is%20fixed%20at%20%240%5Eo%24.%0A%0A%20%20%20%203.%20Derive%20the%20equations%20for%20the%20kinematics%20of%20a%20two-link%20pendulum%20with%20the%20angles%20measured%20relative%20to%20the%20vertical.%0A%0A%20%20%20%204.%20Derive%20the%20equations%20for%20the%20kinematics%20of%20a%20two-link%20system%20using%20segment%20angles%2C%20and%20compare%20them%20with%20the%20derivation%20using%20joint%20angles.%0A%0A%20%20%20%205.%20For%20the%20two-link%20chain%2C%20a%20special%20case%20of%20movement%20occurs%20when%20the%20endpoint%20moves%20along%20a%20line%20passing%20through%20the%20first%20joint%20(the%20joint%20at%20the%20base).%20A%20system%20with%20this%20behaviour%20is%20known%20as%20a%20polar%20manipulator%20(Mussa-Ivaldi%2C%201986).%20For%20simplicity%2C%20consider%20that%20the%20two%20links%20have%20the%20same%20length%2C%20%24%5Cell%24.%20In%20this%20case%2C%20the%20two%20joint%20angles%20are%20related%20by%20%242%5Ctheta_1%2B%5Ctheta_2%3D%5Cpi%24.%3Cbr%3E%0A%20%20%20%20%20%20%20a)%20Calculate%20the%20Jacobian%20of%20this%20polar%20manipulator%20and%20compare%20it%20with%20the%20Jacobian%20of%20the%20standard%20two-link%20chain.%20Note%20the%20difference%20between%20the%20off-diagonal%20terms.%3Cbr%3E%0A%20%20%20%20%20%20%20b)%20Calculate%20the%20expressions%20for%20the%20endpoint%20position%2C%20velocity%20and%20acceleration.%3Cbr%3E%0A%20%20%20%20%20%20%20c)%20For%20the%20endpoint%20acceleration%20of%20the%20polar%20manipulator%2C%20identify%20the%20tangential%2C%20centripetal%20and%20Coriolis%20components%2C%20and%20compare%20them%20with%20the%20expressions%20for%20the%20standard%20two-link%20chain.%0A%0A%20%20%20%206.%20%5BEx.%202.22%2C%20Rade's%20book%5D%20In%20an%20internal%20combustion%20engine%2C%20crank%20%24AB%24%2C%20shown%20in%20the%20figure%20below%2C%20rotates%20at%20a%20constant%201000%20rpm%2C%20counterclockwise.%20For%20the%20position%20%24%5Ctheta%3D30%5Eo%24%2C%20determine%3A%20a)%20the%20angular%20velocity%20of%20the%20connecting%20rod%20%24BD%24%3B%20b)%20the%20speed%20of%20the%20piston%20%24P%24.%0A%20%20%20%20%20%20%20%3Cfigure%3E%3Cimg%20src%3D%22https%3A%2F%2Fgithub.com%2FBMClab%2FBMC%2Fblob%2Fmaster%2Fimages%2Fmotor_2rods.png%3Fraw%3D1%22%20width%3D450%20alt%3D%22crank%2C%20connecting%20rod%20and%20piston%22%2F%3E%3C%2Ffigure%3E%0A%0A%20%20%20%207.%20%5BEx.%202.30%2C%20Rade's%20book%5D%20The%20robot%20shown%20in%20the%20figure%20below%20moves%20in%20a%20plane%2C%20with%20velocity%20%24%5Coverrightarrow%7Bv%7D_0%24%20and%20acceleration%20%24%5Coverrightarrow%7Ba%7D_0%24%20relative%20to%20the%20floor.%20Arm%20%24O_1O_2%24%20rotates%20with%20angular%20velocity%20%24%5Comega_1%3D%5Cdot%7B%5Ctheta%7D_1%24%20and%20angular%20acceleration%20%24%5Calpha_1%3D%5Cddot%7B%5Ctheta%7D_1%24%20relative%20to%20the%20robot's%20body%2C%20and%20arm%20%24O_2P%24%20rotates%20with%20angular%20velocity%20%24%5Comega_2%3D%5Cdot%7B%5Ctheta%7D_2%24%20and%20angular%20acceleration%20%24%5Calpha_2%3D%5Cddot%7B%5Ctheta%7D_2%24%20relative%20to%20arm%20%24O_1O_2%24%2C%20in%20the%20directions%20indicated.%20Designating%20by%20%24%5Cell_1%24%20the%20length%20of%20arm%20%24O_1O_2%24%20and%20by%20%24%5Cell_2%24%20the%20length%20of%20arm%20%24O_2P%24%2C%20obtain%20the%20expressions%20for%20the%20velocity%20%24%5Coverrightarrow%7Bv%7D_P%24%20and%20the%20acceleration%20%24%5Coverrightarrow%7Ba%7D_P%24%20of%20the%20endpoint%20%24P%24%20relative%20to%20the%20floor%2C%20as%20functions%20of%20the%20indicated%20parameters.%0A%20%20%20%20%20%20%20%3Cfigure%3E%3Cimg%20src%3D%22https%3A%2F%2Fgithub.com%2FBMClab%2FBMC%2Fblob%2Fmaster%2Fimages%2Frobot_arm.png%3Fraw%3D1%22%20width%3D450%20alt%3D%22planar%20robot%20with%20two%20arms%22%2F%3E%3C%2Ffigure%3E%0A%0A%20%20%20%208.%20For%20the%20one-link%20minimum-jerk%20movement%20of%20this%20notebook%2C%20find%20the%20instant%20at%20which%20the%20tangential%20and%20centripetal%20accelerations%20of%20the%20endpoint%20have%20the%20same%20magnitude.%20What%20is%20the%20magnitude%20of%20the%20total%20acceleration%20then%3F%0A%0A%20%20%20%209.%20Modify%20the%20simulated%20reach%20so%20that%20the%20endpoint%20moves%20in%20a%20straight%20line%20from%20%24(0.3%2C%200.3)%24%20m%20to%20%24(0.65%2C%200.65)%24%20m%2C%20ending%20close%20to%20the%20edge%20of%20the%20workspace.%20Plot%20the%20joint%20angles%20and%20velocities%2C%20and%20explain%20what%20happens%20to%20the%20elbow%20near%20the%20end%20of%20the%20movement%20using%20the%20determinant%20of%20the%20Jacobian.%0A%0A%20%20%20%2010.%20After%20an%20elbow%20injury%2C%20a%20person%20can%20flex%20the%20elbow%20only%20up%20to%20%24100%5Eo%24.%20Using%20the%20arm%20and%20the%20shoulder%20limits%20of%20the%20example%20on%20the%20workspace%20of%20a%20real%20arm%2C%20calculate%20how%20much%20of%20the%20workspace%20is%20lost%2C%20and%20draw%20the%20region%20that%20remains.%20Which%20everyday%20tasks%2C%20done%20with%20the%20hand%20close%20to%20the%20body%2C%20would%20become%20difficult%3F%0A%0A%20%20%20%2011.%20Add%20a%20third%20link%20to%20the%20chain%2C%20so%20that%20it%20models%20the%20upper%20arm%2C%20the%20forearm%20and%20the%20hand%2C%20with%20a%20wrist%20joint%3A%20%24%5Cell_1%20%3D%200.30%24%20m%2C%20%24%5Cell_2%20%3D%200.25%24%20m%20and%20%24%5Cell_3%20%3D%200.08%24%20m%20(from%20the%20wrist%20to%20the%20fingertip).%3Cbr%3E%0A%20%20%20%20%20%20%20%20a)%20Write%20the%20forward%20kinematics%20of%20the%20fingertip%20and%20its%20%242%5Ctimes3%24%20Jacobian.%3Cbr%3E%0A%20%20%20%20%20%20%20%20b)%20At%20%24%5Ctheta_1%20%3D%2030%5Eo%24%2C%20%24%5Ctheta_2%20%3D%2060%5Eo%24%20and%20%24%5Ctheta_3%20%3D%2020%5Eo%24%2C%20find%20a%20combination%20of%20joint%20velocities%20that%20leaves%20the%20fingertip%20still%20(hint%3A%20%60scipy.linalg.null_space%60).%3Cbr%3E%0A%20%20%20%20%20%20%20%20c)%20Relate%20the%20result%20to%20the%20redundancy%20of%20the%20chain.%0A%0A%20%20%20%2012.%20The%20quantity%20%24%7C%5Cdet%5Cmathbf%7BJ%7D%7C%24%20is%20sometimes%20used%20as%20a%20measure%20of%20how%20easily%20the%20endpoint%20can%20move%20in%20every%20direction%2C%20the%20manipulability%20of%20the%20chain.%20Plot%20it%20as%20a%20colour%20map%20over%20the%20workspace%20of%20the%20chain%20with%20%24%5Cell_1%20%3D%20%5Cell_2%20%3D%200.5%24%20m.%20Where%20is%20it%20largest%2C%20and%20what%20is%20the%20elbow%20angle%20there%3F%20Is%20that%20where%20you%20would%20choose%20to%20hold%20an%20object%20you%20are%20working%20on%3F%0A%0A%20%20%20%2013.%20Inverse%20kinematics%20amplifies%20measurement%20errors%20near%20singular%20configurations.%20For%20the%20chain%20with%20%24%5Cell_1%20%3D%20%5Cell_2%20%3D%200.5%24%20m%2C%20put%20the%20endpoint%20at%200.99%20m%20from%20the%20base%2C%20add%20random%20noise%20with%20a%20standard%20deviation%20of%201%20mm%20to%20its%20coordinates%201000%20times%2C%20and%20calculate%20the%20standard%20deviation%20of%20the%20elbow%20angle%20given%20by%20%60inverse_kinematics%60.%20Repeat%20with%20the%20endpoint%20at%200.70%20m%20from%20the%20base%2C%20and%20explain%20the%20difference%20with%20the%20determinant%20of%20the%20Jacobian.%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%20the%20problems%0A%0A%20%20%20%201.%20%24J%3D%5B%5B2xy%2Cx%5E2%5D%2C%5B5%2C%5Ccos%20y%5D%5D%24.%0A%20%20%20%202.%20Let%20%24u(q)%3D(%5Ccos%20q%2C%5Csin%20q)%24%20and%20%24u_%5Cperp(q)%3D(-%5Csin%20q%2C%5Ccos%20q)%24.%20(a)%20%24r%3D(%5Cell_1%2C0)%2B%5Cell_2u(%5Ctheta_2)%24%2C%20%24v%3D%5Cell_2%5Cdot%5Ctheta_2u_%5Cperp%24%2C%20%24a%3D%5Cell_2%5Cddot%5Ctheta_2u_%5Cperp-%5Cell_2%5Cdot%5Ctheta_2%5E2u%24%3B%20reduced%20Jacobian%20%24%5Cell_2u_%5Cperp%24.%20(b)%20Replace%20the%20chain%20by%20one%20link%20of%20length%20%24%5Cell_1%2B%5Cell_2%24%2C%20angle%20%24%5Ctheta_1%24%3B%20reduced%20Jacobian%20%24(%5Cell_1%2B%5Cell_2)u_%5Cperp(%5Ctheta_1)%24.%0A%20%20%20%203.%20For%20angles%20%24q_1%2Cq_2%24%20measured%20from%20downward%20vertical%20with%20%24q_2%24%20relative%20to%20link%201%3A%20%24r%3D(%5Cell_1%5Csin%20q_1%2B%5Cell_2%5Csin(q_1%2Bq_2)%2C-%5Cell_1%5Ccos%20q_1-%5Cell_2%5Ccos(q_1%2Bq_2))%24.%20The%20velocity%20and%20acceleration%20have%20the%20same%20form%20as%20item%204%20with%20%24u(q)%3D(%5Csin%20q%2C-%5Ccos%20q)%24%20and%20%24u_%5Cperp(q)%3D(%5Ccos%20q%2C%5Csin%20q)%24.%0A%20%20%20%204.%20For%20absolute%20segment%20angles%20%24%5Cphi_1%2C%5Cphi_2%24%3A%20%24r%3D%5Csum_i%5Cell_i%20u(%5Cphi_i)%24%2C%20%24v%3D%5Csum_i%5Cell_i%5Cdot%5Cphi_i%20u_%5Cperp(%5Cphi_i)%24%2C%20%24a%3D%5Csum_i%5Cell_i%5B%5Cddot%5Cphi_i%20u_%5Cperp(%5Cphi_i)-%5Cdot%5Cphi_i%5E2u(%5Cphi_i)%5D%24.%20The%20Jacobian%20columns%20are%20%24%5Cell_i%20u_%5Cperp(%5Cphi_i)%24%3B%20joint%20angles%20obey%20%24%5Cphi_1%3D%5Ctheta_1%24%2C%20%24%5Cphi_2%3D%5Ctheta_1%2B%5Ctheta_2%24.%0A%20%20%20%205.%20Under%20the%20stated%20constraint%20there%20is%20one%20independent%20coordinate%20%24q%3D%5Ctheta_1%24%2C%20with%20%24%5Ctheta_2%3D%5Cpi-2q%24.%20Then%20%24r%3D(0%2C2%5Cell%5Csin%20q)%24%2C%20reduced%20%24J%3D(0%2C2%5Cell%5Ccos%20q)%5ET%24%2C%20%24v%3D(0%2C2%5Cell%5Cdot%20q%5Ccos%20q)%24%20and%20%24a%3D(0%2C2%5Cell%5B%5Cddot%20q%5Ccos%20q-%5Cdot%20q%5E2%5Csin%20q%5D)%24.%20The%20net%20transverse%20component%20is%20zero%3B%20the%20full%20unconstrained%20Jacobian%20must%20be%20multiplied%20by%20%24(1%2C-2)%5ET%24%20before%20comparison.%20Separate%20tangential%2C%20centripetal%20and%20Coriolis%20contributions%20depend%20on%20the%20selected%20unconstrained%20coordinates%2C%20although%20their%20sum%20is%20the%20expression%20above.%0A%20%20%20%206.%20%24%5Comega_%7BBD%7D%3D-44.88%24%20rad%2Fs%3B%20piston%20velocity%20%24v_P%3D-8.976%24%20m%2Fs%2C%20with%20the%20signs%20and%20dimensions%20in%20the%20retained%20figure.%0A%20%20%20%207.%20Let%20%24q%3D%5Ctheta_1%2B%5Ctheta_2%24%2C%20%24%5Cdot%20q%3D%5Comega_1%2B%5Comega_2%24%20and%20%24%5Cddot%20q%3D%5Calpha_1%2B%5Calpha_2%24.%20The%20final%20velocity%20is%20%24(v_0%2B%5Cell_1%5Comega_1%5Ccos%5Ctheta_1%2B%5Cell_2%5Cdot%20q%5Ccos%20q%2C%5Cell_1%5Comega_1%5Csin%5Ctheta_1%2B%5Cell_2%5Cdot%20q%5Csin%20q)%24.%20Acceleration%20is%20%24(a_0%2B%5Cell_1%5B%5Calpha_1%5Ccos%5Ctheta_1-%5Comega_1%5E2%5Csin%5Ctheta_1%5D%2B%5Cell_2%5B%5Cddot%20q%5Ccos%20q-%5Cdot%20q%5E2%5Csin%20q%5D%2C%5Cell_1%5B%5Calpha_1%5Csin%5Ctheta_1%2B%5Comega_1%5E2%5Ccos%5Ctheta_1%5D%2B%5Cell_2%5B%5Cddot%20q%5Csin%20q%2B%5Cdot%20q%5E2%5Ccos%20q%5D)%24.%0A%20%20%20%208.%20Nontrivial%20interior%20instants%200.357325%20and%200.642675%20s%3B%20acceleration%20magnitude%208.73409%20m%2Fs%C2%B2%20at%20each.%20At%20the%20endpoints%20both%20components%20are%20also%20zero.%0A%20%20%20%209.%20End%20elbow%20flexion%20is%20about%2046.37%C2%B0%20for%20the%20positive-elbow%20solution%2C%20compared%20with%20about%20129.79%C2%B0%20initially.%20The%20Jacobian%20determinant%20decreases%20near%20the%20outer%20boundary%2C%20increasing%20sensitivity%20to%20endpoint%20errors%3B%20minimum-jerk%20endpoint%20speed%20still%20goes%20to%20zero%20at%20the%20end.%0A%20%20%20%2010.%20Remaining%20area%200.334607%20m%C2%B2%3B%20area%20lost%200.184033%20m%C2%B2%2C%20about%2035.5%25%20of%20the%20original%20workspace.%20The%20lost%20region%20is%20mainly%20close%20to%20the%20shoulder%3B%20bringing%20the%20hand%20to%20the%20mouth%20or%20chest%20becomes%20more%20difficult.%0A%20%20%20%2011.%20With%20%24%5Cphi_i%3D%5Csum_%7Bk%5Cle%20i%7D%5Ctheta_k%24%2C%20%24r%3D%5Csum_i%5Cell_i%20u(%5Cphi_i)%24%20and%20%24J_%7B%3A%2Cj%7D%3D%5Csum_%7Bi%5Cge%20j%7D%5Cell_i%20u_%5Cperp(%5Cphi_i)%24.%20A%20unit%20null-space%20velocity%20is%20approximately%20%24(0.072823%2C-0.324444%2C0.943097)%24%20rad%2Fs%2C%20or%20any%20scalar%20multiple.%20The%20fingertip%20has%20two%20position%20constraints%20and%20three%20joint%20rates%2C%20leaving%20one%20generic%20redundant%20direction.%0A%20%20%20%2012.%20Maximum%20%24%7C%5Cdet%20J%7C%3D0.25%24%20m%C2%B2%20at%20elbow%20angles%20%C2%B190%C2%B0%2C%20on%20radius%20%24%5Csqrt%7B0.5%7D%3D0.707107%24%20m.%20This%20is%20best%20for%20the%20stated%20kinematic%20area%20measure%3B%20comfort%2C%20strength%20and%20load%20direction%20also%20affect%20a%20useful%20working%20posture.%0A%20%20%20%2013.%20With%20NumPy%20generator%20seed%200%20and%201000%20independent%20coordinate-noise%20samples%2C%20elbow-angle%20standard%20deviations%20are%20about%200.828%C2%B0%20at%200.99%20m%20and%200.164%C2%B0%20at%200.70%20m.%20The%20near-extended%20posture%20has%20the%20smaller%20determinant%20and%20greater%20amplification.%0A%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_(L1_arm%2C%20L2_arm%2C%20mo%2C%20np%2C%20plt)%3A%0A%20%20%20%20_t%3Dnp.linspace(0%2C1%2C501)%3B_s%3D10*_t**3-15*_t**4%2B6*_t**5%0A%20%20%20%20_x%3D.3%2B.35*_s%3B_y%3D_x.copy()%0A%20%20%20%20_q1%2C_q2%3Dinverse_kinematics(_x%2C_y%2C.5%2C.5%2Celbow%3D1)%0A%20%20%20%20_fig%2C_ax%3Dplt.subplots(1%2C3%2Cfigsize%3D(12%2C3.5))%0A%20%20%20%20_ax%5B0%5D.plot(_t%2Cnp.degrees(np.c_%5B_q1%2C_q2%5D))%3B_ax%5B0%5D.set(xlabel%3D%22Time%20%5Bs%5D%22%2Cylabel%3D%22Joint%20angle%20%5Bdeg%5D%22)%0A%20%20%20%20_ax%5B1%5D.plot(_t%2Cnp.degrees(np.gradient(np.c_%5B_q1%2C_q2%5D%2C_t%2Caxis%3D0)))%3B_ax%5B1%5D.set(xlabel%3D%22Time%20%5Bs%5D%22%2Cylabel%3D%22Joint%20rate%20%5Bdeg%2Fs%5D%22)%0A%20%20%20%20_a%2C_b%3Dnp.meshgrid(np.linspace(-30%2C135%2C160)%2Cnp.linspace(0%2C100%2C120))%3B_a%3Dnp.radians(_a)%3B_b%3Dnp.radians(_b)%0A%20%20%20%20_xx%3DL1_arm*np.cos(_a)%2BL2_arm*np.cos(_a%2B_b)%3B_yy%3DL1_arm*np.sin(_a)%2BL2_arm*np.sin(_a%2B_b)%0A%20%20%20%20_ax%5B2%5D.scatter(_xx.ravel()%2C_yy.ravel()%2Cs%3D1%2Calpha%3D.25)%3B_ax%5B2%5D.set(xlabel%3D%22X%20%5Bm%5D%22%2Cylabel%3D%22Y%20%5Bm%5D%22%2Caspect%3D%22equal%22%2Ctitle%3D%22Elbow%20limit%20100%C2%B0%22)%0A%20%20%20%20_ax%5B0%5D.legend(%5B%22shoulder%22%2C%22elbow%22%5D)%3B_ax%5B1%5D.legend(%5B%22shoulder%22%2C%22elbow%22%5D)%3B_fig.tight_layout()%0A%20%20%20%20_problem_plot%20%3D%20mo.accordion(%7B%22Final%20plots%20%E2%80%94%20Problems%209%20and%2010%22%3Amo.as_html(_fig)%7D%2Cexpanded%3DFalse)%0A%20%20%20%20plt.close(_fig)%0A%20%20%20%20_problem_plot%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%20Go%20deeper%0A%0A%20%20%20%20-%20Read%20pages%20477%E2%80%93494%20of%20chapter%2010%20of%20%5BRuina%20and%20Pratap's%20book%5D(https%3A%2F%2Fruina.tam.cornell.edu%2FBook%2Findex.html)%20for%20a%20review%20of%20differential%20equations%20and%20kinematics.%0A%20%20%20%20-%20Read%20section%202.8%20of%20Rade's%20book%20about%20the%20rotation%20matrix%20applied%20to%20kinematic%20chains.%0A%20%20%20%20-%20Read%20%5BComputational%20Motor%20Control%3A%20Kinematics%5D(https%3A%2F%2Fweb.archive.org%2Fweb%2F20221202221546%2Fhttps%3A%2F%2Fgribblelab.org%2Fteaching%2Fcompneuro2012%2F4_Computational_Motor_Control_Kinematics.html)%20(archived%20copy)%20about%20the%20kinematic%20chain%20as%20a%20model%20of%20the%20human%20upper%20limb.%0A%20%20%20%20-%20For%20more%20about%20the%20Jacobian%20matrix%2C%20read%20%5BJacobian%20matrix%20and%20determinant%5D(https%3A%2F%2Fwww.algebrapracticeproblems.com%2Fjacobian-matrix-determinant%2F).%0A%20%20%20%20-%20%5BRigid-body%20transformations%20in%202D%5D(https%3A%2F%2Fmolab.marimo.io%2Fgithub%2FBMClab%2FBMC%2Fblob%2Fmaster%2Fnotebooks%2Ftransformation_2d.py)%3A%20rotation%20matrices%2C%20the%20formalism%20that%20scales%20to%20many%20links%20and%20to%20three%20dimensions.%0A%20%20%20%20-%20%5BThe%20minimum-jerk%20hypothesis%5D(https%3A%2F%2Fmolab.marimo.io%2Fgithub%2FBMClab%2FBMC%2Fblob%2Fmaster%2Fnotebooks%2Fminimum_jerk_hypothesis.py)%3A%20the%20trajectory%20used%20in%20the%20simulations.%0A%0A%20%20%20%20%23%23%23%20Video%20lectures%20on%20the%20Internet%0A%0A%20%20%20%20-%20Khan%20Academy%3A%20%5BDifferential%20Calculus%20Review%5D(https%3A%2F%2Fwww.khanacademy.org%2Fmath%2Fdifferential-calculus)%0A%20%20%20%20-%20Khan%20Academy%3A%20%5BChain%20Rule%20Review%5D(https%3A%2F%2Fwww.khanacademy.org%2Fmath%2Fdifferential-calculus%2Fdc-chain)%0A%20%20%20%20-%20%5BMultivariate%20Calculus%20%E2%80%93%20Jacobian%20applied%5D(https%3A%2F%2Fwww.youtube.com%2Fwatch%3Fv%3D57q-2YxIZss)%0A%20%20%20%20-%20%5BKinematics%20of%20the%20Two%20Link%20Arm%5D(https%3A%2F%2Fyoutu.be%2F3WAk8ABj-kg)%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-%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(7)%2C%201688%E2%80%931703.%0A%0A%20%20%20%20-%20Morasso%20P%20(1981)%20%5BSpatial%20control%20of%20arm%20movements%5D(https%3A%2F%2Fdoi.org%2F10.1007%2FBF00236911).%20Experimental%20Brain%20Research%2C%2042(2)%2C%20223%E2%80%93227.%0A%0A%20%20%20%20-%20Mussa-Ivaldi%20FA%20(1986)%20Compliance.%20In%3A%20Morasso%20P%2C%20Tagliasco%20V%20(eds)%2C%20%5BHuman%20Movement%20Understanding%3A%20from%20computational%20geometry%20to%20artificial%20intelligence%5D(https%3A%2F%2Fbooks.google.com.br%2Fbooks%3Fid%3DZlZyLKNoAtEC).%20North-Holland%2C%20Amsterdam.%0A%0A%20%20%20%20-%20Rade%20D%20(2017)%20%5BCinem%C3%A1tica%20e%20Din%C3%A2mica%20para%20Engenharia%5D(https%3A%2F%2Fwww.grupogen.com.br%2Fe-book-cinematica-e-dinamica-para-engenharia).%20Grupo%20GEN.%0A%0A%20%20%20%20-%20Ruina%20A%2C%20Pratap%20R%20(2019)%20%5BIntroduction%20to%20Statics%20and%20Dynamics%5D(https%3A%2F%2Fruina.tam.cornell.edu%2FBook%2Findex.html).%20Oxford%20University%20Press.%0A%0A%20%20%20%20-%20Siciliano%20B%20et%20al.%20(2009)%20%5BRobotics%20-%20Modelling%2C%20Planning%20and%20Control%5D(https%3A%2F%2Fbooks.google.com.br%2Fbooks%2Fabout%2FRobotics.html%3Fhl%3Dpt-BR%26id%3DjPCAFmE-logC).%20Springer-Verlag%20London.%0A%0A%20%20%20%20-%20Zatsiorsky%20VM%20(1998)%20%5BKinematics%20of%20Human%20Motion%5D(https%3A%2F%2Fbooks.google.com.br%2Fbooks%2Fabout%2FKinematics_of_Human_Motion.html%3Fid%3DPql_xXdbrMcC%26redir_esc%3Dy).%20Champaign%2C%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
7fb90b9fa604cdabc0530a669e32a4d2