Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/crocoddyl/multibody/impulses/multiple-impulses.hxx	Lines:	171	198	86.4 %
Date:	2024-02-13 11:12:33	Branches:	119	443	26.9 %


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2019-2023, LAAS-CNRS, University of Edinburgh,
//                          Heriot-Watt University
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////

#include "crocoddyl/core/utils/exception.hpp"
#include "crocoddyl/multibody/impulses/multiple-impulses.hpp"

namespace crocoddyl {

template <typename Scalar>
ImpulseModelMultipleTpl<Scalar>::ImpulseModelMultipleTpl(
    boost::shared_ptr<StateMultibody> state)
    : state_(state), nc_(0), nc_total_(0) {}

template <typename Scalar>
ImpulseModelMultipleTpl<Scalar>::~ImpulseModelMultipleTpl() {}

template <typename Scalar>
void ImpulseModelMultipleTpl<Scalar>::addImpulse(
    const std::string& name, boost::shared_ptr<ImpulseModelAbstract> impulse,
    const bool active) {
  std::pair<typename ImpulseModelContainer::iterator, bool> ret =
      impulses_.insert(std::make_pair(
          name, boost::make_shared<ImpulseItem>(name, impulse, active)));
  if (ret.second == false) {
    std::cerr << "Warning: we couldn't add the " << name
              << " impulse item, it already existed." << std::endl;
  } else if (active) {
    nc_ += impulse->get_nc();
    nc_total_ += impulse->get_nc();
    active_set_.insert(name);
  } else if (!active) {
    nc_total_ += impulse->get_nc();
    inactive_set_.insert(name);
  }
}

template <typename Scalar>
void ImpulseModelMultipleTpl<Scalar>::removeImpulse(const std::string& name) {
  typename ImpulseModelContainer::iterator it = impulses_.find(name);
  if (it != impulses_.end()) {
    nc_ -= it->second->impulse->get_nc();
    nc_total_ -= it->second->impulse->get_nc();
    impulses_.erase(it);
    active_set_.erase(name);
    inactive_set_.erase(name);
  } else {
    std::cerr << "Warning: we couldn't remove the " << name
              << " impulse item, it doesn't exist." << std::endl;
  }
}

template <typename Scalar>
void ImpulseModelMultipleTpl<Scalar>::changeImpulseStatus(
    const std::string& name, const bool active) {
  typename ImpulseModelContainer::iterator it = impulses_.find(name);
  if (it != impulses_.end()) {
    if (active && !it->second->active) {
      nc_ += it->second->impulse->get_nc();
      active_set_.insert(name);
      inactive_set_.erase(name);
    } else if (!active && it->second->active) {
      nc_ -= it->second->impulse->get_nc();
      inactive_set_.insert(name);
      active_set_.erase(name);
    }
    it->second->active = active;
  } else {
    std::cerr << "Warning: we couldn't change the status of the " << name
              << " impulse item, it doesn't exist." << std::endl;
  }
}

template <typename Scalar>
void ImpulseModelMultipleTpl<Scalar>::calc(
    const boost::shared_ptr<ImpulseDataMultiple>& data,
    const Eigen::Ref<const VectorXs>& x) {
  if (data->impulses.size() != impulses_.size()) {
    throw_pretty("Invalid argument: "
                 << "it doesn't match the number of impulse datas and models");
  }

  std::size_t nc = 0;
  const std::size_t nv = state_->get_nv();
  typename ImpulseModelContainer::iterator it_m, end_m;
  typename ImpulseDataContainer::iterator it_d, end_d;
  for (it_m = impulses_.begin(), end_m = impulses_.end(),
      it_d = data->impulses.begin(), end_d = data->impulses.end();
       it_m != end_m || it_d != end_d; ++it_m, ++it_d) {
    const boost::shared_ptr<ImpulseItem>& m_i = it_m->second;
    if (m_i->active) {
      const boost::shared_ptr<ImpulseDataAbstract>& d_i = it_d->second;
      assert_pretty(it_m->first == it_d->first,
                    "it doesn't match the impulse name between model and data ("
                        << it_m->first << " != " << it_d->first << ")");

      m_i->impulse->calc(d_i, x);
      const std::size_t nc_i = m_i->impulse->get_nc();
      data->Jc.block(nc, 0, nc_i, nv) = d_i->Jc;
      nc += nc_i;
    }
  }
}

template <typename Scalar>
void ImpulseModelMultipleTpl<Scalar>::calcDiff(
    const boost::shared_ptr<ImpulseDataMultiple>& data,
    const Eigen::Ref<const VectorXs>& x) {
  if (data->impulses.size() != impulses_.size()) {
    throw_pretty("Invalid argument: "
                 << "it doesn't match the number of impulse datas and models");
  }

  std::size_t nc = 0;
  const std::size_t nv = state_->get_nv();
  typename ImpulseModelContainer::iterator it_m, end_m;
  typename ImpulseDataContainer::iterator it_d, end_d;
  for (it_m = impulses_.begin(), end_m = impulses_.end(),
      it_d = data->impulses.begin(), end_d = data->impulses.end();
       it_m != end_m || it_d != end_d; ++it_m, ++it_d) {
    const boost::shared_ptr<ImpulseItem>& m_i = it_m->second;
    if (m_i->active) {
      const boost::shared_ptr<ImpulseDataAbstract>& d_i = it_d->second;
      assert_pretty(it_m->first == it_d->first,
                    "it doesn't match the impulse name between model and data ("
                        << it_m->first << " != " << it_d->first << ")");

      m_i->impulse->calcDiff(d_i, x);
      const std::size_t nc_i = m_i->impulse->get_nc();
      data->dv0_dq.block(nc, 0, nc_i, nv) = d_i->dv0_dq;
      nc += nc_i;
    }
  }
}

template <typename Scalar>
void ImpulseModelMultipleTpl<Scalar>::updateVelocity(
    const boost::shared_ptr<ImpulseDataMultiple>& data,
    const VectorXs& vnext) const {
  if (static_cast<std::size_t>(vnext.size()) != state_->get_nv()) {
    throw_pretty("Invalid argument: "
                 << "vnext has wrong dimension (it should be " +
                        std::to_string(state_->get_nv()) + ")");
  }
  data->vnext = vnext;
}

template <typename Scalar>
void ImpulseModelMultipleTpl<Scalar>::updateForce(
    const boost::shared_ptr<ImpulseDataMultiple>& data, const VectorXs& force) {
  if (static_cast<std::size_t>(force.size()) != nc_) {
    throw_pretty("Invalid argument: "
                 << "force has wrong dimension (it should be " +
                        std::to_string(nc_) + ")");
  }
  if (static_cast<std::size_t>(data->impulses.size()) != impulses_.size()) {
    throw_pretty("Invalid argument: "
                 << "it doesn't match the number of impulse datas and models");
  }

  for (ForceIterator it = data->fext.begin(); it != data->fext.end(); ++it) {
    *it = pinocchio::ForceTpl<Scalar>::Zero();
  }

  std::size_t nc = 0;
  typename ImpulseModelContainer::iterator it_m, end_m;
  typename ImpulseDataContainer::iterator it_d, end_d;
  for (it_m = impulses_.begin(), end_m = impulses_.end(),
      it_d = data->impulses.begin(), end_d = data->impulses.end();
       it_m != end_m || it_d != end_d; ++it_m, ++it_d) {
    const boost::shared_ptr<ImpulseItem>& m_i = it_m->second;
    const boost::shared_ptr<ImpulseDataAbstract>& d_i = it_d->second;
    assert_pretty(it_m->first == it_d->first,
                  "it doesn't match the impulse name between data and model");
    if (m_i->active) {
      const std::size_t nc_i = m_i->impulse->get_nc();
      const Eigen::VectorBlock<const VectorXs, Eigen::Dynamic> force_i =
          force.segment(nc, nc_i);
      m_i->impulse->updateForce(d_i, force_i);
      const pinocchio::JointIndex joint =
          state_->get_pinocchio()->frames[d_i->frame].parent;
      data->fext[joint] = d_i->fext;
      nc += nc_i;
    } else {
      m_i->impulse->setZeroForce(d_i);
    }
  }
}

template <typename Scalar>
void ImpulseModelMultipleTpl<Scalar>::updateVelocityDiff(
    const boost::shared_ptr<ImpulseDataMultiple>& data,
    const MatrixXs& dvnext_dx) const {
  if (static_cast<std::size_t>(dvnext_dx.rows()) != state_->get_nv() ||
      static_cast<std::size_t>(dvnext_dx.cols()) != state_->get_ndx()) {
    throw_pretty("Invalid argument: "
                 << "dvnext_dx has wrong dimension (it should be " +
                        std::to_string(state_->get_nv()) + "," +
                        std::to_string(state_->get_ndx()) + ")");
  }
  data->dvnext_dx = dvnext_dx;
}

template <typename Scalar>
void ImpulseModelMultipleTpl<Scalar>::updateForceDiff(
    const boost::shared_ptr<ImpulseDataMultiple>& data,
    const MatrixXs& df_dx) const {
  const std::size_t ndx = state_->get_ndx();
  if (static_cast<std::size_t>(df_dx.rows()) != nc_ ||
      static_cast<std::size_t>(df_dx.cols()) != ndx) {
    throw_pretty("Invalid argument: "
                 << "df_dx has wrong dimension (it should be " +
                        std::to_string(nc_) + "," + std::to_string(ndx) + ")");
  }
  if (static_cast<std::size_t>(data->impulses.size()) != impulses_.size()) {
    throw_pretty("Invalid argument: "
                 << "it doesn't match the number of impulse datas and models");
  }

  std::size_t nc = 0;
  typename ImpulseModelContainer::const_iterator it_m, end_m;
  typename ImpulseDataContainer::const_iterator it_d, end_d;
  for (it_m = impulses_.begin(), end_m = impulses_.end(),
      it_d = data->impulses.begin(), end_d = data->impulses.end();
       it_m != end_m || it_d != end_d; ++it_m, ++it_d) {
    const boost::shared_ptr<ImpulseItem>& m_i = it_m->second;
    const boost::shared_ptr<ImpulseDataAbstract>& d_i = it_d->second;
    assert_pretty(it_m->first == it_d->first,
                  "it doesn't match the impulse name between data and model");
    if (m_i->active) {
      const std::size_t nc_i = m_i->impulse->get_nc();
      const Eigen::Block<const MatrixXs> df_dx_i =
          df_dx.block(nc, 0, nc_i, ndx);
      m_i->impulse->updateForceDiff(d_i, df_dx_i);
      nc += nc_i;
    } else {
      m_i->impulse->setZeroForceDiff(d_i);
    }
  }
}

template <typename Scalar>
void ImpulseModelMultipleTpl<Scalar>::updateRneaDiff(
    const boost::shared_ptr<ImpulseDataMultiple>& data,
    pinocchio::DataTpl<Scalar>& pinocchio) const {
  if (static_cast<std::size_t>(data->impulses.size()) !=
      this->get_impulses().size()) {
    throw_pretty("Invalid argument: "
                 << "it doesn't match the number of impulse datas and models");
  }
  typename ImpulseModelContainer::const_iterator it_m, end_m;
  typename ImpulseDataContainer::const_iterator it_d, end_d;
  for (it_m = impulses_.begin(), end_m = impulses_.end(),
      it_d = data->impulses.begin(), end_d = data->impulses.end();
       it_m != end_m || it_d != end_d; ++it_m, ++it_d) {
    const boost::shared_ptr<ImpulseItem>& m_i = it_m->second;
    const boost::shared_ptr<ImpulseDataAbstract>& d_i = it_d->second;
    assert_pretty(it_m->first == it_d->first,
                  "it doesn't match the impulse name between data and model");
    if (m_i->active) {
      switch (m_i->impulse->get_type()) {
        case pinocchio::ReferenceFrame::LOCAL:
          break;
        case pinocchio::ReferenceFrame::WORLD:
        case pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED:
          pinocchio.dtau_dq += d_i->dtau_dq;
          break;
      }
    }
  }
}

template <typename Scalar>
boost::shared_ptr<ImpulseDataMultipleTpl<Scalar> >
ImpulseModelMultipleTpl<Scalar>::createData(
    pinocchio::DataTpl<Scalar>* const data) {
  return boost::allocate_shared<ImpulseDataMultiple>(
      Eigen::aligned_allocator<ImpulseDataMultiple>(), this, data);
}

template <typename Scalar>
const boost::shared_ptr<StateMultibodyTpl<Scalar> >&
ImpulseModelMultipleTpl<Scalar>::get_state() const {
  return state_;
}

template <typename Scalar>
const typename ImpulseModelMultipleTpl<Scalar>::ImpulseModelContainer&
ImpulseModelMultipleTpl<Scalar>::get_impulses() const {
  return impulses_;
}

template <typename Scalar>
std::size_t ImpulseModelMultipleTpl<Scalar>::get_nc() const {
  return nc_;
}

template <typename Scalar>
std::size_t ImpulseModelMultipleTpl<Scalar>::get_nc_total() const {
  return nc_total_;
}

template <typename Scalar>
const std::set<std::string>& ImpulseModelMultipleTpl<Scalar>::get_active_set()
    const {
  return active_set_;
}

template <typename Scalar>
const std::set<std::string>& ImpulseModelMultipleTpl<Scalar>::get_inactive_set()
    const {
  return inactive_set_;
}

template <typename Scalar>
bool ImpulseModelMultipleTpl<Scalar>::getImpulseStatus(
    const std::string& name) const {
  typename ImpulseModelContainer::const_iterator it = impulses_.find(name);
  if (it != impulses_.end()) {
    return it->second->active;
  } else {
    std::cerr << "Warning: we couldn't get the status of the " << name
              << " impulse item, it doesn't exist." << std::endl;
    return false;
  }
}

template <class Scalar>
std::ostream& operator<<(std::ostream& os,
                         const ImpulseModelMultipleTpl<Scalar>& model) {
  const auto& active = model.get_active_set();
  const auto& inactive = model.get_inactive_set();
  os << "ImpulseModelMultiple:" << std::endl;
  os << "  Active:" << std::endl;
  for (std::set<std::string>::const_iterator it = active.begin();
       it != active.end(); ++it) {
    const boost::shared_ptr<
        typename ImpulseModelMultipleTpl<Scalar>::ImpulseItem>& impulse_item =
        model.get_impulses().find(*it)->second;
    if (it != --active.end()) {
      os << "    " << *it << ": " << *impulse_item << std::endl;
    } else {
      os << "    " << *it << ": " << *impulse_item << std::endl;
    }
  }
  os << "  Inactive:" << std::endl;
  for (std::set<std::string>::const_iterator it = inactive.begin();
       it != inactive.end(); ++it) {
    const boost::shared_ptr<
        typename ImpulseModelMultipleTpl<Scalar>::ImpulseItem>& impulse_item =
        model.get_impulses().find(*it)->second;
    if (it != --inactive.end()) {
      os << "    " << *it << ": " << *impulse_item << std::endl;
    } else {
      os << "    " << *it << ": " << *impulse_item;
    }
  }
  return os;
}

}  // namespace crocoddyl


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			///////////////////////////////////////////////////////////////////////////////
2			// BSD 3-Clause License
3			//
4			// Copyright (C) 2019-2023, LAAS-CNRS, University of Edinburgh,
5			// Heriot-Watt University
6			// Copyright note valid unless otherwise stated in individual files.
7			// All rights reserved.
8			///////////////////////////////////////////////////////////////////////////////
9
10			#include "crocoddyl/core/utils/exception.hpp"
11			#include "crocoddyl/multibody/impulses/multiple-impulses.hpp"
12
13			namespace crocoddyl {
14
15			template <typename Scalar>
16		97	ImpulseModelMultipleTpl<Scalar>::ImpulseModelMultipleTpl(
17			boost::shared_ptr<StateMultibody> state)
18		97	: state_(state), nc_(0), nc_total_(0) {}
19
20			template <typename Scalar>
21		99	ImpulseModelMultipleTpl<Scalar>::~ImpulseModelMultipleTpl() {}
22
23			template <typename Scalar>
24		254	void ImpulseModelMultipleTpl<Scalar>::addImpulse(
25			const std::string& name, boost::shared_ptr<ImpulseModelAbstract> impulse,
26			const bool active) {
27	✓✗✓✗ ✓✗	254	std::pair<typename ImpulseModelContainer::iterator, bool> ret =
28			impulses_.insert(std::make_pair(
29			name, boost::make_shared<ImpulseItem>(name, impulse, active)));
30	✓✓	254	if (ret.second == false) {
31			std::cerr << "Warning: we couldn't add the " << name
32	✓✗✓✗ ✓✗✓✗	1	<< " impulse item, it already existed." << std::endl;
33	✓✓	253	} else if (active) {
34		252	nc_ += impulse->get_nc();
35		252	nc_total_ += impulse->get_nc();
36	✓✗	252	active_set_.insert(name);
37	✓✗	1	} else if (!active) {
38		1	nc_total_ += impulse->get_nc();
39	✓✗	1	inactive_set_.insert(name);
40			}
41		254	}
42
43			template <typename Scalar>
44		2	void ImpulseModelMultipleTpl<Scalar>::removeImpulse(const std::string& name) {
45	✓✗	2	typename ImpulseModelContainer::iterator it = impulses_.find(name);
46	✓✓	2	if (it != impulses_.end()) {
47		1	nc_ -= it->second->impulse->get_nc();
48		1	nc_total_ -= it->second->impulse->get_nc();
49	✓✗	1	impulses_.erase(it);
50	✓✗	1	active_set_.erase(name);
51	✓✗	1	inactive_set_.erase(name);
52			} else {
53			std::cerr << "Warning: we couldn't remove the " << name
54	✓✗✓✗ ✓✗✓✗	1	<< " impulse item, it doesn't exist." << std::endl;
55			}
56		2	}
57
58			template <typename Scalar>
59		9	void ImpulseModelMultipleTpl<Scalar>::changeImpulseStatus(
60			const std::string& name, const bool active) {
61	✓✗	9	typename ImpulseModelContainer::iterator it = impulses_.find(name);
62	✓✓	9	if (it != impulses_.end()) {
63	✓✓✓✗ ✓✓	8	if (active && !it->second->active) {
64		1	nc_ += it->second->impulse->get_nc();
65	✓✗	1	active_set_.insert(name);
66	✓✗	1	inactive_set_.erase(name);
67	✓✗✓✗ ✓✗	7	} else if (!active && it->second->active) {
68		7	nc_ -= it->second->impulse->get_nc();
69	✓✗	7	inactive_set_.insert(name);
70	✓✗	7	active_set_.erase(name);
71			}
72		8	it->second->active = active;
73			} else {
74			std::cerr << "Warning: we couldn't change the status of the " << name
75	✓✗✓✗ ✓✗✓✗	1	<< " impulse item, it doesn't exist." << std::endl;
76			}
77		9	}
78
79			template <typename Scalar>
80		6988	void ImpulseModelMultipleTpl<Scalar>::calc(
81			const boost::shared_ptr<ImpulseDataMultiple>& data,
82			const Eigen::Ref<const VectorXs>& x) {
83	✗✓	6988	if (data->impulses.size() != impulses_.size()) {
84			throw_pretty("Invalid argument: "
85			<< "it doesn't match the number of impulse datas and models");
86			}
87
88		6988	std::size_t nc = 0;
89		6988	const std::size_t nv = state_->get_nv();
90		6988	typename ImpulseModelContainer::iterator it_m, end_m;
91		6988	typename ImpulseDataContainer::iterator it_d, end_d;
92		23464	for (it_m = impulses_.begin(), end_m = impulses_.end(),
93		6988	it_d = data->impulses.begin(), end_d = data->impulses.end();
94	✓✓✗✓ ✓✓	39940	it_m != end_m \|\| it_d != end_d; ++it_m, ++it_d) {
95		16476	const boost::shared_ptr<ImpulseItem>& m_i = it_m->second;
96	✓✓	16476	if (m_i->active) {
97		16472	const boost::shared_ptr<ImpulseDataAbstract>& d_i = it_d->second;
98	✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗	16472	assert_pretty(it_m->first == it_d->first,
99			"it doesn't match the impulse name between model and data ("
100			<< it_m->first << " != " << it_d->first << ")");
101
102	✓✗	16472	m_i->impulse->calc(d_i, x);
103		16472	const std::size_t nc_i = m_i->impulse->get_nc();
104	✓✗✓✗	16472	data->Jc.block(nc, 0, nc_i, nv) = d_i->Jc;
105		16472	nc += nc_i;
106			}
107			}
108		6988	}
109
110			template <typename Scalar>
111		254	void ImpulseModelMultipleTpl<Scalar>::calcDiff(
112			const boost::shared_ptr<ImpulseDataMultiple>& data,
113			const Eigen::Ref<const VectorXs>& x) {
114	✗✓	254	if (data->impulses.size() != impulses_.size()) {
115			throw_pretty("Invalid argument: "
116			<< "it doesn't match the number of impulse datas and models");
117			}
118
119		254	std::size_t nc = 0;
120		254	const std::size_t nv = state_->get_nv();
121		254	typename ImpulseModelContainer::iterator it_m, end_m;
122		254	typename ImpulseDataContainer::iterator it_d, end_d;
123		978	for (it_m = impulses_.begin(), end_m = impulses_.end(),
124		254	it_d = data->impulses.begin(), end_d = data->impulses.end();
125	✓✓✗✓ ✓✓	1702	it_m != end_m \|\| it_d != end_d; ++it_m, ++it_d) {
126		724	const boost::shared_ptr<ImpulseItem>& m_i = it_m->second;
127	✓✓	724	if (m_i->active) {
128		720	const boost::shared_ptr<ImpulseDataAbstract>& d_i = it_d->second;
129	✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗	720	assert_pretty(it_m->first == it_d->first,
130			"it doesn't match the impulse name between model and data ("
131			<< it_m->first << " != " << it_d->first << ")");
132
133	✓✗	720	m_i->impulse->calcDiff(d_i, x);
134		720	const std::size_t nc_i = m_i->impulse->get_nc();
135	✓✗✓✗	720	data->dv0_dq.block(nc, 0, nc_i, nv) = d_i->dv0_dq;
136		720	nc += nc_i;
137			}
138			}
139		254	}
140
141			template <typename Scalar>
142		6980	void ImpulseModelMultipleTpl<Scalar>::updateVelocity(
143			const boost::shared_ptr<ImpulseDataMultiple>& data,
144			const VectorXs& vnext) const {
145	✗✓	6980	if (static_cast<std::size_t>(vnext.size()) != state_->get_nv()) {
146			throw_pretty("Invalid argument: "
147			<< "vnext has wrong dimension (it should be " +
148			std::to_string(state_->get_nv()) + ")");
149			}
150		6980	data->vnext = vnext;
151		6980	}
152
153			template <typename Scalar>
154		6981	void ImpulseModelMultipleTpl<Scalar>::updateForce(
155			const boost::shared_ptr<ImpulseDataMultiple>& data, const VectorXs& force) {
156	✓✗✗✓	6981	if (static_cast<std::size_t>(force.size()) != nc_) {
157			throw_pretty("Invalid argument: "
158			<< "force has wrong dimension (it should be " +
159			std::to_string(nc_) + ")");
160			}
161	✗✓	6981	if (static_cast<std::size_t>(data->impulses.size()) != impulses_.size()) {
162			throw_pretty("Invalid argument: "
163			<< "it doesn't match the number of impulse datas and models");
164			}
165
166	✓✓	217469	for (ForceIterator it = data->fext.begin(); it != data->fext.end(); ++it) {
167	✓✗✓✗	210488	*it = pinocchio::ForceTpl<Scalar>::Zero();
168			}
169
170		6981	std::size_t nc = 0;
171		6981	typename ImpulseModelContainer::iterator it_m, end_m;
172		6981	typename ImpulseDataContainer::iterator it_d, end_d;
173		23434	for (it_m = impulses_.begin(), end_m = impulses_.end(),
174		6981	it_d = data->impulses.begin(), end_d = data->impulses.end();
175	✓✓✗✓ ✓✓	39887	it_m != end_m \|\| it_d != end_d; ++it_m, ++it_d) {
176		16453	const boost::shared_ptr<ImpulseItem>& m_i = it_m->second;
177		16453	const boost::shared_ptr<ImpulseDataAbstract>& d_i = it_d->second;
178	✗✓✗✗ ✗✗✗✗ ✗✗	16453	assert_pretty(it_m->first == it_d->first,
179			"it doesn't match the impulse name between data and model");
180	✓✗	16453	if (m_i->active) {
181		16453	const std::size_t nc_i = m_i->impulse->get_nc();
182	✓✗	16453	const Eigen::VectorBlock<const VectorXs, Eigen::Dynamic> force_i =
183			force.segment(nc, nc_i);
184	✓✗✓✗	16453	m_i->impulse->updateForce(d_i, force_i);
185		16453	const pinocchio::JointIndex joint =
186		16453	state_->get_pinocchio()->frames[d_i->frame].parent;
187	✓✗	16453	data->fext[joint] = d_i->fext;
188		16453	nc += nc_i;
189			} else {
190			m_i->impulse->setZeroForce(d_i);
191			}
192			}
193		6981	}
194
195			template <typename Scalar>
196		249	void ImpulseModelMultipleTpl<Scalar>::updateVelocityDiff(
197			const boost::shared_ptr<ImpulseDataMultiple>& data,
198			const MatrixXs& dvnext_dx) const {
199	✓✗✗✓ ✗✓	498	if (static_cast<std::size_t>(dvnext_dx.rows()) != state_->get_nv() \|\|
200		249	static_cast<std::size_t>(dvnext_dx.cols()) != state_->get_ndx()) {
201			throw_pretty("Invalid argument: "
202			<< "dvnext_dx has wrong dimension (it should be " +
203			std::to_string(state_->get_nv()) + "," +
204			std::to_string(state_->get_ndx()) + ")");
205			}
206		249	data->dvnext_dx = dvnext_dx;
207		249	}
208
209			template <typename Scalar>
210		248	void ImpulseModelMultipleTpl<Scalar>::updateForceDiff(
211			const boost::shared_ptr<ImpulseDataMultiple>& data,
212			const MatrixXs& df_dx) const {
213		248	const std::size_t ndx = state_->get_ndx();
214	✓✗✓✗ ✗✓	496	if (static_cast<std::size_t>(df_dx.rows()) != nc_ \|\|
215	✓✗✗✓	248	static_cast<std::size_t>(df_dx.cols()) != ndx) {
216			throw_pretty("Invalid argument: "
217			<< "df_dx has wrong dimension (it should be " +
218			std::to_string(nc_) + "," + std::to_string(ndx) + ")");
219			}
220	✗✓	248	if (static_cast<std::size_t>(data->impulses.size()) != impulses_.size()) {
221			throw_pretty("Invalid argument: "
222			<< "it doesn't match the number of impulse datas and models");
223			}
224
225		248	std::size_t nc = 0;
226		248	typename ImpulseModelContainer::const_iterator it_m, end_m;
227		248	typename ImpulseDataContainer::const_iterator it_d, end_d;
228		948	for (it_m = impulses_.begin(), end_m = impulses_.end(),
229		248	it_d = data->impulses.begin(), end_d = data->impulses.end();
230	✓✓✗✓ ✓✓	1648	it_m != end_m \|\| it_d != end_d; ++it_m, ++it_d) {
231		700	const boost::shared_ptr<ImpulseItem>& m_i = it_m->second;
232		700	const boost::shared_ptr<ImpulseDataAbstract>& d_i = it_d->second;
233	✗✓✗✗ ✗✗✗✗ ✗✗	700	assert_pretty(it_m->first == it_d->first,
234			"it doesn't match the impulse name between data and model");
235	✓✗	700	if (m_i->active) {
236		700	const std::size_t nc_i = m_i->impulse->get_nc();
237	✓✗	700	const Eigen::Block<const MatrixXs> df_dx_i =
238			df_dx.block(nc, 0, nc_i, ndx);
239	✓✗✓✗	700	m_i->impulse->updateForceDiff(d_i, df_dx_i);
240		700	nc += nc_i;
241			} else {
242			m_i->impulse->setZeroForceDiff(d_i);
243			}
244			}
245		248	}
246
247			template <typename Scalar>
248		248	void ImpulseModelMultipleTpl<Scalar>::updateRneaDiff(
249			const boost::shared_ptr<ImpulseDataMultiple>& data,
250			pinocchio::DataTpl<Scalar>& pinocchio) const {
251	✗✓	496	if (static_cast<std::size_t>(data->impulses.size()) !=
252		248	this->get_impulses().size()) {
253			throw_pretty("Invalid argument: "
254			<< "it doesn't match the number of impulse datas and models");
255			}
256		248	typename ImpulseModelContainer::const_iterator it_m, end_m;
257		248	typename ImpulseDataContainer::const_iterator it_d, end_d;
258		948	for (it_m = impulses_.begin(), end_m = impulses_.end(),
259		248	it_d = data->impulses.begin(), end_d = data->impulses.end();
260	✓✓✗✓ ✓✓	1648	it_m != end_m \|\| it_d != end_d; ++it_m, ++it_d) {
261		700	const boost::shared_ptr<ImpulseItem>& m_i = it_m->second;
262		700	const boost::shared_ptr<ImpulseDataAbstract>& d_i = it_d->second;
263	✗✓✗✗ ✗✗✗✗ ✗✗	700	assert_pretty(it_m->first == it_d->first,
264			"it doesn't match the impulse name between data and model");
265	✓✗	700	if (m_i->active) {
266	✓✓✗	700	switch (m_i->impulse->get_type()) {
267		364	case pinocchio::ReferenceFrame::LOCAL:
268		364	break;
269		336	case pinocchio::ReferenceFrame::WORLD:
270			case pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED:
271	✓✗	336	pinocchio.dtau_dq += d_i->dtau_dq;
272		336	break;
273			}
274			}
275			}
276		248	}
277
278			template <typename Scalar>
279			boost::shared_ptr<ImpulseDataMultipleTpl<Scalar> >
280		4270	ImpulseModelMultipleTpl<Scalar>::createData(
281			pinocchio::DataTpl<Scalar>* const data) {
282			return boost::allocate_shared<ImpulseDataMultiple>(
283	✓✗	4270	Eigen::aligned_allocator<ImpulseDataMultiple>(), this, data);
284			}
285
286			template <typename Scalar>
287			const boost::shared_ptr<StateMultibodyTpl<Scalar> >&
288		25643	ImpulseModelMultipleTpl<Scalar>::get_state() const {
289		25643	return state_;
290			}
291
292			template <typename Scalar>
293			const typename ImpulseModelMultipleTpl<Scalar>::ImpulseModelContainer&
294		19023	ImpulseModelMultipleTpl<Scalar>::get_impulses() const {
295		19023	return impulses_;
296			}
297
298			template <typename Scalar>
299		7282	std::size_t ImpulseModelMultipleTpl<Scalar>::get_nc() const {
300		7282	return nc_;
301			}
302
303			template <typename Scalar>
304		21312	std::size_t ImpulseModelMultipleTpl<Scalar>::get_nc_total() const {
305		21312	return nc_total_;
306			}
307
308			template <typename Scalar>
309		1	const std::set<std::string>& ImpulseModelMultipleTpl<Scalar>::get_active_set()
310			const {
311		1	return active_set_;
312			}
313
314			template <typename Scalar>
315		1	const std::set<std::string>& ImpulseModelMultipleTpl<Scalar>::get_inactive_set()
316			const {
317		1	return inactive_set_;
318			}
319
320			template <typename Scalar>
321			bool ImpulseModelMultipleTpl<Scalar>::getImpulseStatus(
322			const std::string& name) const {
323			typename ImpulseModelContainer::const_iterator it = impulses_.find(name);
324			if (it != impulses_.end()) {
325			return it->second->active;
326			} else {
327			std::cerr << "Warning: we couldn't get the status of the " << name
328			<< " impulse item, it doesn't exist." << std::endl;
329			return false;
330			}
331			}
332
333			template <class Scalar>
334		1	std::ostream& operator<<(std::ostream& os,
335			const ImpulseModelMultipleTpl<Scalar>& model) {
336		1	const auto& active = model.get_active_set();
337		1	const auto& inactive = model.get_inactive_set();
338		1	os << "ImpulseModelMultiple:" << std::endl;
339		1	os << " Active:" << std::endl;
340		1	for (std::set<std::string>::const_iterator it = active.begin();
341	✗✓	1	it != active.end(); ++it) {
342			const boost::shared_ptr<
343			typename ImpulseModelMultipleTpl<Scalar>::ImpulseItem>& impulse_item =
344			model.get_impulses().find(*it)->second;
345			if (it != --active.end()) {
346			os << " " << it << ": " << impulse_item << std::endl;
347			} else {
348			os << " " << it << ": " << impulse_item << std::endl;
349			}
350			}
351		1	os << " Inactive:" << std::endl;
352		1	for (std::set<std::string>::const_iterator it = inactive.begin();
353	✗✓	1	it != inactive.end(); ++it) {
354			const boost::shared_ptr<
355			typename ImpulseModelMultipleTpl<Scalar>::ImpulseItem>& impulse_item =
356			model.get_impulses().find(*it)->second;
357			if (it != --inactive.end()) {
358			os << " " << it << ": " << impulse_item << std::endl;
359			} else {
360			os << " " << it << ": " << impulse_item;
361			}
362			}
363		1	return os;
364			}
365
366			} // namespace crocoddyl