diff --git a/control/autoware_smart_mpc_trajectory_follower/.gitignore b/control/autoware_smart_mpc_trajectory_follower/.gitignore
index d914a0aa99e5a..c5b22066b75c1 100644
--- a/control/autoware_smart_mpc_trajectory_follower/.gitignore
+++ b/control/autoware_smart_mpc_trajectory_follower/.gitignore
@@ -1,3 +1,4 @@
# Files generated when installing smart_mpc_trajectory_follower
build/
*.egg-info/
+.cspell.json
diff --git a/control/autoware_smart_mpc_trajectory_follower/README.md b/control/autoware_smart_mpc_trajectory_follower/README.md
index fd135319ff900..bb31a1be3a8b5 100644
--- a/control/autoware_smart_mpc_trajectory_follower/README.md
+++ b/control/autoware_smart_mpc_trajectory_follower/README.md
@@ -3,7 +3,7 @@
-
+
# Smart MPC Trajectory Follower
@@ -83,19 +83,29 @@ model_trainer.transform_rosbag_to_csv(rosbag_dir)
Here, `rosbag_dir` represents the rosbag directory.
At this time, all CSV files in `rosbag_dir` are automatically deleted first.
-The paths of the rosbag directories used for training, `dir_0`, `dir_1`, `dir_2`,... and the directory `save_dir` where you save the models, the model can be saved in the python environment as follows:
+We move on to an explanation of how the model is trained.
+If `trained_model_parameter:memory_for_training:use_memory_for_training` in [trained_model_param.yaml](./autoware_smart_mpc_trajectory_follower/param/trained_model_param.yaml) is set to `true`, training is performed on models that include LSTM, and if it is set to `false`, training is performed on models that do not include LSTM.
+When using LSTM, cell states and hidden states are updated based on historical time series data and reflected in the prediction.
+
+The paths of the rosbag directories used for training and validation, `dir_0`, `dir_1`, `dir_2`,..., `dir_val_0`, `dir_val_1`, `dir_val_2`,... and the directory `save_dir` where you save the models, the model can be saved in the python environment as follows:
```python
from autoware_smart_mpc_trajectory_follower.training_and_data_check import train_drive_NN_model
model_trainer = train_drive_NN_model.train_drive_NN_model()
-model_trainer.add_data_from_csv(dir_0)
-model_trainer.add_data_from_csv(dir_1)
-model_trainer.add_data_from_csv(dir_2)
+model_trainer.add_data_from_csv(dir_0, add_mode="as_train")
+model_trainer.add_data_from_csv(dir_1, add_mode="as_train")
+model_trainer.add_data_from_csv(dir_2, add_mode="as_train")
+...
+model_trainer.add_data_from_csv(dir_val_0, add_mode="as_val")
+model_trainer.add_data_from_csv(dir_val_1, add_mode="as_val")
+model_trainer.add_data_from_csv(dir_val_2, add_mode="as_val")
...
model_trainer.get_trained_model()
model_trainer.save_models(save_dir)
```
+If `add_mode` is not specified or validation data is not added, the training data is split to be used for training and validation.
+
After performing the polynomial regression, the NN can be trained on the residuals as follows:
```python
@@ -130,6 +140,7 @@ To perform a control test on autoware with the nominal model before training, ma
Record rosbag and train the model in the manner described in "Training of model and reflection in control", and move the generated files `model_for_test_drive.pth` and `polynomial_reg_info.npz` to the home directory.
+Sample models, which work under the condition that`trained_model_parameter:memory_for_training:use_memory_for_training` in [trained_model_param.yaml](./autoware_smart_mpc_trajectory_follower/param/trained_model_param.yaml) is set to `true`, can be obtained at [sample_models/wheel_base_changed](./sample_models/wheel_base_changed/).
> [!NOTE]
> Although the data used for training is small, for the sake of simplicity, we will see how much performance can be improved with this amount of data.
@@ -158,58 +169,84 @@ First, to give wheel base 2.79 m in the python simulator, create the following f
{ "wheel_base": 2.79 }
```
-Next, run the following commands to test the slalom driving on the python simulator with the nominal model:
+Next, after moving to `control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator`, run the following commands to test the slalom driving on the python simulator with the nominal control:
-```python
-import python_simulator
-from autoware_smart_mpc_trajectory_follower.training_and_data_check import train_drive_NN_model
-initial_error = [0.0, 0.03, 0.01, -0.01, 0.0, 0.0]
-save_dir = "test_python_sim"
-python_simulator.slalom_drive(save_dir=save_dir,use_trained_model=False,initial_error=initial_error)
+```bash
+python3 run_python_simulator.py nominal_test
```
-Here, `initial_error` is the initial error from the target trajectory, in the order of x-coordinate, y-coordinate, longitudinal velocity, yaw angle, longitudinal acceleration, and steer angle,
-and `save_dir` is the directory where the driving test results are saved.
+The result of the driving is stored in `test_python_nominal_sim`.
-> [!NOTE]
-> The value of `use_trained_model` given as the argument of `python_simulator.slalom_drive` takes precedence over the value of `trained_model_parameter:control_application:use_trained_model` in [trained_model_param.yaml](./autoware_smart_mpc_trajectory_follower/param/trained_model_param.yaml).
+The following results were obtained.
-Run the following commands to perform training using driving data of the nominal model.
+
+ 
+
-```python
-model_trainer = train_drive_NN_model.train_drive_NN_model()
-model_trainer.add_data_from_csv(save_dir)
-model_trainer.save_train_data(save_dir)
-model_trainer.get_trained_model(use_polynomial_reg=True)
-model_trainer.save_models(save_dir=save_dir)
+The center of the upper row represents the lateral deviation.
+
+Run the following commands to perform training using figure eight driving data under the control of pure pursuit.
+
+To perform training using a figure eight driving and driving based on the obtained model, run the following commands:
+
+```bash
+python3 run_python_simulator.py
```
-This way, files `model_for_test_drive.pth` and `polynomial_reg_info.npz` are saved in `save_dir`.
-The following results were obtained.
+The result of the driving is stored in `test_python_trined_sim`.
+
+When `trained_model_parameter:memory_for_training:use_memory_for_training` in [trained_model_param.yaml](./autoware_smart_mpc_trajectory_follower/param/trained_model_param.yaml) is set to `true`, the following results were obtained.
- 
+ 
-The center of the upper row represents the lateral deviation.
+When `trained_model_parameter:memory_for_training:use_memory_for_training` in [trained_model_param.yaml](./autoware_smart_mpc_trajectory_follower/param/trained_model_param.yaml) is set to `false`, the following results were obtained.
-Finally, to drive with the training model, run the following commands:
+
+ 
+
-```python
-load_dir = save_dir
-save_dir = "test_python_trained_sim"
-python_simulator.slalom_drive(save_dir=save_dir,load_dir=load_dir,use_trained_model=True,initial_error=initial_error)
+It can be seen that the lateral deviation has improved significantly.
+However, the difference in driving with and without LSTM is not very apparent.
+
+To see the difference, for example, we can experiment with parameters such as steer_time_delay.
+
+First, to restore nominal model settings to default values, set the value of `nominal_parameter:vehicle_info:wheel_base` in [nominal_param.yaml](./autoware_smart_mpc_trajectory_follower/param/nominal_param.yaml) to 2.79, and run the following command:
+
+```bash
+python3 -m smart_mpc_trajectory_follower.clear_pycache
```
-The following results were obtained.
+Next, modify `sim_setting.json` as follows:
+
+```json
+{ "steer_time_delay": 1.01 }
+```
+
+In this way, an experiment is performed when `steer_time_delay` is set to 1.01 sec.
+
+The result of the driving using the nominal model is as follows:
- 
+ 
-It can be seen that the lateral deviation has improved significantly.
+The result of the driving using the trained model with LSTM is as follows:
-Here we have described wheel base, but the parameters that can be passed to the python simulator are as follows.
+
+ 
+
+
+The result of the driving using the trained model without LSTM is as follows:
+
+
+ 
+
+
+It can be seen that the performance with the model that includes LSTM is significantly better than with the model that does not.
+
+The parameters that can be passed to the python simulator are as follows.
| Parameter | Type | Description |
| ------------------------ | ----------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
@@ -226,6 +263,7 @@ Here we have described wheel base, but the parameters that can be passed to the
| accel_map_scale | float | Parameter that magnifies the corresponding distortion from acceleration input values to actual acceleration realizations.
Correspondence information is kept in `control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/accel_map.csv`. |
| acc_scaling | float | acceleration scaling |
| steer_scaling | float | steer scaling |
+| vehicle_type | int | Take values from 0 to 4 for pre-designed vehicle types.
A description of each vehicle type is given below. |
For example, to give the simulation side 0.01 [rad] of steer bias and 0.001 [rad] of steer dead band, edit the `sim_setting.json` as follows.
@@ -233,15 +271,74 @@ For example, to give the simulation side 0.01 [rad] of steer bias and 0.001 [rad
{ "steer_bias": 0.01, "steer_dead_band": 0.001 }
```
-#### Auto test on python simulator
+##### vehicle_type_0
-Here, we describe a method for testing adaptive performance by giving the simulation side a predefined range of model parameters while the control side is given constant model parameters.
+This vehicle type matches the default vehicle type used in the control.
-First, to restore nominal model settings to default values, set the value of `nominal_parameter:vehicle_info:wheel_base` in [nominal_param.yaml](./autoware_smart_mpc_trajectory_follower/param/nominal_param.yaml) to 2.79, and run the following command:
+| Parameter | value |
+| ------------------- | ----- |
+| wheel_base | 2.79 |
+| acc_time_delay | 0.1 |
+| steer_time_delay | 0.27 |
+| acc_time_constant | 0.1 |
+| steer_time_constant | 0.24 |
+| acc_scaling | 1.0 |
-```bash
-python3 -m smart_mpc_trajectory_follower.clear_pycache
-```
+##### vehicle_type_1
+
+This vehicle type is intended for a heavy bus.
+
+| Parameter | value |
+| ------------------- | ----- |
+| wheel_base | 4.76 |
+| acc_time_delay | 1.0 |
+| steer_time_delay | 1.0 |
+| acc_time_constant | 1.0 |
+| steer_time_constant | 1.0 |
+| acc_scaling | 0.2 |
+
+##### vehicle_type_2
+
+This vehicle type is intended for a light bus.
+
+| Parameter | value |
+| ------------------- | ----- |
+| wheel_base | 4.76 |
+| acc_time_delay | 0.5 |
+| steer_time_delay | 0.5 |
+| acc_time_constant | 0.5 |
+| steer_time_constant | 0.5 |
+| acc_scaling | 0.5 |
+
+##### vehicle_type_3
+
+This vehicle type is intended for a small vehicle.
+
+| Parameter | value |
+| ------------------- | ----- |
+| wheel_base | 1.335 |
+| acc_time_delay | 0.3 |
+| steer_time_delay | 0.3 |
+| acc_time_constant | 0.3 |
+| steer_time_constant | 0.3 |
+| acc_scaling | 1.5 |
+
+##### vehicle_type_4
+
+This vehicle type is intended for a small robot.
+
+| Parameter | value |
+| ------------------- | ----- |
+| wheel_base | 0.395 |
+| acc_time_delay | 0.2 |
+| steer_time_delay | 0.2 |
+| acc_time_constant | 0.2 |
+| steer_time_constant | 0.2 |
+| acc_scaling | 1.0 |
+
+#### Auto test on python simulator
+
+Here, we describe a method for testing adaptive performance by giving the simulation side a predefined range of model parameters while the control side is given constant model parameters.
To run a driving experiment within the parameter change range set in [run_sim.py](./autoware_smart_mpc_trajectory_follower/python_simulator/run_sim.py), for example, move to `control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator` and run the following command:
@@ -251,27 +348,43 @@ python3 run_sim.py --param_name steer_bias
Here we described the experimental procedure for steer bias, and the same method can be used for other parameters.
-If you want to do it for all parameters at once, run the following command:
+To run the test for all parameters except limits at once, run the following command:
```bash
-yes | python3 run_sim.py
+python3 run_auto_test.py
```
+The results are stored in the `auto_test` directory.
+After the executions were completed, the following results were obtained by running [plot_auto_test_result.ipynb](./autoware_smart_mpc_trajectory_follower/python_simulator/plot_auto_test_result.ipynb):
+
+
+ 
+
+
+The orange line shows the intermediate model trained using pure pursuit figure eight drive, and the blue line shows the final model trained using data from both the intermediate model and the figure eight drive.
+In most cases, sufficient performance is obtained, but for `vehicle_type_1`, which is intended for a heavy bus, a lateral deviation of about 2 m was observed, which is not satisfactory.
+
In `run_sim.py`, the following parameters can be set:
-| Parameter | Type | Description |
-| ------------------------- | ---- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
-| USE_TRAINED_MODEL_DIFF | bool | Whether the derivative of the trained model is reflected in the control |
-| DATA_COLLECTION_MODE | str | Which method will be used to collect the training data
"ff": Straight line driving with feed-forward input
"pp": Figure eight driving with pure pursuit control
"mpc": Slalom driving with mpc |
-| USE_POLYNOMIAL_REGRESSION | bool | Whether to perform polynomial regression before NN |
-| USE_SELECTED_POLYNOMIAL | bool | When USE_POLYNOMIAL_REGRESSION is True, perform polynomial regression using only some preselected polynomials.
The choice of polynomials is intended to be able to absorb the contribution of some parameter shifts based on the nominal model of the vehicle. |
-| FORCE_NN_MODEL_TO_ZERO | bool | Whether to force the NN model to zero (i.e., erase the contribution of the NN model).
When USE_POLYNOMIAL_REGRESSION is True, setting FORCE_MODEL_TO_ZERO to True allows the control to reflect the results of polynomial regression only, without using NN models. |
-| FIT_INTERCEPT | bool | Whether to include bias in polynomial regression.
If it is False, perform the regression with a polynomial of the first degree or higher. |
-| USE_INTERCEPT | bool | When a polynomial regression including bias is performed, whether to use or discard the resulting bias information.
It is meaningful only if FIT_INTERCEPT is True.
If it is False, discard the bias in the polynomial regression in the hope that the NN model can remove the bias term, even if the polynomial regression is performed with the bias included. |
+| Parameter | Type | Description |
+| ------------------------- | ------------------ | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
+| USE_TRAINED_MODEL_DIFF | bool | Whether the derivative of the trained model is reflected in the control |
+| DATA_COLLECTION_MODE | DataCollectionMode | Which method will be used to collect the training data
"DataCollectionMode.ff": Straight line driving with feed-forward input
"DataCollectionMode.pp": Figure eight driving with pure pursuit control
"DataCollectionMode.mpc": Slalom driving with mpc |
+| USE_POLYNOMIAL_REGRESSION | bool | Whether to perform polynomial regression before NN |
+| USE_SELECTED_POLYNOMIAL | bool | When USE_POLYNOMIAL_REGRESSION is True, perform polynomial regression using only some preselected polynomials.
The choice of polynomials is intended to be able to absorb the contribution of some parameter shifts based on the nominal model of the vehicle. |
+| FORCE_NN_MODEL_TO_ZERO | bool | Whether to force the NN model to zero (i.e., erase the contribution of the NN model).
When USE_POLYNOMIAL_REGRESSION is True, setting FORCE_MODEL_TO_ZERO to True allows the control to reflect the results of polynomial regression only, without using NN models. |
+| FIT_INTERCEPT | bool | Whether to include bias in polynomial regression.
If it is False, perform the regression with a polynomial of the first degree or higher. |
+| USE_INTERCEPT | bool | When a polynomial regression including bias is performed, whether to use or discard the resulting bias information.
It is meaningful only if FIT_INTERCEPT is True.
If it is False, discard the bias in the polynomial regression in the hope that the NN model can remove the bias term, even if the polynomial regression is performed with the bias included. |
> [!NOTE]
> When `run_sim.py` is run, the `use_trained_model_diff` set in `run_sim.py` takes precedence over the `trained_model_parameter:control_application:use_trained_model_diff` set in [trained_model_param.yaml](./autoware_smart_mpc_trajectory_follower/param/trained_model_param.yaml).
+#### Kernel density estimation of pure pursuit driving data
+
+The distribution of data obtained from pure pursuit runs can be displayed using Kernel density estimation. To do this, run [density_estimation.ipynb](./autoware_smart_mpc_trajectory_follower/python_simulator/density_estimation.ipynb).
+
+The correlation between the minimum value of the density estimate and the lateral deviation of the run results is low. A scalar indicator that better predicts the value of lateral deviation is under development.
+
## Change of nominal parameters and their reloading
The nominal parameters of vehicle model can be changed by editing the file [nominal_param.yaml](./autoware_smart_mpc_trajectory_follower/param/nominal_param.yaml).
@@ -293,7 +406,7 @@ The nominal parameters include the following:
## Change of control parameters and their reloading
-The control parameters can be changed by editing files [mpc_param.yaml](./smart_mpc_trajectory_follower/param/mpc_param.yaml) and [trained_model_param.yaml](./autoware_smart_mpc_trajectory_follower/param/trained_model_param.yaml).
+The control parameters can be changed by editing files [mpc_param.yaml](./autoware_smart_mpc_trajectory_follower/param/mpc_param.yaml) and [trained_model_param.yaml](./autoware_smart_mpc_trajectory_follower/param/trained_model_param.yaml).
Although it is possible to reflect parameter changes by restarting autoware, the following command allows us to do so without leaving autoware running:
```bash
@@ -304,21 +417,29 @@ The main parameters among the control parameters are as follows.
### `mpc_param.yaml`
-| Parameter | Type | Description |
-| ------------------------------------ | ----------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
-| mpc_parameter:system:mode | str | control mode
"ilqr": iLQR mode
"mppi": MPPI mode
"mppi_ilqr": the initial value of iLQR is given by the MPPI solution. |
-| mpc_parameter:cost_parameters:Q | list[float] | Stage cost for states.
List of length 8, in order: straight deviation, lateral deviation, velocity deviation, yaw angle deviation, acceleration deviation, steer deviation, acceleration input deviation, steer input deviation cost weights. |
-| mpc_parameter:cost_parameters:Q_c | list[float] | Cost in the horizon corresponding to the following timing_Q_c for the states.
The correspondence of the components of the list is the same as for Q. |
-| mpc_parameter:cost_parameters:Q_f | list[float] | Termination cost for the states.
The correspondence of the components of the list is the same as for Q. |
-| mpc_parameter:cost_parameters:R | list[float] | A list of length 2 where R[0] is weight of cost for the change rate of acceleration input value and R[1] is weight of cost for the change rate of steer input value. |
-| mpc_parameter:mpc_setting:timing_Q_c | list[int] | Horizon numbers such that the stage cost for the states is set to Q_c. |
+| Parameter | Type | Description |
+| ------------------------------------------ | ----------- | -------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
+| mpc_parameter:system:mode | str | control mode
"ilqr": iLQR mode
"mppi": MPPI mode
"mppi_ilqr": the initial value of iLQR is given by the MPPI solution. |
+| mpc_parameter:cost_parameters:Q | list[float] | Stage cost for states.
List of length 8, in order: straight deviation, lateral deviation, velocity deviation, yaw angle deviation, acceleration deviation, steer deviation, acceleration input deviation, steer input deviation cost weights. |
+| mpc_parameter:cost_parameters:Q_c | list[float] | Cost in the horizon corresponding to the following timing_Q_c for the states.
The correspondence of the components of the list is the same as for Q. |
+| mpc_parameter:cost_parameters:Q_f | list[float] | Termination cost for the states.
The correspondence of the components of the list is the same as for Q. |
+| mpc_parameter:cost_parameters:R | list[float] | A list of length 2 where R[0] is weight of cost for the change rate of acceleration input value and R[1] is weight of cost for the change rate of steer input value. |
+| mpc_parameter:mpc_setting:timing_Q_c | list[int] | Horizon numbers such that the stage cost for the states is set to Q_c. |
+| mpc_parameter:compensation:acc_fb_decay | float | Coefficient of damping in integrating the error between the observed and predicted acceleration values in the compensator outside the MPC. |
+| mpc_parameter:compensation:acc_fb_gain | float | Gain of acceleration compensation. |
+| mpc_parameter:compensation:max_error_acc | float | Maximum acceleration compensation (m/s^2) |
+| mpc_parameter:compensation:steer_fb_decay | float | Coefficient of damping in integrating the error between the observed and predicted steering values in the compensator outside the MPC. |
+| mpc_parameter:compensation:steer_fb_gain | float | Gain of steering compensation. |
+| mpc_parameter:compensation:max_error_steer | float | Maximum steering compensation (rad) |
### `trained_model_param.yaml`
-| Parameter | Type | Description |
-| ------------------------------------------------------------------ | ---- | --------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
-| trained_model_parameter:control_application:use_trained_model | bool | Whether the trained model is reflected in the control or not. |
-| trained_model_parameter:control_application:use_trained_model_diff | bool | Whether the derivative of the trained model is reflected on the control or not.
It is meaningful only when use_trained_model is True, and if False, the nominal model is used for the derivative of the dynamics, and trained model is used only for prediction. |
+| Parameter | Type | Description |
+| ------------------------------------------------------------------- | ---- | --------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
+| trained_model_parameter:control_application:use_trained_model | bool | Whether the trained model is reflected in the control or not. |
+| trained_model_parameter:control_application:use_trained_model_diff | bool | Whether the derivative of the trained model is reflected on the control or not.
It is meaningful only when use_trained_model is True, and if False, the nominal model is used for the derivative of the dynamics, and trained model is used only for prediction. |
+| trained_model_parameter:memory_for_training:use_memory_for_training | bool | Whether to use the model that includes LSTM for learning or not. |
+| trained_model_parameter:memory_for_training:use_memory_diff | bool | Whether the derivative with respect to the cell state and hidden state at the previous time of LSTM is reflected in the control or not. |
## Request to release the slow stop mode
@@ -336,3 +457,5 @@ ros2 topic pub /pympc_stop_mode_reset_request std_msgs/msg/String "data: ''" --o
- It may take some time until the end of the planning to compile numba functions at the start of the first control.
- In the stopping action near the goal our control switches to another simple control law. As a result, the stopping action may not work except near the goal. Stopping is also difficult if the acceleration map is significantly shifted.
+
+- If the dynamics deviates too much from the nominal model, as in `vehicle_type_1`, which is intended for heavy buses, it may not be well controlled.
diff --git a/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/param/mpc_param.yaml b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/param/mpc_param.yaml
index 710d1cc992949..797976994ff7e 100644
--- a/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/param/mpc_param.yaml
+++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/param/mpc_param.yaml
@@ -1,11 +1,11 @@
mpc_parameter:
system:
- mode: mppi_ilqr # option: ilqr, mppi, mppi_ilqr
+ mode: ilqr # option: ilqr, mppi, mppi_ilqr
mpc_setting:
ctrl_time_step: 0.03333
mpc_freq: 3
N: 50
- steer_ctrl_queue_size: 50
+ steer_ctrl_queue_size: 17
steer_ctrl_queue_size_core: 15
acc_ctrl_queue_size: 12
nx_0: 6
@@ -14,21 +14,21 @@ mpc_parameter:
cost_parameters:
Q: [0.0, 10.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
Q_c: [1e+2, 1e+8, 1e+6, 1e+3, 1.0, 1.0, 1.0, 1.0]
- Q_f: [1e+2, 1e+8, 1e+2, 1e+8, 1.0, 1.0, 1.0, 1.0]
+ Q_f: [1e+2, 1e+8, 1e+2, 1e+7, 1.0, 1.0, 1.0, 1.0]
R: [10.0, 1000.0]
acc_lim_weight: 100.0
steer_lim_weight: 100.0
acc_rate_lim_weight: 10000.0
steer_rate_lim_weight: 10000.0
- min_steer_rate_transform_for_start: 0.01
- power_steer_rate_transform_for_start: 5
- coef_steer_rate_transform_for_start: 3.0
- min_loose_lateral_cost: 0.00001
- power_loose_lateral_cost: 10
- threshold_loose_lateral_cost: 0.2
- min_loose_yaw_cost: 0.00001
- power_loose_yaw_cost: 1
- threshold_loose_yaw_cost: 0.1
+ vel_steer_cost_coef_table: [0.01, 0.01, 0.08, 0.36, 1.0]
+ vel_steer_table: [0.01, 0.1, 0.2, 0.27, 0.33]
+ lateral_cost_coef_table: [0.00001, 0.0005, 0.06, 0.27, 1.0]
+ lateral_error_table: [0.01, 0.1, 0.15, 0.175, 0.2]
+ yaw_cost_coef_table: [0.00001, 1.0]
+ yaw_error_table: [0.00001, 0.1]
+ steer_rate_cost_table: [10.0, 5.0, 1.0]
+ curvature_table: [0.01, 0.03, 0.05]
+ use_max_curvature: true
ilqr:
ls_step: 0.9
max_iter_ls: 10
@@ -40,17 +40,31 @@ mpc_parameter:
max_iter_mppi: 2
sample_num: 100
mppi_tol: 0.5
- mppi_step: 20
+ mppi_step: 7
+ pure_pursuit:
+ acc_kp: 0.5
+ lookahead_time: 3.0
+ min_lookahead: 10.0
+ steer_kp_param: 2.0
+ steer_kd_param: 2.0
+ naive_pure_pursuit:
+ acc_kp: 0.5
+ lookahead_coef: 1.0
+ lookahead_intercept: 5.0
preprocessing:
reference_horizon: 50
cap_pred_error: [0.5, 2.0]
use_sg_for_nominal_inputs: true
sg_deg_for_nominal_inputs: 0
sg_window_size_for_nominal_inputs: 10
- to_be_deprecated:
- tighten_horizon: 20
- min_tighten_steer_rate: 1.0
- power_tighten_steer_rate_by_lateral_error: 1
- threshold_tighten_steer_rate_by_lateral_error: 0.05
- power_tighten_steer_rate_by_yaw_error: 1
- threshold_tighten_steer_rate_by_yaw_error: 0.05
+ compensation:
+ acc_fb_decay: 0.1
+ acc_fb_gain: 1.0
+ acc_fb_sec_order_ratio: 1.0
+ max_error_acc: 1e-3
+ steer_fb_decay: 0.001
+ steer_fb_gain: 1.0
+ steer_fb_sec_order_ratio: 1.0
+ max_error_steer: 1e-2
+ limit:
+ read_limit_file: false
diff --git a/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/param/trained_model_param.yaml b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/param/trained_model_param.yaml
index 6d1973586a5eb..b144bde856016 100644
--- a/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/param/trained_model_param.yaml
+++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/param/trained_model_param.yaml
@@ -5,6 +5,8 @@ trained_model_parameter:
max_train_data_size: 10000
error_decay: [0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5]
use_trained_model_diff: true
+ minimum_steer_diff: 0.03
+ reflect_only_poly_diff: false
use_sg_for_trained_model_diff: true
sg_deg_for_trained_model_diff: 0
sg_window_size_for_trained_model_diff: 25
@@ -17,6 +19,12 @@ trained_model_parameter:
use_theta_noise: false
use_acc_noise: false
use_steer_noise: false
+ memory_for_training:
+ use_memory_for_training: true
+ use_memory_diff: true
+ use_sg_for_memory_diff: true
+ sg_deg_for_memory_diff: 0
+ sg_window_size_for_memory_diff: 10
smoothing:
acc_sigma_for_learning: 5.0
steer_sigma_for_learning: 5.0
@@ -28,3 +36,29 @@ trained_model_parameter:
theta_out_sigma_for_learning: 10.0
acc_out_sigma_for_learning: 5.0
steer_out_sigma_for_learning: 5.0
+ normalize:
+ vel_normalize: 0.2
+ acc_normalize: 1.0
+ steer_normalize: 3.0
+ weight:
+ NN_x_weight: 1.0
+ NN_y_weight: 1.0
+ NN_v_weight: 1.0
+ NN_yaw_weight: 2.0
+ NN_acc_weight: 1.0
+ NN_steer_weight: 2.0
+ NN_x_weight_diff: 0.001
+ NN_y_weight_diff: 0.01
+ NN_v_weight_diff: 0.001
+ NN_yaw_weight_diff: 0.1
+ NN_acc_weight_diff: 0.1
+ NN_steer_weight_diff: 0.1
+ NN_x_weight_two_diff: 0.0001
+ NN_y_weight_two_diff: 0.001
+ NN_v_weight_two_diff: 0.00001
+ NN_yaw_weight_two_diff: 0.01
+ NN_acc_weight_two_diff: 0.01
+ NN_steer_weight_two_diff: 0.01
+ finalize_x_weight: 10.0
+ finalize_y_weight: 10.0
+ finalize_v_weight: 10.0
diff --git a/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/.gitignore b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/.gitignore
index 9f6b19213255b..5b39e844b3405 100644
--- a/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/.gitignore
+++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/.gitignore
@@ -8,6 +8,7 @@ train_drive_*.png
test_feedforward_*/
test_pure_pursuit_*/
test_python_*/
+test_*/
python_sim_log_*/
sim_setting.json
auto_test_result_*.csv
diff --git a/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/assets.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/assets.py
new file mode 100644
index 0000000000000..1bf9c7ac894d3
--- /dev/null
+++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/assets.py
@@ -0,0 +1,261 @@
+# Copyright 2024 Proxima Technology Inc, TIER IV
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+from enum import Enum
+import os
+
+import numpy as np
+
+CHANGE_PARAM_P1 = 0.8
+CHANGE_PARAM_P2 = 1.2
+
+
+class ControlType(Enum):
+ ff = "feed_forward"
+ pp_eight = "pure_pursuit_figure_eight"
+ pp_straight = "pure_pursuit_straight"
+ npp_eight = "naive_pure_pursuit_figure_eight"
+ mpc = "smart_mpc"
+
+ def __str__(self) -> str:
+ return self.name
+
+
+class DataCollectionMode(Enum):
+ ff = "feed_forward"
+ pp = "pure_pursuit"
+ npp = "naive_pure_pursuit"
+ mpc = "nominal_mpc"
+
+ def __str__(self) -> str:
+ return self.name
+
+ def toControlTypes(self) -> list[ControlType]:
+ if self.name == "pp":
+ return [ControlType.pp_eight, ControlType.pp_straight]
+ elif self.name == "npp":
+ return [ControlType.npp_eight]
+ elif self.name == "ff":
+ return [ControlType.ff]
+ elif self.name == "mpc":
+ return [ControlType.mpc]
+ else:
+ print(f"Error: unexpected DataCollectionMode: {self}")
+ raise Exception
+
+
+class ChangeParam(Enum):
+ """Parameters to be changed when running the simulation."""
+
+ steer_bias = [
+ -1.0 * np.pi / 180.0,
+ -0.8 * np.pi / 180.0,
+ -0.6 * np.pi / 180.0,
+ -0.4 * np.pi / 180.0,
+ -0.2 * np.pi / 180.0,
+ 0.0,
+ 0.2 * np.pi / 180.0,
+ 0.4 * np.pi / 180.0,
+ 0.6 * np.pi / 180.0,
+ 0.8 * np.pi / 180.0,
+ 1.0 * np.pi / 180.0,
+ ]
+ """steer midpoint (soft + hard)"""
+
+ steer_rate_lim = [0.020, 0.050, 0.100, 0.150, 0.200, 0.300, 0.400, 0.500]
+ """Maximum steer angular velocity"""
+
+ vel_rate_lim = [0.5, 1.0, 3.0, 5.0, 7.0, 9.0]
+ """Maximum acceleration/deceleration"""
+
+ wheel_base = [0.5, 1.0, 1.5, 2.0, 3.0, 5.0, 7.0]
+ """wheel base"""
+
+ steer_dead_band = [0.0000, 0.0012, 0.0025, 0.0050, 0.01]
+ """steer dead band"""
+
+ adaptive_gear_ratio_coef = [
+ [15.713, 0.053, 0.042, 15.713, 0.053, 0.042],
+ [15.713, 0.053, 0.042, CHANGE_PARAM_P1 * 15.713, 0.053, 0.042],
+ [15.713, 0.053, 0.042, CHANGE_PARAM_P2 * 15.713, 0.053, 0.042],
+ [15.713, 0.053, 0.042, 15.713, CHANGE_PARAM_P1 * 0.053, 0.042],
+ [15.713, 0.053, 0.042, CHANGE_PARAM_P1 * 15.713, CHANGE_PARAM_P1 * 0.053, 0.042],
+ [15.713, 0.053, 0.042, CHANGE_PARAM_P2 * 15.713, CHANGE_PARAM_P1 * 0.053, 0.042],
+ [15.713, 0.053, 0.042, 15.713, CHANGE_PARAM_P2 * 0.053, 0.042],
+ [15.713, 0.053, 0.042, CHANGE_PARAM_P1 * 15.713, CHANGE_PARAM_P2 * 0.053, 0.042],
+ [15.713, 0.053, 0.042, CHANGE_PARAM_P2 * 15.713, CHANGE_PARAM_P2 * 0.053, 0.042],
+ [15.713, 0.053, 0.042, 15.713, 0.053, CHANGE_PARAM_P1 * 0.042],
+ [15.713, 0.053, 0.042, CHANGE_PARAM_P1 * 15.713, 0.053, CHANGE_PARAM_P1 * 0.042],
+ [15.713, 0.053, 0.042, CHANGE_PARAM_P2 * 15.713, 0.053, CHANGE_PARAM_P1 * 0.042],
+ [15.713, 0.053, 0.042, 15.713, CHANGE_PARAM_P1 * 0.053, CHANGE_PARAM_P1 * 0.042],
+ [
+ 15.713,
+ 0.053,
+ 0.042,
+ CHANGE_PARAM_P1 * 15.713,
+ CHANGE_PARAM_P1 * 0.053,
+ CHANGE_PARAM_P1 * 0.042,
+ ],
+ [
+ 15.713,
+ 0.053,
+ 0.042,
+ CHANGE_PARAM_P2 * 15.713,
+ CHANGE_PARAM_P1 * 0.053,
+ CHANGE_PARAM_P1 * 0.042,
+ ],
+ [15.713, 0.053, 0.042, 15.713, CHANGE_PARAM_P2 * 0.053, CHANGE_PARAM_P1 * 0.042],
+ [
+ 15.713,
+ 0.053,
+ 0.042,
+ CHANGE_PARAM_P1 * 15.713,
+ CHANGE_PARAM_P2 * 0.053,
+ CHANGE_PARAM_P1 * 0.042,
+ ],
+ [
+ 15.713,
+ 0.053,
+ 0.042,
+ CHANGE_PARAM_P2 * 15.713,
+ CHANGE_PARAM_P2 * 0.053,
+ CHANGE_PARAM_P1 * 0.042,
+ ],
+ [15.713, 0.053, 0.042, 15.713, 0.053, CHANGE_PARAM_P2 * 0.042],
+ [15.713, 0.053, 0.042, CHANGE_PARAM_P1 * 15.713, 0.053, CHANGE_PARAM_P2 * 0.042],
+ [15.713, 0.053, 0.042, CHANGE_PARAM_P2 * 15.713, 0.053, CHANGE_PARAM_P2 * 0.042],
+ [15.713, 0.053, 0.042, 15.713, CHANGE_PARAM_P1 * 0.053, CHANGE_PARAM_P2 * 0.042],
+ [
+ 15.713,
+ 0.053,
+ 0.042,
+ CHANGE_PARAM_P1 * 15.713,
+ CHANGE_PARAM_P1 * 0.053,
+ CHANGE_PARAM_P2 * 0.042,
+ ],
+ [
+ 15.713,
+ 0.053,
+ 0.042,
+ CHANGE_PARAM_P2 * 15.713,
+ CHANGE_PARAM_P1 * 0.053,
+ CHANGE_PARAM_P2 * 0.042,
+ ],
+ [15.713, 0.053, 0.042, 15.713, CHANGE_PARAM_P2 * 0.053, CHANGE_PARAM_P2 * 0.042],
+ [
+ 15.713,
+ 0.053,
+ 0.042,
+ CHANGE_PARAM_P1 * 15.713,
+ CHANGE_PARAM_P2 * 0.053,
+ CHANGE_PARAM_P2 * 0.042,
+ ],
+ [
+ 15.713,
+ 0.053,
+ 0.042,
+ CHANGE_PARAM_P2 * 15.713,
+ CHANGE_PARAM_P2 * 0.053,
+ CHANGE_PARAM_P2 * 0.042,
+ ],
+ ]
+ """velocity-dependent gear ratio"""
+
+ acc_time_delay = [0.00, 0.1, 0.27, 0.40, 0.60, 0.80, 1.01]
+ """acc time delay"""
+
+ steer_time_delay = [0.00, 0.1, 0.27, 0.40, 0.60, 0.80, 1.02]
+ """steer time delay"""
+
+ acc_time_constant = [0.01, 0.1, 0.20, 0.24, 0.40, 0.60, 0.80, 1.01]
+ """time constant"""
+
+ steer_time_constant = [0.01, 0.1, 0.20, 0.24, 0.40, 0.60, 0.80, 1.02]
+ """time constant"""
+
+ accel_map_scale = [0.2, 0.5, 1.0, 2.0, 3.0, 4.0, 5.0]
+ """pedal - real acceleration correspondence"""
+
+ acc_scaling = [0.2, 0.5, 1.0, 2.0, 3.0, 4.0, 5.01]
+ """Acceleration scaling coefficient"""
+
+ steer_scaling = [0.2, 0.5, 1.0, 2.0, 3.0, 4.0, 5.02]
+ """Steer scaling coefficient"""
+
+ vehicle_type = [0, 1, 2, 3, 4]
+ """change to other vehicle parameters"""
+
+ test_vehicle = [0, 1, 2, 3, 4, 5, 6, 7]
+
+
+def test_dir_name(
+ root: str = ".",
+ data_collection_mode: DataCollectionMode | None = None,
+ control_type: ControlType | None = None,
+ trained: bool = False,
+ change_param: ChangeParam | None = None,
+ index: int | None = None,
+ validation_flag: bool = False,
+ use_memory_diff: bool = False,
+) -> str:
+ """Generate string for directory name."""
+ dir_name = root + "/test"
+ if control_type is not None:
+ dir_name += f"_{control_type.value}_sim"
+ elif data_collection_mode is not None:
+ dir_name += f"_{data_collection_mode}_aided_sim"
+ else:
+ dir_name += "_sim"
+
+ if trained:
+ dir_name += "_trained"
+ if change_param is not None:
+ dir_name += f"_{change_param.name}"
+ if index is not None:
+ dir_name += f"_{index}th"
+ if validation_flag:
+ dir_name += "_val"
+ if use_memory_diff:
+ dir_name += "_mem_diff"
+ return dir_name
+
+
+class DirGenerator:
+ """Class to store parameters for `test_dir_name`."""
+
+ def __init__(self, root: str):
+ # create directory if not exist
+ if not os.path.isdir(root):
+ os.mkdir(root)
+ self.root = root
+
+ def test_dir_name(
+ self,
+ data_collection_mode: DataCollectionMode | None = None,
+ control_type: ControlType | None = None,
+ trained: bool = False,
+ change_param: ChangeParam | None = None,
+ index: int | None = None,
+ validation_flag: bool = False,
+ use_memory_diff: bool = False,
+ ):
+ return test_dir_name(
+ root=self.root,
+ data_collection_mode=data_collection_mode,
+ control_type=control_type,
+ trained=trained,
+ change_param=change_param,
+ index=index,
+ validation_flag=validation_flag,
+ use_memory_diff=use_memory_diff,
+ )
diff --git a/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/data_collection_utils.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/data_collection_utils.py
new file mode 100644
index 0000000000000..5b681328ec7da
--- /dev/null
+++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/data_collection_utils.py
@@ -0,0 +1,617 @@
+# Copyright 2024 Proxima Technology Inc, TIER IV
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import csv
+
+from autoware_smart_mpc_trajectory_follower.scripts import drive_functions
+from numba import njit # type: ignore
+import numpy as np
+from numpy import arctan
+from numpy import cos
+from numpy import pi
+from numpy import sin
+
+RANDOM_SEED_STEP_RESPONSE = 42
+
+
+def get_feedforward_nominal_input(t, trajectory_data):
+ """Calculate the nominal input for feed-forward driving."""
+ total_time = trajectory_data[-1, 0]
+ t_current = t - (t // total_time) * total_time
+ nearest_index = np.argmin(np.abs(trajectory_data[:, 0] - t_current))
+ return trajectory_data[nearest_index, [5, 6]]
+
+
+def create_additional_sine_data(
+ seed,
+ t_range,
+ acc_amp_range,
+ acc_period_range,
+ steer_amp_range,
+ steer_period_range,
+ large_steer_amp_range,
+ large_steer_period_range,
+ start_large_steer_time,
+):
+ """Create sine wave data to be added randomly to feed-forward runs."""
+ np.random.seed(seed=seed)
+ t_acc = 0.0
+ t_steer = 0.0
+ t_large_steer = 0.0
+ t_acc_list = []
+ t_steer_list = []
+ t_large_steer_list = []
+ t_acc_list.append(t_acc)
+ t_steer_list.append(t_steer)
+ t_large_steer_list.append(t_large_steer)
+ t_large_steer += start_large_steer_time
+ t_large_steer_list.append(t_large_steer)
+ amp_acc_list = []
+ amp_steer_list = []
+ amp_large_steer_list = []
+ amp_large_steer_list.append(0)
+ while True:
+ if max(t_acc, t_large_steer) >= t_steer:
+ period = (
+ steer_period_range[1] - steer_period_range[0]
+ ) * np.random.uniform() + steer_period_range[0]
+ t_steer += period
+ t_steer_list.append(t_steer)
+ amp_steer_list.append(steer_amp_range * np.random.uniform())
+ elif t_large_steer >= t_acc:
+ period = (
+ acc_period_range[1] - acc_period_range[0]
+ ) * np.random.uniform() + acc_period_range[0]
+ t_acc += period
+ t_acc_list.append(t_acc)
+ amp_acc_list.append(acc_amp_range * np.random.uniform())
+ else:
+ period = (
+ large_steer_period_range[1] - large_steer_period_range[0]
+ ) * np.random.uniform() + large_steer_period_range[0]
+ t_large_steer += period
+ t_large_steer_list.append(t_large_steer)
+ amp_large_steer_list.append(large_steer_amp_range * np.random.uniform())
+ if t_acc >= t_range[1] and t_steer >= t_range[1] and t_large_steer >= t_range[1]:
+ break
+ return (
+ np.array(t_acc_list),
+ np.array(amp_acc_list),
+ np.array(t_steer_list),
+ np.array(amp_steer_list),
+ np.array(t_large_steer_list),
+ np.array(amp_large_steer_list),
+ )
+
+
+# cSpell:ignore njit fastmath numba
+@njit(cache=False, fastmath=True)
+def get_current_additional_sine(
+ t,
+ t_acc_array,
+ amp_acc_array,
+ t_steer_array,
+ amp_steer_array,
+ t_large_steer_array,
+ amp_large_steer_array,
+):
+ """Calculate current values from already created sine wave data."""
+ acc_index = 0
+ steer_index = 0
+ large_steer_index = 0
+ for i in range(t_acc_array.shape[0] - 1):
+ if t < t_acc_array[i + 1]:
+ break
+ acc_index += 1
+ for i in range(t_steer_array.shape[0] - 1):
+ if t < t_steer_array[i + 1]:
+ break
+ steer_index += 1
+ for i in range(t_large_steer_array.shape[0] - 1):
+ if t < t_large_steer_array[i + 1]:
+ break
+ large_steer_index += 1
+ acc = amp_acc_array[acc_index] * np.sin(
+ 2
+ * np.pi
+ * (t - t_acc_array[acc_index])
+ / (t_acc_array[acc_index + 1] - t_acc_array[acc_index])
+ )
+ steer = amp_steer_array[steer_index] * np.sin(
+ 2
+ * np.pi
+ * (t - t_steer_array[steer_index])
+ / (t_steer_array[steer_index + 1] - t_steer_array[steer_index])
+ )
+ steer += amp_large_steer_array[large_steer_index] * np.sin(
+ 2
+ * np.pi
+ * (t - t_large_steer_array[large_steer_index])
+ / (t_large_steer_array[large_steer_index + 1] - t_large_steer_array[large_steer_index])
+ )
+ return np.array([acc, steer])
+
+
+@njit(cache=False, fastmath=True)
+def get_vel_sine(t, v_mid, v_range, period, constant_vel_time):
+ """Calculate current target velocity values already created sine wave data."""
+ if t < period / 4:
+ vel = v_mid + v_range * np.sin(2 * np.pi * t / period)
+ elif t < period / 4 + constant_vel_time:
+ vel = v_mid + v_range
+ elif t < 3 * period / 4 + constant_vel_time:
+ vel = v_mid + v_range * np.sin(2 * np.pi * (t - constant_vel_time) / period)
+ elif t < 3 * period / 4 + 2 * constant_vel_time:
+ vel = v_mid - v_range
+ else:
+ vel = v_mid + v_range * np.sin(2 * np.pi * (t - 2 * constant_vel_time) / period)
+
+ return vel
+
+
+def get_periodic_count(counter, split_size):
+ return split_size - 0.5 - np.abs(counter % (2 * split_size - 1) - split_size + 0.5)
+
+
+def compute_curvature_radius(trajectory_position_data, trajectory_yaw_data):
+ d_step = 5
+ curvature_radius = []
+ for i in range(len(trajectory_position_data)):
+ tmp_pos = trajectory_position_data[i]
+ tmp_yaw = trajectory_yaw_data[i]
+ tmp_computed_flag = False
+ for j in range(i, len(trajectory_position_data)):
+ distance = np.sqrt(((tmp_pos[:2] - trajectory_position_data[j, :2]) ** 2).sum())
+ if distance >= d_step:
+ diff_yaw = tmp_yaw - trajectory_yaw_data[j]
+ if diff_yaw > np.pi:
+ diff_yaw -= 2 * np.pi
+ if diff_yaw < -np.pi:
+ diff_yaw += 2 * np.pi
+ curvature_radius.append(distance / (1e-12 + np.abs(diff_yaw)))
+ tmp_computed_flag = True
+ break
+ if tmp_computed_flag is False:
+ curvature_radius.append(1 * curvature_radius[-1])
+ curvature_radius = np.array(curvature_radius)
+ return curvature_radius
+
+
+def compute_curvature_radius_loop_trajectory(trajectory_position_data, trajectory_yaw_data):
+ data_length = len(trajectory_position_data)
+ augmented_trajectory_position_data = np.vstack(
+ [trajectory_position_data, trajectory_position_data[: data_length // 2]]
+ )
+ augmented_trajectory_yaw_data = np.hstack(
+ [trajectory_yaw_data, trajectory_yaw_data[: data_length // 2]]
+ )
+ return compute_curvature_radius(
+ augmented_trajectory_position_data, augmented_trajectory_yaw_data
+ )[:data_length]
+
+
+class StraightLine:
+ """Straight line target velocity."""
+
+ def __init__(self, v_min=1.0, v_max=11.0, acc_max=1.2, constant_vel_time=5.0, split_size=5):
+ self.v_min = v_min
+ self.v_max = v_max
+ self.v_mid = 0.5 * (v_min + v_max)
+ self.period = 2 * np.pi * (v_max - self.v_mid) / acc_max
+ self.constant_vel_time = constant_vel_time
+ self.split_size = split_size
+ self.break_flag = False
+
+ def get_current_velocity(self, t):
+ index = int(t / (self.period + 2 * self.constant_vel_time))
+ t1 = t - (self.period + 2 * self.constant_vel_time) * index
+ if index < 2 * self.split_size:
+ adjust = 0.0
+ if index >= self.split_size:
+ adjust = 0.5
+ v_range = (
+ (self.v_max - self.v_mid)
+ * (get_periodic_count(index, self.split_size) + 1 - adjust)
+ / self.split_size
+ )
+ return get_vel_sine(t1, self.v_mid, v_range, self.period, self.constant_vel_time)
+ else:
+ if t1 > 2 * self.constant_vel_time:
+ self.break_flag = True
+ return self.v_mid
+
+
+class FigureEight:
+ """Figure eight trajectory."""
+
+ def __init__(
+ self,
+ y_length: float,
+ x_length: float,
+ v_min=1.0,
+ v_max=11.0,
+ split_size=5,
+ acc_max=1.2,
+ constant_vel_time=5.0,
+ smoothing_trajectory_data_flag=False,
+ ):
+ if y_length >= x_length:
+ raise Exception("invalid argument: y_length must be less than x_length")
+ self.y_length = y_length
+ self.x_length = x_length
+ self.v_min = v_min
+ self.v_max = v_max
+ self.split_size = split_size
+
+ self.v_mid = 0.5 * (v_min + v_max)
+ self.v_start = self.v_mid
+
+ self.period = 2 * np.pi * (v_max - self.v_mid) / acc_max
+ self.constant_vel_time = constant_vel_time
+
+ self.counter = 0
+ self.previous_circle = "left_circle"
+ self.break_flag = False
+ self.accel_mode = 1
+ self.smoothing_trajectory_data_flag = smoothing_trajectory_data_flag
+
+ @property
+ def total_distance(self) -> float:
+ a = self.y_length
+ b = self.x_length
+ arc = a * pi
+ diagonal = 2 * np.sqrt((b - a) ** 2 + a**2)
+ return arc + diagonal
+
+ def get_trajectory_points(self, step: float):
+ """Get the position and yaw angle in world coordinates of the figure eight.
+
+ The return value is a 2-dimensional array of positions and a 1-dimensional array of yaw angles corresponding to `t`.
+ """
+ a = self.y_length
+ b = self.x_length
+
+ t_array = np.arange(start=0.0, stop=self.total_distance, step=step).astype("float")
+ x = t_array.copy()
+ y = t_array.copy()
+ yaw = t_array.copy()
+ curvature_radius = t_array.copy()
+ parts = []
+ achievement_rates = []
+
+ # Boundary points between circular and linear trajectory
+ C = [-(b - a) / 2, -a / 2]
+ D = [(b - a) / 2, -a / 2]
+
+ R = a / 2 # radius of the circle
+ OL = [-(b - a) / 2, 0] # center of the left circle
+ OR = [(b - a) / 2, 0] # center of the right circle
+ OB = np.sqrt((b - a) ** 2 + a**2) / 2 # half length of the linear trajectory
+ AD = 2 * OB
+ θB = arctan(a / (b - a)) # Angle that OB makes with respect to x-axis
+ BD = pi * a / 2 # the length of arc BD
+ AC = BD
+ CO = OB
+
+ i_end = t_array.shape[0]
+ for i, t in enumerate(t_array):
+ if t > OB + BD + AD + AC + CO:
+ i_end = i
+ break
+ if 0 <= t and t <= OB:
+ x[i] = (b - a) * t / (2 * OB)
+ y[i] = a * t / (2 * OB)
+ yaw[i] = θB
+ curvature_radius[i] = 1e12
+ parts.append("linear_positive")
+ achievement_rates.append(t / (2 * OB) + 0.5)
+ if OB <= t and t <= OB + BD:
+ t1 = t - OB
+ t1_rad = t1 / R
+ x[i] = OR[0] + R * cos(pi / 2 - t1_rad)
+ y[i] = OR[1] + R * sin(pi / 2 - t1_rad)
+ yaw[i] = -t1_rad
+ curvature_radius[i] = R
+ parts.append("right_circle")
+ achievement_rates.append(0.0)
+ if OB + BD <= t and t <= OB + BD + AD:
+ t2 = t - (OB + BD)
+ x[i] = D[0] - (b - a) * t2 / (2 * OB)
+ y[i] = D[1] + a * t2 / (2 * OB)
+ yaw[i] = pi - θB
+ curvature_radius[i] = 1e12
+ parts.append("linear_negative")
+ achievement_rates.append(t2 / (2 * OB))
+ if OB + BD + AD <= t and t <= OB + BD + AD + AC:
+ t3 = t - (OB + BD + AD)
+ t3_rad = t3 / R
+ x[i] = OL[0] + R * cos(pi / 2 + t3_rad)
+ y[i] = OL[1] + R * sin(pi / 2 + t3_rad)
+ yaw[i] = pi + t3_rad
+ curvature_radius[i] = R
+ parts.append("left_circle")
+ achievement_rates.append(0.0)
+ if OB + BD + AD + AC <= t and t <= OB + BD + AD + AC + CO:
+ t4 = t - (OB + BD + AD + AC)
+ x[i] = C[0] + (b - a) * t4 / (2 * OB)
+ y[i] = C[1] + a * t4 / (2 * OB)
+ yaw[i] = θB
+ curvature_radius[i] = 1e12
+ parts.append("linear_positive")
+ achievement_rates.append(t4 / (2 * OB))
+
+ # drop rest
+ x = x[:i_end]
+ y = y[:i_end]
+ trajectory_position_data = np.array([x, y]).T
+ trajectory_yaw_data = yaw[:i_end]
+ curvature_radius = curvature_radius[:i_end]
+ parts = parts[:i_end]
+ achievement_rates = achievement_rates[:i_end]
+
+ if self.smoothing_trajectory_data_flag:
+ window = 1000
+ w = np.ones(window) / window
+ augmented_position_data = np.vstack(
+ [
+ trajectory_position_data[-window:],
+ trajectory_position_data,
+ trajectory_position_data[:window],
+ ]
+ )
+ trajectory_position_data[:, 0] = (
+ 1 * np.convolve(augmented_position_data[:, 0], w, mode="same")[window:-window]
+ )
+ trajectory_position_data[:, 1] = (
+ 1 * np.convolve(augmented_position_data[:, 1], w, mode="same")[window:-window]
+ )
+ augmented_yaw_data = np.hstack(
+ [
+ trajectory_yaw_data[-window:],
+ trajectory_yaw_data,
+ trajectory_yaw_data[:window],
+ ]
+ )
+ smoothed_trajectory_yaw_data = trajectory_yaw_data.copy()
+ for i in range(len(trajectory_yaw_data)):
+ tmp_yaw = trajectory_yaw_data[i]
+ tmp_data = (
+ augmented_yaw_data[window + (i - window // 2) : window + (i + window // 2)]
+ - tmp_yaw
+ )
+ for j in range(len(tmp_data)):
+ if tmp_data[j] > np.pi:
+ tmp_data[j] -= 2 * np.pi
+ if tmp_data[j] < -np.pi:
+ tmp_data[j] += 2 * np.pi
+ tmp_data = np.convolve(tmp_data, w, mode="same")
+ smoothed_trajectory_yaw_data[i] = (
+ tmp_yaw + np.convolve(tmp_data, w, mode="same")[window // 2]
+ )
+ if smoothed_trajectory_yaw_data[i] > np.pi:
+ smoothed_trajectory_yaw_data[i] -= 2 * np.pi
+ if smoothed_trajectory_yaw_data[i] < -np.pi:
+ smoothed_trajectory_yaw_data[i] += 2 * np.pi
+
+ trajectory_yaw_data = smoothed_trajectory_yaw_data.copy()
+
+ return (
+ trajectory_position_data,
+ trajectory_yaw_data,
+ curvature_radius,
+ parts,
+ np.array(achievement_rates),
+ )
+
+ def get_current_velocity(self, t):
+ index = int(t / (self.period + 2 * self.constant_vel_time))
+ t1 = t - (self.period + 2 * self.constant_vel_time) * index
+ if index < 2 * self.split_size:
+ adjust = 0.0
+ if index >= self.split_size:
+ adjust = 0.5
+ v_range = (
+ (self.v_max - self.v_mid)
+ * (get_periodic_count(index, self.split_size) + 1 - adjust)
+ / self.split_size
+ )
+ return get_vel_sine(t1, self.v_mid, v_range, self.period, self.constant_vel_time)
+ else:
+ self.break_flag = True
+ return self.v_mid
+
+
+def get_pure_pursuit_info(x_current, trajectory_position_data, trajectory_yaw_data, previous_index):
+ """Calculate the target position and yaw angle required for pure pursuit."""
+ search_range = (
+ np.arange(
+ previous_index - trajectory_position_data.shape[0] // 4,
+ previous_index + trajectory_position_data.shape[0] // 4,
+ )
+ % trajectory_position_data.shape[0]
+ )
+ nearest_index = np.argmin(
+ ((trajectory_position_data[search_range] - x_current[:2].reshape(1, 2)) ** 2).sum(axis=1)
+ )
+ return (
+ trajectory_position_data[search_range[nearest_index]],
+ trajectory_yaw_data[search_range[nearest_index]],
+ search_range[nearest_index],
+ )
+
+
+def get_naive_pure_pursuit_info(
+ x_current, trajectory_position_data, trajectory_yaw_data, previous_index
+):
+ """Calculate the target position and yaw angle required for naive pure pursuit."""
+ search_range = (
+ np.arange(
+ previous_index - trajectory_position_data.shape[0] // 4,
+ previous_index + trajectory_position_data.shape[0] // 4,
+ )
+ % trajectory_position_data.shape[0]
+ )
+ nearest_index = np.argmin(
+ ((trajectory_position_data[search_range] - x_current[:2].reshape(1, 2)) ** 2).sum(axis=1)
+ )
+ lookahead_distance = (
+ drive_functions.naive_pure_pursuit_lookahead_coef * x_current[2]
+ + drive_functions.naive_pure_pursuit_lookahead_intercept
+ )
+
+ aug_trajectory_position_data = np.vstack([trajectory_position_data, trajectory_position_data])
+ i = 0
+ while True:
+ tmp_distance = np.sqrt(
+ (
+ (aug_trajectory_position_data[search_range[nearest_index] + i] - x_current[:2]) ** 2
+ ).sum()
+ )
+ if tmp_distance > lookahead_distance:
+ break
+ if (search_range[nearest_index] + i) == (len(aug_trajectory_position_data) - 1):
+ break
+ i += 1
+ return (
+ trajectory_position_data[search_range[nearest_index]],
+ trajectory_yaw_data[search_range[nearest_index]],
+ search_range[nearest_index],
+ aug_trajectory_position_data[search_range[nearest_index] + i],
+ )
+
+
+def restrict_target_vel(delta, v_m=2.0, v_M=15.0, steer_m=0.01, steer_M=0.3):
+ if np.abs(delta) < steer_m:
+ return v_M
+ elif np.abs(delta) < steer_M:
+ return (-(v_M - v_m) * delta + (v_M * steer_M - v_m * steer_m)) / (steer_M - steer_m)
+ else:
+ return v_m
+
+
+def step_response(
+ t: float,
+ start_time: float,
+ interval: float,
+ max_input: float,
+ max_length: float,
+ min_length: float,
+) -> float:
+ """Calculate the value of the step response."""
+ if t < start_time:
+ return 0.0
+
+ step = int((t - start_time) // interval)
+ step_start_time = step * interval + start_time
+
+ np.random.seed(seed=step + RANDOM_SEED_STEP_RESPONSE)
+
+ if max_length > interval:
+ print(f"warning: max_length = {max_length} > interval = {interval}")
+
+ length = np.random.uniform(min_length, min(max_length, interval))
+ input_u = np.random.uniform(-max_input, max_input)
+
+ if (t - step_start_time) >= length:
+ return 0.0
+
+ return input_u
+
+
+class driving_log_updater:
+ """Class for updating logs when driving on the Python simulator."""
+
+ def __init__(self):
+ self.X_history = []
+ self.U_history = []
+ self.control_cmd_time_stamp_list = []
+ self.control_cmd_steer_list = []
+ self.control_cmd_acc_list = []
+ self.kinematic_state_list = []
+ self.acceleration_list = []
+ self.steering_status_list = []
+ self.control_cmd_orig_list = []
+ self.operation_mode_list = []
+
+ def update(self, t_current, x_current, u_current):
+ """Update logs."""
+ self.X_history.append(x_current)
+ self.U_history.append(u_current)
+ self.control_cmd_time_stamp_list.append(t_current)
+ self.control_cmd_steer_list.append(u_current[1])
+ self.control_cmd_acc_list.append(u_current[0])
+ if self.control_cmd_time_stamp_list[-1] - self.control_cmd_time_stamp_list[0] > 3.0:
+ self.control_cmd_time_stamp_list.pop(0)
+ self.control_cmd_steer_list.pop(0)
+ self.control_cmd_acc_list.pop(0)
+ t_sec = int(t_current)
+ t_n_sec = int(1e9 * (t_current - t_sec))
+ kinematic_state = np.zeros(7)
+ acceleration = np.zeros(4)
+ steering_status = np.zeros(3)
+ control_cmd_orig = np.zeros(17)
+ operation_mode = np.zeros(3)
+ kinematic_state[0] = t_sec
+ kinematic_state[1] = t_n_sec
+ kinematic_state[2] = x_current[0]
+ kinematic_state[3] = x_current[1]
+ kinematic_state[4] = np.sin(0.5 * x_current[3])
+ kinematic_state[5] = np.cos(0.5 * x_current[3])
+ kinematic_state[6] = x_current[2]
+ self.kinematic_state_list.append(kinematic_state)
+ acceleration[0] = t_sec
+ acceleration[1] = t_n_sec
+ acceleration[3] = x_current[4]
+ self.acceleration_list.append(acceleration)
+ steering_status[0] = t_sec
+ steering_status[1] = t_n_sec
+ steering_status[2] = x_current[5]
+ self.steering_status_list.append(steering_status)
+ control_cmd_orig[0] = t_sec
+ control_cmd_orig[1] = t_n_sec
+ control_cmd_orig[8] = u_current[1]
+ control_cmd_orig[16] = u_current[0]
+ self.control_cmd_orig_list.append(control_cmd_orig)
+ operation_mode[0] = t_sec
+ operation_mode[1] = t_n_sec
+ operation_mode[2] = 2.0
+ self.operation_mode_list.append(operation_mode)
+
+ def save(self, save_dir):
+ """Save logs in csv format."""
+ kinematic_states = np.zeros((len(self.kinematic_state_list), 48))
+ kinematic_states[:, [0, 1, 4, 5, 9, 10, 47]] = np.array(self.kinematic_state_list)
+ np.savetxt(save_dir + "/kinematic_state.csv", kinematic_states, delimiter=",")
+ np.savetxt(save_dir + "/acceleration.csv", np.array(self.acceleration_list), delimiter=",")
+ np.savetxt(
+ save_dir + "/steering_status.csv",
+ np.array(self.steering_status_list),
+ delimiter=",",
+ )
+ np.savetxt(
+ save_dir + "/control_cmd_orig.csv",
+ np.array(self.control_cmd_orig_list),
+ delimiter=",",
+ )
+ np.savetxt(
+ save_dir + "/system_operation_mode_state.csv",
+ np.array(self.operation_mode_list),
+ delimiter=",",
+ )
+ with open(save_dir + "/system_operation_mode_state.csv", "w") as f:
+ writer = csv.writer(f)
+ for i in range(len(self.operation_mode_list)):
+ operation_mode_plus_true = self.operation_mode_list[i].tolist()
+ operation_mode_plus_true.append("True")
+ writer.writerow(operation_mode_plus_true)
diff --git a/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/density_estimation.ipynb b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/density_estimation.ipynb
new file mode 100644
index 0000000000000..34e5fa94349f2
--- /dev/null
+++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/density_estimation.ipynb
@@ -0,0 +1,635 @@
+{
+ "cells": [
+ {
+ "cell_type": "code",
+ "execution_count": 1,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "# cSpell:ignore codemirror ipython nbconvert pygments nbformat kernelspec #"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "from density_estimation import load_kinematic_states, KinematicStates\n",
+ "from assets import ControlType, ChangeParam\n",
+ "import json\n",
+ "\n",
+ "def load_kinematic_states_and_result(\n",
+ " load_dir: str, mem_diff=False, max_lateral_deviation: float = 5.0\n",
+ ") -> tuple[KinematicStates, float, float]:\n",
+ " \"\"\"Return the KinematicStates object and the lateral error of the simulation result.\n",
+ "\n",
+ " This read the given directory (the directory where the kinematic states are stored)\n",
+ " To read the lateral deviation, however, it goes to read the appropriate directory next to the given directory.\n",
+ "\n",
+ " Returns:\n",
+ " * KinematicStates: KinematicStates object\n",
+ " * float: the lateral error of the simulation result. this is not for trained data\n",
+ " * float: the lateral error of the simulation result. this is for trained data\n",
+ " \"\"\"\n",
+ " kinematic_states = load_kinematic_states(\n",
+ " control_type=None,\n",
+ " change_param=None,\n",
+ " index=None,\n",
+ " load_dir=load_dir,\n",
+ " )\n",
+ "\n",
+ " load_dir_for_result = load_dir.replace(ControlType.pp_eight.value, \"pp_aided\")\n",
+ " if mem_diff:\n",
+ " load_dir_for_result += \"_mem_diff\"\n",
+ " load_dir_for_trained_result = load_dir_for_result.replace(\"_aided_sim_\", \"_aided_sim_trained_\")\n",
+ "\n",
+ " with open(f\"{load_dir_for_result}/auto_test_performance_result.json\", \"r\") as f:\n",
+ " data = json.load(f)\n",
+ "\n",
+ " with open(f\"{load_dir_for_trained_result}/auto_test_performance_result.json\", \"r\") as f:\n",
+ " data_trained = json.load(f)\n",
+ "\n",
+ " total_abs_max_lateral_deviation = min(data.get(\"total_abs_max_lateral_deviation\"), max_lateral_deviation)\n",
+ " trained_total_abs_max_lateral_deviation = min(data_trained.get(\"total_abs_max_lateral_deviation\"), max_lateral_deviation)\n",
+ " return kinematic_states, total_abs_max_lateral_deviation, trained_total_abs_max_lateral_deviation\n",
+ "\n",
+ "\n",
+ "load_kinematic_states_and_result(\n",
+ " \"test_run_sim_20240607_163543/test_pure_pursuit_figure_eight_sim_steer_scaling_0th\"\n",
+ ")"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "import numpy as np\n",
+ "from typing import Callable\n",
+ "from density_estimation import Shape, calc_minimum_density_point, ScottCoef\n",
+ "from typing import Literal\n",
+ "\n",
+ "\n",
+ "def calc_scalar_indexes(\n",
+ " kinematic_states: KinematicStates,\n",
+ " fst: Literal[\"speed\", \"acc\", \"steer\"],\n",
+ " snd: Literal[\"speed\", \"acc\", \"steer\"],\n",
+ " fst_range: list[float],\n",
+ " snd_range: list[float],\n",
+ " bandwidths: list[float] | list[ScottCoef],\n",
+ " shape: Callable[..., bool] = Shape.trivial,\n",
+ ") -> np.ndarray:\n",
+ " \"\"\"Return an ndarray of scalar indexes calculated by the kernel density estimation.\n",
+ "\n",
+ " This takes as a parameter the method (the minimum value in a particular region) used to compute the scalar index of kernel density.\n",
+ " \"\"\"\n",
+ " array = np.array([])\n",
+ "\n",
+ " for bandwidth in bandwidths:\n",
+ " min_point, min_val = calc_minimum_density_point(\n",
+ " kinematic_states=kinematic_states,\n",
+ " fst=fst,\n",
+ " snd=snd,\n",
+ " fst_range=fst_range,\n",
+ " snd_range=snd_range,\n",
+ " shape=shape,\n",
+ " bandwidth=bandwidth,\n",
+ " )\n",
+ " array = np.append(array, min_val)\n",
+ "\n",
+ " return array\n",
+ "\n",
+ "\n",
+ "kinematic_states_steer_scaling_0th = load_kinematic_states(\n",
+ " ControlType.pp_eight, ChangeParam.steer_scaling, 0\n",
+ ")\n",
+ "\n",
+ "calc_scalar_indexes(\n",
+ " kinematic_states=kinematic_states_steer_scaling_0th,\n",
+ " fst=\"speed\",\n",
+ " snd=\"acc\",\n",
+ " fst_range=[0.0, 12.0],\n",
+ " snd_range=[-1.5, 1.5],\n",
+ " bandwidths= [ScottCoef(i) for i in np.arange(0.5, 5.5, 0.5)],\n",
+ " shape=lambda x, y: Shape.ellipse(\n",
+ " Shape.ELLIPSE_CENTER, Shape.ELLIPSE_WIDTH, Shape.ELLIPSE_HEIGHT, x, y\n",
+ " ),\n",
+ ")"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "def correlation_between_scalar_indexes_and_lateral_errors(\n",
+ " load_dir_list: list[str],\n",
+ " fst: Literal[\"speed\", \"acc\", \"steer\"],\n",
+ " snd: Literal[\"speed\", \"acc\", \"steer\"],\n",
+ " fst_range: list[float],\n",
+ " snd_range: list[float],\n",
+ " bandwidths: list[float] | list[ScottCoef],\n",
+ " shape: Callable[..., bool] = Shape.trivial,\n",
+ ") -> np.ndarray:\n",
+ " accumulation = np.array([])\n",
+ " for load_dir in load_dir_list:\n",
+ " # The one that was additionally learned is named \"trained\"\n",
+ " kinematic_states, result, trained_result = load_kinematic_states_and_result(load_dir)\n",
+ " scalar_indexes = calc_scalar_indexes(\n",
+ " kinematic_states=kinematic_states,\n",
+ " fst=fst,\n",
+ " snd=snd,\n",
+ " fst_range=fst_range,\n",
+ " snd_range=snd_range,\n",
+ " bandwidths=bandwidths,\n",
+ " shape=shape,\n",
+ " )\n",
+ "\n",
+ " # cSpell:ignore corcoef #\n",
+ " # Add lateral errors to the end of the vector of scalar indexes for calculating the correlation coefficient with numpy.corcoef\n",
+ " scalar_indexes = np.append(arr=scalar_indexes, values=[result, trained_result])\n",
+ "\n",
+ " if accumulation.size == 0:\n",
+ " accumulation = accumulation.reshape(scalar_indexes.shape[0], -1)\n",
+ " accumulation = np.hstack((accumulation, scalar_indexes.reshape(-1,1)))\n",
+ "\n",
+ " return np.corrcoef(accumulation)[-1,:-2]"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "import matplotlib.pyplot as plt\n",
+ "\n",
+ "def list_of_kde_and_error(\n",
+ " load_dir_list: list[str],\n",
+ " fst: Literal[\"speed\", \"acc\", \"steer\"],\n",
+ " snd: Literal[\"speed\", \"acc\", \"steer\"],\n",
+ " fst_range: list[float],\n",
+ " snd_range: list[float],\n",
+ " bandwidth: float | ScottCoef,\n",
+ " shape: Callable[..., bool] = Shape.trivial,\n",
+ ") -> np.ndarray:\n",
+ " accumulation = np.array([])\n",
+ " for load_dir in load_dir_list:\n",
+ " # The one that was additionally learned is named \"trained\"\n",
+ " kinematic_states, result, trained_result = load_kinematic_states_and_result(\n",
+ " load_dir\n",
+ " )\n",
+ " min_point, min_val = calc_minimum_density_point(\n",
+ " kinematic_states=kinematic_states,\n",
+ " fst=fst,\n",
+ " snd=snd,\n",
+ " fst_range=fst_range,\n",
+ " snd_range=snd_range,\n",
+ " shape=shape,\n",
+ " bandwidth=bandwidth,\n",
+ " )\n",
+ " accumulation = np.append(accumulation, [trained_result, min_val])\n",
+ " return accumulation.reshape(-1, 2)\n",
+ "\n",
+ "def plot_scatter_acc(bandwidth : float | ScottCoef) -> None:\n",
+ " data = list_of_kde_and_error (\n",
+ " load_dir_list=[\n",
+ " f\"test_param_search_{i}_test_vehicle/test_pure_pursuit_figure_eight_sim_test_vehicle_3th\"\n",
+ " for i in range(5, 35)\n",
+ " ],\n",
+ " fst=\"speed\",\n",
+ " snd=\"acc\",\n",
+ " fst_range=[0.0, 12.0],\n",
+ " snd_range=[-1.5, 1.5],\n",
+ " bandwidth=bandwidth,\n",
+ " shape=lambda x, y: Shape.ellipse(\n",
+ " Shape.ELLIPSE_CENTER, Shape.ELLIPSE_WIDTH, Shape.ELLIPSE_HEIGHT, x, y\n",
+ " ),\n",
+ " )\n",
+ " plt.scatter(data[:, 0], data[:, 1])\n",
+ "\n",
+ " plt.xlabel('Trained lateral error')\n",
+ " plt.ylabel('minimum value of KDE')\n",
+ "\n",
+ " plt.title('Kernel density and lateral error')\n",
+ "\n",
+ " plt.show()\n",
+ "\n",
+ "plot_scatter_acc(0.2)"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "def plot_scatter_steer(bandwidth : float | ScottCoef) -> None:\n",
+ " coef = 4.76/2.79\n",
+ " POINTS = np.array(\n",
+ " [\n",
+ " [1.0+0.3*5, coef * (0.15)],\n",
+ " [4.0, coef * (0.15)],\n",
+ " [11.0, 0.0],\n",
+ " [1.0+0.3*5, coef * -(0.15)],\n",
+ " [4, coef * -(0.15)],\n",
+ " ]\n",
+ " )\n",
+ " data = list_of_kde_and_error (\n",
+ " load_dir_list=[\n",
+ " f\"test_param_search_{i}_test_vehicle/test_pure_pursuit_figure_eight_sim_test_vehicle_2th\"\n",
+ " for i in range(5, 35)\n",
+ " ],\n",
+ " fst=\"speed\",\n",
+ " snd=\"steer\",\n",
+ " fst_range=[0.0, 12.0],\n",
+ " snd_range=[-0.3, 0.3],\n",
+ " bandwidth=bandwidth,\n",
+ " shape=lambda x, y: Shape.convex_hull(POINTS, x, y),\n",
+ " )\n",
+ " plt.scatter(data[:, 0], data[:, 1])\n",
+ "\n",
+ " plt.xlabel('Trained lateral error')\n",
+ " plt.ylabel('minimum value of KDE')\n",
+ "\n",
+ " plt.title('Kernel density and lateral error')\n",
+ "\n",
+ " plt.show()\n",
+ "\n",
+ "plot_scatter_steer(0.3)"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "correlation_between_scalar_indexes_and_lateral_errors(\n",
+ " load_dir_list=[\n",
+ " f\"test_param_search_{i}_test_vehicle/test_pure_pursuit_figure_eight_sim_test_vehicle_3th\" for i in range(5, 35)\n",
+ " ],\n",
+ " fst=\"speed\",\n",
+ " snd=\"acc\",\n",
+ " fst_range=[0.0, 12.0],\n",
+ " snd_range=[-1.5, 1.5],\n",
+ " bandwidths=np.arange(0.5, 5.5, 0.5),\n",
+ " shape=lambda x, y: Shape.ellipse(\n",
+ " Shape.ELLIPSE_CENTER, Shape.ELLIPSE_WIDTH, Shape.ELLIPSE_HEIGHT, x, y\n",
+ " ),\n",
+ ")"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "print(correlation_between_scalar_indexes_and_lateral_errors(\n",
+ " load_dir_list=[\n",
+ " f\"test_param_search_{i}_test_vehicle/test_pure_pursuit_figure_eight_sim_test_vehicle_3th\" for i in range(5, 35)\n",
+ " ],\n",
+ " fst=\"speed\",\n",
+ " snd=\"acc\",\n",
+ " fst_range=[0.0, 12.0],\n",
+ " snd_range=[-1.5, 1.5],\n",
+ " bandwidths=np.arange(0.1, 1.1, 0.1),\n",
+ " shape=lambda x, y: Shape.ellipse(\n",
+ " Shape.ELLIPSE_CENTER, Shape.ELLIPSE_WIDTH, Shape.ELLIPSE_HEIGHT, x, y\n",
+ " ),\n",
+ "))"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "print(correlation_between_scalar_indexes_and_lateral_errors(\n",
+ " load_dir_list=[\n",
+ " f\"test_param_search_{i}_test_vehicle/test_pure_pursuit_figure_eight_sim_test_vehicle_2th\" for i in range(5, 35)\n",
+ " ],\n",
+ " fst=\"speed\",\n",
+ " snd=\"acc\",\n",
+ " fst_range=[0.0, 12.0],\n",
+ " snd_range=[-1.5, 1.5],\n",
+ " bandwidths=np.arange(0.1, 1.1, 0.1),\n",
+ " shape=lambda x, y: Shape.ellipse(\n",
+ " Shape.ELLIPSE_CENTER, Shape.ELLIPSE_WIDTH, Shape.ELLIPSE_HEIGHT, x, y\n",
+ " ),\n",
+ "))"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "print(correlation_between_scalar_indexes_and_lateral_errors(\n",
+ " load_dir_list=[\n",
+ " f\"test_param_search_{i}_test_vehicle/test_pure_pursuit_figure_eight_sim_test_vehicle_3th\" for i in range(5, 35)\n",
+ " ],\n",
+ " fst=\"speed\",\n",
+ " snd=\"acc\",\n",
+ " fst_range=[0.0, 12.0],\n",
+ " snd_range=[-1.5, 1.5],\n",
+ " bandwidths=[ScottCoef(i) for i in np.arange(0.5, 5.5, 0.5)],\n",
+ " shape=lambda x, y: Shape.ellipse(\n",
+ " Shape.ELLIPSE_CENTER, Shape.ELLIPSE_WIDTH, Shape.ELLIPSE_HEIGHT, x, y\n",
+ " ),\n",
+ "))"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "np.arange(1.5, 1.8, 0.1)"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "coef = 4.76/2.79 #get_estimated_wheel_base_coef(\"test_param_search_0_test_vehicle/test_pure_pursuit_figure_eight_sim_test_vehicle_2th\")\n",
+ "for i in range(1):\n",
+ " POINTS = np.array(\n",
+ " [\n",
+ " [1.0+0.3*5, coef * (0.15-0.01*i)],\n",
+ " [4.0, coef * (0.15-0.01*i)],\n",
+ " [11.0, 0.0],\n",
+ " [1.0+0.3*5, coef * -(0.15-0.01*i)],\n",
+ " [4, coef * -(0.15-0.01*i)],\n",
+ " ]\n",
+ " )\n",
+ "\n",
+ " corrcoef_steer = correlation_between_scalar_indexes_and_lateral_errors(\n",
+ " load_dir_list=[\n",
+ " f\"test_param_search_{i}_test_vehicle/test_pure_pursuit_figure_eight_sim_test_vehicle_3th\" for i in range(5, 35)\n",
+ " ],\n",
+ " fst=\"speed\",\n",
+ " snd=\"steer\",\n",
+ " fst_range=[0.0, 12.0],\n",
+ " snd_range=[-0.3, 0.3],\n",
+ " bandwidths=np.arange(0.1, 1.1, 0.1),\n",
+ " shape=lambda x, y: Shape.convex_hull(POINTS, x, y),\n",
+ " )\n",
+ " print(i,corrcoef_steer)"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "coef = 4.76/2.79 #get_estimated_wheel_base_coef(\"test_param_search_0_test_vehicle/test_pure_pursuit_figure_eight_sim_test_vehicle_2th\")\n",
+ "for i in range(1):\n",
+ " POINTS = np.array(\n",
+ " [\n",
+ " [1.0+0.3*5, coef * (0.15-0.01*i)],\n",
+ " [4.0, coef * (0.15-0.01*i)],\n",
+ " [11.0, 0.0],\n",
+ " [1.0+0.3*5, coef * -(0.15-0.01*i)],\n",
+ " [4, coef * -(0.15-0.01*i)],\n",
+ " ]\n",
+ " )\n",
+ "\n",
+ " corrcoef_steer = correlation_between_scalar_indexes_and_lateral_errors(\n",
+ " load_dir_list=[\n",
+ " f\"test_param_search_{i}_test_vehicle/test_pure_pursuit_figure_eight_sim_test_vehicle_2th\" for i in range(5, 35)\n",
+ " ],\n",
+ " fst=\"speed\",\n",
+ " snd=\"steer\",\n",
+ " fst_range=[0.0, 12.0],\n",
+ " snd_range=[-0.3, 0.3],\n",
+ " bandwidths=np.arange(0.1, 1.1, 0.1),\n",
+ " shape=lambda x, y: Shape.convex_hull(POINTS, x, y),\n",
+ " )\n",
+ " print(i,corrcoef_steer)"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": 7,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "import matplotlib.pyplot as plt"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "from density_estimation import visualize_speed_acc, visualize_speed_steer, ScottCoef\n",
+ "\n",
+ "# cSpell:ignore nrows ncols #\n",
+ "fig, axes = plt.subplots(nrows=1, ncols=2, figsize=(18, 12), tight_layout=True)\n",
+ "\n",
+ "ax1 : plt.Axes = axes[0]\n",
+ "ax2 : plt.Axes = axes[1]\n",
+ "\n",
+ "fig, ax1 = visualize_speed_acc(fig=fig, ax=ax1, kinematic_states=kinematic_states_steer_scaling_0th, bandwidth=ScottCoef(0.6))\n",
+ "ax1.plot()\n",
+ "\n",
+ "fig, ax2 = visualize_speed_steer(fig=fig, ax=ax2, kinematic_states=kinematic_states_steer_scaling_0th, bandwidth=ScottCoef(1.3))\n",
+ "ax2.plot()\n",
+ "\n",
+ "fig.show()"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "from density_estimation import KinematicStates\n",
+ "\n",
+ "kinematic_states_steer_scaling_0th.plot(\"speed\", \"acc\", \"steer\")"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "from density_estimation import Shape\n",
+ "\n",
+ "fig = kinematic_states_steer_scaling_0th.plot(\"speed\", \"acc\")\n",
+ "print(type(fig))\n",
+ "\n",
+ "ellipse_center = [6, 0]\n",
+ "ellipse_width = 6\n",
+ "ellipse_height = 2\n",
+ "\n",
+ "# cSpell:ignore yref showarrow arrowcolor #\n",
+ "fig.add_shape(\n",
+ " type=\"circle\",\n",
+ " xref=\"x\", yref=\"y\",\n",
+ " x0=ellipse_center[0] - ellipse_width / 2,\n",
+ " y0=ellipse_center[1] - ellipse_height / 2,\n",
+ " x1=ellipse_center[0] + ellipse_width / 2,\n",
+ " y1=ellipse_center[1] + ellipse_height / 2,\n",
+ " line=dict(color=\"RoyalBlue\"),\n",
+ " fillcolor=\"LightSkyBlue\",\n",
+ " opacity=0.5,\n",
+ " layer=\"below\"\n",
+ ")\n",
+ "\n",
+ "min_element, min_val = calc_minimum_density_point(\n",
+ " kinematic_states=kinematic_states_steer_scaling_0th,\n",
+ " bandwidth=0.50,\n",
+ " fst=\"speed\",\n",
+ " snd=\"acc\",\n",
+ " fst_range=[4, 8],\n",
+ " snd_range=[-1.0, 1.0],\n",
+ " shape=lambda x, y : Shape.ellipse(ellipse_center, ellipse_width, ellipse_height, x, y),\n",
+ ")\n",
+ "\n",
+ "fig.add_annotation(\n",
+ " x=min_element[0],\n",
+ " y=min_element[1],\n",
+ " text=f\"min val: {round(min_val, 4)} at {min_element}\",\n",
+ " showarrow=True,\n",
+ " arrowhead=1,\n",
+ " font=dict(\n",
+ " family=\"Arial, sans-serif\",\n",
+ " size=12,\n",
+ " color=\"blue\"\n",
+ " ),\n",
+ " align=\"center\",\n",
+ " arrowcolor=\"blue\",\n",
+ ")\n",
+ "\n",
+ "fig.show()"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "kinematic_states_steer_scaling_0th.plot(\"acc\", \"steer\")"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "from density_estimation import plot_kernel_density\n",
+ "\n",
+ "kinematic_states_steer_scaling_0th.plot(\"speed\", \"acc\").show()\n",
+ "plot_kernel_density(kinematic_states_steer_scaling_0th, \"speed\", \"acc\", [2, 10], [-2, 2])"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "calc_minimum_density_point(\n",
+ " kinematic_states=kinematic_states_steer_scaling_0th,\n",
+ " bandwidth=0.01,\n",
+ " fst=\"speed\",\n",
+ " snd=\"steer\",\n",
+ " fst_range=[2, 10],\n",
+ " snd_range=[-1.0, 1.0],\n",
+ " shape=lambda x, y : Shape.triangle([4, -0.2], [4, 0.2], [10, 0], x, y),\n",
+ ")"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "kinematic_states_steer_scaling_0th.plot(\"speed\", \"steer\").show()\n",
+ "plot_kernel_density(\n",
+ " kinematic_states=kinematic_states_steer_scaling_0th,\n",
+ " bandwidth=0.15,\n",
+ " fst=\"speed\",\n",
+ " snd=\"steer\",\n",
+ " fst_range=[2, 10],\n",
+ " snd_range=[-0.4, 0.2],\n",
+ ")"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": 15,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "kinematic_states_vehicle = load_kinematic_states(\n",
+ " ControlType.pp_eight, ChangeParam.vehicle_type, 0\n",
+ ")"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "kinematic_states_vehicle.plot(\"speed\", \"acc\", \"steer\").show()"
+ ]
+ },
+ {
+ "cell_type": "code",
+ "execution_count": null,
+ "metadata": {},
+ "outputs": [],
+ "source": [
+ "kinematic_states_vehicle.plot(\"speed\", \"steer\").show()\n",
+ "plot_kernel_density(\n",
+ " kinematic_states=kinematic_states_vehicle,\n",
+ " fst=\"speed\",\n",
+ " snd=\"steer\",\n",
+ " fst_range=[2, 10],\n",
+ " snd_range=[-0.3, 0.4],\n",
+ ")"
+ ]
+ }
+ ],
+ "metadata": {
+ "kernelspec": {
+ "display_name": "Python 3",
+ "language": "python",
+ "name": "python3"
+ },
+ "language_info": {
+ "codemirror_mode": {
+ "name": "ipython",
+ "version": 3
+ },
+ "file_extension": ".py",
+ "mimetype": "text/x-python",
+ "name": "python",
+ "nbconvert_exporter": "python",
+ "pygments_lexer": "ipython3",
+ "version": "3.10.12"
+ }
+ },
+ "nbformat": 4,
+ "nbformat_minor": 2
+}
diff --git a/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/density_estimation.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/density_estimation.py
new file mode 100644
index 0000000000000..0e5f58f352f5d
--- /dev/null
+++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/density_estimation.py
@@ -0,0 +1,468 @@
+# Copyright 2024 Proxima Technology Inc, TIER IV
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from typing import Callable
+from typing import Literal
+
+from assets import ChangeParam # type: ignore
+from assets import ControlType
+from assets import test_dir_name
+from matplotlib.patches import Ellipse # type: ignore
+import matplotlib.pyplot as plt # type: ignore
+import numpy as np
+import plotly.express as px # type: ignore
+import plotly.graph_objs as go # type: ignore
+from scipy.spatial import ConvexHull # type: ignore
+from scipy.spatial import Delaunay
+from sklearn.neighbors import KernelDensity # type: ignore
+from sklearn.preprocessing import StandardScaler # type: ignore
+
+
+class KinematicStates:
+ """The class for kinematic states, which includes speed, acceleration, and steering angle."""
+
+ def __init__(self, speed: np.ndarray, acc: np.ndarray, steer: np.ndarray):
+ self.speed = speed
+ self.acc = acc
+ self.steer = steer
+
+ def to_ndarray(self, field_list: list[str] = ["speed", "acc", "steer"]) -> np.ndarray:
+ return np.stack(
+ arrays=[getattr(self, field) for field in field_list],
+ axis=1,
+ )
+
+ def plot(
+ self,
+ x_axis: Literal["speed", "acc", "steer"],
+ y_axis: Literal["speed", "acc", "steer"],
+ z_axis: Literal["speed", "acc", "steer"] | None = None,
+ ) -> go.Figure:
+ """Plot interactive scatter image of kinematic states in 2D or 3D."""
+ if z_axis is None:
+ fig = px.scatter(x=getattr(self, x_axis), y=getattr(self, y_axis))
+ fig.update_layout(xaxis_title=x_axis, yaxis_title=y_axis)
+ else:
+ fig = px.scatter_3d(
+ x=getattr(self, x_axis),
+ y=getattr(self, y_axis),
+ z=getattr(self, z_axis),
+ )
+ fig.update_layout(
+ scene={"xaxis_title": x_axis, "yaxis_title": y_axis, "zaxis_title": z_axis}
+ )
+ fig.update_traces(marker={"size": 1})
+ return fig
+
+
+def load_kinematic_states(
+ control_type: ControlType | None = None,
+ change_param: ChangeParam | None = None,
+ index: int | None = None,
+ load_dir: str | None = None,
+ verbose: bool = False,
+) -> KinematicStates:
+ """Load kinematic states from the specified directory."""
+ if load_dir is None:
+ load_dir = test_dir_name(
+ control_type=control_type,
+ change_param=change_param,
+ index=index,
+ )
+ if verbose:
+ print(f"loaded directory: {load_dir}")
+
+ # cSpell:ignore usecols
+ observed_speed_array = np.loadtxt(
+ fname=load_dir + "/kinematic_state.csv", delimiter=",", usecols=47
+ )
+ observed_acc_array = np.loadtxt(
+ fname=load_dir + "/acceleration.csv",
+ delimiter=",",
+ usecols=3,
+ )
+ observed_steer_array = np.loadtxt(
+ fname=load_dir + "/steering_status.csv",
+ delimiter=",",
+ usecols=2,
+ )
+ kinematic_states = KinematicStates(
+ speed=observed_speed_array, acc=observed_acc_array, steer=observed_steer_array
+ )
+ return kinematic_states
+
+
+class Shape:
+ """The class for shape functions, just a namespace."""
+
+ # config for ellipse
+ ELLIPSE_CENTER = [6.0, 0.0]
+
+ ELLIPSE_WIDTH = 10.0
+
+ ELLIPSE_HEIGHT = 2.0
+
+ @staticmethod
+ def trivial(_x: float, _y: float) -> bool:
+ """Trivial shape function."""
+ return True
+
+ @staticmethod
+ def ellipse(center: list[float], width: float, hight: float, x: float, y: float) -> bool:
+ """Represent the interior of an ellipse.
+
+ Args:
+ center: the coordinates of the center of the ellipse
+ width: the length of the ellipse along the x-axis
+ height: the length of the ellipse along the y-axis
+ """
+ return (x - center[0]) ** 2 / (width / 2) ** 2 + (y - center[1]) ** 2 / (
+ hight / 2
+ ) ** 2 <= 1
+
+ @staticmethod
+ def triangle(A: list[float], B: list[float], C: list[float], x: float, y: float) -> bool:
+ """Represent the interior of a triangle.
+
+ Determine if the given point is inside the triangle formed by connecting the given three points.
+ """
+
+ def sign(p, q, r):
+ """Check relation of `p`, `q`, `r`."""
+ return (p[0] - r[0]) * (q[1] - r[1]) - (q[0] - r[0]) * (p[1] - r[1])
+
+ P = [x, y]
+ b1 = sign(P, A, B) < 0
+ b2 = sign(P, B, C) < 0
+ b3 = sign(P, C, A) < 0
+
+ # P is inside triangle ABC if all sides are on the same side
+ return b1 == b2 == b3
+
+ @staticmethod
+ def convex_hull(points: np.ndarray, x: float, y: float) -> bool:
+ """Determine if a point is within the convex hull.
+
+ Parameters:
+ points (ndarray): An array of points forming the convex hull (Nx2).
+ x (float): The x-coordinate of the point to be checked.
+ y (float): The y-coordinate of the point to be checked.
+
+ Returns:
+ bool: True if the point is within the convex hull, False otherwise.
+ """
+ hull = ConvexHull(points)
+ delaunay = Delaunay(points[hull.vertices])
+ return delaunay.find_simplex([x, y]) >= 0
+
+
+def get_search_points(
+ n_points: int,
+ x_range: list[float],
+ y_range: list[float],
+ z_range: list[float] = [],
+):
+ """Help for density estimation."""
+ x = np.linspace(x_range[0], x_range[1], num=n_points)
+ y = np.linspace(y_range[0], y_range[1], num=n_points)
+ if z_range != []:
+ z = np.linspace(z_range[0], z_range[1], num=n_points)
+ return np.array(np.meshgrid(x, y, z)).T.reshape(-1, 3)
+ return np.array(np.meshgrid(x, y)).T.reshape(-1, 2)
+
+
+class ScottCoef:
+ """Multiple coefficient for bandwidth of Scott's algorithm."""
+
+ def __init__(self, val: float):
+ self.val = val
+
+
+# cSpell:ignore silverman
+def kde_score_func(
+ kinematic_states: KinematicStates,
+ fst: Literal["speed", "acc", "steer"],
+ snd: Literal["speed", "acc", "steer"],
+ bandwidth: float | Literal["scott", "silverman"] | ScottCoef = "scott",
+ verbose: bool = False,
+) -> Callable[..., float]:
+ """Return the function which is a variation of `KernelDensity.score_samples` including step of standardization."""
+ # standardize the kinematic states
+ scaler = StandardScaler()
+ scaler.fit(kinematic_states.to_ndarray([fst, snd]))
+ standardized_2d_kinematic_states = scaler.transform(kinematic_states.to_ndarray([fst, snd]))
+
+ if not isinstance(bandwidth, ScottCoef):
+ # perform density estimation with standardization
+ if verbose:
+ print(f"info: bandwidth is set to {bandwidth}")
+ standardized_kde = KernelDensity(kernel="gaussian", bandwidth=bandwidth).fit(
+ standardized_2d_kinematic_states
+ )
+ else:
+ scott_kde = KernelDensity(kernel="gaussian", bandwidth="scott").fit(
+ standardized_2d_kinematic_states
+ )
+ scott_bandwidth = scott_kde.bandwidth_
+ if verbose:
+ print(f"info: bandwidth is set to {scott_bandwidth * bandwidth.val}")
+ standardized_kde = KernelDensity(
+ kernel="gaussian", bandwidth=scott_bandwidth * bandwidth.val
+ ).fit(standardized_2d_kinematic_states)
+
+ return lambda arr: standardized_kde.score_samples(scaler.transform(arr))
+
+
+def plot_kernel_density(
+ kinematic_states: KinematicStates,
+ fst: Literal["speed", "acc", "steer"],
+ snd: Literal["speed", "acc", "steer"],
+ fst_range: list[float],
+ snd_range: list[float],
+ point_number: int = 30,
+ bandwidth: float | Literal["scott", "silverman"] | ScottCoef = "scott",
+) -> None:
+ kde_score = kde_score_func(
+ kinematic_states=kinematic_states,
+ fst=fst,
+ snd=snd,
+ bandwidth=bandwidth,
+ )
+ grids = get_search_points(point_number, fst_range, snd_range)
+ kernel_densities = kde_score(grids)
+ Z = np.exp(kernel_densities).reshape(point_number, point_number).T
+
+ x_range = np.linspace(fst_range[0], fst_range[1], point_number)
+ y_range = np.linspace(snd_range[0], snd_range[1], point_number)
+ fig = go.Figure(data=[go.Surface(z=Z, x=x_range, y=y_range)])
+
+ # cSpell:ignore zaxis
+ fig.update_layout(
+ title="3D Surface Plot of Kernel Density",
+ scene={
+ "xaxis": {"title": fst},
+ "yaxis": {"title": snd},
+ "zaxis": {"title": "kernel density"},
+ },
+ )
+ fig.show()
+
+
+def calc_minimum_density_point(
+ kinematic_states: KinematicStates,
+ fst: Literal["speed", "acc", "steer"],
+ snd: Literal["speed", "acc", "steer"],
+ fst_range: list[float],
+ snd_range: list[float],
+ shape: Callable[..., bool] = Shape.trivial,
+ point_number: int = 30,
+ bandwidth: float | Literal["scott", "silverman"] | ScottCoef = "scott",
+) -> tuple[np.ndarray, float]:
+ kde_score = kde_score_func(
+ kinematic_states=kinematic_states,
+ fst=fst,
+ snd=snd,
+ bandwidth=bandwidth,
+ )
+ grids = get_search_points(point_number, fst_range, snd_range)
+ filtered_grids = np.array([point for point in grids if shape(point[0], point[1])])
+ kernel_densities = kde_score(filtered_grids)
+
+ min_value: float = np.exp(kernel_densities).min()
+ min_index = kernel_densities.argmin() # type: ignore
+ min_element = filtered_grids[min_index]
+ return min_element.reshape(1, -1)[0], min_value
+
+
+def kde_data(
+ kinematic_states: KinematicStates,
+ fst: Literal["speed", "acc", "steer"],
+ snd: Literal["speed", "acc", "steer"],
+ fst_range: list[float],
+ snd_range: list[float],
+ n_points: int = 100,
+ bandwidth: float | Literal["scott", "silverman"] | ScottCoef = "scott",
+) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
+ kde_score = kde_score_func(
+ kinematic_states=kinematic_states,
+ fst=fst,
+ snd=snd,
+ bandwidth=bandwidth,
+ )
+
+ x = np.linspace(fst_range[0], fst_range[1], n_points)
+ y = np.linspace(snd_range[0], snd_range[1], n_points)
+ X, Y = np.meshgrid(x, y)
+
+ grids = np.column_stack((X.flatten(), Y.flatten())).reshape((n_points**2, 2))
+ Z = np.exp(kde_score(grids)).reshape(n_points, n_points)
+ return X, Y, Z
+
+
+def visualize_speed_acc(
+ fig: plt.Figure,
+ ax: plt.Axes,
+ kinematic_states: KinematicStates,
+ bandwidth: float | Literal["scott", "silverman"] | ScottCoef = "scott",
+) -> tuple[plt.Figure, plt.Axes]:
+ """Visualizes the speed and acceleration of kinematic states using kernel density estimation.
+
+ This function plots a heatmap of the kernel density estimation of speed and acceleration
+ values from the given kinematic states. It also overlays an ellipse and marks the minimum
+ density point within the elliptical region.
+
+ Parameters:
+ fig (plt.Figure): The figure object to be used for plotting.
+ ax (plt.Axes): The axes object to be used for plotting.
+ kinematic_states (KinematicStates): The kinematic states containing speed and acceleration data.
+
+ Returns:
+ tuple[plt.Figure, plt.Axes]: The modified figure and axes objects.
+ """
+ # config for kernel density estimation
+ fst: Literal["speed", "acc", "steer"] = "speed"
+ snd: Literal["speed", "acc", "steer"] = "acc"
+ fst_range = [0.0, 12.0]
+ snd_range = [-1.5, 1.5]
+
+ ellipse: Ellipse = Ellipse(
+ xy=Shape.ELLIPSE_CENTER,
+ width=Shape.ELLIPSE_WIDTH,
+ height=Shape.ELLIPSE_HEIGHT,
+ angle=0,
+ edgecolor="orange",
+ fc="None",
+ lw=2,
+ )
+
+ # kernel density estimation
+ X, Y, Z = kde_data(
+ kinematic_states=kinematic_states,
+ fst=fst,
+ snd=snd,
+ fst_range=fst_range,
+ snd_range=snd_range,
+ n_points=100,
+ bandwidth=bandwidth,
+ )
+
+ # plot heatmap
+ # cSpell:ignore pcolormesh viridis
+ mesh_acc = ax.pcolormesh(X, Y, Z, cmap="viridis", shading="auto")
+ fig.colorbar(mesh_acc, ax=ax)
+
+ # plot ellipse
+ ax.add_patch(ellipse)
+
+ # Find the minimum value of kernel density in the elliptic region
+ min_element, min_val = calc_minimum_density_point(
+ kinematic_states=kinematic_states,
+ fst=fst,
+ snd=snd,
+ fst_range=fst_range,
+ snd_range=snd_range,
+ shape=lambda x, y: Shape.ellipse(
+ Shape.ELLIPSE_CENTER, Shape.ELLIPSE_WIDTH, Shape.ELLIPSE_HEIGHT, x, y
+ ),
+ bandwidth=bandwidth,
+ )
+ rounded_min_val = round(min_val, 4)
+ rounded_min_element = [round(min_element[0], 4), round(min_element[1], 4)]
+
+ # plot the minimum point as a white point
+ ax.plot(min_element[0], min_element[1], marker="o", color="white", markersize=2)
+
+ ax.set_xlabel(fst, fontsize=12)
+ ax.set_ylabel(snd, fontsize=12)
+ ax.set_title(f"kernel density: min_density={rounded_min_val} at {rounded_min_element}")
+
+ return fig, ax
+
+
+def visualize_speed_steer(
+ fig: plt.Figure,
+ ax: plt.Axes,
+ kinematic_states: KinematicStates,
+ bandwidth: float | Literal["scott", "silverman"] | ScottCoef = "scott",
+) -> tuple[plt.Figure, plt.Axes]:
+ """Visualizes the speed and steering angle of kinematic states using kernel density estimation.
+
+ This function plots a heatmap of the kernel density estimation of speed and steering angle
+ values from the given kinematic states. It also overlays the convex hull and marks the minimum
+ density point within the convex hull.
+
+ Parameters:
+ fig (plt.Figure): The figure object to be used for plotting.
+ ax (plt.Axes): The axes object to be used for plotting.
+ kinematic_states (KinematicStates): The kinematic states containing speed and steering angle data.
+
+ Returns:
+ tuple[plt.Figure, plt.Axes]: The modified figure and axes objects.
+ """
+ # vertexes of convex hull
+ POINTS = np.array(
+ [
+ [1.0, 0.15],
+ [4.0, 0.15],
+ [11.0, 0.0],
+ [1.0, -0.15],
+ [4, -0.15],
+ ]
+ )
+
+ fst: Literal["speed", "acc", "steer"] = "speed"
+ snd: Literal["speed", "acc", "steer"] = "steer"
+ fst_range = [0.0, 12.0]
+ snd_range = [-0.3, 0.3]
+
+ # calculate convex hull
+ hull = ConvexHull(POINTS)
+
+ X_STEER, Y_STEER, Z_STEER = kde_data(
+ kinematic_states=kinematic_states,
+ fst=fst,
+ snd=snd,
+ fst_range=fst_range,
+ snd_range=snd_range,
+ n_points=200,
+ bandwidth=bandwidth,
+ )
+
+ # plot heatmap
+ mesh_steer = ax.pcolormesh(X_STEER, Y_STEER, Z_STEER, cmap="viridis", shading="auto")
+ fig.colorbar(mesh_steer, ax=ax)
+
+ # plot edges of the convex hull
+ for simplex in hull.simplices:
+ ax.plot(POINTS[simplex, 0], POINTS[simplex, 1], color="orange")
+
+ # calculate the minimum value of kernel density in the convex hull
+ min_element, min_val = calc_minimum_density_point(
+ kinematic_states=kinematic_states,
+ fst=fst,
+ snd=snd,
+ fst_range=fst_range,
+ snd_range=snd_range,
+ shape=lambda x, y: Shape.convex_hull(POINTS, x, y),
+ bandwidth=bandwidth,
+ )
+ rounded_min_element = [round(min_element[0], 4), round(min_element[1], 4)]
+
+ # plot the minimum point as a white point
+ ax.plot(min_element[0], min_element[1], marker="o", color="white", markersize=2)
+
+ ax.set_xlabel(fst, fontsize=12)
+ ax.set_ylabel(snd, fontsize=12)
+ ax.set_title(f"kernel density: min_density={min_val: .4f} at {rounded_min_element}")
+ return fig, ax
diff --git a/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/get_estimated_wheel_base.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/get_estimated_wheel_base.py
new file mode 100644
index 0000000000000..752740787d8ab
--- /dev/null
+++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/get_estimated_wheel_base.py
@@ -0,0 +1,23 @@
+# Copyright 2024 Proxima Technology Inc, TIER IV
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from autoware_smart_mpc_trajectory_follower.scripts import drive_functions
+import numpy as np
+
+
+def get_estimated_wheel_base_coef(dir_name: str) -> float:
+ A = np.load(dir_name + "/polynomial_reg_info.npz")["A"]
+ estimated_wheel_base = 1 / (1 / drive_functions.L + A[3, 11])
+ print("estimated wheel base:", estimated_wheel_base)
+ return estimated_wheel_base / drive_functions.L
diff --git a/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/plot_auto_test_result.ipynb b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/plot_auto_test_result.ipynb
index dd4dc9ae25cb2..b876a635ef94a 100644
--- a/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/plot_auto_test_result.ipynb
+++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/plot_auto_test_result.ipynb
@@ -2,16 +2,18 @@
"cells": [
{
"cell_type": "code",
- "execution_count": null,
+ "execution_count": 1,
"id": "e78db57e-3218-4515-9ae5-4458ea071a1c",
"metadata": {},
"outputs": [],
"source": [
- "# cspell: ignore kernelspec ipykernel codemirror ipython nbconvert pygments nbformat \n",
+ "# cspell: ignore kernelspec ipykernel codemirror ipython nbconvert pygments nbformat #\n",
"\n",
"import numpy as np\n",
"import matplotlib.pyplot as plt\n",
- "import pandas as pd"
+ "import pandas as pd\n",
+ "import os\n",
+ "import json"
]
},
{
@@ -21,6 +23,7 @@
"metadata": {},
"outputs": [],
"source": [
+ "from autoware_smart_mpc_trajectory_follower.scripts import drive_functions\n",
"import python_simulator"
]
},
@@ -33,31 +36,28 @@
"source": [
"# header of auto_test_result_*.csv\n",
"header = [\n",
- " \"param_name\", # 0\n",
- " \"param_val\", # 1\n",
- " \"max_abs_lateral_error\", # 2\n",
- " \"max_abs_vel_error\", # 3\n",
- " # \"max_abs_yaw_error\", # 4\n",
- " # \"max_abs_acc_error\", # 5\n",
- " # \"max_abs_steer_error\" # 6\n",
+ " \"max_abs_lateral_error\", \n",
+ " \"straight_line_abs_max_lateral_error\",\n",
+ " \"max_abs_vel_error\", \n",
"]\n",
"\n",
"\n",
"plot_param_list = [\n",
" # [label, header_index, unit_for_plot, nominal_value]\n",
- " [\"steer_bias\", 2, \"[deg]\", 0.0] ,\n",
- " [\"steer_rate_lim\", 2, \"[rad/s]\", python_simulator.steer_rate_lim],\n",
- " [\"vel_rate_lim\", 3, \"[m/ss]\", python_simulator.vel_rate_lim],\n",
- " [\"steer_dead_band\", 2, \"[rad]\", python_simulator.steer_dead_band],\n",
- " [\"wheel_base\",2, \"[m]\", python_simulator.L],\n",
+ " [\"steer_bias\", 0, \"[deg]\", 0.0] ,\n",
+ " #[\"steer_rate_lim\", 0, \"[rad/s]\", python_simulator.steer_rate_lim],\n",
+ " #[\"vel_rate_lim\", 2, \"[m/ss]\", python_simulator.vel_rate_lim],\n",
+ " [\"steer_dead_band\", 0, \"[rad]\", python_simulator.steer_dead_band],\n",
+ " [\"wheel_base\",0, \"[m]\", drive_functions.L],\n",
" # [\"adaptive_gear_ratio_coef\", 2,\"[-]\", None],\n",
- " [\"acc_time_delay\",3, \"[s]\", python_simulator.acc_time_delay],\n",
- " [\"steer_time_delay\",2, \"[s]\", python_simulator.steer_time_delay],\n",
- " [\"acc_time_constant\",3, \"[s]\", python_simulator.acc_time_constant],\n",
- " [\"steer_time_constant\",2, \"[s]\", python_simulator.steer_time_constant],\n",
- " [\"accel_map_scale\",3, \"[-]\", None],\n",
- " [\"acc_scaling\",3, \"[-]\", 1],\n",
- " [\"steer_scaling\",2, \"[-]\", 1],\n",
+ " [\"acc_time_delay\",2, \"[s]\", drive_functions.acc_time_delay],\n",
+ " [\"steer_time_delay\",0, \"[s]\", drive_functions.steer_time_delay],\n",
+ " [\"acc_time_constant\",2, \"[s]\", drive_functions.acc_time_constant],\n",
+ " [\"steer_time_constant\",0, \"[s]\", drive_functions.steer_time_constant],\n",
+ " [\"accel_map_scale\",2, \"[-]\", None],\n",
+ " [\"acc_scaling\",2, \"[-]\", 1],\n",
+ " [\"steer_scaling\",0, \"[-]\", 1],\n",
+ " [\"vehicle_type\", 0, \"[-]\",0],\n",
"]\n",
"print(plot_param_list)\n",
"print(\"len(plot_param_list)\",len(plot_param_list))\n"
@@ -72,85 +72,78 @@
},
"outputs": [],
"source": [
+ "mem_diff = True\n",
+ "root_dir = \"auto_test\"\n",
+ "\n",
+ "def get_dir_name(param_name, index, intermediate):\n",
+ " dir_name = root_dir + \"/test_pp_aided_sim_\"\n",
+ " if not intermediate:\n",
+ " dir_name += \"trained_\"\n",
+ " \n",
+ " dir_name += param_name + \"_\" + str(index) + \"th\"\n",
+ " if mem_diff:\n",
+ " dir_name += \"_mem_diff\"\n",
+ " return dir_name\n",
"fig = plt.figure(figsize=(30, 18), tight_layout=True)\n",
"\n",
"ax_list = []\n",
"for i in range(len(plot_param_list)):\n",
- " ax_list.append(plt.subplot(4, 3, i+1))\n",
+ " ax_list.append(plt.subplot(4, 4, i+1))\n",
"\n",
- "# ff_data_result_csv = \"auto_test_result_final_model_control_trained_with_data_collected_by_ff_control.csv\"\n",
- "nominal_data_result_csv = \"auto_test_result_final_model_control_trained_with_data_collected_by_nominal_control.csv\"\n",
"\n",
"df_info_list = [ \n",
- " {\"file_name\": \"nominal/auto_test_result_nominal_model_control.csv\", \n",
- " \"label_name\": \"nominal\"},\n",
- "\n",
- " # {\"file_name\": \"poly(selected) (data using FF)/\"+ff_data_result_csv, \n",
- " # \"label_name\" : \"poly(selected) (data using FF)\"}, \n",
- " # # {\"file_name\": \"poly(full) (data using FF)/\"+ff_data_result_csv, \n",
- " # # \"label_name\" : \"poly(full) (data using FF)\"}, \n",
- " # {\"file_name\": \"poly(selected)+NN (data using FF)/\"+ff_data_result_csv, \n",
- " # \"label_name\" : \"poly(selected)+NN (data using FF)\"}, \n",
- " # # {\"file_name\": \"poly(full)+NN (data using FF)/\"ff_data_result_csv, \n",
- " # # \"label_name\" : \"poly(full)+NN (data using FF)\"},\n",
- " # {\"file_name\": \"NN (data using FF)/\"+ff_data_result_csv, \n",
- " # \"label_name\" : \"NN (data using FF)\"},\n",
- " \n",
- " {\"file_name\": \"poly(selected) (nominal control data)/\"+nominal_data_result_csv, \n",
- " \"label_name\" : \"poly(selected) (nominal control data)\"}, \n",
- " # {\"file_name\": \"poly(full) (nominal control data)/\"+nominal_data_result_csv, \n",
- " # \"label_name\" : \"poly(full) (nominal control data)\"},\n",
- " {\"file_name\": \"poly(selected)+NN (nominal control data)/\"+nominal_data_result_csv, \n",
- " \"label_name\" : \"poly(selected)+NN (nominal control data)\"}, \n",
- " # {\"file_name\": \"poly(full)+NN (nominal control data)/\"+nominal_data_result_csv, \n",
- " # \"label_name\" : \"poly(full)+NN (nominal control data)\"},\n",
- " {\"file_name\": \"NN (nominal control data)/\"+nominal_data_result_csv, \n",
- " \"label_name\" : \"NN (nominal control data)\"}, \n",
+ " { \n",
+ " \"intermediate\" : False, \n",
+ " \"label_name\" : \"poly(selected)+NN (final model)\"},\n",
+ " { \n",
+ " \"intermediate\" : True, \n",
+ " \"label_name\" : \"poly(selected)+NN (intermediate model)\"},\n",
"]\n",
"\n",
+ "plot_intermediate = []\n",
+ "\n",
"cmap = plt.get_cmap(\"tab10\") \n",
"for l in range(len(df_info_list)):\n",
- " df = pd.read_csv(df_info_list[l][\"file_name\"], header=None,sep=\",\")\n",
+ " #df = pd.read_csv(df_info_list[l][\"file_name\"], header=None,sep=\",\")\n",
" for i in range(len(plot_param_list)):\n",
- " index_bool = df[0].values==plot_param_list[i][0]\n",
- " if any(index_bool):\n",
- " x = df[1].values[index_bool]\n",
- " \n",
- " if plot_param_list[i][0]==\"adaptive_gear_ratio_coef\":\n",
- " x = np.array(range(len(x)))\n",
- " x = np.array([float(x[j]) for j in range(len(x))])\n",
+ " index = 0\n",
+ " x = []\n",
+ " y = []\n",
+ " while os.path.exists(get_dir_name(plot_param_list[i][0],index,df_info_list[l][\"intermediate\"])):\n",
+ " with open(get_dir_name(plot_param_list[i][0],index,df_info_list[l][\"intermediate\"])+\"/auto_test_performance_result.json\",\"r\") as f:\n",
+ " data=json.load(f)\n",
+ " x.append(list(list(data.values())[7].values())[0])\n",
+ " y.append(list(data.values())[plot_param_list[i][1]])\n",
+ " index += 1\n",
+ " if len(x) > 0:\n",
+ " x = np.array(x)\n",
+ " y = np.array(y)\n",
" if plot_param_list[i][0]==\"steer_bias\":\n",
" x *= 180.0/np.pi\n",
- " \n",
- " \n",
- " y = df[plot_param_list[i][1]].values[index_bool]\n",
+ " if i==0:\n",
" ax_list[i].plot(x,y,\"o-\",label=df_info_list[l][\"label_name\"],color=cmap(l))\n",
" else:\n",
- " pass\n",
- " if i==0:\n",
- " ax_list[i].plot([0],[0],\"o-\",label=df_info_list[l][\"label_name\"],color=cmap(l))\n",
- "\n",
+ " ax_list[i].plot(x,y,\"o-\",label=df_info_list[l][\"label_name\"],color=cmap(l))\n",
"\n",
- "for i in range(len(plot_param_list)): \n",
- " #ax_list[i].set_title(header[plot_param_list[i][1]]+\" / \"+plot_param_list[i][0], fontsize=16)\n",
+ "for i in range(len(plot_param_list)):\n",
" ax_list[i].set_xlabel(plot_param_list[i][0] + \" \" + plot_param_list[i][2],fontsize=20)\n",
" ax_list[i].set_ylabel(header[plot_param_list[i][1]],fontsize=20)\n",
" ax_list[i].grid()\n",
"\n",
" if plot_param_list[i][3] is not None:\n",
- " ax_list[i].plot([plot_param_list[i][3],plot_param_list[i][3]],[-0.1,2],\"k--\")\n",
- " \n",
+ " ax_list[i].plot([plot_param_list[i][3],plot_param_list[i][3]],[-0.1,3],\"k--\")\n",
+ "\n",
" # lateral_error\n",
" if 2==plot_param_list[i][1]:\n",
- " ax_list[i].set_ylim([-0.01,1.0])\n",
+ " ax_list[i].set_ylim([-0.01,2.5])\n",
" # vel_error\n",
" if 3==plot_param_list[i][1]:\n",
" ax_list[i].set_ylim([-0.01,1.0])\n",
"\n",
- " # if(y.max()>2.0):\n",
- " # ax_list[i].set_ylim([-0.1,2.0])\n",
" if i==0:\n",
" ax_list[i].legend(fontsize=13)\n",
+ " if plot_param_list[i][0]==\"vehicle_type\":\n",
+ " ax_list[i].text(0.5, 0.8, \"0: nominal vehicle\\n1: heavy-weight bus\\n2: light-weight bus\\n3: small vehicle\\n4: small robot\", va='center', ha='center', fontsize=15, transform=ax_list[i].transAxes)\n",
"plt.savefig(\"auto_test_result.png\")\n",
"plt.show()"
]
diff --git a/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/pure_pursuit_gain_updater.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/pure_pursuit_gain_updater.py
new file mode 100644
index 0000000000000..6e6e4c76c6054
--- /dev/null
+++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/pure_pursuit_gain_updater.py
@@ -0,0 +1,198 @@
+# Copyright 2024 Proxima Technology Inc, TIER IV
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import warnings
+
+from autoware_smart_mpc_trajectory_follower.scripts import drive_functions
+import numpy as np
+from sklearn import linear_model
+
+warnings.simplefilter("ignore")
+
+
+class pure_pursuit_gain_updater:
+ def __init__(self, max_time=10.0, max_input_delay=1.5):
+ self.vel_queue = []
+ self.yaw_queue = []
+ self.acc_queue = []
+ self.steer_queue = []
+ self.acc_input_queue = []
+ self.steer_input_queue = []
+ self.max_queue_size = round(max_time / drive_functions.ctrl_time_step)
+ self.max_input_delay_size = round(max_input_delay / drive_functions.ctrl_time_step)
+ self.A_wheel_base = np.zeros((2, 2))
+ self.B_wheel_base = np.zeros(2)
+
+ def state_queue_updater(self, vel, yaw, acc, steer):
+ self.vel_queue.append(vel)
+ self.yaw_queue.append(yaw)
+ self.acc_queue.append(acc)
+ self.steer_queue.append(steer)
+ if len(self.vel_queue) > self.max_queue_size:
+ self.vel_queue.pop(0)
+ self.yaw_queue.pop(0)
+ self.acc_queue.pop(0)
+ self.steer_queue.pop(0)
+ if len(self.vel_queue) > 1:
+ numerator = self.vel_queue[-2] * np.tan(self.steer_queue[-2])
+ yaw_dot_error = (
+ self.yaw_queue[-1] - self.yaw_queue[-2]
+ ) / drive_functions.ctrl_time_step
+ yaw_dot_error -= numerator / drive_functions.L
+
+ X_wheel_base = np.array([[1.0, numerator]])
+ self.A_wheel_base += X_wheel_base.T @ X_wheel_base + 1e-5 * np.eye(2)
+ self.B_wheel_base += yaw_dot_error * X_wheel_base[0]
+
+ def input_queue_updater(self, acc_input, steer_input):
+ self.acc_input_queue.append(acc_input)
+ self.steer_input_queue.append(steer_input)
+ if len(self.acc_input_queue) > self.max_queue_size - 1:
+ self.acc_input_queue.pop(0)
+ self.steer_input_queue.pop(0)
+
+ def get_acc_gain_scaling(self):
+ if len(self.vel_queue) < self.max_queue_size:
+ return 1.0
+ acc_array = np.array(self.acc_queue)
+ acc_input_array = np.array(self.acc_input_queue)
+
+ acc_dot = (acc_array[1:] - acc_array[:-1]) / drive_functions.ctrl_time_step
+ acc_estimation_error = np.inf
+ best_acc_coef = np.array(
+ [
+ -drive_functions.ctrl_time_step / drive_functions.acc_time_constant,
+ drive_functions.ctrl_time_step / drive_functions.acc_time_constant,
+ ]
+ )
+ for i in range(self.max_input_delay_size):
+ clf_acc = linear_model.ElasticNet(
+ fit_intercept=False, alpha=1e-10, l1_ratio=0.5, max_iter=100000
+ )
+ clf_acc.fit(
+ np.stack(
+ (
+ acc_array[self.max_input_delay_size - 1 : -1],
+ acc_input_array[
+ self.max_input_delay_size - 1 - i : len(acc_input_array) - i
+ ],
+ ),
+ 1,
+ ),
+ acc_dot[self.max_input_delay_size - 1 :],
+ )
+ acc_error = (
+ (
+ clf_acc.coef_[0] * acc_array[self.max_input_delay_size - 1 : -1]
+ + clf_acc.coef_[1]
+ * acc_input_array[self.max_input_delay_size - 1 - i : len(acc_input_array) - i]
+ - acc_dot[self.max_input_delay_size - 1 :]
+ )
+ ** 2
+ ).sum()
+
+ if acc_error < acc_estimation_error and clf_acc.coef_[0] < 0 and clf_acc.coef_[1] > 0:
+ acc_estimation_error = acc_error
+ best_acc_coef = clf_acc.coef_
+ X_acc = np.stack(
+ (
+ acc_array[self.max_input_delay_size - 1 : -1],
+ acc_input_array[self.max_input_delay_size - 1 - i : len(acc_input_array) - i],
+ ),
+ 1,
+ )
+ X_acc = np.hstack((np.ones((X_acc.shape[0], 1)), X_acc))
+
+ A = X_acc.T @ X_acc + 1e-15 * X_acc.shape[0] * np.eye(3)
+
+ B = X_acc.T @ acc_dot[self.max_input_delay_size - 1 :]
+
+ coef = np.linalg.solve(A, B)
+ acc_error = (
+ (
+ coef[1] * acc_array[self.max_input_delay_size - 1 : -1]
+ + coef[2]
+ * acc_input_array[self.max_input_delay_size - 1 - i : len(acc_input_array) - i]
+ + coef[0]
+ - acc_dot[self.max_input_delay_size - 1 :]
+ )
+ ** 2
+ ).sum()
+
+ if acc_error < acc_estimation_error and coef[1] < 0 and coef[2] > 0:
+ acc_estimation_error = acc_error
+ best_acc_coef = coef[[1, 2]]
+ estimated_acc_scaling = np.clip(-best_acc_coef[1] / best_acc_coef[0], 0.1, 10.0)
+
+ return 1 / estimated_acc_scaling
+
+ def get_steer_gain_scaling(self):
+ if len(self.vel_queue) < self.max_queue_size:
+ return 1.0
+
+ steer_array = np.array(self.steer_queue)
+ steer_input_array = np.array(self.steer_input_queue)
+
+ estimated_wheel_base_scaling = 1.0 / np.clip(
+ 1 + np.linalg.solve(self.A_wheel_base, self.B_wheel_base)[1] * drive_functions.L,
+ 0.1,
+ 1.0,
+ )
+
+ steer_dot = (steer_array[1:] - steer_array[:-1]) / drive_functions.ctrl_time_step
+
+ steer_estimation_error = np.inf
+ best_steer_coef = np.array(
+ [
+ -drive_functions.ctrl_time_step / drive_functions.steer_time_constant,
+ drive_functions.ctrl_time_step / drive_functions.steer_time_constant,
+ ]
+ )
+ for i in range(self.max_input_delay_size):
+ X_steer = np.stack(
+ (
+ steer_array[self.max_input_delay_size - 1 : -1],
+ steer_input_array[
+ self.max_input_delay_size - 1 - i : len(steer_input_array) - i
+ ],
+ ),
+ 1,
+ )
+ X_steer = np.hstack((np.ones((X_steer.shape[0], 1)), X_steer))
+
+ A = X_steer.T @ X_steer + 1e-15 * X_steer.shape[0] * np.eye(3)
+
+ B = X_steer.T @ steer_dot[self.max_input_delay_size - 1 :]
+
+ coef = np.linalg.solve(A, B)
+ steer_error = (
+ (
+ coef[1] * steer_array[self.max_input_delay_size - 1 : -1]
+ + coef[2]
+ * steer_input_array[
+ self.max_input_delay_size - 1 - i : len(steer_input_array) - i
+ ]
+ + coef[0]
+ - steer_dot[self.max_input_delay_size - 1 :]
+ )
+ ** 2
+ ).sum()
+
+ if steer_error < steer_estimation_error and coef[1] < 0 and coef[2] > 0:
+ steer_estimation_error = steer_error
+ best_steer_coef = coef[[1, 2]]
+
+ estimated_steer_scaling = np.clip(-best_steer_coef[1] / best_steer_coef[0], 0.1, 10.0)
+
+ return np.clip(estimated_wheel_base_scaling / estimated_steer_scaling, 0.1, 10.0)
diff --git a/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/python_simulator.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/python_simulator.py
index 0eaa6ce1c0fbf..78ccc0dea3922 100644
--- a/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/python_simulator.py
+++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/python_simulator.py
@@ -12,23 +12,30 @@
# See the License for the specific language governing permissions and
# limitations under the License.
-# cspell: ignore numba njit simplejson fastmath interp suptitle fftfreq basefmt markerfmt
-
-import csv
import datetime
import os
+from assets import ControlType # type: ignore
from autoware_smart_mpc_trajectory_follower.scripts import drive_controller
from autoware_smart_mpc_trajectory_follower.scripts import drive_functions
-import matplotlib.pyplot as plt
-from numba import njit
+import data_collection_utils # type: ignore
+from density_estimation import KinematicStates # type: ignore
+from density_estimation import visualize_speed_acc
+from density_estimation import visualize_speed_steer
+import matplotlib.pyplot as plt # type: ignore
+from numba import njit # type: ignore
import numpy as np
-import pandas as pd
-import scipy.interpolate
-import simplejson as json
+import pandas as pd # type: ignore
+import pure_pursuit_gain_updater # type: ignore
+import scipy.interpolate # type: ignore
+import simplejson as json # type: ignore
+# cSpell:ignore numba simplejson
print("\n\n### import python_simulator.py ###")
+ACTIVATE_LIMITS = False
+UPDATE_PP_GAIN = True
+
ctrl_time_step = drive_functions.ctrl_time_step
sim_dt = 0.003333 # Simulation time step
@@ -46,8 +53,12 @@
L = drive_functions.L
N = drive_functions.N
-steer_rate_lim = 0.35
-vel_rate_lim = 7.0
+if ACTIVATE_LIMITS:
+ steer_rate_lim = 0.35
+ vel_rate_lim = 7.0
+else:
+ steer_rate_lim = 10.35
+ vel_rate_lim = 107.0
mpc_freq = drive_functions.mpc_freq
@@ -114,7 +125,7 @@
1 * vgr_coef_a2,
1 * vgr_coef_b2,
1 * vgr_coef_c2,
- ]
+ ] # type: ignore
vgr_coef_a1 = sim_setting_dict["adaptive_gear_ratio_coef"][0]
vgr_coef_b1 = sim_setting_dict["adaptive_gear_ratio_coef"][1]
vgr_coef_c1 = sim_setting_dict["adaptive_gear_ratio_coef"][2]
@@ -171,7 +182,7 @@
print("perturbed steer_time_constant", steer_time_constant)
if "accel_map_scale" in sim_setting_dict.keys():
- nominal_setting_dict_display["accel_map_scale"] = None
+ nominal_setting_dict_display["accel_map_scale"] = None # type: ignore
use_accel_map = True
accel_map_scale = sim_setting_dict["accel_map_scale"]
df = pd.read_csv("accel_map.csv", header=None)
@@ -244,8 +255,9 @@ def accel_map_f(x, y):
pass
elif sim_setting_dict["vehicle_type"] == 1:
# heavy-weight bus
- steer_rate_lim = 5.0
- vel_rate_lim = 7.0
+ if ACTIVATE_LIMITS:
+ steer_rate_lim = 5.0
+ vel_rate_lim = 7.0
L = 4.76
acc_delay_step_sim = round(1.0 / sim_dt)
steer_delay_step_sim = round(1.0 / sim_dt)
@@ -254,8 +266,9 @@ def accel_map_f(x, y):
acc_scaling = 0.2
elif sim_setting_dict["vehicle_type"] == 2:
# light-weight bus
- steer_rate_lim = 5.0
- vel_rate_lim = 7.0
+ if ACTIVATE_LIMITS:
+ steer_rate_lim = 5.0
+ vel_rate_lim = 7.0
L = 4.76
acc_delay_step_sim = round(0.5 / sim_dt)
steer_delay_step_sim = round(0.5 / sim_dt)
@@ -264,8 +277,9 @@ def accel_map_f(x, y):
acc_scaling = 0.5
elif sim_setting_dict["vehicle_type"] == 3:
# small vehicle
- steer_rate_lim = 5.0
- vel_rate_lim = 7.0
+ if ACTIVATE_LIMITS:
+ steer_rate_lim = 5.0
+ vel_rate_lim = 7.0
L = 1.335
acc_delay_step_sim = round(0.3 / sim_dt)
steer_delay_step_sim = round(0.3 / sim_dt)
@@ -274,8 +288,9 @@ def accel_map_f(x, y):
acc_scaling = 1.5
elif sim_setting_dict["vehicle_type"] == 4:
# small robot
- steer_rate_lim = 60.0 * (np.pi / 180.0)
- vel_rate_lim = 3.0
+ if ACTIVATE_LIMITS:
+ steer_rate_lim = 60.0 * (np.pi / 180.0)
+ vel_rate_lim = 3.0
L = 0.395
acc_delay_step_sim = round(0.2 / sim_dt)
steer_delay_step_sim = round(0.2 / sim_dt)
@@ -308,6 +323,109 @@ def accel_map_f(x, y):
)
print("acc_scaling", nominal_setting_dict_display["acc_scaling"], acc_scaling)
+ if "test_vehicle" in sim_setting_dict.keys():
+ nominal_setting_dict_display["steer_rate_lim"] = 1 * steer_rate_lim
+ nominal_setting_dict_display["vel_rate_lim"] = 1 * vel_rate_lim
+ nominal_setting_dict_display["wheel_base"] = 1 * L
+ nominal_setting_dict_display["acc_time_delay"] = sim_dt * acc_delay_step_sim
+ nominal_setting_dict_display["steer_time_delay"] = sim_dt * steer_delay_step_sim
+ nominal_setting_dict_display["acc_time_constant"] = 1 * acc_time_constant
+ nominal_setting_dict_display["steer_time_constant"] = 1 * steer_time_constant
+ nominal_setting_dict_display["acc_scaling"] = 1 * acc_scaling
+ if sim_setting_dict["test_vehicle"] == 0:
+ L = 5.76
+ acc_delay_step_sim = round(0.2 / sim_dt)
+ steer_delay_step_sim = round(0.9 / sim_dt)
+ acc_time_constant = 0.4
+ steer_time_constant = 0.8
+ steer_scaling = 3.0
+ acc_scaling = 0.5
+ steer_dead_band = 0.002
+ measurement_steer_bias = 0.2 * np.pi / 180.0
+ elif sim_setting_dict["test_vehicle"] == 1:
+ L = 1.76
+ acc_delay_step_sim = round(0.7 / sim_dt)
+ steer_delay_step_sim = round(0.8 / sim_dt)
+ acc_time_constant = 0.2
+ steer_time_constant = 0.9
+ steer_scaling = 0.3
+ acc_scaling = 3.0
+ steer_dead_band = 0.001
+ measurement_steer_bias = 0.3 * np.pi / 180.0
+ elif sim_setting_dict["test_vehicle"] == 2:
+ L = 4.56
+ acc_delay_step_sim = round(0.4 / sim_dt)
+ steer_delay_step_sim = round(0.8 / sim_dt)
+ acc_time_constant = 0.4
+ steer_time_constant = 0.9
+ steer_scaling = 0.6
+ acc_scaling = 0.3
+ steer_dead_band = 0.004
+ measurement_steer_bias = 0.7 * np.pi / 180.0
+ elif sim_setting_dict["test_vehicle"] == 3:
+ # heavy-weight bus
+ L = 4.76
+ acc_delay_step_sim = round(1.0 / sim_dt)
+ steer_delay_step_sim = round(1.0 / sim_dt)
+ acc_time_constant = 1.0
+ steer_time_constant = 1.0
+ acc_scaling = 0.2
+ elif sim_setting_dict["test_vehicle"] == 4:
+ L = 3.56
+ acc_delay_step_sim = round(0.3 / sim_dt)
+ steer_delay_step_sim = round(0.8 / sim_dt)
+ acc_time_constant = 0.3
+ steer_time_constant = 0.9
+ steer_scaling = 0.3
+ acc_scaling = 0.3
+ steer_dead_band = 0.003
+ measurement_steer_bias = 0.3 * np.pi / 180.0
+ elif sim_setting_dict["test_vehicle"] == 5:
+ L = 4.76
+ acc_delay_step_sim = round(1.0 / sim_dt)
+ steer_delay_step_sim = round(0.7 / sim_dt)
+ acc_time_constant = 1.0
+ steer_time_constant = 0.7
+ acc_scaling = 0.2
+ elif sim_setting_dict["test_vehicle"] == 6:
+ L = 4.76
+ acc_delay_step_sim = round(1.0 / sim_dt)
+ steer_delay_step_sim = round(0.6 / sim_dt)
+ acc_time_constant = 1.0
+ steer_time_constant = 0.6
+ acc_scaling = 0.2
+ elif sim_setting_dict["test_vehicle"] == 7:
+ L = 4.76
+ acc_delay_step_sim = round(1.0 / sim_dt)
+ steer_delay_step_sim = round(0.5 / sim_dt)
+ acc_time_constant = 1.0
+ steer_time_constant = 0.5
+ acc_scaling = 0.2
+ print("steer_rate_lim", nominal_setting_dict_display["steer_rate_lim"], steer_rate_lim)
+ print("vel_rate_lim", nominal_setting_dict_display["vel_rate_lim"], vel_rate_lim)
+ print("wheel_base", nominal_setting_dict_display["wheel_base"], L)
+ print(
+ "acc_time_delay",
+ nominal_setting_dict_display["acc_time_delay"],
+ sim_dt * acc_delay_step_sim,
+ )
+ print(
+ "steer_time_delay",
+ nominal_setting_dict_display["steer_time_delay"],
+ sim_dt * steer_delay_step_sim,
+ )
+ print(
+ "acc_time_constant",
+ nominal_setting_dict_display["acc_time_constant"],
+ acc_time_constant,
+ )
+ print(
+ "steer_time_constant",
+ nominal_setting_dict_display["steer_time_constant"],
+ steer_time_constant,
+ )
+ print("acc_scaling", nominal_setting_dict_display["acc_scaling"], acc_scaling)
+
# dynamics
# @njit(cache=False, fastmath=True) # Commented out because we want to use scipy.interpolate2d
@@ -336,13 +454,13 @@ def f_sim(
else:
delta_diff = 0.0
- actual_alpha = alpha
+ actual_alpha_input = acc_scaling * inputs[0]
actual_delta = delta - measurement_steer_bias
# Acceleration input value -> Actual acceleration input value distortion applied
if use_accel_map:
- if actual_alpha > 0:
- actual_alpha = accel_map_f(v, actual_alpha)[0] # vel, acc_cmd
+ if actual_alpha_input > 0:
+ actual_alpha_input = accel_map_f(v, actual_alpha_input)[0] # vel, acc_cmd
# Tire angle input value -> Actual tire angle input value distortion application
steer_tire_angle_cmd = 1 * delta
@@ -360,9 +478,9 @@ def f_sim(
states_dot = np.zeros(6)
states_dot[0] = v * np.cos(theta)
states_dot[1] = v * np.sin(theta)
- states_dot[2] = actual_alpha
+ states_dot[2] = alpha
states_dot[3] = v * np.tan(actual_delta) / L
- states_dot[4] = (acc_scaling * inputs[0] - alpha) / acc_time_constant
+ states_dot[4] = (actual_alpha_input - alpha) / acc_time_constant
states_dot[5] = delta_diff / steer_time_constant
return states_dot
@@ -383,6 +501,7 @@ def F_sim(
return states_next
+# cSpell:ignore njit fastmath
@njit(cache=False, fastmath=True)
def d_inputs_to_inputs(u_old, d_inputs):
"""Compute the sequence of input values from the sequence of input change rates."""
@@ -469,360 +588,42 @@ def get_mpc_trajectory(x_current, trajectory_data, trajectory_interpolator_list)
return X_des, U_des, x_current[:6] - trajectory_data[nearest_index, 1:7], break_flag
-def get_feedforward_nominal_input(t, trajectory_data):
- """Calculate the nominal input for feed-forward driving."""
- total_time = trajectory_data[-1, 0]
- t_current = t - (t // total_time) * total_time
- nearest_index = np.argmin(np.abs(trajectory_data[:, 0] - t_current))
- return trajectory_data[nearest_index, [5, 6]]
-
-
-def create_additional_sine_data(
- seed,
- t_range,
- acc_width_range,
- acc_period_range,
- steer_width_range,
- steer_period_range,
- large_steer_width_range,
- large_steer_period_range,
- start_large_steer_time,
-):
- """Create sine wave data to be added randomly to feed-forward runs."""
- np.random.seed(seed=seed)
- t_acc = 0.0
- t_steer = 0.0
- t_large_steer = 0.0
- t_acc_list = []
- t_steer_list = []
- t_large_steer_list = []
- t_acc_list.append(t_acc)
- t_steer_list.append(t_steer)
- t_large_steer_list.append(t_large_steer)
- t_large_steer += start_large_steer_time
- t_large_steer_list.append(t_large_steer)
- width_acc_list = []
- width_steer_list = []
- width_large_steer_list = []
- width_large_steer_list.append(0)
- while True:
- if max(t_acc, t_large_steer) >= t_steer:
- period = (
- steer_period_range[1] - steer_period_range[0]
- ) * np.random.uniform() + steer_period_range[0]
- t_steer += period
- t_steer_list.append(t_steer)
- width_steer_list.append(steer_width_range * np.random.uniform())
- elif t_large_steer >= t_acc:
- period = (
- acc_period_range[1] - acc_period_range[0]
- ) * np.random.uniform() + acc_period_range[0]
- t_acc += period
- t_acc_list.append(t_acc)
- width_acc_list.append(acc_width_range * np.random.uniform())
- else:
- period = (
- large_steer_period_range[1] - large_steer_period_range[0]
- ) * np.random.uniform() + large_steer_period_range[0]
- t_large_steer += period
- t_large_steer_list.append(t_large_steer)
- width_large_steer_list.append(large_steer_width_range * np.random.uniform())
- if t_acc >= t_range[1] and t_steer >= t_range[1] and t_large_steer >= t_range[1]:
- break
- return (
- np.array(t_acc_list),
- np.array(width_acc_list),
- np.array(t_steer_list),
- np.array(width_steer_list),
- np.array(t_large_steer_list),
- np.array(width_large_steer_list),
- )
-
-
-@njit(cache=False, fastmath=True)
-def get_current_additional_sine(
- t,
- t_acc_array,
- width_acc_array,
- t_steer_array,
- width_steer_array,
- t_large_steer_array,
- width_large_steer_array,
-):
- """Calculate current values from already created sine wave data."""
- acc_index = 0
- steer_index = 0
- large_steer_index = 0
- for i in range(t_acc_array.shape[0] - 1):
- if t < t_acc_array[i + 1]:
- break
- acc_index += 1
- for i in range(t_steer_array.shape[0] - 1):
- if t < t_steer_array[i + 1]:
- break
- steer_index += 1
- for i in range(t_large_steer_array.shape[0] - 1):
- if t < t_large_steer_array[i + 1]:
- break
- large_steer_index += 1
- acc = width_acc_array[acc_index] * np.sin(
- 2
- * np.pi
- * (t - t_acc_array[acc_index])
- / (t_acc_array[acc_index + 1] - t_acc_array[acc_index])
- )
- steer = width_steer_array[steer_index] * np.sin(
- 2
- * np.pi
- * (t - t_steer_array[steer_index])
- / (t_steer_array[steer_index + 1] - t_steer_array[steer_index])
- )
- steer += width_large_steer_array[large_steer_index] * np.sin(
- 2
- * np.pi
- * (t - t_large_steer_array[large_steer_index])
- / (t_large_steer_array[large_steer_index + 1] - t_large_steer_array[large_steer_index])
- )
- return np.array([acc, steer])
-
-
-def create_vel_sine_data(seed, t_range, vel_width_range, vel_period_range):
- """Create sine wave data for target velocity."""
- np.random.seed(seed=seed)
- t_vel = 0.0
- t_vel_list = []
- t_vel_list.append(t_vel)
- width_vel_list = []
- while True:
- period = (
- vel_period_range[1] - vel_period_range[0]
- ) * np.random.uniform() + vel_period_range[0]
- t_vel += period
- t_vel_list.append(t_vel)
- width_vel_list.append(vel_width_range * np.random.uniform())
- if t_vel >= t_range[1]:
- break
- return (
- np.array(t_vel_list),
- np.array(width_vel_list),
- )
-
-
-@njit(cache=False, fastmath=True)
-def get_current_vel_sine(t, t_vel_array, width_vel_array, v_mid):
- """Calculate current target velocity values from already created sine wave data."""
- vel_index = 0
- for i in range(t_vel_array.shape[0] - 1):
- if t < t_vel_array[i + 1]:
- break
- vel_index += 1
- vel = v_mid + width_vel_array[vel_index] * np.sin(
- 2
- * np.pi
- * (t - t_vel_array[vel_index])
- / (t_vel_array[vel_index + 1] - t_vel_array[vel_index])
- )
- return vel
-
-
-def get_figure_eight_point(t, circle_radius):
- """
- Get the position and yaw angle in world coordinates of the figure eight given the circle radius.
-
- Here t is a 1-dimensional array of numpy and each t[i] represents the distance traveled.
- The return value is a 2-dimensional array of positions and a 1-dimensional array of yaw angles corresponding to t.
- """
- sign = -2 * (np.floor(t / (2 * np.pi * circle_radius)) % 2).astype(int) + 1
- x = circle_radius * np.sin(t / circle_radius)
- y = sign * circle_radius * (1 - np.cos(t / circle_radius))
- yaw = (
- sign * ((t / circle_radius) % (2 * np.pi) - np.pi) + np.pi
- ) # if sign == 1, then yaw = (t/circle_radius)%(2*np.pi). Else, yaw = 2*np.pi - (t/circle_radius)%(2*np.pi).
- return np.array([x, y]).T, yaw
-
-
-def pure_pursuit(
- x_current,
- target_position,
- target_vel,
- target_yaw,
- acc_lim=7.0,
- steer_lim=1.0,
- wheel_base=drive_functions.L,
- lookahead_time=3.0,
- min_lookahead=3.0,
- acc_kp=0.5,
-):
- """Calculate acceleration and steer angle input values based on pure pursuit."""
- present_position = x_current[:2]
- present_longitudinal_velocity = x_current[2]
- present_point_yaw = x_current[3]
- longitudinal_vel_err = present_longitudinal_velocity - target_vel
- acc_kp = 0.5
- acc_cmd = np.clip(-acc_kp * longitudinal_vel_err, -acc_lim, acc_lim)
-
- # compute steer cmd
- cos_yaw = np.cos(target_yaw)
- sin_yaw = np.sin(target_yaw)
- diff_position = present_position - target_position
- lat_err = -sin_yaw * diff_position[0] + cos_yaw * diff_position[1]
- yaw_err = present_point_yaw - target_yaw
- while True:
- if yaw_err > np.pi:
- yaw_err -= 2.0 * np.pi
- if yaw_err < (-np.pi):
- yaw_err += 2.0 * np.pi
- if np.abs(yaw_err) < np.pi:
- break
-
- lookahead = min_lookahead + lookahead_time * np.abs(present_longitudinal_velocity)
- steer_kp = 2.0 * wheel_base / (lookahead * lookahead)
- steer_kd = 2.0 * wheel_base / lookahead
- steer_cmd = np.clip(-steer_kp * lat_err - steer_kd * yaw_err, -steer_lim, steer_lim)
- return np.array([acc_cmd, steer_cmd])
-
-
-def get_pure_pursuit_info(x_current, trajectory_position_data, trajectory_yaw_data, previous_index):
- """Calculate the target position and yaw angle required for pure pursuit."""
- search_range = (
- np.arange(
- previous_index - trajectory_position_data.shape[0] // 4,
- previous_index + trajectory_position_data.shape[0] // 4,
- )
- % trajectory_position_data.shape[0]
- )
- nearest_index = np.argmin(
- ((trajectory_position_data[search_range] - x_current[:2].reshape(1, 2)) ** 2).sum(axis=1)
- )
- return (
- trajectory_position_data[search_range[nearest_index]],
- trajectory_yaw_data[search_range[nearest_index]],
- search_range[nearest_index],
- )
-
-
-class driving_log_updater:
- """Class for updating logs when driving on the Python simulator."""
-
- def __init__(self):
- self.X_history = []
- self.U_history = []
- self.control_cmd_time_stamp_list = []
- self.control_cmd_steer_list = []
- self.control_cmd_acc_list = []
- self.kinematic_state_list = []
- self.acceleration_list = []
- self.steering_status_list = []
- self.control_cmd_orig_list = []
- self.operation_mode_list = []
-
- def update(self, t_current, x_current, u_current):
- """Update logs."""
- self.X_history.append(x_current)
- self.U_history.append(u_current)
- self.control_cmd_time_stamp_list.append(t_current)
- self.control_cmd_steer_list.append(u_current[1])
- self.control_cmd_acc_list.append(u_current[0])
- if self.control_cmd_time_stamp_list[-1] - self.control_cmd_time_stamp_list[0] > 3.0:
- self.control_cmd_time_stamp_list.pop(0)
- self.control_cmd_steer_list.pop(0)
- self.control_cmd_acc_list.pop(0)
- t_sec = int(t_current)
- t_n_sec = int(1e9 * (t_current - t_sec))
- kinematic_state = np.zeros(7)
- acceleration = np.zeros(4)
- steering_status = np.zeros(3)
- control_cmd_orig = np.zeros(10)
- operation_mode = np.zeros(3)
- kinematic_state[0] = t_sec
- kinematic_state[1] = t_n_sec
- kinematic_state[2] = x_current[0]
- kinematic_state[3] = x_current[1]
- kinematic_state[4] = np.sin(0.5 * x_current[3])
- kinematic_state[5] = np.cos(0.5 * x_current[3])
- kinematic_state[6] = x_current[2]
- self.kinematic_state_list.append(kinematic_state)
- acceleration[0] = t_sec
- acceleration[1] = t_n_sec
- acceleration[3] = x_current[4]
- self.acceleration_list.append(acceleration)
- steering_status[0] = t_sec
- steering_status[1] = t_n_sec
- steering_status[2] = x_current[5]
- self.steering_status_list.append(steering_status)
- control_cmd_orig[0] = t_sec
- control_cmd_orig[1] = t_n_sec
- control_cmd_orig[4] = u_current[1]
- control_cmd_orig[9] = u_current[0]
- self.control_cmd_orig_list.append(control_cmd_orig)
- operation_mode[0] = t_sec
- operation_mode[1] = t_n_sec
- operation_mode[2] = 2.0
- self.operation_mode_list.append(operation_mode)
-
- def save(self, save_dir):
- """Save logs in csv format."""
- kinematic_states = np.zeros((len(self.kinematic_state_list), 48))
- kinematic_states[:, [0, 1, 4, 5, 9, 10, 47]] = np.array(self.kinematic_state_list)
- np.savetxt(save_dir + "/kinematic_state.csv", kinematic_states, delimiter=",")
- np.savetxt(save_dir + "/acceleration.csv", np.array(self.acceleration_list), delimiter=",")
- np.savetxt(
- save_dir + "/steering_status.csv",
- np.array(self.steering_status_list),
- delimiter=",",
- )
- np.savetxt(
- save_dir + "/control_cmd_orig.csv",
- np.array(self.control_cmd_orig_list),
- delimiter=",",
- )
- np.savetxt(
- save_dir + "/system_operation_mode_state.csv",
- np.array(self.operation_mode_list),
- delimiter=",",
- )
- with open(save_dir + "/system_operation_mode_state.csv", "w") as f:
- writer = csv.writer(f)
- for i in range(len(self.operation_mode_list)):
- operation_mode_plus_true = self.operation_mode_list[i].tolist()
- operation_mode_plus_true.append("True")
- writer.writerow(operation_mode_plus_true)
-
-
-def slalom_drive(
+def drive_sim(
save_file=True,
save_dir=None,
load_dir=drive_functions.load_dir,
visualize=True,
use_trained_model=False,
use_trained_model_diff=None,
- control_type="mpc", # feedforward_test=False,
- straight_line_test=False,
+ use_memory_diff=None,
+ control_type: ControlType = ControlType.mpc,
initial_error=np.zeros(6),
t_range=[0, 100],
seed=1,
- acc_width_range=0.005,
+ acc_amp_range=0.05,
acc_period_range=[5.0, 20.0],
- steer_width_range=0.005,
- steer_period_range=[5.0, 20.0],
- large_steer_width_range=0.00,
+ steer_amp_range=0.005,
+ steer_period_range=[5.0, 30.0],
+ large_steer_amp_range=0.00,
large_steer_period_range=[5.0, 20.0],
start_large_steer_time=40.0,
- vel_width_range=5.0,
- vel_period_range=[5.0, 20.0],
- v_mid=6.0,
- circle_radius=30.0,
+ acc_max=1.2,
+ constant_vel_time=5.0,
+ split_size=5,
+ y_length=60.0,
+ x_length=120.0,
+ step_response_max_input=0.01,
+ step_response_max_length=1.5,
+ step_response_start_time_ratio=0.0,
+ step_response_interval=5.0,
+ step_response_min_length=0.5,
+ smoothing_trajectory_data_flag=True,
):
"""Perform a slalom driving simulation."""
- if control_type == "pp":
- print("\n[run figure_eight_drive]\n")
- elif control_type == "ff":
- print("\n[run feedforward_drive]\n")
+ if control_type != ControlType.mpc:
+ print(f"\n[run {control_type.value}]\n")
else:
- if not straight_line_test:
- print("\n[run slalom_drive]\n")
- else:
- print("\n[straight_line_test]\n")
+ print("\n[run slalom_drive]\n")
if save_file:
if save_dir is None:
save_dir_ = "python_sim_log_" + str(datetime.datetime.now())
@@ -830,40 +631,41 @@ def slalom_drive(
save_dir_ = save_dir
if not os.path.isdir(save_dir_):
os.mkdir(save_dir_)
- controller = drive_controller.drive_controller(
- model_file_name=(load_dir + "/model_for_test_drive.pth"),
- load_GP_dir=load_dir,
- load_polynomial_reg_dir=load_dir,
- use_trained_model=use_trained_model,
- use_trained_model_diff=use_trained_model_diff,
- load_train_data_dir=load_dir,
- )
- mode = controller.mode
- if control_type == "mpc":
+ if control_type == ControlType.mpc:
+ controller = drive_controller.drive_controller(
+ model_file_name=(load_dir + "/model_for_test_drive.pth"),
+ load_GP_dir=load_dir,
+ load_polynomial_reg_dir=load_dir,
+ use_trained_model=use_trained_model,
+ use_trained_model_diff=use_trained_model_diff,
+ use_memory_diff=use_memory_diff,
+ load_train_data_dir=load_dir,
+ )
+ mode = controller.mode
print("mode:", mode)
+ # cSpell:ignore interp
plt.rcParams["figure.figsize"] = (8, 8)
- t_eval = np.arange(*t_range, sim_dt)
- if not straight_line_test:
- trajectory_data = np.loadtxt("slalom_course_data.csv", delimiter=",")
- else:
- # Test by straight_line. To run, the following create_straight_line_test_csv() must be executed
- trajectory_data = np.loadtxt("straight_line.csv", delimiter=",")
+ t_eval = np.arange(t_range[0], t_range[1], sim_dt)
+ trajectory_data = np.loadtxt("slalom_course_data.csv", delimiter=",")
trajectory_interpolator_list = [
scipy.interpolate.interp1d(trajectory_data[:, 0], trajectory_data[:, 1 + i])
for i in range(trajectory_data.shape[1] - 1)
]
- x_init = trajectory_data[0, 1:7] + initial_error
-
- x_current = x_init.copy()
+ x_init = trajectory_data[0, 1:7].copy()
acc_des_queue = [0] * acc_delay_step_sim
- steer_des_queue = [0] * steer_delay_step_sim
+
+ initial_steer_input = (
+ measurement_steer_bias + np.sign(measurement_steer_bias) * steer_dead_band
+ ) / steer_scaling
+ steer_des_queue = [initial_steer_input] * steer_delay_step_sim
calculated = 0
break_flag = False
tracking_error_list = []
+ target_vel_list = []
total_abs_max_lateral_deviation = -1
straight_line_abs_max_lateral_deviation = -1
@@ -872,48 +674,80 @@ def slalom_drive(
total_abs_max_acc_error = -1
total_abs_max_steer_error = -1
prev_u_actual_input = np.zeros(2)
- log_updater = driving_log_updater()
+ log_updater = data_collection_utils.driving_log_updater()
+ pp_gain_updater = pure_pursuit_gain_updater.pure_pursuit_gain_updater()
+ acc_gain_scaling = 1.0
+ steer_gain_scaling = 1.0 # L/2.79
+
+ acc_gain_scaling_decay = 0.9
+ steer_gain_scaling_decay = 0.9
+
+ # used for "pp_eight"
previous_pp_index = 0
- if control_type != "mpc": # feedforward_test:
+ if control_type != ControlType.mpc: # feedforward_test:
(
t_acc_array,
- width_acc_array,
+ amp_acc_array,
t_steer_array,
- width_steer_array,
+ amp_steer_array,
t_large_steer_array,
- width_large_steer_array,
- ) = create_additional_sine_data(
+ amp_large_steer_array,
+ ) = data_collection_utils.create_additional_sine_data(
seed,
t_range,
- acc_width_range,
+ acc_amp_range,
acc_period_range,
- steer_width_range,
+ steer_amp_range,
steer_period_range,
- large_steer_width_range,
+ large_steer_amp_range,
large_steer_period_range,
start_large_steer_time,
)
- if control_type == "pp":
- t_figure_eight = np.arange(*[0, 4 * np.pi * circle_radius], 0.01)
- trajectory_position_data, trajectory_yaw_data = get_figure_eight_point(
- t_figure_eight, circle_radius
- )
- # plt.plot(trajectory_position_data[:,0],trajectory_position_data[:,1])
- # plt.show()
- t_vel_array, width_vel_array = create_vel_sine_data(
- seed, t_range, vel_width_range, vel_period_range
- )
+ if control_type in [ControlType.pp_eight, ControlType.pp_straight, ControlType.npp_eight]:
+ if control_type in [ControlType.pp_eight, ControlType.npp_eight]:
+ figure_eight = data_collection_utils.FigureEight(
+ y_length,
+ x_length,
+ acc_max=acc_max,
+ constant_vel_time=constant_vel_time,
+ split_size=split_size,
+ smoothing_trajectory_data_flag=smoothing_trajectory_data_flag,
+ )
+ (
+ trajectory_position_data,
+ trajectory_yaw_data,
+ curvature_radius,
+ parts,
+ achievement_rates,
+ ) = figure_eight.get_trajectory_points(0.01)
+
+ x_init[:2] = trajectory_position_data[0]
+ x_init[3] = trajectory_yaw_data[0]
+ x_init[2] = figure_eight.v_start
+ else:
+ straight_line = data_collection_utils.StraightLine(
+ acc_max=acc_max, constant_vel_time=constant_vel_time, split_size=split_size
+ )
+ x_init[2] = straight_line.v_mid
+
+ x_init += initial_error
+
+ x_current = x_init.copy()
for i in range(t_eval.size):
if i % ctrl_freq == 0: # update u_opt
- if control_type == "mpc": # not feedforward_test:
+ if control_type in [ControlType.pp_eight, ControlType.pp_straight]:
+ pp_gain_updater.state_queue_updater(
+ x_current[2], x_current[3], x_current[4], x_current[5]
+ )
+
+ if control_type == ControlType.mpc: # not feedforward_test:
X_des, U_des, tracking_error, break_flag = get_mpc_trajectory(
x_current, trajectory_data, trajectory_interpolator_list
)
tracking_error_list.append(tracking_error.copy())
-
u_opt = controller.update_input_queue_and_get_optimal_control(
log_updater.control_cmd_time_stamp_list,
log_updater.control_cmd_acc_list,
@@ -921,38 +755,177 @@ def slalom_drive(
x_current,
X_des,
U_des,
+ t_eval[i],
+ t_eval[i],
)
- elif control_type == "ff":
- u_opt = get_current_additional_sine(
+ if t_eval[i] < 5.0:
+ u_opt[1] = initial_steer_input
+ elif control_type == ControlType.ff:
+ u_opt = data_collection_utils.get_current_additional_sine(
t_eval[i],
t_acc_array,
- width_acc_array,
+ amp_acc_array,
t_steer_array,
- width_steer_array,
+ amp_steer_array,
t_large_steer_array,
- width_large_steer_array,
+ amp_large_steer_array,
)
- u_opt[0] += get_feedforward_nominal_input(t_eval[i], trajectory_data)[0]
- elif control_type == "pp":
- target_position, target_yaw, previous_pp_index = get_pure_pursuit_info(
+ u_opt[0] += data_collection_utils.get_feedforward_nominal_input(
+ t_eval[i], trajectory_data
+ )[0]
+
+ elif control_type == ControlType.pp_eight:
+ (
+ target_position,
+ target_yaw,
+ previous_pp_index,
+ ) = data_collection_utils.get_pure_pursuit_info(
x_current, trajectory_position_data, trajectory_yaw_data, previous_pp_index
)
- target_vel = get_current_vel_sine(t_eval[i], t_vel_array, width_vel_array, v_mid)
- u_opt = pure_pursuit(x_current, target_position, target_vel, target_yaw)
- u_opt += get_current_additional_sine(
+ target_vel = figure_eight.get_current_velocity(t_eval[i])
+ target_vel_list.append(np.array([t_eval[i], target_vel]))
+ u_opt = drive_functions.pure_pursuit_control(
+ pos_xy_obs=x_current[:2],
+ pos_yaw_obs=x_current[3],
+ longitudinal_vel_obs=x_current[2],
+ pos_xy_ref=target_position[:2],
+ pos_yaw_ref=target_yaw,
+ longitudinal_vel_ref=target_vel,
+ acc_gain_scaling=acc_gain_scaling,
+ steer_gain_scaling=steer_gain_scaling,
+ )
+ u_opt += data_collection_utils.get_current_additional_sine(
+ t_eval[i],
+ t_acc_array,
+ amp_acc_array,
+ t_steer_array,
+ amp_steer_array,
+ t_large_steer_array,
+ amp_large_steer_array,
+ )
+ u_opt[1] += data_collection_utils.step_response(
+ t_eval[i],
+ step_response_start_time_ratio * t_range[1],
+ step_response_interval,
+ step_response_max_input,
+ step_response_max_length,
+ step_response_min_length,
+ )
+ break_flag = figure_eight.break_flag
+ elif control_type == ControlType.pp_straight:
+ target_vel = straight_line.get_current_velocity(t_eval[i])
+ target_vel_list.append(np.array([t_eval[i], target_vel]))
+ target_position = np.array([x_current[0], 0.0])
+ target_yaw = 0.0
+ u_opt = drive_functions.pure_pursuit_control(
+ pos_xy_obs=x_current[:2],
+ pos_yaw_obs=x_current[3],
+ longitudinal_vel_obs=x_current[2],
+ pos_xy_ref=target_position[:2],
+ pos_yaw_ref=target_yaw,
+ longitudinal_vel_ref=target_vel,
+ acc_gain_scaling=acc_gain_scaling,
+ steer_gain_scaling=steer_gain_scaling,
+ )
+ u_opt += data_collection_utils.get_current_additional_sine(
t_eval[i],
t_acc_array,
- width_acc_array,
+ amp_acc_array,
t_steer_array,
- width_steer_array,
+ amp_steer_array,
t_large_steer_array,
- width_large_steer_array,
+ amp_large_steer_array,
+ )
+ u_opt[1] += data_collection_utils.step_response(
+ t_eval[i],
+ step_response_start_time_ratio * t_range[1],
+ step_response_interval,
+ step_response_max_input,
+ step_response_max_length,
+ step_response_min_length,
)
+ break_flag = straight_line.break_flag
+
+ elif control_type == ControlType.npp_eight:
+ (
+ target_position,
+ target_yaw,
+ previous_pp_index,
+ target_position_ahead,
+ ) = data_collection_utils.get_naive_pure_pursuit_info(
+ x_current, trajectory_position_data, trajectory_yaw_data, previous_pp_index
+ )
+ target_vel = figure_eight.get_current_velocity(t_eval[i])
+
+ applying_velocity_limit_by_lateral_acc_flag = True
+ if applying_velocity_limit_by_lateral_acc_flag:
+ max_lateral_accel = 0.3
+ velocity_limit_by_lateral_acc = np.sqrt(
+ max_lateral_accel * curvature_radius[previous_pp_index]
+ )
+ target_vel = min(target_vel, velocity_limit_by_lateral_acc)
+
+ applying_velocity_limit_by_tracking_error_flag = True
+ if applying_velocity_limit_by_tracking_error_flag:
+ lateral_error = np.sqrt(((x_current[:2] - target_position[:2]) ** 2).sum())
+ yaw_error = x_current[3] - target_yaw
+ if yaw_error > np.pi:
+ yaw_error -= 2 * np.pi
+ if yaw_error < -np.pi:
+ yaw_error += 2 * np.pi
+
+ safety_velocity = 3.0
+ lateral_error_threshold = 2.0
+ yaw_error_threshold = 0.5
+ if lateral_error_threshold < lateral_error or yaw_error_threshold < np.abs(
+ yaw_error
+ ):
+ target_vel = safety_velocity
+
+ target_vel_list.append(np.array([t_eval[i], target_vel]))
+ u_opt = drive_functions.naive_pure_pursuit_control(
+ pos_xy_obs=x_current[:2],
+ pos_yaw_obs=x_current[3],
+ longitudinal_vel_obs=x_current[2],
+ pos_xy_ref_target=target_position_ahead[:2],
+ longitudinal_vel_ref_nearest=target_vel,
+ )
+ u_opt += data_collection_utils.get_current_additional_sine(
+ t_eval[i],
+ t_acc_array,
+ amp_acc_array,
+ t_steer_array,
+ amp_steer_array,
+ t_large_steer_array,
+ amp_large_steer_array,
+ )
+ u_opt[1] += data_collection_utils.step_response(
+ t_eval[i],
+ step_response_start_time_ratio * t_range[1],
+ step_response_interval,
+ step_response_max_input,
+ step_response_max_length,
+ step_response_min_length,
+ )
+ break_flag = figure_eight.break_flag
calculated += 1
log_updater.update(t_eval[i], x_current, u_opt)
- if (
- visualize and (control_type in ["ff", "pp"]) and (i == t_eval.size - 1 or break_flag)
- ): # feedforward_test and (i == t_eval.size - 1 or break_flag):
+ if control_type in [ControlType.pp_eight, ControlType.pp_straight]:
+ pp_gain_updater.input_queue_updater(u_opt[0], u_opt[1])
+ if i % (100 * ctrl_freq) == 0 and UPDATE_PP_GAIN: # update u_opt
+ # pp_gain_updater.get_acc_gain_scaling()
+ acc_gain_scaling = (
+ acc_gain_scaling_decay * acc_gain_scaling
+ + (1 - acc_gain_scaling_decay) * pp_gain_updater.get_acc_gain_scaling()
+ )
+ if control_type == ControlType.pp_eight:
+ steer_gain_scaling = (
+ steer_gain_scaling_decay * steer_gain_scaling
+ + (1 - steer_gain_scaling_decay)
+ * pp_gain_updater.get_steer_gain_scaling()
+ )
+
+ if visualize and (control_type != ControlType.mpc) and (i == t_eval.size - 1 or break_flag):
X = np.array(log_updater.X_history)
U = np.array(log_updater.U_history)
@@ -967,11 +940,11 @@ def slalom_drive(
+ ", nominal_model: "
+ str(nominal_setting_dict_display)
)
- fig = plt.figure(figsize=(18, 12), tight_layout=True)
+ # cSpell:ignore nrows ncols suptitle
+ fig, axes = plt.subplots(nrows=3, ncols=3, figsize=(18, 12), tight_layout=True)
fig.suptitle(title_str)
- plt.subplot(231)
- ax1 = plt.subplot(2, 3, 1)
+ ax1: plt.Axes = axes[0, 0]
ax1.plot(X[:, 0], X[:, 1], label="trajectory")
ax1.legend()
ax1.set_xlabel("x_position [m]")
@@ -980,13 +953,13 @@ def slalom_drive(
time_normalize_1 = ctrl_freq * sim_dt
f_s = int(1.0 / time_normalize_1)
- ax2 = plt.subplot(2, 3, 2)
+ # cSpell:ignore numba simplejson fftfreq basefmt markerfmt
+ ax2: plt.Axes = axes[0, 1]
X_acc = np.fft.fft(U[:, 0]) / len(U[:, 0]) # Fourier transform of waveforms
freqs = np.fft.fftfreq(len(U[:, 0])) * f_s
ax2.stem(
freqs,
np.abs(X_acc),
- use_line_collection=True,
basefmt="k-",
markerfmt="cx",
label="acc input",
@@ -996,13 +969,12 @@ def slalom_drive(
ax2.set_xlabel("freq")
ax2.set_ylabel("amplitude")
- ax3 = plt.subplot(2, 3, 3)
+ ax3: plt.Axes = axes[0, 2]
X_steer = np.fft.fft(U[:, 1]) / len(U[:, 1]) # Fourier transform of waveforms
freqs = np.fft.fftfreq(len(U[:, 1])) * f_s
ax3.stem(
freqs,
np.abs(X_steer),
- use_line_collection=True,
basefmt="k-",
markerfmt="cx",
label="steer input",
@@ -1012,27 +984,55 @@ def slalom_drive(
ax3.set_xlabel("freq")
ax3.set_ylabel("amplitude")
- ax5 = plt.subplot(2, 3, 5)
+ if control_type in [
+ ControlType.pp_eight,
+ ControlType.pp_straight,
+ ControlType.npp_eight,
+ ]:
+ ax4: plt.Axes = axes[1, 0]
+ ax4.plot(
+ np.array(target_vel_list)[:, 0],
+ np.array(target_vel_list)[:, 1],
+ label="vel target",
+ )
+ ax4.plot(time_normalize_1 * np.arange(X.shape[0]), X[:, 2], label="vel")
+ ax4.legend()
+
+ ax5: plt.Axes = axes[1, 1]
ax5.plot(time_normalize_1 * np.arange(U.shape[0]), U[:, 0], label="acc input")
ax5.legend()
- ax6 = plt.subplot(2, 3, 6)
+ ax6: plt.Axes = axes[1, 2]
ax6.plot(time_normalize_1 * np.arange(U.shape[0]), U[:, 1], label="steer input")
ax6.legend()
+ kinematic_states = KinematicStates(
+ speed=X[:, 2],
+ acc=X[:, 4],
+ steer=X[:, 5],
+ )
+
+ ax7: plt.Axes = axes[2, 0]
+ fig, ax7 = visualize_speed_acc(fig=fig, ax=ax7, kinematic_states=kinematic_states)
+ ax7.plot()
+
+ ax8: plt.Axes = axes[2, 1]
+ fig, ax8 = visualize_speed_steer(fig=fig, ax=ax8, kinematic_states=kinematic_states)
+ ax8.plot()
+
+ ax9: plt.Axes = axes[2, 2]
+ ax9.axis("off")
+
if save_file:
png_save_dir = save_dir_
else:
png_save_dir = "."
- if control_type == "ff":
- plt.savefig(png_save_dir + "/python_simulator_feedforward_drive.png")
- elif control_type == "pp":
- plt.savefig(png_save_dir + "/python_simulator_pure_pursuit_drive.png")
+ plt.savefig(f"{png_save_dir}/python_simulator_{control_type.value}_drive.png")
plt.close()
if (
visualize
- and (control_type == "mpc")
+ and (control_type == ControlType.mpc)
and (i % 1000 * ctrl_freq == 999 * ctrl_freq or i == t_eval.size - 1 or break_flag)
):
fig = plt.figure(figsize=(24, 15), tight_layout=True)
@@ -1049,7 +1049,7 @@ def slalom_drive(
)
fig.suptitle(title_str)
- plt.subplot(331)
+ plt.subplot(3, 3, 1)
ax1 = plt.subplot(3, 3, 1)
X = np.array(log_updater.X_history)
@@ -1064,7 +1064,7 @@ def slalom_drive(
c="orange",
label="reference_trajectory",
)
- ax1.plot(X[:, 0], X[:, 1], label="trajectory")
+ ax1.plot(X[:, 0], X[:, 1], label="trajectory", color="tab:blue")
if mode != "mppi":
ax1.scatter(
controller.nominal_traj_ilqr[:, 0],
@@ -1112,51 +1112,15 @@ def slalom_drive(
lateral_deviation,
label="lateral_deviation",
)
- ax2.plot(
- time_normalize_1 * np.arange(tracking_error_array.shape[0]),
- np.zeros(tracking_error_array.shape[0]),
- linestyle="dashed",
- )
- ax2.plot(
- time_normalize_1 * np.arange(tracking_error_array.shape[0]),
- 0.05 * np.ones(tracking_error_array.shape[0]),
- linestyle="dashed",
- )
- ax2.plot(
- time_normalize_1 * np.arange(tracking_error_array.shape[0]),
- -0.05 * np.ones(tracking_error_array.shape[0]),
- linestyle="dashed",
- )
- ax2.plot(
- time_normalize_1 * np.arange(tracking_error_array.shape[0]),
- 0.1 * np.ones(tracking_error_array.shape[0]),
- linestyle="dashed",
- )
- ax2.plot(
- time_normalize_1 * np.arange(tracking_error_array.shape[0]),
- -0.1 * np.ones(tracking_error_array.shape[0]),
- linestyle="dashed",
- )
- ax2.plot(
- time_normalize_1 * np.arange(tracking_error_array.shape[0]),
- 0.15 * np.ones(tracking_error_array.shape[0]),
- linestyle="dashed",
- )
- ax2.plot(
- time_normalize_1 * np.arange(tracking_error_array.shape[0]),
- -0.15 * np.ones(tracking_error_array.shape[0]),
- linestyle="dashed",
- )
- ax2.plot(
- time_normalize_1 * np.arange(tracking_error_array.shape[0]),
- 0.2 * np.ones(tracking_error_array.shape[0]),
- linestyle="dashed",
- )
- ax2.plot(
- time_normalize_1 * np.arange(tracking_error_array.shape[0]),
- -0.2 * np.ones(tracking_error_array.shape[0]),
- linestyle="dashed",
- )
+
+ ax2_coef = [0.00, 0.05, -0.05, 0.10, -0.10, 0.15, -0.15, 0.20, -0.20]
+ for coe in ax2_coef:
+ ax2.plot(
+ time_normalize_1 * np.arange(tracking_error_array.shape[0]),
+ coe * np.ones(tracking_error_array.shape[0]),
+ linestyle="dashed",
+ )
+
ax2.set_xlabel("Time [s]")
ax2.set_ylabel("Lateral deviation [m]")
ax2.legend()
@@ -1177,14 +1141,23 @@ def slalom_drive(
)
ax3 = plt.subplot(3, 3, 4)
- ax3.plot(time_normalize_1 * np.arange(X.shape[0]), X[:, 2], label="velocity")
ax3.plot(
- time_normalize_1 * np.arange(X.shape[0]), X_des_hist[:, 2], label="velocity_target"
+ time_normalize_1 * np.arange(X.shape[0]),
+ X[:, 2],
+ label="velocity",
+ color="tab:blue",
+ )
+ ax3.plot(
+ time_normalize_1 * np.arange(X.shape[0]),
+ X_des_hist[:, 2],
+ label="velocity_target",
+ color="orange",
)
ax3.plot(
time_normalize_1 * np.arange(tracking_error_array.shape[0]),
-tracking_error_array[:, 2],
label="velocity_error",
+ color="lightgrey",
)
if mode != "mppi":
ax3.scatter(
@@ -1193,6 +1166,7 @@ def slalom_drive(
true_prediction[:, 2],
s=4,
label="velocity_true_prediction",
+ c="green",
)
ax3.scatter(
time_normalize_1 * (X.shape[0] - 1)
@@ -1200,11 +1174,13 @@ def slalom_drive(
controller.nominal_traj_ilqr[:, 2],
s=4,
label="velocity_prediction_ilqr",
+ c="red",
)
ax3.plot(
time_normalize_1 * np.arange(tracking_error_array.shape[0]),
np.zeros(tracking_error_array.shape[0]),
linestyle="dashed",
+ color="darkred",
)
ax3.set_xlabel("Time [s]")
ax3.set_ylabel("velocity [m/s]")
@@ -1222,12 +1198,20 @@ def slalom_drive(
)
ax4 = plt.subplot(3, 3, 5)
- ax4.plot(time_normalize_1 * np.arange(X.shape[0]), X[:, 3], label="yaw")
- ax4.plot(time_normalize_1 * np.arange(X.shape[0]), X_des_hist[:, 3], label="yaw_target")
+ ax4.plot(
+ time_normalize_1 * np.arange(X.shape[0]), X[:, 3], label="yaw", color="tab:blue"
+ )
+ ax4.plot(
+ time_normalize_1 * np.arange(X.shape[0]),
+ X_des_hist[:, 3],
+ label="yaw_target",
+ color="orange",
+ )
ax4.plot(
time_normalize_1 * np.arange(tracking_error_array.shape[0]),
-tracking_error_array[:, 3],
label="yaw_error",
+ color="lightgrey",
)
if mode != "mppi":
ax4.scatter(
@@ -1236,6 +1220,7 @@ def slalom_drive(
true_prediction[:, 3],
s=4,
label="yaw_true_prediction",
+ c="green",
)
ax4.scatter(
time_normalize_1 * (X.shape[0] - 1)
@@ -1243,11 +1228,13 @@ def slalom_drive(
controller.nominal_traj_ilqr[:, 3],
s=4,
label="yaw_prediction_ilqr",
+ c="red",
)
ax4.plot(
time_normalize_1 * np.arange(tracking_error_array.shape[0]),
np.zeros(tracking_error_array.shape[0]),
linestyle="dashed",
+ c="darkred",
)
ax4.set_xlabel("Time [s]")
ax4.set_ylabel("yaw [rad]")
@@ -1265,13 +1252,23 @@ def slalom_drive(
)
ax5 = plt.subplot(3, 3, 7)
- ax5.plot(time_normalize_1 * np.arange(X.shape[0]), X[:, 4], label="acc")
- ax5.plot(time_normalize_1 * np.arange(X.shape[0]), U[:, 0], label="acc_input")
- ax5.plot(time_normalize_1 * np.arange(X.shape[0]), X_des_hist[:, 4], label="acc_target")
+ ax5.plot(
+ time_normalize_1 * np.arange(X.shape[0]), X[:, 4], label="acc", color="tab:blue"
+ )
+ ax5.plot(
+ time_normalize_1 * np.arange(X.shape[0]), U[:, 0], label="acc_input", color="violet"
+ )
+ ax5.plot(
+ time_normalize_1 * np.arange(X.shape[0]),
+ X_des_hist[:, 4],
+ label="acc_target",
+ color="orange",
+ )
ax5.plot(
time_normalize_1 * np.arange(tracking_error_array.shape[0]),
-tracking_error_array[:, 4],
label="acc_error",
+ color="lightgrey",
)
if mode != "mppi":
ax5.scatter(
@@ -1280,6 +1277,7 @@ def slalom_drive(
true_prediction[:, 4],
s=4,
label="acc_true_prediction",
+ c="green",
)
ax5.scatter(
time_normalize_1 * (X.shape[0] - 1)
@@ -1287,6 +1285,7 @@ def slalom_drive(
controller.nominal_traj_ilqr[:, 4],
s=4,
label="acc_prediction_ilqr",
+ c="red",
)
ax5.scatter(
time_normalize_1 * (X.shape[0] - 1)
@@ -1294,11 +1293,13 @@ def slalom_drive(
nominal_inputs[:, 0],
s=4,
label="acc_input_schedule",
+ c="plum",
)
ax5.plot(
time_normalize_1 * np.arange(tracking_error_array.shape[0]),
np.zeros(tracking_error_array.shape[0]),
linestyle="dashed",
+ color="darkred",
)
ax5.set_xlabel("Time [s]")
ax5.set_ylabel("acc [m/s^2]")
@@ -1316,15 +1317,26 @@ def slalom_drive(
)
ax6 = plt.subplot(3, 3, 8)
- ax6.plot(time_normalize_1 * np.arange(X.shape[0]), X[:, 5], label="steer")
- ax6.plot(time_normalize_1 * np.arange(X.shape[0]), U[:, 1], label="steer_input")
ax6.plot(
- time_normalize_1 * np.arange(X.shape[0]), X_des_hist[:, 7], label="steer_mpc_target"
+ time_normalize_1 * np.arange(X.shape[0]), X[:, 5], label="steer", color="tab:blue"
+ )
+ ax6.plot(
+ time_normalize_1 * np.arange(X.shape[0]),
+ U[:, 1],
+ label="steer_input",
+ color="violet",
+ )
+ ax6.plot(
+ time_normalize_1 * np.arange(X.shape[0]),
+ X_des_hist[:, 7],
+ label="steer_mpc_target",
+ color="orange",
)
ax6.plot(
time_normalize_1 * np.arange(tracking_error_array.shape[0]),
-tracking_error_array[:, 5],
label="steer_error",
+ color="lightgrey",
)
if mode != "mppi":
ax6.scatter(
@@ -1333,6 +1345,7 @@ def slalom_drive(
true_prediction[:, 5],
s=4,
label="steer_true_prediction",
+ c="green",
)
ax6.scatter(
time_normalize_1 * (X.shape[0] - 1)
@@ -1340,6 +1353,7 @@ def slalom_drive(
controller.nominal_traj_ilqr[:, 5],
s=4,
label="steer_prediction_ilqr",
+ c="red",
)
ax6.scatter(
time_normalize_1 * (X.shape[0] - 1)
@@ -1347,11 +1361,13 @@ def slalom_drive(
nominal_inputs[:, 1],
s=4,
label="steer_input_schedule",
+ c="plum",
)
ax6.plot(
time_normalize_1 * np.arange(tracking_error_array.shape[0]),
np.zeros(tracking_error_array.shape[0]),
linestyle="dashed",
+ color="darkred",
)
ax6.set_xlabel("Time [s]")
ax6.set_ylabel("steer [rad]")
@@ -1377,7 +1393,6 @@ def slalom_drive(
ax7.stem(
freqs,
np.abs(X_steer) / len(steer_state),
- use_line_collection=True,
basefmt="k-",
markerfmt="rx",
label="steer_state",
@@ -1397,7 +1412,6 @@ def slalom_drive(
ax7.stem(
freqs,
np.abs(X_lateral_acc) / len(lateral_acc),
- use_line_collection=True,
basefmt="k-",
markerfmt="gx",
label="lateral_acc",
@@ -1408,7 +1422,6 @@ def slalom_drive(
ax7.stem(
freqs,
np.abs(X_lateral_jerk) / len(lateral_jerk),
- use_line_collection=True,
basefmt="k-",
markerfmt="bx",
label="lateral_jerk",
@@ -1467,13 +1480,14 @@ def slalom_drive(
x_current = x_next.copy()
prev_u_actual_input = u_actual_input.copy()
if break_flag:
- controller.send_initialize_input_queue()
- controller.stop_model_update()
+ if control_type == ControlType.mpc:
+ controller.send_initialize_input_queue()
+ controller.stop_model_update()
break
if save_file:
log_updater.save(save_dir_)
- if control_type == "mpc" and perturbed_sim_flag:
+ if control_type == ControlType.mpc and perturbed_sim_flag:
auto_test_performance_result_dict = {
"total_abs_max_lateral_deviation": total_abs_max_lateral_deviation,
"straight_line_abs_max_lateral_deviation": straight_line_abs_max_lateral_deviation,
@@ -1505,89 +1519,3 @@ def slalom_drive(
print("how_many_time_controlled", calculated)
return auto_test_performance_result_list
-
-
-def create_straight_line_test_csv(
- jerk=0.3, starting_vel=5.0, interval_1=2.0, interval_2=3.0, interval_3=10.0
-):
- """Generate data for a straight line driving test."""
- t_1 = 10.0
- t_2 = t_1 + interval_1 # Increasing acceleration.
- t_3 = t_2 + interval_2 # Constant acceleration
- t_4 = t_3 + interval_1 # Decreasing acceleration.
- t_5 = t_4 + interval_3 # acceleration = 0
- t_6 = t_5 + interval_1 # Decreasing acceleration.
- t_7 = t_6 + interval_2 # Constant acceleration
- t_8 = t_7 + interval_1 # Decreasing acceleration.
- vel_t1 = starting_vel
- x_t1 = starting_vel * t_1
- vel_t2 = vel_t1 + 0.5 * jerk * interval_1 * interval_1
- x_t2 = x_t1 + vel_t1 * interval_1 + 0.5 * jerk * interval_1 * interval_1 * interval_1 / 3
- vel_t3 = vel_t2 + jerk * interval_1 * interval_2
- x_t3 = x_t2 + vel_t2 * interval_2 + 0.5 * jerk * interval_1 * interval_2 * interval_2
- vel_t4 = vel_t3 + 0.5 * jerk * interval_1 * interval_1
- x_t4 = x_t3 + vel_t4 * interval_1 - 0.5 * jerk * interval_1 * interval_1 * interval_1 / 3
- vel_t5 = vel_t4
- x_t5 = x_t4 + vel_t4 * interval_3
- vel_t6 = vel_t5 - 0.5 * jerk * interval_1 * interval_1
- x_t6 = x_t5 + vel_t5 * interval_1 - 0.5 * jerk * interval_1 * interval_1 * interval_1 / 3
- vel_t7 = vel_t6 - jerk * interval_1 * interval_2
- x_t7 = x_t6 + vel_t6 * interval_2 - 0.5 * jerk * interval_1 * interval_2 * interval_2
- vel_t8 = vel_t7 - 0.5 * jerk * interval_1 * interval_1
- x_t8 = x_t7 + vel_t8 * interval_1 + 0.5 * jerk * interval_1 * interval_1 * interval_1 / 3
-
- def calc_longitudinal_state(t):
- if t < t_1:
- return starting_vel * t, starting_vel, 0.0
- elif t < t_2:
- return (
- x_t1 + vel_t1 * (t - t_1) + 0.5 * jerk * (t - t_1) * (t - t_1) * (t - t_1) / 3,
- vel_t1 + 0.5 * jerk * (t - t_1) * (t - t_1),
- jerk * (t - t_1),
- )
- elif t < t_3:
- return (
- x_t2 + vel_t2 * (t - t_2) + 0.5 * jerk * (t_2 - t_1) * (t - t_2) * (t - t_2),
- vel_t2 + jerk * (t_2 - t_1) * (t - t_2),
- jerk * (t_2 - t_1),
- )
- elif t < t_4:
- return (
- x_t4 - vel_t4 * (t_4 - t) + 0.5 * jerk * (t_4 - t) * (t_4 - t) * (t_4 - t) / 3,
- vel_t4 - 0.5 * jerk * (t_4 - t) * (t_4 - t),
- jerk * (t_4 - t),
- )
- elif t < t_5:
- return x_t4 + vel_t4 * (t - t_4), vel_t4, 0.0
- elif t < t_6:
- return (
- x_t5 + vel_t5 * (t - t_5) - 0.5 * jerk * (t - t_5) * (t - t_5) * (t - t_5) / 3,
- vel_t5 - 0.5 * jerk * (t - t_5) * (t - t_5),
- -jerk * (t - t_5),
- )
- elif t < t_7:
- return (
- x_t6 + vel_t6 * (t - t_6) - 0.5 * jerk * (t_6 - t_5) * (t - t_6) * (t - t_6),
- vel_t6 - jerk * (t_6 - t_5) * (t - t_6),
- -jerk * (t_6 - t_5),
- )
- elif t < t_8:
- return (
- x_t8 - vel_t8 * (t_8 - t) - 0.5 * jerk * (t_8 - t) * (t_8 - t) * (t_8 - t) / 3,
- vel_t8 + 0.5 * jerk * (t_8 - t) * (t_8 - t),
- -jerk * (t_8 - t),
- )
- else:
- return x_t8 + vel_t8 * (t - t_8), vel_t8, 0.0
-
- t_range = [0, 100]
- time_stamps = np.arange(*t_range, 0.001)
- trajectory_data = np.zeros((time_stamps.shape[0], 9))
- trajectory_data[:, 0] = time_stamps
- for i in range(time_stamps.shape[0]):
- (
- trajectory_data[i, 1],
- trajectory_data[i, 3],
- trajectory_data[i, 5],
- ) = calc_longitudinal_state(time_stamps[i])
- np.savetxt("straight_line.csv", trajectory_data, delimiter=",")
diff --git a/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/run_auto_test.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/run_auto_test.py
new file mode 100644
index 0000000000000..86ed6470dc14d
--- /dev/null
+++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/run_auto_test.py
@@ -0,0 +1,33 @@
+# Copyright 2024 Proxima Technology Inc, TIER IV
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import subprocess
+
+if __name__ == "__main__":
+ params = [
+ "steer_bias",
+ "steer_dead_band",
+ "wheel_base",
+ "acc_time_delay",
+ "steer_time_delay",
+ "acc_time_constant",
+ "steer_time_constant",
+ "accel_map_scale",
+ "acc_scaling",
+ "steer_scaling",
+ "vehicle_type",
+ ]
+ for param in params:
+ str_run = "python3 run_sim.py " + f" --param_name {param}" + " --root auto_test"
+ subprocess.run(str_run, shell=True)
diff --git a/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/run_figure_eight_param_search.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/run_figure_eight_param_search.py
new file mode 100644
index 0000000000000..9e1ee6f53e16f
--- /dev/null
+++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/run_figure_eight_param_search.py
@@ -0,0 +1,63 @@
+# Copyright 2024 Proxima Technology Inc, TIER IV
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import subprocess
+
+import numpy as np
+
+
+def print_progress(msg: str):
+ print("______________________________")
+ print("start running:", msg)
+ print("______________________________")
+
+
+SEED = 44
+if __name__ == "__main__":
+ for k in range(100):
+ np.random.seed(seed=k + SEED)
+ acc_amp_range = np.random.uniform(0.005, 0.1)
+ acc_period_min = np.random.uniform(2.0, 10.0)
+ acc_period_max = acc_period_min + np.random.uniform(5.0, 30.0)
+ steer_amp_range = np.random.uniform(0.005, 0.1)
+ steer_period_min = np.random.uniform(2.0, 10.0)
+ steer_period_max = steer_period_min + np.random.uniform(5.0, 30.0)
+ step_response_max_input = np.random.uniform(0.001, 0.1)
+ step_response_min_length = np.random.uniform(0.02, 1.0)
+ step_response_max_length = step_response_min_length + np.random.uniform(0.03, 2.0)
+ step_response_interval = step_response_max_length + np.random.uniform(1.0, 20.0)
+ constant_vel_time = np.random.uniform(0.5, 20.0)
+
+ dir_name = "test_param_search_" + str(k) + "_test_vehicle"
+
+ for memory in range(2):
+ use_memory_diff_flag = " " + ("--use_memory_diff" if memory == 1 else "")
+ str_run = (
+ "python3 test_figure_eight_param.py "
+ + f" --acc_amp_range {acc_amp_range}"
+ + f" --acc_period_min {acc_period_min}"
+ + f" --acc_period_max {acc_period_max}"
+ + f" --steer_amp_range {steer_amp_range}"
+ + f" --steer_period_min {steer_period_min}"
+ + f" --steer_period_max {steer_period_max}"
+ + f" --step_response_max_input {step_response_max_input}"
+ + f" --step_response_min_length {step_response_min_length}"
+ + f" --step_response_max_length {step_response_max_length}"
+ + f" --step_response_interval {step_response_interval}"
+ + f" --constant_vel_time {constant_vel_time}"
+ + f" --dir_name {dir_name}"
+ + use_memory_diff_flag
+ )
+ print_progress(str_run)
+ subprocess.run(str_run, shell=True)
diff --git a/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/run_python_simulator.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/run_python_simulator.py
index 68d8a9f64e73f..739d8ea70a656 100644
--- a/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/run_python_simulator.py
+++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/run_python_simulator.py
@@ -12,6 +12,9 @@
# See the License for the specific language governing permissions and
# limitations under the License.
+import sys
+
+from assets import ControlType
from autoware_smart_mpc_trajectory_follower.training_and_data_check import train_drive_NN_model
import numpy as np
import python_simulator
@@ -20,17 +23,40 @@
[0.001, 0.03, 0.01, -0.001, 0, 2 * python_simulator.measurement_steer_bias]
)
-save_dir = "test_python_sim"
-python_simulator.slalom_drive(save_dir=save_dir, initial_error=initial_error)
-model_trainer = train_drive_NN_model.train_drive_NN_model()
-model_trainer.add_data_from_csv(save_dir)
-model_trainer.save_train_data(save_dir)
-model_trainer.get_trained_model()
-model_trainer.save_models(save_dir=save_dir)
+if len(sys.argv) > 1:
+ if sys.argv[1] == "nominal_test":
+ save_dir = "test_python_nominal_sim"
+ python_simulator.drive_sim(save_dir=save_dir, initial_error=initial_error)
+ else:
+ print("Invalid argument")
+ sys.exit(1)
+else:
+ train_dir = "test_python_pure_pursuit_train"
+ val_dir = "test_python_pure_pursuit_val"
-save_dir = "test_python_trained_sim"
-load_dir = "test_python_sim"
-python_simulator.slalom_drive(
- save_dir=save_dir, load_dir=load_dir, use_trained_model=True, initial_error=initial_error
-)
+ python_simulator.drive_sim(
+ seed=0,
+ t_range=[0, 900.0],
+ control_type=ControlType.pp_eight,
+ save_dir=train_dir,
+ initial_error=initial_error,
+ )
+ python_simulator.drive_sim(
+ seed=1,
+ t_range=[0, 900.0],
+ control_type=ControlType.pp_eight,
+ save_dir=val_dir,
+ initial_error=initial_error,
+ )
+
+ model_trainer = train_drive_NN_model.train_drive_NN_model()
+ model_trainer.add_data_from_csv(train_dir, add_mode="as_train")
+ model_trainer.add_data_from_csv(val_dir, add_mode="as_val")
+ model_trainer.get_trained_model(use_polynomial_reg=True)
+ model_trainer.save_models(save_dir=train_dir)
+
+ save_dir = "test_python_trained_sim"
+ python_simulator.drive_sim(
+ save_dir=save_dir, load_dir=train_dir, use_trained_model=True, initial_error=initial_error
+ )
diff --git a/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/run_sim.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/run_sim.py
index d2bb4ea26f0a6..f6e7337997edf 100644
--- a/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/run_sim.py
+++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/run_sim.py
@@ -11,11 +11,8 @@
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
-
-# cspell: ignore oneline
-
import argparse
-from enum import Enum
+import datetime
from importlib import reload as ir
import json
import os
@@ -23,19 +20,17 @@
import traceback
from typing import Dict
+from assets import ChangeParam # type: ignore
+from assets import ControlType
+from assets import DataCollectionMode
+from assets import DirGenerator
from autoware_smart_mpc_trajectory_follower.training_and_data_check import train_drive_NN_model
import numpy as np
-import python_simulator
-
-parser = argparse.ArgumentParser()
-parser.add_argument("--param_name", default=None)
-args = parser.parse_args()
-
-P1 = 0.8
-P2 = 1.2
+import python_simulator # type: ignore
USE_TRAINED_MODEL_DIFF = True
-DATA_COLLECTION_MODE = "pp" # option: "pp": pure_pursuit, "ff": feed_forward, "mpc": smart_mpc
+DATA_COLLECTION_MODE = DataCollectionMode.pp
+CONTROL_TYPE_TO_SKIP = [ControlType.pp_straight]
USE_POLYNOMIAL_REGRESSION = True
USE_SELECTED_POLYNOMIAL = True
FORCE_NN_MODEL_TO_ZERO = False
@@ -45,93 +40,30 @@
USE_INTERCEPT = False # Should be True if FORCE_NN_MODEL_TO_ZERO is True
-class ChangeParam(Enum):
- """Parameters to be changed when running the simulation."""
-
- steer_bias = [
- -1.0 * np.pi / 180.0,
- -0.8 * np.pi / 180.0,
- -0.6 * np.pi / 180.0,
- -0.4 * np.pi / 180.0,
- -0.2 * np.pi / 180.0,
- 0.0,
- 0.2 * np.pi / 180.0,
- 0.4 * np.pi / 180.0,
- 0.6 * np.pi / 180.0,
- 0.8 * np.pi / 180.0,
- 1.0 * np.pi / 180.0,
- ]
- """steer midpoint (soft + hard)"""
-
- steer_rate_lim = [0.020, 0.050, 0.100, 0.150, 0.200, 0.300, 0.400, 0.500]
- """Maximum steer angular velocity"""
-
- vel_rate_lim = [0.5, 1.0, 3.0, 5.0, 7.0, 9.0]
- """Maximum acceleration/deceleration"""
-
- wheel_base = [0.5, 1.0, 1.5, 2.0, 3.0, 5.0, 7.0]
- """wheel base"""
-
- steer_dead_band = [0.0000, 0.0012, 0.0025, 0.0050, 0.01]
- """steer dead band"""
-
- adaptive_gear_ratio_coef = [
- [15.713, 0.053, 0.042, 15.713, 0.053, 0.042],
- [15.713, 0.053, 0.042, P1 * 15.713, 0.053, 0.042],
- [15.713, 0.053, 0.042, P2 * 15.713, 0.053, 0.042],
- [15.713, 0.053, 0.042, 15.713, P1 * 0.053, 0.042],
- [15.713, 0.053, 0.042, P1 * 15.713, P1 * 0.053, 0.042],
- [15.713, 0.053, 0.042, P2 * 15.713, P1 * 0.053, 0.042],
- [15.713, 0.053, 0.042, 15.713, P2 * 0.053, 0.042],
- [15.713, 0.053, 0.042, P1 * 15.713, P2 * 0.053, 0.042],
- [15.713, 0.053, 0.042, P2 * 15.713, P2 * 0.053, 0.042],
- [15.713, 0.053, 0.042, 15.713, 0.053, P1 * 0.042],
- [15.713, 0.053, 0.042, P1 * 15.713, 0.053, P1 * 0.042],
- [15.713, 0.053, 0.042, P2 * 15.713, 0.053, P1 * 0.042],
- [15.713, 0.053, 0.042, 15.713, P1 * 0.053, P1 * 0.042],
- [15.713, 0.053, 0.042, P1 * 15.713, P1 * 0.053, P1 * 0.042],
- [15.713, 0.053, 0.042, P2 * 15.713, P1 * 0.053, P1 * 0.042],
- [15.713, 0.053, 0.042, 15.713, P2 * 0.053, P1 * 0.042],
- [15.713, 0.053, 0.042, P1 * 15.713, P2 * 0.053, P1 * 0.042],
- [15.713, 0.053, 0.042, P2 * 15.713, P2 * 0.053, P1 * 0.042],
- [15.713, 0.053, 0.042, 15.713, 0.053, P2 * 0.042],
- [15.713, 0.053, 0.042, P1 * 15.713, 0.053, P2 * 0.042],
- [15.713, 0.053, 0.042, P2 * 15.713, 0.053, P2 * 0.042],
- [15.713, 0.053, 0.042, 15.713, P1 * 0.053, P2 * 0.042],
- [15.713, 0.053, 0.042, P1 * 15.713, P1 * 0.053, P2 * 0.042],
- [15.713, 0.053, 0.042, P2 * 15.713, P1 * 0.053, P2 * 0.042],
- [15.713, 0.053, 0.042, 15.713, P2 * 0.053, P2 * 0.042],
- [15.713, 0.053, 0.042, P1 * 15.713, P2 * 0.053, P2 * 0.042],
- [15.713, 0.053, 0.042, P2 * 15.713, P2 * 0.053, P2 * 0.042],
- ]
- """velocity-dependent gear ratio"""
-
- acc_time_delay = [0.00, 0.1, 0.27, 0.40, 0.60, 0.80, 1.01]
- """acc time delay"""
-
- steer_time_delay = [0.00, 0.1, 0.27, 0.40, 0.60, 0.80, 1.02]
- """steer time delay"""
-
- acc_time_constant = [0.01, 0.1, 0.20, 0.24, 0.40, 0.60, 0.80, 1.01]
- """time constant"""
-
- steer_time_constant = [0.01, 0.1, 0.20, 0.24, 0.40, 0.60, 0.80, 1.02]
- """time constant"""
-
- accel_map_scale = [0.2, 0.5, 1.0, 2.0, 3.0, 4.0, 5.0]
- """pedal - real acceleration correspondence"""
-
- acc_scaling = [0.2, 0.5, 1.0, 2.0, 3.0, 4.0, 5.01]
- """Acceleration scaling coefficient"""
-
- steer_scaling = [0.2, 0.5, 1.0, 2.0, 3.0, 4.0, 5.02]
- """Steer scaling coefficient"""
-
- vehicle_type = [1, 2, 3, 4]
- """change to other vehicle parameters"""
-
-
-def run_simulator(change_param: ChangeParam):
+def run_simulator(
+ change_param: ChangeParam,
+ root: str = ".",
+ batch_size=5,
+ patience_1=10,
+ patience_2=10,
+ acc_amp_range=0.05,
+ acc_period_range=[5.0, 20.0],
+ hidden_layer_sizes=(16, 16),
+ hidden_layer_lstm=8,
+ steer_amp_range=0.05,
+ steer_period_range=[5.0, 30.0],
+ acc_max=1.2,
+ constant_vel_time=5.0,
+ split_size=5,
+ step_response_max_input=0.01,
+ step_response_max_length=1.5,
+ step_response_start_time_ratio=0.0,
+ step_response_interval=5.0,
+ step_response_min_length=0.5,
+ use_memory_diff=True, # False,
+ skip_data_collection=False,
+ smoothing_trajectory_data_flag=True,
+):
"""Run the simulator."""
# initialize parameters
print("reset sim_setting_json")
@@ -142,6 +74,8 @@ def run_simulator(change_param: ChangeParam):
param_val_list = change_param.value
start_time = time.time()
+ dir_generator = DirGenerator(root=root)
+
for j in range(len(param_val_list)):
i = j + 0
with open("sim_setting.json", "r") as f:
@@ -151,28 +85,18 @@ def run_simulator(change_param: ChangeParam):
with open("sim_setting.json", "w") as f:
json.dump(data, f)
ir(python_simulator)
+ training_data_dirs = []
+ val_data_dirs = []
try:
initial_error = np.array(
- [0.001, 0.03, 0.01, -0.001, 0, 2 * python_simulator.measurement_steer_bias]
+ [0.001, 0.03, 0.01, 0.0, 0, python_simulator.measurement_steer_bias]
)
- if DATA_COLLECTION_MODE in ["ff", "pp"]:
- if DATA_COLLECTION_MODE == "ff":
- save_dir = "test_feedforward_sim" + change_param.name + str(i)
- else:
- save_dir = "test_pure_pursuit_sim" + change_param.name + str(i)
- python_simulator.slalom_drive(
- save_dir=save_dir,
- control_type=DATA_COLLECTION_MODE,
- t_range=[0, 200.0],
- acc_width_range=0.005,
- acc_period_range=[5.0, 20.0],
- steer_width_range=0.005,
- steer_period_range=[5.0, 20.0],
- large_steer_width_range=0.05,
- large_steer_period_range=[10.0, 20.0],
- start_large_steer_time=150.0,
- )
+ if DATA_COLLECTION_MODE in [
+ DataCollectionMode.ff,
+ DataCollectionMode.pp,
+ DataCollectionMode.npp,
+ ]:
if FORCE_NN_MODEL_TO_ZERO:
model_trainer = train_drive_NN_model.train_drive_NN_model()
else:
@@ -181,51 +105,108 @@ def run_simulator(change_param: ChangeParam):
alpha_2_for_polynomial_regression=0.1**5,
)
- model_trainer.add_data_from_csv(save_dir)
- start_time_learning = time.time()
- model_trainer.get_trained_model(
- use_polynomial_reg=USE_POLYNOMIAL_REGRESSION,
- use_selected_polynomial=USE_SELECTED_POLYNOMIAL,
- force_NN_model_to_zero=FORCE_NN_MODEL_TO_ZERO,
- fit_intercept=FIT_INTERCEPT,
- use_intercept=USE_INTERCEPT,
- )
- learning_computation_time = time.time() - start_time_learning
- model_trainer.plot_trained_result(save_dir=save_dir)
- model_trainer.save_models(save_dir=save_dir)
+ for control_type in DATA_COLLECTION_MODE.toControlTypes():
+ if control_type in CONTROL_TYPE_TO_SKIP:
+ continue
+ add_mode = ["as_val", "as_train"]
+ y_length = 60.0
+ x_length = 120.0
+ initial_error_diff = np.zeros(6)
+ if control_type == ControlType.pp_eight:
+ initial_error_diff[0] = -(x_length - y_length) / 4.0
+ initial_error_diff[1] = -y_length / 4.0
+ else:
+ initial_error_diff[1] = -0.05
+ for k in range(2):
+ save_dir = dir_generator.test_dir_name(
+ control_type=control_type,
+ change_param=change_param,
+ index=i,
+ validation_flag=(1 - k),
+ )
+ if not skip_data_collection:
+ python_simulator.drive_sim(
+ save_dir=save_dir,
+ control_type=control_type,
+ seed=k + 1,
+ t_range=[0, 900.0],
+ acc_amp_range=acc_amp_range,
+ acc_period_range=acc_period_range,
+ steer_amp_range=steer_amp_range,
+ steer_period_range=steer_period_range,
+ large_steer_amp_range=0.0,
+ large_steer_period_range=[10.0, 20.0],
+ start_large_steer_time=150.0,
+ acc_max=acc_max,
+ constant_vel_time=constant_vel_time,
+ split_size=split_size,
+ y_length=(0.9 ** (1 - k)) * y_length,
+ x_length=(0.9 ** (1 - k)) * x_length,
+ step_response_max_input=step_response_max_input,
+ step_response_max_length=step_response_max_length,
+ step_response_start_time_ratio=step_response_start_time_ratio,
+ step_response_interval=step_response_interval,
+ step_response_min_length=step_response_min_length,
+ initial_error=initial_error + (1 - k) * initial_error_diff,
+ smoothing_trajectory_data_flag=smoothing_trajectory_data_flag,
+ )
+ model_trainer.add_data_from_csv(save_dir, add_mode=add_mode[k])
+ if k == 0:
+ val_data_dirs.append(save_dir)
+ else:
+ training_data_dirs.append(save_dir)
+
+ if not skip_data_collection:
+ start_time_learning = time.time()
+ model_trainer.get_trained_model(
+ use_polynomial_reg=USE_POLYNOMIAL_REGRESSION,
+ use_selected_polynomial=USE_SELECTED_POLYNOMIAL,
+ force_NN_model_to_zero=FORCE_NN_MODEL_TO_ZERO,
+ fit_intercept=FIT_INTERCEPT,
+ use_intercept=USE_INTERCEPT,
+ hidden_layer_sizes=hidden_layer_sizes,
+ hidden_layer_lstm=hidden_layer_lstm,
+ batch_size=batch_size,
+ patience=patience_1,
+ )
+ learning_computation_time = time.time() - start_time_learning
+ model_trainer.plot_trained_result(save_dir=save_dir)
+ model_trainer.save_models(save_dir=save_dir)
+ print("learning_computation_time:", learning_computation_time)
+ f = open(
+ save_dir
+ + f"/computational_time_learning_from_{DATA_COLLECTION_MODE}_data.txt",
+ "w",
+ )
+ f.write(str(learning_computation_time))
+ f.close()
load_dir = save_dir
- if DATA_COLLECTION_MODE == "ff":
- save_dir = "test_python_ff_aided_sim_" + change_param.name + str(i)
- elif DATA_COLLECTION_MODE == "pp":
- save_dir = "test_python_pp_aided_sim_" + change_param.name + str(i)
- auto_test_performance_result_list = python_simulator.slalom_drive(
+ save_dir = dir_generator.test_dir_name(
+ data_collection_mode=DATA_COLLECTION_MODE,
+ change_param=change_param,
+ index=i,
+ use_memory_diff=use_memory_diff,
+ )
+ auto_test_performance_result_list = python_simulator.drive_sim(
load_dir=load_dir,
save_dir=save_dir,
use_trained_model=True,
use_trained_model_diff=USE_TRAINED_MODEL_DIFF,
initial_error=initial_error,
+ use_memory_diff=use_memory_diff,
)
+ training_data_dirs.append(save_dir)
- print("learning_computation_time:", learning_computation_time)
- if DATA_COLLECTION_MODE == "ff":
- f = open(save_dir + "/computational_time_learning_from_ff_data.txt", "w")
- f.write(str(learning_computation_time))
- f.close()
- f = open(
- "auto_test_result_intermediate_model_control_trained_with_data_collected_by_ff_control.csv",
- mode="a",
- )
- elif DATA_COLLECTION_MODE == "pp":
- f = open(save_dir + "/computational_time_learning_from_pp_data.txt", "w")
- f.write(str(learning_computation_time))
- f.close()
- f = open(
- "auto_test_result_intermediate_model_control_trained_with_data_collected_by_pp_control.csv",
- mode="a",
- )
+ f = open(
+ f"auto_test_result_intermediate_model_control_trained_with_data_collected_by_{control_type}_control.csv",
+ mode="a",
+ )
else:
- save_dir = "test_python_sim_" + change_param.name + str(i)
- auto_test_performance_result_list = python_simulator.slalom_drive(
+ save_dir = dir_generator.test_dir_name(
+ change_param=change_param,
+ index=i,
+ )
+ auto_test_performance_result_list = python_simulator.drive_sim(
save_dir=save_dir,
initial_error=initial_error,
)
@@ -236,7 +217,7 @@ def run_simulator(change_param: ChangeParam):
str(param_val_list[j]).replace(",", "_"),
*auto_test_performance_result_list,
sep=",",
- file=f
+ file=f,
)
f.close()
@@ -246,10 +227,16 @@ def run_simulator(change_param: ChangeParam):
if not skip_learning_for_developing_testcase:
ir(train_drive_NN_model)
model_trainer = train_drive_NN_model.train_drive_NN_model()
- learning_computation_time = None
- if DATA_COLLECTION_MODE in ["ff", "pp"]:
- model_trainer.add_data_from_csv(load_dir)
- model_trainer.add_data_from_csv(save_dir)
+ learning_computation_time = None # type: ignore
+ if DATA_COLLECTION_MODE in [
+ DataCollectionMode.ff,
+ DataCollectionMode.pp,
+ DataCollectionMode.npp,
+ ]:
+ for dir_name in val_data_dirs:
+ model_trainer.add_data_from_csv(dir_name, add_mode="as_val")
+ for dir_name in training_data_dirs:
+ model_trainer.add_data_from_csv(dir_name, add_mode="as_train")
start_time_learning = time.time()
model_trainer.update_saved_trained_model(
path=load_dir + "/model_for_test_drive.pth",
@@ -258,16 +245,25 @@ def run_simulator(change_param: ChangeParam):
force_NN_model_to_zero=FORCE_NN_MODEL_TO_ZERO,
fit_intercept=FIT_INTERCEPT,
use_intercept=USE_INTERCEPT,
+ batch_size=batch_size,
+ patience=patience_2,
)
learning_computation_time = time.time() - start_time_learning
model_trainer.plot_trained_result(save_dir=save_dir)
model_trainer.save_models(save_dir=save_dir)
- if DATA_COLLECTION_MODE == "ff":
- load_dir = "test_python_ff_aided_sim_" + change_param.name + str(i)
- save_dir = "test_python_ff_aided_sim_trained_" + change_param.name + str(i)
- elif DATA_COLLECTION_MODE == "pp":
- load_dir = "test_python_pp_aided_sim_" + change_param.name + str(i)
- save_dir = "test_python_pp_aided_sim_trained_" + change_param.name + str(i)
+ load_dir = dir_generator.test_dir_name(
+ data_collection_mode=DATA_COLLECTION_MODE,
+ change_param=change_param,
+ index=i,
+ use_memory_diff=use_memory_diff,
+ )
+ save_dir = dir_generator.test_dir_name(
+ data_collection_mode=DATA_COLLECTION_MODE,
+ trained=True,
+ change_param=change_param,
+ index=i,
+ use_memory_diff=use_memory_diff,
+ )
else:
model_trainer.add_data_from_csv(save_dir)
start_time_learning = time.time()
@@ -277,20 +273,35 @@ def run_simulator(change_param: ChangeParam):
force_NN_model_to_zero=FORCE_NN_MODEL_TO_ZERO,
fit_intercept=FIT_INTERCEPT,
use_intercept=USE_INTERCEPT,
+ hidden_layer_sizes=hidden_layer_sizes,
+ hidden_layer_lstm=hidden_layer_lstm,
+ batch_size=batch_size,
+ patience=patience_1,
)
learning_computation_time = time.time() - start_time_learning
model_trainer.plot_trained_result(save_dir=save_dir)
model_trainer.save_models(save_dir=save_dir)
- load_dir = "test_python_sim_" + change_param.name + str(i)
- save_dir = "test_python_sim_trained_" + change_param.name + str(i)
- auto_test_performance_result_list = python_simulator.slalom_drive(
+ load_dir = dir_generator.test_dir_name(
+ change_param=change_param,
+ index=i,
+ use_memory_diff=use_memory_diff,
+ )
+ save_dir = dir_generator.test_dir_name(
+ trained=True,
+ change_param=change_param,
+ index=i,
+ use_memory_diff=use_memory_diff,
+ )
+ auto_test_performance_result_list = python_simulator.drive_sim(
load_dir=load_dir,
use_trained_model=True,
use_trained_model_diff=USE_TRAINED_MODEL_DIFF,
save_dir=save_dir,
initial_error=initial_error,
+ use_memory_diff=use_memory_diff,
)
+ # cSpell:ignore oneline
print("learning_computation_time: ", learning_computation_time)
f = open(save_dir + "/test_info.txt", "w")
f.write("commit id: " + str(os.popen("git log --oneline -1").read()) + "\n")
@@ -307,55 +318,33 @@ def run_simulator(change_param: ChangeParam):
f.write("USE_INTERCEPT: " + str(USE_INTERCEPT) + "\n")
f.close()
- if DATA_COLLECTION_MODE == "ff":
- f = open(
- "auto_test_result_final_model_control_trained_with_data_collected_by_ff_control.csv",
- mode="a",
- )
- elif DATA_COLLECTION_MODE == "pp":
- f = open(
- "auto_test_result_final_model_control_trained_with_data_collected_by_pp_control.csv",
- mode="a",
- )
- else:
- f = open(
- "auto_test_result_final_model_control_trained_with_data_collected_by_nominal_control.csv",
- mode="a",
- )
+ f = open(
+ f"auto_test_result_final_model_control_trained_with_data_collected_by_{DATA_COLLECTION_MODE}_control.csv",
+ mode="a",
+ )
print(
change_param.name,
str(param_val_list[j]).replace(",", "_"),
*auto_test_performance_result_list,
sep=",",
- file=f
+ file=f,
)
f.close()
print("experiment success")
except Exception:
print("# Catch Exception #")
- print(traceback.print_exc())
+ traceback.print_exc()
auto_test_performance_result_list = [1e16] * 11
- if DATA_COLLECTION_MODE == "ff":
- f = open(
- "auto_test_result_final_model_control_trained_with_data_collected_by_ff_control.csv",
- mode="a",
- )
- elif DATA_COLLECTION_MODE == "pp":
- f = open(
- "auto_test_result_final_model_control_trained_with_data_collected_by_pp_control.csv",
- mode="a",
- )
- else:
- f = open(
- "auto_test_result_final_model_control_trained_with_data_collected_by_nominal_control.csv",
- mode="a",
- )
+ f = open(
+ f"auto_test_result_final_model_control_trained_with_data_collected_by_{DATA_COLLECTION_MODE}_control.csv",
+ mode="a",
+ )
print(
change_param.name,
str(param_val_list[j]).replace(",", "_"),
*auto_test_performance_result_list,
sep=",",
- file=f
+ file=f,
)
f.close()
print("experiment failure")
@@ -373,20 +362,29 @@ def yes_no_input():
if __name__ == "__main__":
+ parser = argparse.ArgumentParser()
+ parser.add_argument("--param_name", default=None)
+ parser.add_argument("--root", default=".")
+ args = parser.parse_args()
+
+ if args.root == "time":
+ args.root = datetime.datetime.now().strftime("%Y%m%d_%H%M%S")
+ args.root = "test_run_sim_" + args.root
+
if args.param_name is None:
print("Do you want to run the simulation for all parameters at once?")
if yes_no_input():
for _, change_param in ChangeParam.__members__.items():
- run_simulator(change_param)
+ run_simulator(change_param, root=args.root)
else:
print("Enter the name of the parameter for which the simulation is to be run.")
input_string = input()
for name, change_param in ChangeParam.__members__.items():
if name == input_string:
- run_simulator(change_param)
+ run_simulator(change_param, root=args.root)
break
else:
for name, change_param in ChangeParam.__members__.items():
if name == args.param_name:
- run_simulator(change_param)
+ run_simulator(change_param, root=args.root)
break
diff --git a/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/test_figure_eight_param.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/test_figure_eight_param.py
new file mode 100644
index 0000000000000..eaaeeaf8351b5
--- /dev/null
+++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/python_simulator/test_figure_eight_param.py
@@ -0,0 +1,112 @@
+# Copyright 2024 Proxima Technology Inc, TIER IV
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+import argparse
+import os
+
+from assets import ChangeParam # type: ignore
+import numpy as np
+import run_sim # type: ignore
+
+if __name__ == "__main__":
+ test_NN_size = [(16, 16), (8, 0)]
+ batch_size = 5
+
+ parser = argparse.ArgumentParser(description="Process some integers.")
+ parser.add_argument("--acc_amp_range", type=float)
+ parser.add_argument("--acc_period_min", type=float)
+ parser.add_argument("--acc_period_max", type=float)
+ parser.add_argument("--steer_amp_range", type=float)
+ parser.add_argument("--steer_period_min", type=float)
+ parser.add_argument("--steer_period_max", type=float)
+ parser.add_argument("--step_response_max_input", type=float)
+ parser.add_argument("--step_response_max_length", type=float)
+ parser.add_argument("--step_response_interval", type=float)
+ parser.add_argument("--step_response_min_length", type=float)
+ parser.add_argument("--constant_vel_time", type=float)
+ parser.add_argument("--use_memory_diff", action="store_true", help="Use memory diff.")
+ parser.add_argument("--dir_name", type=str, default="test_vehicles")
+
+ args = parser.parse_args()
+
+ acc_amp_range = args.acc_amp_range
+ acc_period_min = args.acc_period_min
+ acc_period_max = args.acc_period_max
+ steer_amp_range = args.steer_amp_range
+ steer_period_min = args.steer_period_min
+ steer_period_max = args.steer_period_max
+ step_response_max_input = args.step_response_max_input
+ step_response_max_length = args.step_response_max_length
+ step_response_interval = args.step_response_interval
+ step_response_min_length = args.step_response_min_length
+ constant_vel_time = args.constant_vel_time
+ use_memory_diff: bool = args.use_memory_diff
+ dir_name = args.dir_name
+
+ if not os.path.isdir(dir_name):
+ os.mkdir(dir_name)
+ condition = np.array(
+ [
+ acc_amp_range,
+ acc_period_min,
+ acc_period_max,
+ steer_amp_range,
+ steer_period_min,
+ steer_period_max,
+ step_response_max_input,
+ step_response_max_length,
+ step_response_interval,
+ step_response_min_length,
+ constant_vel_time,
+ ]
+ )
+ print("condition: ", condition)
+ np.savetxt(dir_name + "/figure_eight_param.csv", condition, delimiter=",")
+
+ if use_memory_diff:
+ run_sim.run_simulator(
+ change_param=ChangeParam.test_vehicle,
+ root=dir_name,
+ batch_size=batch_size,
+ hidden_layer_sizes=test_NN_size[0],
+ hidden_layer_lstm=test_NN_size[1][0],
+ acc_amp_range=acc_amp_range,
+ acc_period_range=[acc_period_min, acc_period_max],
+ steer_amp_range=steer_amp_range,
+ steer_period_range=[steer_period_min, steer_period_max],
+ step_response_max_input=step_response_max_input,
+ step_response_max_length=step_response_max_length,
+ step_response_interval=step_response_interval,
+ step_response_min_length=step_response_min_length,
+ constant_vel_time=constant_vel_time,
+ use_memory_diff=True,
+ skip_data_collection=True,
+ )
+ else:
+ run_sim.run_simulator(
+ change_param=ChangeParam.test_vehicle,
+ root=dir_name,
+ batch_size=batch_size,
+ hidden_layer_sizes=test_NN_size[0],
+ hidden_layer_lstm=test_NN_size[1][0],
+ acc_amp_range=acc_amp_range,
+ acc_period_range=[acc_period_min, acc_period_max],
+ steer_amp_range=steer_amp_range,
+ steer_period_range=[steer_period_min, steer_period_max],
+ step_response_max_input=step_response_max_input,
+ step_response_max_length=step_response_max_length,
+ step_response_interval=step_response_interval,
+ step_response_min_length=step_response_min_length,
+ constant_vel_time=constant_vel_time,
+ use_memory_diff=False,
+ )
diff --git a/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_NN.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_NN.py
index f13aa07b0f795..84a0486b1e074 100644
--- a/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_NN.py
+++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_NN.py
@@ -25,6 +25,37 @@
dim_acc_layer_1 = 16
dim_acc_layer_2 = 16
+loss_weights = torch.tensor(
+ [
+ drive_functions.NN_x_weight,
+ drive_functions.NN_y_weight,
+ drive_functions.NN_yaw_weight,
+ drive_functions.NN_v_weight,
+ drive_functions.NN_acc_weight,
+ drive_functions.NN_steer_weight,
+ ]
+)
+loss_weights_diff = torch.tensor(
+ [
+ drive_functions.NN_x_weight_diff,
+ drive_functions.NN_y_weight_diff,
+ drive_functions.NN_yaw_weight_diff,
+ drive_functions.NN_v_weight_diff,
+ drive_functions.NN_acc_weight_diff,
+ drive_functions.NN_steer_weight_diff,
+ ]
+)
+loss_weights_two_diff = torch.tensor(
+ [
+ drive_functions.NN_x_weight_two_diff,
+ drive_functions.NN_y_weight_two_diff,
+ drive_functions.NN_yaw_weight_two_diff,
+ drive_functions.NN_v_weight_two_diff,
+ drive_functions.NN_acc_weight_two_diff,
+ drive_functions.NN_steer_weight_two_diff,
+ ]
+)
+
def nominal_model_input(Var, lam, step):
"""Calculate prediction with nominal model that takes into account time constants for the first order and time delays related to the input."""
@@ -43,13 +74,41 @@ def nominal_model_input(Var, lam, step):
def loss_fn_plus_tanh(loss_fn, pred, Y, tanh_gain, tanh_weight):
"""Compute the loss function to be used in the training."""
- loss = loss_fn(pred, Y)
+ loss = loss_fn(pred * loss_weights, Y * loss_weights)
loss += tanh_weight * loss_fn(
torch.tanh(tanh_gain * (pred[:, -1] - Y[:, -1])), torch.zeros(Y.shape[0])
)
return loss
+def loss_fn_plus_tanh_with_memory(
+ loss_fn, pred, Y, tanh_gain, tanh_weight, first_order_weight, second_order_weight
+):
+ """Compute the loss function to be used in the training."""
+ loss = loss_fn(pred * loss_weights, Y * loss_weights)
+ loss += (
+ first_order_weight
+ * loss_fn(
+ (pred[:, 1:] - pred[:, :-1]) * loss_weights_diff,
+ (Y[:, 1:] - Y[:, :-1]) * loss_weights_diff,
+ )
+ / drive_functions.ctrl_time_step
+ )
+ loss += (
+ second_order_weight
+ * loss_fn(
+ (pred[:, 2:] + pred[:, :-2] - 2 * pred[:, 1:-1]) * loss_weights_two_diff,
+ (Y[:, 2:] + Y[:, :-2] - 2 * Y[:, 1:-1]) * loss_weights_two_diff,
+ )
+ / (drive_functions.ctrl_time_step * drive_functions.ctrl_time_step)
+ )
+ loss += tanh_weight * loss_fn(
+ torch.tanh(tanh_gain * (pred[:, :, -1] - Y[:, :, -1])),
+ torch.zeros((Y.shape[0], Y.shape[1])),
+ )
+ return loss
+
+
# example usage: `model = DriveNeuralNetwork(params...).to("cpu")`
class DriveNeuralNetwork(nn.Module):
"""Define the neural net model to be used in vehicle control."""
@@ -80,8 +139,6 @@ def __init__(
self.steer_input_index = np.concatenate(
([2], np.arange(steer_queue_size_core) + acc_queue_size + 3)
)
- self.acc_input_index_ = np.arange(acc_queue_size) + 3
- self.steer_input_index_ = np.arange(steer_queue_size_core) + acc_queue_size + 3
self.steer_input_index_full = np.arange(steer_queue_size) + acc_queue_size + 3
self.acc_layer_1 = nn.Sequential(
nn.Linear(self.acc_input_index.shape[0], dim_acc_layer_1),
@@ -102,12 +159,12 @@ def __init__(
nn.init.uniform_(self.steer_layer_1_tail[0].weight, a=lb, b=ub)
nn.init.uniform_(self.steer_layer_1_tail[0].bias, a=lb, b=ub)
- self.acc_layer_2 = nn.Sequential(nn.Linear(dim_acc_layer_1, dim_acc_layer_2), nn.Tanh())
+ self.acc_layer_2 = nn.Sequential(nn.Linear(dim_acc_layer_1, dim_acc_layer_2), nn.ReLU())
nn.init.uniform_(self.acc_layer_2[0].weight, a=lb, b=ub)
nn.init.uniform_(self.acc_layer_2[0].bias, a=lb, b=ub)
self.steer_layer_2 = nn.Sequential(
- nn.Linear(dim_steer_layer_1_head + dim_steer_layer_1_tail, dim_steer_layer_2), nn.Tanh()
+ nn.Linear(dim_steer_layer_1_head + dim_steer_layer_1_tail, dim_steer_layer_2), nn.ReLU()
)
nn.init.uniform_(self.steer_layer_2[0].weight, a=lb, b=ub)
nn.init.uniform_(self.steer_layer_2[0].bias, a=lb, b=ub)
@@ -132,22 +189,148 @@ def __init__(
self.steer_dropout = nn.Dropout(steer_drop_out)
def forward(self, x):
- acc_layer_1 = self.acc_layer_1(x[:, self.acc_input_index])
+ acc_layer_1 = self.acc_layer_1(drive_functions.acc_normalize * x[:, self.acc_input_index])
steer_layer_1 = torch.cat(
(
- self.steer_layer_1_head(x[:, self.steer_input_index]),
- self.steer_layer_1_tail(x[:, self.steer_input_index_full]),
+ self.steer_layer_1_head(
+ drive_functions.steer_normalize * x[:, self.steer_input_index]
+ ),
+ self.steer_layer_1_tail(
+ drive_functions.steer_normalize * x[:, self.steer_input_index_full]
+ ),
),
dim=1,
)
acc_layer_2 = self.acc_layer_2(self.acc_dropout(acc_layer_1))
steer_layer_2 = self.steer_layer_2(self.steer_dropout(steer_layer_1))
- pre_pred = self.linear_relu_stack(torch.cat((x[:, [0]], acc_layer_2, steer_layer_2), dim=1))
+ pre_pred = self.linear_relu_stack(
+ torch.cat(
+ (drive_functions.vel_normalize * x[:, [0]], acc_layer_2, steer_layer_2), dim=1
+ )
+ )
pred = self.finalize(torch.cat((pre_pred, acc_layer_2, steer_layer_2), dim=1))
return pred
+class DriveNeuralNetworkWithMemory(nn.Module):
+ """Define the neural net model with memory to be used in vehicle control."""
+
+ def __init__(
+ self,
+ hidden_layer_sizes=(32, 16),
+ hidden_layer_lstm=64,
+ randomize=0.01,
+ acc_drop_out=0.0,
+ steer_drop_out=0.0,
+ acc_delay_step=drive_functions.acc_delay_step,
+ steer_delay_step=drive_functions.steer_delay_step,
+ acc_time_constant_ctrl=drive_functions.acc_time_constant,
+ steer_time_constant_ctrl=drive_functions.steer_time_constant,
+ acc_queue_size=drive_functions.acc_ctrl_queue_size,
+ steer_queue_size=drive_functions.steer_ctrl_queue_size,
+ steer_queue_size_core=drive_functions.steer_ctrl_queue_size_core,
+ ):
+ super().__init__()
+ self.acc_time_constant_ctrl = acc_time_constant_ctrl
+ self.acc_delay_step = acc_delay_step
+ self.steer_time_constant_ctrl = steer_time_constant_ctrl
+ self.steer_delay_step = steer_delay_step
+
+ lb = -randomize
+ ub = randomize
+ self.acc_input_index = np.concatenate(([1], np.arange(acc_queue_size) + 3))
+ self.steer_input_index = np.concatenate(
+ ([2], np.arange(steer_queue_size_core) + acc_queue_size + 3)
+ )
+ self.steer_input_index_full = np.arange(steer_queue_size) + acc_queue_size + 3
+ self.acc_layer_1 = nn.Sequential(
+ nn.Linear(self.acc_input_index.shape[0], dim_acc_layer_1),
+ nn.ReLU(),
+ )
+ nn.init.uniform_(self.acc_layer_1[0].weight, a=lb, b=ub)
+ nn.init.uniform_(self.acc_layer_1[0].bias, a=lb, b=ub)
+ self.steer_layer_1_head = nn.Sequential(
+ nn.Linear(self.steer_input_index.shape[0], dim_steer_layer_1_head),
+ nn.ReLU(),
+ )
+ nn.init.uniform_(self.steer_layer_1_head[0].weight, a=lb, b=ub)
+ nn.init.uniform_(self.steer_layer_1_head[0].bias, a=lb, b=ub)
+
+ self.steer_layer_1_tail = nn.Sequential(
+ nn.Linear(self.steer_input_index_full.shape[0], dim_steer_layer_1_tail), nn.ReLU()
+ )
+ nn.init.uniform_(self.steer_layer_1_tail[0].weight, a=lb, b=ub)
+ nn.init.uniform_(self.steer_layer_1_tail[0].bias, a=lb, b=ub)
+
+ self.acc_layer_2 = nn.Sequential(nn.Linear(dim_acc_layer_1, dim_acc_layer_2), nn.ReLU())
+ nn.init.uniform_(self.acc_layer_2[0].weight, a=lb, b=ub)
+ nn.init.uniform_(self.acc_layer_2[0].bias, a=lb, b=ub)
+
+ self.steer_layer_2 = nn.Sequential(
+ nn.Linear(dim_steer_layer_1_head + dim_steer_layer_1_tail, dim_steer_layer_2), nn.ReLU()
+ )
+ nn.init.uniform_(self.steer_layer_2[0].weight, a=lb, b=ub)
+ nn.init.uniform_(self.steer_layer_2[0].bias, a=lb, b=ub)
+
+ self.lstm = nn.LSTM(
+ 1 + dim_acc_layer_2 + dim_steer_layer_2, hidden_layer_lstm, batch_first=True
+ )
+
+ nn.init.uniform_(self.lstm.weight_hh_l0, a=lb, b=ub)
+ nn.init.uniform_(self.lstm.weight_ih_l0, a=lb, b=ub)
+ nn.init.uniform_(self.lstm.bias_hh_l0, a=lb, b=ub)
+ nn.init.uniform_(self.lstm.bias_ih_l0, a=lb, b=ub)
+
+ self.linear_relu_stack_1 = nn.Sequential(
+ nn.Linear(1 + dim_acc_layer_2 + dim_steer_layer_2, hidden_layer_sizes[0]),
+ nn.ReLU(),
+ )
+ nn.init.uniform_(self.linear_relu_stack_1[0].weight, a=lb, b=ub)
+ nn.init.uniform_(self.linear_relu_stack_1[0].bias, a=lb, b=ub)
+
+ self.linear_relu_stack_2 = nn.Sequential(
+ nn.Linear(hidden_layer_lstm + hidden_layer_sizes[0], hidden_layer_sizes[1]),
+ nn.ReLU(),
+ )
+ nn.init.uniform_(self.linear_relu_stack_2[0].weight, a=lb, b=ub)
+ nn.init.uniform_(self.linear_relu_stack_2[0].bias, a=lb, b=ub)
+
+ self.finalize = nn.Linear(hidden_layer_sizes[1] + dim_acc_layer_2 + dim_steer_layer_2, 6)
+
+ nn.init.uniform_(self.finalize.weight, a=lb, b=ub)
+ nn.init.uniform_(self.finalize.bias, a=lb, b=ub)
+
+ self.acc_dropout = nn.Dropout(acc_drop_out)
+ self.steer_dropout = nn.Dropout(steer_drop_out)
+
+ def forward(self, x):
+ acc_layer_1 = self.acc_layer_1(
+ drive_functions.acc_normalize * x[:, :, self.acc_input_index]
+ )
+ steer_layer_1 = torch.cat(
+ (
+ self.steer_layer_1_head(
+ drive_functions.steer_normalize * x[:, :, self.steer_input_index]
+ ),
+ self.steer_layer_1_tail(
+ drive_functions.steer_normalize * x[:, :, self.steer_input_index_full]
+ ),
+ ),
+ dim=2,
+ )
+ acc_layer_2 = self.acc_layer_2(self.acc_dropout(acc_layer_1))
+ steer_layer_2 = self.steer_layer_2(self.steer_dropout(steer_layer_1))
+ h_1 = torch.cat(
+ (drive_functions.vel_normalize * x[:, :, [0]], acc_layer_2, steer_layer_2), dim=2
+ )
+ pre_pred, _ = self.lstm(h_1)
+ pre_pred = torch.cat((pre_pred, self.linear_relu_stack_1(h_1)), dim=2)
+ pre_pred = self.linear_relu_stack_2(pre_pred)
+ pred = self.finalize(torch.cat((pre_pred, acc_layer_2, steer_layer_2), dim=2))
+ return pred
+
+
class EarlyStopping:
"""Class for early stopping in NN training."""
@@ -180,68 +363,110 @@ def __init__(
A_for_linear_reg,
b_for_linear_reg,
deg,
- acc_time_constant,
- acc_delay_step,
- steer_time_constant,
- steer_delay_step,
- acc_queue_size,
- steer_queue_size,
- steer_queue_size_core,
+ acc_delay_step=drive_functions.acc_delay_step,
+ steer_delay_step=drive_functions.steer_delay_step,
+ acc_queue_size=drive_functions.acc_ctrl_queue_size,
+ steer_queue_size=drive_functions.steer_ctrl_queue_size,
+ steer_queue_size_core=drive_functions.steer_ctrl_queue_size_core,
+ vel_normalize=drive_functions.vel_normalize,
+ acc_normalize=drive_functions.acc_normalize,
+ steer_normalize=drive_functions.steer_normalize,
+ ):
+ self.transform = proxima_calc.transform_model_to_eigen()
+ self.transform.set_params(
+ model.acc_layer_1[0].weight.detach().numpy().astype(np.float64),
+ model.steer_layer_1_head[0].weight.detach().numpy().astype(np.float64),
+ model.steer_layer_1_tail[0].weight.detach().numpy().astype(np.float64),
+ model.acc_layer_2[0].weight.detach().numpy().astype(np.float64),
+ model.steer_layer_2[0].weight.detach().numpy().astype(np.float64),
+ model.linear_relu_stack[0].weight.detach().numpy().astype(np.float64),
+ model.linear_relu_stack[2].weight.detach().numpy().astype(np.float64),
+ model.finalize.weight.detach().numpy().astype(np.float64),
+ model.acc_layer_1[0].bias.detach().numpy().astype(np.float64),
+ model.steer_layer_1_head[0].bias.detach().numpy().astype(np.float64),
+ model.steer_layer_1_tail[0].bias.detach().numpy().astype(np.float64),
+ model.acc_layer_2[0].bias.detach().numpy().astype(np.float64),
+ model.steer_layer_2[0].bias.detach().numpy().astype(np.float64),
+ model.linear_relu_stack[0].bias.detach().numpy().astype(np.float64),
+ model.linear_relu_stack[2].bias.detach().numpy().astype(np.float64),
+ model.finalize.bias.detach().numpy().astype(np.float64),
+ A_for_linear_reg,
+ b_for_linear_reg,
+ deg,
+ acc_delay_step,
+ steer_delay_step,
+ acc_queue_size,
+ steer_queue_size,
+ steer_queue_size_core,
+ vel_normalize,
+ acc_normalize,
+ steer_normalize,
+ )
+ self.pred = self.transform.rot_and_d_rot_error_prediction
+ self.pred_only_state = self.transform.rotated_error_prediction
+ self.pred_with_diff = self.transform.rot_and_d_rot_error_prediction_with_diff
+ self.pred_with_poly_diff = self.transform.rot_and_d_rot_error_prediction_with_poly_diff
+ self.Pred = self.transform.Rotated_error_prediction
+
+
+class transform_model_with_memory_to_c:
+ """Pass the necessary information to the C++ program to call the trained model at high speed."""
+
+ def __init__(
+ self,
+ model,
+ A_for_linear_reg,
+ b_for_linear_reg,
+ deg,
+ acc_delay_step=drive_functions.acc_delay_step,
+ steer_delay_step=drive_functions.steer_delay_step,
+ acc_queue_size=drive_functions.acc_ctrl_queue_size,
+ steer_queue_size=drive_functions.steer_ctrl_queue_size,
+ steer_queue_size_core=drive_functions.steer_ctrl_queue_size_core,
+ vel_normalize=drive_functions.vel_normalize,
+ acc_normalize=drive_functions.acc_normalize,
+ steer_normalize=drive_functions.steer_normalize,
):
- transformer = proxima_calc.transform_model_to_eigen()
-
- numpy_weight_list = []
- numpy_bias_list = []
- for i, w in enumerate(model.parameters()):
- if i % 2 == 0:
- numpy_weight_list.append(w.detach().numpy().astype(np.float64))
- else:
- numpy_bias_list.append(w.detach().numpy().astype(np.float64))
- array_weight_list_0 = numpy_weight_list[0]
- array_bias_list_0 = numpy_bias_list[0]
- array_weight_list_1 = numpy_weight_list[1]
- array_bias_list_1 = numpy_bias_list[1]
- array_weight_list_2 = numpy_weight_list[2]
- array_bias_list_2 = numpy_bias_list[2]
- array_weight_list_3 = numpy_weight_list[3]
- array_bias_list_3 = numpy_bias_list[3]
- array_weight_list_4 = numpy_weight_list[4]
- array_bias_list_4 = numpy_bias_list[4]
- array_weight_list_5 = numpy_weight_list[5]
- array_bias_list_5 = numpy_bias_list[5]
- array_weight_list_6 = numpy_weight_list[6]
- array_bias_list_6 = numpy_bias_list[6]
- array_weight_list_7 = numpy_weight_list[7]
- array_bias_list_7 = numpy_bias_list[7]
-
- transformer.set_params(
- array_weight_list_0,
- array_weight_list_1,
- array_weight_list_2,
- array_weight_list_3,
- array_weight_list_4,
- array_weight_list_5,
- array_weight_list_6,
- array_weight_list_7,
- array_bias_list_0,
- array_bias_list_1,
- array_bias_list_2,
- array_bias_list_3,
- array_bias_list_4,
- array_bias_list_5,
- array_bias_list_6,
- array_bias_list_7,
+ self.transform = proxima_calc.transform_model_with_memory_to_eigen()
+ self.transform.set_params(
+ model.acc_layer_1[0].weight.detach().numpy().astype(np.float64),
+ model.steer_layer_1_head[0].weight.detach().numpy().astype(np.float64),
+ model.steer_layer_1_tail[0].weight.detach().numpy().astype(np.float64),
+ model.acc_layer_2[0].weight.detach().numpy().astype(np.float64),
+ model.steer_layer_2[0].weight.detach().numpy().astype(np.float64),
+ model.lstm.weight_ih_l0.detach().numpy().astype(np.float64),
+ model.lstm.weight_hh_l0.detach().numpy().astype(np.float64),
+ model.linear_relu_stack_1[0].weight.detach().numpy().astype(np.float64),
+ model.linear_relu_stack_2[0].weight.detach().numpy().astype(np.float64),
+ model.finalize.weight.detach().numpy().astype(np.float64),
+ model.acc_layer_1[0].bias.detach().numpy().astype(np.float64),
+ model.steer_layer_1_head[0].bias.detach().numpy().astype(np.float64),
+ model.steer_layer_1_tail[0].bias.detach().numpy().astype(np.float64),
+ model.acc_layer_2[0].bias.detach().numpy().astype(np.float64),
+ model.steer_layer_2[0].bias.detach().numpy().astype(np.float64),
+ model.lstm.bias_hh_l0.detach().numpy().astype(np.float64),
+ model.lstm.bias_ih_l0.detach().numpy().astype(np.float64),
+ model.linear_relu_stack_1[0].bias.detach().numpy().astype(np.float64),
+ model.linear_relu_stack_2[0].bias.detach().numpy().astype(np.float64),
+ model.finalize.bias.detach().numpy().astype(np.float64),
+ )
+ self.transform.set_params_res(
A_for_linear_reg,
b_for_linear_reg,
deg,
- acc_time_constant,
acc_delay_step,
- steer_time_constant,
steer_delay_step,
acc_queue_size,
steer_queue_size,
steer_queue_size_core,
+ vel_normalize,
+ acc_normalize,
+ steer_normalize,
)
- self.pred = transformer.rot_and_d_rot_error_prediction
- self.pred_with_diff = transformer.rot_and_d_rot_error_prediction_with_diff
- self.Pred = transformer.Rotated_error_prediction
+ self.pred = self.transform.rot_and_d_rot_error_prediction
+ self.pred_only_state = self.transform.rotated_error_prediction
+ self.pred_with_diff = self.transform.rot_and_d_rot_error_prediction_with_diff
+ self.pred_with_memory_diff = self.transform.rot_and_d_rot_error_prediction_with_memory_diff
+ self.pred_with_poly_diff = self.transform.rot_and_d_rot_error_prediction_with_poly_diff
+ self.Pred = self.transform.Rotated_error_prediction
+ self.test_lstm = self.transform.error_prediction
diff --git a/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_controller.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_controller.py
index 3877f7177720b..4fb4fe3a951e1 100644
--- a/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_controller.py
+++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_controller.py
@@ -20,6 +20,7 @@
from importlib import reload as ir
import threading
import time
+from typing import Literal
from autoware_smart_mpc_trajectory_follower.scripts import drive_GP
from autoware_smart_mpc_trajectory_follower.scripts import drive_NN
@@ -57,8 +58,8 @@ class drive_controller:
update_trained_model: bool | None
"""Whether to update the model online with the data under control."""
- mode: str | None
- """which algorithm to use, "mppi", "ilqr", or "mppi_ilqr" """
+ mode: Literal["pure_pursuit", "naive_pure_pursuit", "mppi", "ilqr", "mppi_ilqr"] | None
+ """which algorithm to use"""
use_x_noise: bool | None
"""Whether to reflect the uncertainty of the straight-line position when performing iLQG"""
@@ -109,6 +110,7 @@ def __init__(
self,
use_trained_model: bool | None = None,
use_trained_model_diff=None,
+ use_memory_diff=None,
update_trained_model=None,
mode=None,
use_x_noise=None,
@@ -138,6 +140,14 @@ def __init__(
ir(drive_iLQR)
ir(drive_mppi)
+ if mode is None:
+ self.mode = drive_functions.mode
+ else:
+ self.mode = mode
+
+ if mode == "pure_pursuit" or mode == "naive_pure_pursuit":
+ use_trained_model = False
+
if use_trained_model is None:
self.use_trained_model = drive_functions.use_trained_model
else:
@@ -145,6 +155,7 @@ def __init__(
if not self.use_trained_model:
self.use_trained_model_diff = False
self.update_trained_model = False
+ self.use_memory_diff = False
else:
if use_trained_model_diff is None:
self.use_trained_model_diff = drive_functions.use_trained_model_diff
@@ -154,10 +165,10 @@ def __init__(
self.update_trained_model = drive_functions.update_trained_model
else:
self.update_trained_model = update_trained_model
- if mode is None:
- self.mode = drive_functions.mode
- else:
- self.mode = mode
+ if use_memory_diff is None:
+ self.use_memory_diff = drive_functions.use_memory_diff
+ else:
+ self.use_memory_diff = use_memory_diff
self.acc_delay_step = drive_functions.acc_delay_step
self.steer_delay_step = drive_functions.steer_delay_step
@@ -230,6 +241,8 @@ def __init__(
self.use_trained_model_diff,
)
self.err = 0
+ if self.use_trained_model and drive_functions.use_memory_for_training:
+ print("use_memory_diff", self.use_memory_diff)
if self.use_trained_model:
self.model = torch.load(model_file_name)
polynomial_reg_info = np.load(self.load_polynomial_reg_dir + "/polynomial_reg_info.npz")
@@ -237,30 +250,52 @@ def __init__(
self.b_for_polynomial_reg = polynomial_reg_info["b"]
self.deg = int(polynomial_reg_info["deg"])
self.polynomial_features = PolynomialFeatures(degree=self.deg, include_bias=False)
- transform_model = drive_NN.transform_model_to_c(
- self.model,
- self.A_for_polynomial_reg,
- self.b_for_polynomial_reg,
- self.deg,
- self.acc_time_constant_ctrl,
- self.acc_delay_step,
- self.steer_time_constant_ctrl,
- self.steer_delay_step,
- drive_functions.acc_ctrl_queue_size,
- drive_functions.steer_ctrl_queue_size,
- drive_functions.steer_ctrl_queue_size_core,
- )
- pred = transform_model.pred
- Pred = transform_model.Pred
- self.pred = pred
- self.pred_with_diff = transform_model.pred_with_diff
+ if drive_functions.use_memory_for_training:
+ self.transform_model = drive_NN.transform_model_with_memory_to_c(
+ self.model,
+ self.A_for_polynomial_reg,
+ self.b_for_polynomial_reg,
+ self.deg,
+ self.acc_delay_step,
+ self.steer_delay_step,
+ drive_functions.acc_ctrl_queue_size,
+ drive_functions.steer_ctrl_queue_size,
+ drive_functions.steer_ctrl_queue_size_core,
+ )
+ self.h = np.zeros(self.model.lstm.weight_hh_l0.shape[1])
+ self.c = (self.h).copy()
+ if self.use_memory_diff:
+ self.ilqr.receive_memory_diff(
+ self.transform_model.transform.get_dhc_dx,
+ self.transform_model.transform.get_dhc_dhc,
+ self.transform_model.transform.get_dy_dhc,
+ )
+ else:
+ self.transform_model = drive_NN.transform_model_to_c(
+ self.model,
+ self.A_for_polynomial_reg,
+ self.b_for_polynomial_reg,
+ self.deg,
+ self.acc_delay_step,
+ self.steer_delay_step,
+ drive_functions.acc_ctrl_queue_size,
+ drive_functions.steer_ctrl_queue_size,
+ drive_functions.steer_ctrl_queue_size_core,
+ )
+ self.pred = self.transform_model.pred
+ if drive_functions.reflect_only_poly_diff:
+ self.pred_with_diff = self.transform_model.pred_with_poly_diff
+ elif self.use_memory_diff and drive_functions.use_memory_for_training:
+ self.pred_with_diff = self.transform_model.pred_with_memory_diff
+ else:
+ self.pred_with_diff = self.transform_model.pred_with_diff
self.loss_fn = torch.nn.L1Loss()
self.drive_optimizer = torch.optim.Adam(
params=self.model.parameters(), lr=drive_learning_rate
)
self.F_N_initial_diff = partial(
drive_functions.F_with_model_initial_diff,
- pred=pred,
+ pred=self.transform_model.pred,
i=self.acc_delay_step,
j=self.steer_delay_step,
acc_time_constant_ctrl=self.acc_time_constant_ctrl,
@@ -275,9 +310,17 @@ def __init__(
acc_time_constant_ctrl=self.acc_time_constant_ctrl,
steer_time_constant_ctrl=self.steer_time_constant_ctrl,
)
+ self.F_N_only_state = partial(
+ drive_functions.F_with_model,
+ pred=self.transform_model.pred_only_state,
+ i=self.acc_delay_step,
+ j=self.steer_delay_step,
+ acc_time_constant_ctrl=self.acc_time_constant_ctrl,
+ steer_time_constant_ctrl=self.steer_time_constant_ctrl,
+ )
self.F_N_for_candidates = partial(
drive_functions.F_with_model_for_candidates,
- Pred=Pred,
+ Pred=self.transform_model.Pred,
i=self.acc_delay_step,
j=self.steer_delay_step,
acc_time_constant_ctrl=self.acc_time_constant_ctrl,
@@ -292,6 +335,13 @@ def __init__(
steer_time_constant_ctrl=self.steer_time_constant_ctrl,
)
self.F_N_initial_diff = self.F_N_diff
+ self.F_N_only_state = partial(
+ drive_functions.F_with_history,
+ i=self.acc_delay_step,
+ j=self.steer_delay_step,
+ acc_time_constant_ctrl=self.acc_time_constant_ctrl,
+ steer_time_constant_ctrl=self.steer_time_constant_ctrl,
+ )
self.F_N_for_candidates = partial(
drive_functions.F_with_history_for_candidates,
i=self.acc_delay_step,
@@ -367,12 +417,25 @@ def __init__(
self.x_old = np.empty(6)
+ self.X_queue_for_learning: list[np.ndarray] = []
+ self.acc_input_queue_for_learning: list[np.ndarray] = []
+ self.steer_input_queue_for_learning: list[np.ndarray] = []
+ self.time_stamp_queue_for_learning: list[float] = []
+ self.X_smoothed_queue: list[np.ndarray] = []
+
+ self.pre_X_input_list: list[np.ndarray] = []
+ self.pre_Y_output_list: list[np.ndarray] = []
+
+ self.time_stamp_for_update_lstm: list[float] = []
+ self.X_queue_for_update_lstm: list[np.ndarray] = []
+
def __del__(self):
print("control finished")
def get_optimal_control(
self,
x_current_: np.ndarray,
+ time_stamp: list[float],
X_des_: np.ndarray,
U_des: np.ndarray = np.zeros((drive_functions.N, drive_functions.nu_0)),
) -> np.ndarray:
@@ -387,32 +450,43 @@ def get_optimal_control(
X_des_,
)
self.u_opt = X_des[0, drive_functions.nx_0 + np.arange(drive_functions.nu_0)]
- self.u_old = self.u_opt.copy()
- self.u_old2 = self.u_opt.copy()
self.acc_input_queue = self.u_opt[0] * np.ones(drive_functions.acc_ctrl_queue_size)
self.steer_input_queue = self.u_opt[1] * np.ones(drive_functions.steer_ctrl_queue_size)
- self.nominal_inputs = drive_functions.U_des_from_X_des(self.u_old, X_des, diff_delta)
+ self.nominal_inputs = drive_functions.U_des_from_X_des(self.u_opt, X_des, diff_delta)
self.previous_error = np.zeros(8)
- self.X_queue_for_learning: list[np.ndarray] = []
- self.acc_input_queue_for_learning: list[np.ndarray] = []
- self.steer_input_queue_for_learning: list[np.ndarray] = []
- self.time_stamp_queue_for_learning: list[float] = []
- self.X_smoothed_queue: list[np.ndarray] = []
+ self.X_queue_for_learning.clear()
+ self.acc_input_queue_for_learning.clear()
+ self.steer_input_queue_for_learning.clear()
+ self.time_stamp_queue_for_learning.clear()
+ self.X_smoothed_queue.clear()
+
+ self.pre_X_input_list.clear()
+ self.pre_Y_output_list.clear()
- self.pre_X_input_list: list[np.ndarray] = []
- self.pre_Y_output_list: list[np.ndarray] = []
+ self.time_stamp_for_update_lstm.clear()
+ self.X_queue_for_update_lstm.clear()
+ if self.use_trained_model and drive_functions.use_memory_for_training:
+ self.h = np.zeros(self.model.lstm.weight_hh_l0.shape[1])
+ self.c = (self.h).copy()
self.initialize_X_smoothing_time_stamp = True
+ self.acc_fb_1 = 0.0
+ self.acc_fb_2 = 0.0
+ self.steer_fb_1 = 0.0
+ self.steer_fb_2 = 0.0
+
else:
x_current = drive_functions.transform_yaw_for_x_current(self.x_old, x_current_)
X_des, diff_delta = drive_functions.transform_yaw_for_X_des(
x_current,
X_des_,
)
- self.u_old2 = self.u_old.copy()
- self.u_old = self.u_opt.copy()
-
+ if drive_functions.use_max_curvature:
+ curvature = np.abs(X_des[:, 5]).max() / drive_functions.L
+ else:
+ curvature = np.abs(X_des[:, 5]).mean() / drive_functions.L
+ steer_rate_cost_coef = drive_functions.calc_steer_rate_cost_coef(curvature)
if self.init:
self.X_current_queue = []
self.init = False
@@ -421,6 +495,10 @@ def get_optimal_control(
self.X_current = np.concatenate(
(x_current, self.acc_input_queue[::-1], self.steer_input_queue[::-1])
)
+ self.u_old = np.array([self.acc_input_queue[-1], self.steer_input_queue[-1]])
+ if self.use_trained_model and drive_functions.use_memory_for_training:
+ if len(time_stamp) > 0:
+ self.update_lstm_info(time_stamp[-1])
self.X_current_queue.append(self.X_current.copy())
if len(self.X_current_queue) > drive_functions.mpc_freq:
@@ -429,22 +507,38 @@ def get_optimal_control(
self.nominal_inputs = drive_functions.sg_filter_for_nominal_inputs(self.nominal_inputs)
if self.mode == "mppi" or self.mode == "mppi_ilqr": # Run mppi
self.start_mppi = time.time()
- self.mppi.receive_model(self.F_N_for_candidates)
- self.nominal_inputs, self.u_opt_dot, nominal_traj = (self.mppi).compute_optimal_control(
- self.X_current,
- self.nominal_inputs,
- X_des,
- U_des,
- self.previous_error[:6],
- )
- self.nominal_traj_mppi = nominal_traj.copy()
+ proceed = True
+ for k in range(drive_functions.max_iter_mppi):
+ if not proceed:
+ break
+ if drive_functions.use_memory_for_training and self.use_trained_model:
+ self.transform_model.transform.set_lstm(self.h, self.c)
+ self.transform_model.transform.set_lstm_for_candidate(
+ self.h, self.c, drive_functions.sample_num
+ )
+ self.mppi.receive_model(self.F_N_for_candidates, self.F_N_only_state)
+ self.nominal_inputs, self.u_opt_dot, nominal_traj, self.mppi_candidates, proceed = (
+ self.mppi
+ ).compute_optimal_control(
+ self.X_current,
+ self.nominal_inputs,
+ X_des,
+ U_des,
+ self.previous_error[:6],
+ )
+ self.nominal_traj_mppi = nominal_traj.copy()
self.end_mppi = time.time()
- if self.mode != "mppi":
+ if self.mode == "ilqr" or self.mode == "mppi_ilqr":
self.start_ilqr = time.time()
proceed = True
for k in range(drive_functions.max_iter_ilqr):
if not proceed:
break
+ if drive_functions.use_memory_for_training and self.use_trained_model:
+ self.transform_model.transform.set_lstm(self.h, self.c)
+ self.transform_model.transform.set_lstm_for_candidate(
+ self.h, self.c, drive_functions.max_iter_ls + 1
+ )
self.nominal_inputs = drive_functions.sg_filter_for_nominal_inputs(
self.nominal_inputs
)
@@ -465,6 +559,7 @@ def get_optimal_control(
self.theta_noise,
self.acc_noise,
self.steer_noise,
+ steer_rate_cost_coef,
)
self.nominal_traj_ilqr = nominal_traj.copy()
self.nominal_inputs_ilqr = self.nominal_inputs.copy()
@@ -490,7 +585,41 @@ def get_optimal_control(
self.time_5 = self.ilqr.time_5
self.end_ilqr = time.time()
+ if self.mode == "pure_pursuit":
+ nominal_traj = np.zeros((drive_functions.N + 1, x_current_.shape[0]))
+
+ u_opt = drive_functions.pure_pursuit_control(
+ pos_xy_obs=x_current_[:2],
+ pos_yaw_obs=x_current_[3],
+ longitudinal_vel_obs=x_current_[2],
+ pos_xy_ref=X_des_[0, :2],
+ pos_yaw_ref=X_des_[0, 3],
+ longitudinal_vel_ref=X_des_[0, 2],
+ )
+ self.u_opt_dot = (u_opt - self.u_old) / drive_functions.ctrl_time_step
+
+ if self.mode == "naive_pure_pursuit":
+ nominal_traj = np.zeros((drive_functions.N + 1, x_current_.shape[0]))
+ lookahead_distance = (
+ drive_functions.naive_pure_pursuit_lookahead_coef * x_current_[2]
+ + drive_functions.naive_pure_pursuit_lookahead_intercept
+ )
+ targetIndex = np.abs(
+ ((X_des_[:, :2] - x_current_[:2]) ** 2).sum(axis=1) - lookahead_distance**2
+ ).argmin()
+
+ u_opt = drive_functions.naive_pure_pursuit_control(
+ pos_xy_obs=x_current_[:2],
+ pos_yaw_obs=x_current_[3],
+ longitudinal_vel_obs=x_current_[2],
+ pos_xy_ref_target=X_des_[targetIndex, :2],
+ longitudinal_vel_ref_nearest=X_des_[0, 2],
+ )
+ self.u_opt_dot = (u_opt - self.u_old) / drive_functions.ctrl_time_step
+
if self.use_trained_model:
+ if drive_functions.use_memory_for_training:
+ self.transform_model.transform.set_lstm(self.h, self.c)
self.previous_error = drive_functions.error_decay * self.previous_error + (
1 - drive_functions.error_decay
) * self.pred(self.X_current)
@@ -505,6 +634,41 @@ def get_optimal_control(
acc_lim,
acc_rate_lim,
) = drive_functions.calc_limits(x_current[2], x_current[4], x_current[5])
+ if not self.initialize_input_queue and (
+ self.mode != "pure_pursuit" and self.mode != "naive_pure_pursuit"
+ ):
+ self.acc_fb_1, self.acc_fb_2 = np.clip(
+ drive_functions.acc_prediction_error_compensation(
+ self.X_current,
+ self.X_old,
+ self.u_old,
+ self.previous_error,
+ self.acc_fb_1,
+ self.acc_fb_2,
+ self.acc_time_stamp - self.acc_time_stamp_old,
+ ),
+ -drive_functions.max_error_acc,
+ drive_functions.max_error_acc,
+ )
+ self.steer_fb_1, self.steer_fb_2 = np.clip(
+ drive_functions.steer_prediction_error_compensation(
+ self.X_current,
+ self.X_old,
+ self.u_old,
+ self.previous_error,
+ self.steer_fb_1,
+ self.steer_fb_2,
+ self.steer_time_stamp - self.steer_time_stamp_old,
+ ),
+ -drive_functions.max_error_steer,
+ drive_functions.max_error_steer,
+ )
+ self.u_opt[0] += drive_functions.acc_fb_gain * (
+ 2 * self.acc_fb_1 - drive_functions.acc_fb_sec_order_ratio * self.acc_fb_2
+ )
+ self.u_opt[1] += drive_functions.steer_fb_gain * (
+ 2 * self.steer_fb_1 - drive_functions.steer_fb_sec_order_ratio * self.steer_fb_2
+ )
self.u_opt = drive_functions.u_cut_off(
self.u_opt,
@@ -517,10 +681,13 @@ def get_optimal_control(
)
self.initialize_input_queue = False
- self.X_des_history.append(X_des[0])
+ self.X_des_history.append(X_des_[0])
self.initial_guess_for_debug = (self.nominal_inputs).copy()
self.x_old = x_current
+ self.X_old = (self.X_current).copy()
+ self.acc_time_stamp_old = self.acc_time_stamp
+ self.steer_time_stamp_old = self.steer_time_stamp
return self.u_opt
def update_input_queue(
@@ -529,12 +696,16 @@ def update_input_queue(
acc_history: list[float],
steer_history: list[float],
x_current_: np.ndarray,
+ acc_time_stamp: float,
+ steer_time_stamp: float,
) -> None:
"""Receives the history of the acceleration and steer inputs and their time stamps.
And interpolates them to be the history of the time steps used by the controller.
The return value can be passed to get_optimal_control.
"""
+ self.acc_time_stamp = acc_time_stamp
+ self.steer_time_stamp = steer_time_stamp
if not self.initialize_input_queue:
if len(time_stamp) == 1:
self.acc_input_queue[-1] = acc_history[0]
@@ -849,6 +1020,39 @@ def update_input_queue(
self.steer_input_queue_for_learning.pop(0)
self.time_stamp_queue_for_learning.pop(0)
+ def update_lstm_info(self, time_stamp):
+ self.transform_model.transform.set_lstm(0 * self.h, 0 * self.c)
+ self.time_stamp_for_update_lstm.append(time_stamp)
+ self.X_queue_for_update_lstm.append(self.X_current)
+ if len(self.X_queue_for_update_lstm) > 1:
+ while self.time_stamp_for_update_lstm[-1] - self.time_stamp_for_update_lstm[
+ 1
+ ] > drive_functions.mpc_time_step * (drive_functions.N + 1):
+ self.time_stamp_for_update_lstm.pop(0)
+ self.X_queue_for_update_lstm.pop(0)
+ if (
+ self.time_stamp_for_update_lstm[-1] - self.time_stamp_for_update_lstm[0]
+ > drive_functions.mpc_time_step
+ ):
+ horizon_num = min(
+ int(
+ (self.time_stamp_for_update_lstm[-1] - self.time_stamp_for_update_lstm[0])
+ / drive_functions.mpc_time_step
+ ),
+ drive_functions.N,
+ )
+ time_stamp_interp = (
+ self.time_stamp_for_update_lstm[-1]
+ - drive_functions.mpc_time_step * np.arange(1, horizon_num + 1)[::-1]
+ )
+ X_interp = scipy.interpolate.interp1d(
+ np.array(self.time_stamp_for_update_lstm),
+ np.array(self.X_queue_for_update_lstm).T,
+ )(time_stamp_interp)
+ self.transform_model.transform.update_memory_by_state_history(X_interp)
+ self.h = self.transform_model.transform.get_h()
+ self.c = self.transform_model.transform.get_c()
+
def update_input_queue_and_get_optimal_control(
self,
time_stamp,
@@ -857,10 +1061,14 @@ def update_input_queue_and_get_optimal_control(
x_current_,
X_des_,
U_des=np.zeros((drive_functions.N, drive_functions.nu_0)),
+ acc_time_stamp=0.0,
+ steer_time_stamp=0.0,
):
"""Run update_input_queue and get_optimal_control all at once."""
- self.update_input_queue(time_stamp, acc_history, steer_history, x_current_)
- u_opt = self.get_optimal_control(x_current_, X_des_, U_des)
+ self.update_input_queue(
+ time_stamp, acc_history, steer_history, x_current_, acc_time_stamp, steer_time_stamp
+ )
+ u_opt = self.get_optimal_control(x_current_, time_stamp, X_des_, U_des)
return u_opt
def update_model(self):
@@ -888,25 +1096,27 @@ def update_model(self):
loss.backward()
self.drive_optimizer.step()
- transform_model = drive_NN.transform_model_to_c(
+ self.transform_model = drive_NN.transform_model_to_c(
self.model,
self.A_for_polynomial_reg,
self.b_for_polynomial_reg,
self.deg,
- self.acc_time_constant_ctrl,
self.acc_delay_step,
- self.steer_time_constant_ctrl,
self.steer_delay_step,
drive_functions.acc_ctrl_queue_size,
drive_functions.steer_ctrl_queue_size,
drive_functions.steer_ctrl_queue_size_core,
)
- self.pred = transform_model.pred
- self.Pred = transform_model.Pred
- self.pred_with_diff = transform_model.pred_with_diff
+ self.pred = self.transform_model.pred
+ if drive_functions.reflect_only_poly_diff:
+ self.pred_with_diff = self.transform_model.pred_with_poly_diff
+ elif self.use_memory_diff and drive_functions.use_memory_for_training:
+ self.pred_with_diff = self.transform_model.pred_with_memory_diff
+ else:
+ self.pred_with_diff = self.transform_model.pred_with_diff
self.F_N_initial_diff = partial(
drive_functions.F_with_model_initial_diff,
- pred=self.pred,
+ pred=self.transform_model.pred,
i=self.acc_delay_step,
j=self.steer_delay_step,
acc_time_constant_ctrl=self.acc_time_constant_ctrl,
@@ -920,9 +1130,17 @@ def update_model(self):
acc_time_constant_ctrl=self.acc_time_constant_ctrl,
steer_time_constant_ctrl=self.steer_time_constant_ctrl,
)
+ self.F_N_only_state = partial(
+ drive_functions.F_with_model,
+ pred=self.transform_model.pred_only_state,
+ i=self.acc_delay_step,
+ j=self.steer_delay_step,
+ acc_time_constant_ctrl=self.acc_time_constant_ctrl,
+ steer_time_constant_ctrl=self.steer_time_constant_ctrl,
+ )
self.F_N_for_candidates = partial(
drive_functions.F_with_model_for_candidates,
- Pred=self.Pred,
+ Pred=self.transform_model.Pred,
i=self.acc_delay_step,
j=self.steer_delay_step,
acc_time_constant_ctrl=self.acc_time_constant_ctrl,
diff --git a/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_functions.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_functions.py
index bd6cf28c0b629..81ad4f8b38790 100644
--- a/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_functions.py
+++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_functions.py
@@ -116,53 +116,40 @@
acc_time_constant = float(nominal_param["nominal_parameter"]["acceleration"]["acc_time_constant"])
steer_time_delay = float(nominal_param["nominal_parameter"]["steering"]["steer_time_delay"])
-steer_delay_step = min(
- round(steer_time_delay / ctrl_time_step), steer_ctrl_queue_size_core - mpc_freq
-)
+steer_delay_step = min(round(steer_time_delay / ctrl_time_step), steer_ctrl_queue_size - mpc_freq)
steer_time_constant = float(nominal_param["nominal_parameter"]["steering"]["steer_time_constant"])
-min_steer_rate_transform_for_start = float(
- mpc_param["mpc_parameter"]["cost_parameters"]["min_steer_rate_transform_for_start"]
-)
-power_steer_rate_transform_for_start = int(
- mpc_param["mpc_parameter"]["cost_parameters"]["power_steer_rate_transform_for_start"]
+
+vel_steer_cost_coef_table = np.array(
+ mpc_param["mpc_parameter"]["cost_parameters"]["vel_steer_cost_coef_table"], dtype=float
)
-coef_steer_rate_transform_for_start = float(
- mpc_param["mpc_parameter"]["cost_parameters"]["coef_steer_rate_transform_for_start"]
+vel_steer_table = np.array(
+ mpc_param["mpc_parameter"]["cost_parameters"]["vel_steer_table"], dtype=float
)
-min_tighten_steer_rate = float(
- mpc_param["mpc_parameter"]["to_be_deprecated"]["min_tighten_steer_rate"]
-)
-power_tighten_steer_rate_by_lateral_error = int(
- mpc_param["mpc_parameter"]["to_be_deprecated"]["power_tighten_steer_rate_by_lateral_error"]
+
+lateral_cost_coef_table = np.array(
+ mpc_param["mpc_parameter"]["cost_parameters"]["lateral_cost_coef_table"], dtype=float
)
-threshold_tighten_steer_rate_by_lateral_error = float(
- mpc_param["mpc_parameter"]["to_be_deprecated"]["threshold_tighten_steer_rate_by_lateral_error"]
+lateral_error_table = np.array(
+ mpc_param["mpc_parameter"]["cost_parameters"]["lateral_error_table"], dtype=float
)
-power_tighten_steer_rate_by_yaw_error = int(
- mpc_param["mpc_parameter"]["to_be_deprecated"]["power_tighten_steer_rate_by_yaw_error"]
+yaw_cost_coef_table = np.array(
+ mpc_param["mpc_parameter"]["cost_parameters"]["yaw_cost_coef_table"], dtype=float
)
-threshold_tighten_steer_rate_by_yaw_error = float(
- mpc_param["mpc_parameter"]["to_be_deprecated"]["threshold_tighten_steer_rate_by_yaw_error"]
+yaw_error_table = np.array(
+ mpc_param["mpc_parameter"]["cost_parameters"]["yaw_error_table"], dtype=float
)
-tighten_horizon = int(mpc_param["mpc_parameter"]["to_be_deprecated"]["tighten_horizon"])
-min_loose_lateral_cost = float(
- mpc_param["mpc_parameter"]["cost_parameters"]["min_loose_lateral_cost"]
-)
-power_loose_lateral_cost = int(
- mpc_param["mpc_parameter"]["cost_parameters"]["power_loose_lateral_cost"]
+
+steer_rate_cost_table = np.array(
+ mpc_param["mpc_parameter"]["cost_parameters"]["steer_rate_cost_table"], dtype=float
)
-threshold_loose_lateral_cost = float(
- mpc_param["mpc_parameter"]["cost_parameters"]["threshold_loose_lateral_cost"]
+curvature_table = np.array(
+ mpc_param["mpc_parameter"]["cost_parameters"]["curvature_table"], dtype=float
)
-min_loose_yaw_cost = float(mpc_param["mpc_parameter"]["cost_parameters"]["min_loose_yaw_cost"])
-power_loose_yaw_cost = int(mpc_param["mpc_parameter"]["cost_parameters"]["power_loose_yaw_cost"])
-threshold_loose_yaw_cost = float(
- mpc_param["mpc_parameter"]["cost_parameters"]["threshold_loose_yaw_cost"]
-)
+use_max_curvature = str(mpc_param["mpc_parameter"]["cost_parameters"]["use_max_curvature"])
use_sg_for_nominal_inputs = bool(
mpc_param["mpc_parameter"]["preprocessing"]["use_sg_for_nominal_inputs"]
@@ -173,6 +160,20 @@
sg_window_size_for_nominal_inputs = int(
mpc_param["mpc_parameter"]["preprocessing"]["sg_window_size_for_nominal_inputs"]
)
+
+acc_fb_decay = float(mpc_param["mpc_parameter"]["compensation"]["acc_fb_decay"])
+acc_fb_gain = float(mpc_param["mpc_parameter"]["compensation"]["acc_fb_gain"])
+acc_fb_sec_order_ratio = float(mpc_param["mpc_parameter"]["compensation"]["acc_fb_sec_order_ratio"])
+max_error_acc = float(mpc_param["mpc_parameter"]["compensation"]["max_error_acc"])
+
+steer_fb_decay = float(mpc_param["mpc_parameter"]["compensation"]["steer_fb_decay"])
+steer_fb_gain = float(mpc_param["mpc_parameter"]["compensation"]["steer_fb_gain"])
+steer_fb_sec_order_ratio = float(
+ mpc_param["mpc_parameter"]["compensation"]["steer_fb_sec_order_ratio"]
+)
+max_error_steer = float(mpc_param["mpc_parameter"]["compensation"]["max_error_steer"])
+
+
trained_model_param_path = (
package_path["path"] + "/autoware_smart_mpc_trajectory_follower/param/trained_model_param.yaml"
)
@@ -181,6 +182,16 @@
use_trained_model_diff = bool(
trained_model_param["trained_model_parameter"]["control_application"]["use_trained_model_diff"]
)
+minimum_steer_diff = float(
+ trained_model_param["trained_model_parameter"]["control_application"]["minimum_steer_diff"]
+)
+use_memory_diff = bool(
+ trained_model_param["trained_model_parameter"]["memory_for_training"]["use_memory_diff"]
+)
+
+reflect_only_poly_diff = bool(
+ trained_model_param["trained_model_parameter"]["control_application"]["reflect_only_poly_diff"]
+)
use_sg_for_trained_model_diff = bool(
trained_model_param["trained_model_parameter"]["control_application"][
"use_sg_for_trained_model_diff"
@@ -197,6 +208,18 @@
]
)
+use_sg_for_memory_diff = bool(
+ trained_model_param["trained_model_parameter"]["memory_for_training"]["use_sg_for_memory_diff"]
+)
+sg_deg_for_trained_model_diff = int(
+ trained_model_param["trained_model_parameter"]["memory_for_training"]["sg_deg_for_memory_diff"]
+)
+sg_window_size_for_memory_diff = int(
+ trained_model_param["trained_model_parameter"]["memory_for_training"][
+ "sg_window_size_for_memory_diff"
+ ]
+)
+
use_sg_for_noise = bool(
trained_model_param["trained_model_parameter"]["control_application"]["use_sg_for_noise"]
)
@@ -209,6 +232,9 @@
]
)
+use_memory_for_training = bool(
+ trained_model_param["trained_model_parameter"]["memory_for_training"]["use_memory_for_training"]
+)
load_dir = os.environ["HOME"]
save_dir = os.environ["HOME"] # +"/autoware"
@@ -282,6 +308,66 @@
)
+vel_normalize = float(trained_model_param["trained_model_parameter"]["normalize"]["vel_normalize"])
+acc_normalize = float(trained_model_param["trained_model_parameter"]["normalize"]["acc_normalize"])
+steer_normalize = float(
+ trained_model_param["trained_model_parameter"]["normalize"]["steer_normalize"]
+)
+
+NN_x_weight = float(trained_model_param["trained_model_parameter"]["weight"]["NN_x_weight"])
+NN_y_weight = float(trained_model_param["trained_model_parameter"]["weight"]["NN_y_weight"])
+NN_v_weight = float(trained_model_param["trained_model_parameter"]["weight"]["NN_v_weight"])
+NN_yaw_weight = float(trained_model_param["trained_model_parameter"]["weight"]["NN_yaw_weight"])
+NN_acc_weight = float(trained_model_param["trained_model_parameter"]["weight"]["NN_acc_weight"])
+NN_steer_weight = float(trained_model_param["trained_model_parameter"]["weight"]["NN_steer_weight"])
+
+NN_x_weight_diff = float(
+ trained_model_param["trained_model_parameter"]["weight"]["NN_x_weight_diff"]
+)
+NN_y_weight_diff = float(
+ trained_model_param["trained_model_parameter"]["weight"]["NN_y_weight_diff"]
+)
+NN_v_weight_diff = float(
+ trained_model_param["trained_model_parameter"]["weight"]["NN_v_weight_diff"]
+)
+NN_yaw_weight_diff = float(
+ trained_model_param["trained_model_parameter"]["weight"]["NN_yaw_weight_diff"]
+)
+NN_acc_weight_diff = float(
+ trained_model_param["trained_model_parameter"]["weight"]["NN_acc_weight_diff"]
+)
+NN_steer_weight_diff = float(
+ trained_model_param["trained_model_parameter"]["weight"]["NN_steer_weight_diff"]
+)
+
+NN_x_weight_two_diff = float(
+ trained_model_param["trained_model_parameter"]["weight"]["NN_x_weight_two_diff"]
+)
+NN_y_weight_two_diff = float(
+ trained_model_param["trained_model_parameter"]["weight"]["NN_y_weight_two_diff"]
+)
+NN_v_weight_two_diff = float(
+ trained_model_param["trained_model_parameter"]["weight"]["NN_v_weight_two_diff"]
+)
+NN_yaw_weight_two_diff = float(
+ trained_model_param["trained_model_parameter"]["weight"]["NN_yaw_weight_two_diff"]
+)
+NN_acc_weight_two_diff = float(
+ trained_model_param["trained_model_parameter"]["weight"]["NN_acc_weight_two_diff"]
+)
+NN_steer_weight_two_diff = float(
+ trained_model_param["trained_model_parameter"]["weight"]["NN_steer_weight_two_diff"]
+)
+
+finalize_x_weight = float(
+ trained_model_param["trained_model_parameter"]["weight"]["finalize_x_weight"]
+)
+finalize_y_weight = float(
+ trained_model_param["trained_model_parameter"]["weight"]["finalize_y_weight"]
+)
+finalize_v_weight = float(
+ trained_model_param["trained_model_parameter"]["weight"]["finalize_v_weight"]
+)
# upper limit of input
@@ -293,40 +379,52 @@
break
control_dir_path += "/"
-limit_yaml_path = None
-for curDir, dirs, files in os.walk(control_dir_path):
- for name in files:
- if name == "vehicle_cmd_gate.param.yaml":
- if curDir.split("/")[-2] == "autoware_vehicle_cmd_gate":
- limit_yaml_path = curDir + "/" + name
- break
-
-limit_params = None
-if limit_yaml_path is not None:
- with open(limit_yaml_path, "r") as yml:
- limit_params = yaml.safe_load(yml)
-else:
- print("Error: limit_yaml_path is None")
+read_limit_file = bool(mpc_param["mpc_parameter"]["limit"]["read_limit_file"])
+if read_limit_file:
+ limit_yaml_path = None
+ for curDir, dirs, files in os.walk(control_dir_path):
+ for name in files:
+ if name == "vehicle_cmd_gate.param.yaml":
+ if curDir.split("/")[-2] == "autoware_vehicle_cmd_gate":
+ limit_yaml_path = curDir + "/" + name
+ break
+
+ limit_params = None
+ if limit_yaml_path is not None:
+ with open(limit_yaml_path, "r") as yml:
+ limit_params = yaml.safe_load(yml)
+ else:
+ print("Error: limit_yaml_path is None")
-if limit_params is not None:
- reference_speed_points = np.array(
- limit_params["/**"]["ros__parameters"]["nominal"]["reference_speed_points"]
- )
- steer_lim_points = np.array(limit_params["/**"]["ros__parameters"]["nominal"]["steer_lim"])
- steer_rate_lim_points = np.array(
- limit_params["/**"]["ros__parameters"]["nominal"]["steer_rate_lim"]
- )
- acc_lim_points = np.array(limit_params["/**"]["ros__parameters"]["nominal"]["lon_acc_lim"])
- acc_rate_lim_points = np.array(
- limit_params["/**"]["ros__parameters"]["nominal"]["lon_jerk_lim"]
- )
- lat_acc_lim_points = np.array(limit_params["/**"]["ros__parameters"]["nominal"]["lat_acc_lim"])
- lat_jerk_lim_points = np.array(
- limit_params["/**"]["ros__parameters"]["nominal"]["lat_jerk_lim"]
- )
+ if limit_params is not None:
+ reference_speed_points = np.array(
+ limit_params["/**"]["ros__parameters"]["nominal"]["reference_speed_points"]
+ )
+ steer_lim_points = np.array(limit_params["/**"]["ros__parameters"]["nominal"]["steer_lim"])
+ steer_rate_lim_points = np.array(
+ limit_params["/**"]["ros__parameters"]["nominal"]["steer_rate_lim"]
+ )
+ acc_lim_points = np.array(limit_params["/**"]["ros__parameters"]["nominal"]["lon_acc_lim"])
+ acc_rate_lim_points = np.array(
+ limit_params["/**"]["ros__parameters"]["nominal"]["lon_jerk_lim"]
+ )
+ lat_acc_lim_points = np.array(
+ limit_params["/**"]["ros__parameters"]["nominal"]["lat_acc_lim"]
+ )
+ lat_jerk_lim_points = np.array(
+ limit_params["/**"]["ros__parameters"]["nominal"]["lat_jerk_lim"]
+ )
+ else:
+ print("Error: limit_params is None")
+ sys.exit(1)
else:
- print("Error: limit_params is None")
- sys.exit(1)
+ reference_speed_points = np.array([20.0, 30.0])
+ steer_lim_points = np.array([10.0, 10.0])
+ steer_rate_lim_points = np.array([10.0, 10.0])
+ acc_lim_points = np.array([20.0, 20.0])
+ acc_rate_lim_points = np.array([20.0, 20.0])
+ lat_acc_lim_points = np.array([20.0, 20.0])
+ lat_jerk_lim_points = np.array([20.0, 20.0])
@njit(cache=True, fastmath=True)
@@ -413,6 +511,55 @@ def calc_limits(
)
+@njit(cache=True, fastmath=True)
+def calc_steer_rate_cost_coef(curvature: float) -> float:
+ """Calc steer rate cost coefficient."""
+ interval_index = 0
+ for i in range(curvature_table.shape[0]):
+ if curvature > curvature_table[i]:
+ interval_index += 1
+ else:
+ break
+ if interval_index == 0:
+ steer_rate_cost_coef = steer_rate_cost_table[0]
+
+ elif interval_index == steer_rate_cost_table.shape[0]:
+ steer_rate_cost_coef = steer_rate_cost_table[-1]
+
+ else:
+ r = (curvature - curvature_table[interval_index - 1]) / (
+ curvature_table[interval_index] - curvature_table[interval_index - 1]
+ )
+ steer_rate_cost_coef = (1 - r) * steer_rate_cost_table[
+ interval_index - 1
+ ] + r * steer_rate_cost_table[interval_index]
+
+ return steer_rate_cost_coef
+
+
+@njit(cache=True, fastmath=True)
+def calc_table_value(x: float, table_domain, table_target) -> float:
+ interval_index = 0
+ for i in range(table_domain.shape[0]):
+ if x > table_domain[i]:
+ interval_index += 1
+ else:
+ break
+ if interval_index == 0:
+ target_val = table_target[0]
+
+ elif interval_index == table_target.shape[0]:
+ target_val = table_target[-1]
+
+ else:
+ r = (x - table_domain[interval_index - 1]) / (
+ table_domain[interval_index] - table_domain[interval_index - 1]
+ )
+ target_val = (1 - r) * table_target[interval_index - 1] + r * table_target[interval_index]
+
+ return target_val
+
+
@njit(cache=True, fastmath=True)
def transform_yaw(yaw_old: float, yaw_current_: float) -> float:
"""Transform the yaw angle so that the difference in yaw angle is less than or equal to π."""
@@ -431,7 +578,7 @@ def transform_yaw_for_x_current(x_old: np.ndarray, x_current_: np.ndarray) -> np
return x_current
-def transform_yaw_for_X_des(x_current: np.ndarray, X_des_: np.ndarray) -> np.ndarray:
+def transform_yaw_for_X_des(x_current: np.ndarray, X_des_: np.ndarray) -> tuple[np.ndarray, float]:
"""Transform the yaw angle with respect to the target trajectory.
X_des[0] is set to the current state.
@@ -506,7 +653,11 @@ def calc_maximum_trajectory_error(traj: np.ndarray, X_des: np.ndarray) -> float:
@njit(cache=False, fastmath=True)
def transform_Q_R(
- X_des: np.ndarray, U_des: np.ndarray, nominal_traj: np.ndarray, nominal_input: np.ndarray
+ X_des: np.ndarray,
+ U_des: np.ndarray,
+ nominal_traj: np.ndarray,
+ nominal_input: np.ndarray,
+ steer_rate_coef: float,
) -> tuple:
"""Calculate the MPC cost weight matrix from the current predicted trajectory and target trajectory.
@@ -540,43 +691,17 @@ def transform_Q_R(
lateral_error = np.abs((Rot.T @ (X_des[i, :2] - x_current[:2]))[1])
yaw_error = np.abs(X_des[i, 3] - x_current[3])
- cost_tr_steer_rate_by_error = max(
- min_tighten_steer_rate,
- max(
- (lateral_error / threshold_tighten_steer_rate_by_lateral_error)
- ** power_tighten_steer_rate_by_lateral_error,
- (yaw_error / threshold_tighten_steer_rate_by_yaw_error)
- ** power_tighten_steer_rate_by_yaw_error,
- ),
- )
- cost_tr_steer_rate = 1 / (
- min(
- max(
- min_steer_rate_transform_for_start,
- (coef_steer_rate_transform_for_start * np.abs(v))
- ** power_steer_rate_transform_for_start,
- ),
- 1.0,
- )
+
+ cost_tr_steer_rate = 1 / calc_table_value(
+ np.abs(v), vel_steer_table, vel_steer_cost_coef_table
)
- if i >= tighten_horizon:
- cost_tr_steer_rate = max(cost_tr_steer_rate, 1 / cost_tr_steer_rate_by_error)
+
if i in timing_Q_c or i == X_des.shape[0] - 1:
- lateral_cost_coef = min(
- max(
- min_loose_lateral_cost,
- (lateral_error / threshold_loose_lateral_cost) ** power_loose_lateral_cost,
- ),
- 1.0,
+ lateral_cost_coef = calc_table_value(
+ lateral_error, lateral_error_table, lateral_cost_coef_table
)
Q_[1, 1] = lateral_cost_coef * Q_[1, 1]
- yaw_cost_coef = min(
- max(
- min_loose_yaw_cost,
- (yaw_error / threshold_loose_yaw_cost) ** power_loose_yaw_cost,
- ),
- 1.0,
- )
+ yaw_cost_coef = calc_table_value(yaw_error, yaw_error_table, yaw_cost_coef_table)
Q_[3, 3] = yaw_cost_coef * Q_[3, 3]
Q_[:2, :2] = Rot @ Q_[:2, :2] @ Rot.T
@@ -605,6 +730,7 @@ def transform_Q_R(
steer_lim_center[i] = -steer_lim
if i != X_des.shape[0] - 1:
R_ = R.copy()
+ R_[1, 1] = steer_rate_coef * R_[1, 1]
if acc >= 0:
R_[1, 1] = cost_tr_steer_rate * R_[1, 1]
R_total[i] = R_
@@ -639,7 +765,12 @@ def transform_Q_R(
@njit(cache=False, fastmath=True)
def calc_cost(
- X_des: np.ndarray, U_des: np.ndarray, Traj: np.ndarray, Inputs: np.ndarray, n: int
+ X_des: np.ndarray,
+ U_des: np.ndarray,
+ Traj: np.ndarray,
+ Inputs: np.ndarray,
+ n: int,
+ steer_rate_coef: float,
) -> np.ndarray:
"""Calculate the MPC cost of the nth horizon from several candidate predicted trajectories and target trajectories."""
Cost = np.zeros(Traj.shape[0])
@@ -661,43 +792,17 @@ def calc_cost(
Qi = Q_.copy()
lateral_error = np.abs((Rot.T @ (X_des[:2] - x_current[:2]))[1])
yaw_error = np.abs(X_des[3] - x_current[3])
- cost_tr_steer_rate_by_error = max(
- min_tighten_steer_rate,
- max(
- (lateral_error / threshold_tighten_steer_rate_by_lateral_error)
- ** power_tighten_steer_rate_by_lateral_error,
- (yaw_error / threshold_tighten_steer_rate_by_yaw_error)
- ** power_tighten_steer_rate_by_yaw_error,
- ),
- )
- cost_tr_steer_rate = 1 / (
- min(
- max(
- min_steer_rate_transform_for_start,
- (coef_steer_rate_transform_for_start * np.abs(v))
- ** power_steer_rate_transform_for_start,
- ),
- 1.0,
- )
+
+ cost_tr_steer_rate = 1 / calc_table_value(
+ np.abs(v), vel_steer_table, vel_steer_cost_coef_table
)
- if n >= tighten_horizon:
- cost_tr_steer_rate = max(cost_tr_steer_rate, 1 / cost_tr_steer_rate_by_error)
+
if n in timing_Q_c or n == N:
- lateral_cost_coef = min(
- max(
- min_loose_lateral_cost,
- (lateral_error / threshold_loose_lateral_cost) ** power_loose_lateral_cost,
- ),
- 1.0,
+ lateral_cost_coef = calc_table_value(
+ lateral_error, lateral_error_table, lateral_cost_coef_table
)
Qi[1, 1] = lateral_cost_coef * Qi[1, 1]
- yaw_cost_coef = min(
- max(
- min_loose_yaw_cost,
- (yaw_error / threshold_loose_yaw_cost) ** power_loose_yaw_cost,
- ),
- 1.0,
- )
+ yaw_cost_coef = calc_table_value(yaw_error, yaw_error_table, yaw_cost_coef_table)
Qi[3, 3] = yaw_cost_coef * Qi[3, 3]
Qi[:2, :2] = Rot @ Qi[:2, :2] @ Rot.T
Cost[i] += 0.5 * np.dot(Qi @ (x_current - X_des), x_current - X_des)
@@ -736,6 +841,8 @@ def calc_cost(
if n != N:
R_ = R.copy()
+ R_[1, 1] = steer_rate_coef * R_[1, 1]
+
if acc >= 0:
R_[1, 1] = cost_tr_steer_rate * R_[1, 1]
@@ -795,22 +902,13 @@ def calc_cost_only_for_states(X_des: np.ndarray, Traj: np.ndarray, n: int) -> np
Qi = Q_.copy()
if n in timing_Q_c or n == N:
lateral_error = np.abs((Rot.T @ (X_des[:2] - x_current[:2]))[1])
- lateral_cost_coef = min(
- max(
- min_loose_lateral_cost,
- (lateral_error / threshold_loose_lateral_cost) ** power_loose_lateral_cost,
- ),
- 1.0,
+ lateral_cost_coef = calc_table_value(
+ lateral_error, lateral_error_table, lateral_cost_coef_table
)
Qi[1, 1] = lateral_cost_coef * Qi[1, 1]
yaw_error = np.abs(X_des[3] - x_current[3])
- yaw_cost_coef = min(
- max(
- min_loose_yaw_cost,
- (yaw_error / threshold_loose_yaw_cost) ** power_loose_yaw_cost,
- ),
- 1.0,
- )
+ yaw_cost_coef = calc_table_value(yaw_error, yaw_error_table, yaw_cost_coef_table)
+
Qi[3, 3] = yaw_cost_coef * Qi[3, 3]
Qi[:2, :2] = Rot @ Qi[:2, :2] @ Rot.T
Cost[i] += 0.5 * np.dot(Qi @ (x_current - X_des), x_current - X_des)
@@ -1077,12 +1175,14 @@ def F_multiple_for_candidates(
def F_with_history(
states: np.ndarray,
inputs: np.ndarray,
+ previous_error=None,
+ k: int = 0,
i: int = acc_delay_step,
j: int = steer_delay_step,
acc_time_constant_ctrl: float = acc_time_constant,
steer_time_constant_ctrl: float = steer_time_constant,
steer_dead_band_for_ctrl=steer_dead_band_for_ctrl,
-) -> np.ndarray:
+) -> tuple[np.ndarray, None]:
"""Integrate up to the MPC time width according to the nominal model.
This includes the history of the input to the state.
@@ -1097,12 +1197,12 @@ def F_with_history(
] = states[
nx_0 + acc_ctrl_queue_size : nx_0 + acc_ctrl_queue_size + steer_ctrl_queue_size - mpc_freq
].copy()
- for k in range(mpc_freq):
- states_next[nx_0 + mpc_freq - k - 1] = (
- states_next[nx_0 + mpc_freq - k] + ctrl_time_step * inputs[0]
+ for index in range(mpc_freq):
+ states_next[nx_0 + mpc_freq - index - 1] = (
+ states_next[nx_0 + mpc_freq - index] + ctrl_time_step * inputs[0]
)
- states_next[nx_0 + acc_ctrl_queue_size + mpc_freq - k - 1] = (
- states_next[nx_0 + acc_ctrl_queue_size + mpc_freq - k] + ctrl_time_step * inputs[1]
+ states_next[nx_0 + acc_ctrl_queue_size + mpc_freq - index - 1] = (
+ states_next[nx_0 + acc_ctrl_queue_size + mpc_freq - index] + ctrl_time_step * inputs[1]
)
actual_inputs = np.zeros((mpc_freq, inputs.shape[0]))
@@ -1117,14 +1217,14 @@ def F_with_history(
steer_dead_band_for_ctrl,
)
- return states_next
+ return states_next, None
def F_with_history_and_diff(
states: np.ndarray,
inputs: np.ndarray,
previous_error: None = None,
- init: None = None,
+ k: int = 0,
i: int = acc_delay_step,
j: int = steer_delay_step,
acc_time_constant_ctrl: float = acc_time_constant,
@@ -1156,18 +1256,18 @@ def F_with_history_and_diff(
nx_0 + acc_ctrl_queue_size : nx_0 + acc_ctrl_queue_size + steer_ctrl_queue_size - mpc_freq,
] = np.eye(steer_ctrl_queue_size - mpc_freq)
- for k in range(mpc_freq):
- states_next[nx_0 + mpc_freq - k - 1] = (
- states_next[nx_0 + mpc_freq - k] + ctrl_time_step * inputs[0]
+ for index in range(mpc_freq):
+ states_next[nx_0 + mpc_freq - index - 1] = (
+ states_next[nx_0 + mpc_freq - index] + ctrl_time_step * inputs[0]
)
- states_next[nx_0 + acc_ctrl_queue_size + mpc_freq - k - 1] = (
- states_next[nx_0 + acc_ctrl_queue_size + mpc_freq - k] + ctrl_time_step * inputs[1]
+ states_next[nx_0 + acc_ctrl_queue_size + mpc_freq - index - 1] = (
+ states_next[nx_0 + acc_ctrl_queue_size + mpc_freq - index] + ctrl_time_step * inputs[1]
)
- dF_dx[nx_0 + mpc_freq - k - 1, nx_0] = 1
- dF_dx[nx_0 + acc_ctrl_queue_size + mpc_freq - k - 1, nx_0 + acc_ctrl_queue_size] = 1
+ dF_dx[nx_0 + mpc_freq - index - 1, nx_0] = 1
+ dF_dx[nx_0 + acc_ctrl_queue_size + mpc_freq - index - 1, nx_0 + acc_ctrl_queue_size] = 1
- dF_du[nx_0 + mpc_freq - k - 1, 0] = (k + 1) * ctrl_time_step
- dF_du[nx_0 + acc_ctrl_queue_size + mpc_freq - k - 1, 1] = (k + 1) * ctrl_time_step
+ dF_du[nx_0 + mpc_freq - index - 1, 0] = (index + 1) * ctrl_time_step
+ dF_du[nx_0 + acc_ctrl_queue_size + mpc_freq - index - 1, 1] = (index + 1) * ctrl_time_step
actual_inputs = np.zeros((mpc_freq, inputs.shape[0]))
actual_inputs[:, 0] = states_next[nx_0 + i + np.arange(mpc_freq)[::-1]].copy()
@@ -1193,7 +1293,7 @@ def F_with_history_for_candidates(
States: np.ndarray,
Inputs: np.ndarray,
Previous_error=None,
- init=None,
+ k=0,
i: int = acc_delay_step,
j: int = steer_delay_step,
acc_time_constant_ctrl: float = acc_time_constant,
@@ -1215,12 +1315,12 @@ def F_with_history_for_candidates(
:,
nx_0 + acc_ctrl_queue_size : nx_0 + acc_ctrl_queue_size + steer_ctrl_queue_size - mpc_freq,
].copy()
- for k in range(mpc_freq):
- States_next[:, nx_0 + mpc_freq - k - 1] = (
- States_next[:, nx_0 + mpc_freq - k] + ctrl_time_step * Inputs[:, 0]
+ for index in range(mpc_freq):
+ States_next[:, nx_0 + mpc_freq - index - 1] = (
+ States_next[:, nx_0 + mpc_freq - index] + ctrl_time_step * Inputs[:, 0]
)
- States_next[:, nx_0 + acc_ctrl_queue_size + mpc_freq - k - 1] = (
- States_next[:, nx_0 + acc_ctrl_queue_size + mpc_freq - k]
+ States_next[:, nx_0 + acc_ctrl_queue_size + mpc_freq - index - 1] = (
+ States_next[:, nx_0 + acc_ctrl_queue_size + mpc_freq - index]
+ ctrl_time_step * Inputs[:, 1]
)
actual_Inputs = np.zeros((Inputs.shape[0], mpc_freq, nu_0))
@@ -1235,6 +1335,39 @@ def F_with_history_for_candidates(
return States_next, None
+def F_with_model(
+ states: np.ndarray,
+ inputs: np.ndarray,
+ previous_error: np.ndarray,
+ k: int,
+ pred: Callable,
+ i: int = acc_delay_step,
+ j: int = steer_delay_step,
+ acc_time_constant_ctrl: float = acc_time_constant,
+ steer_time_constant_ctrl: float = steer_time_constant,
+) -> tuple[np.ndarray, np.ndarray]:
+ """Integrate up to the MPC time width according to the trained model."""
+ if k == 0:
+ error = Error_decay * previous_error + (1 - Error_decay) * pred(states)
+ else:
+ error = Error_decay * Error_decay * Error_decay * previous_error + (
+ 1 - Error_decay * Error_decay * Error_decay
+ ) * pred(states)
+ previous_error_ = error
+ states_next, _ = F_with_history(
+ states,
+ inputs,
+ i=i,
+ j=j,
+ acc_time_constant_ctrl=acc_time_constant_ctrl,
+ steer_time_constant_ctrl=steer_time_constant_ctrl,
+ )
+
+ states_next[:nx_0] += error * mpc_time_step
+
+ return states_next, previous_error_
+
+
def F_with_model_initial_diff(
states: np.ndarray,
inputs: np.ndarray,
@@ -1326,7 +1459,13 @@ def F_with_model_diff(
dF_dx[:2, 3] += d_rot_error * mpc_time_step
states_next[:nx_0] += rot_error * mpc_time_step
- return states_next, dF_dx, dF_du, pred_with_diff[:, 2:] * mpc_time_step, pred_error_
+ C = pred_with_diff[:, 2:] * mpc_time_step
+ steer_diff = (
+ dF_dx[5, nx_0 + acc_ctrl_queue_size :].sum() + C[5, nx_0 + acc_ctrl_queue_size :].sum()
+ )
+ if steer_diff < minimum_steer_diff:
+ C[5, -1] += minimum_steer_diff - steer_diff
+ return states_next, dF_dx, dF_du, C, pred_error_
def F_with_model_for_candidates(
@@ -1366,6 +1505,137 @@ def F_with_model_for_candidates(
return States_next, Previous_error_
+def acc_prediction_error_compensation(
+ states: np.ndarray,
+ previous_states: np.ndarray,
+ inputs: np.ndarray,
+ previous_error: np.ndarray,
+ acc_fb_1: float,
+ acc_fb_2: float,
+ acc_time_stamp_interval: float,
+):
+ actual_acc_input = np.concatenate(
+ (np.array([inputs[0]]), previous_states[nx_0 : nx_0 + acc_ctrl_queue_size])
+ )[acc_delay_step]
+
+ predicted_acc = previous_states[4] + acc_time_stamp_interval * (
+ previous_error[4] + (actual_acc_input - previous_states[4]) / acc_time_constant
+ )
+ prediction_error = predicted_acc - states[4]
+ new_acc_fb_1 = np.exp(-acc_fb_decay) * acc_fb_1 + acc_fb_decay * prediction_error
+ new_acc_fb_2 = (
+ acc_fb_decay * np.exp(-acc_fb_decay) * acc_fb_1 + np.exp(-acc_fb_decay) * acc_fb_2
+ )
+ return new_acc_fb_1, new_acc_fb_2
+
+
+def steer_prediction_error_compensation(
+ states: np.ndarray,
+ previous_states: np.ndarray,
+ inputs: np.ndarray,
+ previous_error: np.ndarray,
+ steer_fb_1: float,
+ steer_fb_2: float,
+ steer_time_stamp_interval: float,
+):
+ actual_steer_input = np.concatenate(
+ (np.array([inputs[1]]), previous_states[nx_0 + acc_ctrl_queue_size :])
+ )[steer_delay_step]
+
+ delta_diff = actual_steer_input - previous_states[5]
+ if delta_diff >= steer_dead_band_for_ctrl:
+ delta_diff = delta_diff - steer_dead_band_for_ctrl
+ elif delta_diff <= -steer_dead_band_for_ctrl:
+ delta_diff = delta_diff + steer_dead_band_for_ctrl
+ else:
+ delta_diff = 0.0
+
+ predicted_steer = previous_states[5] + steer_time_stamp_interval * (
+ previous_error[5] + delta_diff / steer_time_constant
+ )
+ prediction_error = predicted_steer - states[5]
+
+ new_steer_fb_1 = np.exp(-steer_fb_decay) * steer_fb_1 + steer_fb_decay * (prediction_error)
+ new_steer_fb_2 = (
+ steer_fb_decay * np.exp(-steer_fb_decay) * steer_fb_1 + np.exp(-steer_fb_decay) * steer_fb_2
+ )
+ return new_steer_fb_1, new_steer_fb_2
+
+
+def pure_pursuit_control(
+ pos_xy_obs,
+ pos_yaw_obs,
+ longitudinal_vel_obs,
+ pos_xy_ref,
+ pos_yaw_ref,
+ longitudinal_vel_ref,
+ acc_gain_scaling=1.0,
+ steer_gain_scaling=1.0,
+):
+ pure_pursuit_acc_kp = acc_gain_scaling * mpc_param["mpc_parameter"]["pure_pursuit"]["acc_kp"]
+ pure_pursuit_lookahead_time = mpc_param["mpc_parameter"]["pure_pursuit"]["lookahead_time"]
+ pure_pursuit_min_lookahead = mpc_param["mpc_parameter"]["pure_pursuit"]["min_lookahead"]
+ pure_pursuit_steer_kp_param = (
+ steer_gain_scaling * mpc_param["mpc_parameter"]["pure_pursuit"]["steer_kp_param"]
+ )
+ pure_pursuit_steer_kd_param = (
+ steer_gain_scaling * mpc_param["mpc_parameter"]["pure_pursuit"]["steer_kd_param"]
+ )
+
+ longitudinal_vel_err = longitudinal_vel_obs - longitudinal_vel_ref
+ pure_pursuit_acc_cmd = -pure_pursuit_acc_kp * longitudinal_vel_err
+
+ cos_yaw = np.cos(pos_yaw_ref)
+ sin_yaw = np.sin(pos_yaw_ref)
+ diff_position = pos_xy_obs - pos_xy_ref
+ lat_err = -sin_yaw * diff_position[0] + cos_yaw * diff_position[1]
+ yaw_err = pos_yaw_obs - pos_yaw_ref
+ while True:
+ if yaw_err > np.pi:
+ yaw_err -= 2.0 * np.pi
+ if yaw_err < (-np.pi):
+ yaw_err += 2.0 * np.pi
+ if np.abs(yaw_err) < np.pi:
+ break
+
+ lookahead = pure_pursuit_min_lookahead + pure_pursuit_lookahead_time * np.abs(
+ longitudinal_vel_obs
+ )
+ pure_pursuit_steer_kp = pure_pursuit_steer_kp_param * L / (lookahead * lookahead)
+ pure_pursuit_steer_kd = pure_pursuit_steer_kd_param * L / lookahead
+ pure_pursuit_steer_cmd = -pure_pursuit_steer_kp * lat_err - pure_pursuit_steer_kd * yaw_err
+ return np.array([pure_pursuit_acc_cmd, pure_pursuit_steer_cmd])
+
+
+naive_pure_pursuit_lookahead_coef = float(
+ mpc_param["mpc_parameter"]["naive_pure_pursuit"]["lookahead_coef"]
+)
+naive_pure_pursuit_lookahead_intercept = float(
+ mpc_param["mpc_parameter"]["naive_pure_pursuit"]["lookahead_intercept"]
+)
+
+
+def naive_pure_pursuit_control(
+ pos_xy_obs,
+ pos_yaw_obs,
+ longitudinal_vel_obs,
+ pos_xy_ref_target,
+ longitudinal_vel_ref_nearest,
+):
+ pure_pursuit_acc_kp = mpc_param["mpc_parameter"]["naive_pure_pursuit"]["acc_kp"]
+ wheel_base = L
+ longitudinal_vel_err = longitudinal_vel_obs - longitudinal_vel_ref_nearest
+ pure_pursuit_acc_cmd = -pure_pursuit_acc_kp * longitudinal_vel_err
+
+ alpha = (
+ np.arctan2(pos_xy_ref_target[1] - pos_xy_obs[1], pos_xy_ref_target[0] - pos_xy_obs[0])
+ - pos_yaw_obs
+ )
+ angular_velocity_z = 2.0 * longitudinal_vel_ref_nearest * np.sin(alpha) / wheel_base
+ steer = np.arctan(angular_velocity_z * wheel_base / max(0.1, longitudinal_vel_ref_nearest))
+ return np.array([pure_pursuit_acc_cmd, steer])
+
+
def sg_filter(
inputs: np.ndarray,
use_sg: bool,
@@ -1439,6 +1709,11 @@ def calc_sg_filter_weight(sg_deg, sg_window_size):
sg_vector_left_edge_for_trained_model_diff,
sg_vector_right_edge_for_trained_model_diff,
) = calc_sg_filter_weight(sg_deg_for_trained_model_diff, sg_window_size_for_trained_model_diff)
+(
+ sg_vector_for_memory_diff,
+ sg_vector_left_edge_for_memory_diff,
+ sg_vector_right_edge_for_memory_diff,
+) = calc_sg_filter_weight(0, sg_window_size_for_memory_diff)
(
sg_vector_for_noise,
sg_vector_left_edge_for_noise,
@@ -1463,6 +1738,16 @@ def calc_sg_filter_weight(sg_deg, sg_window_size):
sg_vector=sg_vector_for_trained_model_diff,
)
+sg_filter_for_memory_diff = partial(
+ sg_filter,
+ use_sg=use_sg_for_memory_diff,
+ sg_window_size=sg_window_size_for_memory_diff, # sg_window_size_for_trained_model_diff,
+ sg_vector_left_edge=sg_vector_left_edge_for_memory_diff, # sg_vector_left_edge_for_trained_model_diff,
+ sg_vector_right_edge=sg_vector_right_edge_for_memory_diff, # sg_vector_right_edge_for_trained_model_diff,
+ sg_vector=sg_vector_for_memory_diff,
+)
+
+
sg_filter_for_noise = partial(
sg_filter,
use_sg=use_sg_for_noise,
diff --git a/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_iLQR.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_iLQR.py
index 2307586f23552..f3e374c5876cc 100644
--- a/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_iLQR.py
+++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_iLQR.py
@@ -33,11 +33,6 @@
)
acc_index = drive_functions.nx_0
steer_index = drive_functions.nx_0 + drive_functions.acc_ctrl_queue_size
-actual_control_index = np.arange(
- drive_functions.nx_0
- + drive_functions.acc_ctrl_queue_size
- + drive_functions.steer_ctrl_queue_size_core
-)
actual_state_dim = drive_functions.nx_0
input_dim = drive_functions.nu_0
mpc_freq = drive_functions.mpc_freq
@@ -52,7 +47,6 @@
def sparse_right_action_for_state_diff(
Mat: np.ndarray,
A: np.ndarray,
- actual_control_index=actual_control_index,
actual_state_dim=actual_state_dim,
acc_ctrl_queue_size=acc_ctrl_queue_size,
steer_ctrl_queue_size=steer_ctrl_queue_size,
@@ -65,7 +59,7 @@ def sparse_right_action_for_state_diff(
result = np.zeros(
(Mat.shape[0], actual_state_dim + acc_ctrl_queue_size + steer_ctrl_queue_size)
)
- result[:, actual_control_index] += Mat[:, :actual_state_dim] @ A
+ result += Mat[:, :actual_state_dim] @ A
result[:, actual_state_dim : actual_state_dim + acc_ctrl_queue_size - mpc_freq] += Mat[
:, actual_state_dim + mpc_freq : actual_state_dim + acc_ctrl_queue_size
@@ -97,7 +91,6 @@ def sparse_right_action_for_state_diff(
def sparse_left_action_for_state_diff(
A: np.ndarray,
Mat: np.ndarray,
- actual_control_index=actual_control_index,
actual_state_dim=actual_state_dim,
acc_ctrl_queue_size=acc_ctrl_queue_size,
steer_ctrl_queue_size=steer_ctrl_queue_size,
@@ -110,7 +103,7 @@ def sparse_left_action_for_state_diff(
result = np.zeros(
(actual_state_dim + acc_ctrl_queue_size + steer_ctrl_queue_size, Mat.shape[1])
)
- result[actual_control_index] += A.T @ Mat[:actual_state_dim]
+ result += A.T @ Mat[:actual_state_dim]
result[actual_state_dim : actual_state_dim + acc_ctrl_queue_size - mpc_freq] += Mat[
actual_state_dim + mpc_freq : actual_state_dim + acc_ctrl_queue_size
@@ -140,7 +133,6 @@ def sparse_left_action_for_state_diff(
def vector_sparse_left_action_for_state_diff(
A: np.ndarray,
vec: np.ndarray,
- actual_control_index=actual_control_index,
actual_state_dim=actual_state_dim,
acc_ctrl_queue_size=acc_ctrl_queue_size,
steer_ctrl_queue_size=steer_ctrl_queue_size,
@@ -151,7 +143,7 @@ def vector_sparse_left_action_for_state_diff(
The remaining A₀ components are known a priori.
"""
result = np.zeros(actual_state_dim + acc_ctrl_queue_size + steer_ctrl_queue_size)
- result[actual_control_index] += A.T @ vec[:actual_state_dim]
+ result += A.T @ vec[:actual_state_dim]
result[actual_state_dim : actual_state_dim + acc_ctrl_queue_size - mpc_freq] += vec[
actual_state_dim + mpc_freq : actual_state_dim + acc_ctrl_queue_size
@@ -245,8 +237,6 @@ def compute_iLQR_coef(
steer_rate_lim_weights,
steer_rate_lim_center,
N,
- nx,
- nu,
X_des,
U_des,
Sigma_x=None,
@@ -261,29 +251,35 @@ def compute_iLQR_coef(
dSigma_acc=None,
Sigma_steer=None,
dSigma_steer=None,
- index_cost=index_cost,
- acc_index=acc_index,
- steer_index=steer_index,
- actual_control_index=actual_control_index,
- actual_state_dim=actual_state_dim,
+ acc_input_index=acc_index,
+ steer_input_index=steer_index,
+ state_dim=actual_state_dim,
):
"""Perform the main part of iLQR.
Performs a Riccati recursion and returns the coefficients needed for the final output.
"""
+ nx = A.shape[2]
+ nu = B.shape[2]
P = np.zeros((N + 1, nx, nx))
w = np.zeros((N + 1, nx))
H_inv_G = np.zeros((N, nu, nx))
H_inv_g = np.zeros((N, nu))
- P[N][:7, :7] = Q_total[-1][:7, :7]
- P[N][steer_index, steer_index] = Q_total[-1][7, 7]
+ P[N][:6, :6] = Q_total[-1][:6, :6]
+ P[N][acc_input_index, acc_input_index] = Q_total[-1][6, 6]
+ P[N][steer_input_index, steer_input_index] = Q_total[-1][7, 7]
- P[N][acc_index, acc_index] += acc_lim_weights[N]
- P[N][steer_index, steer_index] += steer_lim_weights[N]
+ P[N][acc_input_index, acc_input_index] += acc_lim_weights[N]
+ P[N][steer_input_index, steer_input_index] += steer_lim_weights[N]
- w[N][index_cost] = Q_total[-1] @ (traj[N] - X_des[N])
- w[N, acc_index] += acc_lim_weights[N] * (traj[N, acc_index] - acc_lim_center[N])
- w[N, steer_index] += steer_lim_weights[N] * (traj[N, acc_index + 1] - steer_lim_center[N])
+ Q_N_x = Q_total[-1] @ (traj[N] - X_des[N])
+
+ w[N][:6] = Q_N_x[:6]
+ w[N][acc_input_index] = Q_N_x[6]
+ w[N][steer_input_index] = Q_N_x[7]
+
+ w[N, acc_input_index] += acc_lim_weights[N] * (traj[N, acc_index] - acc_lim_center[N])
+ w[N, steer_input_index] += steer_lim_weights[N] * (traj[N, acc_index + 1] - steer_lim_center[N])
for i in range(N):
j = N - i - 1
@@ -291,15 +287,23 @@ def compute_iLQR_coef(
Rj = R_total[j]
Aj = A[j]
Pj1Aj = sparse_right_action_for_state_diff(
- P[j + 1], Aj[:actual_state_dim, actual_control_index]
+ P[j + 1], Aj[:state_dim], actual_state_dim=state_dim
)
- G = sparse_left_action_for_input_diff(Pj1Aj)
+ G = sparse_left_action_for_input_diff(Pj1Aj, actual_state_dim=state_dim)
- H = sparse_left_action_for_input_diff(sparse_right_action_for_input_diff(P[j + 1])) + Rj
+ H = (
+ sparse_left_action_for_input_diff(
+ sparse_right_action_for_input_diff(P[j + 1], actual_state_dim=state_dim),
+ actual_state_dim=state_dim,
+ )
+ + Rj
+ )
H[0, 0] += acc_rate_lim_weights[j]
H[1, 1] += steer_rate_lim_weights[j]
- g_ = vector_sparse_left_action_for_input_diff(w[j + 1]) + Rj @ (inputs[j] - U_des[j])
+ g_ = vector_sparse_left_action_for_input_diff(w[j + 1], actual_state_dim=state_dim) + Rj @ (
+ inputs[j] - U_des[j]
+ )
g_[0] += acc_rate_lim_weights[j] * (inputs[j, 0] - acc_rate_lim_center[j])
g_[1] += steer_rate_lim_weights[j] * (inputs[j, 1] - steer_rate_lim_center[j])
@@ -311,25 +315,32 @@ def compute_iLQR_coef(
H_inv_g[j] = H_inv @ g_
P[j] = (
- sparse_left_action_for_state_diff(Aj[:actual_state_dim, actual_control_index], Pj1Aj)
+ sparse_left_action_for_state_diff(Aj[:state_dim], Pj1Aj, actual_state_dim=state_dim)
- G.T @ H_inv_G[j]
)
- P[j][:7, :7] += Qj[:7, :7]
- P[j][steer_index, steer_index] += Qj[7, 7]
+ P[j][:6, :6] += Qj[:6, :6]
+ P[j][acc_input_index, acc_input_index] += Qj[6, 6]
+ P[j][steer_input_index, steer_input_index] += Qj[7, 7]
- P[j][acc_index, acc_index] += acc_lim_weights[j]
- P[j][steer_index, steer_index] += steer_lim_weights[j]
+ P[j][acc_input_index, acc_input_index] += acc_lim_weights[j]
+ P[j][steer_input_index, steer_input_index] += steer_lim_weights[j]
w[j] = (
vector_sparse_left_action_for_state_diff(
- Aj[:actual_state_dim, actual_control_index], w[j + 1]
+ Aj[:state_dim], w[j + 1], actual_state_dim=state_dim
)
- H_inv_G[j].T @ g_
)
- w[j, index_cost] += Qj @ (traj[j] - X_des[j])
- w[j, acc_index] += acc_lim_weights[j] * (traj[j, acc_index] - acc_lim_center[j])
- w[j, steer_index] += steer_lim_weights[j] * (traj[j, acc_index + 1] - steer_lim_center[j])
+ Q_j_x = Qj @ (traj[j] - X_des[j])
+ w[j, :6] += Q_j_x[:6]
+ w[j, acc_input_index] += Q_j_x[6]
+ w[j, steer_input_index] += Q_j_x[7]
+
+ w[j, acc_input_index] += acc_lim_weights[j] * (traj[j, acc_index] - acc_lim_center[j])
+ w[j, steer_input_index] += steer_lim_weights[j] * (
+ traj[j, acc_index + 1] - steer_lim_center[j]
+ )
P_noise = np.zeros((nx, nx))
w_noise = np.zeros(nx)
if Sigma_x is not None:
@@ -395,19 +406,19 @@ class drive_iLQR:
"""Class that runs iLQR to compute optimal input values."""
ls_step: float
- """ Line search step width """
+ """ Line search step width."""
max_iter_ls: int
- """ Number of candidates for line search. """
+ """ Number of candidates for line search."""
max_iter_ilqr: int
- """ Maximum number of iLQR iterations """
+ """ Maximum number of iLQR iterations."""
ilqr_tol: float
- """ Tolerance to terminate iLQR iterations """
+ """ Tolerance to terminate iLQR iterations."""
use_trained_model_diff: bool
- """ Whether to use the derivative of the trained model """
+ """ Whether to use the derivative of the trained model."""
def __init__(
self,
@@ -429,6 +440,10 @@ def __init__(
ls[i + 1] = ls_step * ls[i]
ls[-1] = 0.0
self.ls = ls
+ self.add_state_hc = False
+ self.state_dim = actual_state_dim
+ self.acc_index = acc_index
+ self.steer_index = steer_index
def calc_forward_trajectories_with_cost(
self,
@@ -437,6 +452,7 @@ def calc_forward_trajectories_with_cost(
X_des: np.ndarray,
U_des: np.ndarray,
previous_error: np.ndarray,
+ steer_rate_cost_coef: float,
) -> tuple[np.ndarray, np.ndarray]:
"""Calculate the predicted trajectory and cost.
@@ -456,6 +472,7 @@ def calc_forward_trajectories_with_cost(
Traj[:, k, index_cost],
Inputs[:, k, :],
k,
+ steer_rate_cost_coef,
)
Traj[:, k + 1], Previous_error = self.F_for_candidates(
Traj[:, k], Inputs[:, k], Previous_error, k
@@ -466,6 +483,7 @@ def calc_forward_trajectories_with_cost(
Traj[:, N, index_cost],
Inputs[:, N - 1, :],
N,
+ steer_rate_cost_coef,
)
return Traj, Cost
@@ -485,18 +503,41 @@ def calc_forward_trajectory_with_diff(
A = np.zeros((N, nx, nx))
B = np.zeros((N, nx, nu))
C = np.zeros((N, 6, nx))
+ if self.add_state_hc:
+ D = np.zeros((N, 6 + self.h_dim_double, nx + self.h_dim_double))
previous_error_ = previous_error.copy()
for k in range(N):
if self.use_trained_model_diff:
traj[k + 1], A[k], B[k], C[k], previous_error_ = self.F_with_diff(
traj[k], inputs[k], previous_error_, k
)
+ if self.add_state_hc:
+ D[k][:6, 6 : 6 + self.h_dim_double] = (
+ self.get_dy_dhc() * drive_functions.mpc_time_step
+ )
+ dhc_dx = self.get_dhc_dx()
+ D[k][6:, :6] = dhc_dx[:, :6]
+ D[k][6:, 6 + self.h_dim_double :] = dhc_dx[:, 6:]
+ D[k][6:, 6 : 6 + self.h_dim_double] = self.get_dhc_dhc()
else:
traj[k + 1], A[k], B[k], previous_error_ = self.F_with_initial_diff(
traj[k], inputs[k], previous_error_, k
)
if self.use_trained_model_diff:
A[:, :6] += drive_functions.sg_filter_for_trained_model_diff(C)
+ if self.add_state_hc:
+ D = drive_functions.sg_filter_for_memory_diff(D)
+ A_ = np.zeros((N, nx + self.h_dim_double, nx + self.h_dim_double))
+ A_[:, :6, :6] = A[:, :6, :6]
+ A_[:, 6 + self.h_dim_double :, :6] = A[:, 6:, :6]
+ A_[:, :6, 6 + self.h_dim_double :] = A[:, :6, 6:]
+ A_[:, 6 + self.h_dim_double :, 6 + self.h_dim_double :] = A[:, 6:, 6:]
+ A_[:, : 6 + self.h_dim_double] += D
+ B_ = np.zeros((N, nx + self.h_dim_double, nu))
+ B_[:, :6] = B[:, :6]
+ B_[:, 6 + self.h_dim_double :] = B[:, 6:]
+ return traj, A_, B_
+
return traj, A, B
def compute_optimal_control(
@@ -512,14 +553,13 @@ def compute_optimal_control(
theta_noise: Callable | None,
acc_noise: Callable | None,
steer_noise: Callable | None,
+ steer_rate_cost_coef: float,
) -> tuple[np.ndarray, np.ndarray, np.ndarray, bool]:
"""Calculate the optimal predictive trajectory and the input sequence at that time.
It also determines whether the tolerance has been reached and returns whether the optimization should continue or not.
Performs one iteration of iLQR.
"""
- nx = x_current.shape[0]
- nu = inputs.shape[1]
N = inputs.shape[0]
self.time_1 = time.time()
traj, A, B = self.calc_forward_trajectory_with_diff(x_current, inputs, previous_error)
@@ -534,7 +574,7 @@ def compute_optimal_control(
acc_rate_lim_center,
steer_rate_lim_weights,
steer_rate_lim_center,
- ) = drive_functions.transform_Q_R(X_des, U_des, traj, inputs)
+ ) = drive_functions.transform_Q_R(X_des, U_des, traj, inputs, steer_rate_cost_coef)
self.time_2 = time.time()
if x_noise is None:
Sigma_x = None
@@ -584,44 +624,45 @@ def compute_optimal_control(
Sigma_steer = drive_functions.sg_filter_for_noise(Sigma_steer)
dSigma_steer = drive_functions.sg_filter_for_noise(dSigma_steer)
P, w, H_inv_G, H_inv_g = compute_iLQR_coef(
- traj[:, index_cost],
- inputs,
- A,
- B,
- Q_total,
- R_total,
- acc_lim_weights,
- acc_lim_center,
- steer_lim_weights,
- steer_lim_center,
- acc_rate_lim_weights,
- acc_rate_lim_center,
- steer_rate_lim_weights,
- steer_rate_lim_center,
- N,
- nx,
- nu,
- X_des,
- U_des,
- Sigma_x,
- dSigma_x,
- Sigma_y,
- dSigma_y,
- Sigma_v,
- dSigma_v,
- Sigma_theta,
- dSigma_theta,
- Sigma_acc,
- dSigma_acc,
- Sigma_steer,
- dSigma_steer,
+ traj=traj[:, index_cost],
+ inputs=inputs,
+ A=A,
+ B=B,
+ Q_total=Q_total,
+ R_total=R_total,
+ acc_lim_weights=acc_lim_weights,
+ acc_lim_center=acc_lim_center,
+ steer_lim_weights=steer_lim_weights,
+ steer_lim_center=steer_lim_center,
+ acc_rate_lim_weights=acc_rate_lim_weights,
+ acc_rate_lim_center=acc_rate_lim_center,
+ steer_rate_lim_weights=steer_rate_lim_weights,
+ steer_rate_lim_center=steer_rate_lim_center,
+ N=N,
+ X_des=X_des,
+ U_des=U_des,
+ Sigma_x=Sigma_x,
+ dSigma_x=dSigma_x,
+ Sigma_y=Sigma_y,
+ dSigma_y=dSigma_y,
+ Sigma_v=Sigma_v,
+ dSigma_v=dSigma_v,
+ Sigma_theta=Sigma_theta,
+ dSigma_theta=dSigma_theta,
+ Sigma_acc=Sigma_acc,
+ dSigma_acc=dSigma_acc,
+ Sigma_steer=Sigma_steer,
+ dSigma_steer=dSigma_steer,
+ acc_input_index=self.acc_index,
+ steer_input_index=self.steer_index,
+ state_dim=self.state_dim,
)
self.time_3 = time.time()
# start line search
Inputs = calc_line_search_candidates(A, B, H_inv_g, H_inv_G, inputs, self.ls)
self.time_4 = time.time()
Traj, Cost = self.calc_forward_trajectories_with_cost(
- x_current, Inputs, X_des, U_des, previous_error
+ x_current, Inputs, X_des, U_des, previous_error, steer_rate_cost_coef
)
best_index = Cost.argmin()
self.time_5 = time.time()
@@ -642,3 +683,17 @@ def receive_model(
self.F_with_initial_diff = F_with_initial_diff
self.F_with_diff = F_with_diff
self.F_for_candidates = F_for_candidates
+
+ def receive_memory_diff(
+ self, get_dhc_dx: Callable, get_dhc_dhc: Callable, get_dy_dhc: Callable
+ ):
+ """Receive getter hc diff."""
+ self.get_dhc_dx = get_dhc_dx
+ self.get_dhc_dhc = get_dhc_dhc
+ self.get_dy_dhc = get_dy_dhc
+ self.h_dim_double = get_dhc_dx().shape[0]
+ self.add_state_hc = True
+ print(self.h_dim_double)
+ self.state_dim += self.h_dim_double
+ self.acc_index += self.h_dim_double
+ self.steer_index += self.h_dim_double
diff --git a/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_mppi.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_mppi.py
index 5e88c79172097..a5cf4c366046f 100644
--- a/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_mppi.py
+++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/drive_mppi.py
@@ -23,7 +23,7 @@
index_cost = np.concatenate(
(
np.arange(drive_functions.nx_0 + 1),
- [drive_functions.nx_0 + drive_functions.acc_ctrl_queue_size],
+ np.array([drive_functions.nx_0 + drive_functions.acc_ctrl_queue_size]),
)
)
@@ -105,29 +105,14 @@ def calc_forward_trajectories_with_cost(self, x_current, Inputs, X_des, U_des, P
Cost += drive_functions.calc_cost_only_for_states(X_des[N], Traj[:, N, index_cost], N)
return Traj, Cost
- def proceed_mppi_step(self, x_current, inputs, X_des, U_des, previous_error):
+ def compute_optimal_control(self, x_current, inputs, X_des, U_des, previous_error):
"""Proceed with MPPI iteration one time."""
N = inputs.shape[0]
- previous_error_ = previous_error.reshape(1, -1).copy()
- nominal_traj = np.zeros((N + 1, x_current.shape[0]))
- nominal_traj[0] = x_current.copy()
- for k in range(N):
- if k == 0:
- Traj_, previous_error_ = self.F_for_candidates(
- nominal_traj[k].reshape(1, -1), inputs[k].reshape(1, -1), previous_error_, True
- )
- nominal_traj[k + 1] = Traj_[0]
- else:
- Traj_, previous_error_ = self.F_for_candidates(
- nominal_traj[k].reshape(1, -1), inputs[k].reshape(1, -1), previous_error_, False
- )
- nominal_traj[k + 1] = Traj_[0]
-
Inputs = self.generate_sample_inputs(inputs)
Previous_error = np.tile(previous_error, (self.sample_num, 1))
- _, Cost = self.calc_forward_trajectories_with_cost(
+ Traj, Cost = self.calc_forward_trajectories_with_cost(
x_current, Inputs, X_des, U_des, Previous_error
)
original_cost = Cost[0]
@@ -135,36 +120,20 @@ def proceed_mppi_step(self, x_current, inputs, X_des, U_des, previous_error):
Exps = np.exp(-(Cost - best_cost) / self.lam)
Exps = Exps / Exps.sum()
new_inputs = (Inputs.T @ Exps).T
- previous_error_ = previous_error.reshape(1, -1).copy()
+ previous_error_ = previous_error.copy()
new_traj = np.zeros((N + 1, x_current.shape[0]))
new_traj[0] = x_current.copy()
for k in range(N):
- if k == 0:
- Traj_, previous_error_ = self.F_for_candidates(
- new_traj[k].reshape(1, -1), new_inputs[k].reshape(1, -1), previous_error_, True
- )
- new_traj[k + 1] = Traj_[0]
- else:
- Traj_, previous_error_ = self.F_for_candidates(
- new_traj[k].reshape(1, -1), new_inputs[k].reshape(1, -1), previous_error_, False
- )
- new_traj[k + 1] = Traj_[0]
+ new_traj[k + 1], previous_error_ = self.F(
+ new_traj[k], new_inputs[k], previous_error_, k
+ )
if np.dot(Exps, Cost) < (1 - self.mppi_tol) * original_cost:
proceed = True
else:
proceed = False
- return new_traj, new_inputs, proceed
-
- def compute_optimal_control(self, x_current, inputs, X_des, U_des, previous_error):
- """Calculate the optimal input based on MPPI."""
- for i in range(self.max_iter_mppi):
- new_traj, new_inputs, proceed = self.proceed_mppi_step(
- x_current, inputs, X_des, U_des, previous_error
- )
- if not proceed:
- break
- return new_inputs, new_inputs[0], new_traj
+ return new_inputs, new_inputs[0], new_traj, Traj, proceed
- def receive_model(self, F_for_candidates: Callable):
+ def receive_model(self, F_for_candidates: Callable, F):
"""Receive vehicle model for control."""
self.F_for_candidates = F_for_candidates
+ self.F = F
diff --git a/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/proxima_calc.cpp b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/proxima_calc.cpp
index de65184ec9f8e..6dd39c72d433d 100644
--- a/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/proxima_calc.cpp
+++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/proxima_calc.cpp
@@ -12,6 +12,8 @@
// See the License for the specific language governing permissions and
// limitations under the License.
+// cSpell:ignore lstm
+
#include
#include
@@ -20,29 +22,19 @@
#include
namespace py = pybind11;
-double nominal_model_input(Eigen::VectorXd var, double lam, int step)
+Eigen::VectorXd tanh(const Eigen::VectorXd & v)
{
- double nominal_pred = var[0];
- nominal_pred += (var[step] - nominal_pred) * 0.03333 / lam;
- nominal_pred += (var[step - 1] - nominal_pred) * 0.03333 / lam;
- nominal_pred += (var[step - 2] - nominal_pred) * 0.03333 / lam;
- return nominal_pred;
+ return v.array().tanh();
}
-Eigen::RowVectorXd Nominal_model_input(Eigen::MatrixXd Var, double lam, int step)
+Eigen::VectorXd d_tanh(const Eigen::VectorXd & v)
{
- Eigen::RowVectorXd nominal_pred = Var.row(0);
- nominal_pred += (Var.row(step) - nominal_pred) * 0.03333 / lam;
- nominal_pred += (Var.row(step - 1) - nominal_pred) * 0.03333 / lam;
- nominal_pred += (Var.row(step - 2) - nominal_pred) * 0.03333 / lam;
- return nominal_pred;
+ return 1 / (v.array().cosh() * v.array().cosh());
}
-
-Eigen::VectorXd d_tanh(Eigen::VectorXd v)
+Eigen::VectorXd sigmoid(const Eigen::VectorXd & v)
{
- Eigen::VectorXd result = 1 / (v.array().cosh() * v.array().cosh());
- return result;
+ return 0.5 * (0.5 * v).array().tanh() + 0.5;
}
-Eigen::VectorXd relu(Eigen::VectorXd x)
+Eigen::VectorXd relu(const Eigen::VectorXd & x)
{
Eigen::VectorXd x_ = x;
for (int i = 0; i < x.size(); i++) {
@@ -52,7 +44,7 @@ Eigen::VectorXd relu(Eigen::VectorXd x)
}
return x_;
}
-Eigen::VectorXd d_relu(Eigen::VectorXd x)
+Eigen::VectorXd d_relu(const Eigen::VectorXd & x)
{
Eigen::VectorXd result = Eigen::VectorXd::Ones(x.size());
for (int i = 0; i < x.size(); i++) {
@@ -62,8 +54,7 @@ Eigen::VectorXd d_relu(Eigen::VectorXd x)
}
return result;
}
-
-Eigen::MatrixXd d_relu_product(Eigen::MatrixXd m, Eigen::VectorXd x)
+Eigen::MatrixXd d_relu_product(const Eigen::MatrixXd & m, const Eigen::VectorXd & x)
{
Eigen::MatrixXd result = Eigen::MatrixXd::Zero(m.rows(), m.cols());
for (int i = 0; i < m.cols(); i++) {
@@ -73,7 +64,7 @@ Eigen::MatrixXd d_relu_product(Eigen::MatrixXd m, Eigen::VectorXd x)
}
return result;
}
-Eigen::MatrixXd d_tanh_product(Eigen::MatrixXd m, Eigen::VectorXd x)
+Eigen::MatrixXd d_tanh_product(const Eigen::MatrixXd & m, const Eigen::VectorXd & x)
{
Eigen::MatrixXd result = Eigen::MatrixXd(m.rows(), m.cols());
for (int i = 0; i < m.cols(); i++) {
@@ -81,13 +72,34 @@ Eigen::MatrixXd d_tanh_product(Eigen::MatrixXd m, Eigen::VectorXd x)
}
return result;
}
-Eigen::MatrixXd test_product(Eigen::VectorXd v, Eigen::MatrixXd m)
+Eigen::VectorXd d_tanh_product_vec(const Eigen::VectorXd & v, const Eigen::VectorXd & x)
+{
+ Eigen::VectorXd result = Eigen::VectorXd(v.size());
+ for (int i = 0; i < v.size(); i++) {
+ result[i] = v[i] / (std::cosh(x[i]) * std::cosh(x[i]));
+ }
+ return result;
+}
+Eigen::MatrixXd d_sigmoid_product(const Eigen::MatrixXd & m, const Eigen::VectorXd & x)
{
- return v.asDiagonal() * m;
+ Eigen::MatrixXd result = Eigen::MatrixXd(m.rows(), m.cols());
+ for (int i = 0; i < m.cols(); i++) {
+ result.col(i) = 0.25 * m.col(i) / (std::cosh(0.5 * x[i]) * std::cosh(0.5 * x[i]));
+ }
+ return result;
}
-Eigen::VectorXd get_polynomial_features(Eigen::VectorXd x, int deg, int dim)
+Eigen::VectorXd d_sigmoid_product_vec(const Eigen::VectorXd & v, const Eigen::VectorXd & x)
{
- int n_features = x.size();
+ Eigen::VectorXd result = Eigen::VectorXd(v.size());
+ for (int i = 0; i < v.size(); i++) {
+ result[i] = 0.25 * v[i] / (std::cosh(0.5 * x[i]) * std::cosh(0.5 * x[i]));
+ }
+ return result;
+}
+
+Eigen::VectorXd get_polynomial_features(const Eigen::VectorXd & x, const int deg, const int dim)
+{
+ const int n_features = x.size();
Eigen::VectorXd result = Eigen::VectorXd(dim);
result.head(n_features) = x;
if (deg >= 2) {
@@ -98,11 +110,11 @@ Eigen::VectorXd get_polynomial_features(Eigen::VectorXd x, int deg, int dim)
int current_idx = n_features;
for (int i = 0; i < deg - 1; i++) {
std::vector new_index = {};
- int end = index[index.size() - 1];
+ const int end = index[index.size() - 1];
for (int feature_idx = 0; feature_idx < n_features; feature_idx++) {
- int start = index[feature_idx];
+ const int start = index[feature_idx];
new_index.push_back(current_idx);
- int next_idx = current_idx + end - start;
+ const int next_idx = current_idx + end - start;
result.segment(current_idx, end - start) =
x[feature_idx] * result.segment(start, end - start);
current_idx = next_idx;
@@ -113,9 +125,10 @@ Eigen::VectorXd get_polynomial_features(Eigen::VectorXd x, int deg, int dim)
}
return result;
}
-Eigen::MatrixXd get_polynomial_features_with_diff(Eigen::VectorXd x, int deg, int dim)
+Eigen::MatrixXd get_polynomial_features_with_diff(
+ const Eigen::VectorXd & x, const int deg, const int dim)
{
- int n_features = x.size();
+ const int n_features = x.size();
Eigen::MatrixXd result = Eigen::MatrixXd::Zero(dim, n_features + 1);
result.block(0, 0, n_features, 1) = x;
result.block(0, 1, n_features, n_features) = Eigen::MatrixXd::Identity(n_features, n_features);
@@ -127,11 +140,11 @@ Eigen::MatrixXd get_polynomial_features_with_diff(Eigen::VectorXd x, int deg, in
int current_idx = n_features;
for (int i = 0; i < deg - 1; i++) {
std::vector new_index = {};
- int end = index[index.size() - 1];
+ const int end = index[index.size() - 1];
for (int feature_idx = 0; feature_idx < n_features; feature_idx++) {
- int start = index[feature_idx];
+ const int start = index[feature_idx];
new_index.push_back(current_idx);
- int next_idx = current_idx + end - start;
+ const int next_idx = current_idx + end - start;
result.block(current_idx, 0, end - start, n_features + 1) =
x[feature_idx] * result.block(start, 0, end - start, n_features + 1);
result.block(current_idx, feature_idx + 1, end - start, 1) +=
@@ -147,124 +160,137 @@ Eigen::MatrixXd get_polynomial_features_with_diff(Eigen::VectorXd x, int deg, in
class transform_model_to_eigen
{
private:
- Eigen::MatrixXd weight_0;
- Eigen::MatrixXd weight_1;
- Eigen::MatrixXd weight_2;
- Eigen::MatrixXd weight_3;
- Eigen::MatrixXd weight_4;
- Eigen::MatrixXd weight_5;
- Eigen::MatrixXd weight_6;
- Eigen::MatrixXd weight_7;
- Eigen::VectorXd bias_0;
- Eigen::VectorXd bias_1;
- Eigen::VectorXd bias_2;
- Eigen::VectorXd bias_3;
- Eigen::VectorXd bias_4;
- Eigen::VectorXd bias_5;
- Eigen::VectorXd bias_6;
- Eigen::VectorXd bias_7;
- Eigen::MatrixXd A_linear_reg;
- Eigen::VectorXd b_linear_reg;
- int deg;
- double acc_time_constant;
- int acc_delay_step;
- double steer_time_constant;
- int steer_delay_step;
- int acc_ctrl_queue_size;
- int steer_ctrl_queue_size;
- int steer_ctrl_queue_size_core;
- double max_acc_error = 20.0;
- double max_steer_error = 20.0;
+ Eigen::MatrixXd weight_acc_layer_1_;
+ Eigen::MatrixXd weight_steer_layer_1_head_;
+ Eigen::MatrixXd weight_steer_layer_1_tail_;
+ Eigen::MatrixXd weight_acc_layer_2_;
+ Eigen::MatrixXd weight_steer_layer_2_;
+ Eigen::MatrixXd weight_linear_relu_1_;
+ Eigen::MatrixXd weight_linear_relu_2_;
+ Eigen::MatrixXd weight_finalize_;
+ Eigen::VectorXd bias_acc_layer_1_;
+ Eigen::VectorXd bias_steer_layer_1_head_;
+ Eigen::VectorXd bias_steer_layer_1_tail_;
+ Eigen::VectorXd bias_acc_layer_2_;
+ Eigen::VectorXd bias_steer_layer_2_;
+ Eigen::VectorXd bias_linear_relu_1_;
+ Eigen::VectorXd bias_linear_relu_2_;
+ Eigen::VectorXd bias_linear_finalize_;
+ Eigen::MatrixXd A_linear_reg_;
+ Eigen::VectorXd b_linear_reg_;
+ int deg_;
+ int acc_delay_step_;
+ int steer_delay_step_;
+ int acc_ctrl_queue_size_;
+ int steer_ctrl_queue_size_;
+ int steer_ctrl_queue_size_core_;
+ double vel_normalize_;
+ double acc_normalize_;
+ double steer_normalize_;
+ static constexpr double max_acc_error_ = 20.0;
+ static constexpr double max_steer_error_ = 20.0;
public:
transform_model_to_eigen() {}
void set_params(
- Eigen::MatrixXd weight_0_, Eigen::MatrixXd weight_1_, Eigen::MatrixXd weight_2_,
- Eigen::MatrixXd weight_3_, Eigen::MatrixXd weight_4_, Eigen::MatrixXd weight_5_,
- Eigen::MatrixXd weight_6_, Eigen::MatrixXd weight_7_, Eigen::VectorXd bias_0_,
- Eigen::VectorXd bias_1_, Eigen::VectorXd bias_2_, Eigen::VectorXd bias_3_,
- Eigen::VectorXd bias_4_, Eigen::VectorXd bias_5_, Eigen::VectorXd bias_6_,
- Eigen::VectorXd bias_7_, Eigen::MatrixXd A_linear_reg_, Eigen::VectorXd b_linear_reg_, int deg_,
- double acc_time_constant_, int acc_delay_step_, double steer_time_constant_,
- int steer_delay_step_, int acc_ctrl_queue_size_, int steer_ctrl_queue_size_,
- int steer_ctrl_queue_size_core_)
+ const Eigen::MatrixXd & weight_acc_layer_1, const Eigen::MatrixXd & weight_steer_layer_1_head,
+ const Eigen::MatrixXd & weight_steer_layer_1_tail, const Eigen::MatrixXd & weight_acc_layer_2,
+ const Eigen::MatrixXd & weight_steer_layer_2, const Eigen::MatrixXd & weight_linear_relu_1,
+ const Eigen::MatrixXd & weight_linear_relu_2, const Eigen::MatrixXd & weight_finalize,
+ const Eigen::VectorXd & bias_acc_layer_1, const Eigen::VectorXd & bias_steer_layer_1_head,
+ const Eigen::VectorXd & bias_steer_layer_1_tail, const Eigen::VectorXd & bias_acc_layer_2,
+ const Eigen::VectorXd & bias_steer_layer_2, const Eigen::VectorXd & bias_linear_relu_1,
+ const Eigen::VectorXd & bias_linear_relu_2, const Eigen::VectorXd & bias_linear_finalize,
+ const Eigen::MatrixXd & A_linear_reg, const Eigen::VectorXd & b_linear_reg, const int deg,
+ const int acc_delay_step, const int steer_delay_step, const int acc_ctrl_queue_size,
+ const int steer_ctrl_queue_size, const int steer_ctrl_queue_size_core,
+ const double vel_normalize, const double acc_normalize, const double steer_normalize)
{
- weight_0 = weight_0_;
- weight_1 = weight_1_;
- weight_2 = weight_2_;
- weight_3 = weight_3_;
- weight_4 = weight_4_;
- weight_5 = weight_5_;
- weight_6 = weight_6_;
- weight_7 = weight_7_;
- bias_0 = bias_0_;
- bias_1 = bias_1_;
- bias_2 = bias_2_;
- bias_3 = bias_3_;
- bias_4 = bias_4_;
- bias_5 = bias_5_;
- bias_6 = bias_6_;
- bias_7 = bias_7_;
- A_linear_reg = A_linear_reg_;
- b_linear_reg = b_linear_reg_;
- deg = deg_;
- acc_time_constant = acc_time_constant_;
- acc_delay_step = acc_delay_step_;
- steer_time_constant = steer_time_constant_;
- steer_delay_step = steer_delay_step_;
- acc_ctrl_queue_size = acc_ctrl_queue_size_;
- steer_ctrl_queue_size = steer_ctrl_queue_size_;
- steer_ctrl_queue_size_core = steer_ctrl_queue_size_core_;
- }
- Eigen::VectorXd error_prediction(Eigen::VectorXd x)
+ weight_acc_layer_1_ = weight_acc_layer_1;
+ weight_steer_layer_1_head_ = weight_steer_layer_1_head;
+ weight_steer_layer_1_tail_ = weight_steer_layer_1_tail;
+ weight_acc_layer_2_ = weight_acc_layer_2;
+ weight_steer_layer_2_ = weight_steer_layer_2;
+ weight_linear_relu_1_ = weight_linear_relu_1;
+ weight_linear_relu_2_ = weight_linear_relu_2;
+ weight_finalize_ = weight_finalize;
+ bias_acc_layer_1_ = bias_acc_layer_1;
+ bias_steer_layer_1_head_ = bias_steer_layer_1_head;
+ bias_steer_layer_1_tail_ = bias_steer_layer_1_tail;
+ bias_acc_layer_2_ = bias_acc_layer_2;
+ bias_steer_layer_2_ = bias_steer_layer_2;
+ bias_linear_relu_1_ = bias_linear_relu_1;
+ bias_linear_relu_2_ = bias_linear_relu_2;
+ bias_linear_finalize_ = bias_linear_finalize;
+ A_linear_reg_ = A_linear_reg;
+ b_linear_reg_ = b_linear_reg;
+ deg_ = deg;
+ acc_delay_step_ = acc_delay_step;
+ steer_delay_step_ = steer_delay_step;
+ acc_ctrl_queue_size_ = acc_ctrl_queue_size;
+ steer_ctrl_queue_size_ = steer_ctrl_queue_size;
+ steer_ctrl_queue_size_core_ = steer_ctrl_queue_size_core;
+ vel_normalize_ = vel_normalize;
+ acc_normalize_ = acc_normalize;
+ steer_normalize_ = steer_normalize;
+ }
+ Eigen::VectorXd error_prediction(const Eigen::VectorXd & x) const
{
- Eigen::VectorXd acc_sub(acc_ctrl_queue_size + 1);
- acc_sub[0] = x[1];
- acc_sub.tail(acc_ctrl_queue_size) = x.segment(3, acc_ctrl_queue_size);
+ Eigen::VectorXd acc_sub(acc_ctrl_queue_size_ + 1);
+ acc_sub[0] = acc_normalize_ * x[1];
+ acc_sub.tail(acc_ctrl_queue_size_) = acc_normalize_ * x.segment(3, acc_ctrl_queue_size_);
- Eigen::VectorXd steer_sub(steer_ctrl_queue_size_core + 1);
- steer_sub[0] = x[2];
- steer_sub.tail(steer_ctrl_queue_size_core) =
- x.segment(3 + acc_ctrl_queue_size, steer_ctrl_queue_size_core);
- Eigen::VectorXd acc_layer_1 = relu(weight_0 * acc_sub + bias_0);
+ Eigen::VectorXd steer_sub(steer_ctrl_queue_size_core_ + 1);
+ steer_sub[0] = steer_normalize_ * x[2];
+ steer_sub.tail(steer_ctrl_queue_size_core_) =
+ steer_normalize_ * x.segment(3 + acc_ctrl_queue_size_, steer_ctrl_queue_size_core_);
+ const Eigen::VectorXd acc_layer_1 = relu(weight_acc_layer_1_ * acc_sub + bias_acc_layer_1_);
- Eigen::VectorXd steer_layer_1(bias_1.size() + bias_2.size());
- steer_layer_1.head(bias_1.size()) = relu(weight_1 * steer_sub + bias_1);
+ Eigen::VectorXd steer_layer_1(
+ bias_steer_layer_1_head_.size() + bias_steer_layer_1_tail_.size());
+ steer_layer_1.head(bias_steer_layer_1_head_.size()) =
+ relu(weight_steer_layer_1_head_ * steer_sub + bias_steer_layer_1_head_);
- Eigen::VectorXd steer_input_full = x.segment(3 + acc_ctrl_queue_size, steer_ctrl_queue_size);
- steer_layer_1.tail(bias_2.size()) = relu(weight_2 * steer_input_full + bias_2);
+ const Eigen::VectorXd steer_input_full =
+ steer_normalize_ * x.segment(3 + acc_ctrl_queue_size_, steer_ctrl_queue_size_);
+ steer_layer_1.tail(bias_steer_layer_1_tail_.size()) =
+ relu(weight_steer_layer_1_tail_ * steer_input_full + bias_steer_layer_1_tail_);
- Eigen::VectorXd acc_layer_2 = (weight_3 * acc_layer_1 + bias_3).array().tanh();
+ const Eigen::VectorXd acc_layer_2 = relu(weight_acc_layer_2_ * acc_layer_1 + bias_acc_layer_2_);
- Eigen::VectorXd steer_layer_2 = (weight_4 * steer_layer_1 + bias_4).array().tanh();
+ const Eigen::VectorXd steer_layer_2 =
+ relu(weight_steer_layer_2_ * steer_layer_1 + bias_steer_layer_2_);
Eigen::VectorXd h1(1 + acc_layer_2.size() + steer_layer_2.size());
- h1[0] = x[0];
+ h1[0] = vel_normalize_ * x[0];
h1.segment(1, acc_layer_2.size()) = acc_layer_2;
h1.tail(steer_layer_2.size()) = steer_layer_2;
- Eigen::VectorXd h2 = relu(weight_5 * h1 + bias_5);
- Eigen::VectorXd h3 = relu(weight_6 * h2 + bias_6);
+ const Eigen::VectorXd h2 = relu(weight_linear_relu_1_ * h1 + bias_linear_relu_1_);
+ const Eigen::VectorXd h3 = relu(weight_linear_relu_2_ * h2 + bias_linear_relu_2_);
Eigen::VectorXd h4(h3.size() + acc_layer_2.size() + steer_layer_2.size());
h4.head(h3.size()) = h3;
h4.segment(h3.size(), acc_layer_2.size()) = acc_layer_2;
h4.tail(steer_layer_2.size()) = steer_layer_2;
- Eigen::VectorXd x_for_polynomial_reg(5);
+ Eigen::VectorXd x_for_polynomial_reg(9);
x_for_polynomial_reg.head(3) = x.head(3);
- x_for_polynomial_reg[3] = x[3 + acc_delay_step];
- x_for_polynomial_reg[4] = x[3 + acc_ctrl_queue_size + steer_delay_step];
+ const int acc_start = 3 + std::max(acc_delay_step_ - 3, 0);
+ x_for_polynomial_reg.segment(3, 3) = x.segment(acc_start, 3);
+ const int steer_start = 3 + acc_ctrl_queue_size_ + std::max(steer_delay_step_ - 3, 0);
+ x_for_polynomial_reg.segment(6, 3) = x.segment(steer_start, 3);
+
Eigen::VectorXd y =
- weight_7 * h4 + bias_7 +
- A_linear_reg * get_polynomial_features(x_for_polynomial_reg, deg, A_linear_reg.cols()) +
- b_linear_reg;
- y[4] = std::min(std::max(y[4], -max_acc_error), max_acc_error);
- y[5] = std::min(std::max(y[5], -max_steer_error), max_steer_error);
+ weight_finalize_ * h4 + bias_linear_finalize_ +
+ A_linear_reg_ * get_polynomial_features(x_for_polynomial_reg, deg_, A_linear_reg_.cols()) +
+ b_linear_reg_;
+ y[4] = std::min(std::max(y[4], -max_acc_error_), max_acc_error_);
+ y[5] = std::min(std::max(y[5], -max_steer_error_), max_steer_error_);
return y;
}
- Eigen::VectorXd rot_and_d_rot_error_prediction(Eigen::VectorXd x)
+ Eigen::VectorXd rot_and_d_rot_error_prediction(const Eigen::VectorXd & x) const
{
- int x_dim = x.size();
- double theta = x[3];
- double v = x[2];
+ const int x_dim = x.size();
+ const double theta = x[3];
+ const double v = x[2];
double coef = 2.0 * std::abs(v);
coef = coef * coef * coef * coef * coef * coef * coef;
if (coef > 1.0) {
@@ -279,8 +305,8 @@ class transform_model_to_eigen
Therefore, it may be safe to always set coef = 1, and this variable may be eliminated
once it is confirmed safe to do so.
*/
- double cos = std::cos(theta);
- double sin = std::sin(theta);
+ const double cos = std::cos(theta);
+ const double sin = std::sin(theta);
Eigen::Matrix2d Rot;
Rot << cos, -sin, sin, cos;
@@ -291,7 +317,7 @@ class transform_model_to_eigen
vars[1] = x[4];
vars[2] = x[5];
vars.tail(x_dim - 6) = x.tail(x_dim - 6);
- Eigen::VectorXd pred = error_prediction(vars);
+ const Eigen::VectorXd pred = error_prediction(vars);
Eigen::VectorXd rot_and_d_rot_pred(8);
rot_and_d_rot_pred.head(2) = Rot * pred.head(2);
rot_and_d_rot_pred.segment(2, 4) = pred.segment(2, 4);
@@ -299,117 +325,165 @@ class transform_model_to_eigen
return coef * rot_and_d_rot_pred;
}
- Eigen::MatrixXd error_prediction_with_diff(Eigen::VectorXd x)
+ Eigen::MatrixXd error_prediction_with_diff(const Eigen::VectorXd & x) const
{
- Eigen::VectorXd acc_sub(acc_ctrl_queue_size + 1);
- acc_sub[0] = x[1];
- acc_sub.tail(acc_ctrl_queue_size) = x.segment(3, acc_ctrl_queue_size);
+ Eigen::VectorXd acc_sub(acc_ctrl_queue_size_ + 1);
+ acc_sub[0] = acc_normalize_ * x[1];
+ acc_sub.tail(acc_ctrl_queue_size_) = acc_normalize_ * x.segment(3, acc_ctrl_queue_size_);
- Eigen::VectorXd steer_sub(steer_ctrl_queue_size_core + 1);
- steer_sub[0] = x[2];
- steer_sub.tail(steer_ctrl_queue_size_core) =
- x.segment(3 + acc_ctrl_queue_size, steer_ctrl_queue_size_core);
- Eigen::VectorXd steer_input_full = x.segment(3 + acc_ctrl_queue_size, steer_ctrl_queue_size);
+ Eigen::VectorXd steer_sub(steer_ctrl_queue_size_core_ + 1);
+ steer_sub[0] = steer_normalize_ * x[2];
+ steer_sub.tail(steer_ctrl_queue_size_core_) =
+ steer_normalize_ * x.segment(3 + acc_ctrl_queue_size_, steer_ctrl_queue_size_core_);
+ const Eigen::VectorXd steer_input_full =
+ steer_normalize_ * x.segment(3 + acc_ctrl_queue_size_, steer_ctrl_queue_size_);
- Eigen::VectorXd u_acc_layer_1 = weight_0 * acc_sub + bias_0;
- Eigen::VectorXd acc_layer_1 = relu(u_acc_layer_1);
+ const Eigen::VectorXd u_acc_layer_1 = weight_acc_layer_1_ * acc_sub + bias_acc_layer_1_;
+ const Eigen::VectorXd acc_layer_1 = relu(u_acc_layer_1);
- Eigen::VectorXd u_steer_layer_1(bias_1.size() + bias_2.size());
- u_steer_layer_1.head(bias_1.size()) = weight_1 * steer_sub + bias_1;
- u_steer_layer_1.tail(bias_2.size()) = weight_2 * steer_input_full + bias_2;
- Eigen::VectorXd steer_layer_1 = relu(u_steer_layer_1);
+ Eigen::VectorXd u_steer_layer_1(
+ bias_steer_layer_1_head_.size() + bias_steer_layer_1_tail_.size());
+ u_steer_layer_1.head(bias_steer_layer_1_head_.size()) =
+ weight_steer_layer_1_head_ * steer_sub + bias_steer_layer_1_head_;
+ u_steer_layer_1.tail(bias_steer_layer_1_tail_.size()) =
+ weight_steer_layer_1_tail_ * steer_input_full + bias_steer_layer_1_tail_;
+ const Eigen::VectorXd steer_layer_1 = relu(u_steer_layer_1);
- Eigen::VectorXd u_acc_layer_2 = weight_3 * acc_layer_1 + bias_3;
- Eigen::VectorXd acc_layer_2 = u_acc_layer_2.array().tanh();
+ const Eigen::VectorXd u_acc_layer_2 = weight_acc_layer_2_ * acc_layer_1 + bias_acc_layer_2_;
+ const Eigen::VectorXd acc_layer_2 = relu(u_acc_layer_2);
- Eigen::VectorXd u_steer_layer_2 = weight_4 * steer_layer_1 + bias_4;
- Eigen::VectorXd steer_layer_2 = u_steer_layer_2.array().tanh();
+ const Eigen::VectorXd u_steer_layer_2 =
+ weight_steer_layer_2_ * steer_layer_1 + bias_steer_layer_2_;
+ const Eigen::VectorXd steer_layer_2 = relu(u_steer_layer_2);
Eigen::VectorXd h1(1 + acc_layer_2.size() + steer_layer_2.size());
- h1[0] = x[0];
+ h1[0] = vel_normalize_ * x[0];
h1.segment(1, acc_layer_2.size()) = acc_layer_2;
h1.tail(steer_layer_2.size()) = steer_layer_2;
- Eigen::VectorXd u2 = weight_5 * h1 + bias_5;
- Eigen::VectorXd h2 = relu(u2);
- Eigen::VectorXd u3 = weight_6 * h2 + bias_6;
- Eigen::VectorXd h3 = relu(u3);
+ const Eigen::VectorXd u2 = weight_linear_relu_1_ * h1 + bias_linear_relu_1_;
+ const Eigen::VectorXd h2 = relu(u2);
+ const Eigen::VectorXd u3 = weight_linear_relu_2_ * h2 + bias_linear_relu_2_;
+ const Eigen::VectorXd h3 = relu(u3);
Eigen::VectorXd h4(h3.size() + acc_layer_2.size() + steer_layer_2.size());
h4.head(h3.size()) = h3;
h4.segment(h3.size(), acc_layer_2.size()) = acc_layer_2;
h4.tail(steer_layer_2.size()) = steer_layer_2;
- Eigen::VectorXd x_for_polynomial_reg(5);
+
+ Eigen::VectorXd x_for_polynomial_reg(9);
x_for_polynomial_reg.head(3) = x.head(3);
- x_for_polynomial_reg[3] = x[3 + acc_delay_step];
- x_for_polynomial_reg[4] = x[3 + acc_ctrl_queue_size + steer_delay_step];
- Eigen::MatrixXd polynomial_features_with_diff =
- get_polynomial_features_with_diff(x_for_polynomial_reg, deg, A_linear_reg.cols());
+ const int acc_start = 3 + std::max(acc_delay_step_ - 3, 0);
+ x_for_polynomial_reg.segment(3, 3) = x.segment(acc_start, 3);
+ const int steer_start = 3 + acc_ctrl_queue_size_ + std::max(steer_delay_step_ - 3, 0);
+ x_for_polynomial_reg.segment(6, 3) = x.segment(steer_start, 3);
+ const Eigen::MatrixXd polynomial_features_with_diff =
+ get_polynomial_features_with_diff(x_for_polynomial_reg, deg_, A_linear_reg_.cols());
Eigen::VectorXd y =
- weight_7 * h4 + bias_7 +
- A_linear_reg * polynomial_features_with_diff.block(0, 0, A_linear_reg.cols(), 1) +
- b_linear_reg;
- y[4] = std::min(std::max(y[4], -max_acc_error), max_acc_error);
- y[5] = std::min(std::max(y[5], -max_steer_error), max_steer_error);
- // Eigen::MatrixXd dy_dh4 = weight_7;
-
- Eigen::MatrixXd dy_dh3 = weight_7.block(0, 0, y.size(), h3.size());
- Eigen::MatrixXd dy_dh2 = d_relu_product(dy_dh3, u3) * weight_6;
- Eigen::MatrixXd dy_dh1 = d_relu_product(dy_dh2, u2) * weight_5;
-
- Eigen::MatrixXd dy_da2 = dy_dh1.block(0, 1, y.size(), acc_layer_2.size()) +
- weight_7.block(0, h3.size(), y.size(), acc_layer_2.size());
- Eigen::MatrixXd dy_ds2 =
+ weight_finalize_ * h4 + bias_linear_finalize_ +
+ A_linear_reg_ * polynomial_features_with_diff.block(0, 0, A_linear_reg_.cols(), 1) +
+ b_linear_reg_;
+ y[4] = std::min(std::max(y[4], -max_acc_error_), max_acc_error_);
+ y[5] = std::min(std::max(y[5], -max_steer_error_), max_steer_error_);
+
+ const Eigen::MatrixXd dy_dh3 = weight_finalize_.block(0, 0, y.size(), h3.size());
+ const Eigen::MatrixXd dy_dh2 = d_relu_product(dy_dh3, u3) * weight_linear_relu_2_;
+ const Eigen::MatrixXd dy_dh1 = d_relu_product(dy_dh2, u2) * weight_linear_relu_1_;
+
+ const Eigen::MatrixXd dy_da2 =
+ dy_dh1.block(0, 1, y.size(), acc_layer_2.size()) +
+ weight_finalize_.block(0, h3.size(), y.size(), acc_layer_2.size());
+ const Eigen::MatrixXd dy_ds2 =
dy_dh1.block(0, 1 + acc_layer_2.size(), y.size(), steer_layer_2.size()) +
- weight_7.block(0, h3.size() + acc_layer_2.size(), y.size(), steer_layer_2.size());
- Eigen::MatrixXd dy_da1 = d_tanh_product(dy_da2, u_acc_layer_2) * weight_3;
- Eigen::MatrixXd dy_ds1 = d_tanh_product(dy_ds2, u_steer_layer_2) * weight_4;
+ weight_finalize_.block(0, h3.size() + acc_layer_2.size(), y.size(), steer_layer_2.size());
+ const Eigen::MatrixXd dy_da1 = d_relu_product(dy_da2, u_acc_layer_2) * weight_acc_layer_2_;
+ const Eigen::MatrixXd dy_ds1 = d_relu_product(dy_ds2, u_steer_layer_2) * weight_steer_layer_2_;
- Eigen::MatrixXd dy_d_acc = d_relu_product(dy_da1, u_acc_layer_1) * weight_0;
+ const Eigen::MatrixXd dy_d_acc = d_relu_product(dy_da1, u_acc_layer_1) * weight_acc_layer_1_;
Eigen::MatrixXd dy_d_steer = Eigen::MatrixXd::Zero(y.size(), steer_input_full.size() + 1);
dy_d_steer.block(0, 1, y.size(), steer_input_full.size()) +=
d_relu_product(
- dy_ds1.block(0, bias_1.size(), y.size(), bias_2.size()),
- u_steer_layer_1.tail(bias_2.size())) *
- weight_2;
+ dy_ds1.block(0, bias_steer_layer_1_head_.size(), y.size(), bias_steer_layer_1_tail_.size()),
+ u_steer_layer_1.tail(bias_steer_layer_1_tail_.size())) *
+ weight_steer_layer_1_tail_;
dy_d_steer.block(0, 0, y.size(), steer_sub.size()) +=
d_relu_product(
- dy_ds1.block(0, 0, y.size(), bias_1.size()), u_steer_layer_1.head(bias_1.size())) *
- weight_1;
+ dy_ds1.block(0, 0, y.size(), bias_steer_layer_1_head_.size()),
+ u_steer_layer_1.head(bias_steer_layer_1_head_.size())) *
+ weight_steer_layer_1_head_;
Eigen::MatrixXd result = Eigen::MatrixXd::Zero(y.size(), x.size() + 1);
result.col(0) = y;
- result.col(1) = dy_dh1.col(0);
- result.col(2) = dy_d_acc.col(0);
- result.col(3) = dy_d_steer.col(0);
- result.block(0, 4, y.size(), acc_ctrl_queue_size) =
- dy_d_acc.block(0, 1, y.size(), acc_ctrl_queue_size);
- result.block(0, 4 + acc_ctrl_queue_size, y.size(), steer_ctrl_queue_size) =
- dy_d_steer.block(0, 1, y.size(), steer_ctrl_queue_size);
-
- Eigen::MatrixXd polynomial_reg_diff =
- A_linear_reg *
- polynomial_features_with_diff.block(0, 1, A_linear_reg.cols(), x_for_polynomial_reg.size());
+ result.col(1) = vel_normalize_ * dy_dh1.col(0);
+ result.col(2) = acc_normalize_ * dy_d_acc.col(0);
+ result.col(3) = steer_normalize_ * dy_d_steer.col(0);
+ result.block(0, 4, y.size(), acc_ctrl_queue_size_) =
+ acc_normalize_ * dy_d_acc.block(0, 1, y.size(), acc_ctrl_queue_size_);
+ result.block(0, 4 + acc_ctrl_queue_size_, y.size(), steer_ctrl_queue_size_) =
+ steer_normalize_ * dy_d_steer.block(0, 1, y.size(), steer_ctrl_queue_size_);
+
+ const Eigen::MatrixXd polynomial_reg_diff =
+ A_linear_reg_ *
+ polynomial_features_with_diff.block(0, 1, A_linear_reg_.cols(), x_for_polynomial_reg.size());
result.block(0, 1, y.size(), 3) += polynomial_reg_diff.block(0, 0, y.size(), 3);
- result.block(0, 4 + acc_delay_step, y.size(), 1) +=
- polynomial_reg_diff.block(0, 3, y.size(), 1);
- result.block(0, 4 + acc_ctrl_queue_size + steer_delay_step, y.size(), 1) +=
- polynomial_reg_diff.block(0, 4, y.size(), 1);
+ result.block(0, 1 + acc_start, y.size(), 3) += polynomial_reg_diff.block(0, 3, y.size(), 3);
+ result.block(0, 1 + steer_start, y.size(), 3) += polynomial_reg_diff.block(0, 6, y.size(), 3);
return result;
}
- Eigen::MatrixXd rot_and_d_rot_error_prediction_with_diff(Eigen::VectorXd x)
+ Eigen::MatrixXd rot_and_d_rot_error_prediction_with_diff(const Eigen::VectorXd & x) const
+ {
+ const int x_dim = x.size();
+ const double theta = x[3];
+ const double v = x[2];
+ double coef = 2.0 * std::abs(v);
+ coef = coef * coef * coef * coef * coef * coef * coef;
+ if (coef > 1.0) {
+ coef = 1.0;
+ }
+ const double cos = std::cos(theta);
+ const double sin = std::sin(theta);
+ Eigen::Matrix2d Rot;
+ Rot << cos, -sin, sin, cos;
+
+ Eigen::Matrix2d dRot;
+ dRot << -sin, -cos, cos, -sin;
+ Eigen::VectorXd vars(x_dim - 3);
+ vars[0] = x[2];
+ vars[1] = x[4];
+ vars[2] = x[5];
+ vars.tail(x_dim - 6) = x.tail(x_dim - 6);
+
+ const Eigen::MatrixXd pred_d_pred = error_prediction_with_diff(vars);
+ const Eigen::VectorXd pred = pred_d_pred.col(0);
+ const Eigen::MatrixXd d_pred = pred_d_pred.block(0, 1, 6, x_dim - 3);
+ Eigen::MatrixXd rot_and_d_rot_pred_with_diff = Eigen::MatrixXd::Zero(6, x_dim + 2);
+ Eigen::MatrixXd rot_pred_with_diff(6, x_dim - 3);
+ rot_pred_with_diff.block(0, 0, 2, x_dim - 3) = Rot * d_pred.block(0, 0, 2, x_dim - 3);
+ rot_pred_with_diff.block(2, 0, 4, x_dim - 3) = d_pred.block(2, 0, 4, x_dim - 3);
+
+ rot_and_d_rot_pred_with_diff.block(0, 0, 2, 1) = Rot * pred.head(2);
+ rot_and_d_rot_pred_with_diff.block(2, 0, 4, 1) = pred.segment(2, 4);
+ rot_and_d_rot_pred_with_diff.block(0, 1, 2, 1) = dRot * pred.head(2);
+ rot_and_d_rot_pred_with_diff.col(2 + 2) = rot_pred_with_diff.col(0);
+ rot_and_d_rot_pred_with_diff.col(2 + 4) = rot_pred_with_diff.col(1);
+ rot_and_d_rot_pred_with_diff.col(2 + 5) = rot_pred_with_diff.col(2);
+ rot_and_d_rot_pred_with_diff.block(0, 2 + 6, 6, x_dim - 6) =
+ rot_pred_with_diff.block(0, 3, 6, x_dim - 6);
+ return coef * rot_and_d_rot_pred_with_diff;
+ }
+ Eigen::MatrixXd rot_and_d_rot_error_prediction_with_poly_diff(const Eigen::VectorXd & x) const
{
- int x_dim = x.size();
- double theta = x[3];
- double v = x[2];
+ const int x_dim = x.size();
+ const double theta = x[3];
+ const double v = x[2];
double coef = 2.0 * std::abs(v);
coef = coef * coef * coef * coef * coef * coef * coef;
if (coef > 1.0) {
coef = 1.0;
}
- double cos = std::cos(theta);
- double sin = std::sin(theta);
+ const double cos = std::cos(theta);
+ const double sin = std::sin(theta);
Eigen::Matrix2d Rot;
Rot << cos, -sin, sin, cos;
@@ -421,9 +495,25 @@ class transform_model_to_eigen
vars[2] = x[5];
vars.tail(x_dim - 6) = x.tail(x_dim - 6);
- Eigen::MatrixXd pred_d_pred = error_prediction_with_diff(vars);
- Eigen::VectorXd pred = pred_d_pred.col(0);
- Eigen::MatrixXd d_pred = pred_d_pred.block(0, 1, 6, x_dim - 3);
+ const Eigen::VectorXd pred = error_prediction(vars);
+ Eigen::MatrixXd d_pred = Eigen::MatrixXd::Zero(6, x_dim - 3);
+
+ Eigen::VectorXd x_for_polynomial_reg(9);
+ x_for_polynomial_reg.head(3) = x.head(3);
+ const int acc_start = 3 + std::max(acc_delay_step_ - 3, 0);
+ x_for_polynomial_reg.segment(3, 3) = x.segment(acc_start, 3);
+ const int steer_start = 3 + acc_ctrl_queue_size_ + std::max(steer_delay_step_ - 3, 0);
+ x_for_polynomial_reg.segment(6, 3) = x.segment(steer_start, 3);
+
+ const Eigen::MatrixXd polynomial_features_with_diff =
+ get_polynomial_features_with_diff(x_for_polynomial_reg, deg_, A_linear_reg_.cols());
+ const Eigen::MatrixXd polynomial_reg_diff =
+ A_linear_reg_ *
+ polynomial_features_with_diff.block(0, 1, A_linear_reg_.cols(), x_for_polynomial_reg.size());
+ d_pred.block(0, 0, 6, 3) += polynomial_reg_diff.block(0, 0, 6, 3);
+ d_pred.block(0, 1 + acc_start, 6, 3) += polynomial_reg_diff.block(0, 3, 6, 3);
+ d_pred.block(0, 1 + steer_start, 6, 3) += polynomial_reg_diff.block(0, 6, 6, 3);
+
Eigen::MatrixXd rot_and_d_rot_pred_with_diff = Eigen::MatrixXd::Zero(6, x_dim + 2);
Eigen::MatrixXd rot_pred_with_diff(6, x_dim - 3);
rot_pred_with_diff.block(0, 0, 2, x_dim - 3) = Rot * d_pred.block(0, 0, 2, x_dim - 3);
@@ -439,22 +529,47 @@ class transform_model_to_eigen
rot_pred_with_diff.block(0, 3, 6, x_dim - 6);
return coef * rot_and_d_rot_pred_with_diff;
}
- Eigen::MatrixXd Rotated_error_prediction(Eigen::MatrixXd X)
+ Eigen::VectorXd rotated_error_prediction(const Eigen::VectorXd & x) const
{
- int X_cols = X.cols();
- int x_dim = X.rows();
+ const int x_dim = x.size();
+ const double theta = x[3];
+ const double v = x[2];
+ double coef = 2.0 * std::abs(v);
+ coef = coef * coef * coef * coef * coef * coef * coef;
+ if (coef > 1.0) {
+ coef = 1.0;
+ }
+ const double cos = std::cos(theta);
+ const double sin = std::sin(theta);
+ Eigen::Matrix2d Rot;
+ Rot << cos, -sin, sin, cos;
+ Eigen::VectorXd vars(x_dim - 3);
+ vars[0] = x[2];
+ vars[1] = x[4];
+ vars[2] = x[5];
+ vars.tail(x_dim - 6) = x.tail(x_dim - 6);
+ const Eigen::VectorXd pred = error_prediction(vars);
+ Eigen::VectorXd rot_pred(6);
+ rot_pred.head(2) = coef * Rot * pred.head(2);
+ rot_pred.tail(4) = coef * pred.tail(4);
+ return rot_pred;
+ }
+ Eigen::MatrixXd Rotated_error_prediction(const Eigen::MatrixXd & X) const
+ {
+ const int X_cols = X.cols();
+ const int x_dim = X.rows();
Eigen::MatrixXd Pred(6, X_cols);
for (int i = 0; i < X_cols; i++) {
- Eigen::VectorXd x = X.col(i);
- double theta = x[3];
- double v = x[2];
+ const Eigen::VectorXd x = X.col(i);
+ const double theta = x[3];
+ const double v = x[2];
double coef = 2.0 * std::abs(v);
coef = coef * coef * coef * coef * coef * coef * coef;
if (coef > 1.0) {
coef = 1.0;
}
- double cos = std::cos(theta);
- double sin = std::sin(theta);
+ const double cos = std::cos(theta);
+ const double sin = std::sin(theta);
Eigen::Matrix2d Rot;
Rot << cos, -sin, sin, cos;
Eigen::VectorXd vars(x_dim - 3);
@@ -464,14 +579,952 @@ class transform_model_to_eigen
vars[1] = x[4];
vars[2] = x[5];
vars.tail(x_dim - 6) = x.tail(x_dim - 6);
- Eigen::VectorXd pred = error_prediction(vars);
+ const Eigen::VectorXd pred = error_prediction(vars);
Pred.block(0, i, 2, 1) = coef * Rot * pred.head(2);
Pred.block(2, i, 4, 1) = coef * pred.tail(4);
}
return Pred;
}
};
+class transform_model_with_memory_to_eigen
+{
+private:
+ Eigen::MatrixXd weight_acc_layer_1_;
+ Eigen::MatrixXd weight_steer_layer_1_head_;
+ Eigen::MatrixXd weight_steer_layer_1_tail_;
+ Eigen::MatrixXd weight_acc_layer_2_;
+ Eigen::MatrixXd weight_steer_layer_2_;
+ Eigen::MatrixXd weight_lstm_ih_;
+ Eigen::MatrixXd weight_lstm_hh_;
+ Eigen::MatrixXd weight_linear_relu_1_;
+ Eigen::MatrixXd weight_linear_relu_2_;
+ Eigen::MatrixXd weight_finalize_;
+ Eigen::VectorXd bias_acc_layer_1_;
+ Eigen::VectorXd bias_steer_layer_1_head_;
+ Eigen::VectorXd bias_steer_layer_1_tail_;
+ Eigen::VectorXd bias_acc_layer_2_;
+ Eigen::VectorXd bias_steer_layer_2_;
+ Eigen::VectorXd bias_lstm_ih_;
+ Eigen::VectorXd bias_lstm_hh_;
+ Eigen::VectorXd bias_linear_relu_1_;
+ Eigen::VectorXd bias_linear_relu_2_;
+ Eigen::VectorXd bias_linear_finalize_;
+ Eigen::MatrixXd A_linear_reg_;
+ Eigen::VectorXd b_linear_reg_;
+ int deg_;
+ int acc_delay_step_;
+ int steer_delay_step_;
+ int acc_ctrl_queue_size_;
+ int steer_ctrl_queue_size_;
+ int steer_ctrl_queue_size_core_;
+ double vel_normalize_;
+ double acc_normalize_;
+ double steer_normalize_;
+
+ static constexpr double max_acc_error_ = 20.0;
+ static constexpr double max_steer_error_ = 20.0;
+ Eigen::VectorXd h_, c_;
+ Eigen::MatrixXd H_, C_;
+ Eigen::MatrixXd dy_dhc_, dhc_dhc_, dhc_dx_;
+ Eigen::MatrixXd dy_dhc_pre_, dhc_dx_pre_;
+
+public:
+ transform_model_with_memory_to_eigen() {}
+ void set_params(
+ const Eigen::MatrixXd & weight_acc_layer_1, const Eigen::MatrixXd & weight_steer_layer_1_head,
+ const Eigen::MatrixXd & weight_steer_layer_1_tail, const Eigen::MatrixXd & weight_acc_layer_2,
+ const Eigen::MatrixXd & weight_steer_layer_2, const Eigen::MatrixXd & weight_lstm_ih,
+ const Eigen::MatrixXd & weight_lstm_hh, const Eigen::MatrixXd & weight_linear_relu_1,
+ const Eigen::MatrixXd & weight_linear_relu_2, const Eigen::MatrixXd & weight_finalize,
+ const Eigen::VectorXd & bias_acc_layer_1, const Eigen::VectorXd & bias_steer_layer_1_head,
+ const Eigen::VectorXd & bias_steer_layer_1_tail, const Eigen::VectorXd & bias_acc_layer_2,
+ const Eigen::VectorXd & bias_steer_layer_2, const Eigen::VectorXd & bias_lstm_ih,
+ const Eigen::VectorXd & bias_lstm_hh, const Eigen::VectorXd & bias_linear_relu_1,
+ const Eigen::VectorXd & bias_linear_relu_2, const Eigen::VectorXd & bias_linear_finalize)
+ {
+ weight_acc_layer_1_ = weight_acc_layer_1;
+ weight_steer_layer_1_head_ = weight_steer_layer_1_head;
+ weight_steer_layer_1_tail_ = weight_steer_layer_1_tail;
+ weight_acc_layer_2_ = weight_acc_layer_2;
+ weight_steer_layer_2_ = weight_steer_layer_2;
+ weight_lstm_ih_ = weight_lstm_ih;
+ weight_lstm_hh_ = weight_lstm_hh;
+ weight_linear_relu_1_ = weight_linear_relu_1;
+ weight_linear_relu_2_ = weight_linear_relu_2;
+ weight_finalize_ = weight_finalize;
+ bias_acc_layer_1_ = bias_acc_layer_1;
+ bias_steer_layer_1_head_ = bias_steer_layer_1_head;
+ bias_steer_layer_1_tail_ = bias_steer_layer_1_tail;
+ bias_acc_layer_2_ = bias_acc_layer_2;
+ bias_steer_layer_2_ = bias_steer_layer_2;
+ bias_lstm_ih_ = bias_lstm_ih;
+ bias_lstm_hh_ = bias_lstm_hh;
+ bias_linear_relu_1_ = bias_linear_relu_1;
+ bias_linear_relu_2_ = bias_linear_relu_2;
+ bias_linear_finalize_ = bias_linear_finalize;
+ }
+ void set_params_res(
+ const Eigen::MatrixXd & A_linear_reg, const Eigen::VectorXd & b_linear_reg, const int deg,
+ const int acc_delay_step, const int steer_delay_step, const int acc_ctrl_queue_size,
+ const int steer_ctrl_queue_size, const int steer_ctrl_queue_size_core,
+ const double vel_normalize, const double acc_normalize, const double steer_normalize)
+ {
+ A_linear_reg_ = A_linear_reg;
+ b_linear_reg_ = b_linear_reg;
+ deg_ = deg;
+ acc_delay_step_ = acc_delay_step;
+ steer_delay_step_ = steer_delay_step;
+ acc_ctrl_queue_size_ = acc_ctrl_queue_size;
+ steer_ctrl_queue_size_ = steer_ctrl_queue_size;
+ steer_ctrl_queue_size_core_ = steer_ctrl_queue_size_core;
+ vel_normalize_ = vel_normalize;
+ acc_normalize_ = acc_normalize;
+ steer_normalize_ = steer_normalize;
+ const int h_dim = weight_lstm_hh_.cols();
+ h_ = Eigen::VectorXd::Zero(h_dim);
+ c_ = Eigen::VectorXd::Zero(h_dim);
+ dy_dhc_pre_ = Eigen::MatrixXd::Zero(6, 2 * h_dim);
+ dhc_dx_pre_ =
+ Eigen::MatrixXd::Zero(2 * h_dim, 3 + acc_ctrl_queue_size_ + steer_ctrl_queue_size_);
+ dy_dhc_ = Eigen::MatrixXd::Zero(6, 2 * h_dim);
+ dhc_dhc_ = Eigen::MatrixXd::Zero(2 * h_dim, 2 * h_dim);
+ dhc_dx_ = Eigen::MatrixXd::Zero(2 * h_dim, 6 + acc_ctrl_queue_size_ + steer_ctrl_queue_size_);
+ }
+ void set_lstm(const Eigen::VectorXd & h, const Eigen::VectorXd & c)
+ {
+ h_ = h;
+ c_ = c;
+ }
+ void set_lstm_for_candidate(
+ const Eigen::VectorXd & h, const Eigen::VectorXd & c, const int sample_size)
+ {
+ H_ = Eigen::MatrixXd::Zero(h.size(), sample_size);
+ C_ = Eigen::MatrixXd::Zero(c.size(), sample_size);
+ for (int i = 0; i < sample_size; i++) {
+ H_.col(i) = h;
+ C_.col(i) = c;
+ }
+ }
+ Eigen::VectorXd get_h() const { return h_; }
+ Eigen::VectorXd get_c() const { return c_; }
+ Eigen::MatrixXd get_dy_dhc() const { return dy_dhc_; }
+ Eigen::MatrixXd get_dhc_dhc() const { return dhc_dhc_; }
+ Eigen::MatrixXd get_dhc_dx() const { return dhc_dx_; }
+ Eigen::VectorXd error_prediction(const Eigen::VectorXd & x, const int cell_index)
+ {
+ Eigen::VectorXd acc_sub(acc_ctrl_queue_size_ + 1);
+ acc_sub[0] = acc_normalize_ * x[1];
+ acc_sub.tail(acc_ctrl_queue_size_) = acc_normalize_ * x.segment(3, acc_ctrl_queue_size_);
+ Eigen::VectorXd steer_sub(steer_ctrl_queue_size_core_ + 1);
+ steer_sub[0] = steer_normalize_ * x[2];
+ steer_sub.tail(steer_ctrl_queue_size_core_) =
+ steer_normalize_ * x.segment(3 + acc_ctrl_queue_size_, steer_ctrl_queue_size_core_);
+ const Eigen::VectorXd acc_layer_1 = relu(weight_acc_layer_1_ * acc_sub + bias_acc_layer_1_);
+ Eigen::VectorXd steer_layer_1(
+ bias_steer_layer_1_head_.size() + bias_steer_layer_1_tail_.size());
+ steer_layer_1.head(bias_steer_layer_1_head_.size()) =
+ relu(weight_steer_layer_1_head_ * steer_sub + bias_steer_layer_1_head_);
+
+ const Eigen::VectorXd steer_input_full =
+ steer_normalize_ * x.segment(3 + acc_ctrl_queue_size_, steer_ctrl_queue_size_);
+ steer_layer_1.tail(bias_steer_layer_1_tail_.size()) =
+ relu(weight_steer_layer_1_tail_ * steer_input_full + bias_steer_layer_1_tail_);
+
+ const Eigen::VectorXd acc_layer_2 = relu(weight_acc_layer_2_ * acc_layer_1 + bias_acc_layer_2_);
+
+ const Eigen::VectorXd steer_layer_2 =
+ relu(weight_steer_layer_2_ * steer_layer_1 + bias_steer_layer_2_);
+ Eigen::VectorXd h1(1 + acc_layer_2.size() + steer_layer_2.size());
+ h1[0] = vel_normalize_ * x[0];
+ h1.segment(1, acc_layer_2.size()) = acc_layer_2;
+ h1.tail(steer_layer_2.size()) = steer_layer_2;
+ Eigen::VectorXd h, c;
+ if (cell_index < 0) {
+ h = h_;
+ c = c_;
+ } else {
+ h = H_.col(cell_index);
+ c = C_.col(cell_index);
+ }
+
+ const Eigen::VectorXd i_new = sigmoid(
+ weight_lstm_ih_.block(0, 0, h_.size(), h1.size()) * h1 + bias_lstm_ih_.head(h_.size()) +
+ weight_lstm_hh_.block(0, 0, h_.size(), h_.size()) * h + bias_lstm_hh_.head(h_.size()));
+ const Eigen::VectorXd f_new = sigmoid(
+ weight_lstm_ih_.block(h_.size(), 0, h_.size(), h1.size()) * h1 +
+ bias_lstm_ih_.segment(h_.size(), h_.size()) +
+ weight_lstm_hh_.block(h_.size(), 0, h_.size(), h_.size()) * h +
+ bias_lstm_hh_.segment(h_.size(), h_.size()));
+ const Eigen::VectorXd g_new = tanh(
+ weight_lstm_ih_.block(2 * h_.size(), 0, h_.size(), h1.size()) * h1 +
+ bias_lstm_ih_.segment(2 * h_.size(), h_.size()) +
+ weight_lstm_hh_.block(2 * h_.size(), 0, h_.size(), h_.size()) * h +
+ bias_lstm_hh_.segment(2 * h_.size(), h_.size()));
+ const Eigen::VectorXd o_new = sigmoid(
+ weight_lstm_ih_.block(3 * h_.size(), 0, h_.size(), h1.size()) * h1 +
+ bias_lstm_ih_.segment(3 * h_.size(), h_.size()) +
+ weight_lstm_hh_.block(3 * h_.size(), 0, h_.size(), h_.size()) * h +
+ bias_lstm_hh_.segment(3 * h_.size(), h_.size()));
+ const Eigen::VectorXd c_new = f_new.array() * c.array() + i_new.array() * g_new.array();
+ const Eigen::VectorXd h_new = o_new.array() * tanh(c_new).array();
+
+ Eigen::VectorXd h2(h_new.size() + bias_linear_relu_1_.size());
+ h2.head(h_new.size()) = h_new;
+ h2.tail(bias_linear_relu_1_.size()) = relu(weight_linear_relu_1_ * h1 + bias_linear_relu_1_);
+
+ const Eigen::VectorXd h3 = relu(weight_linear_relu_2_ * h2 + bias_linear_relu_2_);
+ Eigen::VectorXd h4(h3.size() + acc_layer_2.size() + steer_layer_2.size());
+ h4.head(h3.size()) = h3;
+ h4.segment(h3.size(), acc_layer_2.size()) = acc_layer_2;
+ h4.tail(steer_layer_2.size()) = steer_layer_2;
+
+ Eigen::VectorXd x_for_polynomial_reg(9);
+ x_for_polynomial_reg.head(3) = x.head(3);
+ const int acc_start = 3 + std::max(acc_delay_step_ - 3, 0);
+ x_for_polynomial_reg.segment(3, 3) = x.segment(acc_start, 3);
+ const int steer_start = 3 + acc_ctrl_queue_size_ + std::max(steer_delay_step_ - 3, 0);
+ x_for_polynomial_reg.segment(6, 3) = x.segment(steer_start, 3);
+
+ Eigen::VectorXd y =
+ weight_finalize_ * h4 + bias_linear_finalize_ +
+ A_linear_reg_ * get_polynomial_features(x_for_polynomial_reg, deg_, A_linear_reg_.cols()) +
+ b_linear_reg_;
+
+ y[4] = std::min(std::max(y[4], -max_acc_error_), max_acc_error_);
+ y[5] = std::min(std::max(y[5], -max_steer_error_), max_steer_error_);
+
+ if (cell_index < 0) {
+ h_ = h_new;
+ c_ = c_new;
+ } else {
+ H_.col(cell_index) = h_new;
+ C_.col(cell_index) = c_new;
+ }
+ return y;
+ }
+
+ Eigen::MatrixXd error_prediction_with_diff(const Eigen::VectorXd & x)
+ {
+ Eigen::VectorXd acc_sub(acc_ctrl_queue_size_ + 1);
+ acc_sub[0] = acc_normalize_ * x[1];
+ acc_sub.tail(acc_ctrl_queue_size_) = acc_normalize_ * x.segment(3, acc_ctrl_queue_size_);
+ Eigen::VectorXd steer_sub(steer_ctrl_queue_size_core_ + 1);
+ steer_sub[0] = steer_normalize_ * x[2];
+ steer_sub.tail(steer_ctrl_queue_size_core_) =
+ steer_normalize_ * x.segment(3 + acc_ctrl_queue_size_, steer_ctrl_queue_size_core_);
+ const Eigen::VectorXd steer_input_full =
+ steer_normalize_ * x.segment(3 + acc_ctrl_queue_size_, steer_ctrl_queue_size_);
+
+ const Eigen::VectorXd u_acc_layer_1 = weight_acc_layer_1_ * acc_sub + bias_acc_layer_1_;
+ const Eigen::VectorXd acc_layer_1 = relu(u_acc_layer_1);
+
+ Eigen::VectorXd u_steer_layer_1(
+ bias_steer_layer_1_head_.size() + bias_steer_layer_1_tail_.size());
+ u_steer_layer_1.head(bias_steer_layer_1_head_.size()) =
+ weight_steer_layer_1_head_ * steer_sub + bias_steer_layer_1_head_;
+ u_steer_layer_1.tail(bias_steer_layer_1_tail_.size()) =
+ weight_steer_layer_1_tail_ * steer_input_full + bias_steer_layer_1_tail_;
+ const Eigen::VectorXd steer_layer_1 = relu(u_steer_layer_1);
+
+ const Eigen::VectorXd u_acc_layer_2 = weight_acc_layer_2_ * acc_layer_1 + bias_acc_layer_2_;
+ const Eigen::VectorXd acc_layer_2 = relu(u_acc_layer_2);
+
+ const Eigen::VectorXd u_steer_layer_2 =
+ weight_steer_layer_2_ * steer_layer_1 + bias_steer_layer_2_;
+ const Eigen::VectorXd steer_layer_2 = relu(u_steer_layer_2);
+
+ Eigen::VectorXd h1(1 + acc_layer_2.size() + steer_layer_2.size());
+ h1[0] = vel_normalize_ * x[0];
+ h1.segment(1, acc_layer_2.size()) = acc_layer_2;
+ h1.tail(steer_layer_2.size()) = steer_layer_2;
+
+ const Eigen::VectorXd u_i_new =
+ weight_lstm_ih_.block(0, 0, h_.size(), h1.size()) * h1 + bias_lstm_ih_.head(h_.size()) +
+ weight_lstm_hh_.block(0, 0, h_.size(), h_.size()) * h_ + bias_lstm_hh_.head(h_.size());
+ const Eigen::VectorXd u_f_new = weight_lstm_ih_.block(h_.size(), 0, h_.size(), h1.size()) * h1 +
+ bias_lstm_ih_.segment(h_.size(), h_.size()) +
+ weight_lstm_hh_.block(h_.size(), 0, h_.size(), h_.size()) * h_ +
+ bias_lstm_hh_.segment(h_.size(), h_.size());
+ const Eigen::VectorXd u_g_new =
+ weight_lstm_ih_.block(2 * h_.size(), 0, h_.size(), h1.size()) * h1 +
+ bias_lstm_ih_.segment(2 * h_.size(), h_.size()) +
+ weight_lstm_hh_.block(2 * h_.size(), 0, h_.size(), h_.size()) * h_ +
+ bias_lstm_hh_.segment(2 * h_.size(), h_.size());
+ const Eigen::VectorXd u_o_new =
+ weight_lstm_ih_.block(3 * h_.size(), 0, h_.size(), h1.size()) * h1 +
+ bias_lstm_ih_.segment(3 * h_.size(), h_.size()) +
+ weight_lstm_hh_.block(3 * h_.size(), 0, h_.size(), h_.size()) * h_ +
+ bias_lstm_hh_.segment(3 * h_.size(), h_.size());
+ const Eigen::VectorXd i_new = sigmoid(u_i_new);
+ const Eigen::VectorXd f_new = sigmoid(u_f_new);
+ const Eigen::VectorXd g_new = tanh(u_g_new);
+ const Eigen::VectorXd o_new = sigmoid(u_o_new);
+
+ const Eigen::VectorXd c_new = f_new.array() * c_.array() + i_new.array() * g_new.array();
+ const Eigen::VectorXd h_new = o_new.array() * tanh(c_new).array();
+
+ Eigen::VectorXd h2(h_new.size() + bias_linear_relu_1_.size());
+ h2.head(h_new.size()) = h_new;
+ const Eigen::VectorXd u2 = weight_linear_relu_1_ * h1 + bias_linear_relu_1_;
+ h2.tail(bias_linear_relu_1_.size()) = relu(u2);
+
+ const Eigen::VectorXd u3 = weight_linear_relu_2_ * h2 + bias_linear_relu_2_;
+ const Eigen::VectorXd h3 = relu(u3);
+ Eigen::VectorXd h4(h3.size() + acc_layer_2.size() + steer_layer_2.size());
+
+ h4.head(h3.size()) = h3;
+ h4.segment(h3.size(), acc_layer_2.size()) = acc_layer_2;
+ h4.tail(steer_layer_2.size()) = steer_layer_2;
+
+ Eigen::VectorXd x_for_polynomial_reg(9);
+ x_for_polynomial_reg.head(3) = x.head(3);
+ const int acc_start = 3 + std::max(acc_delay_step_ - 3, 0);
+ x_for_polynomial_reg.segment(3, 3) = x.segment(acc_start, 3);
+ const int steer_start = 3 + acc_ctrl_queue_size_ + std::max(steer_delay_step_ - 3, 0);
+ x_for_polynomial_reg.segment(6, 3) = x.segment(steer_start, 3);
+
+ const Eigen::MatrixXd polynomial_features_with_diff =
+ get_polynomial_features_with_diff(x_for_polynomial_reg, deg_, A_linear_reg_.cols());
+
+ Eigen::VectorXd y =
+ weight_finalize_ * h4 + bias_linear_finalize_ +
+ A_linear_reg_ * polynomial_features_with_diff.block(0, 0, A_linear_reg_.cols(), 1) +
+ b_linear_reg_;
+
+ y[4] = std::min(std::max(y[4], -max_acc_error_), max_acc_error_);
+ y[5] = std::min(std::max(y[5], -max_steer_error_), max_steer_error_);
+
+ const Eigen::MatrixXd dy_dh3 = weight_finalize_.block(0, 0, y.size(), h3.size());
+ const Eigen::MatrixXd dy_dh2 = d_relu_product(dy_dh3, u3) * weight_linear_relu_2_;
+ const Eigen::MatrixXd dy_dh2_head = dy_dh2.block(0, 0, y.size(), h_new.size());
+ const Eigen::MatrixXd dy_dh2_tail =
+ dy_dh2.block(0, h_new.size(), y.size(), bias_linear_relu_1_.size());
+
+ const Eigen::MatrixXd dy_do = dy_dh2_head * tanh(c_new).asDiagonal();
+ const Eigen::MatrixXd dy_dc_new = d_tanh_product(dy_dh2_head * o_new.asDiagonal(), c_new);
+ Eigen::MatrixXd dy_dh1 = d_sigmoid_product(dy_do, u_o_new) *
+ weight_lstm_ih_.block(3 * h_.size(), 0, h_.size(), h1.size());
+
+ dy_dh1 += d_sigmoid_product(dy_dc_new * c_.asDiagonal(), u_f_new) *
+ weight_lstm_ih_.block(h_.size(), 0, h_.size(), h1.size());
+ dy_dh1 += d_tanh_product(dy_dc_new * i_new.asDiagonal(), u_g_new) *
+ weight_lstm_ih_.block(2 * h_.size(), 0, h_.size(), h1.size());
+ dy_dh1 += d_sigmoid_product(dy_dc_new * g_new.asDiagonal(), u_i_new) *
+ weight_lstm_ih_.block(0, 0, h_.size(), h1.size());
+
+ dy_dh1 += d_relu_product(dy_dh2_tail, u2) * weight_linear_relu_1_;
+
+ const Eigen::MatrixXd dy_da2 =
+ dy_dh1.block(0, 1, y.size(), acc_layer_2.size()) +
+ weight_finalize_.block(0, h3.size(), y.size(), acc_layer_2.size());
+ const Eigen::MatrixXd dy_ds2 =
+ dy_dh1.block(0, 1 + acc_layer_2.size(), y.size(), steer_layer_2.size()) +
+ weight_finalize_.block(0, h3.size() + acc_layer_2.size(), y.size(), steer_layer_2.size());
+ const Eigen::MatrixXd dy_da1 = d_relu_product(dy_da2, u_acc_layer_2) * weight_acc_layer_2_;
+ const Eigen::MatrixXd dy_ds1 = d_relu_product(dy_ds2, u_steer_layer_2) * weight_steer_layer_2_;
+
+ const Eigen::MatrixXd dy_d_acc = d_relu_product(dy_da1, u_acc_layer_1) * weight_acc_layer_1_;
+ Eigen::MatrixXd dy_d_steer = Eigen::MatrixXd::Zero(y.size(), steer_input_full.size() + 1);
+ dy_d_steer.block(0, 1, y.size(), steer_input_full.size()) +=
+ d_relu_product(
+ dy_ds1.block(0, bias_steer_layer_1_head_.size(), y.size(), bias_steer_layer_1_tail_.size()),
+ u_steer_layer_1.tail(bias_steer_layer_1_tail_.size())) *
+ weight_steer_layer_1_tail_;
+ dy_d_steer.block(0, 0, y.size(), steer_sub.size()) +=
+ d_relu_product(
+ dy_ds1.block(0, 0, y.size(), bias_steer_layer_1_head_.size()),
+ u_steer_layer_1.head(bias_steer_layer_1_head_.size())) *
+ weight_steer_layer_1_head_;
+
+ Eigen::MatrixXd result = Eigen::MatrixXd::Zero(y.size(), x.size() + 1);
+
+ result.col(0) = y;
+
+ result.col(1) = vel_normalize_ * dy_dh1.col(0);
+ result.col(2) = acc_normalize_ * dy_d_acc.col(0);
+ result.col(3) = steer_normalize_ * dy_d_steer.col(0);
+ result.block(0, 4, y.size(), acc_ctrl_queue_size_) =
+ acc_normalize_ * dy_d_acc.block(0, 1, y.size(), acc_ctrl_queue_size_);
+ result.block(0, 4 + acc_ctrl_queue_size_, y.size(), steer_ctrl_queue_size_) =
+ steer_normalize_ * dy_d_steer.block(0, 1, y.size(), steer_ctrl_queue_size_);
+
+ const Eigen::MatrixXd polynomial_reg_diff =
+ A_linear_reg_ *
+ polynomial_features_with_diff.block(0, 1, A_linear_reg_.cols(), x_for_polynomial_reg.size());
+ result.block(0, 1, y.size(), 3) += polynomial_reg_diff.block(0, 0, y.size(), 3);
+ result.block(0, 1 + acc_start, y.size(), 3) += polynomial_reg_diff.block(0, 3, y.size(), 3);
+ result.block(0, 1 + steer_start, y.size(), 3) += polynomial_reg_diff.block(0, 6, y.size(), 3);
+
+ h_ = h_new;
+ c_ = c_new;
+ return result;
+ }
+ Eigen::MatrixXd error_prediction_with_memory_diff(const Eigen::VectorXd & x)
+ {
+ Eigen::VectorXd acc_sub(acc_ctrl_queue_size_ + 1);
+ acc_sub[0] = acc_normalize_ * x[1];
+ acc_sub.tail(acc_ctrl_queue_size_) = acc_normalize_ * x.segment(3, acc_ctrl_queue_size_);
+ Eigen::VectorXd steer_sub(steer_ctrl_queue_size_core_ + 1);
+ steer_sub[0] = steer_normalize_ * x[2];
+ steer_sub.tail(steer_ctrl_queue_size_core_) =
+ steer_normalize_ * x.segment(3 + acc_ctrl_queue_size_, steer_ctrl_queue_size_core_);
+ const Eigen::VectorXd steer_input_full =
+ steer_normalize_ * x.segment(3 + acc_ctrl_queue_size_, steer_ctrl_queue_size_);
+
+ const Eigen::VectorXd u_acc_layer_1 = weight_acc_layer_1_ * acc_sub + bias_acc_layer_1_;
+ const Eigen::VectorXd acc_layer_1 = relu(u_acc_layer_1);
+
+ Eigen::VectorXd u_steer_layer_1(
+ bias_steer_layer_1_head_.size() + bias_steer_layer_1_tail_.size());
+ u_steer_layer_1.head(bias_steer_layer_1_head_.size()) =
+ weight_steer_layer_1_head_ * steer_sub + bias_steer_layer_1_head_;
+ u_steer_layer_1.tail(bias_steer_layer_1_tail_.size()) =
+ weight_steer_layer_1_tail_ * steer_input_full + bias_steer_layer_1_tail_;
+ const Eigen::VectorXd steer_layer_1 = relu(u_steer_layer_1);
+
+ const Eigen::VectorXd u_acc_layer_2 = weight_acc_layer_2_ * acc_layer_1 + bias_acc_layer_2_;
+ const Eigen::VectorXd acc_layer_2 = relu(u_acc_layer_2);
+
+ const Eigen::VectorXd u_steer_layer_2 =
+ weight_steer_layer_2_ * steer_layer_1 + bias_steer_layer_2_;
+ const Eigen::VectorXd steer_layer_2 = relu(u_steer_layer_2);
+
+ Eigen::VectorXd h1(1 + acc_layer_2.size() + steer_layer_2.size());
+ h1[0] = vel_normalize_ * x[0];
+ h1.segment(1, acc_layer_2.size()) = acc_layer_2;
+ h1.tail(steer_layer_2.size()) = steer_layer_2;
+
+ const Eigen::VectorXd u_i_new =
+ weight_lstm_ih_.block(0, 0, h_.size(), h1.size()) * h1 + bias_lstm_ih_.head(h_.size()) +
+ weight_lstm_hh_.block(0, 0, h_.size(), h_.size()) * h_ + bias_lstm_hh_.head(h_.size());
+ const Eigen::VectorXd u_f_new = weight_lstm_ih_.block(h_.size(), 0, h_.size(), h1.size()) * h1 +
+ bias_lstm_ih_.segment(h_.size(), h_.size()) +
+ weight_lstm_hh_.block(h_.size(), 0, h_.size(), h_.size()) * h_ +
+ bias_lstm_hh_.segment(h_.size(), h_.size());
+ const Eigen::VectorXd u_g_new =
+ weight_lstm_ih_.block(2 * h_.size(), 0, h_.size(), h1.size()) * h1 +
+ bias_lstm_ih_.segment(2 * h_.size(), h_.size()) +
+ weight_lstm_hh_.block(2 * h_.size(), 0, h_.size(), h_.size()) * h_ +
+ bias_lstm_hh_.segment(2 * h_.size(), h_.size());
+ const Eigen::VectorXd u_o_new =
+ weight_lstm_ih_.block(3 * h_.size(), 0, h_.size(), h1.size()) * h1 +
+ bias_lstm_ih_.segment(3 * h_.size(), h_.size()) +
+ weight_lstm_hh_.block(3 * h_.size(), 0, h_.size(), h_.size()) * h_ +
+ bias_lstm_hh_.segment(3 * h_.size(), h_.size());
+ const Eigen::VectorXd i_new = sigmoid(u_i_new);
+ const Eigen::VectorXd f_new = sigmoid(u_f_new);
+ const Eigen::VectorXd g_new = tanh(u_g_new);
+ const Eigen::VectorXd o_new = sigmoid(u_o_new);
+
+ const Eigen::VectorXd c_new = f_new.array() * c_.array() + i_new.array() * g_new.array();
+ const Eigen::VectorXd h_new = o_new.array() * tanh(c_new).array();
+
+ Eigen::VectorXd h2(h_new.size() + bias_linear_relu_1_.size());
+ h2.head(h_new.size()) = h_new;
+ const Eigen::VectorXd u2 = weight_linear_relu_1_ * h1 + bias_linear_relu_1_;
+ h2.tail(bias_linear_relu_1_.size()) = relu(u2);
+
+ const Eigen::VectorXd u3 = weight_linear_relu_2_ * h2 + bias_linear_relu_2_;
+ const Eigen::VectorXd h3 = relu(u3);
+ Eigen::VectorXd h4(h3.size() + acc_layer_2.size() + steer_layer_2.size());
+
+ h4.head(h3.size()) = h3;
+ h4.segment(h3.size(), acc_layer_2.size()) = acc_layer_2;
+ h4.tail(steer_layer_2.size()) = steer_layer_2;
+
+ Eigen::VectorXd x_for_polynomial_reg(9);
+ x_for_polynomial_reg.head(3) = x.head(3);
+ const int acc_start = 3 + std::max(acc_delay_step_ - 3, 0);
+ x_for_polynomial_reg.segment(3, 3) = x.segment(acc_start, 3);
+ const int steer_start = 3 + acc_ctrl_queue_size_ + std::max(steer_delay_step_ - 3, 0);
+ x_for_polynomial_reg.segment(6, 3) = x.segment(steer_start, 3);
+
+ const Eigen::MatrixXd polynomial_features_with_diff =
+ get_polynomial_features_with_diff(x_for_polynomial_reg, deg_, A_linear_reg_.cols());
+
+ Eigen::VectorXd y =
+ weight_finalize_ * h4 + bias_linear_finalize_ +
+ A_linear_reg_ * polynomial_features_with_diff.block(0, 0, A_linear_reg_.cols(), 1) +
+ b_linear_reg_;
+
+ y[4] = std::min(std::max(y[4], -max_acc_error_), max_acc_error_);
+ y[5] = std::min(std::max(y[5], -max_steer_error_), max_steer_error_);
+
+ const Eigen::MatrixXd dy_dh3 = weight_finalize_.block(0, 0, y.size(), h3.size());
+ const Eigen::MatrixXd dy_dh2 = d_relu_product(dy_dh3, u3) * weight_linear_relu_2_;
+ const Eigen::MatrixXd dy_dh2_head = dy_dh2.block(0, 0, y.size(), h_new.size());
+ const Eigen::MatrixXd dy_dh2_tail =
+ dy_dh2.block(0, h_new.size(), y.size(), bias_linear_relu_1_.size());
+
+ const Eigen::MatrixXd dy_do = dy_dh2_head * tanh(c_new).asDiagonal();
+ const Eigen::MatrixXd dy_dc_new = d_tanh_product(dy_dh2_head * o_new.asDiagonal(), c_new);
+
+ // calc dy_dhc_pre_, dhc_dhc_, dhc_dx_pre_
+ const Eigen::VectorXd dc_du_f = d_sigmoid_product_vec(c_, u_f_new);
+ const Eigen::VectorXd dc_du_g = d_tanh_product_vec(i_new, u_g_new);
+ const Eigen::VectorXd dc_du_i = d_sigmoid_product_vec(g_new, u_i_new);
+ const Eigen::VectorXd dh_dc_new = d_tanh_product_vec(o_new, c_new);
+ const Eigen::VectorXd dh_du_o = d_sigmoid_product_vec(tanh(c_new), u_o_new);
+
+ const Eigen::MatrixXd dc_dc = f_new.asDiagonal();
+ const Eigen::MatrixXd dy_dc = dy_dc_new * dc_dc;
+
+ Eigen::MatrixXd dc_dh =
+ dc_du_f.asDiagonal() * weight_lstm_hh_.block(h_.size(), 0, h_.size(), h_.size());
+ dc_dh += dc_du_g.asDiagonal() * weight_lstm_hh_.block(2 * h_.size(), 0, h_.size(), h_.size());
+ dc_dh += dc_du_i.asDiagonal() * weight_lstm_hh_.block(0, 0, h_.size(), h_.size());
+ const Eigen::VectorXd dh_dc = dh_dc_new.array() * f_new.array();
+
+ Eigen::MatrixXd dh_dh =
+ dh_du_o.asDiagonal() * weight_lstm_hh_.block(3 * h_.size(), 0, h_.size(), h_.size());
+
+ dh_dh += dh_dc_new.asDiagonal() * dc_dh;
+
+ const Eigen::MatrixXd dy_dh = dy_dh2_head * dh_dh;
+ Eigen::MatrixXd dc_dh1 =
+ dc_du_f.asDiagonal() * weight_lstm_ih_.block(h_.size(), 0, h_.size(), h1.size());
+ dc_dh1 += dc_du_g.asDiagonal() * weight_lstm_ih_.block(2 * h_.size(), 0, h_.size(), h1.size());
+ dc_dh1 += dc_du_i.asDiagonal() * weight_lstm_ih_.block(0, 0, h_.size(), h1.size());
+
+ Eigen::MatrixXd dh_dh1 =
+ dh_du_o.asDiagonal() * weight_lstm_ih_.block(3 * h_.size(), 0, h_.size(), h1.size());
+ dh_dh1 += dh_dc_new.asDiagonal() * dc_dh1;
+
+ const Eigen::MatrixXd dc_da2 = dc_dh1.block(0, 1, h_.size(), acc_layer_2.size());
+ const Eigen::MatrixXd dc_ds2 =
+ dc_dh1.block(0, 1 + acc_layer_2.size(), h_.size(), steer_layer_2.size());
+
+ const Eigen::MatrixXd dh_da2 = dh_dh1.block(0, 1, h_.size(), acc_layer_2.size());
+ const Eigen::MatrixXd dh_ds2 =
+ dh_dh1.block(0, 1 + acc_layer_2.size(), h_.size(), steer_layer_2.size());
+
+ const Eigen::MatrixXd dc_da1 = d_relu_product(dc_da2, u_acc_layer_2) * weight_acc_layer_2_;
+ const Eigen::MatrixXd dc_ds1 = d_relu_product(dc_ds2, u_steer_layer_2) * weight_steer_layer_2_;
+
+ const Eigen::MatrixXd dh_da1 = d_relu_product(dh_da2, u_acc_layer_2) * weight_acc_layer_2_;
+ const Eigen::MatrixXd dh_ds1 = d_relu_product(dh_ds2, u_steer_layer_2) * weight_steer_layer_2_;
+
+ const Eigen::MatrixXd dc_d_acc = d_relu_product(dc_da1, u_acc_layer_1) * weight_acc_layer_1_;
+
+ Eigen::MatrixXd dc_d_steer = Eigen::MatrixXd::Zero(h_.size(), steer_input_full.size() + 1);
+ dc_d_steer.block(0, 1, h_.size(), steer_input_full.size()) +=
+ d_relu_product(
+ dc_ds1.block(
+ 0, bias_steer_layer_1_head_.size(), h_.size(), bias_steer_layer_1_tail_.size()),
+ u_steer_layer_1.tail(bias_steer_layer_1_tail_.size())) *
+ weight_steer_layer_1_tail_;
+ dc_d_steer.block(0, 0, h_.size(), steer_sub.size()) +=
+ d_relu_product(
+ dc_ds1.block(0, 0, h_.size(), bias_steer_layer_1_head_.size()),
+ u_steer_layer_1.head(bias_steer_layer_1_head_.size())) *
+ weight_steer_layer_1_head_;
+
+ const Eigen::MatrixXd dh_d_acc = d_relu_product(dh_da1, u_acc_layer_1) * weight_acc_layer_1_;
+
+ Eigen::MatrixXd dh_d_steer = Eigen::MatrixXd::Zero(h_.size(), steer_input_full.size() + 1);
+ dh_d_steer.block(0, 1, h_.size(), steer_input_full.size()) +=
+ d_relu_product(
+ dh_ds1.block(
+ 0, bias_steer_layer_1_head_.size(), h_.size(), bias_steer_layer_1_tail_.size()),
+ u_steer_layer_1.tail(bias_steer_layer_1_tail_.size())) *
+ weight_steer_layer_1_tail_;
+ dh_d_steer.block(0, 0, h_.size(), steer_sub.size()) +=
+ d_relu_product(
+ dh_ds1.block(0, 0, h_.size(), bias_steer_layer_1_head_.size()),
+ u_steer_layer_1.head(bias_steer_layer_1_head_.size())) *
+ weight_steer_layer_1_head_;
+
+ Eigen::MatrixXd dc_dx = Eigen::MatrixXd(h_.size(), x.size());
+ Eigen::MatrixXd dh_dx = Eigen::MatrixXd(h_.size(), x.size());
+ dc_dx.col(0) = vel_normalize_ * dc_dh1.col(0);
+ dc_dx.col(1) = acc_normalize_ * dc_d_acc.col(0);
+ dc_dx.col(2) = steer_normalize_ * dc_d_steer.col(0);
+ dc_dx.block(0, 3, h_.size(), acc_ctrl_queue_size_) =
+ acc_normalize_ * dc_d_acc.block(0, 1, h_.size(), acc_ctrl_queue_size_);
+ dc_dx.block(0, 3 + acc_ctrl_queue_size_, h_.size(), steer_ctrl_queue_size_) =
+ steer_normalize_ * dc_d_steer.block(0, 1, h_.size(), steer_ctrl_queue_size_);
+
+ dh_dx.col(0) = vel_normalize_ * dh_dh1.col(0);
+ dh_dx.col(1) = acc_normalize_ * dh_d_acc.col(0);
+ dh_dx.col(2) = steer_normalize_ * dh_d_steer.col(0);
+ dh_dx.block(0, 3, h_.size(), acc_ctrl_queue_size_) =
+ acc_normalize_ * dh_d_acc.block(0, 1, h_.size(), acc_ctrl_queue_size_);
+ dh_dx.block(0, 3 + acc_ctrl_queue_size_, h_.size(), steer_ctrl_queue_size_) =
+ steer_normalize_ * dh_d_steer.block(0, 1, h_.size(), steer_ctrl_queue_size_);
+
+ dy_dhc_pre_.block(0, 0, y.size(), h_.size()) = dy_dh;
+ dy_dhc_pre_.block(0, h_.size(), y.size(), h_.size()) = dy_dc;
+ dhc_dhc_.block(0, 0, h_.size(), h_.size()) = dh_dh;
+ dhc_dhc_.block(h_.size(), 0, h_.size(), h_.size()) = dc_dh;
+ dhc_dhc_.block(0, h_.size(), h_.size(), h_.size()) = dh_dc.asDiagonal();
+ dhc_dhc_.block(h_.size(), h_.size(), h_.size(), h_.size()) = dc_dc;
+ dhc_dx_pre_.block(0, 0, h_.size(), x.size()) = dh_dx;
+ dhc_dx_pre_.block(h_.size(), 0, h_.size(), x.size()) = dc_dx;
+
+ // finished calc dy_dhc_pre_, dhc_dhc_, dhc_dx_pre_
+
+ Eigen::MatrixXd dy_dh1 = d_sigmoid_product(dy_do, u_o_new) *
+ weight_lstm_ih_.block(3 * h_.size(), 0, h_.size(), h1.size());
+
+ dy_dh1 += d_sigmoid_product(dy_dc_new * c_.asDiagonal(), u_f_new) *
+ weight_lstm_ih_.block(h_.size(), 0, h_.size(), h1.size());
+ dy_dh1 += d_tanh_product(dy_dc_new * i_new.asDiagonal(), u_g_new) *
+ weight_lstm_ih_.block(2 * h_.size(), 0, h_.size(), h1.size());
+ dy_dh1 += d_sigmoid_product(dy_dc_new * g_new.asDiagonal(), u_i_new) *
+ weight_lstm_ih_.block(0, 0, h_.size(), h1.size());
+
+ dy_dh1 += d_relu_product(dy_dh2_tail, u2) * weight_linear_relu_1_;
+ const Eigen::MatrixXd dy_da2 =
+ dy_dh1.block(0, 1, y.size(), acc_layer_2.size()) +
+ weight_finalize_.block(0, h3.size(), y.size(), acc_layer_2.size());
+ const Eigen::MatrixXd dy_ds2 =
+ dy_dh1.block(0, 1 + acc_layer_2.size(), y.size(), steer_layer_2.size()) +
+ weight_finalize_.block(0, h3.size() + acc_layer_2.size(), y.size(), steer_layer_2.size());
+ const Eigen::MatrixXd dy_da1 = d_relu_product(dy_da2, u_acc_layer_2) * weight_acc_layer_2_;
+ const Eigen::MatrixXd dy_ds1 = d_relu_product(dy_ds2, u_steer_layer_2) * weight_steer_layer_2_;
+
+ const Eigen::MatrixXd dy_d_acc = d_relu_product(dy_da1, u_acc_layer_1) * weight_acc_layer_1_;
+ Eigen::MatrixXd dy_d_steer = Eigen::MatrixXd::Zero(y.size(), steer_input_full.size() + 1);
+ dy_d_steer.block(0, 1, y.size(), steer_input_full.size()) +=
+ d_relu_product(
+ dy_ds1.block(0, bias_steer_layer_1_head_.size(), y.size(), bias_steer_layer_1_tail_.size()),
+ u_steer_layer_1.tail(bias_steer_layer_1_tail_.size())) *
+ weight_steer_layer_1_tail_;
+ dy_d_steer.block(0, 0, y.size(), steer_sub.size()) +=
+ d_relu_product(
+ dy_ds1.block(0, 0, y.size(), bias_steer_layer_1_head_.size()),
+ u_steer_layer_1.head(bias_steer_layer_1_head_.size())) *
+ weight_steer_layer_1_head_;
+
+ Eigen::MatrixXd result = Eigen::MatrixXd::Zero(y.size(), x.size() + 1);
+
+ result.col(0) = y;
+
+ result.col(1) = vel_normalize_ * dy_dh1.col(0);
+ result.col(2) = acc_normalize_ * dy_d_acc.col(0);
+ result.col(3) = steer_normalize_ * dy_d_steer.col(0);
+ result.block(0, 4, y.size(), acc_ctrl_queue_size_) =
+ acc_normalize_ * dy_d_acc.block(0, 1, y.size(), acc_ctrl_queue_size_);
+ result.block(0, 4 + acc_ctrl_queue_size_, y.size(), steer_ctrl_queue_size_) =
+ steer_normalize_ * dy_d_steer.block(0, 1, y.size(), steer_ctrl_queue_size_);
+
+ const Eigen::MatrixXd polynomial_reg_diff =
+ A_linear_reg_ *
+ polynomial_features_with_diff.block(0, 1, A_linear_reg_.cols(), x_for_polynomial_reg.size());
+ result.block(0, 1, y.size(), 3) += polynomial_reg_diff.block(0, 0, y.size(), 3);
+ result.block(0, 1 + acc_start, y.size(), 3) += polynomial_reg_diff.block(0, 3, y.size(), 3);
+ result.block(0, 1 + steer_start, y.size(), 3) += polynomial_reg_diff.block(0, 6, y.size(), 3);
+
+ h_ = h_new;
+ c_ = c_new;
+ return result;
+ }
+
+ void update_memory(const Eigen::VectorXd & x)
+ {
+ Eigen::VectorXd acc_sub(acc_ctrl_queue_size_ + 1);
+ acc_sub[0] = acc_normalize_ * x[1];
+ acc_sub.tail(acc_ctrl_queue_size_) = acc_normalize_ * x.segment(3, acc_ctrl_queue_size_);
+ Eigen::VectorXd steer_sub(steer_ctrl_queue_size_core_ + 1);
+ steer_sub[0] = steer_normalize_ * x[2];
+ steer_sub.tail(steer_ctrl_queue_size_core_) =
+ steer_normalize_ * x.segment(3 + acc_ctrl_queue_size_, steer_ctrl_queue_size_core_);
+ const Eigen::VectorXd acc_layer_1 = relu(weight_acc_layer_1_ * acc_sub + bias_acc_layer_1_);
+ Eigen::VectorXd steer_layer_1(
+ bias_steer_layer_1_head_.size() + bias_steer_layer_1_tail_.size());
+ steer_layer_1.head(bias_steer_layer_1_head_.size()) =
+ relu(weight_steer_layer_1_head_ * steer_sub + bias_steer_layer_1_head_);
+
+ const Eigen::VectorXd steer_input_full =
+ steer_normalize_ * x.segment(3 + acc_ctrl_queue_size_, steer_ctrl_queue_size_);
+ steer_layer_1.tail(bias_steer_layer_1_tail_.size()) =
+ relu(weight_steer_layer_1_tail_ * steer_input_full + bias_steer_layer_1_tail_);
+
+ const Eigen::VectorXd acc_layer_2 = relu(weight_acc_layer_2_ * acc_layer_1 + bias_acc_layer_2_);
+
+ const Eigen::VectorXd steer_layer_2 =
+ relu(weight_steer_layer_2_ * steer_layer_1 + bias_steer_layer_2_);
+ Eigen::VectorXd h1(1 + acc_layer_2.size() + steer_layer_2.size());
+ h1[0] = vel_normalize_ * x[0];
+ h1.segment(1, acc_layer_2.size()) = acc_layer_2;
+ h1.tail(steer_layer_2.size()) = steer_layer_2;
+
+ const Eigen::VectorXd i_new = sigmoid(
+ weight_lstm_ih_.block(0, 0, h_.size(), h1.size()) * h1 + bias_lstm_ih_.head(h_.size()) +
+ weight_lstm_hh_.block(0, 0, h_.size(), h_.size()) * h_ + bias_lstm_hh_.head(h_.size()));
+ const Eigen::VectorXd f_new = sigmoid(
+ weight_lstm_ih_.block(h_.size(), 0, h_.size(), h1.size()) * h1 +
+ bias_lstm_ih_.segment(h_.size(), h_.size()) +
+ weight_lstm_hh_.block(h_.size(), 0, h_.size(), h_.size()) * h_ +
+ bias_lstm_hh_.segment(h_.size(), h_.size()));
+ const Eigen::VectorXd g_new = tanh(
+ weight_lstm_ih_.block(2 * h_.size(), 0, h_.size(), h1.size()) * h1 +
+ bias_lstm_ih_.segment(2 * h_.size(), h_.size()) +
+ weight_lstm_hh_.block(2 * h_.size(), 0, h_.size(), h_.size()) * h_ +
+ bias_lstm_hh_.segment(2 * h_.size(), h_.size()));
+ const Eigen::VectorXd o_new = sigmoid(
+ weight_lstm_ih_.block(3 * h_.size(), 0, h_.size(), h1.size()) * h1 +
+ bias_lstm_ih_.segment(3 * h_.size(), h_.size()) +
+ weight_lstm_hh_.block(3 * h_.size(), 0, h_.size(), h_.size()) * h_ +
+ bias_lstm_hh_.segment(3 * h_.size(), h_.size()));
+ const Eigen::VectorXd c_new = f_new.array() * c_.array() + i_new.array() * g_new.array();
+ const Eigen::VectorXd h_new = o_new.array() * tanh(c_new).array();
+ h_ = h_new;
+ c_ = c_new;
+ }
+ Eigen::VectorXd rot_and_d_rot_error_prediction(const Eigen::VectorXd & x)
+ {
+ const int x_dim = x.size();
+ const double theta = x[3];
+ const double v = x[2];
+ double coef = 2.0 * std::abs(v);
+ coef = coef * coef * coef * coef * coef * coef * coef;
+ if (coef > 1.0) {
+ coef = 1.0;
+ }
+ const double cos = std::cos(theta);
+ const double sin = std::sin(theta);
+ Eigen::Matrix2d Rot;
+ Rot << cos, -sin, sin, cos;
+
+ Eigen::Matrix2d dRot;
+ dRot << -sin, -cos, cos, -sin;
+ Eigen::VectorXd vars(x_dim - 3);
+ vars[0] = x[2];
+ vars[1] = x[4];
+ vars[2] = x[5];
+ vars.tail(x_dim - 6) = x.tail(x_dim - 6);
+ const Eigen::VectorXd pred = error_prediction(vars, -1);
+ Eigen::VectorXd rot_and_d_rot_pred(8);
+ rot_and_d_rot_pred.head(2) = Rot * pred.head(2);
+ rot_and_d_rot_pred.segment(2, 4) = pred.segment(2, 4);
+ rot_and_d_rot_pred.tail(2) = dRot * pred.head(2);
+
+ return coef * rot_and_d_rot_pred;
+ }
+ Eigen::MatrixXd rot_and_d_rot_error_prediction_with_diff(const Eigen::VectorXd & x)
+ {
+ const int x_dim = x.size();
+ const double theta = x[3];
+ const double v = x[2];
+ double coef = 2.0 * std::abs(v);
+ coef = coef * coef * coef * coef * coef * coef * coef;
+ if (coef > 1.0) {
+ coef = 1.0;
+ }
+ const double cos = std::cos(theta);
+ const double sin = std::sin(theta);
+ Eigen::Matrix2d Rot;
+ Rot << cos, -sin, sin, cos;
+
+ Eigen::Matrix2d dRot;
+ dRot << -sin, -cos, cos, -sin;
+ Eigen::VectorXd vars(x_dim - 3);
+ vars[0] = x[2];
+ vars[1] = x[4];
+ vars[2] = x[5];
+ vars.tail(x_dim - 6) = x.tail(x_dim - 6);
+
+ const Eigen::MatrixXd pred_d_pred = error_prediction_with_diff(vars);
+ const Eigen::VectorXd pred = pred_d_pred.col(0);
+ const Eigen::MatrixXd d_pred = pred_d_pred.block(0, 1, 6, x_dim - 3);
+ Eigen::MatrixXd rot_and_d_rot_pred_with_diff = Eigen::MatrixXd::Zero(6, x_dim + 2);
+ Eigen::MatrixXd rot_pred_with_diff(6, x_dim - 3);
+ rot_pred_with_diff.block(0, 0, 2, x_dim - 3) = Rot * d_pred.block(0, 0, 2, x_dim - 3);
+ rot_pred_with_diff.block(2, 0, 4, x_dim - 3) = d_pred.block(2, 0, 4, x_dim - 3);
+
+ rot_and_d_rot_pred_with_diff.block(0, 0, 2, 1) = Rot * pred.head(2);
+ rot_and_d_rot_pred_with_diff.block(2, 0, 4, 1) = pred.segment(2, 4);
+ rot_and_d_rot_pred_with_diff.block(0, 1, 2, 1) = dRot * pred.head(2);
+ rot_and_d_rot_pred_with_diff.col(2 + 2) = rot_pred_with_diff.col(0);
+ rot_and_d_rot_pred_with_diff.col(2 + 4) = rot_pred_with_diff.col(1);
+ rot_and_d_rot_pred_with_diff.col(2 + 5) = rot_pred_with_diff.col(2);
+ rot_and_d_rot_pred_with_diff.block(0, 2 + 6, 6, x_dim - 6) =
+ rot_pred_with_diff.block(0, 3, 6, x_dim - 6);
+
+ return coef * rot_and_d_rot_pred_with_diff;
+ }
+ Eigen::MatrixXd rot_and_d_rot_error_prediction_with_memory_diff(const Eigen::VectorXd & x)
+ {
+ const int x_dim = x.size();
+ const double theta = x[3];
+ const double v = x[2];
+ double coef = 2.0 * std::abs(v);
+ coef = coef * coef * coef * coef * coef * coef * coef;
+ if (coef > 1.0) {
+ coef = 1.0;
+ }
+ const double cos = std::cos(theta);
+ const double sin = std::sin(theta);
+ Eigen::Matrix2d Rot;
+ Rot << cos, -sin, sin, cos;
+
+ Eigen::Matrix2d dRot;
+ dRot << -sin, -cos, cos, -sin;
+ Eigen::VectorXd vars(x_dim - 3);
+ vars[0] = x[2];
+ vars[1] = x[4];
+ vars[2] = x[5];
+ vars.tail(x_dim - 6) = x.tail(x_dim - 6);
+
+ const Eigen::MatrixXd pred_d_pred = error_prediction_with_memory_diff(vars);
+ const Eigen::VectorXd pred = pred_d_pred.col(0);
+ const Eigen::MatrixXd d_pred = pred_d_pred.block(0, 1, 6, x_dim - 3);
+ Eigen::MatrixXd rot_and_d_rot_pred_with_diff = Eigen::MatrixXd::Zero(6, x_dim + 2);
+ Eigen::MatrixXd rot_pred_with_diff(6, x_dim - 3);
+ rot_pred_with_diff.block(0, 0, 2, x_dim - 3) = Rot * d_pred.block(0, 0, 2, x_dim - 3);
+ rot_pred_with_diff.block(2, 0, 4, x_dim - 3) = d_pred.block(2, 0, 4, x_dim - 3);
+
+ rot_and_d_rot_pred_with_diff.block(0, 0, 2, 1) = Rot * pred.head(2);
+ rot_and_d_rot_pred_with_diff.block(2, 0, 4, 1) = pred.segment(2, 4);
+ rot_and_d_rot_pred_with_diff.block(0, 1, 2, 1) = dRot * pred.head(2);
+ rot_and_d_rot_pred_with_diff.col(2 + 2) = rot_pred_with_diff.col(0);
+ rot_and_d_rot_pred_with_diff.col(2 + 4) = rot_pred_with_diff.col(1);
+ rot_and_d_rot_pred_with_diff.col(2 + 5) = rot_pred_with_diff.col(2);
+ rot_and_d_rot_pred_with_diff.block(0, 2 + 6, 6, x_dim - 6) =
+ rot_pred_with_diff.block(0, 3, 6, x_dim - 6);
+
+ dy_dhc_.block(0, 0, 2, 2 * h_.size()) = Rot * dy_dhc_pre_.block(0, 0, 2, 2 * h_.size());
+ dy_dhc_.block(2, 0, 4, 2 * h_.size()) = dy_dhc_pre_.block(2, 0, 4, 2 * h_.size());
+
+ dhc_dx_.col(2) = dhc_dx_pre_.col(0);
+ dhc_dx_.col(4) = dhc_dx_pre_.col(1);
+ dhc_dx_.col(5) = dhc_dx_pre_.col(2);
+ dhc_dx_.block(0, 6, 2 * h_.size(), x_dim - 6) =
+ dhc_dx_pre_.block(0, 3, 2 * h_.size(), x_dim - 6);
+
+ return coef * rot_and_d_rot_pred_with_diff;
+ }
+ Eigen::MatrixXd rot_and_d_rot_error_prediction_with_poly_diff(const Eigen::VectorXd & x)
+ {
+ const int x_dim = x.size();
+ const double theta = x[3];
+ const double v = x[2];
+ double coef = 2.0 * std::abs(v);
+ coef = coef * coef * coef * coef * coef * coef * coef;
+ if (coef > 1.0) {
+ coef = 1.0;
+ }
+ const double cos = std::cos(theta);
+ const double sin = std::sin(theta);
+ Eigen::Matrix2d Rot;
+ Rot << cos, -sin, sin, cos;
+
+ Eigen::Matrix2d dRot;
+ dRot << -sin, -cos, cos, -sin;
+ Eigen::VectorXd vars(x_dim - 3);
+ vars[0] = x[2];
+ vars[1] = x[4];
+ vars[2] = x[5];
+ vars.tail(x_dim - 6) = x.tail(x_dim - 6);
+
+ const Eigen::VectorXd pred = error_prediction(vars, -1);
+ Eigen::MatrixXd d_pred = Eigen::MatrixXd::Zero(6, x_dim - 3);
+
+ Eigen::VectorXd x_for_polynomial_reg(9);
+ x_for_polynomial_reg.head(3) = x.head(3);
+ const int acc_start = 3 + std::max(acc_delay_step_ - 3, 0);
+ x_for_polynomial_reg.segment(3, 3) = x.segment(acc_start, 3);
+ const int steer_start = 3 + acc_ctrl_queue_size_ + std::max(steer_delay_step_ - 3, 0);
+ x_for_polynomial_reg.segment(6, 3) = x.segment(steer_start, 3);
+
+ const Eigen::MatrixXd polynomial_features_with_diff =
+ get_polynomial_features_with_diff(x_for_polynomial_reg, deg_, A_linear_reg_.cols());
+ const Eigen::MatrixXd polynomial_reg_diff =
+ A_linear_reg_ *
+ polynomial_features_with_diff.block(0, 1, A_linear_reg_.cols(), x_for_polynomial_reg.size());
+ d_pred.block(0, 0, 6, 3) += polynomial_reg_diff.block(0, 0, 6, 3);
+ d_pred.block(0, 1 + acc_start, 6, 3) += polynomial_reg_diff.block(0, 3, 6, 3);
+ d_pred.block(0, 1 + steer_start, 6, 3) += polynomial_reg_diff.block(0, 6, 6, 3);
+
+ Eigen::MatrixXd rot_and_d_rot_pred_with_diff = Eigen::MatrixXd::Zero(6, x_dim + 2);
+ Eigen::MatrixXd rot_pred_with_diff(6, x_dim - 3);
+ rot_pred_with_diff.block(0, 0, 2, x_dim - 3) = Rot * d_pred.block(0, 0, 2, x_dim - 3);
+ rot_pred_with_diff.block(2, 0, 4, x_dim - 3) = d_pred.block(2, 0, 4, x_dim - 3);
+
+ rot_and_d_rot_pred_with_diff.block(0, 0, 2, 1) = Rot * pred.head(2);
+ rot_and_d_rot_pred_with_diff.block(2, 0, 4, 1) = pred.segment(2, 4);
+ rot_and_d_rot_pred_with_diff.block(0, 1, 2, 1) = dRot * pred.head(2);
+ rot_and_d_rot_pred_with_diff.col(2 + 2) = rot_pred_with_diff.col(0);
+ rot_and_d_rot_pred_with_diff.col(2 + 4) = rot_pred_with_diff.col(1);
+ rot_and_d_rot_pred_with_diff.col(2 + 5) = rot_pred_with_diff.col(2);
+ rot_and_d_rot_pred_with_diff.block(0, 2 + 6, 6, x_dim - 6) =
+ rot_pred_with_diff.block(0, 3, 6, x_dim - 6);
+
+ return coef * rot_and_d_rot_pred_with_diff;
+ }
+ Eigen::VectorXd rotated_error_prediction(const Eigen::VectorXd & x)
+ {
+ const int x_dim = x.size();
+ const double theta = x[3];
+ const double v = x[2];
+ double coef = 2.0 * std::abs(v);
+ coef = coef * coef * coef * coef * coef * coef * coef;
+ if (coef > 1.0) {
+ coef = 1.0;
+ }
+ const double cos = std::cos(theta);
+ const double sin = std::sin(theta);
+ Eigen::Matrix2d Rot;
+ Rot << cos, -sin, sin, cos;
+ Eigen::VectorXd vars(x_dim - 3);
+ vars[0] = x[2];
+ vars[1] = x[4];
+ vars[2] = x[5];
+ vars.tail(x_dim - 6) = x.tail(x_dim - 6);
+ const Eigen::VectorXd pred = error_prediction(vars, -1);
+ Eigen::VectorXd rot_pred(6);
+ rot_pred.head(2) = coef * Rot * pred.head(2);
+ rot_pred.tail(4) = coef * pred.tail(4);
+ return rot_pred;
+ }
+ Eigen::MatrixXd Rotated_error_prediction(const Eigen::MatrixXd & X)
+ {
+ const int X_cols = X.cols();
+ const int x_dim = X.rows();
+ Eigen::MatrixXd Pred(6, X_cols);
+ for (int i = 0; i < X_cols; i++) {
+ const Eigen::VectorXd x = X.col(i);
+ const double theta = x[3];
+ const double v = x[2];
+ double coef = 2.0 * std::abs(v);
+ coef = coef * coef * coef * coef * coef * coef * coef;
+ if (coef > 1.0) {
+ coef = 1.0;
+ }
+ const double cos = std::cos(theta);
+ const double sin = std::sin(theta);
+ Eigen::Matrix2d Rot;
+ Rot << cos, -sin, sin, cos;
+ Eigen::VectorXd vars(x_dim - 3);
+
+ vars[0] = x[2];
+
+ vars[1] = x[4];
+ vars[2] = x[5];
+ vars.tail(x_dim - 6) = x.tail(x_dim - 6);
+ const Eigen::VectorXd pred = error_prediction(vars, i);
+ Pred.block(0, i, 2, 1) = coef * Rot * pred.head(2);
+ Pred.block(2, i, 4, 1) = coef * pred.tail(4);
+ }
+ return Pred;
+ }
+ void update_memory_by_state_history(const Eigen::MatrixXd & X)
+ {
+ const int X_cols = X.cols();
+ const int x_dim = X.rows();
+ for (int i = 0; i < X_cols; i++) {
+ const Eigen::VectorXd x = X.col(i);
+ Eigen::VectorXd vars(x_dim - 3);
+ vars[0] = x[2];
+ vars[1] = x[4];
+ vars[2] = x[5];
+ vars.tail(x_dim - 6) = x.tail(x_dim - 6);
+ update_memory(vars);
+ }
+ }
+};
PYBIND11_MODULE(proxima_calc, m)
{
py::class_(m, "transform_model_to_eigen")
@@ -483,5 +1536,40 @@ PYBIND11_MODULE(proxima_calc, m)
.def(
"rot_and_d_rot_error_prediction_with_diff",
&transform_model_to_eigen::rot_and_d_rot_error_prediction_with_diff)
+ .def(
+ "rot_and_d_rot_error_prediction_with_poly_diff",
+ &transform_model_to_eigen::rot_and_d_rot_error_prediction_with_poly_diff)
+ .def("rotated_error_prediction", &transform_model_to_eigen::rotated_error_prediction)
.def("Rotated_error_prediction", &transform_model_to_eigen::Rotated_error_prediction);
+ py::class_(m, "transform_model_with_memory_to_eigen")
+ .def(py::init())
+ .def("set_params", &transform_model_with_memory_to_eigen::set_params)
+ .def("set_params_res", &transform_model_with_memory_to_eigen::set_params_res)
+ .def("set_lstm", &transform_model_with_memory_to_eigen::set_lstm)
+ .def("set_lstm_for_candidate", &transform_model_with_memory_to_eigen::set_lstm_for_candidate)
+ .def("get_h", &transform_model_with_memory_to_eigen::get_h)
+ .def("get_c", &transform_model_with_memory_to_eigen::get_c)
+ .def("get_dy_dhc", &transform_model_with_memory_to_eigen::get_dy_dhc)
+ .def("get_dhc_dx", &transform_model_with_memory_to_eigen::get_dhc_dx)
+ .def("get_dhc_dhc", &transform_model_with_memory_to_eigen::get_dhc_dhc)
+ .def("error_prediction", &transform_model_with_memory_to_eigen::error_prediction)
+ .def(
+ "rot_and_d_rot_error_prediction",
+ &transform_model_with_memory_to_eigen::rot_and_d_rot_error_prediction)
+ .def(
+ "rot_and_d_rot_error_prediction_with_diff",
+ &transform_model_with_memory_to_eigen::rot_and_d_rot_error_prediction_with_diff)
+ .def(
+ "rot_and_d_rot_error_prediction_with_memory_diff",
+ &transform_model_with_memory_to_eigen::rot_and_d_rot_error_prediction_with_memory_diff)
+ .def(
+ "rot_and_d_rot_error_prediction_with_poly_diff",
+ &transform_model_with_memory_to_eigen::rot_and_d_rot_error_prediction_with_poly_diff)
+ .def(
+ "update_memory_by_state_history",
+ &transform_model_with_memory_to_eigen::update_memory_by_state_history)
+ .def(
+ "rotated_error_prediction", &transform_model_with_memory_to_eigen::rotated_error_prediction)
+ .def(
+ "Rotated_error_prediction", &transform_model_with_memory_to_eigen::Rotated_error_prediction);
}
diff --git a/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py
index 196657fab6e91..f767dad223f0d 100755
--- a/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py
+++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/scripts/pympc_trajectory_follower.py
@@ -31,9 +31,11 @@
import diagnostic_updater
from diagnostic_updater import DiagnosticStatusWrapper
from geometry_msgs.msg import AccelWithCovarianceStamped
+from geometry_msgs.msg import Point
from geometry_msgs.msg import PoseStamped
from nav_msgs.msg import Odometry
import numpy as np
+from rcl_interfaces.msg import ParameterDescriptor
import rclpy
from rclpy.node import Node
import scipy
@@ -44,6 +46,8 @@
from tier4_debug_msgs.msg import BoolStamped
from tier4_debug_msgs.msg import Float32MultiArrayStamped
from tier4_debug_msgs.msg import Float32Stamped
+from visualization_msgs.msg import Marker
+from visualization_msgs.msg import MarkerArray
class ControlStatus(Enum):
@@ -66,6 +70,13 @@ def __init__(self):
# ROS 2
super().__init__("pympc_trajectory_follower")
+
+ self.declare_parameter(
+ "plot_sampling_paths",
+ False,
+ ParameterDescriptor(description="Publish MPPI sampling paths as MarkerArray."),
+ )
+
self.sub_trajectory_ = self.create_subscription(
Trajectory,
"/planning/scenario_planning/trajectory",
@@ -122,14 +133,6 @@ def __init__(self):
)
self.sub_goal_pose_
- self.sub_step_response_trigger_ = self.create_subscription(
- String,
- "/pympc_step_response_trigger",
- self.onStepResponseTrigger,
- 1,
- )
- self.sub_step_response_trigger_
-
self.sub_reload_mpc_param_trigger_ = self.create_subscription(
String,
"/pympc_reload_mpc_param_trigger",
@@ -264,6 +267,12 @@ def __init__(self):
1,
)
+ self.debug_mpc_sampling_paths_marker_array_ = self.create_publisher(
+ MarkerArray,
+ "/debug_mpc_sampling_paths_marker_array",
+ 1,
+ )
+
self._present_trajectory = None
self._present_kinematic_state = None
self._present_acceleration = None
@@ -276,9 +285,6 @@ def __init__(self):
self.emergency_stop_mode_flag = False
self.stop_mode_reset_request = False
- self.step_response_flag = False
- self.step_response_trigger = False
- self.step_response_time_step = 0
self.last_acc_cmd = 0.0
self.last_steer_cmd = 0.0
self.past_control_trajectory_mode = 1
@@ -304,16 +310,11 @@ def onOperationMode(self, msg):
def onStopModeResetRequest(self, msg):
self.stop_mode_reset_request = True
- self.step_response_flag = False
self.get_logger().info("receive stop mode reset request")
def onGoalPose(self, msg):
self._goal_pose = msg
- def onStepResponseTrigger(self, msg):
- self.step_response_trigger = True
- self.get_logger().info("receive step response trigger")
-
def onReloadMpcParamTrigger(self, msg):
self.get_logger().info("receive reload mpc param trigger")
@@ -460,42 +461,7 @@ def control(self):
self.stop_mode_reset_request = False
# [3] Control logic calculation
- # [3-1] pure pursuit
- # [3-1-1] compute pure pursuit acc cmd
- longitudinal_vel_err = (
- present_linear_velocity - trajectory_longitudinal_velocity[nearestIndex]
- )
- acc_kp = 0.5
- acc_lim = 2.0
- pure_pursuit_acc_cmd = np.clip(-acc_kp * longitudinal_vel_err, -acc_lim, acc_lim)[0]
-
- # [3-1-2] compute pure pursuit steer cmd
- nearest_trajectory_point_yaw = getYaw(trajectory_orientation[nearestIndex])
- cos_yaw = np.cos(nearest_trajectory_point_yaw)
- sin_yaw = np.sin(nearest_trajectory_point_yaw)
- diff_position = present_position - trajectory_position[nearestIndex]
- lat_err = -sin_yaw * diff_position[0] + cos_yaw * diff_position[1]
- yaw_err = getYaw(present_orientation) - nearest_trajectory_point_yaw
- while True:
- if yaw_err > np.pi:
- yaw_err -= 2.0 * np.pi
- if yaw_err < (-np.pi):
- yaw_err += 2.0 * np.pi
- if np.abs(yaw_err) < np.pi:
- break
- wheel_base = drive_functions.L # 4.0
- lookahead_time = 3.0
- min_lookahead = 3.0
- lookahead = min_lookahead + lookahead_time * np.abs(present_linear_velocity[0])
- steer_kp = 2.0 * wheel_base / (lookahead * lookahead)
- steer_kd = 2.0 * wheel_base / lookahead
- steer_lim = 0.6
- pure_pursuit_steer_cmd = np.clip(
- -steer_kp * lat_err - steer_kd * yaw_err, -steer_lim, steer_lim
- )[0]
-
- # [3-2] MPC control logic calculation
- # [3-2-1] State variables in the MPC
+ # [3-1] State variables in the MPC
present_mpc_x = np.array(
[
present_position[0],
@@ -507,7 +473,7 @@ def control(self):
]
)
- # [3-2-2] resampling trajectory by time (except steer angle).
+ # [3-2] resampling trajectory by time (except steer angle).
X_des = np.zeros((drive_functions.N + 1, 8))
dist = np.sqrt(((trajectory_position[1:] - trajectory_position[:-1]) ** 2).sum(axis=1))
timestamp_from_start = [0.0]
@@ -549,7 +515,7 @@ def control(self):
X_des[:, 4] = acceleration_interpolate(timestamp_mpc)
X_des[:, 6] = 1 * X_des[:, 4]
- # [3-2-3] resampling trajectory by time (steer angle)
+ # [3-3] resampling trajectory by time (steer angle)
previous_des_steer = 0.0
steer_trajectory2 = np.zeros(len(timestamp_mpc))
downsampling = 5
@@ -638,9 +604,17 @@ def control(self):
X_des[:, 5] = steer_trajectory2
X_des[:, 7] = 1 * X_des[:, 5]
- # [3-2-4] mpc computation
+ # [3-4] (optimal) control computation
U_des = np.zeros((drive_functions.N, 2))
start_calc_u_opt = time.time()
+ acc_time_stamp = (
+ self._present_acceleration.header.stamp.sec
+ + 1e-9 * self._present_acceleration.header.stamp.nanosec
+ )
+ steer_time_stamp = (
+ self._present_steering_status.stamp.sec
+ + 1e-9 * self._present_steering_status.stamp.nanosec
+ )
u_opt = self.controller.update_input_queue_and_get_optimal_control(
self.control_cmd_time_stamp_list,
self.control_cmd_acc_list,
@@ -648,12 +622,14 @@ def control(self):
present_mpc_x,
X_des,
U_des,
+ acc_time_stamp,
+ steer_time_stamp,
)
end_calc_u_opt = time.time()
mpc_acc_cmd = u_opt[0]
mpc_steer_cmd = u_opt[1]
- # [3-3] Enter goal stop mode (override command value) to maintain stop at goal
+ # [3-5] Enter goal stop mode (override command value) to maintain stop at goal
self.goal_stop_mode_flag = False
if self._goal_pose is not None:
goal_position = np.array(
@@ -669,39 +645,13 @@ def control(self):
if distance_from_goal < goal_distance_threshold:
self.goal_stop_mode_flag = True
- # [3-4] Override when doing step response
- if self.step_response_trigger:
- self.step_response_time_step = 0
- self.step_response_flag = True
- self.step_response_trigger = False
-
- if self.step_response_flag:
- step_response_elapsed_time = self.step_response_time_step * self.timer_period_callback
- mpc_acc_cmd = 1.0 * self.last_acc_cmd
- if step_response_elapsed_time < 3.0:
- mpc_steer_cmd = 0.0
- else:
- mpc_steer_cmd = -0.005
- self.get_logger().info(
- "step_response_elapsed_time: "
- + str(step_response_elapsed_time)
- + ", mpc_steer_cmd: "
- + str(mpc_steer_cmd)
- )
- self.step_response_time_step += 1
-
- # [3-5] Determine the control logic to be finally applied and publish it
+ # [3-6] Determine the control logic to be finally applied and publish it
acc_cmd = 0.0
steer_cmd = 0.0
if (not self.emergency_stop_mode_flag) and (not self.goal_stop_mode_flag):
# in normal mode
acc_cmd = mpc_acc_cmd
steer_cmd = mpc_steer_cmd
-
- overwrite_cmd_by_pure_pursuit_flag = False
- if overwrite_cmd_by_pure_pursuit_flag:
- steer_cmd = pure_pursuit_steer_cmd
- acc_cmd = pure_pursuit_acc_cmd
else:
# in stop mode
acc_cmd_decrease_limit = 5.0 * self.timer_period_callback # lon_jerk_lim
@@ -740,7 +690,7 @@ def control(self):
self.control_cmd_steer_list.pop(0)
self.control_cmd_acc_list.pop(0)
- # [3-6] Update control state
+ # [3-7] Update control state
if control_state != ControlStatus.EMERGENCY:
stopped_velocity_threshold = 0.1
if present_linear_velocity[0] <= stopped_velocity_threshold:
@@ -869,6 +819,39 @@ def control(self):
data=(end_ctrl_time - start_ctrl_time),
)
)
+ if self.controller.mode == "mppi" or self.controller.mode == "mppi_ilqr":
+ if self.get_parameter("plot_sampling_paths").get_parameter_value().bool_value:
+ marker_array = MarkerArray()
+ for i in range(self.controller.mppi_candidates.shape[0]):
+ marker = Marker()
+ marker.type = marker.LINE_STRIP
+ marker.header.stamp = cmd_msg.stamp
+ marker.header.frame_id = "map"
+ marker.id = i
+ marker.action = marker.ADD
+
+ marker.scale.x = 0.05
+ marker.scale.y = 0.0
+ marker.scale.z = 0.0
+
+ marker.color.a = 1.0
+ marker.color.r = np.random.uniform()
+ marker.color.g = np.random.uniform()
+ marker.color.b = np.random.uniform()
+
+ marker.lifetime.nanosec = 500000000
+ marker.frame_locked = True
+
+ marker.points = []
+
+ for j in range(self.controller.mppi_candidates.shape[1]):
+ tmp_point = Point()
+ tmp_point.x = self.controller.mppi_candidates[i, j, 0]
+ tmp_point.y = self.controller.mppi_candidates[i, j, 1]
+ tmp_point.z = self._present_kinematic_state.pose.pose.position.z
+ marker.points.append(tmp_point)
+ marker_array.markers.append(marker)
+ self.debug_mpc_sampling_paths_marker_array_.publish(marker_array)
self.diagnostic_updater.force_update()
diff --git a/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/add_training_data_from_csv.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/add_training_data_from_csv.py
index 5338f3cbaddce..006a51bfe6557 100644
--- a/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/add_training_data_from_csv.py
+++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/add_training_data_from_csv.py
@@ -22,12 +22,34 @@
import json
import os
from pathlib import Path
+import subprocess
from autoware_smart_mpc_trajectory_follower.scripts import drive_functions
import numpy as np
import scipy.interpolate
from scipy.ndimage import gaussian_filter
from scipy.spatial.transform import Rotation
+from sklearn import linear_model
+from sklearn.preprocessing import PolynomialFeatures
+
+ctrl_index_for_polynomial_reg = np.concatenate(
+ (
+ np.arange(3),
+ 3
+ + max(drive_functions.acc_delay_step - drive_functions.mpc_freq, 0)
+ + np.arange(drive_functions.mpc_freq),
+ 3
+ + drive_functions.acc_ctrl_queue_size
+ + max(drive_functions.steer_delay_step - drive_functions.mpc_freq, 0)
+ + np.arange(drive_functions.mpc_freq),
+ )
+)
+
+
+if drive_functions.use_memory_for_training:
+ default_add_mode = "divide"
+else:
+ default_add_mode = "as_train"
def data_smoothing(data: np.ndarray, sigma: float) -> np.ndarray:
@@ -53,19 +75,29 @@ class add_data_from_csv:
"""Class for loading csv files containing driving data, converting them into teacher data for the NN training, and storing them in lists."""
X_input_list: list[np.ndarray]
- """Input side of NN teacher data"""
+ """Input side of NN teacher data."""
Y_output_list: list[np.ndarray]
- """Output side of NN teacher data"""
+ """Output side of NN teacher data."""
- def __init__(self):
+ def __init__(
+ self, alpha_1_for_polynomial_regression=0.1**5, alpha_2_for_polynomial_regression=0.1**5
+ ):
self.X_input_list = []
self.Y_output_list = []
+ self.X_val_list = []
+ self.Y_val_list = []
+ self.division_indices = []
+ self.division_indices_val = []
+ self.alpha_1_for_polynomial_regression = alpha_1_for_polynomial_regression
+ self.alpha_2_for_polynomial_regression = alpha_2_for_polynomial_regression
def clear_data(self):
"""Clear the teacher data for the training."""
self.X_input_list.clear()
self.Y_output_list.clear()
+ self.X_val_list.clear()
+ self.Y_val_list.clear()
def transform_rosbag_to_csv(self, dir_name: str, delete_csv_first: bool = True) -> None:
"""Convert rosbag file to CSV format."""
@@ -73,19 +105,20 @@ def transform_rosbag_to_csv(self, dir_name: str, delete_csv_first: bool = True)
with open(package_path_json, "r") as file:
package_path = json.load(file)
dir_exe = os.getcwd()
- os.system(
+ subprocess.run(
"cp "
+ package_path["path"]
+ "/autoware_smart_mpc_trajectory_follower/training_and_data_check/rosbag2.bash "
- + dir_name
+ + dir_name,
+ shell=True,
)
os.chdir(dir_name)
if len(glob.glob("*.csv")) > 0 and delete_csv_first:
- os.system("rm *.csv")
- os.system("bash rosbag2.bash")
+ subprocess.run("rm *.csv", shell=True)
+ subprocess.run("bash rosbag2.bash", shell=True)
os.chdir(dir_exe)
- def add_data_from_csv(self, dir_name: str) -> None:
+ def add_data_from_csv(self, dir_name: str, add_mode=default_add_mode) -> None:
"""Adding teacher data for training from a CSV file."""
acc_ctrl_queue_size = drive_functions.acc_ctrl_queue_size
steer_ctrl_queue_size = drive_functions.steer_ctrl_queue_size
@@ -119,7 +152,7 @@ def add_data_from_csv(self, dir_name: str) -> None:
steer_sm = data_smoothing(steer, drive_functions.steer_sigma_for_learning)
control_cmd = np.loadtxt(
- dir_name + "/control_cmd_orig.csv", delimiter=",", usecols=[0, 1, 4, 9]
+ dir_name + "/control_cmd_orig.csv", delimiter=",", usecols=[0, 1, 8, 16]
)
acc_des = control_cmd[:, 3]
acc_des_sm = data_smoothing(acc_des, drive_functions.acc_des_sigma_for_learning)
@@ -279,10 +312,30 @@ def get_interpolated_input(s):
Y_smooth[:, 5] = data_smoothing(
Y_output_list_array[:, 5], drive_functions.steer_out_sigma_for_learning
)
-
- for i in range(len(X_input_list)):
- self.X_input_list.append(X_input_list[i])
- self.Y_output_list.append(Y_smooth[i])
+ if add_mode == "divide":
+ for i in range(len(X_input_list)):
+ if i < 3 * len(X_input_list) / 4:
+ self.X_input_list.append(X_input_list[i])
+ self.Y_output_list.append(Y_smooth[i])
+ else:
+ self.X_val_list.append(X_input_list[i])
+ self.Y_val_list.append(Y_smooth[i])
+
+ self.division_indices.append(len(self.X_input_list))
+ self.division_indices_val.append(len(self.X_val_list))
+
+ elif add_mode == "as_train":
+ for i in range(len(X_input_list)):
+ self.X_input_list.append(X_input_list[i])
+ self.Y_output_list.append(Y_smooth[i])
+
+ self.division_indices.append(len(self.X_input_list))
+ if add_mode == "as_val":
+ for i in range(len(X_input_list)):
+ self.X_val_list.append(X_input_list[i])
+ self.Y_val_list.append(Y_smooth[i])
+
+ self.division_indices_val.append(len(self.X_val_list))
def save_train_data(self, save_dir=".") -> None:
"""Save neural net teacher data."""
@@ -291,3 +344,126 @@ def save_train_data(self, save_dir=".") -> None:
X_input=np.array(self.X_input_list),
Y_output=np.array(self.Y_output_list),
)
+
+ def get_polynomial_regression_result(
+ self,
+ X_input_list: list[np.ndarray],
+ Y_output_list: list[np.ndarray],
+ use_polynomial_reg: bool,
+ use_selected_polynomial: bool,
+ deg: int,
+ fit_intercept: bool,
+ use_intercept: bool,
+ ) -> tuple[np.ndarray, np.ndarray]:
+ """Get the results of a polynomial regression."""
+ X_input = np.array(X_input_list)
+ Y_output = np.array(Y_output_list)
+ if use_selected_polynomial:
+ self.deg = 2
+ else:
+ self.deg = deg
+ self.A, self.b, Y_output_minus = polynomial_regression(
+ X_input,
+ Y_output,
+ self.alpha_1_for_polynomial_regression,
+ self.alpha_2_for_polynomial_regression,
+ use_polynomial_reg,
+ use_selected_polynomial,
+ self.deg,
+ fit_intercept,
+ use_intercept,
+ )
+ return X_input, Y_output_minus
+
+
+def polynomial_regression(
+ X,
+ Y,
+ alpha_1,
+ alpha_2,
+ use_polynomial_reg,
+ use_selected_polynomial,
+ deg,
+ fit_intercept,
+ use_intercept,
+ fit_acc_steer=False,
+):
+ polynomial_features = PolynomialFeatures(degree=deg, include_bias=False)
+ alpha = alpha_1 + alpha_2
+ if use_polynomial_reg:
+ if use_selected_polynomial:
+ clf_1 = linear_model.ElasticNet(
+ fit_intercept=fit_intercept, alpha=alpha, l1_ratio=alpha_1 / alpha, max_iter=100000
+ )
+ clf_2 = linear_model.ElasticNet(
+ fit_intercept=fit_intercept, alpha=alpha, l1_ratio=alpha_1 / alpha, max_iter=100000
+ )
+ clf_3 = linear_model.ElasticNet(
+ fit_intercept=fit_intercept, alpha=alpha, l1_ratio=alpha_1 / alpha, max_iter=100000
+ )
+ X_poly = polynomial_features.fit_transform(X[:, ctrl_index_for_polynomial_reg])
+ A = np.zeros((Y.shape[1], X_poly.shape[1]))
+ b = np.zeros(Y.shape[1])
+ clf_1.fit(X_poly[:, [0, ctrl_index_for_polynomial_reg.shape[0] + 2]], Y[:, [3]])
+ # clf_2.fit(X_poly[:, [1, 3,4,5]], Y[:, [4]])
+
+ acc_input_sum = np.zeros((3, 1))
+ acc_input_sum[0, 0] = drive_functions.ctrl_time_step / drive_functions.acc_time_constant
+ acc_input_sum[1, 0] = acc_input_sum[0, 0] * (1 - acc_input_sum[0, 0])
+ acc_input_sum[2, 0] = acc_input_sum[1, 0] * (1 - acc_input_sum[0, 0])
+ clf_2.fit(X_poly[:, [3, 4, 5]] @ acc_input_sum, Y[:, [4]])
+
+ # clf_3.fit(X_poly[:, [2, 6,7,8]], Y[:, [5]])
+
+ steer_input_sum = np.zeros((3, 1))
+ steer_input_sum[0, 0] = (
+ drive_functions.ctrl_time_step / drive_functions.steer_time_constant
+ )
+ steer_input_sum[1, 0] = steer_input_sum[0, 0] * (1 - steer_input_sum[0, 0])
+ steer_input_sum[2, 0] = steer_input_sum[1, 0] * (1 - steer_input_sum[0, 0])
+ clf_3.fit(X_poly[:, [6, 7, 8]] @ steer_input_sum, Y[:, [5]])
+
+ A[3, [0, ctrl_index_for_polynomial_reg.shape[0] + 2]] = clf_1.coef_
+ # A[4, [1, 3,4,5]] = clf_2.coef_
+ if fit_acc_steer:
+ A[4, [3, 4, 5]] = clf_2.coef_ * acc_input_sum.flatten()
+
+ # A[5, [2, 6,7,8]] = clf_3.coef_
+ if fit_acc_steer:
+ A[5, [6, 7, 8]] = clf_3.coef_ * steer_input_sum.flatten()
+
+ if fit_intercept:
+ b[3] = clf_1.intercept_
+ if fit_acc_steer:
+ b[4] = clf_2.intercept_
+ b[5] = clf_3.intercept_
+
+ if not use_intercept:
+ b = 0 * b
+ return A, b, Y - X_poly @ A.T - b
+ else:
+ clf = linear_model.ElasticNet(
+ fit_intercept=fit_intercept, alpha=alpha, l1_ratio=alpha_1 / alpha, max_iter=100000
+ )
+ X_poly = polynomial_features.fit_transform(X[:, ctrl_index_for_polynomial_reg])
+ clf.fit(X_poly, Y)
+ if fit_intercept:
+ if use_intercept:
+ return (
+ clf.coef_,
+ clf.intercept_,
+ Y - clf.predict(X_poly),
+ )
+ else:
+ return (
+ clf.coef_,
+ 0 * clf.intercept_,
+ Y - clf.predict(X_poly) + clf.intercept_,
+ )
+ else:
+ return clf.coef_, np.zeros(Y.shape[1]), Y - clf.predict(X_poly)
+ else:
+ poly_dim = polynomial_features.fit_transform(
+ X[0, ctrl_index_for_polynomial_reg].reshape(1, -1)
+ ).shape[1]
+ return np.zeros((Y.shape[1], poly_dim)), np.zeros(Y.shape[1]), Y
diff --git a/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/data_checker.ipynb b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/data_checker.ipynb
index 9ddd5fb6fad2a..c93b091b00888 100644
--- a/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/data_checker.ipynb
+++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/data_checker.ipynb
@@ -7,7 +7,7 @@
"metadata": {},
"outputs": [],
"source": [
- "# cspell: ignore ndimage usecols kernelspec ipykernel codemirror ipython nbconvert pygments nbformat \n",
+ "# cspell: ignore ndimage usecols kernelspec ipykernel codemirror ipython nbconvert pygments nbformat #\n",
"import numpy as np\n",
"from scipy.spatial.transform import Rotation\n",
"import scipy.interpolate\n",
@@ -31,7 +31,7 @@
" total_time_default=kinematic[-1,0]+1e-9*kinematic[-1,1]-start_time\n",
" vehicle_steer = np.loadtxt(dir_name+\"/steering_status.csv\",delimiter=',',usecols=[0,1,2])\n",
" steer = vehicle_steer[:,2]\n",
- " control_cmd = np.loadtxt(dir_name+\"/control_cmd_orig.csv\",delimiter=',',usecols=[0,1,4,9])\n",
+ " control_cmd = np.loadtxt(dir_name+\"/control_cmd_orig.csv\",delimiter=',',usecols=[0,1,8,16])\n",
" steer_des = control_cmd[:,2]\n",
" plt.plot(control_cmd[:,0] + 1e-9*control_cmd[:,1],steer_des,label=\"proxima_input\")\n",
" plt.plot(vehicle_steer[:,0] + 1e-9*vehicle_steer[:,1],steer,label=\"observed\")\n",
@@ -79,7 +79,7 @@
" total_time_default=kinematic[-1,0]+1e-9*kinematic[-1,1]-start_time\n",
" loc_acc = np.loadtxt(dir_name + \"/acceleration.csv\", delimiter=\",\", usecols=[0, 1, 3])\n",
" acc = loc_acc[:,2]\n",
- " control_cmd = np.loadtxt(dir_name+\"/control_cmd_orig.csv\",delimiter=',',usecols=[0,1,4,9])\n",
+ " control_cmd = np.loadtxt(dir_name+\"/control_cmd_orig.csv\",delimiter=',',usecols=[0,1,8,16])\n",
" acc_des = control_cmd[:,3]\n",
" plt.plot(control_cmd[:,0] + 1e-9*control_cmd[:,1],acc_des,label=\"proxima_input\")\n",
" plt.plot(loc_acc[:,0] + 1e-9*loc_acc[:,1],acc,label=\"observed\")\n",
diff --git a/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/execute_train_drive_NN_model.ipynb b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/execute_train_drive_NN_model.ipynb
index c2955883d5699..1f8934256831e 100644
--- a/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/execute_train_drive_NN_model.ipynb
+++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/execute_train_drive_NN_model.ipynb
@@ -7,7 +7,7 @@
"metadata": {},
"outputs": [],
"source": [
- "# cspell: ignore kernelspec ipykernel codemirror ipython nbconvert pygments nbformat \n",
+ "# cspell: ignore kernelspec ipykernel codemirror ipython nbconvert pygments nbformat #\n",
"import train_drive_NN_model"
]
},
diff --git a/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/train_drive_GP_model.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/train_drive_GP_model.py
index 914ed7b53c6ec..c16e301f2ebb8 100644
--- a/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/train_drive_GP_model.py
+++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/train_drive_GP_model.py
@@ -15,8 +15,10 @@
# cspell: ignore lengthscale savez
import GPy
+from autoware_smart_mpc_trajectory_follower.training_and_data_check import (
+ add_training_data_from_csv,
+)
import numpy as np
-from smart_mpc_trajectory_follower.training_and_data_check import add_training_data_from_csv
class train_drive_GP_model(add_training_data_from_csv.add_data_from_csv):
diff --git a/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/train_drive_NN_model.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/train_drive_NN_model.py
index 449ddda45f460..cf1b54e2868de 100644
--- a/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/train_drive_NN_model.py
+++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/train_drive_NN_model.py
@@ -12,707 +12,16 @@
# See the License for the specific language governing permissions and
# limitations under the License.
-# cspell: ignore optim savez suptitle
-
"""Class for training neural nets from driving data."""
-from autoware_smart_mpc_trajectory_follower.scripts import drive_NN
from autoware_smart_mpc_trajectory_follower.scripts import drive_functions
from autoware_smart_mpc_trajectory_follower.training_and_data_check import (
- add_training_data_from_csv,
+ train_drive_NN_model_with_memory,
)
-import matplotlib.pyplot as plt
-import numpy as np
-from sklearn import linear_model
-from sklearn.preprocessing import PolynomialFeatures
-import torch
-from torch import nn
-from torch.utils.data import DataLoader
-from torch.utils.data import TensorDataset
-
-ctrl_index_for_polynomial_reg = np.concatenate(
- (
- np.arange(3),
- [3 + drive_functions.acc_delay_step],
- [3 + drive_functions.acc_ctrl_queue_size + drive_functions.steer_delay_step],
- )
+from autoware_smart_mpc_trajectory_follower.training_and_data_check import (
+ train_drive_NN_model_without_memory,
)
-
-def nominal_model_acc(v: float, alpha_0: float, alpha: float) -> np.ndarray:
- """Predicted acceleration value according to the nominal model."""
- nominal_pred = alpha_0
- nominal_v = v + nominal_pred * drive_functions.ctrl_time_step
- nominal_pred = (
- nominal_pred
- + (alpha - nominal_pred)
- * drive_functions.ctrl_time_step
- / drive_functions.acc_time_constant
- )
- nominal_v = v + nominal_pred * drive_functions.ctrl_time_step
- nominal_pred = (
- nominal_pred
- + (alpha - nominal_pred)
- * drive_functions.ctrl_time_step
- / drive_functions.acc_time_constant
- )
- nominal_v = v + nominal_pred * drive_functions.ctrl_time_step
- nominal_pred = (
- nominal_pred
- + (alpha - nominal_pred)
- * drive_functions.ctrl_time_step
- / drive_functions.acc_time_constant
- )
- return np.array([nominal_v, nominal_pred])
-
-
-def nominal_model_steer(delta_0: float, delta: float) -> float:
- """Predicted steer angle according to the nominal model."""
- nominal_pred = delta_0
- nominal_pred = (
- nominal_pred
- + (delta - nominal_pred)
- * drive_functions.ctrl_time_step
- / drive_functions.steer_time_constant
- )
- nominal_pred = (
- nominal_pred
- + (delta - nominal_pred)
- * drive_functions.ctrl_time_step
- / drive_functions.steer_time_constant
- )
- nominal_pred = (
- nominal_pred
- + (delta - nominal_pred)
- * drive_functions.ctrl_time_step
- / drive_functions.steer_time_constant
- )
- return nominal_pred
-
-
-class train_drive_NN_model(add_training_data_from_csv.add_data_from_csv):
- """Class for training neural nets from driving data."""
-
- tanh_gain: float
- """Gain of tanh in the NN cost function"""
-
- lam: float
- """Weights of the tanh term in the NN cost function."""
-
- tol: float
- """Tolerances for terminating training iterations."""
-
- alpha_1: float
- """L¹ regularization weights in the NN cost function"""
-
- alpha_2: float
- """L² regularization weights in the NN cost function."""
-
- alpha_1_for_polynomial_regression: float
- """L¹ regularization weights for polynomial regression"""
-
- alpha_2_for_polynomial_regression: float
- """L² regularization weights for polynomial regression"""
-
- x_loss: list[float]
- """List of values for the loss function of the x component in the NN training."""
-
- y_loss: list[float]
- """List of values for the loss function of the y component in the NN training."""
-
- v_loss: list[float]
- """List of values for the loss function of the velocity component in the NN training."""
-
- theta_loss: list[float]
- """List of values for the loss function of the yaw angle component in the NN training."""
-
- acc_loss: list[float]
- """List of values for the loss function of the acceleration component in the NN training."""
-
- steer_loss: list[float]
- """List of values for the loss function of the steer angle component in the NN training."""
-
- steer_loss_plus_tanh: list[float]
- """List of values of the loss function for the steer angle component with tanh term in the NN training."""
-
- total_loss: list[float]
- """In NN training x_loss, ... , acc_loss, steer_loss_plus_tanh summed list of values"""
-
- model: drive_NN.DriveNeuralNetwork
- """trained neural network model"""
-
- A: np.ndarray
- """Coefficient matrix of polynomial regression model."""
-
- b: np.ndarray
- """Constant terms in polynomial regression models (bias)"""
-
- def __init__(
- self,
- tanh_gain=10,
- lam=0.1,
- tol=0.00001,
- alpha_1=0.1**7,
- alpha_2=0.1**7,
- alpha_1_for_polynomial_regression=0.1**5,
- alpha_2_for_polynomial_regression=0.1**5,
- ):
- super(train_drive_NN_model, self).__init__()
- self.tanh_gain = tanh_gain
- self.lam = lam
- self.tol = tol
- self.alpha_1 = alpha_1
- self.alpha_2 = alpha_2
- self.alpha_1_for_polynomial_regression = alpha_1_for_polynomial_regression
- self.alpha_2_for_polynomial_regression = alpha_2_for_polynomial_regression
- self.x_loss = []
- self.y_loss = []
- self.v_loss = []
- self.theta_loss = []
- self.acc_loss = []
- self.steer_loss = []
- self.steer_loss_plus_tanh = []
- self.total_loss = []
-
- def train_model(
- self,
- model: drive_NN.DriveNeuralNetwork,
- X_input: np.ndarray,
- Y_output: np.ndarray,
- learning_rates: list[float],
- patience: int = 10,
- batch_size: int | None = 50,
- max_iter: int = 100000,
- ) -> None:
- """Train the model."""
- if batch_size is None:
- batch_size = X_input.shape[0] // 50
- self.x_loss.clear()
- self.y_loss.clear()
- self.v_loss.clear()
- self.theta_loss.clear()
- self.acc_loss.clear()
- self.steer_loss.clear()
- self.steer_loss_plus_tanh.clear()
- self.total_loss.clear()
- sample_size = X_input.shape[0]
- num_train = int(3 * sample_size / 4)
- id_all = np.random.choice(sample_size, sample_size, replace=False)
- id_train = id_all[:num_train]
- id_val = id_all[num_train:]
- X_tensor = torch.tensor(X_input.astype(np.float32)).clone()
- Y_tensor = torch.tensor(Y_output.astype(np.float32)).clone()
- X_train = X_tensor[id_train]
- Y_train = Y_tensor[id_train]
- X_val = X_tensor[id_val]
- Y_val = Y_tensor[id_val]
-
- print("sample_size: ", X_input.shape[0])
- print("patience:", patience)
- loss_fn = torch.nn.L1Loss()
- learning_rate = learning_rates[0]
- optimizer = torch.optim.Adam(params=model.parameters(), lr=learning_rate)
-
- drive_data = DataLoader(
- TensorDataset(X_train, Y_train), batch_size=batch_size, shuffle=True
- )
- initial_loss = drive_NN.loss_fn_plus_tanh(
- loss_fn,
- torch.tensor(np.zeros(Y_val.shape), dtype=torch.float32),
- Y_val,
- self.tanh_gain,
- self.lam,
- )
- print("initial_loss:", initial_loss.detach().item())
- early_stopping = drive_NN.EarlyStopping(
- initial_loss=initial_loss, tol=self.tol, patience=patience
- )
- k = 0
- print("learning_rate:", learning_rates[0])
- for i in range(max_iter):
- model.train()
- for X_batch, y_batch in drive_data:
- y_pred = model(X_batch)
- loss = drive_NN.loss_fn_plus_tanh(
- loss_fn, y_pred, y_batch, self.tanh_gain, self.lam
- )
- for w in model.parameters():
- loss = (
- loss + self.alpha_1 * torch.norm(w, p=1) + self.alpha_2 * torch.norm(w) ** 2
- )
- optimizer.zero_grad()
- loss.backward()
- optimizer.step()
-
- model.eval()
- pred = model(X_val)
- self.total_loss.append(
- drive_NN.loss_fn_plus_tanh(loss_fn, pred, Y_val, self.tanh_gain, self.lam)
- .detach()
- .item()
- )
- self.x_loss.append(loss_fn(pred[:, [0]], Y_val[:, [0]]).detach().item())
- self.y_loss.append(loss_fn(pred[:, [1]], Y_val[:, [1]]).detach().item())
- self.v_loss.append(loss_fn(pred[:, [2]], Y_val[:, [2]]).detach().item())
- self.theta_loss.append(loss_fn(pred[:, [3]], Y_val[:, [3]]).detach().item())
- self.acc_loss.append(loss_fn(pred[:, [4]], Y_val[:, [4]]).detach().item())
- self.steer_loss.append(loss_fn(pred[:, [5]], Y_val[:, [5]]).detach().item())
- self.steer_loss_plus_tanh.append(
- loss_fn(pred[:, [5]], Y_val[:, [5]]).detach().item()
- + self.lam
- * loss_fn(
- torch.tanh(self.tanh_gain * (pred[:, -1] - Y_val[:, -1])),
- torch.zeros(Y_val.shape[0]),
- )
- .detach()
- .item()
- )
- if i % 10 == 1:
- current_loss = drive_NN.loss_fn_plus_tanh(
- loss_fn, model(X_val), Y_val, self.tanh_gain, self.lam
- )
- print(current_loss.detach().item(), i)
- if early_stopping(current_loss):
- k += 1
- if k == len(learning_rates):
- break
- else:
- learning_rate = learning_rates[k]
- print("update learning_rate to ", learning_rates[k])
- optimizer = torch.optim.Adam(params=model.parameters(), lr=learning_rate)
- early_stopping = drive_NN.EarlyStopping(
- initial_loss=initial_loss, tol=self.tol, patience=patience
- )
-
- def get_polynomial_regression_result(
- self,
- X_input_list: list[np.ndarray],
- Y_output_list: list[np.ndarray],
- use_polynomial_reg: bool,
- use_selected_polynomial: bool,
- deg: int,
- fit_intercept: bool,
- use_intercept: bool,
- ) -> tuple[np.ndarray, np.ndarray]:
- """Get the results of a polynomial regression."""
- X_input = np.array(X_input_list)
- X_input_core = X_input[
- :,
- : 3 + drive_functions.acc_ctrl_queue_size + drive_functions.steer_ctrl_queue_size_core,
- ].copy()
- Y_output = np.array(Y_output_list)
- if use_selected_polynomial:
- self.deg = 2
- else:
- self.deg = deg
- self.A, self.b, Y_output_minus = polynomial_regression(
- X_input_core,
- Y_output,
- self.alpha_1_for_polynomial_regression,
- self.alpha_2_for_polynomial_regression,
- use_polynomial_reg,
- use_selected_polynomial,
- self.deg,
- fit_intercept,
- use_intercept,
- )
- return X_input, Y_output_minus
-
- def plot_trained_result(
- self, show_flag=False, save_dir=".", plot_range=np.arange(500, 1200)
- ) -> None:
- """Plot the results of the training."""
- polynomial_features = PolynomialFeatures(degree=self.deg, include_bias=False)
-
- X_input = np.array(self.X_input_list)
- Y_output = np.array(self.Y_output_list)
- X_tensor = torch.tensor(X_input.astype(np.float32)).clone()
- acc_ctrl_queue_size = drive_functions.acc_ctrl_queue_size
- steer_ctrl_queue_size = drive_functions.steer_ctrl_queue_size
- ctrl_time_step = drive_functions.ctrl_time_step
- Y_pred = (
- self.model(X_tensor).detach().numpy()
- + polynomial_features.fit_transform(X_input[:, ctrl_index_for_polynomial_reg])
- @ (self.A).T
- + self.b
- )
- x = max(acc_ctrl_queue_size, steer_ctrl_queue_size) * ctrl_time_step
- y_labels = [
- "x_error [m]",
- "y_error [m]",
- "vel_error [m/s]",
- "yaw_error [rad]",
- "acc_error [m/s^2]",
- "steer_error [rad]",
- ]
-
- plt.figure(figsize=(24, 15), tight_layout=True)
- plt.subplot(231)
- ax = []
- for i in range(6):
- ax.append(plt.subplot(2, 3, i + 1))
- ax[-1].plot(
- drive_functions.ctrl_time_step * plot_range + x,
- Y_output[plot_range, i] * drive_functions.mpc_time_step,
- label="nominal_error",
- )
- ax[-1].plot(
- drive_functions.ctrl_time_step * plot_range + x,
- Y_pred[plot_range, i] * drive_functions.mpc_time_step,
- label="pred_error",
- )
- ax[-1].set_xlabel("sec")
- ax[-1].set_ylabel(y_labels[i])
- ax[-1].legend()
- if show_flag:
- plt.show()
- else:
- plt.savefig(save_dir + "/train_drive_NN_model_fig.png")
- plt.close()
-
- def plot_loss(self, show_flag=False, save_dir=".") -> None:
- """Plot the progression of values of the loss function of the training."""
- plt.figure(figsize=(24, 15), tight_layout=True)
-
- y_loss_labels = [
- "total_loss",
- "x_loss",
- "y_loss",
- "vel_loss",
- "yaw_loss",
- "acc_loss",
- "steer_loss",
- "steer_plus_tanh_loss",
- ]
- plt.subplot(241)
- ax_2 = []
- loss_list = [
- np.array(self.total_loss),
- np.array(self.x_loss),
- np.array(self.y_loss),
- np.array(self.v_loss),
- np.array(self.theta_loss),
- np.array(self.acc_loss),
- np.array(self.steer_loss),
- np.array(self.steer_loss_plus_tanh),
- ]
- for i in range(8):
- ax_2.append(plt.subplot(2, 4, i + 1))
- ax_2[-1].plot(
- np.arange(loss_list[i].shape[0]),
- loss_list[i],
- label="loss",
- )
- ax_2[-1].set_xlabel("iteration")
- ax_2[-1].set_ylabel(y_loss_labels[i])
- ax_2[-1].legend()
- if show_flag:
- plt.show()
- else:
- plt.savefig(save_dir + "/loss.png")
- plt.close()
-
- def get_trained_model(
- self,
- learning_rates: list[float] = [1e-3, 1e-4, 1e-5, 1e-6],
- randomize=0.01,
- acc_drop_out=0.0,
- steer_drop_out=0.0,
- patience: int = 10,
- batch_size: int | None = 50,
- max_iter=100000,
- use_polynomial_reg=False,
- use_selected_polynomial=True,
- force_NN_model_to_zero=False,
- fit_intercept=True,
- use_intercept=None,
- deg: int = 2,
- ):
- """Train on a model for which initial values are randomly given in a suitable range."""
- if use_intercept is None:
- if force_NN_model_to_zero:
- use_intercept = True
- else:
- use_intercept = False
- X_input, Y_output_minus = self.get_polynomial_regression_result(
- self.X_input_list,
- self.Y_output_list,
- use_polynomial_reg,
- use_selected_polynomial,
- deg,
- fit_intercept,
- use_intercept,
- )
- self.model = drive_NN.DriveNeuralNetwork(
- randomize=randomize,
- acc_drop_out=acc_drop_out,
- steer_drop_out=steer_drop_out,
- acc_queue_size=drive_functions.acc_ctrl_queue_size,
- steer_queue_size=drive_functions.steer_ctrl_queue_size,
- )
- if force_NN_model_to_zero:
- for w in self.model.parameters():
- w.data = nn.Parameter(torch.zeros(w.shape))
- else:
- self.train_model(
- self.model, X_input, Y_output_minus, learning_rates, patience, batch_size, max_iter
- )
-
- def update_trained_model(
- self,
- learning_rates=[1e-4, 1e-5, 1e-6],
- patience=5,
- batch_size=50,
- max_iter=100000,
- use_polynomial_reg=False,
- use_selected_polynomial=True,
- force_NN_model_to_zero=False,
- fit_intercept=True,
- use_intercept=None,
- deg: int = 2,
- ):
- """Update `self.model` with additional learning."""
- if use_intercept is None:
- if force_NN_model_to_zero:
- use_intercept = True
- else:
- use_intercept = False
- X_input, Y_output_minus = self.get_polynomial_regression_result(
- self.X_input_list,
- self.Y_output_list,
- use_polynomial_reg,
- use_selected_polynomial,
- deg,
- fit_intercept,
- use_intercept,
- )
- if force_NN_model_to_zero:
- for w in self.model.parameters():
- w.data = nn.Parameter(torch.zeros(w.shape))
- else:
- self.train_model(
- self.model, X_input, Y_output_minus, learning_rates, patience, batch_size, max_iter
- )
-
- def update_saved_trained_model(
- self,
- path="model_for_test_drive.pth",
- learning_rates: list[float] = [1e-4, 1e-5, 1e-6],
- patience=5,
- batch_size: int | None = 50,
- max_iter=100000,
- use_polynomial_reg=False,
- use_selected_polynomial=True,
- force_NN_model_to_zero=False,
- fit_intercept=True,
- use_intercept=None,
- deg: int = 2,
- ) -> None:
- """Load the saved model and update the model with additional training."""
- if use_intercept is None:
- if force_NN_model_to_zero:
- use_intercept = True
- else:
- use_intercept = False
- X_input, Y_output_minus = self.get_polynomial_regression_result(
- self.X_input_list,
- self.Y_output_list,
- use_polynomial_reg,
- use_selected_polynomial,
- deg,
- fit_intercept,
- use_intercept,
- )
- self.model = torch.load(path)
- if force_NN_model_to_zero:
- for w in self.model.parameters():
- w.data = nn.Parameter(torch.zeros(w.shape))
- else:
- self.train_model(
- self.model, X_input, Y_output_minus, learning_rates, patience, batch_size, max_iter
- )
-
- def save_model(self, save_dir=".", path="model_for_test_drive.pth") -> None:
- """Save trained NN models."""
- torch.save(self.model, save_dir + "/" + path)
-
- def save_polynomial_reg_info(self, save_dir=".", path="polynomial_reg_info") -> None:
- """Save the coefficients and degree of the resulting polynomial regression."""
- np.savez(save_dir + "/" + path, A=self.A, b=self.b, deg=self.deg)
-
- def save_models(
- self,
- save_dir=".",
- model_name="model_for_test_drive.pth",
- polynomial_reg_info_name="polynomial_reg_info",
- ) -> None:
- """Run save_model and save_polynomial_reg_info."""
- self.save_model(save_dir, model_name)
- self.save_polynomial_reg_info(save_dir, polynomial_reg_info_name)
-
- def predict_error(
- self, v: float, alpha_0: float, delta_0: float, alpha: float, delta: float
- ) -> np.ndarray:
- """Predicts the prediction error when following a nominal model."""
- polynomial_features = PolynomialFeatures(degree=self.deg, include_bias=False)
- x_input = np.zeros(
- 3 + drive_functions.acc_ctrl_queue_size + drive_functions.steer_ctrl_queue_size
- )
- x_input[0] = v
- x_input[1] = alpha_0
- x_input[2] = delta_0
- x_input[3 : 3 + drive_functions.acc_ctrl_queue_size] += alpha
- x_input[3 + drive_functions.acc_ctrl_queue_size :] += delta
- x_input_tensor = torch.tensor(x_input.reshape(1, -1).astype(np.float32))
- return (
- self.model(x_input_tensor)[0].detach().numpy()
- + self.A
- @ polynomial_features.fit_transform(
- x_input[ctrl_index_for_polynomial_reg].reshape(1, -1)
- )[0]
- + self.b
- )
-
- def sim(
- self, v: float, alpha_0: float, alpha: float, delta_0: float, delta: float, iter_times: int
- ) -> tuple[float, float, float]:
- """Simulate velocity, acceleration and steer changes according to a trained model."""
- v_pred = v
- alpha_pred = alpha_0
- delta_pred = delta_0
- for i in range(iter_times):
- pred = (
- np.concatenate(
- (
- nominal_model_acc(v_pred, alpha_pred, alpha),
- np.array([nominal_model_steer(delta_pred, delta)]),
- )
- )
- + self.predict_error(v_pred, alpha_pred, delta_pred, alpha, delta)[[2, 4, 5]]
- * drive_functions.mpc_time_step
- )
- v_pred = pred[0]
- alpha_pred = pred[1]
- steer_pred = pred[2]
- return v_pred, alpha_pred, steer_pred
-
- def plot_acc_map(self, iter_times: int, starting_steer=0.0, target_steer=0.0) -> None:
- """Run `sim` `iter_times` times. The simulation time is approximately 0.1*iter_times.
-
- Draw the resulting acceleration map.
- """
- acc = np.arange(-1, 1, 0.05)
- v_tests = np.arange(0, 12, 0.3)
- acc_result = np.zeros((acc.shape[0], v_tests.shape[0]))
- v_result = np.zeros((acc.shape[0], v_tests.shape[0]))
- steer_result = np.zeros((acc.shape[0], v_tests.shape[0]))
- for i in range(acc.shape[0]):
- v_tests_tmp = v_tests - 0.1 * iter_times * acc[i]
- for j in range(v_tests.shape[0]):
- v_result[i, j], acc_result[i, j], steer_result[i, j] = self.sim(
- v_tests_tmp[j], acc[i], acc[i], starting_steer, target_steer, iter_times
- )
- fig = plt.figure()
- ax = fig.add_subplot(111, projection="3d")
- ax.plot_surface(np.tile(acc, (v_tests.shape[0], 1)).T, v_result, acc_result)
- ax.set_xlabel("acc_input [m/s^2]")
- ax.set_ylabel("vel [m/s]")
- ax.set_zlabel("acc_sim [m/s^2]")
- fig.suptitle(str(iter_times * drive_functions.mpc_time_step) + " sec simulation")
- plt.show()
-
- def plot_steer_map(self, iter_times: int, starting_acc=0.0, target_acc=0.0) -> None:
- """Run `sim` `iter_times` times. The simulation time is approximately 0.1*iter_times.
-
- Draw the resulting steer map.
- """
- steer = np.arange(-1, 1, 0.05)
- v_tests = np.arange(0, 12, 0.3)
- acc_result = np.zeros((steer.shape[0], v_tests.shape[0]))
- v_result = np.zeros((steer.shape[0], v_tests.shape[0]))
- steer_result = np.zeros((steer.shape[0], v_tests.shape[0]))
- for i in range(steer.shape[0]):
- for j in range(v_tests.shape[0]):
- v_result[i, j], acc_result[i, j], steer_result[i, j] = self.sim(
- v_tests[j], starting_acc, target_acc, steer[i], steer[i], iter_times
- )
- fig = plt.figure()
- ax = fig.add_subplot(111, projection="3d")
- ax.plot_surface(np.tile(steer, (v_tests.shape[0], 1)).T, v_result, steer_result)
- ax.set_xlabel("steer_input [rad]")
- ax.set_ylabel("vel [m/s]")
- ax.set_zlabel("steer_sim [rad]")
- fig.suptitle(str(iter_times * drive_functions.mpc_time_step) + " sec simulation")
- plt.show()
-
-
-def polynomial_regression(
- X,
- Y,
- alpha_1,
- alpha_2,
- use_polynomial_reg,
- use_selected_polynomial,
- deg,
- fit_intercept,
- use_intercept,
-):
- polynomial_features = PolynomialFeatures(degree=deg, include_bias=False)
- alpha = alpha_1 + alpha_2
- if use_polynomial_reg:
- if use_selected_polynomial:
- clf_1 = linear_model.ElasticNet(
- fit_intercept=fit_intercept, alpha=alpha, l1_ratio=alpha_1 / alpha, max_iter=100000
- )
- clf_2 = linear_model.ElasticNet(
- fit_intercept=fit_intercept, alpha=alpha, l1_ratio=alpha_1 / alpha, max_iter=100000
- )
- clf_3 = linear_model.ElasticNet(
- fit_intercept=fit_intercept, alpha=alpha, l1_ratio=alpha_1 / alpha, max_iter=100000
- )
- X_poly = polynomial_features.fit_transform(X[:, ctrl_index_for_polynomial_reg])
- Y_error = Y.copy()
- A = np.zeros((Y.shape[1], X_poly.shape[1]))
- b = np.zeros(Y.shape[1])
- clf_1.fit(X_poly[:, [0, 7]], Y[:, [3]])
- clf_2.fit(X_poly[:, [1, 3]], Y[:, [4]])
- clf_3.fit(X_poly[:, [2, 4]], Y[:, [5]])
- A[3, [0, 7]] = clf_1.coef_
- A[4, [1, 3]] = clf_2.coef_
- A[5, [2, 4]] = clf_3.coef_
- if fit_intercept:
- b[3] = clf_1.intercept_
- b[4] = clf_2.intercept_
- b[5] = clf_3.intercept_
- Y_error[:, 3] -= clf_1.predict(X_poly[:, [0, 7]])
- Y_error[:, 4] -= clf_2.predict(X_poly[:, [1, 3]])
- Y_error[:, 5] -= clf_3.predict(X_poly[:, [2, 4]])
- if not use_intercept:
- Y_error += b
- b = 0 * b
- return A, b, Y_error
- else:
- clf = linear_model.ElasticNet(
- fit_intercept=fit_intercept, alpha=alpha, l1_ratio=alpha_1 / alpha, max_iter=100000
- )
- X_poly = polynomial_features.fit_transform(X[:, ctrl_index_for_polynomial_reg])
- clf.fit(X_poly, Y)
- if fit_intercept:
- if use_intercept:
- return (
- clf.coef_,
- clf.intercept_,
- Y - clf.predict(X_poly),
- )
- else:
- return (
- clf.coef_,
- 0 * clf.intercept_,
- Y - clf.predict(X_poly) + clf.intercept_,
- )
- else:
- return clf.coef_, np.zeros(Y.shape[1]), Y - clf.predict(X_poly)
- else:
- poly_dim = polynomial_features.fit_transform(
- X[0, ctrl_index_for_polynomial_reg].reshape(1, -1)
- ).shape[1]
- return np.zeros((Y.shape[1], poly_dim)), np.zeros(Y.shape[1]), Y
+if drive_functions.use_memory_for_training:
+ train_drive_NN_model = train_drive_NN_model_with_memory.train_drive_NN_model_with_memory
+else:
+ train_drive_NN_model = train_drive_NN_model_without_memory.train_drive_NN_model_without_memory
diff --git a/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/train_drive_NN_model_with_memory.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/train_drive_NN_model_with_memory.py
new file mode 100644
index 0000000000000..84e0bbe7a7a83
--- /dev/null
+++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/train_drive_NN_model_with_memory.py
@@ -0,0 +1,664 @@
+# Copyright 2024 Proxima Technology Inc, TIER IV
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+# cspell: ignore optim savez suptitle
+
+"""Class for training neural nets with memory from driving data."""
+from autoware_smart_mpc_trajectory_follower.scripts import drive_NN
+from autoware_smart_mpc_trajectory_follower.scripts import drive_functions
+from autoware_smart_mpc_trajectory_follower.training_and_data_check import (
+ add_training_data_from_csv,
+)
+import matplotlib.pyplot as plt # type: ignore
+import numpy as np
+from sklearn.preprocessing import PolynomialFeatures # type: ignore
+import torch
+from torch import nn
+from torch.utils.data import DataLoader
+from torch.utils.data import TensorDataset
+
+ctrl_index_for_polynomial_reg = np.concatenate(
+ (
+ np.arange(3),
+ 3
+ + max(drive_functions.acc_delay_step - drive_functions.mpc_freq, 0)
+ + np.arange(drive_functions.mpc_freq),
+ 3
+ + drive_functions.acc_ctrl_queue_size
+ + max(drive_functions.steer_delay_step - drive_functions.mpc_freq, 0)
+ + np.arange(drive_functions.mpc_freq),
+ )
+)
+
+
+def transform_to_sequence_data(X, seq_size, indices):
+ X_seq = []
+ start_num = 0
+ for i in range(len(indices)):
+ j = 0
+ while start_num + j + drive_functions.mpc_freq * seq_size <= indices[i]:
+ X_seq.append(X[start_num + j + drive_functions.mpc_freq * np.arange(seq_size)])
+ j += 1
+ start_num = indices[i]
+
+ return np.array(X_seq)
+
+
+class train_drive_NN_model_with_memory(add_training_data_from_csv.add_data_from_csv):
+ """Class for training neural nets from driving data."""
+
+ tanh_gain: float
+ """Gain of tanh in the NN cost function."""
+
+ lam: float
+ """Weights of the tanh term in the NN cost function."""
+
+ tol: float
+ """Tolerances for terminating training iterations."""
+
+ alpha_1: float
+ """L¹ regularization weights in the NN cost function."""
+
+ alpha_2: float
+ """L² regularization weights in the NN cost function."""
+
+ alpha_1_for_polynomial_regression: float
+ """L¹ regularization weights for polynomial regression."""
+
+ alpha_2_for_polynomial_regression: float
+ """L² regularization weights for polynomial regression."""
+
+ x_loss: list[float]
+ """List of values for the loss function of the x component in the NN training."""
+
+ y_loss: list[float]
+ """List of values for the loss function of the y component in the NN training."""
+
+ v_loss: list[float]
+ """List of values for the loss function of the velocity component in the NN training."""
+
+ theta_loss: list[float]
+ """List of values for the loss function of the yaw angle component in the NN training."""
+
+ acc_loss: list[float]
+ """List of values for the loss function of the acceleration component in the NN training."""
+
+ steer_loss: list[float]
+ """List of values for the loss function of the steer angle component in the NN training."""
+
+ steer_loss_plus_tanh: list[float]
+ """List of values of the loss function for the steer angle component with tanh term in the NN training."""
+
+ total_loss: list[float]
+ """In NN training x_loss, ... , acc_loss, steer_loss_plus_tanh summed list of values."""
+
+ model: drive_NN.DriveNeuralNetworkWithMemory
+ """trained neural network model."""
+
+ A: np.ndarray
+ """Coefficient matrix of polynomial regression model."""
+
+ b: np.ndarray
+ """Constant terms in polynomial regression models (bias)."""
+
+ def __init__(
+ self,
+ tanh_gain=10,
+ lam=0.1,
+ tol=0.00001,
+ alpha_1=0.1**7,
+ alpha_2=0.1**7,
+ alpha_1_for_polynomial_regression=0.1**5,
+ alpha_2_for_polynomial_regression=0.1**5,
+ seq_size=2 * drive_functions.N + 1,
+ first_order_weight=1.0,
+ second_order_weight=1.0,
+ ):
+ super(train_drive_NN_model_with_memory, self).__init__()
+ self.tanh_gain = tanh_gain
+ self.lam = lam
+ self.tol = tol
+ self.alpha_1 = alpha_1
+ self.alpha_2 = alpha_2
+ self.alpha_1_for_polynomial_regression = alpha_1_for_polynomial_regression
+ self.alpha_2_for_polynomial_regression = alpha_2_for_polynomial_regression
+ self.x_loss = []
+ self.y_loss = []
+ self.v_loss = []
+ self.theta_loss = []
+ self.acc_loss = []
+ self.steer_loss = []
+ self.steer_loss_plus_tanh = []
+ self.total_loss = []
+ self.seq_size = seq_size
+ self.first_order_weight = first_order_weight
+ self.second_order_weight = second_order_weight
+
+ def train_model(
+ self,
+ model: drive_NN.DriveNeuralNetworkWithMemory,
+ X_input: np.ndarray,
+ Y_output: np.ndarray,
+ learning_rates: list[float],
+ patience: int = 10,
+ batch_size: int | None = 5,
+ max_iter: int = 100000,
+ X_val_np=None,
+ Y_val_np=None,
+ ) -> None:
+ """Train the model."""
+ if batch_size is None:
+ batch_size = X_input.shape[0] // 50
+ self.x_loss.clear()
+ self.y_loss.clear()
+ self.v_loss.clear()
+ self.theta_loss.clear()
+ self.acc_loss.clear()
+ self.steer_loss.clear()
+ self.steer_loss_plus_tanh.clear()
+ self.total_loss.clear()
+
+ if X_val_np is None:
+ X_tensor = torch.tensor(
+ transform_to_sequence_data(X_input, self.seq_size, self.division_indices).astype(
+ np.float32
+ )
+ ).clone()
+ Y_tensor = torch.tensor(
+ transform_to_sequence_data(Y_output, self.seq_size, self.division_indices).astype(
+ np.float32
+ )
+ ).clone()
+ sample_size = X_tensor.shape[0]
+ num_train = int(3 * sample_size / 4)
+ id_all = np.random.choice(sample_size, sample_size, replace=False)
+ id_train = id_all[:num_train]
+ id_val = id_all[num_train:]
+ X_train = X_tensor[id_train]
+ Y_train = Y_tensor[id_train]
+ X_val = X_tensor[id_val]
+ Y_val = Y_tensor[id_val]
+ print("sample_size: ", X_input.shape[0])
+ else:
+ X_train = torch.tensor(
+ transform_to_sequence_data(X_input, self.seq_size, self.division_indices).astype(
+ np.float32
+ )
+ ).clone()
+ Y_train = torch.tensor(
+ transform_to_sequence_data(Y_output, self.seq_size, self.division_indices).astype(
+ np.float32
+ )
+ ).clone()
+ X_val = torch.tensor(
+ transform_to_sequence_data(
+ X_val_np, self.seq_size, self.division_indices_val
+ ).astype(np.float32)
+ ).clone()
+ Y_val = torch.tensor(
+ transform_to_sequence_data(
+ Y_val_np, self.seq_size, self.division_indices_val
+ ).astype(np.float32)
+ ).clone()
+ print("sample_size", X_input.shape[0] + X_val_np.shape[0])
+ print("patience:", patience)
+ loss_fn = torch.nn.L1Loss()
+ learning_rate = learning_rates[0]
+ optimizer = torch.optim.Adam(params=model.parameters(), lr=learning_rate)
+
+ drive_data = DataLoader(
+ TensorDataset(X_train, Y_train), batch_size=batch_size, shuffle=True
+ )
+ initial_loss = drive_NN.loss_fn_plus_tanh_with_memory(
+ loss_fn,
+ torch.tensor(np.zeros(Y_val.shape)[:, self.seq_size // 2 :], dtype=torch.float32),
+ Y_val[:, self.seq_size // 2 :],
+ self.tanh_gain,
+ self.lam,
+ self.first_order_weight,
+ self.second_order_weight,
+ )
+ print("initial_loss:", initial_loss.detach().item())
+ early_stopping = drive_NN.EarlyStopping(
+ initial_loss=initial_loss, tol=self.tol, patience=patience
+ )
+ k = 0
+ print("learning_rate:", learning_rates[0])
+ for i in range(max_iter):
+ model.train()
+ for X_batch, y_batch in drive_data:
+ y_pred = model(X_batch)
+ loss = drive_NN.loss_fn_plus_tanh_with_memory(
+ loss_fn,
+ y_pred[:, self.seq_size // 2 :],
+ y_batch[:, self.seq_size // 2 :],
+ self.tanh_gain,
+ self.lam,
+ self.first_order_weight,
+ self.second_order_weight,
+ )
+ for w in model.named_parameters():
+ loss = (
+ loss
+ + self.alpha_1 * torch.norm(w[1], p=1)
+ + self.alpha_2 * torch.norm(w[1]) ** 2
+ )
+ if w[0] in ["finalize.weight", "finalize.bias"]:
+ loss = loss + (drive_functions.finalize_x_weight - 1) * (
+ self.alpha_1 * torch.norm(w[1][0], p=1)
+ + self.alpha_2 * torch.norm(w[1][0]) ** 2
+ )
+ loss = loss + (drive_functions.finalize_y_weight - 1) * (
+ self.alpha_1 * torch.norm(w[1][1], p=1)
+ + self.alpha_2 * torch.norm(w[1][1]) ** 2
+ )
+ loss = loss + (drive_functions.finalize_v_weight - 1) * (
+ self.alpha_1 * torch.norm(w[1][2], p=1)
+ + self.alpha_2 * torch.norm(w[1][2]) ** 2
+ )
+ optimizer.zero_grad()
+ loss.backward()
+ optimizer.step()
+
+ model.eval()
+ pred = model(X_val)
+ self.total_loss.append(
+ drive_NN.loss_fn_plus_tanh_with_memory(
+ loss_fn,
+ pred[:, self.seq_size // 2 :],
+ Y_val[:, self.seq_size // 2 :],
+ self.tanh_gain,
+ self.lam,
+ self.first_order_weight,
+ self.second_order_weight,
+ )
+ .detach()
+ .item()
+ )
+ self.x_loss.append(
+ loss_fn(pred[:, self.seq_size // 2 :, [0]], Y_val[:, self.seq_size // 2 :, [0]])
+ .detach()
+ .item()
+ )
+ self.y_loss.append(
+ loss_fn(pred[:, self.seq_size // 2 :, [1]], Y_val[:, self.seq_size // 2 :, [1]])
+ .detach()
+ .item()
+ )
+ self.v_loss.append(
+ loss_fn(pred[:, self.seq_size // 2 :, [2]], Y_val[:, self.seq_size // 2 :, [2]])
+ .detach()
+ .item()
+ )
+ self.theta_loss.append(
+ loss_fn(pred[:, self.seq_size // 2 :, [3]], Y_val[:, self.seq_size // 2 :, [3]])
+ .detach()
+ .item()
+ )
+ self.acc_loss.append(
+ loss_fn(pred[:, self.seq_size // 2 :, [4]], Y_val[:, self.seq_size // 2 :, [4]])
+ .detach()
+ .item()
+ )
+ self.steer_loss.append(
+ loss_fn(pred[:, self.seq_size // 2 :, [5]], Y_val[:, self.seq_size // 2 :, [5]])
+ .detach()
+ .item()
+ )
+ self.steer_loss_plus_tanh.append(
+ loss_fn(pred[:, self.seq_size // 2 :, [5]], Y_val[:, self.seq_size // 2 :, [5]])
+ .detach()
+ .item()
+ + self.lam
+ * loss_fn(
+ torch.tanh(
+ self.tanh_gain
+ * (pred[:, self.seq_size // 2 :, -1] - Y_val[:, self.seq_size // 2 :, -1])
+ ),
+ torch.zeros((Y_val.shape[0], Y_val.shape[1] - self.seq_size // 2)),
+ )
+ .detach()
+ .item()
+ )
+
+ current_loss = drive_NN.loss_fn_plus_tanh_with_memory(
+ loss_fn,
+ model(X_val)[:, self.seq_size // 2 :],
+ Y_val[:, self.seq_size // 2 :],
+ self.tanh_gain,
+ self.lam,
+ self.first_order_weight,
+ self.second_order_weight,
+ )
+ if i % 10 == 1:
+ print(current_loss.detach().item(), i)
+ if early_stopping(current_loss):
+ k += 1
+ if k == len(learning_rates):
+ break
+ else:
+ learning_rate = learning_rates[k]
+ print("update learning_rate to ", learning_rates[k])
+ optimizer = torch.optim.Adam(params=model.parameters(), lr=learning_rate)
+ early_stopping = drive_NN.EarlyStopping(
+ initial_loss=initial_loss, tol=self.tol, patience=patience
+ )
+
+ def plot_loss(self, show_flag=False, save_dir=".") -> None:
+ """Plot the progression of values of the loss function of the training."""
+ plt.figure(figsize=(24, 15), tight_layout=True)
+
+ y_loss_labels = [
+ "total_loss",
+ "x_loss",
+ "y_loss",
+ "vel_loss",
+ "yaw_loss",
+ "acc_loss",
+ "steer_loss",
+ "steer_plus_tanh_loss",
+ ]
+ plt.subplot(2, 4, 1)
+ ax_2 = []
+ loss_list = [
+ np.array(self.total_loss),
+ np.array(self.x_loss),
+ np.array(self.y_loss),
+ np.array(self.v_loss),
+ np.array(self.theta_loss),
+ np.array(self.acc_loss),
+ np.array(self.steer_loss),
+ np.array(self.steer_loss_plus_tanh),
+ ]
+ for i in range(8):
+ ax_2.append(plt.subplot(2, 4, i + 1))
+ ax_2[-1].plot(
+ np.arange(loss_list[i].shape[0]),
+ loss_list[i],
+ label="loss",
+ )
+ ax_2[-1].set_xlabel("iteration")
+ ax_2[-1].set_ylabel(y_loss_labels[i])
+ ax_2[-1].legend()
+ if show_flag:
+ plt.show()
+ else:
+ plt.savefig(save_dir + "/loss.png")
+ plt.close()
+
+ def plot_trained_result(
+ self, show_flag=False, save_dir=".", plot_range=np.arange(500, 1200)
+ ) -> None:
+ """Plot the results of the training."""
+ polynomial_features = PolynomialFeatures(degree=self.deg, include_bias=False)
+
+ X_input = np.array(self.X_input_list)
+ Y_output = np.array(self.Y_output_list)
+ X_tensor = torch.tensor(X_input.astype(np.float32)).clone()
+ acc_ctrl_queue_size = drive_functions.acc_ctrl_queue_size
+ steer_ctrl_queue_size = drive_functions.steer_ctrl_queue_size
+ ctrl_time_step = drive_functions.ctrl_time_step
+ Y_pred = np.zeros(Y_output.shape)
+ start_index = 0
+ for i in range(len(self.division_indices)):
+ Y_pred[start_index : self.division_indices[i]] = (
+ self.model(X_tensor[start_index : self.division_indices[i]].unsqueeze(dim=0))[0]
+ .detach()
+ .numpy()
+ + polynomial_features.fit_transform(
+ X_input[start_index : self.division_indices[i], ctrl_index_for_polynomial_reg]
+ )
+ @ (self.A).T
+ + self.b
+ )
+ start_index = self.division_indices[i]
+ x = max(acc_ctrl_queue_size, steer_ctrl_queue_size) * ctrl_time_step
+ y_labels = [
+ "x_error [m]",
+ "y_error [m]",
+ "vel_error [m/s]",
+ "yaw_error [rad]",
+ "acc_error [m/s^2]",
+ "steer_error [rad]",
+ ]
+
+ plt.figure(figsize=(24, 15), tight_layout=True)
+ plt.subplot(2, 3, 1)
+ ax = []
+ for i in range(6):
+ ax.append(plt.subplot(2, 3, i + 1))
+ ax[-1].plot(
+ drive_functions.ctrl_time_step * plot_range + x,
+ Y_output[plot_range, i] * drive_functions.mpc_time_step,
+ label="nominal_error",
+ )
+ ax[-1].plot(
+ drive_functions.ctrl_time_step * plot_range + x,
+ Y_pred[plot_range, i] * drive_functions.mpc_time_step,
+ label="pred_error",
+ )
+ ax[-1].set_xlabel("sec")
+ ax[-1].set_ylabel(y_labels[i])
+ ax[-1].legend()
+ if show_flag:
+ plt.show()
+ else:
+ plt.savefig(save_dir + "/train_drive_NN_model_fig.png")
+ plt.close()
+
+ def get_trained_model(
+ self,
+ hidden_layer_sizes=(16, 16),
+ hidden_layer_lstm=8,
+ learning_rates: list[float] = [1e-3, 1e-4, 1e-5, 1e-6],
+ randomize=0.01,
+ acc_drop_out=0.0,
+ steer_drop_out=0.0,
+ patience: int = 10,
+ batch_size: int | None = 5,
+ max_iter=100000,
+ use_polynomial_reg=False,
+ use_selected_polynomial=True,
+ force_NN_model_to_zero=False,
+ fit_intercept=True,
+ use_intercept=None,
+ deg: int = 2,
+ ):
+ """Train on a model for which initial values are randomly given in a suitable range."""
+ if use_intercept is None:
+ if force_NN_model_to_zero:
+ use_intercept = True
+ else:
+ use_intercept = False
+ X_input, Y_output_minus = self.get_polynomial_regression_result(
+ self.X_input_list,
+ self.Y_output_list,
+ use_polynomial_reg,
+ use_selected_polynomial,
+ deg,
+ fit_intercept,
+ use_intercept,
+ )
+
+ if len(self.X_val_list) > 0:
+ X_val = np.array(self.X_val_list)
+ Y_val = np.array(self.Y_val_list)
+ polynomial_features = PolynomialFeatures(degree=self.deg, include_bias=False)
+ Y_val = (
+ Y_val
+ - polynomial_features.fit_transform(X_val[:, ctrl_index_for_polynomial_reg])
+ @ (self.A).T
+ - self.b
+ )
+ else:
+ X_val = None
+ Y_val = None
+ self.model = drive_NN.DriveNeuralNetworkWithMemory(
+ hidden_layer_sizes=hidden_layer_sizes,
+ hidden_layer_lstm=hidden_layer_lstm,
+ randomize=randomize,
+ acc_drop_out=acc_drop_out,
+ steer_drop_out=steer_drop_out,
+ acc_queue_size=drive_functions.acc_ctrl_queue_size,
+ steer_queue_size=drive_functions.steer_ctrl_queue_size,
+ )
+ if force_NN_model_to_zero:
+ for w in self.model.parameters():
+ w.data = nn.Parameter(torch.zeros(w.shape))
+ else:
+ self.train_model(
+ self.model,
+ X_input,
+ Y_output_minus,
+ learning_rates,
+ patience,
+ batch_size,
+ max_iter,
+ X_val,
+ Y_val,
+ )
+
+ def update_trained_model(
+ self,
+ learning_rates=[1e-4, 1e-5, 1e-6],
+ patience=10,
+ batch_size=5,
+ max_iter=100000,
+ use_polynomial_reg=False,
+ use_selected_polynomial=True,
+ force_NN_model_to_zero=False,
+ fit_intercept=True,
+ use_intercept=None,
+ deg: int = 2,
+ ):
+ """Update `self.model` with additional learning."""
+ if use_intercept is None:
+ if force_NN_model_to_zero:
+ use_intercept = True
+ else:
+ use_intercept = False
+ X_input, Y_output_minus = self.get_polynomial_regression_result(
+ self.X_input_list,
+ self.Y_output_list,
+ use_polynomial_reg,
+ use_selected_polynomial,
+ deg,
+ fit_intercept,
+ use_intercept,
+ )
+ if len(self.X_val_list) > 0:
+ X_val = np.array(self.X_val_list)
+ Y_val = np.array(self.Y_val_list)
+ polynomial_features = PolynomialFeatures(degree=self.deg, include_bias=False)
+ Y_val = (
+ Y_val
+ - polynomial_features.fit_transform(X_val[:, ctrl_index_for_polynomial_reg])
+ @ (self.A).T
+ - self.b
+ )
+ else:
+ X_val = None
+ Y_val = None
+ if force_NN_model_to_zero:
+ for w in self.model.parameters():
+ w.data = nn.Parameter(torch.zeros(w.shape))
+ else:
+ self.train_model(
+ self.model,
+ X_input,
+ Y_output_minus,
+ learning_rates,
+ patience,
+ batch_size,
+ max_iter,
+ X_val,
+ Y_val,
+ )
+
+ def update_saved_trained_model(
+ self,
+ path="model_for_test_drive.pth",
+ learning_rates: list[float] = [1e-4, 1e-5, 1e-6],
+ patience=10,
+ batch_size: int | None = 5,
+ max_iter=100000,
+ use_polynomial_reg=False,
+ use_selected_polynomial=True,
+ force_NN_model_to_zero=False,
+ fit_intercept=True,
+ use_intercept=None,
+ deg: int = 2,
+ ) -> None:
+ """Load the saved model and update the model with additional training."""
+ if use_intercept is None:
+ if force_NN_model_to_zero:
+ use_intercept = True
+ else:
+ use_intercept = False
+ X_input, Y_output_minus = self.get_polynomial_regression_result(
+ self.X_input_list,
+ self.Y_output_list,
+ use_polynomial_reg,
+ use_selected_polynomial,
+ deg,
+ fit_intercept,
+ use_intercept,
+ )
+ if len(self.X_val_list) > 0:
+ X_val = np.array(self.X_val_list)
+ Y_val = np.array(self.Y_val_list)
+ polynomial_features = PolynomialFeatures(degree=self.deg, include_bias=False)
+ Y_val = (
+ Y_val
+ - polynomial_features.fit_transform(X_val[:, ctrl_index_for_polynomial_reg])
+ @ (self.A).T
+ - self.b
+ )
+ else:
+ X_val = None
+ Y_val = None
+ self.model = torch.load(path)
+ if force_NN_model_to_zero:
+ for w in self.model.parameters():
+ w.data = nn.Parameter(torch.zeros(w.shape))
+ else:
+ self.train_model(
+ self.model,
+ X_input,
+ Y_output_minus,
+ learning_rates,
+ patience,
+ batch_size,
+ max_iter,
+ X_val,
+ Y_val,
+ )
+
+ def save_model(self, save_dir=".", path="model_for_test_drive.pth") -> None:
+ """Save trained NN models."""
+ torch.save(self.model, save_dir + "/" + path)
+
+ def save_polynomial_reg_info(self, save_dir=".", path="polynomial_reg_info") -> None:
+ """Save the coefficients and degree of the resulting polynomial regression."""
+ np.savez(save_dir + "/" + path, A=self.A, b=self.b, deg=self.deg)
+
+ def save_models(
+ self,
+ save_dir=".",
+ model_name="model_for_test_drive.pth",
+ polynomial_reg_info_name="polynomial_reg_info",
+ ) -> None:
+ """Run save_model and save_polynomial_reg_info."""
+ self.save_model(save_dir, model_name)
+ self.save_polynomial_reg_info(save_dir, polynomial_reg_info_name)
diff --git a/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/train_drive_NN_model_without_memory.py b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/train_drive_NN_model_without_memory.py
new file mode 100644
index 0000000000000..0117bb1707682
--- /dev/null
+++ b/control/autoware_smart_mpc_trajectory_follower/autoware_smart_mpc_trajectory_follower/training_and_data_check/train_drive_NN_model_without_memory.py
@@ -0,0 +1,704 @@
+# Copyright 2024 Proxima Technology Inc, TIER IV
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+# cspell: ignore optim savez suptitle
+
+"""Class for training neural nets from driving data."""
+from autoware_smart_mpc_trajectory_follower.scripts import drive_NN
+from autoware_smart_mpc_trajectory_follower.scripts import drive_functions
+from autoware_smart_mpc_trajectory_follower.training_and_data_check import (
+ add_training_data_from_csv,
+)
+import matplotlib.pyplot as plt # type: ignore
+import numpy as np
+from sklearn.preprocessing import PolynomialFeatures # type: ignore
+import torch
+from torch import nn
+from torch.utils.data import DataLoader
+from torch.utils.data import TensorDataset
+
+ctrl_index_for_polynomial_reg = np.concatenate(
+ (
+ np.arange(3),
+ 3
+ + max(drive_functions.acc_delay_step - drive_functions.mpc_freq, 0)
+ + np.arange(drive_functions.mpc_freq),
+ 3
+ + drive_functions.acc_ctrl_queue_size
+ + max(drive_functions.steer_delay_step - drive_functions.mpc_freq, 0)
+ + np.arange(drive_functions.mpc_freq),
+ )
+)
+
+
+def nominal_model_acc(v: float, alpha_0: float, alpha: float) -> np.ndarray:
+ """Predicted acceleration value according to the nominal model."""
+ nominal_pred = alpha_0
+ nominal_v = v + nominal_pred * drive_functions.ctrl_time_step
+ nominal_pred = (
+ nominal_pred
+ + (alpha - nominal_pred)
+ * drive_functions.ctrl_time_step
+ / drive_functions.acc_time_constant
+ )
+ nominal_v = v + nominal_pred * drive_functions.ctrl_time_step
+ nominal_pred = (
+ nominal_pred
+ + (alpha - nominal_pred)
+ * drive_functions.ctrl_time_step
+ / drive_functions.acc_time_constant
+ )
+ nominal_v = v + nominal_pred * drive_functions.ctrl_time_step
+ nominal_pred = (
+ nominal_pred
+ + (alpha - nominal_pred)
+ * drive_functions.ctrl_time_step
+ / drive_functions.acc_time_constant
+ )
+ return np.array([nominal_v, nominal_pred])
+
+
+def nominal_model_steer(delta_0: float, delta: float) -> float:
+ """Predicted steer angle according to the nominal model."""
+ nominal_pred = delta_0
+ nominal_pred = (
+ nominal_pred
+ + (delta - nominal_pred)
+ * drive_functions.ctrl_time_step
+ / drive_functions.steer_time_constant
+ )
+ nominal_pred = (
+ nominal_pred
+ + (delta - nominal_pred)
+ * drive_functions.ctrl_time_step
+ / drive_functions.steer_time_constant
+ )
+ nominal_pred = (
+ nominal_pred
+ + (delta - nominal_pred)
+ * drive_functions.ctrl_time_step
+ / drive_functions.steer_time_constant
+ )
+ return nominal_pred
+
+
+class train_drive_NN_model_without_memory(add_training_data_from_csv.add_data_from_csv):
+ """Class for training neural nets from driving data."""
+
+ tanh_gain: float
+ """Gain of tanh in the NN cost function"""
+
+ lam: float
+ """Weights of the tanh term in the NN cost function."""
+
+ tol: float
+ """Tolerances for terminating training iterations."""
+
+ alpha_1: float
+ """L¹ regularization weights in the NN cost function"""
+
+ alpha_2: float
+ """L² regularization weights in the NN cost function."""
+
+ alpha_1_for_polynomial_regression: float
+ """L¹ regularization weights for polynomial regression"""
+
+ alpha_2_for_polynomial_regression: float
+ """L² regularization weights for polynomial regression"""
+
+ x_loss: list[float]
+ """List of values for the loss function of the x component in the NN training."""
+
+ y_loss: list[float]
+ """List of values for the loss function of the y component in the NN training."""
+
+ v_loss: list[float]
+ """List of values for the loss function of the velocity component in the NN training."""
+
+ theta_loss: list[float]
+ """List of values for the loss function of the yaw angle component in the NN training."""
+
+ acc_loss: list[float]
+ """List of values for the loss function of the acceleration component in the NN training."""
+
+ steer_loss: list[float]
+ """List of values for the loss function of the steer angle component in the NN training."""
+
+ steer_loss_plus_tanh: list[float]
+ """List of values of the loss function for the steer angle component with tanh term in the NN training."""
+
+ total_loss: list[float]
+ """In NN training x_loss, ... , acc_loss, steer_loss_plus_tanh summed list of values"""
+
+ model: drive_NN.DriveNeuralNetwork
+ """trained neural network model"""
+
+ A: np.ndarray
+ """Coefficient matrix of polynomial regression model."""
+
+ b: np.ndarray
+ """Constant terms in polynomial regression models (bias)"""
+
+ def __init__(
+ self,
+ tanh_gain=10,
+ lam=0.1,
+ tol=0.00001,
+ alpha_1=0.1**7,
+ alpha_2=0.1**7,
+ alpha_1_for_polynomial_regression=0.1**5,
+ alpha_2_for_polynomial_regression=0.1**5,
+ ):
+ super(train_drive_NN_model_without_memory, self).__init__(
+ alpha_1_for_polynomial_regression, alpha_2_for_polynomial_regression
+ )
+ self.tanh_gain = tanh_gain
+ self.lam = lam
+ self.tol = tol
+ self.alpha_1 = alpha_1
+ self.alpha_2 = alpha_2
+ self.alpha_1_for_polynomial_regression = alpha_1_for_polynomial_regression
+ self.alpha_2_for_polynomial_regression = alpha_2_for_polynomial_regression
+ self.x_loss = []
+ self.y_loss = []
+ self.v_loss = []
+ self.theta_loss = []
+ self.acc_loss = []
+ self.steer_loss = []
+ self.steer_loss_plus_tanh = []
+ self.total_loss = []
+
+ def train_model(
+ self,
+ model: drive_NN.DriveNeuralNetwork,
+ X_input: np.ndarray,
+ Y_output: np.ndarray,
+ learning_rates: list[float],
+ patience: int = 10,
+ batch_size: int | None = 50,
+ max_iter: int = 100000,
+ X_val_np=None,
+ Y_val_np=None,
+ ) -> None:
+ """Train the model."""
+ if batch_size is None:
+ batch_size = X_input.shape[0] // 50
+ self.x_loss.clear()
+ self.y_loss.clear()
+ self.v_loss.clear()
+ self.theta_loss.clear()
+ self.acc_loss.clear()
+ self.steer_loss.clear()
+ self.steer_loss_plus_tanh.clear()
+ self.total_loss.clear()
+ if X_val_np is None:
+ sample_size = X_input.shape[0]
+ num_train = int(3 * sample_size / 4)
+ id_all = np.random.choice(sample_size, sample_size, replace=False)
+ id_train = id_all[:num_train]
+ id_val = id_all[num_train:]
+ X_tensor = torch.tensor(X_input.astype(np.float32)).clone()
+ Y_tensor = torch.tensor(Y_output.astype(np.float32)).clone()
+ X_train = X_tensor[id_train]
+ Y_train = Y_tensor[id_train]
+ X_val = X_tensor[id_val]
+ Y_val = Y_tensor[id_val]
+ print("sample_size: ", X_input.shape[0])
+ else:
+ X_train = torch.tensor(X_input.astype(np.float32)).clone()
+ Y_train = torch.tensor(Y_output.astype(np.float32)).clone()
+ X_val = torch.tensor(X_val_np.astype(np.float32)).clone()
+ Y_val = torch.tensor(Y_val_np.astype(np.float32)).clone()
+ print("sample_size", X_input.shape[0] + X_val_np.shape[0])
+ print("patience:", patience)
+ loss_fn = torch.nn.L1Loss()
+ learning_rate = learning_rates[0]
+ optimizer = torch.optim.Adam(params=model.parameters(), lr=learning_rate)
+
+ drive_data = DataLoader(
+ TensorDataset(X_train, Y_train), batch_size=batch_size, shuffle=True
+ )
+ initial_loss = drive_NN.loss_fn_plus_tanh(
+ loss_fn,
+ torch.tensor(np.zeros(Y_val.shape), dtype=torch.float32),
+ Y_val,
+ self.tanh_gain,
+ self.lam,
+ )
+ print("initial_loss:", initial_loss.detach().item())
+ early_stopping = drive_NN.EarlyStopping(
+ initial_loss=initial_loss, tol=self.tol, patience=patience
+ )
+ k = 0
+ print("learning_rate:", learning_rates[0])
+ for i in range(max_iter):
+ model.train()
+ for X_batch, y_batch in drive_data:
+ y_pred = model(X_batch)
+ loss = drive_NN.loss_fn_plus_tanh(
+ loss_fn, y_pred, y_batch, self.tanh_gain, self.lam
+ )
+ for w in model.parameters():
+ loss = (
+ loss + self.alpha_1 * torch.norm(w, p=1) + self.alpha_2 * torch.norm(w) ** 2
+ )
+ if w[0] in ["finalize.weight", "finalize.bias"]:
+ loss = loss + (drive_functions.finalize_x_weight - 1) * (
+ self.alpha_1 * torch.norm(w[1][0], p=1)
+ + self.alpha_2 * torch.norm(w[1][0]) ** 2
+ )
+ loss = loss + (drive_functions.finalize_y_weight - 1) * (
+ self.alpha_1 * torch.norm(w[1][1], p=1)
+ + self.alpha_2 * torch.norm(w[1][1]) ** 2
+ )
+ loss = loss + (drive_functions.finalize_v_weight - 1) * (
+ self.alpha_1 * torch.norm(w[1][2], p=1)
+ + self.alpha_2 * torch.norm(w[1][2]) ** 2
+ )
+ optimizer.zero_grad()
+ loss.backward()
+ optimizer.step()
+
+ model.eval()
+ pred = model(X_val)
+ self.total_loss.append(
+ drive_NN.loss_fn_plus_tanh(loss_fn, pred, Y_val, self.tanh_gain, self.lam)
+ .detach()
+ .item()
+ )
+ self.x_loss.append(loss_fn(pred[:, [0]], Y_val[:, [0]]).detach().item())
+ self.y_loss.append(loss_fn(pred[:, [1]], Y_val[:, [1]]).detach().item())
+ self.v_loss.append(loss_fn(pred[:, [2]], Y_val[:, [2]]).detach().item())
+ self.theta_loss.append(loss_fn(pred[:, [3]], Y_val[:, [3]]).detach().item())
+ self.acc_loss.append(loss_fn(pred[:, [4]], Y_val[:, [4]]).detach().item())
+ self.steer_loss.append(loss_fn(pred[:, [5]], Y_val[:, [5]]).detach().item())
+ self.steer_loss_plus_tanh.append(
+ loss_fn(pred[:, [5]], Y_val[:, [5]]).detach().item()
+ + self.lam
+ * loss_fn(
+ torch.tanh(self.tanh_gain * (pred[:, -1] - Y_val[:, -1])),
+ torch.zeros(Y_val.shape[0]),
+ )
+ .detach()
+ .item()
+ )
+ current_loss = drive_NN.loss_fn_plus_tanh(
+ loss_fn, model(X_val), Y_val, self.tanh_gain, self.lam
+ )
+ if i % 10 == 1:
+ print(current_loss.detach().item(), i)
+ if early_stopping(current_loss):
+ k += 1
+ if k == len(learning_rates):
+ break
+ else:
+ learning_rate = learning_rates[k]
+ print("update learning_rate to ", learning_rates[k])
+ optimizer = torch.optim.Adam(params=model.parameters(), lr=learning_rate)
+ early_stopping = drive_NN.EarlyStopping(
+ initial_loss=initial_loss, tol=self.tol, patience=patience
+ )
+
+ def plot_trained_result(
+ self, show_flag=False, save_dir=".", plot_range=np.arange(500, 1200)
+ ) -> None:
+ """Plot the results of the training."""
+ polynomial_features = PolynomialFeatures(degree=self.deg, include_bias=False)
+
+ X_input = np.array(self.X_input_list)
+ Y_output = np.array(self.Y_output_list)
+ X_tensor = torch.tensor(X_input.astype(np.float32)).clone()
+ acc_ctrl_queue_size = drive_functions.acc_ctrl_queue_size
+ steer_ctrl_queue_size = drive_functions.steer_ctrl_queue_size
+ ctrl_time_step = drive_functions.ctrl_time_step
+ Y_pred = (
+ self.model(X_tensor).detach().numpy()
+ + polynomial_features.fit_transform(X_input[:, ctrl_index_for_polynomial_reg])
+ @ (self.A).T
+ + self.b
+ )
+ x = max(acc_ctrl_queue_size, steer_ctrl_queue_size) * ctrl_time_step
+ y_labels = [
+ "x_error [m]",
+ "y_error [m]",
+ "vel_error [m/s]",
+ "yaw_error [rad]",
+ "acc_error [m/s^2]",
+ "steer_error [rad]",
+ ]
+
+ plt.figure(figsize=(24, 15), tight_layout=True)
+ plt.subplot(2, 3, 1)
+ ax = []
+ for i in range(6):
+ ax.append(plt.subplot(2, 3, i + 1))
+ ax[-1].plot(
+ drive_functions.ctrl_time_step * plot_range + x,
+ Y_output[plot_range, i] * drive_functions.mpc_time_step,
+ label="nominal_error",
+ )
+ ax[-1].plot(
+ drive_functions.ctrl_time_step * plot_range + x,
+ Y_pred[plot_range, i] * drive_functions.mpc_time_step,
+ label="pred_error",
+ )
+ ax[-1].set_xlabel("sec")
+ ax[-1].set_ylabel(y_labels[i])
+ ax[-1].legend()
+ if show_flag:
+ plt.show()
+ else:
+ plt.savefig(save_dir + "/train_drive_NN_model_fig.png")
+ plt.close()
+
+ def plot_loss(self, show_flag=False, save_dir=".") -> None:
+ """Plot the progression of values of the loss function of the training."""
+ plt.figure(figsize=(24, 15), tight_layout=True)
+
+ y_loss_labels = [
+ "total_loss",
+ "x_loss",
+ "y_loss",
+ "vel_loss",
+ "yaw_loss",
+ "acc_loss",
+ "steer_loss",
+ "steer_plus_tanh_loss",
+ ]
+ plt.subplot(2, 4, 1)
+ ax_2 = []
+ loss_list = [
+ np.array(self.total_loss),
+ np.array(self.x_loss),
+ np.array(self.y_loss),
+ np.array(self.v_loss),
+ np.array(self.theta_loss),
+ np.array(self.acc_loss),
+ np.array(self.steer_loss),
+ np.array(self.steer_loss_plus_tanh),
+ ]
+ for i in range(8):
+ ax_2.append(plt.subplot(2, 4, i + 1))
+ ax_2[-1].plot(
+ np.arange(loss_list[i].shape[0]),
+ loss_list[i],
+ label="loss",
+ )
+ ax_2[-1].set_xlabel("iteration")
+ ax_2[-1].set_ylabel(y_loss_labels[i])
+ ax_2[-1].legend()
+ if show_flag:
+ plt.show()
+ else:
+ plt.savefig(save_dir + "/loss.png")
+ plt.close()
+
+ def get_trained_model(
+ self,
+ hidden_layer_sizes=(32, 16),
+ hidden_layer_lstm=None,
+ learning_rates: list[float] = [1e-3, 1e-4, 1e-5, 1e-6],
+ randomize=0.01,
+ acc_drop_out=0.0,
+ steer_drop_out=0.0,
+ patience: int = 10,
+ batch_size: int | None = 50,
+ max_iter=100000,
+ use_polynomial_reg=False,
+ use_selected_polynomial=True,
+ force_NN_model_to_zero=False,
+ fit_intercept=True,
+ use_intercept=None,
+ deg: int = 2,
+ ):
+ """Train on a model for which initial values are randomly given in a suitable range."""
+ if use_intercept is None:
+ if force_NN_model_to_zero:
+ use_intercept = True
+ else:
+ use_intercept = False
+ X_input, Y_output_minus = self.get_polynomial_regression_result(
+ self.X_input_list,
+ self.Y_output_list,
+ use_polynomial_reg,
+ use_selected_polynomial,
+ deg,
+ fit_intercept,
+ use_intercept,
+ )
+ if len(self.X_val_list) > 0:
+ X_val = np.array(self.X_val_list)
+ Y_val = np.array(self.Y_val_list)
+ polynomial_features = PolynomialFeatures(degree=self.deg, include_bias=False)
+ Y_val = (
+ Y_val
+ - polynomial_features.fit_transform(X_val[:, ctrl_index_for_polynomial_reg])
+ @ (self.A).T
+ - self.b
+ )
+ else:
+ X_val = None
+ Y_val = None
+ self.model = drive_NN.DriveNeuralNetwork(
+ hidden_layer_sizes=hidden_layer_sizes,
+ randomize=randomize,
+ acc_drop_out=acc_drop_out,
+ steer_drop_out=steer_drop_out,
+ acc_queue_size=drive_functions.acc_ctrl_queue_size,
+ steer_queue_size=drive_functions.steer_ctrl_queue_size,
+ )
+ if force_NN_model_to_zero:
+ for w in self.model.parameters():
+ w.data = nn.Parameter(torch.zeros(w.shape))
+ else:
+ self.train_model(
+ self.model,
+ X_input,
+ Y_output_minus,
+ learning_rates,
+ patience,
+ batch_size,
+ max_iter,
+ X_val,
+ Y_val,
+ )
+
+ def update_trained_model(
+ self,
+ learning_rates=[1e-4, 1e-5, 1e-6],
+ patience=10,
+ batch_size=50,
+ max_iter=100000,
+ use_polynomial_reg=False,
+ use_selected_polynomial=True,
+ force_NN_model_to_zero=False,
+ fit_intercept=True,
+ use_intercept=None,
+ deg: int = 2,
+ ):
+ """Update `self.model` with additional learning."""
+ if use_intercept is None:
+ if force_NN_model_to_zero:
+ use_intercept = True
+ else:
+ use_intercept = False
+ X_input, Y_output_minus = self.get_polynomial_regression_result(
+ self.X_input_list,
+ self.Y_output_list,
+ use_polynomial_reg,
+ use_selected_polynomial,
+ deg,
+ fit_intercept,
+ use_intercept,
+ )
+ if len(self.X_val_list) > 0:
+ X_val = np.array(self.X_val_list)
+ Y_val = np.array(self.Y_val_list)
+ polynomial_features = PolynomialFeatures(degree=self.deg, include_bias=False)
+ Y_val = (
+ Y_val
+ - polynomial_features.fit_transform(X_val[:, ctrl_index_for_polynomial_reg])
+ @ (self.A).T
+ - self.b
+ )
+ else:
+ X_val = None
+ Y_val = None
+ if force_NN_model_to_zero:
+ for w in self.model.parameters():
+ w.data = nn.Parameter(torch.zeros(w.shape))
+ else:
+ self.train_model(
+ self.model,
+ X_input,
+ Y_output_minus,
+ learning_rates,
+ patience,
+ batch_size,
+ max_iter,
+ X_val,
+ Y_val,
+ )
+
+ def update_saved_trained_model(
+ self,
+ path="model_for_test_drive.pth",
+ learning_rates: list[float] = [1e-4, 1e-5, 1e-6],
+ patience=10,
+ batch_size: int | None = 50,
+ max_iter=100000,
+ use_polynomial_reg=False,
+ use_selected_polynomial=True,
+ force_NN_model_to_zero=False,
+ fit_intercept=True,
+ use_intercept=None,
+ deg: int = 2,
+ ) -> None:
+ """Load the saved model and update the model with additional training."""
+ if use_intercept is None:
+ if force_NN_model_to_zero:
+ use_intercept = True
+ else:
+ use_intercept = False
+ X_input, Y_output_minus = self.get_polynomial_regression_result(
+ self.X_input_list,
+ self.Y_output_list,
+ use_polynomial_reg,
+ use_selected_polynomial,
+ deg,
+ fit_intercept,
+ use_intercept,
+ )
+ if len(self.X_val_list) > 0:
+ X_val = np.array(self.X_val_list)
+ Y_val = np.array(self.Y_val_list)
+ polynomial_features = PolynomialFeatures(degree=self.deg, include_bias=False)
+ Y_val = (
+ Y_val
+ - polynomial_features.fit_transform(X_val[:, ctrl_index_for_polynomial_reg])
+ @ (self.A).T
+ - self.b
+ )
+ else:
+ X_val = None
+ Y_val = None
+ self.model = torch.load(path)
+ if force_NN_model_to_zero:
+ for w in self.model.parameters():
+ w.data = nn.Parameter(torch.zeros(w.shape))
+ else:
+ self.train_model(
+ self.model,
+ X_input,
+ Y_output_minus,
+ learning_rates,
+ patience,
+ batch_size,
+ max_iter,
+ X_val,
+ Y_val,
+ )
+
+ def save_model(self, save_dir=".", path="model_for_test_drive.pth") -> None:
+ """Save trained NN models."""
+ torch.save(self.model, save_dir + "/" + path)
+
+ def save_polynomial_reg_info(self, save_dir=".", path="polynomial_reg_info") -> None:
+ """Save the coefficients and degree of the resulting polynomial regression."""
+ np.savez(save_dir + "/" + path, A=self.A, b=self.b, deg=self.deg)
+
+ def save_models(
+ self,
+ save_dir=".",
+ model_name="model_for_test_drive.pth",
+ polynomial_reg_info_name="polynomial_reg_info",
+ ) -> None:
+ """Run save_model and save_polynomial_reg_info."""
+ self.save_model(save_dir, model_name)
+ self.save_polynomial_reg_info(save_dir, polynomial_reg_info_name)
+
+ def predict_error(
+ self, v: float, alpha_0: float, delta_0: float, alpha: float, delta: float
+ ) -> np.ndarray:
+ """Predicts the prediction error when following a nominal model."""
+ polynomial_features = PolynomialFeatures(degree=self.deg, include_bias=False)
+ x_input = np.zeros(
+ 3 + drive_functions.acc_ctrl_queue_size + drive_functions.steer_ctrl_queue_size
+ )
+ x_input[0] = v
+ x_input[1] = alpha_0
+ x_input[2] = delta_0
+ x_input[3 : 3 + drive_functions.acc_ctrl_queue_size] += alpha
+ x_input[3 + drive_functions.acc_ctrl_queue_size :] += delta
+ x_input_tensor = torch.tensor(x_input.reshape(1, -1).astype(np.float32))
+ return (
+ self.model(x_input_tensor)[0].detach().numpy()
+ + self.A
+ @ polynomial_features.fit_transform(
+ x_input[ctrl_index_for_polynomial_reg].reshape(1, -1)
+ )[0]
+ + self.b
+ )
+
+ def sim(
+ self, v: float, alpha_0: float, alpha: float, delta_0: float, delta: float, iter_times: int
+ ) -> tuple[float, float, float]:
+ """Simulate velocity, acceleration and steer changes according to a trained model."""
+ v_pred = v
+ alpha_pred = alpha_0
+ delta_pred = delta_0
+ for i in range(iter_times):
+ pred = (
+ np.concatenate(
+ (
+ nominal_model_acc(v_pred, alpha_pred, alpha),
+ np.array([nominal_model_steer(delta_pred, delta)]),
+ )
+ )
+ + self.predict_error(v_pred, alpha_pred, delta_pred, alpha, delta)[[2, 4, 5]]
+ * drive_functions.mpc_time_step
+ )
+ v_pred = pred[0]
+ alpha_pred = pred[1]
+ steer_pred = pred[2]
+ return v_pred, alpha_pred, steer_pred
+
+ def plot_acc_map(self, iter_times: int, starting_steer=0.0, target_steer=0.0) -> None:
+ """Run `sim` `iter_times` times. The simulation time is approximately 0.1*iter_times.
+
+ Draw the resulting acceleration map.
+ """
+ acc = np.arange(-1, 1, 0.05)
+ v_tests = np.arange(0, 12, 0.3)
+ acc_result = np.zeros((acc.shape[0], v_tests.shape[0]))
+ v_result = np.zeros((acc.shape[0], v_tests.shape[0]))
+ steer_result = np.zeros((acc.shape[0], v_tests.shape[0]))
+ for i in range(acc.shape[0]):
+ v_tests_tmp = v_tests - 0.1 * iter_times * acc[i]
+ for j in range(v_tests.shape[0]):
+ v_result[i, j], acc_result[i, j], steer_result[i, j] = self.sim(
+ v_tests_tmp[j], acc[i], acc[i], starting_steer, target_steer, iter_times
+ )
+ fig = plt.figure()
+ ax = fig.add_subplot(111, projection="3d")
+ ax.plot_surface(np.tile(acc, (v_tests.shape[0], 1)).T, v_result, acc_result)
+ ax.set_xlabel("acc_input [m/s^2]")
+ ax.set_ylabel("vel [m/s]")
+ ax.set_zlabel("acc_sim [m/s^2]")
+ fig.suptitle(str(iter_times * drive_functions.mpc_time_step) + " sec simulation")
+ plt.show()
+
+ def plot_steer_map(self, iter_times: int, starting_acc=0.0, target_acc=0.0) -> None:
+ """Run `sim` `iter_times` times. The simulation time is approximately 0.1*iter_times.
+
+ Draw the resulting steer map.
+ """
+ steer = np.arange(-1, 1, 0.05)
+ v_tests = np.arange(0, 12, 0.3)
+ acc_result = np.zeros((steer.shape[0], v_tests.shape[0]))
+ v_result = np.zeros((steer.shape[0], v_tests.shape[0]))
+ steer_result = np.zeros((steer.shape[0], v_tests.shape[0]))
+ for i in range(steer.shape[0]):
+ for j in range(v_tests.shape[0]):
+ v_result[i, j], acc_result[i, j], steer_result[i, j] = self.sim(
+ v_tests[j], starting_acc, target_acc, steer[i], steer[i], iter_times
+ )
+ fig = plt.figure()
+ ax = fig.add_subplot(111, projection="3d")
+ ax.plot_surface(np.tile(steer, (v_tests.shape[0], 1)).T, v_result, steer_result)
+ ax.set_xlabel("steer_input [rad]")
+ ax.set_ylabel("vel [m/s]")
+ ax.set_zlabel("steer_sim [rad]")
+ fig.suptitle(str(iter_times * drive_functions.mpc_time_step) + " sec simulation")
+ plt.show()
diff --git a/control/autoware_smart_mpc_trajectory_follower/images/lateral_error_nominal_model.png b/control/autoware_smart_mpc_trajectory_follower/images/lateral_error_nominal_model.png
index 3a96faa8dcba1..0d2321c2c079e 100644
Binary files a/control/autoware_smart_mpc_trajectory_follower/images/lateral_error_nominal_model.png and b/control/autoware_smart_mpc_trajectory_follower/images/lateral_error_nominal_model.png differ
diff --git a/control/autoware_smart_mpc_trajectory_follower/images/lateral_error_trained_model.png b/control/autoware_smart_mpc_trajectory_follower/images/lateral_error_trained_model.png
index 3396b9e7dd444..e826f7f137ae9 100644
Binary files a/control/autoware_smart_mpc_trajectory_follower/images/lateral_error_trained_model.png and b/control/autoware_smart_mpc_trajectory_follower/images/lateral_error_trained_model.png differ
diff --git a/control/autoware_smart_mpc_trajectory_follower/images/proxima_test_result_with_lstm.png b/control/autoware_smart_mpc_trajectory_follower/images/proxima_test_result_with_lstm.png
new file mode 100644
index 0000000000000..43cff7ea80384
Binary files /dev/null and b/control/autoware_smart_mpc_trajectory_follower/images/proxima_test_result_with_lstm.png differ
diff --git a/control/autoware_smart_mpc_trajectory_follower/images/python_sim_lateral_error_nominal_model.png b/control/autoware_smart_mpc_trajectory_follower/images/python_sim_lateral_error_nominal_model.png
deleted file mode 100644
index 84e64dde20e17..0000000000000
Binary files a/control/autoware_smart_mpc_trajectory_follower/images/python_sim_lateral_error_nominal_model.png and /dev/null differ
diff --git a/control/autoware_smart_mpc_trajectory_follower/images/python_sim_lateral_error_nominal_model_steer_time_delay.png b/control/autoware_smart_mpc_trajectory_follower/images/python_sim_lateral_error_nominal_model_steer_time_delay.png
new file mode 100644
index 0000000000000..dc64ca3d7fa0c
Binary files /dev/null and b/control/autoware_smart_mpc_trajectory_follower/images/python_sim_lateral_error_nominal_model_steer_time_delay.png differ
diff --git a/control/autoware_smart_mpc_trajectory_follower/images/python_sim_lateral_error_nominal_model_wheel_base.png b/control/autoware_smart_mpc_trajectory_follower/images/python_sim_lateral_error_nominal_model_wheel_base.png
new file mode 100644
index 0000000000000..0c6d882b9a687
Binary files /dev/null and b/control/autoware_smart_mpc_trajectory_follower/images/python_sim_lateral_error_nominal_model_wheel_base.png differ
diff --git a/control/autoware_smart_mpc_trajectory_follower/images/python_sim_lateral_error_trained_model.png b/control/autoware_smart_mpc_trajectory_follower/images/python_sim_lateral_error_trained_model.png
deleted file mode 100644
index 481375fcf68b1..0000000000000
Binary files a/control/autoware_smart_mpc_trajectory_follower/images/python_sim_lateral_error_trained_model.png and /dev/null differ
diff --git a/control/autoware_smart_mpc_trajectory_follower/images/python_sim_lateral_error_trained_model_lstm_steer_time_delay.png b/control/autoware_smart_mpc_trajectory_follower/images/python_sim_lateral_error_trained_model_lstm_steer_time_delay.png
new file mode 100644
index 0000000000000..b01dce7fa3d2b
Binary files /dev/null and b/control/autoware_smart_mpc_trajectory_follower/images/python_sim_lateral_error_trained_model_lstm_steer_time_delay.png differ
diff --git a/control/autoware_smart_mpc_trajectory_follower/images/python_sim_lateral_error_trained_model_lstm_wheel_base.png b/control/autoware_smart_mpc_trajectory_follower/images/python_sim_lateral_error_trained_model_lstm_wheel_base.png
new file mode 100644
index 0000000000000..974b01302e641
Binary files /dev/null and b/control/autoware_smart_mpc_trajectory_follower/images/python_sim_lateral_error_trained_model_lstm_wheel_base.png differ
diff --git a/control/autoware_smart_mpc_trajectory_follower/images/python_sim_lateral_error_trained_model_steer_time_delay.png b/control/autoware_smart_mpc_trajectory_follower/images/python_sim_lateral_error_trained_model_steer_time_delay.png
new file mode 100644
index 0000000000000..7638d6ec86595
Binary files /dev/null and b/control/autoware_smart_mpc_trajectory_follower/images/python_sim_lateral_error_trained_model_steer_time_delay.png differ
diff --git a/control/autoware_smart_mpc_trajectory_follower/images/python_sim_lateral_error_trained_model_wheel_base.png b/control/autoware_smart_mpc_trajectory_follower/images/python_sim_lateral_error_trained_model_wheel_base.png
new file mode 100644
index 0000000000000..d0bed59bd0ab5
Binary files /dev/null and b/control/autoware_smart_mpc_trajectory_follower/images/python_sim_lateral_error_trained_model_wheel_base.png differ
diff --git a/control/autoware_smart_mpc_trajectory_follower/package.xml b/control/autoware_smart_mpc_trajectory_follower/package.xml
index 01b9478aeeaee..46ce55058a000 100644
--- a/control/autoware_smart_mpc_trajectory_follower/package.xml
+++ b/control/autoware_smart_mpc_trajectory_follower/package.xml
@@ -2,7 +2,7 @@
autoware_smart_mpc_trajectory_follower
- 1.0.0
+ 2.0.0
Nodes to follow a trajectory by generating control commands using smart mpc
diff --git a/control/autoware_smart_mpc_trajectory_follower/sample_models/wheel_base_changed/model_for_test_drive.pth b/control/autoware_smart_mpc_trajectory_follower/sample_models/wheel_base_changed/model_for_test_drive.pth
new file mode 100644
index 0000000000000..1aec4fe8c71ff
Binary files /dev/null and b/control/autoware_smart_mpc_trajectory_follower/sample_models/wheel_base_changed/model_for_test_drive.pth differ
diff --git a/control/autoware_smart_mpc_trajectory_follower/sample_models/wheel_base_changed/polynomial_reg_info.npz b/control/autoware_smart_mpc_trajectory_follower/sample_models/wheel_base_changed/polynomial_reg_info.npz
new file mode 100644
index 0000000000000..be3e01443b805
Binary files /dev/null and b/control/autoware_smart_mpc_trajectory_follower/sample_models/wheel_base_changed/polynomial_reg_info.npz differ
diff --git a/control/autoware_smart_mpc_trajectory_follower/setup.py b/control/autoware_smart_mpc_trajectory_follower/setup.py
index 7df489ff29377..22844b0f2d869 100644
--- a/control/autoware_smart_mpc_trajectory_follower/setup.py
+++ b/control/autoware_smart_mpc_trajectory_follower/setup.py
@@ -1,25 +1,30 @@
# cspell: ignore numba
import glob
import json
-import os
from pathlib import Path
+import subprocess
from setuptools import find_packages
from setuptools import setup
-os.system("pip3 install numba==0.58.1 --force-reinstall")
-os.system("pip3 install pybind11")
-os.system("pip3 install GPy")
-os.system("pip3 install torch")
-package_path = {}
-package_path["path"] = str(Path(__file__).parent)
-with open("autoware_smart_mpc_trajectory_follower/package_path.json", "w") as f:
- json.dump(package_path, f)
-build_cpp_command = "g++ -Ofast -Wall -shared -std=c++11 -fPIC $(python3 -m pybind11 --includes) "
-build_cpp_command += "autoware_smart_mpc_trajectory_follower/scripts/proxima_calc.cpp "
-build_cpp_command += "-o autoware_smart_mpc_trajectory_follower/scripts/proxima_calc$(python3-config --extension-suffix) "
-build_cpp_command += "-lrt -I/usr/include/eigen3"
-os.system(build_cpp_command)
+SKIP_PRE_INSTALL_FLAG = False
+
+if not SKIP_PRE_INSTALL_FLAG:
+ subprocess.run("pip3 install numba==0.58.1 --force-reinstall", shell=True)
+ subprocess.run("pip3 install pybind11", shell=True)
+ subprocess.run("pip3 install GPy", shell=True)
+ subprocess.run("pip3 install torch", shell=True)
+ package_path = {}
+ package_path["path"] = str(Path(__file__).parent)
+ with open("autoware_smart_mpc_trajectory_follower/package_path.json", "w") as f:
+ json.dump(package_path, f)
+ build_cpp_command = (
+ "g++ -Ofast -Wall -shared -std=c++11 -fPIC $(python3 -m pybind11 --includes) "
+ )
+ build_cpp_command += "autoware_smart_mpc_trajectory_follower/scripts/proxima_calc.cpp "
+ build_cpp_command += "-o autoware_smart_mpc_trajectory_follower/scripts/proxima_calc$(python3-config --extension-suffix) "
+ build_cpp_command += "-lrt -I/usr/include/eigen3"
+ subprocess.run(build_cpp_command, shell=True)
so_path = (
"scripts/"
@@ -29,7 +34,7 @@
)
setup(
name="autoware_smart_mpc_trajectory_follower",
- version="1.0.0",
+ version="2.0.0",
packages=find_packages(),
package_data={
"autoware_smart_mpc_trajectory_follower": ["package_path.json", so_path],