multiple-impulses.hxx
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2018-2020, LAAS-CNRS, University of Edinburgh
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 #include "crocoddyl/core/utils/exception.hpp"
10 #include "crocoddyl/multibody/impulses/multiple-impulses.hpp"
11 
12 namespace crocoddyl {
13 
14 template <typename Scalar>
15 ImpulseModelMultipleTpl<Scalar>::ImpulseModelMultipleTpl(boost::shared_ptr<StateMultibody> state)
16  : state_(state), ni_(0), ni_total_(0) {}
17 
18 template <typename Scalar>
19 ImpulseModelMultipleTpl<Scalar>::~ImpulseModelMultipleTpl() {}
20 
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;
28  } else if (active) {
29  ni_ += impulse->get_ni();
30  ni_total_ += impulse->get_ni();
31  } else if (!active) {
32  ni_total_ += impulse->get_ni();
33  }
34 }
35 
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();
42  impulses_.erase(it);
43  } else {
44  std::cout << "Warning: this impulse item doesn't exist, we cannot remove it" << std::endl;
45  }
46 }
47 
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();
56  }
57  it->second->active = active;
58  } else {
59  std::cout << "Warning: this impulse item doesn't exist, we cannot change its status" << std::endl;
60  }
61 }
62 
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");
69  }
70  std::size_t ni = 0;
71 
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;
78  if (m_i->active) {
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");
81 
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;
85  ni += ni_i;
86  }
87  }
88 }
89 
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");
96  }
97  std::size_t ni = 0;
98 
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;
105  if (m_i->active) {
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");
108 
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;
112  ni += ni_i;
113  }
114  }
115 }
116 
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()) + ")");
123  }
124  data->vnext = vnext;
125 }
126 
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_) + ")");
133  }
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");
137  }
138  std::size_t ni = 0;
139 
140  for (ForceIterator it = data->fext.begin(); it != data->fext.end(); ++it) {
141  *it = pinocchio::ForceTpl<Scalar>::Zero();
142  }
143 
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;
149  if (m_i->active) {
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");
152 
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;
157  ni += ni_i;
158  }
159  }
160 }
161 
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()) + ")");
170  }
171  data->dvnext_dx = dvnext_dx;
172 }
173 
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) + ")");
181  }
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");
185  }
186  std::size_t ni = 0;
187 
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;
193  if (m_i->active) {
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");
196 
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);
200  ni += ni_i;
201  }
202  }
203 }
204 
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);
209 }
210 
211 template <typename Scalar>
212 const boost::shared_ptr<StateMultibodyTpl<Scalar> >& ImpulseModelMultipleTpl<Scalar>::get_state() const {
213  return state_;
214 }
215 
216 template <typename Scalar>
217 const typename ImpulseModelMultipleTpl<Scalar>::ImpulseModelContainer& ImpulseModelMultipleTpl<Scalar>::get_impulses()
218  const {
219  return impulses_;
220 }
221 
222 template <typename Scalar>
223 const std::size_t& ImpulseModelMultipleTpl<Scalar>::get_ni() const {
224  return ni_;
225 }
226 
227 template <typename Scalar>
228 const std::size_t& ImpulseModelMultipleTpl<Scalar>::get_ni_total() const {
229  return ni_total_;
230 }
231 
232 } // namespace crocoddyl
Definition: action-base.hxx:11