Skip to content

Commit

Permalink
update vehicle adaptor
Browse files Browse the repository at this point in the history
  • Loading branch information
@masayukiaino
masayukiaino committed Nov 28, 2024
1 parent 7e57643 commit 429c86b
Show file tree
Hide file tree
Showing 9 changed files with 103 additions and 65 deletions.
3 changes: 2 additions & 1 deletion ...e_adaptor/autoware_vehicle_adaptor/autoware_vehicle_adaptor/include/inputs_ref_smoother.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,14 +13,15 @@ class InputsRefSmoother {
Eigen::VectorXd terminal_integration_vector_;
Eigen::MatrixXd A_, A_inv_;
double lambda_smooth_, lambda_decay_, lambda_terminal_decay_;
double terminal_lambda_smooth_;
double control_dt_;
bool initialized_=false;
bool params_set_=false;
bool use_smoother_=true;
public:
InputsRefSmoother();
virtual ~InputsRefSmoother();
void set_params(const double control_dt, const double lambda_smooth, const double lambda_decay, const double lambda_terminal_decay);
void set_params(const double control_dt, const double lambda_smooth, const double terminal_lambda_smooth, const double lambda_decay, const double lambda_terminal_decay);
void set_prediction_len(const int prediction_len);
void set_use_smoother(const bool use_smoother);
void calc_matrices();
Expand Down
32 changes: 17 additions & 15 deletions ...ter/src/vehicle_adaptor/autoware_vehicle_adaptor/autoware_vehicle_adaptor/include/utils.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -568,7 +568,7 @@ class VehicleAdaptor
PolynomialFilter polynomial_filter_for_acc_inputs_, polynomial_filter_for_steer_inputs_;
InputsSchedulePrediction acc_input_schedule_prediction_, steer_input_schedule_prediction_;
InputsRefSmoother acc_input_ref_smoother_, steer_input_ref_smoother_;
GetInitialHidden get_initial_hidden_;
// GetInitialHidden get_initial_hidden_;
bool use_nonzero_initial_hidden_ = false;
double prob_update_memory_bank_;
int memory_bank_size_;
Expand Down Expand Up @@ -701,11 +701,13 @@ class VehicleAdaptor
bool acc_input_schedule_prediction_initialized_ = false;
bool steer_input_schedule_prediction_initialized_ = false;

int h_gru_dim_for_initial_hidden_;
//int h_gru_dim_for_initial_hidden_;
Eigen::VectorXd offline_features_;
bool use_offline_features_ = false;
public:
bool use_controller_steer_input_schedule_ = false;
bool use_vehicle_adaptor_;
bool use_nonzero_initial_hidden_autoware_;
//bool use_nonzero_initial_hidden_autoware_;
bool use_offline_features_autoware_;
VehicleAdaptor();
virtual ~VehicleAdaptor();
Expand All @@ -728,20 +730,20 @@ class VehicleAdaptor
const Eigen::VectorXd & bias_complimentary_layer,
const Eigen::VectorXd & bias_linear_relu, const Eigen::VectorXd & bias_final_layer,
const double vel_scaling, const double vel_bias, const std::vector<std::string> state_component_predicted);
void set_initial_hidden_params(
const Eigen::MatrixXd & weight_initial_hidden_acc_layer_1, const Eigen::MatrixXd & weight_initial_hidden_steer_layer_1,
const Eigen::MatrixXd & weight_initial_hidden_acc_layer_2, const Eigen::MatrixXd & weight_initial_hidden_steer_layer_2,
const Eigen::MatrixXd & weight_initial_hidden_gru_ih, const Eigen::MatrixXd & weight_initial_hidden_gru_hh,
const Eigen::MatrixXd & weight_initial_hidden_final_layer,
const Eigen::MatrixXd & weight_initial_hidden_fusion_layer, const Eigen::MatrixXd & weight_initial_hidden_only_online,
const Eigen::VectorXd & bias_initial_hidden_acc_layer_1, const Eigen::VectorXd & bias_initial_hidden_steer_layer_1,
const Eigen::VectorXd & bias_initial_hidden_acc_layer_2, const Eigen::VectorXd & bias_initial_hidden_steer_layer_2,
const Eigen::VectorXd & bias_initial_hidden_gru_ih, const Eigen::VectorXd & bias_initial_hidden_gru_hh,
const Eigen::VectorXd & bias_initial_hidden_final_layer,
const Eigen::VectorXd & bias_initial_hidden_fusion_layer, const Eigen::VectorXd & bias_initial_hidden_only_online);
//void set_initial_hidden_params(
// const Eigen::MatrixXd & weight_initial_hidden_acc_layer_1, const Eigen::MatrixXd & weight_initial_hidden_steer_layer_1,
// const Eigen::MatrixXd & weight_initial_hidden_acc_layer_2, const Eigen::MatrixXd & weight_initial_hidden_steer_layer_2,
// const Eigen::MatrixXd & weight_initial_hidden_gru_ih, const Eigen::MatrixXd & weight_initial_hidden_gru_hh,
// const Eigen::MatrixXd & weight_initial_hidden_final_layer,
// const Eigen::MatrixXd & weight_initial_hidden_fusion_layer, const Eigen::MatrixXd & weight_initial_hidden_only_online,
// const Eigen::VectorXd & bias_initial_hidden_acc_layer_1, const Eigen::VectorXd & bias_initial_hidden_steer_layer_1,
// const Eigen::VectorXd & bias_initial_hidden_acc_layer_2, const Eigen::VectorXd & bias_initial_hidden_steer_layer_2,
// const Eigen::VectorXd & bias_initial_hidden_gru_ih, const Eigen::VectorXd & bias_initial_hidden_gru_hh,
// const Eigen::VectorXd & bias_initial_hidden_final_layer,
// const Eigen::VectorXd & bias_initial_hidden_fusion_layer, const Eigen::VectorXd & bias_initial_hidden_only_online);
void set_offline_features(const Eigen::VectorXd & offline_features);
void set_NN_params_from_csv(std::string csv_dir);
void set_initial_hidden_params_from_csv(std::string csv_dir);
// void set_initial_hidden_params_from_csv(std::string csv_dir);
void set_offline_features_from_csv(std::string csv_dir);
void clear_NN_params();
void set_offline_data_set_for_compensation(Eigen::MatrixXd XXT, Eigen::MatrixXd YYT);
Expand Down
3 changes: 2 additions & 1 deletion ...e_adaptor/autoware_vehicle_adaptor/autoware_vehicle_adaptor/param/optimization_param.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -122,15 +122,16 @@ optimization_parameter:
use_controller_inputs_as_target: false
inputs_ref_smoother:
lambda_smooth_acc_ref_smoother: 1e-1
terminal_lambda_smooth_acc_ref_smoother: 1e+0
lambda_decay_acc_ref_smoother: 1e-8
lambda_terminal_decay_acc_ref_smoother: 1e-3
use_acc_ref_smoother: true
lambda_smooth_steer_ref_smoother: 1e-1
terminal_lambda_smooth_steer_ref_smoother: 1e+0
lambda_decay_steer_ref_smoother: 1e-8
lambda_terminal_decay_steer_ref_smoother: 1e-3
use_steer_ref_smoother: false
autoware_alignment:
use_controller_steer_input_schedule: true
use_vehicle_adaptor: true
use_nonzero_initial_hidden: true
use_offline_features: true
9 changes: 7 additions & 2 deletions ...cle_adaptor/autoware_vehicle_adaptor/autoware_vehicle_adaptor/src/inputs_ref_smoother.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,9 +2,10 @@

InputsRefSmoother::InputsRefSmoother(){}
InputsRefSmoother::~InputsRefSmoother(){}
void InputsRefSmoother::set_params(const double control_dt, const double lambda_smooth, const double lambda_decay, const double lambda_terminal_decay){
void InputsRefSmoother::set_params(const double control_dt, const double lambda_smooth, const double terminal_lambda_smooth, const double lambda_decay, const double lambda_terminal_decay){
control_dt_ = control_dt;
lambda_smooth_ = lambda_smooth;
terminal_lambda_smooth_ = terminal_lambda_smooth;
lambda_decay_ = lambda_decay;
lambda_terminal_decay_ = lambda_terminal_decay;
initialized_ = false;
Expand All @@ -27,9 +28,13 @@ void InputsRefSmoother::calc_matrices(){
integration_matrix_(i, j) = control_dt_;
}
}
Eigen::VectorXd lambda_smooth_vector = Eigen::VectorXd::LinSpaced(prediction_len_, lambda_smooth_, terminal_lambda_smooth_);
Eigen::MatrixXd lambda_smooth_matrix = lambda_smooth_vector.asDiagonal();

terminal_integration_vector_ = integration_matrix_.row(prediction_len_ - 1).transpose();
A_ = (1 + lambda_decay_) * integration_matrix_.transpose() * integration_matrix_
+ lambda_smooth_ * Eigen::MatrixXd::Identity(prediction_len_, prediction_len_)
+ lambda_smooth_matrix
//+ lambda_smooth_ * Eigen::MatrixXd::Identity(prediction_len_, prediction_len_)
+ lambda_terminal_decay_ * terminal_integration_vector_ * terminal_integration_vector_.transpose();
A_inv_ = A_.llt().solve(Eigen::MatrixXd::Identity(prediction_len_, prediction_len_));
initialized_ = true;
Expand Down
26 changes: 19 additions & 7 deletions ...erter/src/vehicle_adaptor/autoware_vehicle_adaptor/autoware_vehicle_adaptor/src/utils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1753,7 +1753,7 @@ double PolynomialFilter::fit_transform(double timestamp, double sample)
}
use_controller_steer_input_schedule_ = optimization_param_node["optimization_parameter"]["autoware_alignment"]["use_controller_steer_input_schedule"].as<bool>();
use_vehicle_adaptor_ = optimization_param_node["optimization_parameter"]["autoware_alignment"]["use_vehicle_adaptor"].as<bool>();
use_nonzero_initial_hidden_autoware_ = optimization_param_node["optimization_parameter"]["autoware_alignment"]["use_nonzero_initial_hidden"].as<bool>();
//use_nonzero_initial_hidden_autoware_ = optimization_param_node["optimization_parameter"]["autoware_alignment"]["use_nonzero_initial_hidden"].as<bool>();
use_offline_features_autoware_ = optimization_param_node["optimization_parameter"]["autoware_alignment"]["use_offline_features"].as<bool>();

use_acc_input_schedule_prediction_ = optimization_param_node["optimization_parameter"]["inputs_schedule_prediction_NN"]["use_acc_input_schedule_prediction"].as<bool>();
Expand All @@ -1775,18 +1775,22 @@ double PolynomialFilter::fit_transform(double timestamp, double sample)
}

double lambda_smooth_steer_ref_smoother = optimization_param_node["optimization_parameter"]["inputs_ref_smoother"]["lambda_smooth_steer_ref_smoother"].as<double>();
double terminal_lambda_smooth_steer_ref_smoother = optimization_param_node["optimization_parameter"]["inputs_ref_smoother"]["terminal_lambda_smooth_steer_ref_smoother"].as<double>();

double lambda_decay_steer_ref_smoother = optimization_param_node["optimization_parameter"]["inputs_ref_smoother"]["lambda_decay_steer_ref_smoother"].as<double>();
double lambda_terminal_decay_steer_ref_smoother = optimization_param_node["optimization_parameter"]["inputs_ref_smoother"]["lambda_terminal_decay_steer_ref_smoother"].as<double>();
bool use_steer_ref_smoother = optimization_param_node["optimization_parameter"]["inputs_ref_smoother"]["use_steer_ref_smoother"].as<bool>();
steer_input_ref_smoother_.set_params(control_dt_, lambda_smooth_steer_ref_smoother, lambda_decay_steer_ref_smoother, lambda_terminal_decay_steer_ref_smoother);
steer_input_ref_smoother_.set_params(control_dt_, lambda_smooth_steer_ref_smoother,terminal_lambda_smooth_steer_ref_smoother, lambda_decay_steer_ref_smoother, lambda_terminal_decay_steer_ref_smoother);
steer_input_ref_smoother_.set_use_smoother(use_steer_ref_smoother);

double lambda_smooth_acc_ref_smoother = optimization_param_node["optimization_parameter"]["inputs_ref_smoother"]["lambda_smooth_acc_ref_smoother"].as<double>();
double terminal_lambda_smooth_acc_ref_smoother = optimization_param_node["optimization_parameter"]["inputs_ref_smoother"]["terminal_lambda_smooth_acc_ref_smoother"].as<double>();

double lambda_decay_acc_ref_smoother = optimization_param_node["optimization_parameter"]["inputs_ref_smoother"]["lambda_decay_acc_ref_smoother"].as<double>();
double lambda_terminal_decay_acc_ref_smoother = optimization_param_node["optimization_parameter"]["inputs_ref_smoother"]["lambda_terminal_decay_acc_ref_smoother"].as<double>();
bool use_acc_ref_smoother = optimization_param_node["optimization_parameter"]["inputs_ref_smoother"]["use_acc_ref_smoother"].as<bool>();

acc_input_ref_smoother_.set_params(control_dt_, lambda_smooth_acc_ref_smoother, lambda_decay_acc_ref_smoother, lambda_terminal_decay_acc_ref_smoother);
acc_input_ref_smoother_.set_params(control_dt_, lambda_smooth_acc_ref_smoother, terminal_lambda_smooth_acc_ref_smoother, lambda_decay_acc_ref_smoother, lambda_terminal_decay_acc_ref_smoother);
acc_input_ref_smoother_.set_use_smoother(use_acc_ref_smoother);
if (use_acc_input_schedule_prediction_){
acc_input_ref_smoother_.set_prediction_len(acc_input_schedule_prediction_len_);
Expand Down Expand Up @@ -1842,6 +1846,7 @@ double PolynomialFilter::fit_transform(double timestamp, double sample)
num_layers_encoder_ = weight_lstm_encoder_ih.size();
NN_prediction_target_dim_ = state_component_predicted.size();
}
/*
void VehicleAdaptor::set_initial_hidden_params(
const Eigen::MatrixXd & weight_initial_hidden_acc_layer_1, const Eigen::MatrixXd & weight_initial_hidden_steer_layer_1,
const Eigen::MatrixXd & weight_initial_hidden_acc_layer_2, const Eigen::MatrixXd & weight_initial_hidden_steer_layer_2,
Expand Down Expand Up @@ -1897,8 +1902,11 @@ double PolynomialFilter::fit_transform(double timestamp, double sample)
bias_initial_hidden_gru_ih, bias_initial_hidden_gru_hh,
bias_initial_hidden_final_layer, bias_initial_hidden_fusion_layer, bias_initial_hidden_only_online);
}
*/
void VehicleAdaptor::set_offline_features(const Eigen::VectorXd & offline_features){
get_initial_hidden_.set_offline_features(offline_features);
//get_initial_hidden_.set_offline_features(offline_features);
offline_features_ = offline_features;
use_offline_features_ = true;
}
void VehicleAdaptor::set_offline_features_from_csv(std::string csv_dir){
std::cout << "csv_dir: " << csv_dir << std::endl;
Expand Down Expand Up @@ -2235,7 +2243,7 @@ double PolynomialFilter::fit_transform(double timestamp, double sample)
Eigen::Vector2d acc_steer_error = Eigen::Vector2d::Zero();
Eigen::VectorXd previous_error_compensator = Eigen::VectorXd::Zero(previous_error_.size());


/*
if (use_nonzero_initial_hidden_)
{
Eigen::VectorXd h_gru = Eigen::VectorXd::Zero(h_gru_dim_for_initial_hidden_);
Expand Down Expand Up @@ -2265,7 +2273,11 @@ double PolynomialFilter::fit_transform(double timestamp, double sample)
c_lstm = initial_hidden.tail(h_dim_full_);
}

*/
if (use_offline_features_){
h_lstm = offline_features_.head(h_dim_full_);
c_lstm = offline_features_.tail(h_dim_full_);
}

for (int i = 0; i<update_lstm_len_; i++) {

Expand Down Expand Up @@ -2652,7 +2664,7 @@ PYBIND11_MODULE(utils, m)
.def(py::init())
.def("set_NN_params", &VehicleAdaptor::set_NN_params)
.def("set_NN_params_from_csv", &VehicleAdaptor::set_NN_params_from_csv)
.def("set_initial_hidden_params_from_csv", &VehicleAdaptor::set_initial_hidden_params_from_csv)
//.def("set_initial_hidden_params_from_csv", &VehicleAdaptor::set_initial_hidden_params_from_csv)
.def("set_offline_features_from_csv", &VehicleAdaptor::set_offline_features_from_csv)
.def("set_offline_data_set_for_compensation", &VehicleAdaptor::set_offline_data_set_for_compensation)
.def("set_offline_data_set_for_compensation_from_csv", &VehicleAdaptor::set_offline_data_set_for_compensation_from_csv)
Expand Down
41 changes: 23 additions & 18 deletions ...icle_adaptor/autoware_vehicle_adaptor/training/get_initial_hidden_NN_with_offline_data.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -160,35 +160,40 @@ def __init__(
)

# fusion case
self.fusion = nn.Sequential(
nn.Linear(2*output_size, output_size),
self.transform = nn.Sequential(
nn.Linear(output_size, output_size),
nn.Tanh()
)
nn.init.uniform_(self.fusion[0].weight, a=lb, b=ub)
nn.init.uniform_(self.fusion[0].bias, a=lb, b=ub)
nn.init.uniform_(self.transform[0].weight, a=lb, b=ub)
nn.init.uniform_(self.transform[0].bias, a=lb, b=ub)

# only online case
self.only_online = nn.Sequential(
nn.Linear(output_size, output_size),
nn.Tanh()
)
nn.init.uniform_(self.only_online[0].weight, a=lb, b=ub)
nn.init.uniform_(self.only_online[0].bias, a=lb, b=ub)
def forward(self, offline_out, online_x, mask, preprocessor):
# self.only_online = nn.Sequential(
# nn.Linear(output_size, output_size),
# nn.Tanh()
# )
# nn.init.uniform_(self.only_online[0].weight, a=lb, b=ub)
# nn.init.uniform_(self.only_online[0].bias, a=lb, b=ub)
def forward(self, offline_out, mask=None): #online_x, mask, preprocessor):
"""
mask: Tensor of shape (batch_size, 1), 1 if offline data is available, 0 otherwise
"""
online_combined_input = preprocessor(online_x)
online_gru_out = self.gru_online(online_combined_input)[1][0]
if mask is None:
return self.transform(offline_out)
else:
mask = mask.float().unsqueeze(1)
return mask * self.transform(offline_out)
#online_combined_input = preprocessor(online_x)
#online_gru_out = self.gru_online(online_combined_input)[1][0]
#online_gru_out = self.gru_online(online_combined_input)[0]
#online_attn = self.attn_online(online_gru_out) # (batch_size, seq_len, 1)
#online_attn = torch.softmax(online_attn, dim=1)
#online_context = torch.sum(online_attn * online_gru_out, dim=1)
online_out = self.final_layer_online(online_gru_out)
#online_out = self.final_layer_online(online_gru_out)

mask = mask.float().unsqueeze(1)
fused_out = self.fusion(torch.cat((offline_out, online_out), dim=1))
only_online_out = self.only_online(online_out)
#mask = mask.float().unsqueeze(1)
#fused_out = self.fusion(torch.cat((offline_out, online_out), dim=1))
#only_online_out = self.only_online(online_out)
# print(mask.shape, fused_out.shape, only_online_out.shape)
return mask * fused_out + (1 - mask) * only_online_out
# return mask * fused_out + (1 - mask) * only_online_out

8 changes: 5 additions & 3 deletions ..._adaptor/autoware_vehicle_adaptor/training/train_error_prediction_NN_with_offline_data.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -700,7 +700,7 @@ def save_model(self, path="vehicle_model.pth", path_for_preprocessor="vehicle_mo
torch.save(self.model_for_initial_hidden, path_for_initial_hidden)
save_dir = path.replace(".pth","")
convert_model_to_csv.convert_model_to_csv(self.model, save_dir)
convert_model_to_csv.convert_initial_hidden_with_offline_data_model_to_csv(self.preprocessor, self.model_for_initial_hidden, save_dir)
#convert_model_to_csv.convert_initial_hidden_with_offline_data_model_to_csv(self.preprocessor, self.model_for_initial_hidden, save_dir)

def save_given_model(self,model,preprocessor,model_for_offline_features,model_for_initial_hidden,path="vehicle_model.pth", path_for_preprocessor="vehicle_model_for_preprocessor.pth", path_for_offline_features="vehicle_model_for_offline_features.pth", path_for_initial_hidden="vehicle_model_for_initial_hidden.pth"):
torch.save(model, path)
Expand All @@ -709,9 +709,10 @@ def save_given_model(self,model,preprocessor,model_for_offline_features,model_fo
torch.save(model_for_initial_hidden, path_for_initial_hidden)
save_dir = path.replace(".pth","")
convert_model_to_csv.convert_model_to_csv(model, save_dir)
convert_model_to_csv.convert_initial_hidden_with_offline_data_model_to_csv(preprocessor, model_for_initial_hidden, save_dir)
#convert_model_to_csv.convert_initial_hidden_with_offline_data_model_to_csv(preprocessor, model_for_initial_hidden, save_dir)
def save_offline_features(self, path="offline_features.csv"):
offline_features = get_offline_feature_dict(self.preprocessor,self.model_for_offline_features,self.offline_data_dict_train)[0]
offline_features = self.model_for_initial_hidden(offline_features.unsqueeze(0))[0]
offline_features_np = offline_features.cpu().detach().numpy()
np.savetxt(path, offline_features_np, delimiter=",")
def update_adaptive_weight(
Expand Down Expand Up @@ -754,7 +755,8 @@ def update_adaptive_weight(
indices_batch,
offline_feature_dim
)
hc_initial_concat = model_for_initial_hidden(offline_features, X_batch[:,:past_initial_hidden_length],mask,preprocessor)
#hc_initial_concat = model_for_initial_hidden(offline_features, X_batch[:,:past_initial_hidden_length],mask,preprocessor)
hc_initial_concat = model_for_initial_hidden(offline_features,mask)

hc_initial = (hc_initial_concat[:, :model.lstm_hidden_total_size].unsqueeze(0).contiguous(), hc_initial_concat[:, model.lstm_hidden_total_size:].unsqueeze(0).contiguous())
Y_pred, _ = model(X_batch[:,past_length- past_initial_hidden_length:], previous_error=Y_batch[:, past_length- past_initial_hidden_length:past_initial_hidden_length, -2:],hc=hc_initial, mode="get_lstm_states")
Expand Down
Loading

0 comments on commit 429c86b

Please sign in to comment.