Loading...
Searching...
No Matches

Public Member Functions

 __init__ (self, str robot_name, int robot_dimension, Optional[int] port=None)
 reset (self)
 set_camera (self, position, target)
 load_mbm_environment (self, Dict[str, Any] problem_data, List[str] ignore_names=[], color=(0.8, 0.4, 0.2, 0.7), float padding=0.0)
 add_point_cloud (self, np.ndarray points, Optional[np.ndarray] color=None, float point_size=0.01)
 add_sphere (self, np.ndarray position, float radius, color=(1, 0, 0, 0.75), Optional[str] name=None)
 add_box (self, np.ndarray position, List[float] half_extents, Optional[np.ndarray] rotation_matrix=None, color=(0.8, 0.4, 0.2, 0.75), Optional[str] name=None)
 add_cylinder (self, np.ndarray position, float radius, float length, Optional[np.ndarray] rotation_matrix=None, color=(0.8, 0.4, 0.2, 0.75), Optional[str] name=None)
 add_grid (self, float width=2.0, float height=2.0, float cell_size=0.1)
 add_point_cloud (self, np.ndarray points, Optional[np.ndarray] color=None, float point_size=0.01)
 visualize_trajectory (self, np.ndarray trajectory)
 visualization_step (self)
 visualization_loop (self)
 play_once (self, dt=0.1)
 play_until_key_pressed (self, key="any", dt=0.1)

Public Attributes

str robot_name = robot_name
 dimension = robot_dimension
dict joint_mapping = self.JOINT_MAPPINGS.get(robot_name, None)
 server = viser.ViserServer(port=port)
 robot_urdf
 urdf_vis

Static Public Attributes

dict JOINT_MAPPINGS

Protected Member Functions

str _generate_name (self, str prefix)
np.ndarray _rotation_to_wxyz (self, np.ndarray rotation_matrix)
None _update_robot_config (self, int trajectory_idx, float gripper_dof=0.0)
List[float] _map_plan_config_to_urdf (self, List[float] plan_config)

Protected Attributes

 _trajectory = None
 _slider = None
 _playing = None
 _start_time = None

Detailed Description

Definition at line 18 of file viser_visualizer.py.

Constructor & Destructor Documentation

◆ __init__()

viser_visualizer.viser_visualizer.ViserVisualizer.__init__ ( self,
str robot_name,
int robot_dimension,
Optional[int] port = None )
Initialize the visualizer

Args:
    robot_name: Name of the robot
    robot_dimension: Number of degrees of freedom for the robot
    port: Optional port number for viser server (default: 8080)

Definition at line 33 of file viser_visualizer.py.

Member Function Documentation

◆ _generate_name()

str viser_visualizer.viser_visualizer.ViserVisualizer._generate_name ( self,
str prefix )
protected
Generate a unique name based on existing objects with the same prefix

Args:
    prefix: Name prefix (e.g., '/sphere_', '/box_', '/cylinder_')

Returns:
    Unique name with numeric suffix

Definition at line 225 of file viser_visualizer.py.

◆ _map_plan_config_to_urdf()

List[float] viser_visualizer.viser_visualizer.ViserVisualizer._map_plan_config_to_urdf ( self,
List[float] plan_config )
protected
Map planning configuration to full URDF configuration

For robots like fetch that use a subset of joints in planning, this method
maps the planning DOFs to the correct positions in the full URDF configuration.

Args:
    plan_config: Configuration from planner (e.g., 9 DOFs for fetch)

Returns:
    Full URDF configuration with all joints

Definition at line 406 of file viser_visualizer.py.

◆ _rotation_to_wxyz()

np.ndarray viser_visualizer.viser_visualizer.ViserVisualizer._rotation_to_wxyz ( self,
np.ndarray rotation_matrix )
protected
Convert rotation matrix to wxyz quaternion format

Args:
    rotation_matrix: 3x3 rotation matrix

Returns:
    Quaternion in wxyz format

Definition at line 243 of file viser_visualizer.py.

◆ _update_robot_config()

None viser_visualizer.viser_visualizer.ViserVisualizer._update_robot_config ( self,
int trajectory_idx,
float gripper_dof = 0.0 )
protected
Update the robot configuration in the visualization

Args:
    trajectory_idx: Index into the current trajectory
    gripper_dof: Value for extra gripper DOF if needed (default: 0.0)

Definition at line 257 of file viser_visualizer.py.

◆ add_box()

viser_visualizer.viser_visualizer.ViserVisualizer.add_box ( self,
np.ndarray position,
List[float] half_extents,
Optional[np.ndarray] rotation_matrix = None,
color = (0.8, 0.4, 0.2, 0.75),
Optional[str] name = None )
Add a box obstacle to the scene

Args:
    position: 3D position [x, y, z]
    half_extents: Half extents [x, y, z] (full size will be 2x these values)
    rotation_matrix: 3x3 rotation matrix (identity if None)
    color: RGBA color tuple (0-1 range)
    name: Optional name for the box (auto-generated if not provided)

Definition at line 301 of file viser_visualizer.py.

◆ add_cylinder()

viser_visualizer.viser_visualizer.ViserVisualizer.add_cylinder ( self,
np.ndarray position,
float radius,
float length,
Optional[np.ndarray] rotation_matrix = None,
color = (0.8, 0.4, 0.2, 0.75),
Optional[str] name = None )
Add a cylinder obstacle to the scene

Args:
    position: 3D position [x, y, z]
    radius: Cylinder radius
    length: Cylinder length (height)
    rotation_matrix: 3x3 rotation matrix (identity if None)
    color: RGBA color tuple (0-1 range)
    name: Optional name for the cylinder (auto-generated if not provided)

Definition at line 338 of file viser_visualizer.py.

◆ add_grid()

viser_visualizer.viser_visualizer.ViserVisualizer.add_grid ( self,
float width = 2.0,
float height = 2.0,
float cell_size = 0.1 )
Add a grid to the scene

Args:
    width: Grid width
    height: Grid height
    cell_size: Size of each grid cell

Definition at line 375 of file viser_visualizer.py.

◆ add_point_cloud() [1/2]

viser_visualizer.viser_visualizer.ViserVisualizer.add_point_cloud ( self,
np.ndarray points,
Optional[np.ndarray] color = None,
float point_size = 0.01 )
Add a point cloud to the scene

Args:
    points: Array of shape (N, 3) containing point coordinates
    color: Optional array of shape (N, 3) containing RGB colors for each point

Definition at line 387 of file viser_visualizer.py.

◆ add_point_cloud() [2/2]

viser_visualizer.viser_visualizer.ViserVisualizer.add_point_cloud ( self,
np.ndarray points,
Optional[np.ndarray] color = None,
float point_size = 0.01 )
Add a point cloud to the scene

Args:
    points: Array of shape (N, 3) containing point coordinates
    color: Optional array of shape (N, 3) containing RGB colors for each point

Definition at line 206 of file viser_visualizer.py.

◆ add_sphere()

viser_visualizer.viser_visualizer.ViserVisualizer.add_sphere ( self,
np.ndarray position,
float radius,
color = (1, 0, 0, 0.75),
Optional[str] name = None )
Add a sphere obstacle to the scene

Args:
    position: 3D position [x, y, z]
    radius: Sphere radius
    color: RGBA color tuple (0-1 range)
    name: Optional name for the sphere (auto-generated if not provided)

Definition at line 275 of file viser_visualizer.py.

◆ load_mbm_environment()

viser_visualizer.viser_visualizer.ViserVisualizer.load_mbm_environment ( self,
Dict[str, Any] problem_data,
List[str] ignore_names = [],
color = (0.8, 0.4, 0.2, 0.7),
float padding = 0.0 )
Load environment from MBM problem format

Args:
    problem_data: Dictionary containing 'sphere', 'cylinder', 'box' keys with obstacle data
    ignore_names: List of obstacle names to ignore
    color: RGBA color tuple (0-1 range) for obstacles
    padding: Additional padding to add to obstacle sizes

Definition at line 120 of file viser_visualizer.py.

◆ play_once()

viser_visualizer.viser_visualizer.ViserVisualizer.play_once ( self,
dt = 0.1 )
Play through the trajectory once at the specified speed

Args:
    dt: Time delay between frames in seconds (default: 0.1)

Definition at line 506 of file viser_visualizer.py.

◆ play_until_key_pressed()

viser_visualizer.viser_visualizer.ViserVisualizer.play_until_key_pressed ( self,
key = "any",
dt = 0.1 )
Play visualization until specified key is pressed

Args:
    key: Key to wait for. Use 'any' to stop on any key press (default: 'any')
    dt: Time delay between frames in seconds (default: 0.1)

Returns:
    str: The key that was pressed

Definition at line 522 of file viser_visualizer.py.

◆ reset()

viser_visualizer.viser_visualizer.ViserVisualizer.reset ( self)
Reset the entire scene including the robot

Definition at line 97 of file viser_visualizer.py.

◆ set_camera()

viser_visualizer.viser_visualizer.ViserVisualizer.set_camera ( self,
position,
target )
Set the camera position and target

Args:
    position: Camera position as array [x, y, z]
    target: Camera look-at target as array [x, y, z]

Definition at line 110 of file viser_visualizer.py.

◆ visualization_loop()

viser_visualizer.viser_visualizer.ViserVisualizer.visualization_loop ( self)
Visualization loop

Definition at line 496 of file viser_visualizer.py.

◆ visualization_step()

viser_visualizer.viser_visualizer.ViserVisualizer.visualization_step ( self)
Perform one step of the visualization update

Definition at line 480 of file viser_visualizer.py.

◆ visualize_trajectory()

viser_visualizer.viser_visualizer.ViserVisualizer.visualize_trajectory ( self,
np.ndarray trajectory )
Visualize a robot trajectory with interactive controls

Args:
    trajectory: Array of shape (timesteps, joints) containing joint configurations

Definition at line 450 of file viser_visualizer.py.

Member Data Documentation

◆ _playing

viser_visualizer.viser_visualizer.ViserVisualizer._playing = None
protected

Definition at line 95 of file viser_visualizer.py.

◆ _slider

viser_visualizer.viser_visualizer.ViserVisualizer._slider = None
protected

Definition at line 94 of file viser_visualizer.py.

◆ _start_time

viser_visualizer.viser_visualizer.ViserVisualizer._start_time = None
protected

Definition at line 108 of file viser_visualizer.py.

◆ _trajectory

viser_visualizer.viser_visualizer.ViserVisualizer._trajectory = None
protected

Definition at line 93 of file viser_visualizer.py.

◆ dimension

viser_visualizer.viser_visualizer.ViserVisualizer.dimension = robot_dimension

Definition at line 45 of file viser_visualizer.py.

◆ joint_mapping

viser_visualizer.viser_visualizer.ViserVisualizer.joint_mapping = self.JOINT_MAPPINGS.get(robot_name, None)

Definition at line 48 of file viser_visualizer.py.

◆ JOINT_MAPPINGS

dict viser_visualizer.viser_visualizer.ViserVisualizer.JOINT_MAPPINGS
static
Initial value:
= {
"fetch": [
"torso_lift_joint",
"shoulder_pan_joint",
"shoulder_lift_joint",
"upperarm_roll_joint",
"elbow_flex_joint",
"forearm_roll_joint",
"wrist_flex_joint",
"wrist_roll_joint",
]
}

Definition at line 20 of file viser_visualizer.py.

◆ robot_name

str viser_visualizer.viser_visualizer.ViserVisualizer.robot_name = robot_name

Definition at line 44 of file viser_visualizer.py.

◆ robot_urdf

viser_visualizer.viser_visualizer.ViserVisualizer.robot_urdf
Initial value:
= yourdfpy.URDF.load(
str(ur5_urdf_file),
filename_handler=package_aware_handler,
mesh_dir=mesh_dir,
load_meshes=True,
load_collision_meshes=True,
)

Definition at line 73 of file viser_visualizer.py.

◆ server

viser_visualizer.viser_visualizer.ViserVisualizer.server = viser.ViserServer(port=port)

Definition at line 51 of file viser_visualizer.py.

◆ urdf_vis

viser_visualizer.viser_visualizer.ViserVisualizer.urdf_vis
Initial value:
= ViserUrdf(
self.server, self.robot_urdf, root_node_name=f"/{robot_name}"
)

Definition at line 89 of file viser_visualizer.py.


The documentation for this class was generated from the following file: