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

cleaned up some warning + added unittest for lpf when alpha=0 and n_lpf=0

parent 9e734eb8
......@@ -137,7 +137,7 @@ class ContactModel1DTpl : public crocoddyl::ContactModel1DTpl<_Scalar> {
/**
* @brief Get pinocchio::ReferenceFrame
*/
const pinocchio::ReferenceFrame get_type() const;
const pinocchio::ReferenceFrame& get_type() const;
/**
* @brief Modify contact 1D mask
......@@ -147,7 +147,7 @@ class ContactModel1DTpl : public crocoddyl::ContactModel1DTpl<_Scalar> {
/**
* @brief Get contact 1D mask
*/
const Vector3MaskType get_mask() const;
const Vector3MaskType& get_mask() const;
/**
* @brief Print relevant information of the 1d contact model
......
......@@ -205,7 +205,7 @@ void ContactModel1DTpl<Scalar>::set_type(const pinocchio::ReferenceFrame type) {
}
template <typename Scalar>
const pinocchio::ReferenceFrame ContactModel1DTpl<Scalar>::get_type() const {
const pinocchio::ReferenceFrame& ContactModel1DTpl<Scalar>::get_type() const {
return type_;
}
......@@ -215,7 +215,7 @@ void ContactModel1DTpl<Scalar>::set_mask(const Vector3MaskType mask) {
}
template <typename Scalar>
const Vector3MaskType ContactModel1DTpl<Scalar>::get_mask() const {
const Vector3MaskType& ContactModel1DTpl<Scalar>::get_mask() const {
return mask_;
}
......
......@@ -130,7 +130,7 @@ class ContactModel3DTpl : public crocoddyl::ContactModel3DTpl<_Scalar> {
/**
* @brief Get pinocchio::ReferenceFrame
*/
const pinocchio::ReferenceFrame get_type() const;
const pinocchio::ReferenceFrame& get_type() const;
/**
* @brief Print relevant information of the 3d contact model
......
......@@ -199,7 +199,7 @@ void ContactModel3DTpl<Scalar>::set_type(const pinocchio::ReferenceFrame type) {
}
template <typename Scalar>
const pinocchio::ReferenceFrame ContactModel3DTpl<Scalar>::get_type() const {
const pinocchio::ReferenceFrame& ContactModel3DTpl<Scalar>::get_type() const {
return type_;
}
......
......@@ -81,6 +81,7 @@ class IntegratedActionModelLPFTpl : public ActionModelAbstractTpl<_Scalar> {
void set_dt(const Scalar& dt);
void set_fc(const Scalar& fc);
void set_alpha(const Scalar& alpha);
void set_differential(
boost::shared_ptr<DifferentialActionModelAbstract> model);
......
......@@ -44,7 +44,7 @@ IntegratedActionModelLPFTpl<Scalar>::IntegratedActionModelLPFTpl(
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);
lpf_joint_ids_.push_back(jointId);
lpf_joint_ids_.push_back(static_cast<int>(jointId));
std::size_t jointNv = pin_model_->nvs[jointId];
if (jointNv != (std::size_t)1) {
throw_pretty(
......@@ -115,7 +115,6 @@ void IntegratedActionModelLPFTpl<Scalar>::calc(
const boost::shared_ptr<ActionDataAbstract>& data,
const Eigen::Ref<const VectorXs>& y, const Eigen::Ref<const VectorXs>& w) {
const std::size_t& nv = differential_->get_state()->get_nv();
const std::size_t& nq = differential_->get_state()->get_nq();
const std::size_t& nx = differential_->get_state()->get_nx();
if (static_cast<std::size_t>(y.size()) != ny_) {
......@@ -138,7 +137,7 @@ void IntegratedActionModelLPFTpl<Scalar>::calc(
#if EIGEN_VERSION_AT_LEAST(3, 4, 0)
d->tau_tmp(lpf_torque_ids_) = y.tail(ntau_); // LPF dimensions
#else
for (int i = 0; i < lpf_torque_ids_.size(); i++) {
for (std::size_t i = 0; i < lpf_torque_ids_.size(); i++) {
d->tau_tmp(lpf_torque_ids_[i]) = y.tail(ntau_)(i);
}
#endif
......@@ -227,7 +226,7 @@ void IntegratedActionModelLPFTpl<Scalar>::calc(
#if EIGEN_VERSION_AT_LEAST(3, 4, 0)
d->dy.tail(ntau_) = ((1 - alpha_) * (w - tau))(lpf_torque_ids_);
#else
for (int i = 0; i < lpf_torque_ids_.size(); i++) {
for (std::size_t i = 0; i < lpf_torque_ids_.size(); i++) {
d->dy.tail(ntau_)(i) = ((1 - alpha_) * (w - tau))(lpf_torque_ids_[i]);
}
#endif
......@@ -240,7 +239,7 @@ void IntegratedActionModelLPFTpl<Scalar>::calc(
#if EIGEN_VERSION_AT_LEAST(3, 4, 0)
tauReg_residual_ = w(lpf_torque_ids_) - tauReg_reference_;
#else
for (int i = 0; i < lpf_torque_ids_.size(); i++) {
for (std::size_t i = 0; i < lpf_torque_ids_.size(); i++) {
tauReg_residual_(i) = w(lpf_torque_ids_[i]) - tauReg_reference_(i);
}
#endif
......@@ -255,7 +254,7 @@ void IntegratedActionModelLPFTpl<Scalar>::calc(
w(lpf_torque_ids_)); // Compute limit cost torque residual of w
tauLim_residual_ = w(lpf_torque_ids_);
#else
for (int i = 0; i < lpf_torque_ids_.size(); i++) {
for (std::size_t i = 0; i < lpf_torque_ids_.size(); i++) {
tauLim_residual_(i) = w(lpf_torque_ids_[i]);
}
activation_model_tauLim_->calc(
......@@ -296,7 +295,6 @@ void IntegratedActionModelLPFTpl<Scalar>::calcDiff(
const boost::shared_ptr<ActionDataAbstract>& data,
const Eigen::Ref<const VectorXs>& y, const Eigen::Ref<const VectorXs>& w) {
const std::size_t& nv = differential_->get_state()->get_nv();
const std::size_t& nq = differential_->get_state()->get_nq();
const std::size_t& nx = differential_->get_state()->get_nx();
const std::size_t& ndx = differential_->get_state()->get_ndx();
......@@ -322,7 +320,7 @@ void IntegratedActionModelLPFTpl<Scalar>::calcDiff(
#if EIGEN_VERSION_AT_LEAST(3, 4, 0)
d->tau_tmp(lpf_torque_ids_) = y.tail(ntau_); // LPF dimensions
#else
for (int i = 0; i < lpf_torque_ids_.size(); i++) {
for (std::size_t i = 0; i < lpf_torque_ids_.size(); i++) {
d->tau_tmp(lpf_torque_ids_[i]) = y.tail(ntau_)(i);
}
#endif
......@@ -338,7 +336,7 @@ void IntegratedActionModelLPFTpl<Scalar>::calcDiff(
#if EIGEN_VERSION_AT_LEAST(3, 4, 0)
activation_model_tauLim_->calcDiff(d->activation, w(lpf_torque_ids_));
#else
for (int i = 0; i < lpf_torque_ids_.size(); i++) {
for (std::size_t i = 0; i < lpf_torque_ids_.size(); i++) {
tauLim_residual_(i) = w(lpf_torque_ids_[i]);
}
activation_model_tauLim_->calcDiff(d->activation, tauLim_residual_);
......@@ -361,7 +359,7 @@ void IntegratedActionModelLPFTpl<Scalar>::calcDiff(
d->Fy.block(nv, ndx, nv, ntau_).noalias() =
da_du(Eigen::all, lpf_torque_ids_) * time_step_;
#else
for (int i = 0; i < lpf_torque_ids_.size(); i++) {
for (std::size_t i = 0; i < lpf_torque_ids_.size(); i++) {
d->Fy.block(0, ndx, nv, ntau_).col(i).noalias() =
da_du.col(lpf_torque_ids_[i]) * time_step2_;
d->Fy.block(nv, ndx, nv, ntau_).col(i).noalias() =
......@@ -395,7 +393,7 @@ void IntegratedActionModelLPFTpl<Scalar>::calcDiff(
d->Lyy.bottomRightCorner(ntau_, ntau_).noalias() =
time_step_ * d->differential->Luu(lpf_torque_ids_, lpf_torque_ids_);
#else
for (int i = 0; i < lpf_torque_ids_.size(); i++) {
for (std::size_t i = 0; i < lpf_torque_ids_.size(); i++) {
d->Ly.tail(ntau_)(i) =
time_step_ * d->differential->Lu(lpf_torque_ids_[i]);
d->Lyy.topLeftCorner(ndx, ndx).noalias() =
......@@ -405,7 +403,7 @@ void IntegratedActionModelLPFTpl<Scalar>::calcDiff(
d->Lyy.block(ndx, 0, ntau_, ndx).row(i).noalias() =
time_step_ *
d->differential->Lxu.transpose().row(lpf_torque_ids_[i]);
for (int j = 0; j < lpf_torque_ids_.size(); j++) {
for (std::size_t j = 0; j < lpf_torque_ids_.size(); j++) {
d->Lyy.bottomRightCorner(ntau_, ntau_)(i, j) =
time_step_ *
d->differential->Luu(lpf_torque_ids_[i], lpf_torque_ids_[j]);
......@@ -422,7 +420,7 @@ void IntegratedActionModelLPFTpl<Scalar>::calcDiff(
d->Lww.diagonal().array()(lpf_torque_ids_) =
Scalar(time_step_ * tauReg_weight_); // tau reg
#else
for (int i = 0; i < lpf_torque_ids_.size(); i++) {
for (std::size_t i = 0; i < lpf_torque_ids_.size(); i++) {
d->Lw(lpf_torque_ids_[i]) =
time_step_ * tauReg_weight_ *
d->r(differential_->get_nr() + i); // tau reg
......@@ -439,7 +437,7 @@ void IntegratedActionModelLPFTpl<Scalar>::calcDiff(
time_step_ * tauLim_weight_ *
d->activation->Arr.diagonal(); // tau lim
#else
for (int i = 0; i < lpf_torque_ids_.size(); i++) {
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]) +=
......@@ -465,14 +463,14 @@ void IntegratedActionModelLPFTpl<Scalar>::calcDiff(
d->Lyy.bottomRightCorner(ntau_, ntau_).noalias() =
d->differential->Luu(lpf_torque_ids_, lpf_torque_ids_);
#else
for (int i = 0; i < lpf_torque_ids_.size(); i++) {
for (std::size_t i = 0; i < lpf_torque_ids_.size(); i++) {
d->Ly.tail(ntau_)(i) = d->differential->Lu(lpf_torque_ids_[i]);
d->Lyy.topLeftCorner(ndx, ndx).noalias() = d->differential->Lxx;
d->Lyy.block(0, ndx, ndx, ntau_).col(i).noalias() =
d->differential->Lxu.col(lpf_torque_ids_[i]);
d->Lyy.block(ndx, 0, ntau_, ndx).row(i).noalias() =
d->differential->Lxu.transpose().row(lpf_torque_ids_[i]);
for (int j = 0; j < lpf_torque_ids_.size(); j++) {
for (std::size_t j = 0; j < lpf_torque_ids_.size(); j++) {
d->Lyy.bottomRightCorner(ntau_, ntau_)(i, j) =
d->differential->Luu(lpf_torque_ids_[i], lpf_torque_ids_[j]);
}
......@@ -508,14 +506,14 @@ void IntegratedActionModelLPFTpl<Scalar>::calcDiff(
d->Fy.block(nv, ndx, nv, ntau_).noalias() =
alpha_ * da_du(Eigen::all, lpf_torque_ids_) * time_step_;
#else
for (int i = 0; i < lpf_torque_ids_.size(); i++) {
for (std::size_t i = 0; i < lpf_torque_ids_.size(); i++) {
d->Fy.block(0, ndx, nv, ntau_).col(i).noalias() =
alpha_ * alpha_ * da_du.col(lpf_torque_ids_[i]) * time_step2_;
d->Fy.block(nv, ndx, nv, ntau_).col(i).noalias() =
alpha_ * da_du.col(lpf_torque_ids_[i]) * time_step_;
}
#endif
d->Fy.block(0, nq, nv, nv).diagonal().array() +=
d->Fy.block(0, nv, nv, nv).diagonal().array() +=
Scalar(time_step_); // dt*identity top row middle col (eq.
// Jsecond = d(xnext)/d(dx))
// d->Fy.topLeftCorner(nx, nx).diagonal().array() += Scalar(1.); //
......@@ -526,7 +524,7 @@ void IntegratedActionModelLPFTpl<Scalar>::calcDiff(
d->Fw.block(nv, 0, nv, ntau_).noalias() =
da_du(Eigen::all, lpf_torque_ids_) * time_step_ * (1 - alpha_);
#else
for (int i = 0; i < lpf_torque_ids_.size(); i++) {
for (std::size_t i = 0; i < lpf_torque_ids_.size(); i++) {
d->Fw.block(nv, 0, nv, ntau_).col(i).noalias() =
da_du.col(lpf_torque_ids_[i]) * time_step_ * (1 - alpha_);
}
......@@ -544,7 +542,7 @@ void IntegratedActionModelLPFTpl<Scalar>::calcDiff(
alpha_ * time_step_ * d->differential->Lu(lpf_torque_ids_);
#else
for (int i = 0; i < lpf_torque_ids_.size(); i++) {
for (std::size_t i = 0; i < lpf_torque_ids_.size(); i++) {
d->Ly.tail(ntau_)(i) =
alpha_ * time_step_ * d->differential->Lu(lpf_torque_ids_[i]);
}
......@@ -569,7 +567,7 @@ void IntegratedActionModelLPFTpl<Scalar>::calcDiff(
(1 - alpha_) * alpha_ * time_step_ *
d->differential->Luu(lpf_torque_ids_, lpf_torque_ids_);
#else
for (int i = 0; i < lpf_torque_ids_.size(); i++) {
for (std::size_t i = 0; i < lpf_torque_ids_.size(); i++) {
d->Lyy.block(0, ndx, ndx, ntau_).col(i).noalias() =
alpha_ * time_step_ * d->differential->Lxu.col(lpf_torque_ids_[i]);
d->Lyy.block(ndx, 0, ntau_, ndx).row(i).noalias() =
......@@ -578,7 +576,7 @@ void IntegratedActionModelLPFTpl<Scalar>::calcDiff(
d->Lyw.topRows(ndx).col(i).noalias() =
(1 - alpha_) * time_step_ *
d->differential->Lxu.col(lpf_torque_ids_[i]);
for (int j = 0; j < lpf_torque_ids_.size(); j++) {
for (std::size_t j = 0; j < lpf_torque_ids_.size(); j++) {
d->Lyy.bottomRightCorner(ntau_, ntau_)(i, j) =
alpha_ * alpha_ * time_step_ *
d->differential->Luu(lpf_torque_ids_[i], lpf_torque_ids_[j]);
......@@ -627,7 +625,7 @@ void IntegratedActionModelLPFTpl<Scalar>::calcDiff(
(1 - alpha_) * alpha_ *
d->differential->Luu(lpf_torque_ids_, lpf_torque_ids_);
#else
for (int i = 0; i < lpf_torque_ids_.size(); i++) {
for (std::size_t i = 0; i < lpf_torque_ids_.size(); i++) {
d->Ly.tail(ntau_)(i) = alpha_ * d->differential->Lu(lpf_torque_ids_[i]);
d->Lw.noalias() = (1 - alpha_) * d->differential->Lu;
d->Lyy.topLeftCorner(ndx, ndx).noalias() = d->differential->Lxx;
......@@ -636,7 +634,7 @@ void IntegratedActionModelLPFTpl<Scalar>::calcDiff(
d->Lyy.block(ndx, 0, ntau_, ndx).row(i).noalias() =
alpha_ * d->differential->Lxu.transpose().row(lpf_torque_ids_[i]);
d->Lyw.topRows(ndx).noalias() = (1 - alpha_) * d->differential->Lxu;
for (int j = 0; j < lpf_torque_ids_.size(); j++) {
for (std::size_t j = 0; j < lpf_torque_ids_.size(); j++) {
d->Lyy.bottomRightCorner(ntau_, ntau_)(i, j) =
alpha_ * alpha_ *
d->differential->Luu(lpf_torque_ids_[i], lpf_torque_ids_[j]);
......@@ -715,6 +713,17 @@ void IntegratedActionModelLPFTpl<Scalar>::set_fc(const Scalar& fc) {
}
}
template <typename Scalar>
void IntegratedActionModelLPFTpl<Scalar>::set_alpha(const Scalar& alpha) {
// Set the cut-off frequency
if (alpha < 0. || alpha > 1) {
throw_pretty("Invalid argument: "
<< "alpha must be in [0,1]");
} else {
alpha_ = alpha;
}
}
template <typename Scalar>
void IntegratedActionModelLPFTpl<Scalar>::compute_alpha(const Scalar& fc) {
// Update alpha parameter
......@@ -758,10 +767,10 @@ void IntegratedActionModelLPFTpl<Scalar>::set_differential(
template <typename Scalar>
void IntegratedActionModelLPFTpl<Scalar>::set_control_reg_cost(
const Scalar& weight, const VectorXs& ref) {
if (weight <= 0.) {
if (weight < 0.) {
throw_pretty("cost weight is positive ");
}
if (ref.size() != ntau_) {
if ((std::size_t)ref.size() != (std::size_t)(ntau_)) {
throw_pretty("cost ref must have size " << ntau_);
}
tauReg_weight_ = weight;
......@@ -771,7 +780,7 @@ void IntegratedActionModelLPFTpl<Scalar>::set_control_reg_cost(
template <typename Scalar>
void IntegratedActionModelLPFTpl<Scalar>::set_control_lim_cost(
const Scalar& weight) {
if (weight <= 0.) {
if (weight < 0.) {
throw_pretty("cost weight is positive ");
}
tauLim_weight_ = weight;
......
......@@ -48,7 +48,7 @@ StateLPFTpl<Scalar>::StateLPFTpl(
// -pinocchio_->effortLimit.tail[model->idx_vs[lpf_joint_ids[i]]];
// ub_.tail(ntau_) =
// pinocchio_->effortLimit.tail(nw_)[model->idx_vs[lpf_joint_ids[i]]];
for (int i = 0; i < lpf_joint_ids.size(); i++) {
for (std::size_t i = 0; i < lpf_joint_ids.size(); i++) {
if ((int)model->nvs[lpf_joint_ids[i]] != (int)1) {
throw_pretty("Invalid argument: "
<< "Joint " << lpf_joint_ids[i]
......
......@@ -27,6 +27,9 @@ std::ostream& operator<<(std::ostream& os, ActionModelLPFTypes::Type type) {
case ActionModelLPFTypes::IntegratedActionModelLPF_NONE:
os << "IntegratedActionModelLPF_NONE";
break;
case ActionModelLPFTypes::IntegratedActionModelLPF_alpha0:
os << "IntegratedActionModelLPF_alpha0";
break;
case ActionModelLPFTypes::NbActionModelLPFTypes:
os << "NbActionModelLPFTypes";
break;
......@@ -127,6 +130,34 @@ ActionModelLPFFactory::create(ActionModelLPFTypes::Type iam_type,
iam->set_control_lim_cost(tauLim_weight);
break;
}
case ActionModelLPFTypes::IntegratedActionModelLPF_alpha0: {
double time_step = 1e-6;
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>(
dam->get_state());
boost::shared_ptr<pinocchio::Model> model =
stateMultibody->get_pinocchio();
std::vector<std::string> lpf_joint_names =
LPFJointListFactory().create_names(model, LPFJointMaskType::ALL);
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.001;
Eigen::VectorXd tauReg_ref = Eigen::VectorXd::Zero(iam->get_ntau());
double tauLim_weight = 0.1;
iam->set_control_reg_cost(tauReg_weight, tauReg_ref);
iam->set_control_lim_cost(tauLim_weight);
iam->set_alpha(alpha);
break;
}
default:
throw_pretty(__FILE__ ": Wrong ActionModelLPFTypes::Type given");
break;
......
......@@ -24,7 +24,7 @@ struct ActionModelLPFTypes {
IntegratedActionModelLPF_ALL,
IntegratedActionModelLPF_RAND,
IntegratedActionModelLPF_NONE,
// IntegratedActionModelLPF_zero_costs,
IntegratedActionModelLPF_alpha0,
// IntegratedActionModelLPF_terminal,
NbActionModelLPFTypes
};
......
......@@ -34,7 +34,7 @@ std::vector<std::string> LPFJointListFactory::create_names(
case LPFJointMaskType::ALL: {
for (std::vector<std::string>::iterator iter = model->names.begin();
iter != model->names.end(); ++iter) {
if (model->getJointId(*iter) < model->njoints &&
if ((int)model->getJointId(*iter) < model->njoints &&
model->nvs[model->getJointId(*iter)] == 1) {
lpf_joint_names.push_back(*iter);
}
......@@ -62,9 +62,9 @@ std::vector<int> LPFJointListFactory::create_ids(
case LPFJointMaskType::ALL: {
for (std::vector<std::string>::iterator iter = model->names.begin();
iter != model->names.end(); ++iter) {
if (model->getJointId(*iter) < model->njoints &&
if ((int)model->getJointId(*iter) < model->njoints &&
model->nvs[model->getJointId(*iter)] == 1) {
lpf_joint_ids.push_back(model->getJointId(*iter));
lpf_joint_ids.push_back((int)model->getJointId(*iter));
}
}
break;
......
......@@ -15,6 +15,8 @@
#include "factory/diff-action.hpp"
#include "factory/lpf.hpp"
#include <crocoddyl/core/integrator/euler.hpp>
using namespace boost::unit_test;
using namespace sobec::unittest;
......@@ -155,6 +157,67 @@ void test_partial_derivatives_action_model(
test_partial_derivatives_against_numdiff(model);
}
void test_calc_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
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();
// Checking the partial derivatives against NumDiff
double tol = 1e-2;
// 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;
}
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));
// 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));
}
//----------------------------------------------------------------------------//
void register_action_model_unit_tests(
......@@ -184,6 +247,10 @@ void register_action_model_unit_tests(
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)));
}
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