Skip to content

Hardcoded Model

robot_nav.models.HCM.hardcoded_model

HCM

Bases: object

A class representing a hybrid control model (HCM) for a robot's navigation system.

This class contains methods for generating actions based on the robot's state, preparing state representations, training (placeholder method), saving/loading models, and logging experiences.

Attributes:

Name Type Description
max_action float

The maximum possible action value.
state_dim int

The dimension of the state representation.
writer SummaryWriter

The writer for logging purposes.
iterator int

A counter for tracking sample addition.
save_samples bool

Whether to save the samples to a file.
max_added_samples int

Maximum number of samples to be added to the saved file.
file_location str

The file location for saving samples.
Source code in robot_nav/models/HCM/hardcoded_model.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
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
class HCM(object):
    """
    A class representing a hybrid control model (HCM) for a robot's navigation system.

    This class contains methods for generating actions based on the robot's state, preparing state
    representations, training (placeholder method), saving/loading models, and logging experiences.

    Attributes:
        max_action (float): The maximum possible action value.
        state_dim (int): The dimension of the state representation.
        writer (SummaryWriter): The writer for logging purposes.
        iterator (int): A counter for tracking sample addition.
        save_samples (bool): Whether to save the samples to a file.
        max_added_samples (int): Maximum number of samples to be added to the saved file.
        file_location (str): The file location for saving samples.
    """

    def __init__(
        self,
        state_dim,
        max_action,
        save_samples,
        max_added_samples=10_000,
        file_location="robot_nav/assets/data.yml",
    ):
        """
        Initialize the HCM class with the provided configuration.

        Args:
            state_dim (int): The dimension of the state space.
            max_action (float): The maximum possible action value.
            save_samples (bool): Whether to save samples to a file.
            max_added_samples (int): The maximum number of samples to save.
            file_location (str): The file path for saving samples.
        """
        self.max_action = max_action
        self.state_dim = state_dim
        self.writer = SummaryWriter()
        self.iterator = 0
        self.save_samples = save_samples
        self.max_added_samples = max_added_samples
        self.file_location = file_location

    def get_action(self, state, add_noise):
        """
        Compute the action to be taken based on the current state of the robot.

        Args:
            state (list): The current state of the robot, including LIDAR scan, distance,
                          and other relevant features.
            add_noise (bool): Whether to add noise to the action for exploration.

        Returns:
            list: The computed action [linear velocity, angular velocity].
        """
        sin = state[-3]
        cos = state[-4]
        angle = atan2(sin, cos)
        laser_nr = self.state_dim - 5
        limit = 1.5

        if min(state[4 : self.state_dim - 9]) < limit:
            state = state.tolist()
            idx = state[:laser_nr].index(min(state[:laser_nr]))
            if idx > laser_nr / 2:
                sign = -1
            else:
                sign = 1

            idx = clip(idx + sign * 5 * (limit / min(state[:laser_nr])), 0, laser_nr)

            angle = ((3.14 / (laser_nr)) * idx) - 1.57

        rot_vel = clip(angle, -1.0, 1.0)
        lin_vel = -abs(rot_vel / 2)
        return [lin_vel, rot_vel]

    # training cycle
    def train(
        self,
        replay_buffer,
        iterations,
        batch_size,
        discount=0.99999,
        tau=0.005,
        policy_noise=0.2,
        noise_clip=0.5,
        policy_freq=2,
    ):
        """
        Placeholder method for training the hybrid control model.

        Args:
            replay_buffer (object): The replay buffer containing past experiences.
            iterations (int): The number of training iterations.
            batch_size (int): The batch size for training.
            discount (float): The discount factor for future rewards.
            tau (float): The soft update parameter for target networks.
            policy_noise (float): The noise added to actions during training.
            noise_clip (float): The clipping value for action noise.
            policy_freq (int): The frequency at which to update the policy.

        Note:
            This method is a placeholder and currently does nothing.
        """
        pass

    def save(self, filename, directory):
        """
        Placeholder method to save the current model state to a file.

        Args:
            filename (str): The name of the file where the model will be saved.
            directory (str): The directory where the file will be stored.

        Note:
            This method is a placeholder and currently does nothing.
        """
        pass

    def load(self, filename, directory):
        """
        Placeholder method to load a model state from a file.

        Args:
            filename (str): The name of the file to load the model from.
            directory (str): The directory where the model file is stored.

        Note:
            This method is a placeholder and currently does nothing.
        """
        pass

    def prepare_state(self, latest_scan, distance, cos, sin, collision, goal, action):
        """
        Prepare the state representation for the model based on the current environment.

        Args:
            latest_scan (list): The LIDAR scan data.
            distance (float): The distance to the goal.
            cos (float): The cosine of the robot's orientation angle.
            sin (float): The sine of the robot's orientation angle.
            collision (bool): Whether a collision occurred.
            goal (bool): Whether the goal has been reached.
            action (list): The action taken by the robot, [linear velocity, angular velocity].

        Returns:
            tuple: A tuple containing the prepared state and a terminal flag (1 if terminal state, 0 otherwise).
        """
        latest_scan = np.array(latest_scan)

        inf_mask = np.isinf(latest_scan)
        latest_scan[inf_mask] = 7.0

        max_bins = self.state_dim - 5
        bin_size = int(np.ceil(len(latest_scan) / max_bins))

        # Initialize the list to store the minimum values of each bin
        min_values = []

        # Loop through the data and create bins
        for i in range(0, len(latest_scan), bin_size):
            # Get the current bin
            bin = latest_scan[i : i + min(bin_size, len(latest_scan) - i)]
            # Find the minimum value in the current bin and append it to the min_values list
            min_values.append(min(bin))
        state = min_values + [distance, cos, sin] + [action[0], action[1]]

        assert len(state) == self.state_dim
        terminal = 1 if collision or goal else 0

        self.iterator += 1
        if self.save_samples and self.iterator < self.max_added_samples:
            action = action if type(action) is list else action
            action = [float(a) for a in action]
            sample = {
                self.iterator: {
                    "latest_scan": latest_scan.tolist(),
                    "distance": distance.tolist(),
                    "cos": cos.tolist(),
                    "sin": sin.tolist(),
                    "collision": collision,
                    "goal": goal,
                    "action": action,
                }
            }
            with open(self.file_location, "a") as outfile:
                yaml.dump(sample, outfile, default_flow_style=False)

        return state, terminal

__init__(state_dim, max_action, save_samples, max_added_samples=10000, file_location='robot_nav/assets/data.yml')

Initialize the HCM class with the provided configuration.

Parameters:

Name Type Description Default
state_dim int

The dimension of the state space.

 required
max_action float

The maximum possible action value.

 required
save_samples bool

Whether to save samples to a file.

 required
max_added_samples int

The maximum number of samples to save.

 10000
file_location str

The file path for saving samples.

 'robot_nav/assets/data.yml'
Source code in robot_nav/models/HCM/hardcoded_model.py 
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
def __init__(
    self,
    state_dim,
    max_action,
    save_samples,
    max_added_samples=10_000,
    file_location="robot_nav/assets/data.yml",
):
    """
    Initialize the HCM class with the provided configuration.

    Args:
        state_dim (int): The dimension of the state space.
        max_action (float): The maximum possible action value.
        save_samples (bool): Whether to save samples to a file.
        max_added_samples (int): The maximum number of samples to save.
        file_location (str): The file path for saving samples.
    """
    self.max_action = max_action
    self.state_dim = state_dim
    self.writer = SummaryWriter()
    self.iterator = 0
    self.save_samples = save_samples
    self.max_added_samples = max_added_samples
    self.file_location = file_location

get_action(state, add_noise)

Compute the action to be taken based on the current state of the robot.

Parameters:

Name Type Description Default
state list

The current state of the robot, including LIDAR scan, distance, and other relevant features.

 required
add_noise bool

Whether to add noise to the action for exploration.

 required

Returns:

Name Type Description
list

The computed action [linear velocity, angular velocity].
Source code in robot_nav/models/HCM/hardcoded_model.py 
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
def get_action(self, state, add_noise):
    """
    Compute the action to be taken based on the current state of the robot.

    Args:
        state (list): The current state of the robot, including LIDAR scan, distance,
                      and other relevant features.
        add_noise (bool): Whether to add noise to the action for exploration.

    Returns:
        list: The computed action [linear velocity, angular velocity].
    """
    sin = state[-3]
    cos = state[-4]
    angle = atan2(sin, cos)
    laser_nr = self.state_dim - 5
    limit = 1.5

    if min(state[4 : self.state_dim - 9]) < limit:
        state = state.tolist()
        idx = state[:laser_nr].index(min(state[:laser_nr]))
        if idx > laser_nr / 2:
            sign = -1
        else:
            sign = 1

        idx = clip(idx + sign * 5 * (limit / min(state[:laser_nr])), 0, laser_nr)

        angle = ((3.14 / (laser_nr)) * idx) - 1.57

    rot_vel = clip(angle, -1.0, 1.0)
    lin_vel = -abs(rot_vel / 2)
    return [lin_vel, rot_vel]

load(filename, directory)

Placeholder method to load a model state from a file.

Parameters:

Name Type Description Default
filename str

The name of the file to load the model from.

 required
directory str

The directory where the model file is stored.

 required
Note

This method is a placeholder and currently does nothing.

Source code in robot_nav/models/HCM/hardcoded_model.py 
129
130
131
132
133
134
135
136
137
138
139
140
def load(self, filename, directory):
    """
    Placeholder method to load a model state from a file.

    Args:
        filename (str): The name of the file to load the model from.
        directory (str): The directory where the model file is stored.

    Note:
        This method is a placeholder and currently does nothing.
    """
    pass

prepare_state(latest_scan, distance, cos, sin, collision, goal, action)

Prepare the state representation for the model based on the current environment.

Parameters:

Name Type Description Default
latest_scan list

The LIDAR scan data.

 required
distance float

The distance to the goal.

 required
cos float

The cosine of the robot's orientation angle.

 required
sin float

The sine of the robot's orientation angle.

 required
collision bool

Whether a collision occurred.

 required
goal bool

Whether the goal has been reached.

 required
action list

The action taken by the robot, [linear velocity, angular velocity].

 required

Returns:

Name Type Description
tuple

A tuple containing the prepared state and a terminal flag (1 if terminal state, 0 otherwise).
Source code in robot_nav/models/HCM/hardcoded_model.py 
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
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
def prepare_state(self, latest_scan, distance, cos, sin, collision, goal, action):
    """
    Prepare the state representation for the model based on the current environment.

    Args:
        latest_scan (list): The LIDAR scan data.
        distance (float): The distance to the goal.
        cos (float): The cosine of the robot's orientation angle.
        sin (float): The sine of the robot's orientation angle.
        collision (bool): Whether a collision occurred.
        goal (bool): Whether the goal has been reached.
        action (list): The action taken by the robot, [linear velocity, angular velocity].

    Returns:
        tuple: A tuple containing the prepared state and a terminal flag (1 if terminal state, 0 otherwise).
    """
    latest_scan = np.array(latest_scan)

    inf_mask = np.isinf(latest_scan)
    latest_scan[inf_mask] = 7.0

    max_bins = self.state_dim - 5
    bin_size = int(np.ceil(len(latest_scan) / max_bins))

    # Initialize the list to store the minimum values of each bin
    min_values = []

    # Loop through the data and create bins
    for i in range(0, len(latest_scan), bin_size):
        # Get the current bin
        bin = latest_scan[i : i + min(bin_size, len(latest_scan) - i)]
        # Find the minimum value in the current bin and append it to the min_values list
        min_values.append(min(bin))
    state = min_values + [distance, cos, sin] + [action[0], action[1]]

    assert len(state) == self.state_dim
    terminal = 1 if collision or goal else 0

    self.iterator += 1
    if self.save_samples and self.iterator < self.max_added_samples:
        action = action if type(action) is list else action
        action = [float(a) for a in action]
        sample = {
            self.iterator: {
                "latest_scan": latest_scan.tolist(),
                "distance": distance.tolist(),
                "cos": cos.tolist(),
                "sin": sin.tolist(),
                "collision": collision,
                "goal": goal,
                "action": action,
            }
        }
        with open(self.file_location, "a") as outfile:
            yaml.dump(sample, outfile, default_flow_style=False)

    return state, terminal

save(filename, directory)

Placeholder method to save the current model state to a file.

Parameters:

Name Type Description Default
filename str

The name of the file where the model will be saved.

 required
directory str

The directory where the file will be stored.

 required
Note

This method is a placeholder and currently does nothing.

Source code in robot_nav/models/HCM/hardcoded_model.py 
116
117
118
119
120
121
122
123
124
125
126
127
def save(self, filename, directory):
    """
    Placeholder method to save the current model state to a file.

    Args:
        filename (str): The name of the file where the model will be saved.
        directory (str): The directory where the file will be stored.

    Note:
        This method is a placeholder and currently does nothing.
    """
    pass

train(replay_buffer, iterations, batch_size, discount=0.99999, tau=0.005, policy_noise=0.2, noise_clip=0.5, policy_freq=2)

Placeholder method for training the hybrid control model.

Parameters:

Name Type Description Default
replay_buffer object

The replay buffer containing past experiences.

 required
iterations int

The number of training iterations.

 required
batch_size int

The batch size for training.

 required
discount float

The discount factor for future rewards.

 0.99999
tau float

The soft update parameter for target networks.

 0.005
policy_noise float

The noise added to actions during training.

 0.2
noise_clip float

The clipping value for action noise.

 0.5
policy_freq int

The frequency at which to update the policy.

 2
Note

This method is a placeholder and currently does nothing.

Source code in robot_nav/models/HCM/hardcoded_model.py 
 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
def train(
    self,
    replay_buffer,
    iterations,
    batch_size,
    discount=0.99999,
    tau=0.005,
    policy_noise=0.2,
    noise_clip=0.5,
    policy_freq=2,
):
    """
    Placeholder method for training the hybrid control model.

    Args:
        replay_buffer (object): The replay buffer containing past experiences.
        iterations (int): The number of training iterations.
        batch_size (int): The batch size for training.
        discount (float): The discount factor for future rewards.
        tau (float): The soft update parameter for target networks.
        policy_noise (float): The noise added to actions during training.
        noise_clip (float): The clipping value for action noise.
        policy_freq (int): The frequency at which to update the policy.

    Note:
        This method is a placeholder and currently does nothing.
    """
    pass