| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | /////////////////////////////////////////////////////////////////////////////// | ||
| 2 | // BSD 3-Clause License | ||
| 3 | // | ||
| 4 | // Copyright (C) 2021-2025, University of Edinburgh, Heriot-Watt University | ||
| 5 | // Copyright note valid unless otherwise stated in individual files. | ||
| 6 | // All rights reserved. | ||
| 7 | /////////////////////////////////////////////////////////////////////////////// | ||
| 8 | |||
| 9 | namespace crocoddyl { | ||
| 10 | |||
| 11 | template <typename Scalar> | ||
| 12 | ✗ | ResidualModelAbstractTpl<Scalar>::ResidualModelAbstractTpl( | |
| 13 | std::shared_ptr<StateAbstract> state, const std::size_t nr, | ||
| 14 | const std::size_t nu, const bool q_dependent, const bool v_dependent, | ||
| 15 | const bool u_dependent) | ||
| 16 | ✗ | : state_(state), | |
| 17 | ✗ | nr_(nr), | |
| 18 | ✗ | nu_(nu), | |
| 19 | ✗ | unone_(VectorXs::Zero(nu)), | |
| 20 | ✗ | q_dependent_(q_dependent), | |
| 21 | ✗ | v_dependent_(v_dependent), | |
| 22 | ✗ | u_dependent_(u_dependent) {} | |
| 23 | |||
| 24 | template <typename Scalar> | ||
| 25 | ✗ | ResidualModelAbstractTpl<Scalar>::ResidualModelAbstractTpl( | |
| 26 | std::shared_ptr<StateAbstract> state, const std::size_t nr, | ||
| 27 | const bool q_dependent, const bool v_dependent, const bool u_dependent) | ||
| 28 | ✗ | : state_(state), | |
| 29 | ✗ | nr_(nr), | |
| 30 | ✗ | nu_(state->get_nv()), | |
| 31 | ✗ | unone_(VectorXs::Zero(state->get_nv())), | |
| 32 | ✗ | q_dependent_(q_dependent), | |
| 33 | ✗ | v_dependent_(v_dependent), | |
| 34 | ✗ | u_dependent_(u_dependent) {} | |
| 35 | |||
| 36 | template <typename Scalar> | ||
| 37 | ✗ | void ResidualModelAbstractTpl<Scalar>::calc( | |
| 38 | const std::shared_ptr<ResidualDataAbstract>&, | ||
| 39 | ✗ | const Eigen::Ref<const VectorXs>&, const Eigen::Ref<const VectorXs>&) {} | |
| 40 | |||
| 41 | template <typename Scalar> | ||
| 42 | ✗ | void ResidualModelAbstractTpl<Scalar>::calc( | |
| 43 | const std::shared_ptr<ResidualDataAbstract>& data, | ||
| 44 | const Eigen::Ref<const VectorXs>& x) { | ||
| 45 | ✗ | calc(data, x, unone_); | |
| 46 | ✗ | } | |
| 47 | |||
| 48 | template <typename Scalar> | ||
| 49 | ✗ | void ResidualModelAbstractTpl<Scalar>::calcDiff( | |
| 50 | const std::shared_ptr<ResidualDataAbstract>&, | ||
| 51 | ✗ | const Eigen::Ref<const VectorXs>&, const Eigen::Ref<const VectorXs>&) {} | |
| 52 | |||
| 53 | template <typename Scalar> | ||
| 54 | ✗ | void ResidualModelAbstractTpl<Scalar>::calcDiff( | |
| 55 | const std::shared_ptr<ResidualDataAbstract>& data, | ||
| 56 | const Eigen::Ref<const VectorXs>& x) { | ||
| 57 | ✗ | calcDiff(data, x, unone_); | |
| 58 | ✗ | } | |
| 59 | |||
| 60 | template <typename Scalar> | ||
| 61 | std::shared_ptr<ResidualDataAbstractTpl<Scalar> > | ||
| 62 | ✗ | ResidualModelAbstractTpl<Scalar>::createData( | |
| 63 | DataCollectorAbstract* const data) { | ||
| 64 | return std::allocate_shared<ResidualDataAbstract>( | ||
| 65 | ✗ | Eigen::aligned_allocator<ResidualDataAbstract>(), this, data); | |
| 66 | } | ||
| 67 | |||
| 68 | template <typename Scalar> | ||
| 69 | ✗ | void ResidualModelAbstractTpl<Scalar>::calcCostDiff( | |
| 70 | const std::shared_ptr<CostDataAbstract>& cdata, | ||
| 71 | const std::shared_ptr<ResidualDataAbstract>& rdata, | ||
| 72 | const std::shared_ptr<ActivationDataAbstract>& adata, const bool update_u) { | ||
| 73 | // This function computes the derivatives of the cost function based on a | ||
| 74 | // Gauss-Newton approximation | ||
| 75 | ✗ | const bool is_ru = u_dependent_ && nu_ != 0 && update_u; | |
| 76 | ✗ | const std::size_t nv = state_->get_nv(); | |
| 77 | ✗ | if (is_ru) { | |
| 78 | ✗ | cdata->Lu.noalias() = rdata->Ru.transpose() * adata->Ar; | |
| 79 | ✗ | rdata->Arr_Ru.noalias() = adata->Arr.diagonal().asDiagonal() * rdata->Ru; | |
| 80 | ✗ | cdata->Luu.noalias() = rdata->Ru.transpose() * rdata->Arr_Ru; | |
| 81 | } | ||
| 82 | ✗ | if (q_dependent_ && v_dependent_) { | |
| 83 | ✗ | cdata->Lx.noalias() = rdata->Rx.transpose() * adata->Ar; | |
| 84 | ✗ | rdata->Arr_Rx.noalias() = adata->Arr.diagonal().asDiagonal() * rdata->Rx; | |
| 85 | ✗ | cdata->Lxx.noalias() = rdata->Rx.transpose() * rdata->Arr_Rx; | |
| 86 | ✗ | if (is_ru) { | |
| 87 | ✗ | cdata->Lxu.noalias() = rdata->Rx.transpose() * rdata->Arr_Ru; | |
| 88 | } | ||
| 89 | ✗ | } else if (q_dependent_) { | |
| 90 | Eigen::Block<MatrixXs, Eigen::Dynamic, Eigen::Dynamic, true> Rq = | ||
| 91 | ✗ | rdata->Rx.leftCols(nv); | |
| 92 | ✗ | cdata->Lx.head(nv).noalias() = Rq.transpose() * adata->Ar; | |
| 93 | ✗ | rdata->Arr_Rx.leftCols(nv).noalias() = | |
| 94 | ✗ | adata->Arr.diagonal().asDiagonal() * Rq; | |
| 95 | ✗ | cdata->Lxx.topLeftCorner(nv, nv).noalias() = | |
| 96 | ✗ | Rq.transpose() * rdata->Arr_Rx.leftCols(nv); | |
| 97 | ✗ | if (is_ru) { | |
| 98 | ✗ | cdata->Lxu.topRows(nv).noalias() = Rq.transpose() * rdata->Arr_Ru; | |
| 99 | } | ||
| 100 | ✗ | } else if (v_dependent_) { | |
| 101 | Eigen::Block<MatrixXs, Eigen::Dynamic, Eigen::Dynamic, true> Rv = | ||
| 102 | ✗ | rdata->Rx.rightCols(nv); | |
| 103 | ✗ | cdata->Lx.tail(nv).noalias() = Rv.transpose() * adata->Ar; | |
| 104 | ✗ | rdata->Arr_Rx.rightCols(nv).noalias() = | |
| 105 | ✗ | adata->Arr.diagonal().asDiagonal() * Rv; | |
| 106 | ✗ | cdata->Lxx.bottomRightCorner(nv, nv).noalias() = | |
| 107 | ✗ | Rv.transpose() * rdata->Arr_Rx.rightCols(nv); | |
| 108 | ✗ | if (is_ru) { | |
| 109 | ✗ | cdata->Lxu.bottomRows(nv).noalias() = Rv.transpose() * rdata->Arr_Ru; | |
| 110 | } | ||
| 111 | } | ||
| 112 | ✗ | } | |
| 113 | |||
| 114 | template <typename Scalar> | ||
| 115 | ✗ | void ResidualModelAbstractTpl<Scalar>::print(std::ostream& os) const { | |
| 116 | ✗ | os << boost::core::demangle(typeid(*this).name()); | |
| 117 | ✗ | } | |
| 118 | |||
| 119 | template <typename Scalar> | ||
| 120 | const std::shared_ptr<StateAbstractTpl<Scalar> >& | ||
| 121 | ✗ | ResidualModelAbstractTpl<Scalar>::get_state() const { | |
| 122 | ✗ | return state_; | |
| 123 | } | ||
| 124 | |||
| 125 | template <typename Scalar> | ||
| 126 | ✗ | std::size_t ResidualModelAbstractTpl<Scalar>::get_nr() const { | |
| 127 | ✗ | return nr_; | |
| 128 | } | ||
| 129 | |||
| 130 | template <typename Scalar> | ||
| 131 | ✗ | std::size_t ResidualModelAbstractTpl<Scalar>::get_nu() const { | |
| 132 | ✗ | return nu_; | |
| 133 | } | ||
| 134 | |||
| 135 | template <typename Scalar> | ||
| 136 | ✗ | bool ResidualModelAbstractTpl<Scalar>::get_q_dependent() const { | |
| 137 | ✗ | return q_dependent_; | |
| 138 | } | ||
| 139 | |||
| 140 | template <typename Scalar> | ||
| 141 | ✗ | bool ResidualModelAbstractTpl<Scalar>::get_v_dependent() const { | |
| 142 | ✗ | return v_dependent_; | |
| 143 | } | ||
| 144 | |||
| 145 | template <typename Scalar> | ||
| 146 | ✗ | bool ResidualModelAbstractTpl<Scalar>::get_u_dependent() const { | |
| 147 | ✗ | return u_dependent_; | |
| 148 | } | ||
| 149 | |||
| 150 | template <typename Scalar> | ||
| 151 | ✗ | std::ostream& operator<<(std::ostream& os, | |
| 152 | const ResidualModelAbstractTpl<Scalar>& model) { | ||
| 153 | ✗ | model.print(os); | |
| 154 | ✗ | return os; | |
| 155 | } | ||
| 156 | |||
| 157 | } // namespace crocoddyl | ||
| 158 |