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 <boost/core/demangle.hpp> |
11 |
|
|
|
12 |
|
|
#include "crocoddyl/core/utils/math.hpp" |
13 |
|
|
|
14 |
|
|
namespace crocoddyl { |
15 |
|
|
|
16 |
|
|
template <typename Scalar> |
17 |
|
2131 |
ActuationModelAbstractTpl<Scalar>::ActuationModelAbstractTpl( |
18 |
|
|
boost::shared_ptr<StateAbstract> state, const std::size_t nu) |
19 |
|
2131 |
: nu_(nu), state_(state) { |
20 |
|
|
if (nu_ < 0) { |
21 |
|
|
throw_pretty("Invalid argument: " |
22 |
|
|
<< "nu cannot be smaller than zero"); |
23 |
|
|
} |
24 |
|
2131 |
} |
25 |
|
|
|
26 |
|
|
template <typename Scalar> |
27 |
|
4278 |
ActuationModelAbstractTpl<Scalar>::~ActuationModelAbstractTpl() {} |
28 |
|
|
|
29 |
|
|
template <typename Scalar> |
30 |
|
|
boost::shared_ptr<ActuationDataAbstractTpl<Scalar> > |
31 |
|
6 |
ActuationModelAbstractTpl<Scalar>::createData() { |
32 |
|
|
return boost::allocate_shared<ActuationDataAbstract>( |
33 |
✓✗ |
6 |
Eigen::aligned_allocator<ActuationDataAbstract>(), this); |
34 |
|
|
} |
35 |
|
|
|
36 |
|
|
template <typename Scalar> |
37 |
|
941 |
void ActuationModelAbstractTpl<Scalar>::calc( |
38 |
|
|
const boost::shared_ptr<ActuationDataAbstract>&, |
39 |
|
941 |
const Eigen::Ref<const VectorXs>&) {} |
40 |
|
|
|
41 |
|
|
template <typename Scalar> |
42 |
|
207 |
void ActuationModelAbstractTpl<Scalar>::calcDiff( |
43 |
|
|
const boost::shared_ptr<ActuationDataAbstract>&, |
44 |
|
207 |
const Eigen::Ref<const VectorXs>&) {} |
45 |
|
|
|
46 |
|
|
template <typename Scalar> |
47 |
|
3062 |
void ActuationModelAbstractTpl<Scalar>::torqueTransform( |
48 |
|
|
const boost::shared_ptr<ActuationDataAbstract>& data, |
49 |
|
|
const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) { |
50 |
✗✓ |
3062 |
if (static_cast<std::size_t>(x.size()) != state_->get_nx()) { |
51 |
|
|
throw_pretty("Invalid argument: " |
52 |
|
|
<< "x has wrong dimension (it should be " + |
53 |
|
|
std::to_string(state_->get_nx()) + ")"); |
54 |
|
|
} |
55 |
✗✓ |
3062 |
if (static_cast<std::size_t>(u.size()) != nu_) { |
56 |
|
|
throw_pretty("Invalid argument: " |
57 |
|
|
<< "u has wrong dimension (it should be " + |
58 |
|
|
std::to_string(nu_) + ")"); |
59 |
|
|
} |
60 |
|
3062 |
calc(data, x, u); |
61 |
|
3062 |
calcDiff(data, x, u); |
62 |
✓✗ |
3062 |
data->Mtau = pseudoInverse(data->dtau_du); |
63 |
|
3062 |
} |
64 |
|
|
|
65 |
|
|
template <typename Scalar> |
66 |
|
656590 |
std::size_t ActuationModelAbstractTpl<Scalar>::get_nu() const { |
67 |
|
656590 |
return nu_; |
68 |
|
|
} |
69 |
|
|
|
70 |
|
|
template <typename Scalar> |
71 |
|
|
const boost::shared_ptr<StateAbstractTpl<Scalar> >& |
72 |
|
632470 |
ActuationModelAbstractTpl<Scalar>::get_state() const { |
73 |
|
632470 |
return state_; |
74 |
|
|
} |
75 |
|
|
|
76 |
|
|
template <typename Scalar> |
77 |
|
|
std::ostream& operator<<(std::ostream& os, |
78 |
|
|
const ActuationModelAbstractTpl<Scalar>& model) { |
79 |
|
|
model.print(os); |
80 |
|
|
return os; |
81 |
|
|
} |
82 |
|
|
|
83 |
|
|
template <typename Scalar> |
84 |
|
|
void ActuationModelAbstractTpl<Scalar>::print(std::ostream& os) const { |
85 |
|
|
os << boost::core::demangle(typeid(*this).name()); |
86 |
|
|
} |
87 |
|
|
|
88 |
|
|
} // namespace crocoddyl |