1 |
|
|
/////////////////////////////////////////////////////////////////////////////// |
2 |
|
|
// BSD 3-Clause License |
3 |
|
|
// |
4 |
|
|
// Copyright (C) 2019-2023, LAAS-CNRS, University of Edinburgh, |
5 |
|
|
// Heriot-Watt University |
6 |
|
|
// Copyright note valid unless otherwise stated in individual files. |
7 |
|
|
// All rights reserved. |
8 |
|
|
/////////////////////////////////////////////////////////////////////////////// |
9 |
|
|
|
10 |
|
|
#include "crocoddyl/core/utils/exception.hpp" |
11 |
|
|
#include "crocoddyl/multibody/contact-base.hpp" |
12 |
|
|
|
13 |
|
|
namespace crocoddyl { |
14 |
|
|
|
15 |
|
|
template <typename Scalar> |
16 |
|
2059 |
ContactModelAbstractTpl<Scalar>::ContactModelAbstractTpl( |
17 |
|
|
boost::shared_ptr<StateMultibody> state, |
18 |
|
|
const pinocchio::ReferenceFrame type, const std::size_t nc, |
19 |
|
|
const std::size_t nu) |
20 |
|
2059 |
: state_(state), nc_(nc), nu_(nu), id_(0), type_(type) {} |
21 |
|
|
|
22 |
|
|
template <typename Scalar> |
23 |
|
22 |
ContactModelAbstractTpl<Scalar>::ContactModelAbstractTpl( |
24 |
|
|
boost::shared_ptr<StateMultibody> state, |
25 |
|
|
const pinocchio::ReferenceFrame type, const std::size_t nc) |
26 |
|
22 |
: state_(state), nc_(nc), nu_(state->get_nv()), id_(0), type_(type) {} |
27 |
|
|
|
28 |
|
|
template <typename Scalar> |
29 |
|
|
ContactModelAbstractTpl<Scalar>::ContactModelAbstractTpl( |
30 |
|
|
boost::shared_ptr<StateMultibody> state, const std::size_t nc, |
31 |
|
|
const std::size_t nu) |
32 |
|
|
: state_(state), |
33 |
|
|
nc_(nc), |
34 |
|
|
nu_(nu), |
35 |
|
|
id_(0), |
36 |
|
|
type_(pinocchio::ReferenceFrame::LOCAL) { |
37 |
|
|
std::cerr << "Deprecated: Use constructor that passes the type of contact, " |
38 |
|
|
"this assumes is pinocchio::LOCAL." |
39 |
|
|
<< std::endl; |
40 |
|
|
} |
41 |
|
|
|
42 |
|
|
template <typename Scalar> |
43 |
|
|
ContactModelAbstractTpl<Scalar>::ContactModelAbstractTpl( |
44 |
|
|
boost::shared_ptr<StateMultibody> state, const std::size_t nc) |
45 |
|
|
: state_(state), |
46 |
|
|
nc_(nc), |
47 |
|
|
nu_(state->get_nv()), |
48 |
|
|
id_(0), |
49 |
|
|
type_(pinocchio::ReferenceFrame::LOCAL) { |
50 |
|
|
std::cerr << "Deprecated: Use constructor that passes the type of contact, " |
51 |
|
|
"this assumes is pinocchio::LOCAL." |
52 |
|
|
<< std::endl; |
53 |
|
|
} |
54 |
|
|
|
55 |
|
|
template <typename Scalar> |
56 |
|
4178 |
ContactModelAbstractTpl<Scalar>::~ContactModelAbstractTpl() {} |
57 |
|
|
|
58 |
|
|
template <typename Scalar> |
59 |
|
86442 |
void ContactModelAbstractTpl<Scalar>::updateForceDiff( |
60 |
|
|
const boost::shared_ptr<ContactDataAbstract>& data, const MatrixXs& df_dx, |
61 |
|
|
const MatrixXs& df_du) const { |
62 |
✓✗✗✓
✗✓ |
172884 |
if (static_cast<std::size_t>(df_dx.rows()) != nc_ || |
63 |
|
86442 |
static_cast<std::size_t>(df_dx.cols()) != state_->get_ndx()) |
64 |
|
|
throw_pretty("df_dx has wrong dimension"); |
65 |
|
|
|
66 |
✓✗✗✓
|
172884 |
if (static_cast<std::size_t>(df_du.rows()) != nc_ || |
67 |
✗✓ |
86442 |
static_cast<std::size_t>(df_du.cols()) != nu_) |
68 |
|
|
throw_pretty("df_du has wrong dimension"); |
69 |
|
|
|
70 |
|
86442 |
data->df_dx = df_dx; |
71 |
|
86442 |
data->df_du = df_du; |
72 |
|
86442 |
} |
73 |
|
|
|
74 |
|
|
template <typename Scalar> |
75 |
|
8 |
void ContactModelAbstractTpl<Scalar>::setZeroForce( |
76 |
|
|
const boost::shared_ptr<ContactDataAbstract>& data) const { |
77 |
|
8 |
data->f.setZero(); |
78 |
|
8 |
data->fext.setZero(); |
79 |
|
8 |
} |
80 |
|
|
|
81 |
|
|
template <typename Scalar> |
82 |
|
|
void ContactModelAbstractTpl<Scalar>::setZeroForceDiff( |
83 |
|
|
const boost::shared_ptr<ContactDataAbstract>& data) const { |
84 |
|
|
data->df_dx.setZero(); |
85 |
|
|
data->df_du.setZero(); |
86 |
|
|
} |
87 |
|
|
|
88 |
|
|
template <typename Scalar> |
89 |
|
|
boost::shared_ptr<ContactDataAbstractTpl<Scalar> > |
90 |
|
|
ContactModelAbstractTpl<Scalar>::createData( |
91 |
|
|
pinocchio::DataTpl<Scalar>* const data) { |
92 |
|
|
return boost::allocate_shared<ContactDataAbstract>( |
93 |
|
|
Eigen::aligned_allocator<ContactDataAbstract>(), this, data); |
94 |
|
|
} |
95 |
|
|
|
96 |
|
|
template <typename Scalar> |
97 |
|
|
void ContactModelAbstractTpl<Scalar>::print(std::ostream& os) const { |
98 |
|
|
os << boost::core::demangle(typeid(*this).name()); |
99 |
|
|
} |
100 |
|
|
|
101 |
|
|
template <typename Scalar> |
102 |
|
|
const boost::shared_ptr<StateMultibodyTpl<Scalar> >& |
103 |
|
2412759 |
ContactModelAbstractTpl<Scalar>::get_state() const { |
104 |
|
2412759 |
return state_; |
105 |
|
|
} |
106 |
|
|
|
107 |
|
|
template <typename Scalar> |
108 |
|
1283322 |
std::size_t ContactModelAbstractTpl<Scalar>::get_nc() const { |
109 |
|
1283322 |
return nc_; |
110 |
|
|
} |
111 |
|
|
|
112 |
|
|
template <typename Scalar> |
113 |
|
167129 |
std::size_t ContactModelAbstractTpl<Scalar>::get_nu() const { |
114 |
|
167129 |
return nu_; |
115 |
|
|
} |
116 |
|
|
|
117 |
|
|
template <typename Scalar> |
118 |
|
164175 |
pinocchio::FrameIndex ContactModelAbstractTpl<Scalar>::get_id() const { |
119 |
|
164175 |
return id_; |
120 |
|
|
} |
121 |
|
|
|
122 |
|
|
template <typename Scalar> |
123 |
|
182486 |
pinocchio::ReferenceFrame ContactModelAbstractTpl<Scalar>::get_type() const { |
124 |
|
182486 |
return type_; |
125 |
|
|
} |
126 |
|
|
|
127 |
|
|
template <typename Scalar> |
128 |
|
9 |
void ContactModelAbstractTpl<Scalar>::set_id(const pinocchio::FrameIndex id) { |
129 |
|
9 |
id_ = id; |
130 |
|
9 |
} |
131 |
|
|
|
132 |
|
|
template <typename Scalar> |
133 |
|
|
void ContactModelAbstractTpl<Scalar>::set_type( |
134 |
|
|
const pinocchio::ReferenceFrame type) { |
135 |
|
|
type_ = type; |
136 |
|
|
} |
137 |
|
|
|
138 |
|
|
template <class Scalar> |
139 |
|
50 |
std::ostream& operator<<(std::ostream& os, |
140 |
|
|
const ContactModelAbstractTpl<Scalar>& model) { |
141 |
|
50 |
model.print(os); |
142 |
|
50 |
return os; |
143 |
|
|
} |
144 |
|
|
|
145 |
|
|
} // namespace crocoddyl |