pyrcf.components.ctrl_loop_debuggers ==================================== .. py:module:: pyrcf.components.ctrl_loop_debuggers .. autoapi-nested-parse:: Module containing interface and definitions of control loop debuggers that can be used in the control loop. Submodules ---------- .. toctree:: :maxdepth: 1 /autoapi/pyrcf/components/ctrl_loop_debuggers/ctrl_loop_data_publisher_base/index /autoapi/pyrcf/components/ctrl_loop_debuggers/ctrl_loop_debugger_base/index /autoapi/pyrcf/components/ctrl_loop_debuggers/data_publish_debuggers/index /autoapi/pyrcf/components/ctrl_loop_debuggers/data_recorder_debugger/index /autoapi/pyrcf/components/ctrl_loop_debuggers/pybullet_robot_plan_debugger/index /autoapi/pyrcf/components/ctrl_loop_debuggers/pybullet_robot_state_debugger/index Classes ------- .. autoapisummary:: pyrcf.components.ctrl_loop_debuggers.CtrlLoopDebuggerBase pyrcf.components.ctrl_loop_debuggers.DummyDebugger pyrcf.components.ctrl_loop_debuggers.ComponentDataRecorderDebugger pyrcf.components.ctrl_loop_debuggers.ComponentDataPublisherDebugger pyrcf.components.ctrl_loop_debuggers.PlotjugglerLoopDebugger pyrcf.components.ctrl_loop_debuggers.PybulletRobotPlanDebugger pyrcf.components.ctrl_loop_debuggers.PybulletRobotStateDebugger Package Contents ---------------- .. py:class:: CtrlLoopDebuggerBase(rate: float = None, clock: pyrcf.utils.time_utils.ClockBase = PythonPerfClock()) Bases: :py:obj:`abc.ABC` Base class for control loop debuggers. Can only be used with control loops. .. py:attribute:: _rate :value: None .. py:method:: _should_run() .. py:method:: run_once(t: float, dt: float, robot_state: pyrcf.core.types.RobotState, global_plan: pyrcf.core.types.GlobalMotionPlan, agent_outputs: List[Tuple[pyrcf.core.types.LocalMotionPlan, pyrcf.core.types.RobotCmd]], robot_cmd: pyrcf.core.types.RobotCmd) The main method that will be run in every control loop. NOTE: SHOULD NOT BE OVERRIDDEN. Override the `_run_once_impl` method. This method automatically checks if it is ok to trigger the implemented child method, and calls it if appropriate. :param t: Current time in the control loop. :type t: float :param dt: The current dt in the control loop. :type dt: float :param robot_state: robot state after update from the state estimator in the control loop. :type robot_state: RobotState :param global_plan: The global plan output produced by the global planner. :type global_plan: GlobalMotionPlan :param agent_outputs: The list of outputs from all agents in the control loop. :type agent_outputs: List[Tuple[LocalMotionPlan, RobotCmd]] :param robot_cmd: The final command written to the robot. :type robot_cmd: RobotCmd .. py:method:: _run_once_impl(t: float, dt: float, robot_state: pyrcf.core.types.RobotState, global_plan: pyrcf.core.types.GlobalMotionPlan, agent_outputs: List[Tuple[pyrcf.core.types.LocalMotionPlan, pyrcf.core.types.RobotCmd]], robot_cmd: pyrcf.core.types.RobotCmd) :abstractmethod: The main method that will be run in every control loop. Should be overridden in the implemented child of `CtrlLoopDebuggerBase`. This method will be called automatically if the timer is triggered, as long as the main `run_once()` method is called continuously in the control loop. :param t: Current time in the control loop. :type t: float :param dt: The current dt in the control loop. :type dt: float :param robot_state: robot state after update from the state estimator in the control loop. :type robot_state: RobotState :param global_plan: The global plan output produced by the global planner. :type global_plan: GlobalMotionPlan :param agent_outputs: The list of outputs from all agents in the control loop. :type agent_outputs: List[Tuple[LocalMotionPlan, RobotCmd]] :param robot_cmd: The final command written to the robot. :type robot_cmd: RobotCmd .. py:method:: shutdown() Cleanly shutdown the debugger. Override in child class if required. The base class implements an empty function. .. py:class:: DummyDebugger(squawk: bool = True, rate: float = 10, clock: pyrcf.utils.time_utils.ClockBase = PythonPerfClock()) Bases: :py:obj:`CtrlLoopDebuggerBase` A dummy debugger to test in pipeline. .. py:attribute:: _squawk :value: True .. py:method:: _run_once_impl(t: float, dt: float, robot_state: pyrcf.core.types.RobotState, global_plan: pyrcf.core.types.GlobalMotionPlan, agent_outputs: List[Tuple[pyrcf.core.types.LocalMotionPlan | pyrcf.core.types.RobotCmd]], robot_cmd: pyrcf.core.types.RobotCmd) The main method that will be run in every control loop. Should be overridden in the implemented child of `CtrlLoopDebuggerBase`. This method will be called automatically if the timer is triggered, as long as the main `run_once()` method is called continuously in the control loop. :param t: Current time in the control loop. :type t: float :param dt: The current dt in the control loop. :type dt: float :param robot_state: robot state after update from the state estimator in the control loop. :type robot_state: RobotState :param global_plan: The global plan output produced by the global planner. :type global_plan: GlobalMotionPlan :param agent_outputs: The list of outputs from all agents in the control loop. :type agent_outputs: List[Tuple[LocalMotionPlan, RobotCmd]] :param robot_cmd: The final command written to the robot. :type robot_cmd: RobotCmd .. py:class:: ComponentDataRecorderDebugger(file_name: str, rate: float = None, clock: pyrcf.utils.time_utils.ClockBase = PythonPerfClock(), extra_data_callables: Callable[[], Any] | List[Callable[[], Any]] = None, buffer_size: int = 50) Bases: :py:obj:`pyrcf.components.ctrl_loop_debuggers.ctrl_loop_debugger_base.CtrlLoopDebuggerBase` A data recording debugger that records all the control loop data directly to a file. Use `ComponentDataRecorderDataParser` from `pyrcf.utils.data_io_utils` to parse the recorded data file. NOTE: if your computer has enough RAM and compute power, this debugger is better (in terms of keeping desired control loop rate) than `ComponentDataPublisherDebugger`. .. py:attribute:: _file .. py:attribute:: _additional_handles :value: [] .. py:attribute:: _buffer :value: [] .. py:attribute:: _buffer_size :value: 50 .. py:method:: _run_once_impl(t: float, dt: float, robot_state: pyrcf.core.types.RobotState, global_plan: pyrcf.core.types.GlobalMotionPlan, agent_outputs: List[Tuple[pyrcf.core.types.LocalMotionPlan, pyrcf.core.types.RobotCmd]], robot_cmd: pyrcf.core.types.RobotCmd) The main method that will be run in every control loop. Should be overridden in the implemented child of `CtrlLoopDebuggerBase`. This method will be called automatically if the timer is triggered, as long as the main `run_once()` method is called continuously in the control loop. :param t: Current time in the control loop. :type t: float :param dt: The current dt in the control loop. :type dt: float :param robot_state: robot state after update from the state estimator in the control loop. :type robot_state: RobotState :param global_plan: The global plan output produced by the global planner. :type global_plan: GlobalMotionPlan :param agent_outputs: The list of outputs from all agents in the control loop. :type agent_outputs: List[Tuple[LocalMotionPlan, RobotCmd]] :param robot_cmd: The final command written to the robot. :type robot_cmd: RobotCmd .. py:method:: shutdown() Cleanly shutdown the debugger. Override in child class if required. The base class implements an empty function. .. py:class:: ComponentDataPublisherDebugger(rate: float = None, clock: pyrcf.utils.time_utils.ClockBase = PythonPerfClock(), debug_publish_callables: Callable[[], Any] | List[Callable[[], Any]] = None, port: int = DEFAULT_ZMQ_PUBLISH_PORT) Bases: :py:obj:`pyrcf.components.ctrl_loop_debuggers.ctrl_loop_data_publisher_base.CtrlLoopDataPublisherBase` This control loop debugger will stream the data from all the components in the control loop so that it can be subscribed to by a zmq subscriber (see `PyRCFSubscriberZMQ`). .. py:class:: PlotjugglerLoopDebugger(rate: float = None, clock: pyrcf.utils.time_utils.ClockBase = PythonPerfClock(), debug_publish_callables: Callable[[], Any] | List[Callable[[], Any]] = None, port: int = DEFAULT_PLOTJUGGLER_PUBLISH_PORT) Bases: :py:obj:`ComponentDataPublisherDebugger` This control loop debugger will stream the data from all the components in the control loop so that it can be visualised in plotjuggler. .. py:class:: PybulletRobotPlanDebugger(urdf_path: str, rate: float = None, clock: pyrcf.utils.time_utils.ClockBase = PythonPerfClock(), cid: int = 0, enable_toggle_button: bool = True, show_ee_targets: bool = True, use_curr_pose_if_plan_unavailable: bool = True, **pybullet_debug_robot_kwargs) Bases: :py:obj:`pyrcf.components.ctrl_loop_debuggers.ctrl_loop_debugger_base.CtrlLoopDebuggerBase` This control loop debugger will create a dummy visual robot in pybullet which will try to follow position/pose plan in the control loop. This can be used to visualise ideal robot poses/configurations and to compare performance of controllers in tracking them. .. py:attribute:: _debugger_robot .. py:attribute:: _toggler_button :value: None .. py:attribute:: _show_ee_targets :value: True .. py:attribute:: _ee_frame_viz :type: Mapping[str, pyrcf.utils.gui_utils.pb_gui_utils.PybulletDebugFrameViz] .. py:attribute:: _use_curr_pose :value: True .. py:method:: _run_once_impl(t: float, dt: float, robot_state: pyrcf.core.types.RobotState, global_plan: pyrcf.core.types.GlobalMotionPlan, agent_outputs: List[Tuple[pyrcf.core.types.LocalMotionPlan, pyrcf.core.types.RobotCmd]], robot_cmd: pyrcf.core.types.RobotCmd) The main method that will be run in every control loop. Should be overridden in the implemented child of `CtrlLoopDebuggerBase`. This method will be called automatically if the timer is triggered, as long as the main `run_once()` method is called continuously in the control loop. :param t: Current time in the control loop. :type t: float :param dt: The current dt in the control loop. :type dt: float :param robot_state: robot state after update from the state estimator in the control loop. :type robot_state: RobotState :param global_plan: The global plan output produced by the global planner. :type global_plan: GlobalMotionPlan :param agent_outputs: The list of outputs from all agents in the control loop. :type agent_outputs: List[Tuple[LocalMotionPlan, RobotCmd]] :param robot_cmd: The final command written to the robot. :type robot_cmd: RobotCmd .. py:method:: shutdown() Cleanly shutdown the debugger. Override in child class if required. The base class implements an empty function. .. py:method:: fromBulletRobotInstance(robot: pyrcf.components.robot_interfaces.simulation.pybullet_robot.PybulletRobot, rate: float = None, clock: pyrcf.utils.time_utils.ClockBase = PythonPerfClock(), enable_toggle_button: bool = True, show_ee_targets: bool = True, use_curr_pose_if_plan_unavailable: bool = True, **pybullet_debug_robot_kwargs) -> PybulletRobotPlanDebugger :classmethod: Creates an instance of PybulletRobotPlanDebugger using the provided PybulletRobot child class object. :param robot: The robot instance (will not be modified). :type robot: PybulletRobot :param rate: Rate at which this should be triggered. Defaults to None (i.e. use control loop rate). :type rate: float, optional :param clock: The clock to use for timer. Defaults to PythonPerfClock(). :type clock: ClockBase, optional :param cid: The pybullet physics client ID to connect to. Defaults to 0. :type cid: int, optional :param enable_toggle_button: If set to True, this will create a button in the pybullet GUI to enable/disable the VISUALISATION of this debugger. Defaults to True. NOTE: This only affects the visualisation! Does not disable the actual debugger. NOTE: There can be an overhead when the visualisation is toggled on and off. :type enable_toggle_button: bool, optional :param \*\*pybullet_debug_robot_kwargs: Additional keyword arguments can be passed here. All keyword arguments to `PybulletDebugRobot` is allowed here (e.g. use `rgba=[,,,]` for setting the visualisation color of this debug robot) :returns: PybulletRobotPlanDebugger instance created using the urdf and cid from the passed `PybulletRobot` object. :rtype: PybulletRobotPlanDebugger .. py:class:: PybulletRobotStateDebugger(urdf_path: str, rate: float = None, clock: pyrcf.utils.time_utils.ClockBase = PythonPerfClock(), cid: int = 0, enable_toggle_button: bool = True, **bullet_debug_robot_kwargs) Bases: :py:obj:`pyrcf.components.ctrl_loop_debuggers.ctrl_loop_debugger_base.CtrlLoopDebuggerBase` This control loop debugger will create a dummy visual robot in pybullet which will simply mimic the joint states and base pose read from the `robot_state` (after passing through the state estimator update). This class can be used to visualise the robot states being read when the robot is running outside pybullet (e.g. real robot or remote). .. py:attribute:: _debugger_robot .. py:attribute:: _toggler_button :value: None .. py:method:: _run_once_impl(t: float, dt: float, robot_state: pyrcf.core.types.RobotState, global_plan: pyrcf.core.types.GlobalMotionPlan, agent_outputs: List[Tuple[pyrcf.core.types.LocalMotionPlan, pyrcf.core.types.RobotCmd]], robot_cmd: pyrcf.core.types.RobotCmd) The main method that will be run in every control loop. Should be overridden in the implemented child of `CtrlLoopDebuggerBase`. This method will be called automatically if the timer is triggered, as long as the main `run_once()` method is called continuously in the control loop. :param t: Current time in the control loop. :type t: float :param dt: The current dt in the control loop. :type dt: float :param robot_state: robot state after update from the state estimator in the control loop. :type robot_state: RobotState :param global_plan: The global plan output produced by the global planner. :type global_plan: GlobalMotionPlan :param agent_outputs: The list of outputs from all agents in the control loop. :type agent_outputs: List[Tuple[LocalMotionPlan, RobotCmd]] :param robot_cmd: The final command written to the robot. :type robot_cmd: RobotCmd .. py:method:: shutdown() Cleanly shutdown the debugger. Override in child class if required. The base class implements an empty function. .. py:method:: fromBulletRobotInstance(robot: pyrcf.components.robot_interfaces.simulation.pybullet_robot.PybulletRobot, rate: float = None, clock: pyrcf.utils.time_utils.ClockBase = PythonPerfClock(), enable_toggle_button: bool = True, **bullet_debug_robot_kwargs) -> PybulletRobotStateDebugger :classmethod: Creates an instance of PybulletRobotStateDebugger using an existing PybulletRobot child class object. :param robot: The robot instance (will not be modified). :type robot: PybulletRobot :param rate: Rate at which this should be triggered. Defaults to None (i.e. use control loop rate). :type rate: float, optional :param clock: The clock to use for timer. Defaults to PythonPerfClock(). :type clock: ClockBase, optional :param cid: The pybullet physics client ID to connect to. Defaults to 0. :type cid: int, optional :param enable_toggle_button: If set to True, this will create a button in the pybullet GUI to enable/disable the VISUALISATION of this debugger. Defaults to True. NOTE: This only affects the visualisation! Does not disable the actual debugger. NOTE: There can be an overhead when the visualisation is toggled on and off. :type enable_toggle_button: bool, optional :param \*\*bullet_debug_robot_kwargs: Additional keyword arguments can be passed here. All keyword arguments to `PybulletDebugRobot` is allowed here (e.g. use `rgba=[,,,]` for setting the visualisation color of this debug robot) :returns: PybulletRobotStateDebugger instance created using the urdf and cid from the passed `robot` object. :rtype: PybulletRobotStateDebugger