import%20marimo%0A%0A__generated_with%20%3D%20%220.14.17%22%0Aapp%20%3D%20marimo.App(width%3D%22medium%22)%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%23%20An%20RRT%20Example%20of%20Robot%20P1%0A%20%20%20%20An%20example%20demonstrates%20the%20use%20of%20the%20RRT%20algorithm%20to%20find%20a%20path%20for%20a%206-DOF%20robot%20in%20a%20simulated%20environment%20with%20obstacles.%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_()%3A%0A%20%20%20%20import%20marimo%20as%20mo%0A%20%20%20%20from%20rrt%20import%20RRT%0A%20%20%20%20from%20robosandbox.models.URDF.Generic%20import%20GenericDH%0A%20%20%20%20from%20pathlib%20import%20Path%0A%20%20%20%20import%20numpy%20as%20np%0A%20%20%20%20import%20swift%0A%20%20%20%20import%20spatialgeometry%20as%20sg%0A%20%20%20%20from%20spatialmath%20import%20SE3%0A%20%20%20%20return%20GenericDH%2C%20RRT%2C%20SE3%2C%20mo%2C%20np%2C%20sg%2C%20swift%0A%0A%0A%40app.cell%0Adef%20_(mo)%3A%0A%20%20%20%20mo.md(r%22%22%22%23%23%20Define%20P1%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(GenericDH%2C%20np)%3A%0A%20%20%20%20def%20create_p1()%3A%0A%20%20%20%20%20%20%20%20%22%22%22Create%20and%20return%20a%206-DOF%20robot.%22%22%22%0A%20%20%20%20%20%20%20%20design_params%20%3D%20np.load(%22data%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%20design_params%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%20from%20UR5%0A%20%20%20%20%20%20%20%20link_radius%20%3D%20%5B%0A%20%20%20%20%20%20%20%20%20%20%20%200.02%2C%0A%20%20%20%20%20%20%20%20%20%20%20%200.02%2C%0A%20%20%20%20%20%20%20%20%20%20%20%200.02%2C%0A%20%20%20%20%20%20%20%20%20%20%20%200.02%2C%0A%20%20%20%20%20%20%20%20%20%20%20%200.02%2C%0A%20%20%20%20%20%20%20%20%20%20%20%200.02%2C%0A%20%20%20%20%20%20%20%20%5D%20%20%23%20all%20links%20have%20same%20radius%0A%20%20%20%20%20%20%20%20actuator_radius%20%3D%20%5B0.02%2C%200.02%2C%200.020%2C%200.020%2C%200.022%2C%200.02%5D%0A%20%20%20%20%20%20%20%20actuator_length%20%3D%20%5B0.02%2C%200.06%2C%200.06%2C%200.06%2C%200.02%2C%200.04%5D%0A%20%20%20%20%20%20%20%20return%20GenericDH(%0A%20%20%20%20%20%20%20%20%20%20%20%20dofs%3D6%2C%0A%20%20%20%20%20%20%20%20%20%20%20%20a%3Da%2C%0A%20%20%20%20%20%20%20%20%20%20%20%20alpha%3Dalpha%2C%0A%20%20%20%20%20%20%20%20%20%20%20%20d%3Dd%2C%0A%20%20%20%20%20%20%20%20%20%20%20%20link_radius%3Dlink_radius%2C%0A%20%20%20%20%20%20%20%20%20%20%20%20actuator_radius%3Dactuator_radius%2C%0A%20%20%20%20%20%20%20%20%20%20%20%20actuator_length%3Dactuator_length%2C%0A%20%20%20%20%20%20%20%20)%0A%20%20%20%20return%20(create_p1%2C)%0A%0A%0A%40app.cell%0Adef%20_(SE3%2C%20np%2C%20sg)%3A%0A%20%20%20%20def%20set_env(env)%3A%0A%20%20%20%20%20%20%20%20obstacle1%20%3D%20sg.Cuboid(%5B1%2C%200.05%2C%201%5D%2C%20pose%3DSE3(0%2C%20-0.5%2C%200.5))%0A%20%20%20%20%20%20%20%20env.add(obstacle1)%0A%20%20%20%20%20%20%20%20obstacle2%20%3D%20sg.Cuboid(%5B1%2C%200.05%2C%201%5D%2C%20pose%3DSE3(0%2C%200.5%2C%200.5))%0A%20%20%20%20%20%20%20%20env.add(obstacle2)%0A%20%20%20%20%20%20%20%20obstacle3%20%3D%20sg.Cuboid(%0A%20%20%20%20%20%20%20%20%20%20%20%20%5B1%2C%200.05%2C%201%5D%2C%20pose%3DSE3(0.5%2C%200%2C%200.5)%20*%20SE3.Rz(np.pi%20%2F%202)%0A%20%20%20%20%20%20%20%20)%0A%20%20%20%20%20%20%20%20env.add(obstacle3)%0A%20%20%20%20%20%20%20%20obstacle4%20%3D%20sg.Cuboid(%0A%20%20%20%20%20%20%20%20%20%20%20%20%5B1%2C%200.05%2C%201%5D%2C%20pose%3DSE3(-0.5%2C%200%2C%200.5)%20*%20SE3.Rz(np.pi%20%2F%202)%0A%20%20%20%20%20%20%20%20)%0A%20%20%20%20%20%20%20%20env.add(obstacle4)%0A%0A%20%20%20%20%20%20%20%20list%20%3D%20%5Bobstacle1%2C%20obstacle2%2C%20obstacle3%2C%20obstacle4%5D%0A%20%20%20%20%20%20%20%20return%20env%2C%20list%0A%20%20%20%20return%20(set_env%2C)%0A%0A%0A%40app.cell%0Adef%20_(RRT%2C%20SE3%2C%20create_p1%2C%20np%2C%20set_env%2C%20sg%2C%20swift)%3A%0A%20%20%20%20np.random.seed(42)%20%20%23%20for%20reproducibility%0A%20%20%20%20robot%20%3D%20create_p1()%0A%0A%20%20%20%20env%20%3D%20swift.Swift()%0A%20%20%20%20env.launch(realtime%3DTrue%2C%20browser%3D%22notebook%22)%0A%20%20%20%20env.set_camera_pose(position%3D%5B-0.3%2C%20-0.3%2C%202.2%5D%2C%20look_at%3D%5B0%2C%200%2C%200.2%5D)%0A%20%20%20%20robot.q%20%3D%20np.array(%0A%20%20%20%20%20%20%20%20%5B0%2C%20-np.pi%20%2F%202%2C%200%2C%20np.pi%20%2F%202%2C%20np.pi%20%2F%202%2C%200%5D%0A%20%20%20%20)%20%20%23%20set%20initial%20pose%0A%0A%20%20%20%20env.add(robot)%0A%20%20%20%20env%2C%20list%20%3D%20set_env(env)%0A%20%20%20%20ax_goal%20%3D%20sg.Axes(0.1)%0A%20%20%20%20goal%20%3D%20SE3(%0A%20%20%20%20%20%20%20%20-0.25%2C%0A%20%20%20%20%20%20%20%20-0.25%2C%0A%20%20%20%20%20%20%20%200.35%2C%0A%20%20%20%20)%20*%20SE3.Rz(np.pi%20%2F%202)%0A%20%20%20%20ax_goal.T%20%3D%20goal.A%0A%20%20%20%20env.add(ax_goal)%0A%0A%20%20%20%20%23%20setup%20rrt%0A%20%20%20%20rrt%20%3D%20RRT(%0A%20%20%20%20%20%20%20%20robot%2C%0A%20%20%20%20%20%20%20%20obstacle_list%3Dlist%2C%0A%20%20%20%20%20%20%20%20joint_limits%3D%5B(-np.pi%2C%20np.pi)%5D%20*%206%2C%0A%20%20%20%20)%0A%20%20%20%20q_start%20%3D%20robot.q%0A%20%20%20%20np.random.seed(42)%0A%20%20%20%20rrt.find_path(start_config%3Dq_start%2C%20goal%3Dgoal.A%2C%20goal_is_pose%3DTrue)%0A%20%20%20%20rrt.print_results()%0A%0A%20%20%20%20if%20rrt.path%3A%0A%20%20%20%20%20%20%20%20for%20config%20in%20rrt.path%3A%0A%20%20%20%20%20%20%20%20%20%20%20%20robot.q%20%3D%20config%0A%20%20%20%20%20%20%20%20%20%20%20%20env.step(0.1)%20%20%23%20Step%20the%20simulator%0A%20%20%20%20else%3A%0A%20%20%20%20%20%20%20%20print(%22No%20path%20found%2C%20cannot%20simulate.%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
eadd2a8b8f5d55defdb241656da3140c11ebfbe71c07057a99a669bf562435e4