irsim.lib.algorithm#

Core algorithms for IR-SIM simulation.

This package contains: - kinematics: Robot kinematics functions - rvo: Reciprocal Velocity Obstacle algorithm - generation: Polygon generation utilities

Submodules#

Classes#

reciprocal_vel_obs

A class to implement the Reciprocal Velocity Obstacle (RVO) algorithm for multi-robot collision avoidance.

Functions#

generate_polygon(→ numpy.ndarray)

Generate a random polygon around a center point.

random_generate_polygon(...)

reference: https://stackoverflow.com/questions/8997099/algorithm-to-generate-random-2d-polygon

ackermann_kinematics(→ numpy.ndarray)

Calculate the next state for an Ackermann steering vehicle.

differential_kinematics(→ numpy.ndarray)

Calculate the next state for a differential wheel robot.

omni_kinematics(→ numpy.ndarray)

Calculate the next position for an omnidirectional robot.

Package Contents#

irsim.lib.algorithm.generate_polygon(center: list[float], avg_radius: float, irregularity: float, spikeyness: float, num_vertices: int) → numpy.ndarray[source]#

Generate a random polygon around a center point.

Parameters:

  • center (Tuple[float, float]) – Center of the polygon.

  • avg_radius (float) – Average radius from the center to vertices.

  • irregularity (float) – Variance of angle spacing between vertices. Range [0, 1]

  • spikeyness (float) – Variance of radius from the center. Range [0, 1]

  • num_vertices (int) – Number of vertices for the polygon.

Returns:

Vertices of the polygon in CCW order.

Return type:

numpy.ndarray

irsim.lib.algorithm.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][source]#

reference: https://stackoverflow.com/questions/8997099/algorithm-to-generate-random-2d-polygon

Generate random polygons with specified properties.

Parameters:

  • number (int) – Number of polygons to generate (default 1).

  • center_range (List[float]) – Range for the polygon center [min_x, min_y, max_x, max_y].

  • avg_radius_range (List[float]) – Range for the average radius of the polygons.

  • irregularity_range (List[float]) – Range for the irregularity of the polygons.

  • spikeyness_range (List[float]) – Range for the spikeyness of the polygons.

  • num_vertices_range (List[int]) – Range for the number of vertices of the polygons.

Returns:

List of vertices for each polygon or a single polygon’s vertices if number=1.

irsim.lib.algorithm.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[source]#

Calculate the next state for an Ackermann steering vehicle.

Parameters:

  • state – A 4x1 vector [x, y, theta, steer_angle] representing the current state.

  • 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.

  • step_time – The time step for the simulation.

  • noise – Boolean indicating whether to add noise to the velocity (default False).

  • 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.

  • mode – The kinematic mode, either “steer” or “angular” (default “steer”).

  • wheelbase – The distance between the front and rear axles (default 1).

Returns:

A 4x1 vector representing the next state.

Return type:

new_state

irsim.lib.algorithm.differential_kinematics(state: numpy.ndarray, velocity: numpy.ndarray, step_time: float, noise: bool = False, alpha: list[float] | None = None) → numpy.ndarray[source]#

Calculate the next state for a differential wheel robot.

Parameters:

  • state – A 3x1 vector [x, y, theta] representing the current position and orientation.

  • velocity – A 2x1 vector [linear, angular] representing the current velocities.

  • step_time – The time step for the simulation.

  • noise – Boolean indicating whether to add noise to the velocity (default False).

  • 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.

Return type:

next_state

irsim.lib.algorithm.omni_kinematics(state: numpy.ndarray, velocity: numpy.ndarray, step_time: float, noise: bool = False, alpha: list[float] | None = None) → numpy.ndarray[source]#

Calculate the next position for an omnidirectional robot.

Parameters:

  • state – A 2x1 vector [x, y] representing the current position.

  • velocity – A 2x1 vector [vx, vy] representing the current velocities.

  • step_time – The time step for the simulation.

  • noise – Boolean indicating whether to add noise to the velocity (default False).

  • 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.

Return type:

new_position

class irsim.lib.algorithm.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)[source]#

A class to implement the Reciprocal Velocity Obstacle (RVO) algorithm for multi-robot collision avoidance.

Parameters:

  • state (list) – The rvo state of the agent [x, y, vx, vy, radius, vx_des, vy_des].

  • obs_state_list (list) – List of states of static obstacles [[x, y, vx, vy, radius]].

  • vxmax (float) – Maximum velocity in the x direction.

  • vymax (float) – Maximum velocity in the y direction.

  • acce (float) – Acceleration limit.

  • factor (float) – Penalty weighting factor for velocity selection.

  • line_obs_list (list) – List of line segments [[x1, y1, x2, y2], …].

state#
obs_state_list = None#
line_obs_list = None#
vxmax = 1.5#
vymax = 1.5#
acce = 0.5#
factor = 1.0#
update(state, obs_state_list, line_obs_list=None)[source]#
cal_vel(mode='rvo')[source]#

Calculate the velocity of the agent based on the Reciprocal Velocity Obstacle (RVO) algorithm.

Parameters:

mode (str) – 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.

Returns:

Selected velocity [vx, vy].

Return type:

list[float]

config_rvo()[source]#
config_rvo_mode(obstacle)[source]#
config_hrvo()[source]#
config_hrvo_mode(obstacle)[source]#
config_vo()[source]#
config_vo_mode(obstacle)[source]#
config_vo_lines()[source]#

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.

vel_candidate(rvo_list)[source]#
vo_out(vx, vy, rvo_list)[source]#
vel_select(vo_outside, vo_inside)[source]#
penalty(vel, vel_des, factor)[source]#
static between_vector(line_left_vector, line_right_vector, line_vector)[source]#
static cross_product(vector1, vector2)[source]#