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