Commit d642fc7c authored by Sébastien Kleff's avatar Sébastien Kleff
Browse files

randomized weights and cutoff frequency for action lpf unittests

parent d1ddcc22
Pipeline #20066 passed with stage
in 30 minutes and 33 seconds
......@@ -247,7 +247,7 @@ void IntegratedActionModelLPFTpl<Scalar>::calc(
// Add hard-coded cost on unfiltered torque a[r(w)] only at LPF joints
// Torque REG
if (tauReg_weight_ != 0) {
if (tauReg_weight_ > 0) {
#if EIGEN_VERSION_AT_LEAST(3, 4, 0)
tauReg_residual_ = w(lpf_torque_ids_) - tauReg_reference_;
#else
......@@ -259,7 +259,7 @@ void IntegratedActionModelLPFTpl<Scalar>::calc(
d->cost += Scalar(0.5 * time_step_ * tauReg_weight_ * tauReg_residual_.transpose() * tauReg_residual_);
}
// Torque LIM
if (tauLim_weight_ != 0) {
if (tauLim_weight_ > 0) {
#if EIGEN_VERSION_AT_LEAST(3, 4, 0)
activation_model_tauLim_->calc(
d->activation,
......@@ -280,11 +280,11 @@ void IntegratedActionModelLPFTpl<Scalar>::calc(
d->r.head(differential_->get_nr()) = d->differential->r;
// Add unfiltered torque residuals
if (tauReg_weight_ != 0) {
if (tauReg_weight_ > 0) {
d->r.segment(differential_->get_nr(), ntau_) =
tauReg_residual_;
}
if (tauLim_weight_ != 0) {
if (tauLim_weight_ > 0) {
d->r.tail(ntau_) = tauLim_residual_;
}
}
......@@ -369,7 +369,7 @@ void IntegratedActionModelLPFTpl<Scalar>::calcDiff(
differential_->calcDiff(d->differential, x, tau);
// Get cost lim w derivatives
if (tauLim_weight_ != 0) {
if (tauLim_weight_ > 0) {
#if EIGEN_VERSION_AT_LEAST(3, 4, 0)
activation_model_tauLim_->calcDiff(d->activation, w(lpf_torque_ids_));
#else
......@@ -469,7 +469,7 @@ void IntegratedActionModelLPFTpl<Scalar>::calcDiff(
#endif
// Partials of hard-coded cost+(tauReg) & cost+(tauLim) w.r.t. (y,w)
if (tauReg_weight_ != 0) {
if (tauReg_weight_ > 0) {
#if EIGEN_VERSION_AT_LEAST(3, 4, 0)
d->Lw(lpf_torque_ids_) += time_step_ * tauReg_weight_ * d->r.segment(differential_->get_nr(), ntau_); // tau reg
d->Lww.diagonal().array()(lpf_torque_ids_) += Scalar(time_step_ * tauReg_weight_); // tau reg
......@@ -480,18 +480,17 @@ void IntegratedActionModelLPFTpl<Scalar>::calcDiff(
}
#endif
} // tauReg !=0
if (tauLim_weight_ != 0) {
if (tauLim_weight_ > 0) {
#if EIGEN_VERSION_AT_LEAST(3, 4, 0)
d->Lw(lpf_torque_ids_) += time_step_ * tauLim_weight_ * d->activation->Ar; // tau lim
d->Lww.diagonal()(lpf_torque_ids_) += time_step_ * tauLim_weight_ * d->activation->Arr.diagonal(); // tau lim
d->Lw(lpf_torque_ids_) += time_step_ * tauLim_weight_ * d->activation->Ar; // tau lim
d->Lww.diagonal()(lpf_torque_ids_) += time_step_ * tauLim_weight_ * d->activation->Arr.diagonal(); // tau lim
#else
for (std::size_t i = 0; i < lpf_torque_ids_.size(); i++) {
d->Lw(lpf_torque_ids_[i]) += time_step_ * tauLim_weight_ * d->activation->Ar(i); // tau lim
d->Lww.diagonal()(lpf_torque_ids_[i]) += time_step_ * tauLim_weight_ * d->activation->Arr.diagonal()(i); // tau lim
}
for (std::size_t i = 0; i < lpf_torque_ids_.size(); i++) {
d->Lw(lpf_torque_ids_[i]) += time_step_ * tauLim_weight_ * d->activation->Ar(i); // tau lim
d->Lww.diagonal()(lpf_torque_ids_[i]) += time_step_ * tauLim_weight_ * d->activation->Arr.diagonal()(i); // tau lim
}
#endif
} // tauLim !=0
} // tauLim !=0
} // tau integration
// // TAU PLUS INTEGRATION
......
......@@ -46,7 +46,8 @@ boost::shared_ptr<sobec::IntegratedActionModelLPF>
ActionModelLPFFactory::create(ActionModelLPFTypes::Type iam_type,
DifferentialActionModelTypes::Type dam_type,
PinocchioReferenceTypes::Type ref_type,
ContactModelMaskTypes::Type mask_type) const {
ContactModelMaskTypes::Type mask_type,
bool is_terminal) const {
// LPFJointMaskType lpf_mask_type) const {
boost::shared_ptr<sobec::IntegratedActionModelLPF> iam;
boost::shared_ptr<crocoddyl::DifferentialActionModelAbstract> dam =
......@@ -55,10 +56,9 @@ ActionModelLPFFactory::create(ActionModelLPFTypes::Type iam_type,
case ActionModelLPFTypes::IntegratedActionModelLPF_ALL: {
double time_step = 1e-3;
bool with_cost_residual = true;
double fc = 5;
double fc = 1 + (double) (rand()) /( (double)(RAND_MAX/(50-1)));
bool tau_plus_integration = false;
int filter = 1;
bool is_terminal = false;
// Select LPF joints
boost::shared_ptr<crocoddyl::StateMultibody> stateMultibody =
boost::static_pointer_cast<crocoddyl::StateMultibody>(
......@@ -71,9 +71,9 @@ ActionModelLPFFactory::create(ActionModelLPFTypes::Type iam_type,
dam, lpf_joint_names, time_step, with_cost_residual, fc,
tau_plus_integration, filter, is_terminal);
// set hard-coded costs on unfiltered torque
double tauReg_weight = 0.02;
double tauReg_weight = 1e-6 + (double) (rand()) /( (double)(RAND_MAX/(1e-1-1e-3)));
Eigen::VectorXd tauReg_ref = Eigen::VectorXd::Zero(iam->get_ntau());
double tauLim_weight = 1.;
double tauLim_weight = 1e-6 + (double) (rand()) /( (double)(RAND_MAX/(1e-1-1e-3)));
iam->set_control_reg_cost(tauReg_weight, tauReg_ref);
iam->set_control_lim_cost(tauLim_weight);
break;
......@@ -81,10 +81,10 @@ ActionModelLPFFactory::create(ActionModelLPFTypes::Type iam_type,
case ActionModelLPFTypes::IntegratedActionModelLPF_RAND: {
double time_step = 1e-3;
bool with_cost_residual = true;
double fc = 5;
// double r3 = 1 + static_cast <double> (rand()) /( static_cast <double> (RAND_MAX/(50-1)));
double fc = 1 + (double) (rand()) /( (double)(RAND_MAX/(50-1)));
bool tau_plus_integration = false;
int filter = 1;
bool is_terminal = false;
// Select LPF joints
boost::shared_ptr<crocoddyl::StateMultibody> stateMultibody =
boost::static_pointer_cast<crocoddyl::StateMultibody>(
......@@ -97,9 +97,9 @@ ActionModelLPFFactory::create(ActionModelLPFTypes::Type iam_type,
dam, lpf_joint_names, time_step, with_cost_residual, fc,
tau_plus_integration, filter, is_terminal);
// set hard-coded costs on unfiltered torque
double tauReg_weight = 0.02;
double tauReg_weight = 1e-6 + (double) (rand()) /( (double)(RAND_MAX/(1e-1-1e-3)));
Eigen::VectorXd tauReg_ref = Eigen::VectorXd::Zero(iam->get_ntau());
double tauLim_weight = 1.;
double tauLim_weight = 1e-6 + (double) (rand()) /( (double)(RAND_MAX/(1e-1-1e-3)));
iam->set_control_reg_cost(tauReg_weight, tauReg_ref);
iam->set_control_lim_cost(tauLim_weight);
break;
......@@ -107,10 +107,9 @@ ActionModelLPFFactory::create(ActionModelLPFTypes::Type iam_type,
case ActionModelLPFTypes::IntegratedActionModelLPF_NONE: {
double time_step = 1e-3;
bool with_cost_residual = true;
double fc = 5;
double fc = rand() % (50) + 1;
bool tau_plus_integration = false;
int filter = 1;
bool is_terminal = false;
// Select LPF joints
boost::shared_ptr<crocoddyl::StateMultibody> stateMultibody =
boost::static_pointer_cast<crocoddyl::StateMultibody>(
......@@ -122,22 +121,19 @@ ActionModelLPFFactory::create(ActionModelLPFTypes::Type iam_type,
iam = boost::make_shared<sobec::IntegratedActionModelLPF>(
dam, lpf_joint_names, time_step, with_cost_residual, fc,
tau_plus_integration, filter, is_terminal);
// set hard-coded costs on unfiltered torque
// double tauReg_weight = 0.;
// Eigen::VectorXd tauReg_ref = Eigen::VectorXd::Zero(iam->get_ntau());
// double tauLim_weight = 0.;
// iam->set_control_reg_cost(tauReg_weight, tauReg_ref);
// iam->set_control_lim_cost(tauLim_weight);
double tauReg_weight = 1e-6 + (double) (rand()) /( (double)(RAND_MAX/(1e-1-1e-3)));
Eigen::VectorXd tauReg_ref = Eigen::VectorXd::Zero(iam->get_ntau());
double tauLim_weight = 1e-6 + (double) (rand()) /( (double)(RAND_MAX/(1e-1-1e-3)));
iam->set_control_reg_cost(tauReg_weight, tauReg_ref);
iam->set_control_lim_cost(tauLim_weight);
break;
}
case ActionModelLPFTypes::IntegratedActionModelLPF_alpha0: {
double time_step = 1e-3;
bool with_cost_residual = true;
double alpha = 0.;
double fc = 50000;
bool tau_plus_integration = false;
int filter = 1;
bool is_terminal = false;
// Select LPF joints
boost::shared_ptr<crocoddyl::StateMultibody> stateMultibody =
boost::static_pointer_cast<crocoddyl::StateMultibody>(
......@@ -150,12 +146,7 @@ ActionModelLPFFactory::create(ActionModelLPFTypes::Type iam_type,
dam, lpf_joint_names, time_step, with_cost_residual, fc,
tau_plus_integration, filter, is_terminal);
// set hard-coded costs on unfiltered torque
// double tauReg_weight = 0.;
// Eigen::VectorXd tauReg_ref = Eigen::VectorXd::Zero(iam->get_ntau());
// double tauLim_weight = 0.;
// iam->set_control_reg_cost(tauReg_weight, tauReg_ref);
// iam->set_control_lim_cost(tauLim_weight);
iam->set_alpha(alpha);
iam->set_alpha(0.);
break;
}
default:
......
......@@ -52,7 +52,8 @@ class ActionModelLPFFactory {
ActionModelLPFTypes::Type iam_type,
DifferentialActionModelTypes::Type dam_type,
PinocchioReferenceTypes::Type ref_type = PinocchioReferenceTypes::LOCAL,
ContactModelMaskTypes::Type mask_type = ContactModelMaskTypes::Z) const;
ContactModelMaskTypes::Type mask_type = ContactModelMaskTypes::Z,
bool is_terminal = false) const;
// LPFJointMaskType lpf_mask_type = LPFJointMaskType::ALL) const;
};
......
......@@ -129,6 +129,16 @@ void test_partial_derivatives_against_numdiff(
BOOST_CHECK((data->Fx - data_num_diff->Fx).isZero(NUMDIFF_MODIFIER * tol));
BOOST_CHECK((data->Fu - data_num_diff->Fu).isZero(NUMDIFF_MODIFIER * tol));
if(!(data->Lx - data_num_diff->Lx).isZero(NUMDIFF_MODIFIER * tol)){
std::cout << " Lx - Lx_nd : " << std::endl;
// std::cout << (data->Lx - data_num_diff->Lx).lpNorm<Eigen::Infinity>() << std::endl;
std::cout << (data->Lx - data_num_diff->Lx) << std::endl;
}
if(!(data->Lu - data_num_diff->Lu).isZero(NUMDIFF_MODIFIER * tol)){
std::cout << " Lu - Lu_nd : " << std::endl;
// std::cout << (data->Lu - data_num_diff->Lu).lpNorm<Eigen::Infinity>() << std::endl;
std::cout << (data->Lu - data_num_diff->Lu) << std::endl;
}
BOOST_CHECK((data->Lx - data_num_diff->Lx).isZero(NUMDIFF_MODIFIER * tol));
BOOST_CHECK((data->Lu - data_num_diff->Lu).isZero(NUMDIFF_MODIFIER * tol));
if (model_num_diff.get_with_gauss_approx()) {
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment