contact-force.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 #include "contact-force.hpp"
10 
11 namespace sobec {
12 namespace newcontacts {
13 
14 template <typename Scalar>
15 ResidualModelContactForceTpl<Scalar>::ResidualModelContactForceTpl(
16  boost::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
17  const Force& fref, const std::size_t nc, const std::size_t nu)
18  : Base(state, id, fref, nc, nu) {}
19 
20 template <typename Scalar>
21 ResidualModelContactForceTpl<Scalar>::ResidualModelContactForceTpl(
22  boost::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
23  const Force& fref, const std::size_t nc)
24  : Base(state, id, fref, nc) {}
25 
26 template <typename Scalar>
27 ResidualModelContactForceTpl<Scalar>::~ResidualModelContactForceTpl() {}
28 
29 template <typename Scalar>
30 void ResidualModelContactForceTpl<Scalar>::calc(
31  const boost::shared_ptr<ResidualDataAbstract>& data,
32  const Eigen::Ref<const VectorXs>&, const Eigen::Ref<const VectorXs>&) {
33  Data* d = static_cast<Data*>(data.get());
34 
35  // We transform the force to the contact frame
36  switch (d->contact_type) {
37  case Contact1D: {
38  ContactData1DTpl<Scalar>* d1d =
39  static_cast<ContactData1DTpl<Scalar>*>(d->contact.get());
40  if (d1d->type == pinocchio::LOCAL) {
41  data->r = d->contact->jMf.rotation().transpose().row(d1d->mask) *
42  d->contact->f.linear() -
43  this->get_reference().linear().row(d1d->mask);
44  } else if (d1d->type == pinocchio::WORLD ||
45  d1d->type == pinocchio::LOCAL_WORLD_ALIGNED) {
46  data->r =
47  (d1d->oRf * d->contact->jMf.rotation().transpose()).row(d1d->mask) *
48  d->contact->f.linear() -
49  this->get_reference().linear().row(d1d->mask);
50  }
51  break;
52  }
53  case Contact3D: {
54  ContactData3DTpl<Scalar>* d3d =
55  static_cast<ContactData3DTpl<Scalar>*>(d->contact.get());
56  if (d3d->type == pinocchio::LOCAL) {
57  data->r =
58  (d->contact->jMf.rotation().transpose() * d->contact->f.linear() -
59  this->get_reference().linear());
60  } else if (d3d->type == pinocchio::WORLD ||
61  d3d->type == pinocchio::LOCAL_WORLD_ALIGNED) {
62  data->r = (d3d->oRf * d->contact->jMf.rotation().transpose() *
63  d->contact->f.linear() -
64  this->get_reference().linear());
65  }
66  break;
67  }
68  case Contact6D:
69  data->r = (d->contact->jMf.actInv(d->contact->f) - this->get_reference())
70  .toVector();
71  break;
72  default:
73  break;
74  }
75 }
76 
77 template <typename Scalar>
78 void ResidualModelContactForceTpl<Scalar>::calcDiff(
79  const boost::shared_ptr<ResidualDataAbstract>& data,
80  const Eigen::Ref<const VectorXs>&, const Eigen::Ref<const VectorXs>&) {
81  Data* d = static_cast<Data*>(data.get());
82 
83  const MatrixXs& df_dx = d->contact->df_dx;
84  const MatrixXs& df_du = d->contact->df_du;
85 
86  switch (d->contact_type) {
87  case Contact1D: {
88  data->Rx = df_dx.template topRows<1>();
89  data->Ru = df_du.template topRows<1>();
90  break;
91  }
92  case Contact3D: {
93  data->Rx = df_dx.template topRows<3>();
94  data->Ru = df_du.template topRows<3>();
95  break;
96  }
97  case Contact6D:
98  data->Rx = df_dx;
99  data->Ru = df_du;
100  break;
101  default:
102  break;
103  }
104 }
105 
106 } // namespace newcontacts
107 } // namespace sobec
Definition: contact-force.hxx:11