diff --git a/scripts/viser_utils.py b/scripts/viser_utils.py index 8a201602..90111fa1 100644 --- a/scripts/viser_utils.py +++ b/scripts/viser_utils.py @@ -11,10 +11,10 @@ def setup_viser_with_robot(robot_dir, robot_urdf_name): robot = ViserUrdf( server, urdf, - load_meshes=True, - load_collision_meshes=False, - root_node_name="/robot", - ) + load_meshes = True, + load_collision_meshes = False, + root_node_name = "/robot", + ) return server, robot @@ -25,7 +25,7 @@ def add_spheres( sphere_radii: Sequence, colors: Union[Sequence[int], Sequence[Sequence[int]]] = [], prefix: str = "my_sphere", -): + ): """ Add spheres to the env/ Sphere positions are (N,3) and sphere radii are (N) @@ -39,11 +39,11 @@ def add_spheres( assert len(colors) == len(sphere_positions) for i, (sphere_pos, sphere_rad) in enumerate(zip(sphere_positions, sphere_radii)): sphere_handles[i] = server.scene.add_icosphere( - name=f"{prefix}_{i}", - radius=sphere_rad, - position=tuple(sphere_pos[:3]), - color=tuple(colors[i]), - ) + name = f"{prefix}_{i}", + radius = sphere_rad, + position = tuple(sphere_pos[:3]), + color = tuple(colors[i]), + ) return sphere_handles @@ -67,8 +67,8 @@ def add_trajectory(server, waypoints, robot, attachment_handles, attachment_posi return assert len(attachment_handles) == len(attachment_positions[0]) traj_slider = server.gui.add_slider( - "Current Waypoint", min=0, max=len(waypoints) - 1, step=1, initial_value=0 - ) + "Current Waypoint", min = 0, max = len(waypoints) - 1, step = 1, initial_value = 0 + ) @traj_slider.on_update def update_robot_pose(event): diff --git a/scripts/visualize_viser.py b/scripts/visualize_viser.py index 4698f0f3..c3bb21ce 100644 --- a/scripts/visualize_viser.py +++ b/scripts/visualize_viser.py @@ -7,14 +7,12 @@ import vamp from fire import Fire - # Starting configuration a = [0.0, -0.785, 0.0, -2.356, 0.0, 1.571, 0.785] # Goal configuration b = [2.35, 1.0, 0.0, -0.8, 0, 2.5, 0.785] - # Problem specification: a list of sphere centers problem = [ [0.55, 0, 0.25], @@ -31,7 +29,7 @@ [-0.35, -0.35, 0.8], [0, -0.55, 0.8], [0.35, -0.35, 0.8], -] + ] def main( @@ -40,11 +38,10 @@ def main( attachment_offset: float = 0.02, planner: str = "rrtc", **kwargs, -): + ): - (vamp_module, planner_func, plan_settings, simp_settings) = ( - vamp.configure_robot_and_planner_with_kwargs("panda", planner, **kwargs) - ) + (vamp_module, planner_func, plan_settings, + simp_settings) = (vamp.configure_robot_and_planner_with_kwargs("panda", planner, **kwargs)) # Create an attachment offset on the Z-axis from the end-effector frame tf = np.identity(4) @@ -64,14 +61,14 @@ def main( _problem_sphere_handles = add_spheres( server, np.array(problem), np.array([obstacle_radius] * len(problem)) - ) + ) # Add the attchment to the VAMP environment e.attach(attachment) # Add attachment sphere to visualization attachment_sph = add_spheres( - server, np.zeros((1, 3)), np.array([attachment_radius]), colors=[[0, 255, 0]] - ) + server, np.zeros((1, 3)), np.array([attachment_radius]), colors = [[0, 255, 0]] + ) # Update attachment sphere positions corresponding to the waypoints. # this could also be made into a callable that can be called during trajectory viz @@ -87,9 +84,7 @@ def get_attachment_pos(configuration): attachment_positions = [get_attachment_pos(pos) for pos in simple.path.numpy()] - add_trajectory( - server, simple.path.numpy(), robot, attachment_sph, attachment_positions - ) + add_trajectory(server, simple.path.numpy(), robot, attachment_sph, attachment_positions) # display while True: