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

added unittests for equivalence of action model lpf block partials with Euler's + alpha=0 + n_lpf=0

parent 9e6f72d9
Pipeline #20043 passed with stage
in 27 minutes and 8 seconds
......@@ -79,6 +79,10 @@ class IntegratedActionModelLPFTpl : public ActionModelAbstractTpl<_Scalar> {
return lpf_joint_names_;
};
const std::vector<int>& get_lpf_torque_ids() const {
return lpf_torque_ids_;
};
void set_dt(const Scalar& dt);
void set_fc(const Scalar& fc);
void set_alpha(const Scalar& alpha);
......@@ -113,9 +117,9 @@ class IntegratedActionModelLPFTpl : public ActionModelAbstractTpl<_Scalar> {
boost::shared_ptr<DifferentialActionModelAbstract> differential_;
Scalar time_step_;
Scalar time_step2_;
Scalar fc_;
Scalar alpha_;
bool with_cost_residual_;
Scalar fc_;
bool enable_integration_;
Scalar tauReg_weight_; //!< Cost weight for unfiltered torque regularization
VectorXs tauReg_reference_; //!< Cost reference for unfiltered torque
......
......@@ -28,9 +28,8 @@ IntegratedActionModelLPFTpl<Scalar>::IntegratedActionModelLPFTpl(
time_step2_(time_step * time_step),
with_cost_residual_(with_cost_residual),
fc_(fc),
tau_plus_integration_(tau_plus_integration),
nw_(model->get_nu()),
ntau_(lpf_joint_names.size()),
tau_plus_integration_(tau_plus_integration),
ny_(model->get_state()->get_nx() + lpf_joint_names.size()),
enable_integration_(true),
filter_(filter),
......@@ -41,6 +40,7 @@ IntegratedActionModelLPFTpl<Scalar>::IntegratedActionModelLPFTpl(
pin_model_ = state->get_pinocchio();
// Check that used-specified LPF joints are valid (no free-flyer) and collect
// ids
ntau_ = lpf_joint_names.size();
for (std::vector<std::string>::iterator iter = lpf_joint_names.begin();
iter != lpf_joint_names.end(); ++iter) {
std::size_t jointId = pin_model_->getJointId(*iter);
......
......@@ -150,9 +150,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.001;
double tauReg_weight = 0.;
Eigen::VectorXd tauReg_ref = Eigen::VectorXd::Zero(iam->get_ntau());
double tauLim_weight = 0.1;
double tauLim_weight = 0.;
iam->set_control_reg_cost(tauReg_weight, tauReg_ref);
iam->set_control_lim_cost(tauLim_weight);
iam->set_alpha(alpha);
......
......@@ -159,7 +159,7 @@ void test_partial_derivatives_action_model(
void test_calc_equivalent_euler(
void test_calc_alpha0_equivalent_euler(
ActionModelLPFTypes::Type iam_type,
DifferentialActionModelTypes::Type dam_type,
PinocchioReferenceTypes::Type ref_type = PinocchioReferenceTypes::LOCAL,
......@@ -180,41 +180,136 @@ void test_calc_equivalent_euler(
const boost::shared_ptr<crocoddyl::ActionDataAbstract>& dataEuler = modelEuler->createData();
// Generating random values for the state and control
Eigen::VectorXd y = modelLPF->get_state()->rand();
const Eigen::VectorXd& w = Eigen::VectorXd::Random(modelLPF->get_nw());
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 nq = nx - nv;
std::size_t ntau = boost::static_pointer_cast<sobec::IntegratedActionModelLPF>(modelLPF)->get_ntau();
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);
const Eigen::VectorXd tau = y.tail(ntau);
// Checking the partial derivatives against NumDiff
double tol = 1e-2;
double tol = 1e-6;
// Computing the action
modelLPF->calc(dataLPF, y, w);
modelEuler->calc(dataEuler, x, tau);
// Test perfect actuation and state integration
BOOST_CHECK((dataLPF->xnext.tail(ntau) - w).isZero(tol));
BOOST_CHECK((dataLPF->xnext.head(nx) - dataEuler->xnext).isZero(tol));
}
void test_calc_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)
DifferentialActionModelFactory factory_dam;
boost::shared_ptr<crocoddyl::DifferentialActionModelAbstract> dam =
factory_dam.create(dam_type, ref_type, mask_type);
boost::shared_ptr<crocoddyl::IntegratedActionModelEuler> modelEuler =
boost::make_shared<crocoddyl::IntegratedActionModelEuler>(dam, 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 ntau_state = boost::static_pointer_cast<sobec::StateLPF>(modelLPF->get_state())->get_ntau();
BOOST_CHECK(ntau == 0);
BOOST_CHECK(ntau_state == 0);
const std::vector<int>& lpf_torque_ids = modelLPF->get_lpf_torque_ids();
BOOST_CHECK(lpf_torque_ids.size() == 0);
const Eigen::VectorXd y = modelLPF->get_state()->rand();
BOOST_CHECK(y.size() == nx);
const Eigen::VectorXd& w = Eigen::VectorXd::Random(modelLPF->get_nw());
BOOST_CHECK(w.size() == modelEuler->get_nu());
// Checking the partial derivatives against NumDiff
double tol = 1e-6;
// Computing the action
modelLPF->calc(dataLPF, y, w);
if(!(dataLPF->xnext.tail(nv) - w).isZero(tol)){
std::cout << " lpf " << std::endl;
std::cout << dataLPF->xnext.tail(nv) << std::endl;
std::cout << w << std::endl;
// std::cout << dataEuler->Lxx << std::endl;
modelEuler->calc(dataEuler, y, w);
// Test perfect actuation and state integration
BOOST_CHECK((dataLPF->xnext - dataEuler->xnext).isZero(tol));
}
void test_calcDiff_explicit_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)
DifferentialActionModelFactory factory_dam;
boost::shared_ptr<crocoddyl::DifferentialActionModelAbstract> dam =
factory_dam.create(dam_type, ref_type, mask_type);
boost::shared_ptr<crocoddyl::IntegratedActionModelEuler> modelEuler =
boost::make_shared<crocoddyl::IntegratedActionModelEuler>(dam, 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();
for(std::size_t i=0; i<lpf_torque_ids.size();i++){
tau(lpf_torque_ids[i]) = y.tail(ntau)[i];
}
BOOST_CHECK((dataLPF->xnext.tail(nv) - w).isZero(tol));
modelEuler->calc(dataEuler, y.head(nx), w);
BOOST_CHECK((dataLPF->xnext.head(nx) - dataEuler->xnext).isZero(tol));
// Checking the partial derivatives against NumDiff
double tol = 1e-3;
// Computing the action
modelLPF->calc(dataLPF, y, w);
modelEuler->calc(dataEuler, x, tau);
// Computing the derivatives
modelLPF->calcDiff(dataLPF, y, w);
modelEuler->calcDiff(dataEuler, y.head(nx), w);
BOOST_CHECK((dataLPF->Fx.topLeftCorner(nv, ndx) - dataEuler->Fx).isZero(tol));
BOOST_CHECK((dataLPF->Fu.topLeftCorner(nv, ndx) - dataEuler->Fu).isZero(tol));
BOOST_CHECK((dataLPF->Lx.head(nx) - dataEuler->Lx).isZero(tol));
BOOST_CHECK((dataLPF->Lu.head(nv) - dataEuler->Lu).isZero(tol));
BOOST_CHECK((dataLPF->Lxx.topLeftCorner(ndx, ndx) - dataEuler->Lxx).isZero(tol));
// if(!(dataLPF->Lxx.topLeftCorner(ndx, nv) - dataEuler->Lxx).isZero(tol)){
// std::cout << " lpf " << std::endl;
// std::cout << dataLPF->Lxx.topLeftCorner(ndx, ndx) << std::endl;
// std::cout << " euler " << std::endl;
// std::cout << dataEuler->Lxx << std::endl;
// }
BOOST_CHECK((dataLPF->Lxu.topLeftCorner(ndx, nv) - dataEuler->Lxu).isZero(tol));
BOOST_CHECK((dataLPF->Luu.topLeftCorner(nv,nv) - dataEuler->Luu).isZero(tol));
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));
}
// 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));
}
......@@ -244,13 +339,15 @@ void register_action_model_unit_tests(
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,
ts->add(BOOST_TEST_CASE(boost::bind(&test_partial_derivatives_action_model,
iam_type, dam_type, ref_type, mask_type)));
if(iam_type == ActionModelLPFTypes::Type::IntegratedActionModelLPF_alpha0 ||
iam_type == ActionModelLPFTypes::Type::IntegratedActionModelLPF_NONE){
ts->add(BOOST_TEST_CASE(boost::bind(&test_calc_equivalent_euler, iam_type, dam_type, ref_type, mask_type)));
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_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