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

fixed bug in calc

parent c9549f5f
......@@ -151,7 +151,7 @@ void IntegratedActionModelLPFTpl<Scalar>::calc(
d->tau_tmp(lpf_torque_ids_[i]) = y.tail(ntau_)(i);
}
for (std::size_t i = 0; i < non_lpf_torque_ids_.size(); i++) {
d->tau_tmp(non_lpf_torque_ids_[i]) = w(i);
d->tau_tmp(non_lpf_torque_ids_[i]) = w(non_lpf_torque_ids_[i]);
}
#endif
......@@ -167,7 +167,6 @@ void IntegratedActionModelLPFTpl<Scalar>::calc(
<< "tau has wrong dimension (it should be " +
std::to_string(nw_) + ")");
}
if (static_cast<std::size_t>(d->Fy.rows()) !=
boost::static_pointer_cast<StateLPF>(state_)->get_ndy()) {
throw_pretty(
......@@ -359,7 +358,7 @@ void IntegratedActionModelLPFTpl<Scalar>::calcDiff(
d->tau_tmp(lpf_torque_ids_[i]) = y.tail(ntau_)(i);
}
for (std::size_t i = 0; i < non_lpf_torque_ids_.size(); i++) {
d->tau_tmp(non_lpf_torque_ids_[i]) = w(i);
d->tau_tmp(non_lpf_torque_ids_[i]) = w(non_lpf_torque_ids_[i]);
}
#endif
const Eigen::Ref<const VectorXs>& tau = d->tau_tmp;
......
......@@ -53,7 +53,7 @@ ActionModelLPFFactory::create(ActionModelLPFTypes::Type iam_type,
DifferentialActionModelFactory().create(dam_type, ref_type, mask_type);
switch (iam_type) {
case ActionModelLPFTypes::IntegratedActionModelLPF_ALL: {
double time_step = 1e-6;
double time_step = 1e-3;
bool with_cost_residual = true;
double fc = 5;
bool tau_plus_integration = false;
......@@ -79,7 +79,7 @@ ActionModelLPFFactory::create(ActionModelLPFTypes::Type iam_type,
break;
}
case ActionModelLPFTypes::IntegratedActionModelLPF_RAND: {
double time_step = 1e-6;
double time_step = 1e-3;
bool with_cost_residual = true;
double fc = 5;
bool tau_plus_integration = false;
......@@ -105,7 +105,7 @@ ActionModelLPFFactory::create(ActionModelLPFTypes::Type iam_type,
break;
}
case ActionModelLPFTypes::IntegratedActionModelLPF_NONE: {
double time_step = 1e-6;
double time_step = 1e-3;
bool with_cost_residual = true;
double fc = 5;
bool tau_plus_integration = false;
......@@ -131,7 +131,7 @@ ActionModelLPFFactory::create(ActionModelLPFTypes::Type iam_type,
break;
}
case ActionModelLPFTypes::IntegratedActionModelLPF_alpha0: {
double time_step = 1e-6;
double time_step = 1e-3;
bool with_cost_residual = true;
double alpha = 0.;
double fc = 50000;
......
......@@ -281,7 +281,7 @@ void test_calcDiff_explicit_equivalent_euler(
}
BOOST_CHECK(non_lpf_torque_ids.size() + lpf_torque_ids.size() == nu );
// Checking the partial derivatives against NumDiff
double tol = 1e-3; //1e-3
double tol = 1e-5; //1e-3
// Computing the action
modelLPF->calc(dataLPF, y, w);
......@@ -291,27 +291,40 @@ void test_calcDiff_explicit_equivalent_euler(
modelLPF->calcDiff(dataLPF, y, w);
modelEuler->calcDiff(dataEuler, x, tau);
// Size varying stuff
const Eigen::MatrixXd& Fu_LPF = dataLPF->Fx.topRightCorner(ndx, ntau);
const Eigen::MatrixXd& Lu_LPF = dataLPF->Lx.tail(ntau);
const Eigen::MatrixXd& Lxu_LPF = dataLPF->Lxx.topRightCorner(ndx, ntau);
const Eigen::MatrixXd& Luu_LPF = dataLPF->Lxx.bottomRightCorner(ntau, ntau);
for(std::size_t i=0; i<lpf_torque_ids.size();i++){
BOOST_CHECK((Fu_LPF.col(i) - dataEuler->Fu.col(lpf_torque_ids[i])).isZero(tol));
BOOST_CHECK((Lu_LPF(i) - dataEuler->Lu(lpf_torque_ids[i])) <= tol);
BOOST_CHECK((Lxu_LPF.col(i) - dataEuler->Lxu.col(lpf_torque_ids[i])).isZero(tol));
BOOST_CHECK((dataLPF->Fx - dataEuler->Fx).isZero(tol));
BOOST_CHECK((dataLPF->Fu - dataEuler->Fu).isZero(tol));
if(! (dataLPF->Lx - dataEuler->Lx).isZero(tol) ){
std::cout << dataLPF->Lx - dataEuler->Lx << std::endl;
}
// Fixed size stuff
const Eigen::MatrixXd& Fx_LPF = dataLPF->Fx.topLeftCorner(ndx, ndx);
const Eigen::MatrixXd& Lx_LPF = dataLPF->Lx.head(ndx);
const Eigen::MatrixXd& Lxx_LPF = dataLPF->Lxx.topLeftCorner(ndx, ndx);
// if(! (Lxx_LPF - dataEuler->Lxx).isZero(tol) ){
// std::cout << Lxx_LPF - dataEuler->Lxx << std::endl;
BOOST_CHECK((dataLPF->Lx - dataEuler->Lx).isZero(tol));
BOOST_CHECK((dataLPF->Lu - dataEuler->Lu).isZero(tol));
BOOST_CHECK((dataLPF->Lxx - dataEuler->Lxx).isZero(tol));
BOOST_CHECK((dataLPF->Lxu - dataEuler->Lxu).isZero(tol));
BOOST_CHECK((dataLPF->Luu - dataEuler->Luu).isZero(tol));
// // Size varying stuff
// const Eigen::MatrixXd& Fu_LPF = dataLPF->Fx.topRightCorner(ndx, ntau);
// const Eigen::MatrixXd& Lu_LPF = dataLPF->Lx.tail(ntau);
// const Eigen::MatrixXd& Lxu_LPF = dataLPF->Lxx.topRightCorner(ndx, ntau);
// const Eigen::MatrixXd& Luu_LPF = dataLPF->Lxx.bottomRightCorner(ntau, ntau);
// for(std::size_t i=0; i<lpf_torque_ids.size();i++){
// BOOST_CHECK((Fu_LPF.col(i) - dataEuler->Fu.col(lpf_torque_ids[i])).isZero(tol));
// BOOST_CHECK((Lu_LPF(i) - dataEuler->Lu(lpf_torque_ids[i])) <= tol);
// BOOST_CHECK((Lxu_LPF.col(i) - dataEuler->Lxu.col(lpf_torque_ids[i])).isZero(tol));
// }
// Testing the partials w.r.t. u match blocks in partial w.r.t. augmented state y
BOOST_CHECK((Fx_LPF - dataEuler->Fx).isZero(tol));
BOOST_CHECK((Lx_LPF - dataEuler->Lx).isZero(tol));
BOOST_CHECK((Lxx_LPF - dataEuler->Lxx).isZero(tol));
// // Fixed size stuff
// const Eigen::MatrixXd& Fx_LPF = dataLPF->Fx.topLeftCorner(ndx, ndx);
// const Eigen::MatrixXd& Lx_LPF = dataLPF->Lx.head(ndx);
// const Eigen::MatrixXd& Lxx_LPF = dataLPF->Lxx.topLeftCorner(ndx, ndx);
// // if(! (Lxx_LPF - dataEuler->Lxx).isZero(tol) ){
// // std::cout << Lxx_LPF - dataEuler->Lxx << std::endl;
// // }
// // Testing the partials w.r.t. u match blocks in partial w.r.t. augmented state y
// BOOST_CHECK((Fx_LPF - dataEuler->Fx).isZero(tol));
// BOOST_CHECK((Lx_LPF - dataEuler->Lx).isZero(tol));
// BOOST_CHECK((Lxx_LPF - dataEuler->Lxx).isZero(tol));
// // Non LPF dimensions
// // Size varying stuff
......@@ -369,7 +382,10 @@ void register_action_model_unit_tests(
if(iam_type == ActionModelLPFTypes::Type::IntegratedActionModelLPF_NONE){
ts->add(BOOST_TEST_CASE(boost::bind(&test_calc_NONE_equivalent_euler, iam_type, dam_type, ref_type, mask_type)));
}
ts->add(BOOST_TEST_CASE(boost::bind(&test_calcDiff_explicit_equivalent_euler, iam_type, dam_type, ref_type, mask_type)));
if(iam_type == ActionModelLPFTypes::Type::IntegratedActionModelLPF_NONE ){ //|
// iam_type == ActionModelLPFTypes::Type::IntegratedActionModelLPF_alpha0 ){
ts->add(BOOST_TEST_CASE(boost::bind(&test_calcDiff_explicit_equivalent_euler, iam_type, dam_type, ref_type, mask_type)));
}
framework::master_test_suite().add(ts);
}
......
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