1 |
|
|
/////////////////////////////////////////////////////////////////////////////// |
2 |
|
|
// BSD 3-Clause License |
3 |
|
|
// |
4 |
|
|
// Copyright (C) 2021-2023, University of Edinburgh, INRIA, |
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 |
|
|
|
12 |
|
|
namespace crocoddyl { |
13 |
|
|
template <typename Scalar> |
14 |
|
5356 |
CostModelResidualTpl<Scalar>::CostModelResidualTpl( |
15 |
|
|
boost::shared_ptr<typename Base::StateAbstract> state, |
16 |
|
|
boost::shared_ptr<ActivationModelAbstract> activation, |
17 |
|
|
boost::shared_ptr<ResidualModelAbstract> residual) |
18 |
✓✗ |
5356 |
: Base(state, activation, residual) {} |
19 |
|
|
|
20 |
|
|
template <typename Scalar> |
21 |
|
1661 |
CostModelResidualTpl<Scalar>::CostModelResidualTpl( |
22 |
|
|
boost::shared_ptr<typename Base::StateAbstract> state, |
23 |
|
|
boost::shared_ptr<ResidualModelAbstract> residual) |
24 |
✓✗ |
1661 |
: Base(state, residual) {} |
25 |
|
|
|
26 |
|
|
template <typename Scalar> |
27 |
|
14038 |
CostModelResidualTpl<Scalar>::~CostModelResidualTpl() {} |
28 |
|
|
|
29 |
|
|
template <typename Scalar> |
30 |
|
374262 |
void CostModelResidualTpl<Scalar>::calc( |
31 |
|
|
const boost::shared_ptr<CostDataAbstract>& data, |
32 |
|
|
const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) { |
33 |
|
|
// Compute the cost residual |
34 |
|
374262 |
residual_->calc(data->residual, x, u); |
35 |
|
|
|
36 |
|
|
// Compute the cost |
37 |
✓✗ |
374262 |
activation_->calc(data->activation, data->residual->r); |
38 |
|
374262 |
data->cost = data->activation->a_value; |
39 |
|
374262 |
} |
40 |
|
|
|
41 |
|
|
template <typename Scalar> |
42 |
|
83113 |
void CostModelResidualTpl<Scalar>::calc( |
43 |
|
|
const boost::shared_ptr<CostDataAbstract>& data, |
44 |
|
|
const Eigen::Ref<const VectorXs>& x) { |
45 |
|
83113 |
const bool is_rq = residual_->get_q_dependent(); |
46 |
|
83113 |
const bool is_rv = residual_->get_v_dependent(); |
47 |
✓✓✓✗
|
83113 |
if (!is_rq && !is_rv) { |
48 |
|
24009 |
data->activation->a_value = 0.; |
49 |
|
24009 |
data->cost = 0.; |
50 |
|
24009 |
return; // do nothing |
51 |
|
|
} |
52 |
|
|
|
53 |
|
|
// Compute the cost residual |
54 |
|
59104 |
residual_->calc(data->residual, x); |
55 |
|
|
|
56 |
|
|
// Compute the cost |
57 |
✓✗ |
59104 |
activation_->calc(data->activation, data->residual->r); |
58 |
|
59104 |
data->cost = data->activation->a_value; |
59 |
|
|
} |
60 |
|
|
|
61 |
|
|
template <typename Scalar> |
62 |
|
60090 |
void CostModelResidualTpl<Scalar>::calcDiff( |
63 |
|
|
const boost::shared_ptr<CostDataAbstract>& data, |
64 |
|
|
const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) { |
65 |
|
|
// Compute the derivatives of the activation and contact wrench cone residual |
66 |
|
|
// models |
67 |
|
60090 |
residual_->calcDiff(data->residual, x, u); |
68 |
✓✗ |
60090 |
activation_->calcDiff(data->activation, data->residual->r); |
69 |
|
|
|
70 |
|
|
// Compute the derivatives of the cost function based on a Gauss-Newton |
71 |
|
|
// approximation |
72 |
|
60090 |
residual_->calcCostDiff(data, data->residual, data->activation); |
73 |
|
60090 |
} |
74 |
|
|
|
75 |
|
|
template <typename Scalar> |
76 |
|
3189 |
void CostModelResidualTpl<Scalar>::calcDiff( |
77 |
|
|
const boost::shared_ptr<CostDataAbstract>& data, |
78 |
|
|
const Eigen::Ref<const VectorXs>& x) { |
79 |
|
|
// Compute the derivatives of the activation and contact wrench cone residual |
80 |
|
|
// models |
81 |
|
3189 |
const bool is_rq = residual_->get_q_dependent(); |
82 |
|
3189 |
const bool is_rv = residual_->get_v_dependent(); |
83 |
✓✓✓✗
|
3189 |
if (!is_rq && !is_rv) { |
84 |
|
1183 |
data->Lx.setZero(); |
85 |
|
1183 |
data->Lxx.setZero(); |
86 |
|
1183 |
return; // do nothing |
87 |
|
|
} |
88 |
|
2006 |
residual_->calcDiff(data->residual, x); |
89 |
✓✗ |
2006 |
activation_->calcDiff(data->activation, data->residual->r); |
90 |
|
|
|
91 |
|
|
// Compute the derivatives of the cost function based on a Gauss-Newton |
92 |
|
|
// approximation |
93 |
|
2006 |
residual_->calcCostDiff(data, data->residual, data->activation, false); |
94 |
|
|
} |
95 |
|
|
|
96 |
|
|
template <typename Scalar> |
97 |
|
|
boost::shared_ptr<CostDataAbstractTpl<Scalar> > |
98 |
|
476270 |
CostModelResidualTpl<Scalar>::createData(DataCollectorAbstract* const data) { |
99 |
|
|
return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this, |
100 |
✓✗ |
476270 |
data); |
101 |
|
|
} |
102 |
|
|
|
103 |
|
|
template <typename Scalar> |
104 |
|
326 |
void CostModelResidualTpl<Scalar>::print(std::ostream& os) const { |
105 |
|
326 |
os << "CostModelResidual {" << *residual_ << ", " << *activation_ << "}"; |
106 |
|
326 |
} |
107 |
|
|
|
108 |
|
|
} // namespace crocoddyl |