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>
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>
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>
28 
29 template <typename Scalar>
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: {
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: {
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>
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
virtual void calc(const boost::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the contact force residual.
Definition: contact-force.hxx:30
virtual void calcDiff(const boost::shared_ptr< ResidualDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the derivatives of the contact force residual.
Definition: contact-force.hxx:78
pinocchio::ForceTpl< Scalar > Force
Definition: contact-force.hpp:64
ResidualModelContactForceTpl(boost::shared_ptr< StateMultibody > state, const pinocchio::FrameIndex id, const Force &fref, const std::size_t nc, const std::size_t nu)
Initialize the contact force residual model.
Definition: contact-force.hxx:15
crocoddyl::ResidualDataContactForceTpl< Scalar > Data
Definition: contact-force.hpp:61
MathBase::MatrixXs MatrixXs
Definition: contact-force.hpp:66
crocoddyl::ResidualModelContactForceTpl< Scalar > Base
Definition: contact-force.hpp:60
virtual ~ResidualModelContactForceTpl()
Definition: contact-force.hxx:27
@ Contact6D
Definition: fwd.hpp:185
@ Contact3D
Definition: fwd.hpp:184
@ Contact1D
Definition: fwd.hpp:182
Definition: activation-quad-ref.hpp:19
Definition: contact1d.hpp:173
Matrix3s oRf
Definition: contact1d.hpp:249
pinocchio::ReferenceFrame type
Definition: contact1d.hpp:251
Vector3MaskType mask
Definition: contact1d.hpp:252
Definition: contact3d.hpp:155
Matrix3s oRf
Definition: contact3d.hpp:229
pinocchio::ReferenceFrame type
Definition: contact3d.hpp:231