contact-base.hxx
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2018-2020, LAAS-CNRS, University of Edinburgh
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 #include "crocoddyl/core/utils/exception.hpp"
10 #include "crocoddyl/multibody/contact-base.hpp"
11 
12 namespace crocoddyl {
13 
14 template <typename Scalar>
15 ContactModelAbstractTpl<Scalar>::ContactModelAbstractTpl(boost::shared_ptr<StateMultibody> state,
16  const std::size_t& nc, const std::size_t& nu)
17  : state_(state), nc_(nc), nu_(nu) {}
18 
19 template <typename Scalar>
20 ContactModelAbstractTpl<Scalar>::ContactModelAbstractTpl(boost::shared_ptr<StateMultibody> state,
21  const std::size_t& nc)
22  : state_(state), nc_(nc), nu_(state->get_nv()) {}
23 
24 template <typename Scalar>
25 ContactModelAbstractTpl<Scalar>::~ContactModelAbstractTpl() {}
26 
27 template <typename Scalar>
28 void ContactModelAbstractTpl<Scalar>::updateForceDiff(const boost::shared_ptr<ContactDataAbstract>& data,
29  const MatrixXs& df_dx, const MatrixXs& df_du) const {
30  assert_pretty(
31  (static_cast<std::size_t>(df_dx.rows()) == nc_ || static_cast<std::size_t>(df_dx.cols()) == state_->get_nx()),
32  "df_dx has wrong dimension");
33  assert_pretty((static_cast<std::size_t>(df_du.rows()) == nc_ || static_cast<std::size_t>(df_du.cols()) == nu_),
34  "df_du has wrong dimension");
35  data->df_dx = df_dx;
36  data->df_du = df_du;
37 }
38 
39 template <typename Scalar>
40 boost::shared_ptr<ContactDataAbstractTpl<Scalar> > ContactModelAbstractTpl<Scalar>::createData(
41  pinocchio::DataTpl<Scalar>* const data) {
42  return boost::make_shared<ContactDataAbstract>(this, data);
43 }
44 
45 template <typename Scalar>
46 const boost::shared_ptr<StateMultibodyTpl<Scalar> >& ContactModelAbstractTpl<Scalar>::get_state() const {
47  return state_;
48 }
49 
50 template <typename Scalar>
51 const std::size_t& ContactModelAbstractTpl<Scalar>::get_nc() const {
52  return nc_;
53 }
54 
55 template <typename Scalar>
56 const std::size_t& ContactModelAbstractTpl<Scalar>::get_nu() const {
57  return nu_;
58 }
59 
60 } // namespace crocoddyl
Definition: action-base.hxx:11