irsim.lib ========= .. py:module:: irsim.lib Submodules ---------- .. toctree:: :maxdepth: 1 /api/irsim/lib/algorithm/index /api/irsim/lib/behavior/index /api/irsim/lib/handler/index /api/irsim/lib/path_planners/index Attributes ---------- .. autoapisummary:: irsim.lib.register_behavior irsim.lib.register_behavior_class irsim.lib.register_group_behavior irsim.lib.register_group_behavior_class irsim.lib.kinematics_factory Classes ------- .. autoapisummary:: irsim.lib.reciprocal_vel_obs irsim.lib.Behavior irsim.lib.GeometryFactory irsim.lib.KinematicsFactory irsim.lib.KinematicsHandler Functions --------- .. autoapisummary:: irsim.lib.generate_polygon irsim.lib.random_generate_polygon irsim.lib.ackermann_kinematics irsim.lib.differential_kinematics irsim.lib.omni_kinematics irsim.lib.register_kinematics Package Contents ---------------- .. py:function:: generate_polygon(center: list[float], avg_radius: float, irregularity: float, spikeyness: float, num_vertices: int) -> numpy.ndarray Generate a random polygon around a center point. :param center: Center of the polygon. :type center: Tuple[float, float] :param avg_radius: Average radius from the center to vertices. :type avg_radius: float :param irregularity: Variance of angle spacing between vertices. Range [0, 1] :type irregularity: float :param spikeyness: Variance of radius from the center. Range [0, 1] :type spikeyness: float :param num_vertices: Number of vertices for the polygon. :type num_vertices: int :returns: Vertices of the polygon in CCW order. :rtype: numpy.ndarray .. py:function:: random_generate_polygon(number: int = 1, center_range: list[float] | None = None, avg_radius_range: list[float] | None = None, irregularity_range: list[float] | None = None, spikeyness_range: list[float] | None = None, num_vertices_range: list[int] | None = None, **kwargs: Any) -> numpy.ndarray | list[numpy.ndarray] reference: https://stackoverflow.com/questions/8997099/algorithm-to-generate-random-2d-polygon Generate random polygons with specified properties. :param number: Number of polygons to generate (default 1). :type number: int :param center_range: Range for the polygon center [min_x, min_y, max_x, max_y]. :type center_range: List[float] :param avg_radius_range: Range for the average radius of the polygons. :type avg_radius_range: List[float] :param irregularity_range: Range for the irregularity of the polygons. :type irregularity_range: List[float] :param spikeyness_range: Range for the spikeyness of the polygons. :type spikeyness_range: List[float] :param num_vertices_range: Range for the number of vertices of the polygons. :type num_vertices_range: List[int] :returns: List of vertices for each polygon or a single polygon's vertices if number=1. .. py:function:: ackermann_kinematics(state: numpy.ndarray, velocity: numpy.ndarray, step_time: float, noise: bool = False, alpha: list[float] | None = None, mode: str = 'steer', wheelbase: float = 1) -> numpy.ndarray Calculate the next state for an Ackermann steering vehicle. :param state: A 4x1 vector [x, y, theta, steer_angle] representing the current state. :param velocity: A 2x1 vector representing the current velocities, format depends on mode. For "steer" mode, [linear, steer_angle] is expected. For "angular" mode, [linear, angular] is expected. :param step_time: The time step for the simulation. :param noise: Boolean indicating whether to add noise to the velocity (default False). :param alpha: List of noise parameters for the velocity model (default [0.03, 0, 0, 0.03]). alpha[0] and alpha[1] are for linear velocity, alpha[2] and alpha[3] are for angular velocity. :param mode: The kinematic mode, either "steer" or "angular" (default "steer"). :param wheelbase: The distance between the front and rear axles (default 1). :returns: A 4x1 vector representing the next state. :rtype: new_state .. py:function:: differential_kinematics(state: numpy.ndarray, velocity: numpy.ndarray, step_time: float, noise: bool = False, alpha: list[float] | None = None) -> numpy.ndarray Calculate the next state for a differential wheel robot. :param state: A 3x1 vector [x, y, theta] representing the current position and orientation. :param velocity: A 2x1 vector [linear, angular] representing the current velocities. :param step_time: The time step for the simulation. :param noise: Boolean indicating whether to add noise to the velocity (default False). :param alpha: List of noise parameters for the velocity model (default [0.03, 0, 0, 0.03]). alpha[0] and alpha[1] are for linear velocity, alpha[2] and alpha[3] are for angular velocity. :returns: A 3x1 vector [x, y, theta] representing the next state. :rtype: next_state .. py:function:: omni_kinematics(state: numpy.ndarray, velocity: numpy.ndarray, step_time: float, noise: bool = False, alpha: list[float] | None = None) -> numpy.ndarray Calculate the next position for an omnidirectional robot. :param state: A 2x1 vector [x, y] representing the current position. :param velocity: A 2x1 vector [vx, vy] representing the current velocities. :param step_time: The time step for the simulation. :param noise: Boolean indicating whether to add noise to the velocity (default False). :param alpha: List of noise parameters for the velocity model (default [0.03, 0.03]). alpha[0] is for x velocity, alpha[1] is for y velocity. :returns: A 2x1 vector [x, y] representing the next position. :rtype: new_position .. py:class:: reciprocal_vel_obs(state: list, obs_state_list=None, vxmax=1.5, vymax=1.5, acce=0.5, factor=1.0, line_obs_list=None) A class to implement the Reciprocal Velocity Obstacle (RVO) algorithm for multi-robot collision avoidance. :param state: The rvo state of the agent [x, y, vx, vy, radius, vx_des, vy_des]. :type state: list :param obs_state_list: List of states of static obstacles [[x, y, vx, vy, radius]]. :type obs_state_list: list :param vxmax: Maximum velocity in the x direction. :type vxmax: float :param vymax: Maximum velocity in the y direction. :type vymax: float :param acce: Acceleration limit. :type acce: float :param factor: Penalty weighting factor for velocity selection. :type factor: float :param line_obs_list: List of line segments [[x1, y1, x2, y2], ...]. :type line_obs_list: list .. py:attribute:: state .. py:attribute:: obs_state_list :value: None .. py:attribute:: line_obs_list :value: None .. py:attribute:: vxmax :value: 1.5 .. py:attribute:: vymax :value: 1.5 .. py:attribute:: acce :value: 0.5 .. py:attribute:: factor :value: 1.0 .. py:method:: update(state, obs_state_list, line_obs_list=None) .. py:method:: cal_vel(mode='rvo') Calculate the velocity of the agent based on the Reciprocal Velocity Obstacle (RVO) algorithm. :param mode: The vo configure to calculate the velocity. It can be "rvo", "hrvo", or "vo". - rvo: Reciprocal Velocity Obstacle (RVO) algorithm, for multi-robot collision avoidance. - hrvo: Hybrid Reciprocal Velocity Obstacle (HRVO) algorithm, for multi-robot collision avoidance. - vo: Velocity Obstacle (VO) algorithm, for obstacle-robot collision avoidance. :type mode: str :returns: Selected velocity [vx, vy]. :rtype: list[float] .. py:method:: config_rvo() .. py:method:: config_rvo_mode(obstacle) .. py:method:: config_hrvo() .. py:method:: config_hrvo_mode(obstacle) .. py:method:: config_vo() .. py:method:: config_vo_mode(obstacle) .. py:method:: config_vo_lines() Compute VO cones for line segment obstacles. For each segment, compute the angular span as seen from the agent, expanded by asin(r / dist) on each side to account for the agent radius. The apex is [0, 0] since line obstacles are static. .. py:method:: vel_candidate(rvo_list) .. py:method:: vo_out(vx, vy, rvo_list) .. py:method:: vel_select(vo_outside, vo_inside) .. py:method:: penalty(vel, vel_des, factor) .. py:method:: between_vector(line_left_vector, line_right_vector, line_vector) :staticmethod: .. py:method:: cross_product(vector1, vector2) :staticmethod: .. py:class:: Behavior(object_info=None, behavior_dict=None) Represents the behavior of an agent in the simulation. :param object_info: Object information from the object_base class ObjectInfo. :type object_info: object :param behavior_dict: Dictionary containing behavior parameters for different behaviors. Name Options include: 'dash', 'rvo'. target_roles: - 'all': all objects in the environment will be considered within this behavior. - 'obstacle': only obstacles will be considered within this behavior. - 'robot': only robots will be considered within this behavior. :type behavior_dict: dict Initialize the behavior with object info and parameters. :param object_info: Information about the agent (from ObjectBase.ObjectInfo). :param behavior_dict: Behavior parameters; if ``None``, defaults to an empty dict. :type behavior_dict: dict | None .. py:attribute:: object_info :value: None .. py:attribute:: behavior_dict .. py:method:: gen_vel(ego_object, external_objects=None) Generate a velocity for the agent based on configured behavior. :param ego_object: The agent itself (object with needed attributes). :param external_objects: Other objects in the environment. :type external_objects: list | None :returns: A 2x1 velocity vector appropriate for the agent kinematics. :rtype: numpy.ndarray .. py:method:: load_behavior(behaviors: str = '.behavior_methods') Load behavior parameters from the script. :param behaviors: name of the behavior script. :type behaviors: str .. py:method:: invoke_behavior(kinematics: str, action: str, **kwargs: Any) -> Any Invoke a specific behavior method based on kinematics model and action type. This method looks up and executes the appropriate behavior function from the behavior registry based on the combination of kinematics model and action name. :param kinematics: Kinematics model identifier. Supported values: - 'diff': Differential drive kinematics - 'omni': Omnidirectional kinematics - 'acker': Ackermann steering kinematics :type kinematics: str :param action: Behavior action name. Examples: - 'dash': Direct movement toward goal - 'rvo': Reciprocal Velocity Obstacles for collision avoidance :type action: str :param \*\*kwargs: Additional keyword arguments passed to the behavior function. Common parameters include ego_object, external_objects, goal, etc. :returns: Generated velocity vector (2x1) in the format appropriate for the specified kinematics model. :rtype: np.ndarray :raises ValueError: If no behavior method is found for the given kinematics and action combination. .. rubric:: Example >>> # Invoke differential drive dash behavior >>> vel = behavior.invoke_behavior('diff', 'dash', ... ego_object=robot, ... external_objects=obstacles) .. py:data:: register_behavior .. py:data:: register_behavior_class .. py:data:: register_group_behavior .. py:data:: register_group_behavior_class .. py:class:: GeometryFactory Factory class to create geometry handlers. .. py:method:: create_geometry(name: str = 'circle', **kwargs) -> geometry_handler :staticmethod: .. py:class:: KinematicsFactory Factory class to create kinematics handlers. .. py:method:: create_kinematics(name: str | None = None, noise: bool = False, alpha: list | None = None, mode: str = 'steer', wheelbase: float | None = None, role: str = 'robot') -> KinematicsHandler :staticmethod: .. py:method:: get_handler_class(name: str) -> type[KinematicsHandler] | None :staticmethod: Look up a registered handler class by name without instantiation. :param name: Kinematics name (e.g. ``"diff"``, ``"omni"``). :type name: str :returns: The class, or ``None`` if not found. :rtype: type[KinematicsHandler] | None .. py:class:: KinematicsHandler(name, noise: bool = False, alpha: list | None = None) Bases: :py:obj:`abc.ABC` Abstract base class for handling robot kinematics. Subclasses should set the class-attribute metadata described below and implement :meth:`step`, :meth:`velocity_to_xy`, :meth:`compute_max_speed`, and :meth:`compute_heading`. Initialize the KinematicsHandler class. :param name: Kinematics model name. :type name: str :param noise: Boolean indicating whether to add noise to the velocity (default False). :type noise: bool :param alpha: List of noise parameters for the velocity model (default [0.03, 0, 0, 0.03]). :type alpha: list .. py:attribute:: action_dim :type: int :value: 2 .. py:attribute:: min_state_dim :type: int :value: 3 .. py:attribute:: state_dim :type: int :value: 3 .. py:attribute:: vel_max :type: ClassVar[list[float]] :value: [1, 1] .. py:attribute:: vel_min :type: ClassVar[list[float]] .. py:attribute:: acce :type: ClassVar[list[float]] .. py:attribute:: color :type: str :value: 'g' .. py:attribute:: obstacle_color :type: str :value: 'k' .. py:attribute:: description :type: str | None :value: None .. py:attribute:: show_arrow :type: bool :value: True .. py:attribute:: name .. py:attribute:: noise :value: False .. py:attribute:: alpha :value: [0.03, 0, 0, 0.03] .. py:method:: step(state: numpy.ndarray, velocity: numpy.ndarray, step_time: float) -> numpy.ndarray :abstractmethod: Calculate the next state using the kinematics model. :param state: Current state. :type state: np.ndarray :param velocity: Velocity vector. :type velocity: np.ndarray :param step_time: Time step for simulation. :type step_time: float :returns: Next state. :rtype: np.ndarray .. py:method:: velocity_to_xy(state: numpy.ndarray, velocity: numpy.ndarray) -> numpy.ndarray Convert velocity to [vx, vy] in world frame. The default implementation follows differential-drive conventions: ``velocity[0]`` is the linear speed projected through the heading angle ``state[2]``. Subclasses with different velocity semantics (e.g. omnidirectional) should override this. :param state: Current state vector. :type state: np.ndarray :param velocity: Velocity vector in kinematics frame. :type velocity: np.ndarray :returns: (2, 1) array of [vx, vy]. :rtype: np.ndarray .. py:method:: compute_max_speed(vel_max: numpy.ndarray) -> float Compute the scalar maximum speed from the vel_max vector. The default implementation follows differential-drive conventions: the first component ``vel_max[0, 0]`` is the translational speed limit. Subclasses where max speed is derived differently (e.g. omnidirectional using the L2 norm) should override this. :param vel_max: Maximum velocity vector. :type vel_max: np.ndarray :returns: Scalar maximum speed. :rtype: float .. py:method:: compute_heading(state: numpy.ndarray, velocity: numpy.ndarray) -> float Compute the heading angle. The default implementation follows differential-drive conventions: heading is ``state[2]`` (the orientation component). Returns 0.0 if the state has fewer than 3 rows. :param state: Current state vector. :type state: np.ndarray :param velocity: Current velocity vector. :type velocity: np.ndarray :returns: Heading in radians. :rtype: float .. py:function:: register_kinematics(name: str) Decorator to register a KinematicsHandler subclass. :param name: Name used in YAML configs (e.g. ``"diff"``, ``"omni"``). :type name: str :returns: Class decorator that registers and returns the class unchanged. :rtype: Callable .. py:data:: kinematics_factory :type: dict[str, collections.abc.Callable[Ellipsis, Any]]