enhance: allow data selection with arbitrary setpoint topics and vari…

…ables (#230) * enhance: allow data selection with arbitrary setpoint topics and variables * fix: ensure that default argument for initialization of data handler is correct
ethz-asl · Aug 16, 2023 · 59e656e · 59e656e
1 parent 39cbc29
commit 59e656e
Show file tree

Hide file tree

Showing 12 changed files with 71 additions and 164 deletions.
diff --git a/Makefile b/Makefile
@@ -6,6 +6,7 @@ model?=quadrotor_model
 log?=${root_dir}/resources/${model}.ulg
 config?=${root_dir}/Tools/parametric_model/configs/${model}.yaml
 data_selection?=none
+selection_var?=none
 plot?=True
 
 submodulesupdate:
@@ -34,6 +35,7 @@ estimate-model:
 	python3 Tools/parametric_model/generate_parametric_model.py \
 	--config ${config} \
 	--data_selection ${data_selection} \
+	--selection_var ${selection_var} \
 	--plot ${plot} \
 	${log}
 

diff --git a/README.md b/README.md
@@ -70,7 +70,7 @@ source setup.bash
 Generate the parametric model using a log file (ulog or csv):
 
 ```
-make estimate-model [model=<modeltype>] [config=<config_file_path>] [data_selection=<none|interactive|setpoint|auto>] [plot=<True/False>] [log=<log_file_path>]
+make estimate-model [model=<modeltype>] [config=<config_file_path>] [data_selection=<none|interactive|setpoint|auto>] [selection_var=topic_name/variable_name] [plot=<True/False>] [log=<log_file_path>]
 ```
 
 ### Pipeline Arguments
@@ -97,7 +97,7 @@ The data_selection argument is optional (per default none) and can be used to vi
 
 - none(default): Data selection is disabled, and the whole section of the log is used
 - interactive: Data is selected interactively using the [Visual Dataframe Selector](https://github.com/manumerous/visual_dataframe_selector), before running the model estimation. It is also possible to save the selected subportion of data to a csv file in order to use this exact dataset multiple times.
-- setpoint: Data is selected based on a certain `manual_control_setpoint` of the `ulog` or `csv` file. The `manual_control_setpoint` topic that should be used for selection, can be specified in the configuration file of the model as `selection_variable` (possible values are `aux1` to `aux6`). Finally, specific `activations` of the `selection_variable` can also be specified as a list in the configuration file (default: all activations will be used).
+- setpoint: Data is selected based on a certain topic value, which has to be specified with the variable `selection_var` through the command line. The variable has to be provided in the format `topic_name/variable_name` to be recognized and loaded correctly. For example, to select data based on the `aux1` value of the `manual_control_setpoint` topic, the variable `manual_control_setpoint/aux1` has to be specified.
 - auto: Data is selected automatically (Beta)
 
 ### Results
@@ -112,7 +112,6 @@ As an example to get started you estimate the parameters of a quadrotor model wi
 make estimate-model model=quadrotor_model log=resources/quadrotor_model.ulg
 ```
 
-
 ## Generating a Model Prediction for Given Parameters and Log
 
 It is also possible to test the obtained parameters for a certain model on a different log using:

diff --git a/Tools/parametric_model/configs/config_template.yaml b/Tools/parametric_model/configs/config_template.yaml
@@ -4,10 +4,6 @@ model_type: "template_model"
 model_class: "template"
 extractor_class: "template_extractor"
 
-# data handling with setpoint setting
-selection_variable: "aux1" # value can be aux1, aux2, aux3, aux4, aux5, aux6
-# activations: [1, 2, 3]    # optional - Caution: indexing starts from 1
-
 extractor_config: "extractor_config"
 
 # all vectors in FRD body frame if not specified otherwise

diff --git a/Tools/parametric_model/configs/dynamics_model_test_config.yaml b/Tools/parametric_model/configs/dynamics_model_test_config.yaml
@@ -4,10 +4,6 @@ model_type: "test_dynamics_model"
 model_class: "test_dynamics_model"
 extractor_class: "Not implemented"
 
-# data handling with setpoint setting
-selection_variable: "aux1" # value can be aux1, aux2, aux3, aux4, aux5, aux6
-# activations: [1, 2, 3]    # optional - Caution: indexing starts from 1
-
 extractor_config: "Not implemented"
 
 # all vectors in FRD body frame if not specified otherwise
@@ -82,23 +78,6 @@ dynamics_model_config:
           - "accelerometer_m_s2[0]"
           - "accelerometer_m_s2[1]"
           - "accelerometer_m_s2[2]"
-      manual_control_setpoint:
-        ulog_name:
-          - "timestamp"
-          - "aux1"
-          - "aux2"
-          - "aux3"
-          - "aux4"
-          - "aux5"
-          - "aux6"
-        dataframe_name:
-          - "timestamp"
-          - "aux1"
-          - "aux2"
-          - "aux3"
-          - "aux4"
-          - "aux5"
-          - "aux6"
       vehicle_land_detected:
         ulog_name:
           - "timestamp"

diff --git a/Tools/parametric_model/configs/fixedwing_model.yaml b/Tools/parametric_model/configs/fixedwing_model.yaml
@@ -4,10 +4,6 @@ model_type: "Standard Plane Longitudinal Model"
 model_class: "FixedWingModel"
 extractor_class: "FixedWingExtractorModel"
 
-# data handling with setpoint setting
-selection_variable: "aux1" # value can be aux1, aux2, aux3, aux4, aux5, aux6
-# activations: [1, 2, 3]    # optional - Caution: indexing starts from 1
-
 extractor_config:
   vmin: 8.0
   vmax: 20.0
@@ -114,23 +110,6 @@ dynamics_model_config:
           - "acc_b_x"
           - "acc_b_y"
           - "acc_b_z"
-      manual_control_setpoint:
-        ulog_name:
-          - "timestamp"
-          - "aux1"
-          - "aux2"
-          - "aux3"
-          - "aux4"
-          - "aux5"
-          - "aux6"
-        dataframe_name:
-          - "timestamp"
-          - "aux1"
-          - "aux2"
-          - "aux3"
-          - "aux4"
-          - "aux5"
-          - "aux6"
       vehicle_thrust_setpoint:
         ulog_name:
           - "timestamp"

diff --git a/Tools/parametric_model/configs/fixedwing_singularityfree_model.yaml b/Tools/parametric_model/configs/fixedwing_singularityfree_model.yaml
@@ -3,10 +3,6 @@ model_name: "Gazebo Standard Plane"
 model_type: "Standard Plane Longitudinal Model"
 model_class: "FixedWingModel"
 
-# data handling with setpoint setting
-selection_variable: "aux1" # value can be aux1, aux2, aux3, aux4, aux5, aux6
-# activations: [1, 2, 3]    # optional - Caution: indexing starts from 1
-
 extractor_class: "SingularityFreeExtractorModel"
 extractor_config:
   vmin: 8.0
@@ -114,23 +110,6 @@ dynamics_model_config:
           - "acc_b_x"
           - "acc_b_y"
           - "acc_b_z"
-      manual_control_setpoint:
-        ulog_name:
-          - "timestamp"
-          - "aux1"
-          - "aux2"
-          - "aux3"
-          - "aux4"
-          - "aux5"
-          - "aux6"
-        dataframe_name:
-          - "timestamp"
-          - "aux1"
-          - "aux2"
-          - "aux3"
-          - "aux4"
-          - "aux5"
-          - "aux6"
       vehicle_thrust_setpoint:
         ulog_name:
           - "timestamp"

diff --git a/Tools/parametric_model/configs/quadplane_model.yaml b/Tools/parametric_model/configs/quadplane_model.yaml
@@ -4,10 +4,6 @@ model_type: "Standard VTOL"
 model_class: "QuadPlaneModel"
 extractor_class: "Not implemented"
 
-# data handling with setpoint setting
-selection_variable: "aux1" # value can be aux1, aux2, aux3, aux4, aux5, aux6
-# activations: [1, 2, 3]    # optional - Caution: indexing starts from 1
-
 extractor_config:
   "Not implemented"
 
@@ -193,23 +189,6 @@ dynamics_model_config:
           - "ang_acc_b_x"
           - "ang_acc_b_y"
           - "ang_acc_b_z"
-      manual_control_setpoint:
-        ulog_name:
-          - "timestamp"
-          - "aux1"
-          - "aux2"
-          - "aux3"
-          - "aux4"
-          - "aux5"
-          - "aux6"
-        dataframe_name:
-          - "timestamp"
-          - "aux1"
-          - "aux2"
-          - "aux3"
-          - "aux4"
-          - "aux5"
-          - "aux6"
       vehicle_land_detected:
         ulog_name:
           - "timestamp"

diff --git a/Tools/parametric_model/configs/quadrotor_model.yaml b/Tools/parametric_model/configs/quadrotor_model.yaml
@@ -4,10 +4,6 @@ model_type: "Standard Multirotor"
 model_class: "MultiRotorModel"
 extractor_class: "Not implemented"
 
-# data handling with setpoint setting
-selection_variable: "aux1" # value can be aux1, aux2, aux3, aux4, aux5, aux6
-# activations: [1, 2, 3]    # optional - Caution: indexing starts from 1
-
 extractor_config:
   "Not implemented"
 
@@ -166,23 +162,6 @@ dynamics_model_config:
           - "ang_acc_b_x"
           - "ang_acc_b_y"
           - "ang_acc_b_z"
-      manual_control_setpoint:
-        ulog_name:
-          - "timestamp"
-          - "aux1"
-          - "aux2"
-          - "aux3"
-          - "aux4"
-          - "aux5"
-          - "aux6"
-        dataframe_name:
-          - "timestamp"
-          - "aux1"
-          - "aux2"
-          - "aux3"
-          - "aux4"
-          - "aux5"
-          - "aux6"
       vehicle_land_detected:
         ulog_name:
           - "timestamp"

diff --git a/Tools/parametric_model/configs/vtol_tiltrotor_model.yaml b/Tools/parametric_model/configs/vtol_tiltrotor_model.yaml
@@ -4,10 +4,6 @@ model_type: "Standard Plane Longitudinal Model"
 model_class: "FixedWingModel"
 extractor_class: "FixedWingExtractorModel"
 
-# data handling with setpoint setting
-selection_variable: "aux1" # value can be aux1, aux2, aux3, aux4, aux5, aux6
-# activations: [1, 2, 3]    # optional - Caution: indexing starts from 1
-
 extractor_config:
   vmin: 14.0
   vmax: 22.0
@@ -111,23 +107,6 @@ dynamics_model_config:
           - "acc_b_x"
           - "acc_b_y"
           - "acc_b_z"
-      manual_control_setpoint:
-        ulog_name:
-          - "timestamp"
-          - "aux1"
-          - "aux2"
-          - "aux3"
-          - "aux4"
-          - "aux5"
-          - "aux6"
-        dataframe_name:
-          - "timestamp"
-          - "aux1"
-          - "aux2"
-          - "aux3"
-          - "aux4"
-          - "aux5"
-          - "aux6"
       vehicle_thrust_setpoint:
         ulog_name:
           - "timestamp"

diff --git a/Tools/parametric_model/generate_parametric_model.py b/Tools/parametric_model/generate_parametric_model.py
@@ -49,12 +49,13 @@ def start_model_estimation(
     config,
     log_path,
     data_selection="none",
+    selection_var="none",
     plot=False,
     normalization=True,
     extraction=False,
 ):
     # Flag for enabling automatic data selection.
-    data_handler = DataHandler(config)
+    data_handler = DataHandler(config, selection_var)
     data_handler.loadLogs(log_path)
     data_df = data_handler.get_dataframes()
     model_class = data_handler.config.model_class
@@ -106,53 +107,43 @@ def start_model_estimation(
                 model.data_df, visual_dataframe_selector_config_dict
             )
         )
+        print("Interactive data selection completed.")
+
         model.prepare_regression_matrices()
         model.compute_fisher_information()
-        print("Interactive data selection completed.")
 
     # Setpoint based data selection
     elif data_selection == "setpoint":
         print("Setpoint based data selection enabled...")
 
-        selector = data_handler.config.selection_variable
-
-        if selector not in ["aux1", "aux2", "aux3", "aux4", "aux5", "aux6"]:
-            error_str = "Variable '{0}' is not valid for data filtering".format(
-                selector
-            )
-            raise AttributeError(error_str)
+        selector = selection_var.split("/")[1]
 
         zero_crossings = np.where(
             np.diff(np.sign(data_df[selector] + (data_df[selector] == 0)))
         )[0]
 
-        if len(zero_crossings) % 2 != 0 or len(zero_crossings) == 0:
+        if len(zero_crossings) == 0:
             raise AttributeError(
-                "All manual trigger activations have to start and end during the flight phase"
+                "No selection variable activations have been found in the log."
+            )
+
+        if len(zero_crossings) % 2 != 0:
+            raise AttributeError(
+                "All selection variable activations have to start and end during the flight phase"
             )
 
         acc_df = pd.DataFrame()
 
         for i in range(0, len(zero_crossings), 2):
             start = zero_crossings[i]
             end = zero_crossings[i + 1]
-            activations = data_handler.config.activations
-
-            if (activations is None) or len(activations) == 0:
-                # no activations have been specified by the user and all activations are used
-                acc_df = pd.concat([acc_df, data_df.iloc[start:end]], ignore_index=True)
-            else:
-                # activations have been specified by the user and only the specified activations are used
-                if ((i + 2) / 2) in activations:
-                    acc_df = pd.concat(
-                        [acc_df, data_df.iloc[start:end]], ignore_index=True
-                    )
-                continue
+            acc_df = pd.concat([acc_df, data_df.iloc[start:end]], ignore_index=True)
 
         model.load_dataframes(acc_df)
+        print("Setpoint based data selection completed.")
+
         model.prepare_regression_matrices()
         model.compute_fisher_information()
-        print("Setpoint based data selection completed.")
 
     elif data_selection == "auto":  # Automatic data selection (WIP)
         print("Automatic data selection enabled...")
@@ -166,9 +157,10 @@ def start_model_estimation(
         # can vary drastically from log to log.
         data_selector = AutomaticDataSelector(model.data_df)
         model.load_dataframes(data_selector.select_dataframes(10))
+        print("Automatic data selection completed.")
+
         model.prepare_regression_matrices()
         model.compute_fisher_information()
-        print("Automatic data selection completed.")
 
     else:
         model.load_dataframes(data_df)
@@ -229,6 +221,13 @@ def start_model_estimation(
         default="none",
         help="Data selection scheme none | interactive | setpoint | auto (Beta)",
     )
+    parser.add_argument(
+        "--selection_var",
+        metavar="selection_var",
+        type=str,
+        default="",
+        help="Setpoint variable, which should be used to filter the data - notation: 'topic/variable'.",
+    )
     parser.add_argument(
         "--plot",
         metavar="plot",

diff --git a/Tools/parametric_model/src/models/model_config.py b/Tools/parametric_model/src/models/model_config.py
@@ -74,11 +74,6 @@ def __init__(self, config_file_name):
         self.extractor_class = config_dict["extractor_class"]
         self.extractor_config = config_dict["extractor_config"]
 
-        self.selection_variable = config_dict["selection_variable"]
-        self.activations = (
-            config_dict["activations"] if "activations" in config_dict else None
-        )
-
         print("Initializing of configuration successful. ")
 
         return