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/impulse-base.hpp" |
12 |
|
|
|
13 |
|
|
namespace crocoddyl { |
14 |
|
|
|
15 |
|
|
template <typename Scalar> |
16 |
|
409 |
ImpulseModelAbstractTpl<Scalar>::ImpulseModelAbstractTpl( |
17 |
|
|
boost::shared_ptr<StateMultibody> state, |
18 |
|
|
const pinocchio::ReferenceFrame type, const std::size_t nc) |
19 |
|
409 |
: state_(state), nc_(nc), id_(0), type_(type) {} |
20 |
|
|
|
21 |
|
|
template <typename Scalar> |
22 |
|
|
ImpulseModelAbstractTpl<Scalar>::ImpulseModelAbstractTpl( |
23 |
|
|
boost::shared_ptr<StateMultibody> state, const std::size_t nc) |
24 |
|
|
: state_(state), nc_(nc), id_(0), type_(pinocchio::ReferenceFrame::LOCAL) { |
25 |
|
|
std::cerr << "Deprecated: Use constructor that passes the type of contact, " |
26 |
|
|
"this assumes is pinocchio::LOCAL." |
27 |
|
|
<< std::endl; |
28 |
|
|
} |
29 |
|
|
|
30 |
|
|
template <typename Scalar> |
31 |
|
826 |
ImpulseModelAbstractTpl<Scalar>::~ImpulseModelAbstractTpl() {} |
32 |
|
|
|
33 |
|
|
template <typename Scalar> |
34 |
|
730 |
void ImpulseModelAbstractTpl<Scalar>::updateForceDiff( |
35 |
|
|
const boost::shared_ptr<ImpulseDataAbstract>& data, |
36 |
|
|
const MatrixXs& df_dx) const { |
37 |
✓✗✗✓
✗✓ |
1460 |
if (static_cast<std::size_t>(df_dx.rows()) != nc_ || |
38 |
|
730 |
static_cast<std::size_t>(df_dx.cols()) != state_->get_ndx()) |
39 |
|
|
throw_pretty("df_dq has wrong dimension"); |
40 |
|
|
|
41 |
|
730 |
data->df_dx = df_dx; |
42 |
|
730 |
} |
43 |
|
|
|
44 |
|
|
template <typename Scalar> |
45 |
|
|
void ImpulseModelAbstractTpl<Scalar>::setZeroForce( |
46 |
|
|
const boost::shared_ptr<ImpulseDataAbstract>& data) const { |
47 |
|
|
data->f.setZero(); |
48 |
|
|
data->fext.setZero(); |
49 |
|
|
} |
50 |
|
|
|
51 |
|
|
template <typename Scalar> |
52 |
|
|
void ImpulseModelAbstractTpl<Scalar>::setZeroForceDiff( |
53 |
|
|
const boost::shared_ptr<ImpulseDataAbstract>& data) const { |
54 |
|
|
data->df_dx.setZero(); |
55 |
|
|
} |
56 |
|
|
|
57 |
|
|
template <typename Scalar> |
58 |
|
|
boost::shared_ptr<ImpulseDataAbstractTpl<Scalar> > |
59 |
|
|
ImpulseModelAbstractTpl<Scalar>::createData( |
60 |
|
|
pinocchio::DataTpl<Scalar>* const data) { |
61 |
|
|
return boost::allocate_shared<ImpulseDataAbstract>( |
62 |
|
|
Eigen::aligned_allocator<ImpulseDataAbstract>(), this, data); |
63 |
|
|
} |
64 |
|
|
|
65 |
|
|
template <typename Scalar> |
66 |
|
|
void ImpulseModelAbstractTpl<Scalar>::print(std::ostream& os) const { |
67 |
|
|
os << boost::core::demangle(typeid(*this).name()); |
68 |
|
|
} |
69 |
|
|
|
70 |
|
|
template <typename Scalar> |
71 |
|
|
const boost::shared_ptr<StateMultibodyTpl<Scalar> >& |
72 |
|
115280 |
ImpulseModelAbstractTpl<Scalar>::get_state() const { |
73 |
|
115280 |
return state_; |
74 |
|
|
} |
75 |
|
|
|
76 |
|
|
template <typename Scalar> |
77 |
|
76770 |
std::size_t ImpulseModelAbstractTpl<Scalar>::get_nc() const { |
78 |
|
76770 |
return nc_; |
79 |
|
|
} |
80 |
|
|
|
81 |
|
|
template <typename Scalar> |
82 |
|
|
std::size_t ImpulseModelAbstractTpl<Scalar>::get_ni() const { |
83 |
|
|
return nc_; |
84 |
|
|
} |
85 |
|
|
|
86 |
|
|
template <typename Scalar> |
87 |
|
10460 |
std::size_t ImpulseModelAbstractTpl<Scalar>::get_nu() const { |
88 |
|
10460 |
return 0; |
89 |
|
|
} |
90 |
|
|
|
91 |
|
|
template <typename Scalar> |
92 |
|
20960 |
pinocchio::FrameIndex ImpulseModelAbstractTpl<Scalar>::get_id() const { |
93 |
|
20960 |
return id_; |
94 |
|
|
} |
95 |
|
|
|
96 |
|
|
template <typename Scalar> |
97 |
|
11242 |
pinocchio::ReferenceFrame ImpulseModelAbstractTpl<Scalar>::get_type() const { |
98 |
|
11242 |
return type_; |
99 |
|
|
} |
100 |
|
|
|
101 |
|
|
template <typename Scalar> |
102 |
|
6 |
void ImpulseModelAbstractTpl<Scalar>::set_id(const pinocchio::FrameIndex id) { |
103 |
|
6 |
id_ = id; |
104 |
|
6 |
} |
105 |
|
|
|
106 |
|
|
template <typename Scalar> |
107 |
|
|
void ImpulseModelAbstractTpl<Scalar>::set_type( |
108 |
|
|
const pinocchio::ReferenceFrame type) { |
109 |
|
|
type_ = type; |
110 |
|
|
} |
111 |
|
|
|
112 |
|
|
template <class Scalar> |
113 |
|
30 |
std::ostream& operator<<(std::ostream& os, |
114 |
|
|
const ImpulseModelAbstractTpl<Scalar>& model) { |
115 |
|
30 |
model.print(os); |
116 |
|
30 |
return os; |
117 |
|
|
} |
118 |
|
|
|
119 |
|
|
} // namespace crocoddyl |