src.utils package

Subpackages

• src.utils.device_runtime package
  • Submodules
  • src.utils.device_runtime.devices module
    • DeviceState
      • DeviceState.clutch_button
      • DeviceState.gripper_button
      • DeviceState.button
      • DeviceState.position
      • DeviceState.joints
      • DeviceState.gimbals
      • DeviceState.force
      • DeviceState.button
      • DeviceState.clutch_button
      • DeviceState.force
      • DeviceState.gimbals
      • DeviceState.gripper_button
      • DeviceState.joints
      • DeviceState.position
    • make_state_callback()
    • setup_device()
    • setup_devices()
    • state_to_q()
  • src.utils.device_runtime.runners module
    • run_with_dual_devices()
    • run_with_single_device()
  • Module contents
    • DeviceState
      • DeviceState.clutch_button
      • DeviceState.gripper_button
      • DeviceState.button
      • DeviceState.position
      • DeviceState.joints
      • DeviceState.gimbals
      • DeviceState.force
      • DeviceState.button
      • DeviceState.clutch_button
      • DeviceState.force
      • DeviceState.gimbals
      • DeviceState.gripper_button
      • DeviceState.joints
      • DeviceState.position
    • make_state_callback()
    • run_with_dual_devices()
    • run_with_single_device()
    • setup_device()
    • setup_devices()
    • state_to_q()
• src.utils.visualization package
  • Submodules
  • src.utils.visualization.camera module
    • EcmControl
      • EcmControl.q
      • EcmControl.yaw_idx
      • EcmControl.pitch_idx
      • EcmControl.insertion_idx
      • EcmControl.yaw_step_rad
      • EcmControl.pitch_step_rad
      • EcmControl.insertion_step_m
      • EcmControl.insertion_idx
      • EcmControl.insertion_step_m
      • EcmControl.pitch_idx
      • EcmControl.pitch_step_rad
      • EcmControl.q
      • EcmControl.yaw_idx
      • EcmControl.yaw_step_rad
    • build_arg_parser()
    • build_ecm_control_from_camera_pose()
    • create_camera_plotter()
    • find_joint_indices()
    • register_keys()
    • update_cameras()
  • src.utils.visualization.helpers module
    • add_world_floor_and_object()
    • apply_camera_pose()
    • hfov_to_vfov_deg()
  • src.utils.visualization.robot_points module
    • create_point_poly()
    • tool_position_world()
    • update_point_poly()
  • src.utils.visualization.teleop_common module
    • ClutchEvent
      • ClutchEvent.NONE
      • ClutchEvent.PRESSED
      • ClutchEvent.RELEASED
      • ClutchEvent.NONE
      • ClutchEvent.PRESSED
      • ClutchEvent.RELEASED
    • build_dual_phantom_base_transforms()
    • build_dual_psm_base_transforms()
    • camera_pose_from_psm_tools()
    • compute_desired_pose_world()
    • compute_desired_position_world()
    • set_robot_mesh_color()
    • update_clutch_state()
    • update_jaw_command()
  • src.utils.visualization.viewers module
    • DvrkRealtimeViz
      • DvrkRealtimeViz.add_canvas()
      • DvrkRealtimeViz.add_canvas_waypoints()
      • DvrkRealtimeViz.add_fk_marker()
      • DvrkRealtimeViz.add_polyline_mesh()
      • DvrkRealtimeViz.add_robot()
      • DvrkRealtimeViz.add_waypoint_highlight_marker()
      • DvrkRealtimeViz.hide_canvas_waypoint()
      • DvrkRealtimeViz.make_line()
      • DvrkRealtimeViz.reset_camera()
      • DvrkRealtimeViz.reset_canvas_waypoints()
      • DvrkRealtimeViz.set_camera()
      • DvrkRealtimeViz.show()
      • DvrkRealtimeViz.update_robot()
      • DvrkRealtimeViz.update_waypoint_highlight()
    • PlotterRobotMeshState
      • PlotterRobotMeshState.base_vertices_by_key
      • PlotterRobotMeshState.poly_by_key
      • PlotterRobotMeshState.robot
    • add_robot_meshes_to_plotter()
    • demo_two_robots()
    • set_camera_view()
    • update_robot_meshes_on_plotter()
    • visualize()
  • Module contents
    • ClutchEvent
      • ClutchEvent.NONE
      • ClutchEvent.PRESSED
      • ClutchEvent.RELEASED
      • ClutchEvent.NONE
      • ClutchEvent.PRESSED
      • ClutchEvent.RELEASED
    • DvrkRealtimeViz
      • DvrkRealtimeViz.add_canvas()
      • DvrkRealtimeViz.add_canvas_waypoints()
      • DvrkRealtimeViz.add_fk_marker()
      • DvrkRealtimeViz.add_polyline_mesh()
      • DvrkRealtimeViz.add_robot()
      • DvrkRealtimeViz.add_waypoint_highlight_marker()
      • DvrkRealtimeViz.hide_canvas_waypoint()
      • DvrkRealtimeViz.make_line()
      • DvrkRealtimeViz.plotter
      • DvrkRealtimeViz.reset_camera()
      • DvrkRealtimeViz.reset_canvas_waypoints()
      • DvrkRealtimeViz.set_camera()
      • DvrkRealtimeViz.show()
      • DvrkRealtimeViz.update_robot()
      • DvrkRealtimeViz.update_waypoint_highlight()
    • EcmControl
      • EcmControl.q
      • EcmControl.yaw_idx
      • EcmControl.pitch_idx
      • EcmControl.insertion_idx
      • EcmControl.yaw_step_rad
      • EcmControl.pitch_step_rad
      • EcmControl.insertion_step_m
      • EcmControl.insertion_idx
      • EcmControl.insertion_step_m
      • EcmControl.pitch_idx
      • EcmControl.pitch_step_rad
      • EcmControl.q
      • EcmControl.yaw_idx
      • EcmControl.yaw_step_rad
    • PlotterRobotMeshState
      • PlotterRobotMeshState.base_vertices_by_key
      • PlotterRobotMeshState.poly_by_key
      • PlotterRobotMeshState.robot
    • add_robot_meshes_to_plotter()
    • add_world_floor_and_object()
    • apply_camera_pose()
    • build_arg_parser()
    • build_dual_phantom_base_transforms()
    • build_dual_psm_base_transforms()
    • build_ecm_control_from_camera_pose()
    • camera_pose_from_psm_tools()
    • compute_desired_pose_world()
    • compute_desired_position_world()
    • create_camera_plotter()
    • create_point_poly()
    • demo_two_robots()
    • find_joint_indices()
    • hfov_to_vfov_deg()
    • register_keys()
    • set_camera_view()
    • set_robot_mesh_color()
    • tool_position_world()
    • update_cameras()
    • update_clutch_state()
    • update_jaw_command()
    • update_point_poly()
    • update_robot_meshes_on_plotter()
    • visualize()

Submodules

src.utils.transforms module

src.utils.transforms.inv_transform(T)

Compute the closed-form inverse of a homogeneous transform.

\[\begin{split}T^{-1} = \begin{bmatrix} R^\top & -R^\top p \\ 0 & 1 \end{bmatrix}\end{split}\]

Parameters:

T (ndarray) – 4×4 homogeneous transform \(T \in SE(3)\).

Return type:

ndarray

Returns:

4×4 inverse \(T^{-1} \in SE(3)\).

src.utils.transforms.make_base_transform(y_offset)

Return a pure Y-axis translation as a 4×4 homogeneous transform.

\[\begin{split}T = \begin{bmatrix} I_3 & [0, y, 0]^\top \\ 0 & 1 \end{bmatrix}\end{split}\]

Parameters:

y_offset (float) – Translation along the Y axis (metres).

Return type:

ndarray

Returns:

4×4 homogeneous transform with identity rotation.

src.utils.transforms.make_transform(R, p)

Assemble a 4×4 homogeneous transform from a rotation matrix and translation.

\[\begin{split}T = \begin{bmatrix} R & p \\ 0 & 1 \end{bmatrix}\end{split}\]

Parameters:

• R (ndarray) – 3×3 rotation matrix \(R \in SO(3)\).

• p (ndarray) – 3-vector translation.

Return type:

ndarray

Returns:

4×4 homogeneous transform \(T \in SE(3)\).

src.utils.transforms.normalize_vector(v, eps=1e-12)

Return the unit vector of v, or the zero vector when v is near-zero.

\[\begin{split}\hat{v} = \begin{cases} v / \|v\| & \|v\| \ge \varepsilon \\ 0 & \text{otherwise} \end{cases}\end{split}\]

Parameters:

• v (ndarray) – Input vector of arbitrary shape.

• eps (float) – Threshold below which v is treated as zero.

Return type:

ndarray

Returns:

Unit vector with the same shape as v, or zeros.

src.utils.transforms.project_to_canvas_point(p_world)

Project a world-frame point onto the 2-D visualisation canvas.

Zeroes out the X component and shifts Y by +2 m to match the canvas coordinate origin used in the tracking scripts:

\[p_{\text{canvas}} = [0,\; p_y + 2,\; p_z]^\top\]

Parameters:

p_world (ndarray) – 3-vector in the world frame.

Return type:

ndarray

Returns:

3-vector in the canvas frame (X zeroed, Y shifted).

src.utils.transforms.scaled_relative_transform(T_ref, T_cur, translation_gain=1.0)

Return the relative transform from T_ref to T_cur with scaled translation.

\[\begin{split}T_{\Delta} = T_{\text{ref}}^{-1}\, T_{\text{cur}}, \qquad T_{\text{scaled}} = \begin{bmatrix} R_\Delta & k\, p_\Delta \\ 0 & 1 \end{bmatrix}\end{split}\]

Parameters:

• T_ref (ndarray) – 4×4 reference transform.

• T_cur (ndarray) – 4×4 current transform.

• translation_gain (float) – Scalar gain \(k\) applied to the relative translation (default 1.0).

Return type:

ndarray

Returns:

4×4 scaled relative transform.

src.utils.transforms.tool_transform_world(robot, theta=None, base_transform=None)

Compute the tool-link pose in the world frame.

\[\begin{split}T_{\text{tool}}^{\text{world}} = \begin{cases} T_{\text{base}}^{\text{world}} \cdot T_{\text{FK}}(\theta) & \text{if base_transform is given} \\ T_{\text{FK}}(\theta) & \text{otherwise} \end{cases}\end{split}\]

Parameters:

• robot (ToolKinematicsRobotLike) – Robot object implementing ToolKinematicsRobotLike.

• theta (Optional[ndarray]) – (dof,) joint configuration. Defaults to the robot’s stored state when None.

• base_transform (Optional[ndarray]) – Optional 4×4 base-to-world transform.

Return type:

ndarray

Returns:

4×4 tool-link pose in the world frame.

src.utils.urdf_parser module

src.utils.urdf_parser.parse_urdf(robot, urdf_path)

Parse a URDF file and populate robot with links, joints, and visuals.

Iterates over the top-level <link> and <joint> elements and calls the corresponding robot.add_link, robot.add_joint, and robot.add_visual methods. The following sub-elements are handled:

• <visual><origin> and <visual><geometry><mesh> for visual mesh registration.

• <joint> attributes: type, <parent>, <child>, <origin>, <axis>, <limit>, <mimic>.

Mesh paths are resolved via robot.resolve_mesh_path and STL files are preferred over DAE where available.

Note

This function does not set active_joint_names; that responsibility lies with the robot subclass (e.g. UrdfRobot).

Parameters:

• robot (Any) – Partially-constructed robot object exposing add_link, add_joint, add_visual, and resolve_mesh_path.

• urdf_path (str | Path) – Path to the .urdf file to parse.

Return type:

None

Module contents

Utility package namespace.

Keep package import lightweight by exporting module names only. Import concrete symbols from their submodules.

Example:

from src.utils.transforms import inv_transform