multiple-contacts.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 namespace crocoddyl {
10 
11 template <typename Scalar>
12 ContactModelMultipleTpl<Scalar>::ContactModelMultipleTpl(boost::shared_ptr<StateMultibody> state,
13  const std::size_t& nu)
14  : state_(state), nc_(0), nc_total_(0), nu_(nu) {}
15 
16 template <typename Scalar>
17 ContactModelMultipleTpl<Scalar>::ContactModelMultipleTpl(boost::shared_ptr<StateMultibody> state)
18  : state_(state), nc_(0), nc_total_(0), nu_(state->get_nv()) {}
19 
20 template <typename Scalar>
21 ContactModelMultipleTpl<Scalar>::~ContactModelMultipleTpl() {}
22 
23 template <typename Scalar>
24 void ContactModelMultipleTpl<Scalar>::addContact(const std::string& name,
25  boost::shared_ptr<ContactModelAbstract> contact, bool active) {
26  if (contact->get_nu() != nu_) {
27  throw_pretty("Invalid argument: "
28  << "contact item doesn't have the same control dimension (" + std::to_string(nu_) + ")");
29  }
30  std::pair<typename ContactModelContainer::iterator, bool> ret =
31  contacts_.insert(std::make_pair(name, boost::make_shared<ContactItem>(name, contact, active)));
32  if (ret.second == false) {
33  std::cout << "Warning: this contact item already existed, we cannot add it" << std::endl;
34  } else if (active) {
35  nc_ += contact->get_nc();
36  nc_total_ += contact->get_nc();
37  } else if (!active) {
38  nc_total_ += contact->get_nc();
39  }
40 }
41 
42 template <typename Scalar>
43 void ContactModelMultipleTpl<Scalar>::removeContact(const std::string& name) {
44  typename ContactModelContainer::iterator it = contacts_.find(name);
45  if (it != contacts_.end()) {
46  nc_ -= it->second->contact->get_nc();
47  nc_total_ -= it->second->contact->get_nc();
48  contacts_.erase(it);
49  } else {
50  std::cout << "Warning: this contact item doesn't exist, we cannot remove it" << std::endl;
51  }
52 }
53 
54 template <typename Scalar>
55 void ContactModelMultipleTpl<Scalar>::changeContactStatus(const std::string& name, bool active) {
56  typename ContactModelContainer::iterator it = contacts_.find(name);
57  if (it != contacts_.end()) {
58  if (active && !it->second->active) {
59  nc_ += it->second->contact->get_nc();
60  } else if (!active && it->second->active) {
61  nc_ -= it->second->contact->get_nc();
62  }
63  it->second->active = active;
64  } else {
65  std::cout << "Warning: this contact item doesn't exist, we cannot change its status" << std::endl;
66  }
67 }
68 
69 template <typename Scalar>
70 void ContactModelMultipleTpl<Scalar>::calc(const boost::shared_ptr<ContactDataMultiple>& data,
71  const Eigen::Ref<const VectorXs>& x) {
72  if (data->contacts.size() != contacts_.size()) {
73  throw_pretty("Invalid argument: "
74  << "it doesn't match the number of contact datas and models");
75  }
76  std::size_t nc = 0;
77 
78  const std::size_t& nv = state_->get_nv();
79  typename ContactModelContainer::iterator it_m, end_m;
80  typename ContactDataContainer::iterator it_d, end_d;
81  for (it_m = contacts_.begin(), end_m = contacts_.end(), it_d = data->contacts.begin(), end_d = data->contacts.end();
82  it_m != end_m || it_d != end_d; ++it_m, ++it_d) {
83  const boost::shared_ptr<ContactItem>& m_i = it_m->second;
84  if (m_i->active) {
85  const boost::shared_ptr<ContactDataAbstract>& d_i = it_d->second;
86  assert_pretty(it_m->first == it_d->first, "it doesn't match the contact name between data and model");
87 
88  m_i->contact->calc(d_i, x);
89  const std::size_t& nc_i = m_i->contact->get_nc();
90  data->a0.segment(nc, nc_i) = d_i->a0;
91  data->Jc.block(nc, 0, nc_i, nv) = d_i->Jc;
92  nc += nc_i;
93  }
94  }
95 }
96 
97 template <typename Scalar>
98 void ContactModelMultipleTpl<Scalar>::calcDiff(const boost::shared_ptr<ContactDataMultiple>& data,
99  const Eigen::Ref<const VectorXs>& x) {
100  if (data->contacts.size() != contacts_.size()) {
101  throw_pretty("Invalid argument: "
102  << "it doesn't match the number of contact datas and models");
103  }
104  std::size_t nc = 0;
105 
106  const std::size_t& ndx = state_->get_ndx();
107  typename ContactModelContainer::iterator it_m, end_m;
108  typename ContactDataContainer::iterator it_d, end_d;
109  for (it_m = contacts_.begin(), end_m = contacts_.end(), it_d = data->contacts.begin(), end_d = data->contacts.end();
110  it_m != end_m || it_d != end_d; ++it_m, ++it_d) {
111  const boost::shared_ptr<ContactItem>& m_i = it_m->second;
112  if (m_i->active) {
113  const boost::shared_ptr<ContactDataAbstract>& d_i = it_d->second;
114  assert_pretty(it_m->first == it_d->first, "it doesn't match the contact name between data and model");
115 
116  m_i->contact->calcDiff(d_i, x);
117  const std::size_t& nc_i = m_i->contact->get_nc();
118  data->da0_dx.block(nc, 0, nc_i, ndx) = d_i->da0_dx;
119  nc += nc_i;
120  }
121  }
122 }
123 
124 template <typename Scalar>
125 void ContactModelMultipleTpl<Scalar>::updateAcceleration(const boost::shared_ptr<ContactDataMultiple>& data,
126  const VectorXs& dv) const {
127  if (static_cast<std::size_t>(dv.size()) != state_->get_nv()) {
128  throw_pretty("Invalid argument: "
129  << "dv has wrong dimension (it should be " + std::to_string(state_->get_nv()) + ")");
130  }
131  data->dv = dv;
132 }
133 
134 template <typename Scalar>
135 void ContactModelMultipleTpl<Scalar>::updateForce(const boost::shared_ptr<ContactDataMultiple>& data,
136  const VectorXs& force) {
137  if (static_cast<std::size_t>(force.size()) != nc_) {
138  throw_pretty("Invalid argument: "
139  << "force has wrong dimension (it should be " + std::to_string(nc_) + ")");
140  }
141  if (static_cast<std::size_t>(data->contacts.size()) != contacts_.size()) {
142  throw_pretty("Invalid argument: "
143  << "it doesn't match the number of contact datas and models");
144  }
145  std::size_t nc = 0;
146 
147  for (ForceIterator it = data->fext.begin(); it != data->fext.end(); ++it) {
148  *it = pinocchio::ForceTpl<Scalar>::Zero();
149  }
150 
151  typename ContactModelContainer::iterator it_m, end_m;
152  typename ContactDataContainer::iterator it_d, end_d;
153  for (it_m = contacts_.begin(), end_m = contacts_.end(), it_d = data->contacts.begin(), end_d = data->contacts.end();
154  it_m != end_m || it_d != end_d; ++it_m, ++it_d) {
155  const boost::shared_ptr<ContactItem>& m_i = it_m->second;
156  if (m_i->active) {
157  const boost::shared_ptr<ContactDataAbstract>& d_i = it_d->second;
158  assert_pretty(it_m->first == it_d->first, "it doesn't match the contact name between data and model");
159 
160  const std::size_t& nc_i = m_i->contact->get_nc();
161  const Eigen::VectorBlock<const VectorXs, Eigen::Dynamic> force_i = force.segment(nc, nc_i);
162  m_i->contact->updateForce(d_i, force_i);
163  data->fext[d_i->joint] = d_i->f;
164  nc += nc_i;
165  }
166  }
167 }
168 
169 template <typename Scalar>
170 void ContactModelMultipleTpl<Scalar>::updateAccelerationDiff(const boost::shared_ptr<ContactDataMultiple>& data,
171  const MatrixXs& ddv_dx) const {
172  if (static_cast<std::size_t>(ddv_dx.rows()) != state_->get_nv() ||
173  static_cast<std::size_t>(ddv_dx.cols()) != state_->get_ndx()) {
174  throw_pretty("Invalid argument: "
175  << "ddv_dx has wrong dimension (it should be " + std::to_string(state_->get_nv()) + "," +
176  std::to_string(state_->get_ndx()) + ")");
177  }
178  data->ddv_dx = ddv_dx;
179 }
180 
181 template <typename Scalar>
182 void ContactModelMultipleTpl<Scalar>::updateForceDiff(const boost::shared_ptr<ContactDataMultiple>& data,
183  const MatrixXs& df_dx, const MatrixXs& df_du) const {
184  const std::size_t& ndx = state_->get_ndx();
185  if (static_cast<std::size_t>(df_dx.rows()) != nc_ || static_cast<std::size_t>(df_dx.cols()) != ndx) {
186  throw_pretty("Invalid argument: "
187  << "df_dx has wrong dimension (it should be " + std::to_string(nc_) + "," + std::to_string(ndx) +
188  ")");
189  }
190  if (static_cast<std::size_t>(df_du.rows()) != nc_ || static_cast<std::size_t>(df_du.cols()) != nu_) {
191  throw_pretty("Invalid argument: "
192  << "df_du has wrong dimension (it should be " + std::to_string(nc_) + "," + std::to_string(nu_) +
193  ")");
194  }
195  if (static_cast<std::size_t>(data->contacts.size()) != contacts_.size()) {
196  throw_pretty("Invalid argument: "
197  << "it doesn't match the number of contact datas and models");
198  }
199  std::size_t nc = 0;
200 
201  typename ContactModelContainer::const_iterator it_m, end_m;
202  typename ContactDataContainer::const_iterator it_d, end_d;
203  for (it_m = contacts_.begin(), end_m = contacts_.end(), it_d = data->contacts.begin(), end_d = data->contacts.end();
204  it_m != end_m || it_d != end_d; ++it_m, ++it_d) {
205  const boost::shared_ptr<ContactItem>& m_i = it_m->second;
206  if (m_i->active) {
207  const boost::shared_ptr<ContactDataAbstract>& d_i = it_d->second;
208  assert_pretty(it_m->first == it_d->first, "it doesn't match the contact name between data and model");
209 
210  std::size_t const& nc_i = m_i->contact->get_nc();
211  const Eigen::Block<const MatrixXs> df_dx_i = df_dx.block(nc, 0, nc_i, ndx);
212  const Eigen::Block<const MatrixXs> df_du_i = df_du.block(nc, 0, nc_i, nu_);
213  m_i->contact->updateForceDiff(d_i, df_dx_i, df_du_i);
214  nc += nc_i;
215  }
216  }
217 }
218 
219 template <typename Scalar>
220 boost::shared_ptr<ContactDataMultipleTpl<Scalar> > ContactModelMultipleTpl<Scalar>::createData(
221  pinocchio::DataTpl<Scalar>* const data) {
222  return boost::make_shared<ContactDataMultiple>(this, data);
223 }
224 
225 template <typename Scalar>
226 const boost::shared_ptr<StateMultibodyTpl<Scalar> >& ContactModelMultipleTpl<Scalar>::get_state() const {
227  return state_;
228 }
229 
230 template <typename Scalar>
231 const typename ContactModelMultipleTpl<Scalar>::ContactModelContainer& ContactModelMultipleTpl<Scalar>::get_contacts()
232  const {
233  return contacts_;
234 }
235 
236 template <typename Scalar>
237 const std::size_t& ContactModelMultipleTpl<Scalar>::get_nc() const {
238  return nc_;
239 }
240 
241 template <typename Scalar>
242 const std::size_t& ContactModelMultipleTpl<Scalar>::get_nc_total() const {
243  return nc_total_;
244 }
245 
246 template <typename Scalar>
247 const std::size_t& ContactModelMultipleTpl<Scalar>::get_nu() const {
248  return nu_;
249 }
250 
251 } // namespace crocoddyl
Definition: action-base.hxx:11