9 #include "crocoddyl/core/utils/exception.hpp" 10 #include "crocoddyl/multibody/impulses/multiple-impulses.hpp" 14 template <
typename Scalar>
15 ImpulseModelMultipleTpl<Scalar>::ImpulseModelMultipleTpl(boost::shared_ptr<StateMultibody> state)
16 : state_(state), ni_(0), ni_total_(0) {}
18 template <
typename Scalar>
19 ImpulseModelMultipleTpl<Scalar>::~ImpulseModelMultipleTpl() {}
21 template <
typename Scalar>
22 void ImpulseModelMultipleTpl<Scalar>::addImpulse(
const std::string& name,
23 boost::shared_ptr<ImpulseModelAbstract> impulse,
bool active) {
24 std::pair<typename ImpulseModelContainer::iterator, bool> ret =
25 impulses_.insert(std::make_pair(name, boost::make_shared<ImpulseItem>(name, impulse, active)));
26 if (ret.second ==
false) {
27 std::cout <<
"Warning: this impulse item already existed, we cannot add it" << std::endl;
29 ni_ += impulse->get_ni();
30 ni_total_ += impulse->get_ni();
32 ni_total_ += impulse->get_ni();
36 template <
typename Scalar>
37 void ImpulseModelMultipleTpl<Scalar>::removeImpulse(
const std::string& name) {
38 typename ImpulseModelContainer::iterator it = impulses_.find(name);
39 if (it != impulses_.end()) {
40 ni_ -= it->second->impulse->get_ni();
41 ni_total_ -= it->second->impulse->get_ni();
44 std::cout <<
"Warning: this impulse item doesn't exist, we cannot remove it" << std::endl;
48 template <
typename Scalar>
49 void ImpulseModelMultipleTpl<Scalar>::changeImpulseStatus(
const std::string& name,
bool active) {
50 typename ImpulseModelContainer::iterator it = impulses_.find(name);
51 if (it != impulses_.end()) {
52 if (active && !it->second->active) {
53 ni_ += it->second->impulse->get_ni();
54 }
else if (!active && it->second->active) {
55 ni_ -= it->second->impulse->get_ni();
57 it->second->active = active;
59 std::cout <<
"Warning: this impulse item doesn't exist, we cannot change its status" << std::endl;
63 template <
typename Scalar>
64 void ImpulseModelMultipleTpl<Scalar>::calc(
const boost::shared_ptr<ImpulseDataMultiple>& data,
65 const Eigen::Ref<const VectorXs>& x) {
66 if (data->impulses.size() != impulses_.size()) {
67 throw_pretty(
"Invalid argument: " 68 <<
"it doesn't match the number of impulse datas and models");
72 const std::size_t& nv = state_->get_nv();
73 typename ImpulseModelContainer::iterator it_m, end_m;
74 typename ImpulseDataContainer::iterator it_d, end_d;
75 for (it_m = impulses_.begin(), end_m = impulses_.end(), it_d = data->impulses.begin(), end_d = data->impulses.end();
76 it_m != end_m || it_d != end_d; ++it_m, ++it_d) {
77 const boost::shared_ptr<ImpulseItem>& m_i = it_m->second;
79 const boost::shared_ptr<ImpulseDataAbstract>& d_i = it_d->second;
80 assert_pretty(it_m->first == it_d->first,
"it doesn't match the impulse name between data and model");
82 m_i->impulse->calc(d_i, x);
83 const std::size_t& ni_i = m_i->impulse->get_ni();
84 data->Jc.block(ni, 0, ni_i, nv) = d_i->Jc;
90 template <
typename Scalar>
91 void ImpulseModelMultipleTpl<Scalar>::calcDiff(
const boost::shared_ptr<ImpulseDataMultiple>& data,
92 const Eigen::Ref<const VectorXs>& x) {
93 if (data->impulses.size() != impulses_.size()) {
94 throw_pretty(
"Invalid argument: " 95 <<
"it doesn't match the number of impulse datas and models");
99 const std::size_t& nv = state_->get_nv();
100 typename ImpulseModelContainer::iterator it_m, end_m;
101 typename ImpulseDataContainer::iterator it_d, end_d;
102 for (it_m = impulses_.begin(), end_m = impulses_.end(), it_d = data->impulses.begin(), end_d = data->impulses.end();
103 it_m != end_m || it_d != end_d; ++it_m, ++it_d) {
104 const boost::shared_ptr<ImpulseItem>& m_i = it_m->second;
106 const boost::shared_ptr<ImpulseDataAbstract>& d_i = it_d->second;
107 assert_pretty(it_m->first == it_d->first,
"it doesn't match the impulse name between data and model");
109 m_i->impulse->calcDiff(d_i, x);
110 const std::size_t& ni_i = m_i->impulse->get_ni();
111 data->dv0_dq.block(ni, 0, ni_i, nv) = d_i->dv0_dq;
117 template <
typename Scalar>
118 void ImpulseModelMultipleTpl<Scalar>::updateVelocity(
const boost::shared_ptr<ImpulseDataMultiple>& data,
119 const VectorXs& vnext)
const {
120 if (static_cast<std::size_t>(vnext.size()) != state_->get_nv()) {
121 throw_pretty(
"Invalid argument: " 122 <<
"vnext has wrong dimension (it should be " + std::to_string(state_->get_nv()) +
")");
127 template <
typename Scalar>
128 void ImpulseModelMultipleTpl<Scalar>::updateForce(
const boost::shared_ptr<ImpulseDataMultiple>& data,
129 const VectorXs& force) {
130 if (static_cast<std::size_t>(force.size()) != ni_) {
131 throw_pretty(
"Invalid argument: " 132 <<
"force has wrong dimension (it should be " + std::to_string(ni_) +
")");
134 if (static_cast<std::size_t>(data->impulses.size()) != impulses_.size()) {
135 throw_pretty(
"Invalid argument: " 136 <<
"it doesn't match the number of impulse datas and models");
140 for (ForceIterator it = data->fext.begin(); it != data->fext.end(); ++it) {
141 *it = pinocchio::ForceTpl<Scalar>::Zero();
144 typename ImpulseModelContainer::iterator it_m, end_m;
145 typename ImpulseDataContainer::iterator it_d, end_d;
146 for (it_m = impulses_.begin(), end_m = impulses_.end(), it_d = data->impulses.begin(), end_d = data->impulses.end();
147 it_m != end_m || it_d != end_d; ++it_m, ++it_d) {
148 const boost::shared_ptr<ImpulseItem>& m_i = it_m->second;
150 const boost::shared_ptr<ImpulseDataAbstract>& d_i = it_d->second;
151 assert_pretty(it_m->first == it_d->first,
"it doesn't match the impulse name between data and model");
153 const std::size_t& ni_i = m_i->impulse->get_ni();
154 const Eigen::VectorBlock<const VectorXs, Eigen::Dynamic> force_i = force.segment(ni, ni_i);
155 m_i->impulse->updateForce(d_i, force_i);
156 data->fext[d_i->joint] = d_i->f;
162 template <
typename Scalar>
163 void ImpulseModelMultipleTpl<Scalar>::updateVelocityDiff(
const boost::shared_ptr<ImpulseDataMultiple>& data,
164 const MatrixXs& dvnext_dx)
const {
165 if (static_cast<std::size_t>(dvnext_dx.rows()) != state_->get_nv() ||
166 static_cast<std::size_t
>(dvnext_dx.cols()) != state_->get_ndx()) {
167 throw_pretty(
"Invalid argument: " 168 <<
"dvnext_dx has wrong dimension (it should be " + std::to_string(state_->get_nv()) +
"," +
169 std::to_string(state_->get_ndx()) +
")");
171 data->dvnext_dx = dvnext_dx;
174 template <
typename Scalar>
175 void ImpulseModelMultipleTpl<Scalar>::updateForceDiff(
const boost::shared_ptr<ImpulseDataMultiple>& data,
176 const MatrixXs& df_dq)
const {
177 const std::size_t& nv = state_->get_nv();
178 if (static_cast<std::size_t>(df_dq.rows()) != ni_ || static_cast<std::size_t>(df_dq.cols()) != nv) {
179 throw_pretty(
"Invalid argument: " 180 <<
"df_dq has wrong dimension (it should be " + std::to_string(ni_) +
"," + std::to_string(nv) +
")");
182 if (static_cast<std::size_t>(data->impulses.size()) != impulses_.size()) {
183 throw_pretty(
"Invalid argument: " 184 <<
"it doesn't match the number of impulse datas and models");
188 typename ImpulseModelContainer::const_iterator it_m, end_m;
189 typename ImpulseDataContainer::const_iterator it_d, end_d;
190 for (it_m = impulses_.begin(), end_m = impulses_.end(), it_d = data->impulses.begin(), end_d = data->impulses.end();
191 it_m != end_m || it_d != end_d; ++it_m, ++it_d) {
192 const boost::shared_ptr<ImpulseItem>& m_i = it_m->second;
194 const boost::shared_ptr<ImpulseDataAbstract>& d_i = it_d->second;
195 assert_pretty(it_m->first == it_d->first,
"it doesn't match the impulse name between data and model");
197 const std::size_t& ni_i = m_i->impulse->get_ni();
198 const Eigen::Block<const MatrixXs> df_dq_i = df_dq.block(ni, 0, ni_i, nv);
199 m_i->impulse->updateForceDiff(d_i, df_dq_i);
205 template <
typename Scalar>
206 boost::shared_ptr<ImpulseDataMultipleTpl<Scalar> > ImpulseModelMultipleTpl<Scalar>::createData(
207 pinocchio::DataTpl<Scalar>*
const data) {
208 return boost::make_shared<ImpulseDataMultiple>(
this, data);
211 template <
typename Scalar>
212 const boost::shared_ptr<StateMultibodyTpl<Scalar> >& ImpulseModelMultipleTpl<Scalar>::get_state()
const {
216 template <
typename Scalar>
217 const typename ImpulseModelMultipleTpl<Scalar>::ImpulseModelContainer& ImpulseModelMultipleTpl<Scalar>::get_impulses()
222 template <
typename Scalar>
223 const std::size_t& ImpulseModelMultipleTpl<Scalar>::get_ni()
const {
227 template <
typename Scalar>
228 const std::size_t& ImpulseModelMultipleTpl<Scalar>::get_ni_total()
const {
Definition: action-base.hxx:11