import%20marimo%0A%0A__generated_with%20%3D%20%220.14.16%22%0Aapp%20%3D%20marimo.App()%0A%0A%0A%40app.cell%0Adef%20_()%3A%0A%20%20%20%20import%20marimo%20as%20mo%0A%20%20%20%20import%20numpy%20as%20np%0A%20%20%20%20import%20gc%0A%0A%20%20%20%20from%20robosandbox.performance.workspace%20import%20WorkSpace%0A%20%20%20%20from%20robosandbox.optimization.problem%20import%20DesignProblem%0A%20%20%20%20from%20robosandbox.models.DH.Generic.Generic%20import%20Generic%0A%20%20%20%20from%20pymoo.algorithms.soo.nonconvex.de%20import%20DE%0A%20%20%20%20from%20pymoo.optimize%20import%20minimize%0A%20%20%20%20from%20pymoo.termination%20import%20get_termination%0A%20%20%20%20from%20ansitable%20import%20ANSITable%2C%20Column%0A%20%20%20%20return%20(%0A%20%20%20%20%20%20%20%20ANSITable%2C%0A%20%20%20%20%20%20%20%20Column%2C%0A%20%20%20%20%20%20%20%20DE%2C%0A%20%20%20%20%20%20%20%20DesignProblem%2C%0A%20%20%20%20%20%20%20%20Generic%2C%0A%20%20%20%20%20%20%20%20WorkSpace%2C%0A%20%20%20%20%20%20%20%20gc%2C%0A%20%20%20%20%20%20%20%20get_termination%2C%0A%20%20%20%20%20%20%20%20minimize%2C%0A%20%20%20%20%20%20%20%20mo%2C%0A%20%20%20%20%20%20%20%20np%2C%0A%20%20%20%20)%0A%0A%0A%40app.cell%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%23%20Use%20case%20B%20-%20an%20optimization-driven%20design%20loop%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%23%23%20Problem%20P1%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(%0A%20%20%20%20%20%20%20%20r%22%22%22%0A%20%20%20%20%24%24%0A%20%20%20%20%20%20%20%20%5Cbegin%7Baligned%7D%0A%20%20%20%20%20%20%20%20%20%20%20%20%5Ctext%7B(P1)%7D%5Cquad%20%26%20%5Cmin_%7B%5Cmathbf%7Ba%7D%2C%20%5Cmathbf%7Bd%7D%7D%5Cquad%20f_%7B1%7D(%5Cmathbf%7Ba%7D%2C%20%5Cmathbf%7Bd%7D%3B%20%5Cboldsymbol%7B%5Calpha%5E0%7D)%20%3D%20%5Cdelta%20%20%20%20%20%20%20%20%20%5C%5C%0A%20%20%20%20%20%20%20%20%20%20%20%20%5Ctext%7B(P2)%7D%5Cquad%20%26%20%5Cmin_%7B%5Cmathbf%7Ba%7D%2C%20%5Cmathbf%7Bd%7D%7D%5Cquad%20f_%7B2%7D(%5Cmathbf%7Ba%7D%2C%20%5Cmathbf%7Bd%7D%3B%20%5Cboldsymbol%7B%5Calpha%5E0%7D)%20%3D%20%5Cdelta%20-%20G_%7Bn%7D%20%5C%5C%0A%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%26%20%5Ctext%7Bs.t.%7D%5Cquad%20%5Cboldsymbol%7B%5Calpha%5E0%7D%3D%5B90%5E%7B%5Ccirc%7D%2C%2090%5E%7B%5Ccirc%7D%2C%20-90%5E%7B%5Ccirc%7D%5D%5E%7BT%7D%2C%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%5C%5C%0A%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%26%20%5Cquad%20a_%7Bk%7D%5E%7B-%7D%5Cleq%20a_%7Bk%7D%5Cleq%20a_%7Bk%7D%5E%7B%2B%7D%5Cquad%20%5Cforall%20k%20%5Cin%20%5C%7B1%2C%202%2C%205%5C%7D%2C%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%5C%5C%0A%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%26%20%5Cquad%20d_%7Bl%7D%5E%7B-%7D%5Cleq%20d_%7Bl%7D%5Cleq%20d_%7Bl%7D%5E%7B%2B%7D%5Cquad%20%5Cforall%20l%20%5Cin%20%5C%7B%200%2C%203%2C%204%5C%7D%2C%0A%20%20%20%20%20%20%20%20%5Cend%7Baligned%7D%0A%20%20%20%20%24%24%0A%0A%20%20%20%20where%20%24%5Cmathbf%7Ba%7D%3D%5Ba_%7B1%7D%2C%20a_%7B2%7D%2C%20a_%7B5%7D%5D%5E%7BT%7D%5Cin%20%5Cmathbb%7BR%7D%5E%7B3%7D%24%2C%0A%20%20%20%20%24%5Cmathbf%7Bd%7D%3D%5Bd_%7B0%7D%2C%20d_%7B3%7D%2C%20d_%7B4%7D%5D%5E%7BT%7D%5Cin%20%5Cmathbb%7BR%7D%5E%7B3%7D%24%20are%20the%20design%0A%20%20%20%20variables%20corresponding%20to%20the%20non-zero%20link%20length%20and%20link%20offset%20of%20the%20UR5%2C%20and%0A%20%20%20%20%24%5Cboldsymbol%7B%5Calpha%5E0%7D%3D%5B%20%5Calpha_%7B0%7D%2C%20%5Calpha_%7B3%7D%2C%20%5Calpha_%7B4%7D%5D%5E%7BT%7D%24%20is%20the%20constant%0A%20%20%20%20value%20of%20link%20twist%20of%20the%20UR5.%20%24a_%7Bk%7D%5E%7B-%7D%2C%20a_%7Bk%7D%5E%7B%2B%7D%24%20and%0A%20%20%20%20%24d_%7Bl%7D%5E%7B-%7D%2C%20d_%7Bl%7D%5E%7B%2B%7D%24%20are%20the%20lower%20and%20upper%20bounds%20of%20the%20design%20variables%2C%20respectively.%0A%20%20%20%20%24%5Cdelta%24%20is%20the%20reach%20error%2C%20defined%20as%0A%20%20%20%20%22%22%22%0A%20%20%20%20)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(np)%3A%0A%20%20%20%20np.random.seed(42)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%23%23%20Functions%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%23%23%23%20Reach%20error%20%24%5Cdelta%24%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(%0A%20%20%20%20%20%20%20%20r%22%22%22%0A%20%20%20%20%24%24%0A%20%20%20%20%20%20%20%20%5Cbegin%7Baligned%7D%0A%20%20%20%20%20%20%20%20%20%20%20%20%20%26%20%5Cdelta%20%3D%20%5Csum_%7Bi%20%5Cin%20%5C%7Bx%2C%20y%2C%20z%5C%7D%7D%5Cdelta_%7Bi%7D%2C%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%5C%5C%0A%20%20%20%20%20%20%20%20%20%20%20%20%20%26%20%5Cdelta_%7Bi%7D%3D%20%5Cleft%7C%20%5Cleft(r_%7B%5Ctext%7Bmax%7D%2Ci%7D-%20r_%7B%5Ctext%7Bmin%7D%2Ci%7D%5Cright)%20-%20R_%7B%5Ctext%7Btarget%7D%2Ci%7D%5Cright%7C%2C%0A%20%20%20%20%20%20%20%20%5Cend%7Baligned%7D%0A%20%20%20%20%24%24%0A%0A%20%20%20%20where%20%24r_%7B%5Ctext%7Bmax%7D%2Ci%7D%24%20and%20%24r_%7B%5Ctext%7Bmin%7D%2Ci%7D%24%20are%20the%20maximum%20and%20minimum%20reach%0A%20%20%20%20of%20the%20manipulator%20in%20the%20%24i%24-th%20axis%2C%20and%20%24R_%7B%5Ctext%7Btarget%7D%2Ci%7D%24%20is%20the%20target%0A%20%20%20%20reach%20range%20in%20the%20%24i%24-th%20axis.%20%24%5Cdelta_%7Bi%7D%24%20is%20the%20reach%20error%20in%20the%20%24i%24-th%20axis.%0A%20%20%20%20%22%22%22%0A%20%20%20%20)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(np)%3A%0A%20%20%20%20def%20compute_%CE%B4(R_target%2C%20workspace)%3A%0A%20%20%20%20%20%20%20%20%22%22%22%0A%20%20%20%20%20%20%20%20Constrain%20the%20range%20to%20the%20target%20range.%0A%20%20%20%20%20%20%20%20Returns%20the%20difference%20between%20current%20range%20and%20target%20range.%0A%20%20%20%20%20%20%20%20%22%22%22%0A%0A%20%20%20%20%20%20%20%20def%20%CE%B4_i(r%2C%20R_target)%3A%0A%20%20%20%20%20%20%20%20%20%20%20%20%22%22%22%0A%20%20%20%20%20%20%20%20%20%20%20%20ex.1%3A%20current_range%20%3D%20(-0.6%2C%200.6)%2C%20target_range%20%3D%201.2%0A%20%20%20%20%20%20%20%20%20%20%20%20Returns%3A%20d%20%3D%20abs((0.6%20-%20(-0.6))%20-%201.2)%20%3D%20abs(1.2%20-%201.2)%20%3D%200.0%0A%20%20%20%20%20%20%20%20%20%20%20%20ex.2%3A%20current_range%20%3D%20(-0.6%2C%200.6)%2C%20target_range%20%3D%201%0A%20%20%20%20%20%20%20%20%20%20%20%20Returns%3A%20d%20%3D%20abs((0.6%20-%20(-0.6))%20-%201)%20%3D%20abs(1.2%20-%201)%20%3D%200.2%0A%20%20%20%20%20%20%20%20%20%20%20%20%22%22%22%0A%20%20%20%20%20%20%20%20%20%20%20%20rmin%2C%20rmax%20%3D%20r%0A%20%20%20%20%20%20%20%20%20%20%20%20ranges%20%3D%20rmax%20-%20rmin%0A%20%20%20%20%20%20%20%20%20%20%20%20d%20%3D%20np.abs(ranges%20-%20R_target)%0A%20%20%20%20%20%20%20%20%20%20%20%20return%20d%0A%0A%20%20%20%20%20%20%20%20%CE%B4_x%20%3D%20%CE%B4_i(workspace.reach(axes%3D%22x%22)%2C%20R_target%5B%22x%22%5D)%0A%20%20%20%20%20%20%20%20%CE%B4_y%20%3D%20%CE%B4_i(workspace.reach(axes%3D%22y%22)%2C%20R_target%5B%22y%22%5D)%0A%20%20%20%20%20%20%20%20%CE%B4_z%20%3D%20%CE%B4_i(workspace.reach(axes%3D%22z%22)%2C%20R_target%5B%22z%22%5D)%0A%20%20%20%20%20%20%20%20%CE%B4%20%3D%20%CE%B4_x%20%2B%20%CE%B4_y%20%2B%20%CE%B4_z%0A%20%20%20%20%20%20%20%20return%20%CE%B4%2C%20%CE%B4_x%2C%20%CE%B4_y%2C%20%CE%B4_z%0A%20%20%20%20return%20(compute_%CE%B4%2C)%0A%0A%0A%40app.cell%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%23%23%23%20Global%20Normalized%20Manipulability%20%24G_%7Bn%7D%24%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(%0A%20%20%20%20%20%20%20%20r%22%22%22%0A%20%20%20%20%24G_%7Bn%7D%24%20is%20the%20global%20normalized%20manipulability%20index%3A%0A%0A%20%20%20%20%24%24%0A%20%20%20%20%20%20%20%20%5Cbegin%7Baligned%7D%0A%20%20%20%20%20%20%20%20%20%20%20%20G_%7Bn%7D%3D%20%5Cfrac%7B1%7D%7BN%7D%5Csum_%7Bi%3D1%7D%5E%7BN%7D%5Cmu_%7Bn%7D(i)%20%3D%20%5Cfrac%7B1%7D%7BN%7D%5Csum_%7Bi%3D1%7D%5E%7BN%7D%5Cfrac%7B%5Cmu(i)%7D%7B%5Cmax%5Climits_%7Bj%7D%5Cmu(j)%7D%2C%0A%20%20%20%20%20%20%20%20%5Cend%7Baligned%7D%0A%20%20%20%20%24%24%0A%0A%20%20%20%20where%20%24N%24%20is%20the%20number%20of%20sampled%20configurations%2C%20%24%5Cmu_%7Bn%7D(i)%24%20is%20the%0A%20%20%20%20normalized%20manipulability%20index%20for%20the%20%24i%24-th%20configuration%2C%20%24%5Cmax%5Climits_%7Bj%7D%5Cmu%0A%20%20%20%20_%7Bn%7D(j)%24%20is%20the%20maximum%20normalized%20manipulability%20index%20among%20all%20sampled%20configurations.%0A%20%20%20%20%22%22%22%0A%20%20%20%20)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(Generic%2C%20WorkSpace%2C%20np)%3A%0A%20%20%20%20def%20compute_Gn(%0A%20%20%20%20%20%20%20%20design_params%2C%20method%3D%22yoshikawa%22%2C%20axes%3D%22all%22%2C%20is_normalized%3DTrue%0A%20%20%20%20)%3A%0A%20%20%20%20%20%20%20%20d0%2C%20a1%2C%20a2%2C%20d3%2C%20d4%2C%20d5%20%3D%20design_params%0A%0A%20%20%20%20%20%20%20%20d%20%3D%20%5Bd0%2C%200%2C%200%2C%20d3%2C%20d4%2C%20d5%5D%20%20%23%20d1%2C%20d2%20are%200%0A%20%20%20%20%20%20%20%20a%20%3D%20%5B0%2C%20a1%2C%20a2%2C%200%2C%200%2C%200%5D%20%20%23%20a0%2C%20a3%2C%20a4%2C%20a5%20are%200%0A%20%20%20%20%20%20%20%20alpha%20%3D%20%5Bnp.pi%20%2F%202%2C%200%2C%200%2C%20np.pi%20%2F%202%2C%20-np.pi%20%2F%202%2C%200%5D%20%20%23%20same%20as%20UR5%0A%20%20%20%20%20%20%20%20robot%20%3D%20Generic(dofs%3D6%2C%20a%3Da%2C%20alpha%3Dalpha%2C%20d%3Dd)%0A%20%20%20%20%20%20%20%20ws%20%3D%20WorkSpace(robot%3Drobot)%0A%20%20%20%20%20%20%20%20Gn%20%3D%20ws.global_indice(%0A%20%20%20%20%20%20%20%20%20%20%20%20initial_samples%3D3000%2C%0A%20%20%20%20%20%20%20%20%20%20%20%20batch_ratio%3D0.1%2C%0A%20%20%20%20%20%20%20%20%20%20%20%20error_tolerance_percentage%3D1e-3%2C%0A%20%20%20%20%20%20%20%20%20%20%20%20method%3Dmethod%2C%0A%20%20%20%20%20%20%20%20%20%20%20%20axes%3Daxes%2C%0A%20%20%20%20%20%20%20%20%20%20%20%20max_samples%3D15000%2C%0A%20%20%20%20%20%20%20%20%20%20%20%20is_normalized%3Dis_normalized%2C%0A%20%20%20%20%20%20%20%20)%0A%20%20%20%20%20%20%20%20return%20Gn%2C%20ws%0A%20%20%20%20return%20(compute_Gn%2C)%0A%0A%0A%40app.cell%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%23%23%23%20Objective%20funtion%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(%0A%20%20%20%20DE%2C%0A%20%20%20%20DesignProblem%2C%0A%20%20%20%20compute_Gn%2C%0A%20%20%20%20compute_%CE%B4%2C%0A%20%20%20%20gc%2C%0A%20%20%20%20get_termination%2C%0A%20%20%20%20minimize%2C%0A%20%20%20%20np%2C%0A)%3A%0A%20%20%20%20def%20f1(design_params%2C%20method%3D%22yoshikawa%22%2C%20axes%3D%22all%22%2C%20is_normalized%3DTrue)%3A%0A%20%20%20%20%20%20%20%20R_target%20%3D%20%7B%22x%22%3A%201.2%2C%20%22y%22%3A%201.2%2C%20%22z%22%3A%201.2%7D%0A%0A%20%20%20%20%20%20%20%20Gn%2C%20ws%20%3D%20compute_Gn(%0A%20%20%20%20%20%20%20%20%20%20%20%20design_params%2C%20method%3Dmethod%2C%20axes%3Daxes%2C%20is_normalized%3Dis_normalized%0A%20%20%20%20%20%20%20%20)%0A%20%20%20%20%20%20%20%20H%2C%20_%2C%20_%2C%20_%20%3D%20compute_%CE%B4(R_target%2C%20ws)%0A%20%20%20%20%20%20%20%20F%20%3D%20H%0A%20%20%20%20%20%20%20%20%23%20Clean%20up%20to%20free%20memory%0A%20%20%20%20%20%20%20%20del%20ws%2C%20Gn%0A%20%20%20%20%20%20%20%20gc.collect()%0A%20%20%20%20%20%20%20%20return%20F%0A%0A%0A%20%20%20%20def%20run_optimization()%3A%0A%20%20%20%20%20%20%20%20n_var%20%3D%206%20%20%23%20Number%20of%20design%20parameters%0A%20%20%20%20%20%20%20%20xl%20%3D%20np.array(%0A%20%20%20%20%20%20%20%20%20%20%20%20%5B%0A%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%200.04%2C%20%20%23%20d0%0A%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20-0.4%2C%20%20%23%20a1%0A%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20-0.4%2C%20%20%23%20a2%0A%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%200.04%2C%20%20%23%20d3%0A%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%200.04%2C%20%20%23%20d4%0A%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%200.04%2C%20%20%23%20d5%0A%20%20%20%20%20%20%20%20%20%20%20%20%5D%0A%20%20%20%20%20%20%20%20)%0A%20%20%20%20%20%20%20%20xu%20%3D%20np.array(%0A%20%20%20%20%20%20%20%20%20%20%20%20%5B%0A%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%200.1%2C%20%20%23%20d0%0A%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20-0.2%2C%20%20%23%20a1%0A%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20-0.2%2C%20%20%23%20a2%0A%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%200.1%2C%20%20%23%20d3%0A%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%200.1%2C%20%20%23%20d4%0A%20%20%20%20%20%20%20%20%20%20%20%20%20%20%20%200.1%2C%20%20%23%20d5%0A%20%20%20%20%20%20%20%20%20%20%20%20%5D%0A%20%20%20%20%20%20%20%20)%0A%20%20%20%20%20%20%20%20problem%20%3D%20DesignProblem(%0A%20%20%20%20%20%20%20%20%20%20%20%20n_var%3Dn_var%2C%0A%20%20%20%20%20%20%20%20%20%20%20%20xl%3Dxl%2C%0A%20%20%20%20%20%20%20%20%20%20%20%20xu%3Dxu%2C%0A%20%20%20%20%20%20%20%20%20%20%20%20objs%3Df1%2C%0A%20%20%20%20%20%20%20%20%20%20%20%20constr_ieq%3D%5B%5D%2C%0A%20%20%20%20%20%20%20%20%20%20%20%20constr_eq%3D%5B%5D%2C%0A%20%20%20%20%20%20%20%20)%0A%20%20%20%20%20%20%20%20algorithm%20%3D%20DE(pop_size%3D20%2C%20variant%3D%22DE%2Frand%2F1%2Fbin%22)%0A%20%20%20%20%20%20%20%20termination%20%3D%20get_termination(%22n_gen%22%2C%2020)%20%20%23%20Run%20for%2020%20generations%0A%20%20%20%20%20%20%20%20result%20%3D%20minimize(problem%2C%20algorithm%2C%20termination%2C%20seed%3D42%2C%20verbose%3DTrue)%0A%20%20%20%20%20%20%20%20best_x%20%3D%20result.X%0A%0A%20%20%20%20%20%20%20%20best_f%20%3D%20result.F%5B0%5D%0A%20%20%20%20%20%20%20%20print(%22%5CnOptimization%20Completed!%22)%0A%20%20%20%20%20%20%20%20print(f%22Best%20objective%20value%3A%20%7Bbest_f%7D%22)%0A%20%20%20%20%20%20%20%20print(f%22Best%20design%20parameters%3A%20%7Bbest_x%7D%22)%0A%20%20%20%20%20%20%20%20return%20best_x%2C%20best_f%0A%20%20%20%20return%20(run_optimization%2C)%0A%0A%0A%40app.cell%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%23%23%20Run%20optimization%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(np%2C%20run_optimization)%3A%0A%20%20%20%20best_x_for_p1%2C%20best_f_for_p1%20%3D%20run_optimization()%0A%20%20%20%20np.save(%22.%2Fdata%2Fuse_case_b%2Fbest_x_for_p1.npy%22%2C%20best_x_for_p1)%0A%20%20%20%20print(%22Optimization%20finished%20for%20Problem%201%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%23%23%20Analysis%20of%20robot%20P1%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(Generic%2C%20WorkSpace%2C%20compute_%CE%B4%2C%20np)%3A%0A%20%20%20%20def%20analysis_p1()%3A%0A%20%20%20%20%20%20%20%20best_x_for_p1%20%3D%20np.load(%22.%2Fdata%2Fuse_case_b%2Fbest_x_for_p1.npy%22)%0A%20%20%20%20%20%20%20%20d0%2C%20a1%2C%20a2%2C%20d3%2C%20d4%2C%20d5%20%3D%20best_x_for_p1%0A%0A%20%20%20%20%20%20%20%20d%20%3D%20%5Bd0%2C%200%2C%200%2C%20d3%2C%20d4%2C%20d5%5D%20%20%23%20d1%2C%20d2%20are%200%0A%20%20%20%20%20%20%20%20a%20%3D%20%5B0%2C%20a1%2C%20a2%2C%200%2C%200%2C%200%5D%20%20%23%20a0%2C%20a3%2C%20a4%2C%20a5%20are%200%0A%20%20%20%20%20%20%20%20alpha%20%3D%20%5Bnp.pi%20%2F%202%2C%200%2C%200%2C%20np.pi%20%2F%202%2C%20-np.pi%20%2F%202%2C%200%5D%20%20%23%20same%20as%20UR5%0A%20%20%20%20%20%20%20%20p1%20%3D%20Generic(dofs%3D6%2C%20a%3Da%2C%20alpha%3Dalpha%2C%20d%3Dd)%0A%20%20%20%20%20%20%20%20ws%20%3D%20WorkSpace(robot%3Dp1)%0A%0A%20%20%20%20%20%20%20%20Gn_p1%20%3D%20ws.global_indice(%0A%20%20%20%20%20%20%20%20%20%20%20%20initial_samples%3D3000%2C%0A%20%20%20%20%20%20%20%20%20%20%20%20batch_ratio%3D0.1%2C%0A%20%20%20%20%20%20%20%20%20%20%20%20error_tolerance_percentage%3D1e-3%2C%0A%20%20%20%20%20%20%20%20%20%20%20%20method%3D%22yoshikawa%22%2C%0A%20%20%20%20%20%20%20%20%20%20%20%20axes%3D%22all%22%2C%0A%20%20%20%20%20%20%20%20%20%20%20%20max_samples%3D15000%2C%0A%20%20%20%20%20%20%20%20%20%20%20%20is_normalized%3DTrue%2C%0A%20%20%20%20%20%20%20%20)%0A%20%20%20%20%20%20%20%20%CE%B4%2C%20%CE%B4_x%2C%20%CE%B4_y%2C%20%CE%B4_z%20%3D%20compute_%CE%B4(%0A%20%20%20%20%20%20%20%20%20%20%20%20R_target%3D%7B%22x%22%3A%201.2%2C%20%22y%22%3A%201.2%2C%20%22z%22%3A%201.2%7D%2C%20workspace%3Dws%0A%20%20%20%20%20%20%20%20)%0A%20%20%20%20%20%20%20%20return%20(Gn_p1%2C%20%CE%B4%2C%20%CE%B4_x%2C%20%CE%B4_y%2C%20%CE%B4_z%2C%20p1%2C%20ws)%0A%0A%0A%20%20%20%20Gn_p1%2C%20%CE%B4%2C%20%CE%B4_x%2C%20%CE%B4_y%2C%20%CE%B4_z%2C%20p1%2C%20ws%20%3D%20analysis_p1()%0A%20%20%20%20return%20Gn_p1%2C%20p1%2C%20ws%2C%20%CE%B4%2C%20%CE%B4_x%2C%20%CE%B4_y%2C%20%CE%B4_z%0A%0A%0A%40app.cell%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%23%23%23%20Kinematic%20structure%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(p1)%3A%0A%20%20%20%20p1_fig%20%3D%20p1.plotly(q%3Dp1.qz)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%23%23%23%20Workspace%20visulization%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(ws)%3A%0A%20%20%20%20p1_ws%20%3D%20ws.plot(color%3D%22yoshikawa%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%23%23%23%20Terms%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(ANSITable%2C%20Column%2C%20Gn_p1%2C%20%CE%B4%2C%20%CE%B4_x%2C%20%CE%B4_y%2C%20%CE%B4_z)%3A%0A%20%20%20%20table%20%3D%20ANSITable()%0A%20%20%20%20table%20%3D%20ANSITable(%0A%20%20%20%20%20%20%20%20Column(%22Robot%22)%2C%0A%20%20%20%20%20%20%20%20Column(%22Gn%22)%2C%0A%20%20%20%20%20%20%20%20Column(%22%CE%B4%22)%2C%0A%20%20%20%20%20%20%20%20Column(%22%CE%B4_x%22)%2C%0A%20%20%20%20%20%20%20%20Column(%22%CE%B4_y%22)%2C%0A%20%20%20%20%20%20%20%20Column(%22%CE%B4_z%22)%2C%0A%20%20%20%20%20%20%20%20border%3D%22thin%22%2C%0A%20%20%20%20)%0A%20%20%20%20table.row(%0A%20%20%20%20%20%20%20%20%22p1%22%2C%0A%20%20%20%20%20%20%20%20f%22%7BGn_p1%3A.2f%7D%22%2C%0A%20%20%20%20%20%20%20%20f%22%7B%CE%B4%3A.2f%7D%22%2C%0A%20%20%20%20%20%20%20%20f%22%7B%CE%B4_x%3A.2f%7D%22%2C%0A%20%20%20%20%20%20%20%20f%22%7B%CE%B4_y%3A.2f%7D%22%2C%0A%20%20%20%20%20%20%20%20f%22%7B%CE%B4_z%3A.2f%7D%22%2C%0A%20%20%20%20)%0A%20%20%20%20table.print()%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
377582218ad421307b127a8f5d094e0f62ec1b47bf14f469e207383dc206aade