11 template <
typename Scalar>
12 ContactModelMultipleTpl<Scalar>::ContactModelMultipleTpl(
13 boost::shared_ptr<StateMultibody> state,
const std::size_t nu)
16 template <
typename Scalar>
17 ContactModelMultipleTpl<Scalar>::ContactModelMultipleTpl(
18 boost::shared_ptr<StateMultibody> state)
21 template <
typename Scalar>
22 ContactModelMultipleTpl<Scalar>::~ContactModelMultipleTpl() {}
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) +
")");
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()) +
")");
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");
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");
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()));
66 m_i->contact->setZeroForceDiff(d_i);
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");
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");
91 const std::size_t nc_i = m_i->contact->get_nc();
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_;
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_;
Definition: contact-force.hxx:11