residual-cop.hxx
Go to the documentation of this file.
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2021, LAAS-CNRS, University of Edinburgh, INRIA
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 #include <crocoddyl/core/utils/exception.hpp>
10 #include <pinocchio/algorithm/frames-derivatives.hpp>
11 #include <pinocchio/algorithm/frames.hpp>
12 #include <pinocchio/algorithm/kinematics-derivatives.hpp>
13 
14 #include "sobec/residual-cop.hpp"
15 
16 namespace sobec {
17 
18 using namespace crocoddyl;
19 
20 template <typename Scalar>
21 ResidualModelCenterOfPressureTpl<Scalar>::ResidualModelCenterOfPressureTpl(
22  boost::shared_ptr<StateMultibody> state,
23  const pinocchio::FrameIndex contact_id, const std::size_t nu)
24  : Base(state, 2, nu, true, true, true), contact_id_(contact_id) {}
25 
26 template <typename Scalar>
27 ResidualModelCenterOfPressureTpl<Scalar>::~ResidualModelCenterOfPressureTpl() {}
28 
29 template <typename Scalar>
30 void ResidualModelCenterOfPressureTpl<Scalar>::calc(
31  const boost::shared_ptr<ResidualDataAbstract> &data,
32  const Eigen::Ref<const VectorXs> & /*x*/,
33  const Eigen::Ref<const VectorXs> &) {
34  Data *d = static_cast<Data *>(data.get());
35  Force f = d->contact->jMf.actInv(d->contact->f);
36 
37  data->r[0] = f.angular()[1] / f.linear()[2];
38  data->r[1] = -f.angular()[0] / f.linear()[2];
39 }
40 
41 template <typename Scalar>
42 void ResidualModelCenterOfPressureTpl<Scalar>::calcDiff(
43  const boost::shared_ptr<ResidualDataAbstract> &data,
44  const Eigen::Ref<const VectorXs> &, const Eigen::Ref<const VectorXs> &) {
45  Data *d = static_cast<Data *>(data.get());
46  Force f = d->contact->jMf.actInv(d->contact->f);
47  const MatrixXs &df_dx = d->contact->df_dx;
48  const MatrixXs &df_du = d->contact->df_du;
49 
50  // r = tau/f
51  // r'= tau'/f - tau/f^2 f' = (tau'-cop.f')/f
52  data->Rx.row(0) = df_dx.row(4);
53  data->Rx.row(1) = -df_dx.row(3);
54  data->Rx.row(0) -= data->r[0] * df_dx.row(2);
55  data->Rx.row(1) -= data->r[1] * df_dx.row(2);
56  data->Rx /= f.linear()[2];
57 
58  data->Ru.row(0) = df_du.row(4);
59  data->Ru.row(1) = -df_du.row(3);
60  data->Ru.row(0) -= data->r[0] * df_du.row(2);
61  data->Ru.row(1) -= data->r[1] * df_du.row(2);
62  data->Ru /= f.linear()[2];
63 }
64 
65 template <typename Scalar>
66 boost::shared_ptr<ResidualDataAbstractTpl<Scalar> >
67 ResidualModelCenterOfPressureTpl<Scalar>::createData(
68  DataCollectorAbstract *const data) {
69  return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this,
70  data);
71 }
72 
73 } // namespace sobec
sobec
Definition: contact-force.hxx:11