multiple-contacts.hxx
Go to the documentation of this file.
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2022, LAAS-CNRS, University of Edinburgh
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 namespace sobec {
10 
11 template <typename Scalar>
12 ContactModelMultipleTpl<Scalar>::ContactModelMultipleTpl(
13  boost::shared_ptr<StateMultibody> state, const std::size_t nu)
14  : Base(state, nu) {}
15 
16 template <typename Scalar>
17 ContactModelMultipleTpl<Scalar>::ContactModelMultipleTpl(
18  boost::shared_ptr<StateMultibody> state)
19  : Base(state) {}
20 
21 template <typename Scalar>
22 ContactModelMultipleTpl<Scalar>::~ContactModelMultipleTpl() {}
23 
24 template <typename Scalar>
25 void ContactModelMultipleTpl<Scalar>::updateForceDiff(
26  const boost::shared_ptr<ContactDataMultiple>& data,
27  const boost::shared_ptr<MatrixXs> df_dx,
28  const boost::shared_ptr<MatrixXs> df_du) const {
29  const std::size_t ndx = this->get_state()->get_ndx();
30  if (static_cast<std::size_t>(df_dx->rows()) != this->get_nc() ||
31  static_cast<std::size_t>(df_dx->cols()) != ndx) {
32  throw_pretty("Invalid argument: "
33  << "df_dx has wrong dimension (it should be " +
34  std::to_string(this->get_nc()) + "," +
35  std::to_string(ndx) + ")");
36  }
37  if (static_cast<std::size_t>(df_du->rows()) != this->get_nc() ||
38  static_cast<std::size_t>(df_du->cols()) != this->get_nu()) {
39  throw_pretty("Invalid argument: "
40  << "df_du has wrong dimension (it should be " +
41  std::to_string(this->get_nc()) + "," +
42  std::to_string(this->get_nu()) + ")");
43  }
44  if (static_cast<std::size_t>(data->contacts.size()) !=
45  this->get_contacts().size()) {
46  throw_pretty("Invalid argument: "
47  << "it doesn't match the number of contact datas and models");
48  }
49 
50  std::size_t nc = 0;
51  typename ContactModelContainer::const_iterator it_m, end_m;
52  typename ContactDataContainer::const_iterator it_d, end_d;
53  for (it_m = this->get_contacts().begin(), end_m = this->get_contacts().end(),
54  it_d = data->contacts.begin(), end_d = data->contacts.end();
55  it_m != end_m || it_d != end_d; ++it_m, ++it_d) {
56  const boost::shared_ptr<ContactItem>& m_i = it_m->second;
57  const boost::shared_ptr<ContactDataAbstract>& d_i = it_d->second;
58  assert_pretty(it_m->first == it_d->first,
59  "it doesn't match the contact name between data and model");
60  if (m_i->active) {
61  const std::size_t nc_i = m_i->contact->get_nc();
62  m_i->contact->updateForceDiff(d_i, df_dx->block(nc, 0, nc_i, ndx),
63  df_du->block(nc, 0, nc_i, this->get_nu()));
64  nc += nc_i;
65  } else {
66  m_i->contact->setZeroForceDiff(d_i);
67  }
68  }
69 }
70 
71 template <typename Scalar>
72 void ContactModelMultipleTpl<Scalar>::updateRneaDerivatives(
73  const boost::shared_ptr<ContactDataMultiple>& data,
74  pinocchio::DataTpl<Scalar>& pinocchio) const {
75  const std::size_t nv = this->get_state()->get_nv();
76  if (static_cast<std::size_t>(data->contacts.size()) !=
77  this->get_contacts().size()) {
78  throw_pretty("Invalid argument: "
79  << "it doesn't match the number of contact datas and models");
80  }
81  typename ContactModelContainer::const_iterator it_m, end_m;
82  typename ContactDataContainer::const_iterator it_d, end_d;
83  for (it_m = this->get_contacts().begin(), end_m = this->get_contacts().end(),
84  it_d = data->contacts.begin(), end_d = data->contacts.end();
85  it_m != end_m || it_d != end_d; ++it_m, ++it_d) {
86  const boost::shared_ptr<ContactItem>& m_i = it_m->second;
87  const boost::shared_ptr<ContactDataAbstract>& d_i = it_d->second;
88  assert_pretty(it_m->first == it_d->first,
89  "it doesn't match the contact name between data and model");
90  if (m_i->active) {
91  const std::size_t nc_i = m_i->contact->get_nc();
92  if (nc_i == 3) {
93  ContactModel3DTpl<Scalar>* cm_i =
94  static_cast<ContactModel3DTpl<Scalar>*>(m_i->contact.get());
95  ContactData3DTpl<Scalar>* cd_i =
96  static_cast<ContactData3DTpl<Scalar>*>(d_i.get());
97  if (cm_i->get_type() == pinocchio::WORLD ||
98  cm_i->get_type() == pinocchio::LOCAL_WORLD_ALIGNED) {
99  pinocchio.dtau_dq.block(0, 0, nv, nv) += cd_i->drnea_skew_term_;
100  }
101  }
102  if (nc_i == 1) {
103  ContactModel1DTpl<Scalar>* cm_i =
104  static_cast<ContactModel1DTpl<Scalar>*>(m_i->contact.get());
105  ContactData1DTpl<Scalar>* cd_i =
106  static_cast<ContactData1DTpl<Scalar>*>(d_i.get());
107  if (cm_i->get_type() == pinocchio::WORLD ||
108  cm_i->get_type() == pinocchio::LOCAL_WORLD_ALIGNED) {
109  pinocchio.dtau_dq.block(0, 0, nv, nv) += cd_i->drnea_skew_term_;
110  }
111  }
112  }
113  }
114 }
115 
116 // template <typename Scalar>
117 // MathBase::MatrixXs ContactModelMultipleTpl<Scalar>::rotateJacobians(const
118 // boost::shared_ptr<MathBase::MatrixXs>& Jin) {
119 // MathBase::MatrixXs Jout = MatrixXs(Jin);
120 // std::size_t nc = 0;
121 // const std::size_t nv = this->get_state()->get_nv();
122 // typename ContactModelContainer::const_iterator it_m, end_m;
123 // typename ContactDataContainer::const_iterator it_d, end_d;
124 // for (it_m = this->get_contacts().begin(), end_m =
125 // this->get_contacts().end(), it_d = data->contacts.begin(), end_d =
126 // data->contacts.end();
127 // it_m != end_m || it_d != end_d; ++it_m, ++it_d) {
128 // const boost::shared_ptr<ContactItem>& m_i = it_m->second;
129 // const boost::shared_ptr<ContactDataAbstract>& d_i = it_d->second;
130 // assert_pretty(it_m->first == it_d->first, "it doesn't match the contact
131 // name between data and model"); if (m_i->active) {
132 // const std::size_t nc_i = m_i->contact->get_nc();
133 // // if (nc_i == 3) {
134 // // ContactModel3DTpl<Scalar>* cm_i =
135 // static_cast<ContactModel3DTpl<Scalar>*>(m_i->contact.get());
136 // // ContactData3DTpl<Scalar>* cd_i =
137 // static_cast<ContactData3DTpl<Scalar>*>(d_i.get());
138 // // Jout.block(nc, 0, nc_i, nv) = m_i->contact->Jc;
139 // // }
140 // if (nc_i == 1) {
141 // ContactModel1DTpl<Scalar>* cm_i =
142 // static_cast<ContactModel1DTpl<Scalar>*>(m_i->contact.get());
143 // ContactData1DTpl<Scalar>* cd_i =
144 // static_cast<ContactData1DTpl<Scalar>*>(d_i.get()); Jout.block(nc, 0,
145 // nc_i, nv) = (d_i->oRf * d_i->fJf.topRows(3)).row(cm_i->get_mask());
146 // // d->oRf * d->fJf
147 // }
148 // nc += nc_i;
149 // }
150 // }
151 // return Jout
152 // }
153 
154 } // namespace sobec
Definition: contact-force.hxx:11