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

finished ActionDataLPF->Lyw + unittests

parent cf17860d
......@@ -453,16 +453,18 @@ void IntegratedActionModelLPFTpl<Scalar>::calcDiff(
#if EIGEN_VERSION_AT_LEAST(3, 4, 0)
d->Lw(non_lpf_torque_ids_).noalias() = time_step_ * d->differential->Lu(non_lpf_torque_ids_);
d->Lyw.topRows(ndx)(Eigen::all, non_lpf_torque_ids_).noalias() = time_step_ * d->differential->Lxu(Eigen::all, non_lpf_torque_ids_);
// d->Lyw.bottomRows(ntau_)(Eigen::all, non_lpf_torque_ids_).noalias() = time_step_ * d->differential->Luu(non_lpf_torque_ids_, non_lpf_torque_ids_);
d->Lyw.bottomRows(ntau_)(Eigen::all, non_lpf_torque_ids_).noalias() = time_step_ * d->differential->Luu(lpf_torque_ids_, non_lpf_torque_ids_);
d->Lww(non_lpf_torque_ids_, non_lpf_torque_ids_).noalias() = time_step_ * d->differential->Luu(non_lpf_torque_ids_, non_lpf_torque_ids_);
#else
for (std::size_t i = 0; i < non_lpf_torque_ids_.size(); i++) {
d->Lw(non_lpf_torque_ids_[i]) = time_step_ * d->differential->Lu(non_lpf_torque_ids_[i]);
d->Lyw.topRows(ndx).col(non_lpf_torque_ids_[i]).noalias() = time_step_ * d->differential->Lxu.col(non_lpf_torque_ids_[i]);
// d->Lyw.bottomRows(ntau_).col(non_lpf_torque_ids_[i]).noalias() = time_step_ * d->differential->Luu.col(non_lpf_torque_ids_[i]);
for (std::size_t j = 0; j < non_lpf_torque_ids_.size(); j++) {
d->Lww(non_lpf_torque_ids_[i], non_lpf_torque_ids_[j]) = time_step_ * d->differential->Luu(non_lpf_torque_ids_[i], non_lpf_torque_ids_[j]);
}
for (std::size_t j = 0; j < lpf_torque_ids_.size(); j++) {
d->Lyw.bottomRows(ntau_)(j, non_lpf_torque_ids_[i]) = time_step_ * d->differential->Luu(lpf_torque_ids_[j], non_lpf_torque_ids_[i]);
}
}
#endif
......@@ -719,18 +721,20 @@ void IntegratedActionModelLPFTpl<Scalar>::calcDiff(
d->Lyw.setZero();
d->Lww.setZero();
#if EIGEN_VERSION_AT_LEAST(3, 4, 0)
d->Lw(non_lpf_torque_ids_).noalias() = d->differential->Lu(non_lpf_torque_ids_);
d->Lyw.topRows(ndx)(Eigen::all, non_lpf_torque_ids_).noalias() = d->differential->Lxu(Eigen::all, non_lpf_torque_ids_);
// d->Lyw.bottomRows(ntau_)(Eigen::all, non_lpf_torque_ids_).noalias() = d->differential->Luu(non_lpf_torque_ids_, non_lpf_torque_ids_);
d->Lw(non_lpf_torque_ids_).noalias() = d->differential->Lu(non_lpf_torque_ids_);
d->Lyw.topRows(ndx)(Eigen::all, non_lpf_torque_ids_).noalias() = d->differential->Lxu(Eigen::all, non_lpf_torque_ids_);
d->Lyw.bottomRows(ntau_)(Eigen::all, non_lpf_torque_ids_).noalias() = d->differential->Luu(lpf_torque_ids_, non_lpf_torque_ids_);
d->Lww(non_lpf_torque_ids_, non_lpf_torque_ids_).noalias() = d->differential->Luu(non_lpf_torque_ids_, non_lpf_torque_ids_);
#else
for (std::size_t i = 0; i < non_lpf_torque_ids_.size(); i++) {
d->Lw(non_lpf_torque_ids_[i]) = d->differential->Lu(non_lpf_torque_ids_[i]);
d->Lyw.topRows(ndx).col(non_lpf_torque_ids_[i]).noalias() = d->differential->Lxu.col(non_lpf_torque_ids_[i]);
// d->Lyw.bottomRows(ntau_).row(i).noalias() = d->differential->Luu.row(non_lpf_torque_ids_[i]);
for (std::size_t j = 0; j < non_lpf_torque_ids_.size(); j++) {
d->Lww(non_lpf_torque_ids_[i], non_lpf_torque_ids_[j]) = d->differential->Luu(non_lpf_torque_ids_[i], non_lpf_torque_ids_[j]);
}
for (std::size_t j = 0; j < lpf_torque_ids_.size(); j++) {
d->Lyw.bottomRows(ntau_)(j, non_lpf_torque_ids_[i]) = d->differential->Luu(lpf_torque_ids_[j], non_lpf_torque_ids_[i]);
}
}
#endif
......
......@@ -397,15 +397,18 @@ void test_calcDiff_explicit_equivalent_euler(
// 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& Lxu_nonLPF = dataLPF->Lxu;
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((Lxu_nonLPF.topRows(ndx).col(non_lpf_torque_ids[i]) - dataEuler->Lxu.col(non_lpf_torque_ids[i])).isZero(tol));
for(std::size_t j=0; j<non_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 );
}
for(std::size_t j=0; j<lpf_torque_ids.size();j++){
BOOST_CHECK((Lxu_nonLPF.bottomRows(ntau)(j, non_lpf_torque_ids[i]) - dataEuler->Luu(lpf_torque_ids[j], non_lpf_torque_ids[i])) <= tol );
}
}
}
......
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