1 |
|
|
/////////////////////////////////////////////////////////////////////////////// |
2 |
|
|
// BSD 3-Clause License |
3 |
|
|
// |
4 |
|
|
// Copyright (C) 2020-2022, LAAS-CNRS, University of Edinburgh |
5 |
|
|
// Copyright note valid unless otherwise stated in individual files. |
6 |
|
|
// All rights reserved. |
7 |
|
|
/////////////////////////////////////////////////////////////////////////////// |
8 |
|
|
|
9 |
|
|
#include <pinocchio/algorithm/rnea-derivatives.hpp> |
10 |
|
|
#include <pinocchio/algorithm/rnea.hpp> |
11 |
|
|
|
12 |
|
|
namespace crocoddyl { |
13 |
|
|
|
14 |
|
|
template <typename Scalar> |
15 |
|
99 |
ResidualModelControlGravTpl<Scalar>::ResidualModelControlGravTpl( |
16 |
|
|
boost::shared_ptr<StateMultibody> state, const std::size_t nu) |
17 |
|
99 |
: Base(state, state->get_nv(), nu, true, false), |
18 |
✓✗✓✗
|
99 |
pin_model_(*state->get_pinocchio()) { |
19 |
✗✓ |
99 |
if (nu_ == 0) { |
20 |
|
|
throw_pretty("Invalid argument: " |
21 |
|
|
<< "it seems to be an autonomous system, if so, don't add " |
22 |
|
|
"this residual function"); |
23 |
|
|
} |
24 |
|
99 |
} |
25 |
|
|
|
26 |
|
|
template <typename Scalar> |
27 |
|
1 |
ResidualModelControlGravTpl<Scalar>::ResidualModelControlGravTpl( |
28 |
|
|
boost::shared_ptr<StateMultibody> state) |
29 |
|
1 |
: Base(state, state->get_nv(), state->get_nv(), true, false), |
30 |
✓✗✓✗
|
2 |
pin_model_(*state->get_pinocchio()) {} |
31 |
|
|
|
32 |
|
|
template <typename Scalar> |
33 |
|
204 |
ResidualModelControlGravTpl<Scalar>::~ResidualModelControlGravTpl() {} |
34 |
|
|
|
35 |
|
|
template <typename Scalar> |
36 |
|
1498 |
void ResidualModelControlGravTpl<Scalar>::calc( |
37 |
|
|
const boost::shared_ptr<ResidualDataAbstract> &data, |
38 |
|
|
const Eigen::Ref<const VectorXs> &x, const Eigen::Ref<const VectorXs> &) { |
39 |
|
1498 |
Data *d = static_cast<Data *>(data.get()); |
40 |
|
|
|
41 |
✓✗ |
1498 |
const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q = |
42 |
|
|
x.head(state_->get_nq()); |
43 |
✓✗✓✗
|
1498 |
data->r = d->actuation->tau - |
44 |
✓✗ |
1498 |
pinocchio::computeGeneralizedGravity(pin_model_, d->pinocchio, q); |
45 |
|
1498 |
} |
46 |
|
|
|
47 |
|
|
template <typename Scalar> |
48 |
|
968 |
void ResidualModelControlGravTpl<Scalar>::calc( |
49 |
|
|
const boost::shared_ptr<ResidualDataAbstract> &data, |
50 |
|
|
const Eigen::Ref<const VectorXs> &x) { |
51 |
|
968 |
Data *d = static_cast<Data *>(data.get()); |
52 |
|
|
|
53 |
✓✗ |
968 |
const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q = |
54 |
|
|
x.head(state_->get_nq()); |
55 |
✓✗✓✗
✓✗ |
968 |
data->r = -pinocchio::computeGeneralizedGravity(pin_model_, d->pinocchio, q); |
56 |
|
968 |
} |
57 |
|
|
|
58 |
|
|
template <typename Scalar> |
59 |
|
45 |
void ResidualModelControlGravTpl<Scalar>::calcDiff( |
60 |
|
|
const boost::shared_ptr<ResidualDataAbstract> &data, |
61 |
|
|
const Eigen::Ref<const VectorXs> &x, const Eigen::Ref<const VectorXs> &) { |
62 |
|
45 |
Data *d = static_cast<Data *>(data.get()); |
63 |
|
|
|
64 |
|
|
// Compute the derivatives of the residual residual |
65 |
✓✗ |
45 |
const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q = |
66 |
|
|
x.head(state_->get_nq()); |
67 |
✓✗ |
45 |
Eigen::Block<MatrixXs, Eigen::Dynamic, Eigen::Dynamic, true> Rq = |
68 |
|
45 |
data->Rx.leftCols(state_->get_nv()); |
69 |
✓✗ |
45 |
pinocchio::computeGeneralizedGravityDerivatives(pin_model_, d->pinocchio, q, |
70 |
|
|
Rq); |
71 |
✓✗ |
45 |
Rq *= -1; |
72 |
✓✗ |
45 |
data->Ru = d->actuation->dtau_du; |
73 |
|
45 |
} |
74 |
|
|
|
75 |
|
|
template <typename Scalar> |
76 |
|
27 |
void ResidualModelControlGravTpl<Scalar>::calcDiff( |
77 |
|
|
const boost::shared_ptr<ResidualDataAbstract> &data, |
78 |
|
|
const Eigen::Ref<const VectorXs> &x) { |
79 |
|
27 |
Data *d = static_cast<Data *>(data.get()); |
80 |
|
|
|
81 |
|
|
// Compute the derivatives of the residual residual |
82 |
✓✗ |
27 |
const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q = |
83 |
|
|
x.head(state_->get_nq()); |
84 |
✓✗ |
27 |
Eigen::Block<MatrixXs, Eigen::Dynamic, Eigen::Dynamic, true> Rq = |
85 |
|
27 |
data->Rx.leftCols(state_->get_nv()); |
86 |
✓✗ |
27 |
pinocchio::computeGeneralizedGravityDerivatives(pin_model_, d->pinocchio, q, |
87 |
|
|
Rq); |
88 |
✓✗ |
27 |
Rq *= -1; |
89 |
|
27 |
} |
90 |
|
|
|
91 |
|
|
template <typename Scalar> |
92 |
|
|
boost::shared_ptr<ResidualDataAbstractTpl<Scalar> > |
93 |
|
2845 |
ResidualModelControlGravTpl<Scalar>::createData( |
94 |
|
|
DataCollectorAbstract *const data) { |
95 |
|
|
return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this, |
96 |
✓✗ |
2845 |
data); |
97 |
|
|
} |
98 |
|
|
|
99 |
|
|
template <typename Scalar> |
100 |
|
27 |
void ResidualModelControlGravTpl<Scalar>::print(std::ostream &os) const { |
101 |
|
27 |
os << "ResidualModelControlGrav"; |
102 |
|
27 |
} |
103 |
|
|
|
104 |
|
|
} // namespace crocoddyl |