example rrt ur5

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%20P2%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.UR5%20import%20UR5%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%20RRT%2C%20SE3%2C%20UR5%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%20P2%22%22%22)%0A%20%20%20%20return%0A%0A%0A%40app.cell%0Adef%20_(UR5)%3A%0A%20%20%20%20def%20create_ur5()%3A%0A%20%20%20%20%20%20%20%20return%20UR5()%0A%20%20%20%20return%20(create_ur5%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_ur5%2C%20np%2C%20set_env%2C%20sg%2C%20swift)%3A%0A%20%20%20%20robot%20%3D%20create_ur5()%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%20rrt.find_path(%0A%20%20%20%20%20%20%20%20start_config%3Dq_start%2C%20goal%3Dgoal.A%2C%20goal_is_pose%3DTrue%2C%20max_iter%3D5000%0A%20%20%20%20)%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