| Directory: | ./ |
|---|---|
| File: | include/crocoddyl/core/costs/residual.hxx |
| Date: | 2025-01-16 08:47:40 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 40 | 41 | 97.6% |
| Branches: | 13 | 22 | 59.1% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 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 | 5372 | CostModelResidualTpl<Scalar>::CostModelResidualTpl( | |
| 15 | boost::shared_ptr<typename Base::StateAbstract> state, | ||
| 16 | boost::shared_ptr<ActivationModelAbstract> activation, | ||
| 17 | boost::shared_ptr<ResidualModelAbstract> residual) | ||
| 18 |
1/2✓ Branch 4 taken 5372 times.
✗ Branch 5 not taken.
|
5372 | : Base(state, activation, residual) {} |
| 19 | |||
| 20 | template <typename Scalar> | ||
| 21 | 1673 | CostModelResidualTpl<Scalar>::CostModelResidualTpl( | |
| 22 | boost::shared_ptr<typename Base::StateAbstract> state, | ||
| 23 | boost::shared_ptr<ResidualModelAbstract> residual) | ||
| 24 |
1/2✓ Branch 3 taken 1673 times.
✗ Branch 4 not taken.
|
1673 | : Base(state, residual) {} |
| 25 | |||
| 26 | template <typename Scalar> | ||
| 27 | 14094 | CostModelResidualTpl<Scalar>::~CostModelResidualTpl() {} | |
| 28 | |||
| 29 | template <typename Scalar> | ||
| 30 | 376818 | 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 | 376818 | residual_->calc(data->residual, x, u); | |
| 35 | |||
| 36 | // Compute the cost | ||
| 37 |
1/2✓ Branch 6 taken 376818 times.
✗ Branch 7 not taken.
|
376818 | activation_->calc(data->activation, data->residual->r); |
| 38 | 376818 | data->cost = data->activation->a_value; | |
| 39 | 376818 | } | |
| 40 | |||
| 41 | template <typename Scalar> | ||
| 42 | 83131 | void CostModelResidualTpl<Scalar>::calc( | |
| 43 | const boost::shared_ptr<CostDataAbstract>& data, | ||
| 44 | const Eigen::Ref<const VectorXs>& x) { | ||
| 45 | 83131 | const bool is_rq = residual_->get_q_dependent(); | |
| 46 | 83131 | const bool is_rv = residual_->get_v_dependent(); | |
| 47 |
3/4✓ Branch 0 taken 24011 times.
✓ Branch 1 taken 59120 times.
✓ Branch 2 taken 24011 times.
✗ Branch 3 not taken.
|
83131 | if (!is_rq && !is_rv) { |
| 48 | 24011 | data->activation->a_value = 0.; | |
| 49 | 24011 | data->cost = 0.; | |
| 50 | 24011 | return; // do nothing | |
| 51 | } | ||
| 52 | |||
| 53 | // Compute the cost residual | ||
| 54 | 59120 | residual_->calc(data->residual, x); | |
| 55 | |||
| 56 | // Compute the cost | ||
| 57 |
1/2✓ Branch 6 taken 59120 times.
✗ Branch 7 not taken.
|
59120 | activation_->calc(data->activation, data->residual->r); |
| 58 | 59120 | data->cost = data->activation->a_value; | |
| 59 | } | ||
| 60 | |||
| 61 | template <typename Scalar> | ||
| 62 | 61356 | 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 | 61356 | residual_->calcDiff(data->residual, x, u); | |
| 68 |
1/2✓ Branch 6 taken 61356 times.
✗ Branch 7 not taken.
|
61356 | activation_->calcDiff(data->activation, data->residual->r); |
| 69 | |||
| 70 | // Compute the derivatives of the cost function based on a Gauss-Newton | ||
| 71 | // approximation | ||
| 72 | 61356 | residual_->calcCostDiff(data, data->residual, data->activation); | |
| 73 | 61356 | } | |
| 74 | |||
| 75 | template <typename Scalar> | ||
| 76 | 3207 | 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 | 3207 | const bool is_rq = residual_->get_q_dependent(); | |
| 82 | 3207 | const bool is_rv = residual_->get_v_dependent(); | |
| 83 |
3/4✓ Branch 0 taken 1185 times.
✓ Branch 1 taken 2022 times.
✓ Branch 2 taken 1185 times.
✗ Branch 3 not taken.
|
3207 | if (!is_rq && !is_rv) { |
| 84 | 1185 | data->Lx.setZero(); | |
| 85 | 1185 | data->Lxx.setZero(); | |
| 86 | 1185 | return; // do nothing | |
| 87 | } | ||
| 88 | 2022 | residual_->calcDiff(data->residual, x); | |
| 89 |
1/2✓ Branch 6 taken 2022 times.
✗ Branch 7 not taken.
|
2022 | activation_->calcDiff(data->activation, data->residual->r); |
| 90 | |||
| 91 | // Compute the derivatives of the cost function based on a Gauss-Newton | ||
| 92 | // approximation | ||
| 93 | 2022 | residual_->calcCostDiff(data, data->residual, data->activation, false); | |
| 94 | } | ||
| 95 | |||
| 96 | template <typename Scalar> | ||
| 97 | boost::shared_ptr<CostDataAbstractTpl<Scalar> > | ||
| 98 | 476960 | CostModelResidualTpl<Scalar>::createData(DataCollectorAbstract* const data) { | |
| 99 | ✗ | return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this, | |
| 100 |
1/2✓ Branch 2 taken 476960 times.
✗ Branch 3 not taken.
|
476960 | 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 | ||
| 109 |