Commit 446e2bac authored by pre-commit-ci[bot]'s avatar pre-commit-ci[bot]
Browse files

[pre-commit.ci] auto fixes from pre-commit.com hooks

for more information, see https://pre-commit.ci
parent e11db975
......@@ -11,12 +11,12 @@
#define BOOST_TEST_NO_MAIN
#define BOOST_TEST_ALTERNATIVE_INIT_API
#include <crocoddyl/core/integrator/euler.hpp>
#include "common.hpp"
#include "factory/diff-action.hpp"
#include "factory/lpf.hpp"
#include <crocoddyl/core/integrator/euler.hpp>
using namespace boost::unit_test;
using namespace sobec::unittest;
......@@ -157,41 +157,43 @@ void test_partial_derivatives_action_model(
test_partial_derivatives_against_numdiff(model);
}
void test_calc_alpha0_equivalent_euler(
void test_calc_alpha0_equivalent_euler(
ActionModelLPFTypes::Type iam_type,
DifferentialActionModelTypes::Type dam_type,
PinocchioReferenceTypes::Type ref_type = PinocchioReferenceTypes::LOCAL,
ContactModelMaskTypes::Type mask_type = ContactModelMaskTypes::Z){
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();
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 =
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();
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 nq = nx - nv;
std::size_t ntau = boost::static_pointer_cast<sobec::IntegratedActionModelLPF>(modelLPF)->get_ntau();
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-6;
// Computing the action
// Computing the action
modelLPF->calc(dataLPF, y, w);
modelEuler->calc(dataEuler, x, tau);
// Test perfect actuation and state integration
......@@ -199,33 +201,38 @@ void test_calc_alpha0_equivalent_euler(
BOOST_CHECK((dataLPF->xnext.head(nx) - dataEuler->xnext).isZero(tol));
}
void test_calc_NONE_equivalent_euler(
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){
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();
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 =
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();
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();
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();
......@@ -236,52 +243,55 @@ void test_calc_NONE_equivalent_euler(
BOOST_CHECK(w.size() == modelEuler->get_nu());
// Checking the partial derivatives against NumDiff
double tol = 1e-6;
// Computing the action
// Computing the action
modelLPF->calc(dataLPF, y, w);
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(
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){
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();
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 =
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();
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 =
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;
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++){
for (std::size_t i = 0; i < lpf_torque_ids.size(); i++) {
tau(lpf_torque_ids[i]) = y.tail(ntau)[i];
}
// Checking the partial derivatives against NumDiff
double tol = 1e-3;
// Computing the action
// Computing the action
modelLPF->calc(dataLPF, y, w);
modelEuler->calc(dataEuler, x, tau);
......@@ -289,30 +299,32 @@ void test_calcDiff_explicit_equivalent_euler(
modelLPF->calcDiff(dataLPF, y, w);
modelEuler->calcDiff(dataEuler, x, tau);
// Size varying stuff
// 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));
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(
(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) ){
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
// 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));
}
//----------------------------------------------------------------------------//
void register_action_model_unit_tests(
......@@ -339,15 +351,22 @@ 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){
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_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)));
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)));
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