Skip to content

IR-SIM

robot_nav.sim

SIM_ENV

A simulation environment interface for robot navigation using IRSim.

This class wraps around the IRSim environment and provides methods for stepping, resetting, and interacting with a mobile robot, including reward computation.

Attributes:

Name Type Description
env object

The simulation environment instance from IRSim.
robot_goal ndarray

The goal position of the robot.
Source code in robot_nav/sim.py 
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
class SIM_ENV:
    """
    A simulation environment interface for robot navigation using IRSim.

    This class wraps around the IRSim environment and provides methods for stepping,
    resetting, and interacting with a mobile robot, including reward computation.

    Attributes:
        env (object): The simulation environment instance from IRSim.
        robot_goal (np.ndarray): The goal position of the robot.
    """

    def __init__(self, world_file="robot_world.yaml", disable_plotting=False):
        """
        Initialize the simulation environment.

        Args:
            world_file (str): Path to the world configuration YAML file.
            disable_plotting (bool): If True, disables rendering and plotting.
        """
        self.env = irsim.make(world_file, disable_all_plot=disable_plotting)
        robot_info = self.env.get_robot_info(0)
        self.robot_goal = robot_info.goal

    def step(self, lin_velocity=0.0, ang_velocity=0.1):
        """
        Perform one step in the simulation using the given control commands.

        Args:
            lin_velocity (float): Linear velocity to apply to the robot.
            ang_velocity (float): Angular velocity to apply to the robot.

        Returns:
            tuple: Contains the latest LIDAR scan, distance to goal, cosine and sine of angle to goal,
                   collision flag, goal reached flag, applied action, and computed reward.
        """
        self.env.step(action_id=0, action=np.array([[lin_velocity], [ang_velocity]]))
        self.env.render()

        scan = self.env.get_lidar_scan()
        latest_scan = scan["ranges"]

        robot_state = self.env.get_robot_state()
        goal_vector = [
            self.robot_goal[0].item() - robot_state[0].item(),
            self.robot_goal[1].item() - robot_state[1].item(),
        ]
        distance = np.linalg.norm(goal_vector)
        goal = self.env.robot.arrive
        pose_vector = [np.cos(robot_state[2]).item(), np.sin(robot_state[2]).item()]
        cos, sin = self.cossin(pose_vector, goal_vector)
        collision = self.env.robot.collision
        action = [lin_velocity, ang_velocity]
        reward = self.get_reward(goal, collision, action, latest_scan)

        return latest_scan, distance, cos, sin, collision, goal, action, reward

    def reset(
        self,
        robot_state=None,
        robot_goal=None,
        random_obstacles=True,
        random_obstacle_ids=None,
    ):
        """
        Reset the simulation environment, optionally setting robot and obstacle states.

        Args:
            robot_state (list or None): Initial state of the robot as a list of [x, y, theta, speed].
            robot_goal (list or None): Goal state for the robot.
            random_obstacles (bool): Whether to randomly reposition obstacles.
            random_obstacle_ids (list or None): Specific obstacle IDs to randomize.

        Returns:
            tuple: Initial observation after reset, including LIDAR scan, distance, cos/sin,
                   and reward-related flags and values.
        """
        if robot_state is None:
            robot_state = [[random.uniform(1, 9)], [random.uniform(1, 9)], [0], [0]]

        self.env.robot.set_state(
            state=np.array(robot_state),
            init=True,
        )

        if random_obstacles:
            if random_obstacle_ids is None:
                random_obstacle_ids = [i + 1 for i in range(7)]
            self.env.random_obstacle_position(
                range_low=[0, 0, -3.14],
                range_high=[10, 10, 3.14],
                ids=random_obstacle_ids,
                non_overlapping=True,
            )

        if robot_goal is None:
            unique = True
            while unique:
                robot_goal = [[random.uniform(1, 9)], [random.uniform(1, 9)], [0]]
                shape = {"name": "circle", "radius": 0.4}
                state = [robot_goal[0], robot_goal[1], robot_goal[2]]
                gf = GeometryFactory.create_geometry(**shape)
                geometry = gf.step(np.c_[state])
                unique = any(
                    [
                        shapely.intersects(geometry, obj._geometry)
                        for obj in self.env.obstacle_list
                    ]
                )
        self.env.robot.set_goal(np.array(robot_goal), init=True)
        self.env.reset()
        self.robot_goal = self.env.robot.goal

        action = [0.0, 0.0]
        latest_scan, distance, cos, sin, _, _, action, reward = self.step(
            lin_velocity=action[0], ang_velocity=action[1]
        )
        return latest_scan, distance, cos, sin, False, False, action, reward

    @staticmethod
    def cossin(vec1, vec2):
        """
        Compute the cosine and sine of the angle between two 2D vectors.

        Args:
            vec1 (list): First 2D vector.
            vec2 (list): Second 2D vector.

        Returns:
            tuple: (cosine, sine) of the angle between the vectors.
        """
        vec1 = vec1 / np.linalg.norm(vec1)
        vec2 = vec2 / np.linalg.norm(vec2)
        cos = np.dot(vec1, vec2)
        sin = vec1[0] * vec2[1] - vec1[1] * vec2[0]
        return cos, sin

    @staticmethod
    def get_reward(goal, collision, action, laser_scan):
        """
        Calculate the reward for the current step.

        Args:
            goal (bool): Whether the goal has been reached.
            collision (bool): Whether a collision occurred.
            action (list): The action taken [linear velocity, angular velocity].
            laser_scan (list): The LIDAR scan readings.

        Returns:
            float: Computed reward for the current state.
        """
        if goal:
            return 100.0
        elif collision:
            return -100.0
        else:
            r3 = lambda x: 1.35 - x if x < 1.35 else 0.0
            return action[0] - abs(action[1]) / 2 - r3(min(laser_scan)) / 2

__init__(world_file='robot_world.yaml', disable_plotting=False)

Initialize the simulation environment.

Parameters:

Name Type Description Default
world_file str

Path to the world configuration YAML file.

 'robot_world.yaml'
disable_plotting bool

If True, disables rendering and plotting.

 False
Source code in robot_nav/sim.py 
21
22
23
24
25
26
27
28
29
30
31
def __init__(self, world_file="robot_world.yaml", disable_plotting=False):
    """
    Initialize the simulation environment.

    Args:
        world_file (str): Path to the world configuration YAML file.
        disable_plotting (bool): If True, disables rendering and plotting.
    """
    self.env = irsim.make(world_file, disable_all_plot=disable_plotting)
    robot_info = self.env.get_robot_info(0)
    self.robot_goal = robot_info.goal

cossin(vec1, vec2) staticmethod

Compute the cosine and sine of the angle between two 2D vectors.

Parameters:

Name Type Description Default
vec1 list

First 2D vector.

 required
vec2 list

Second 2D vector.

 required

Returns:

Name Type Description
tuple

(cosine, sine) of the angle between the vectors.
Source code in robot_nav/sim.py 
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
@staticmethod
def cossin(vec1, vec2):
    """
    Compute the cosine and sine of the angle between two 2D vectors.

    Args:
        vec1 (list): First 2D vector.
        vec2 (list): Second 2D vector.

    Returns:
        tuple: (cosine, sine) of the angle between the vectors.
    """
    vec1 = vec1 / np.linalg.norm(vec1)
    vec2 = vec2 / np.linalg.norm(vec2)
    cos = np.dot(vec1, vec2)
    sin = vec1[0] * vec2[1] - vec1[1] * vec2[0]
    return cos, sin

get_reward(goal, collision, action, laser_scan) staticmethod

Calculate the reward for the current step.

Parameters:

Name Type Description Default
goal bool

Whether the goal has been reached.

 required
collision bool

Whether a collision occurred.

 required
action list

The action taken [linear velocity, angular velocity].

 required
laser_scan list

The LIDAR scan readings.

 required

Returns:

Name Type Description
float

Computed reward for the current state.
Source code in robot_nav/sim.py 
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
@staticmethod
def get_reward(goal, collision, action, laser_scan):
    """
    Calculate the reward for the current step.

    Args:
        goal (bool): Whether the goal has been reached.
        collision (bool): Whether a collision occurred.
        action (list): The action taken [linear velocity, angular velocity].
        laser_scan (list): The LIDAR scan readings.

    Returns:
        float: Computed reward for the current state.
    """
    if goal:
        return 100.0
    elif collision:
        return -100.0
    else:
        r3 = lambda x: 1.35 - x if x < 1.35 else 0.0
        return action[0] - abs(action[1]) / 2 - r3(min(laser_scan)) / 2

reset(robot_state=None, robot_goal=None, random_obstacles=True, random_obstacle_ids=None)

Reset the simulation environment, optionally setting robot and obstacle states.

Parameters:

Name Type Description Default
robot_state list or None

Initial state of the robot as a list of [x, y, theta, speed].

 None
robot_goal list or None

Goal state for the robot.

 None
random_obstacles bool

Whether to randomly reposition obstacles.

 True
random_obstacle_ids list or None

Specific obstacle IDs to randomize.

 None

Returns:

Name Type Description
tuple

Initial observation after reset, including LIDAR scan, distance, cos/sin, and reward-related flags and values.
Source code in robot_nav/sim.py 
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
def reset(
    self,
    robot_state=None,
    robot_goal=None,
    random_obstacles=True,
    random_obstacle_ids=None,
):
    """
    Reset the simulation environment, optionally setting robot and obstacle states.

    Args:
        robot_state (list or None): Initial state of the robot as a list of [x, y, theta, speed].
        robot_goal (list or None): Goal state for the robot.
        random_obstacles (bool): Whether to randomly reposition obstacles.
        random_obstacle_ids (list or None): Specific obstacle IDs to randomize.

    Returns:
        tuple: Initial observation after reset, including LIDAR scan, distance, cos/sin,
               and reward-related flags and values.
    """
    if robot_state is None:
        robot_state = [[random.uniform(1, 9)], [random.uniform(1, 9)], [0], [0]]

    self.env.robot.set_state(
        state=np.array(robot_state),
        init=True,
    )

    if random_obstacles:
        if random_obstacle_ids is None:
            random_obstacle_ids = [i + 1 for i in range(7)]
        self.env.random_obstacle_position(
            range_low=[0, 0, -3.14],
            range_high=[10, 10, 3.14],
            ids=random_obstacle_ids,
            non_overlapping=True,
        )

    if robot_goal is None:
        unique = True
        while unique:
            robot_goal = [[random.uniform(1, 9)], [random.uniform(1, 9)], [0]]
            shape = {"name": "circle", "radius": 0.4}
            state = [robot_goal[0], robot_goal[1], robot_goal[2]]
            gf = GeometryFactory.create_geometry(**shape)
            geometry = gf.step(np.c_[state])
            unique = any(
                [
                    shapely.intersects(geometry, obj._geometry)
                    for obj in self.env.obstacle_list
                ]
            )
    self.env.robot.set_goal(np.array(robot_goal), init=True)
    self.env.reset()
    self.robot_goal = self.env.robot.goal

    action = [0.0, 0.0]
    latest_scan, distance, cos, sin, _, _, action, reward = self.step(
        lin_velocity=action[0], ang_velocity=action[1]
    )
    return latest_scan, distance, cos, sin, False, False, action, reward

step(lin_velocity=0.0, ang_velocity=0.1)

Perform one step in the simulation using the given control commands.

Parameters:

Name Type Description Default
lin_velocity float

Linear velocity to apply to the robot.

 0.0
ang_velocity float

Angular velocity to apply to the robot.

 0.1

Returns:

Name Type Description
tuple

Contains the latest LIDAR scan, distance to goal, cosine and sine of angle to goal, collision flag, goal reached flag, applied action, and computed reward.
Source code in robot_nav/sim.py 
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
def step(self, lin_velocity=0.0, ang_velocity=0.1):
    """
    Perform one step in the simulation using the given control commands.

    Args:
        lin_velocity (float): Linear velocity to apply to the robot.
        ang_velocity (float): Angular velocity to apply to the robot.

    Returns:
        tuple: Contains the latest LIDAR scan, distance to goal, cosine and sine of angle to goal,
               collision flag, goal reached flag, applied action, and computed reward.
    """
    self.env.step(action_id=0, action=np.array([[lin_velocity], [ang_velocity]]))
    self.env.render()

    scan = self.env.get_lidar_scan()
    latest_scan = scan["ranges"]

    robot_state = self.env.get_robot_state()
    goal_vector = [
        self.robot_goal[0].item() - robot_state[0].item(),
        self.robot_goal[1].item() - robot_state[1].item(),
    ]
    distance = np.linalg.norm(goal_vector)
    goal = self.env.robot.arrive
    pose_vector = [np.cos(robot_state[2]).item(), np.sin(robot_state[2]).item()]
    cos, sin = self.cossin(pose_vector, goal_vector)
    collision = self.env.robot.collision
    action = [lin_velocity, ang_velocity]
    reward = self.get_reward(goal, collision, action, latest_scan)

    return latest_scan, distance, cos, sin, collision, goal, action, reward