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

finished unittest for action lpf non-lpf dimensions vs Euler

parent f0f44284
......@@ -78,6 +78,7 @@ class IntegratedActionModelLPFTpl : public ActionModelAbstractTpl<_Scalar> {
const;
const Scalar& get_dt() const;
const Scalar& get_fc() const;
const Scalar& get_alpha() const { return alpha_; };
const std::size_t& get_nw() const { return nw_; };
const std::size_t& get_ntau() const { return ntau_; };
......
......@@ -265,6 +265,62 @@ void test_calc_NONE_equivalent_euler(
}
// void test_calcDiff_NONE_equivalent_euler(
// ActionModelLPFTypes::Type iam_type,
// DifferentialActionModelTypes::Type dam_type,
// PinocchioReferenceTypes::Type ref_type = PinocchioReferenceTypes::LOCAL,
// ContactModelMaskTypes::Type mask_type = ContactModelMaskTypes::Z){
// // Create IAM LPF
// ActionModelLPFFactory factory_iam;
// const boost::shared_ptr<sobec::IntegratedActionModelLPF>& modelLPF =
// factory_iam.create(iam_type, dam_type, ref_type, mask_type);
// const boost::shared_ptr<crocoddyl::ActionDataAbstract>& dataLPF = modelLPF->createData();
// // Create IAM Euler from DAM and iamLPF.dt (with cost residual)
// boost::shared_ptr<crocoddyl::IntegratedActionModelEuler> modelEuler =
// boost::make_shared<crocoddyl::IntegratedActionModelEuler>(modelLPF->get_differential(), modelLPF->get_dt(), true);
// const boost::shared_ptr<crocoddyl::ActionDataAbstract>& dataEuler = modelEuler->createData();
// // Generating random values for the state and control
// std::size_t nx = modelEuler->get_state()->get_nx();
// std::size_t ndx = modelEuler->get_state()->get_ndx();
// std::size_t nv = modelEuler->get_state()->get_nv();
// std::size_t ntau = boost::static_pointer_cast<sobec::IntegratedActionModelLPF>(modelLPF)->get_ntau();
// std::size_t nu = modelEuler->get_nu();
// const Eigen::VectorXd y = modelLPF->get_state()->rand();
// const Eigen::VectorXd& w = Eigen::VectorXd::Random(modelLPF->get_nw());
// const Eigen::VectorXd x = y.head(nx);
// Eigen::VectorXd tau = w;
// const std::vector<int>& lpf_torque_ids = modelLPF->get_lpf_torque_ids();
// const std::vector<int>& non_lpf_torque_ids = modelLPF->get_non_lpf_torque_ids();
// BOOST_CHECK(ntau == lpf_torque_ids.size());
// for(std::size_t i=0; i<lpf_torque_ids.size();i++){
// tau(lpf_torque_ids[i]) = y.tail(ntau)[i];
// }
// BOOST_CHECK(non_lpf_torque_ids.size() + lpf_torque_ids.size() == nu );
// // Checking the partial derivatives against NumDiff
// double tol = 1e-6;
// // Computing the action
// modelLPF->calc(dataLPF, y, w);
// modelEuler->calc(dataEuler, x, tau);
// // Computing the derivatives
// modelLPF->calcDiff(dataLPF, y, w);
// modelEuler->calcDiff(dataEuler, x, tau);
// // Case no joint is LPF
// BOOST_CHECK((dataLPF->Fx - dataEuler->Fx).isZero(tol));
// BOOST_CHECK((dataLPF->Fu - dataEuler->Fu).isZero(tol));
// 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));
// }
void test_calcDiff_explicit_equivalent_euler(
ActionModelLPFTypes::Type iam_type,
DifferentialActionModelTypes::Type dam_type,
......@@ -301,67 +357,52 @@ 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-6; //1e-3
double tol = 1e-6;
// Computing the action
modelLPF->calc(dataLPF, y, w);
modelEuler->calc(dataEuler, x, w);
modelEuler->calc(dataEuler, x, tau);
// Computing the derivatives
modelLPF->calcDiff(dataLPF, y, w);
modelEuler->calcDiff(dataEuler, x, w);
// Case no joint is LPF
if(ntau == 0){
BOOST_CHECK((dataLPF->Fx - dataEuler->Fx).isZero(tol));
BOOST_CHECK((dataLPF->Fu - dataEuler->Fu).isZero(tol));
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));
}
modelEuler->calcDiff(dataEuler, x, tau);
// All or some joint are LPF
else{
// 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));
// 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));
for(std::size_t j=0; j<lpf_torque_ids.size();j++){
BOOST_CHECK( (Luu_LPF(i,j)- dataEuler->Luu(lpf_torque_ids[i], lpf_torque_ids[j]) ) <= 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);
// 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
const Eigen::MatrixXd& Fu_nonLPF = dataLPF->Fu.topRows(ndx);
const Eigen::MatrixXd& Lu_nonLPF = dataLPF->Lu;
// const Eigen::MatrixXd& Lxu_nonLPF = dataLPF->Lxx.topRightCorner(ndx, ntau);
const Eigen::MatrixXd& Luu_nonLPF = dataLPF->Luu;
for(std::size_t i=0; i<non_lpf_torque_ids.size();i++){
BOOST_CHECK((Fu_nonLPF.col(non_lpf_torque_ids[i]) - dataEuler->Fu.col(non_lpf_torque_ids[i])).isZero(tol));
BOOST_CHECK((Lu_nonLPF(non_lpf_torque_ids[i]) - dataEuler->Lu(non_lpf_torque_ids[i])) <= tol);
// BOOST_CHECK((Lxu_nonLPF.col(i) - dataEuler->Lxu.col(lpf_torque_ids[i])).isZero(tol));
for(std::size_t j=0; j<lpf_torque_ids.size();j++){
BOOST_CHECK((Luu_nonLPF(non_lpf_torque_ids[i],non_lpf_torque_ids[j]) - dataEuler->Luu(non_lpf_torque_ids[i], non_lpf_torque_ids[j])) <= 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);
// 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
// const Eigen::MatrixXd& Fu_nonLPF = dataLPF->Fu.topRows(ndx);
// // const Eigen::MatrixXd& Lu_nonLPF = dataLPF->Lw;
// // const Eigen::MatrixXd& Lxu_nonLPF = dataLPF->Lxx.topRightCorner(ndx, ntau);
// // const Eigen::MatrixXd& Luu_nonLPF = dataLPF->Lxx.bottomRightCorner(ntau, ntau);
// for(std::size_t i=0; i<lpf_torque_ids.size();i++){
// BOOST_CHECK((Fu_nonLPF.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));
// }
// // 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);
// // 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));
}
}
......@@ -388,22 +429,21 @@ void register_action_model_unit_tests(
// boost::bind(&test_check_data, iam_type, dam_type, ref_type,
// mask_type)));
// ts->add(BOOST_TEST_CASE(boost::bind(&test_calc_returns_state, iam_type,
// dam_type, ref_type, mask_type)));
// ts->add(BOOST_TEST_CASE(boost::bind(&test_calc_returns_a_cost, iam_type,
// dam_type, ref_type, mask_type)));
// ts->add(BOOST_TEST_CASE(boost::bind(&test_partial_derivatives_action_model,
// iam_type, dam_type, ref_type, mask_type)));
ts->add(BOOST_TEST_CASE(boost::bind(&test_calc_returns_state, iam_type,
dam_type, ref_type, mask_type)));
ts->add(BOOST_TEST_CASE(boost::bind(&test_calc_returns_a_cost, iam_type,
dam_type, ref_type, mask_type)));
ts->add(BOOST_TEST_CASE(boost::bind(&test_partial_derivatives_action_model,
iam_type, dam_type, ref_type, mask_type)));
// Equivalence with Euler when alpha=0 or ntau=0
if(iam_type == ActionModelLPFTypes::Type::IntegratedActionModelLPF_alpha0){
ts->add(BOOST_TEST_CASE(boost::bind(&test_calc_alpha0_equivalent_euler, iam_type, dam_type, ref_type, mask_type)));
}
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_NONE_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)));
}
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