| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | |||
| 2 | /////////////////////////////////////////////////////////////////////////////// | ||
| 3 | // BSD 3-Clause License | ||
| 4 | // | ||
| 5 | // Copyright (C) 2021-2025, LAAS-CNRS, University of Edinburgh, | ||
| 6 | // University of Trento, Heriot-Watt University | ||
| 7 | // Copyright note valid unless otherwise stated in individual files. All | ||
| 8 | // rights reserved. | ||
| 9 | /////////////////////////////////////////////////////////////////////////////// | ||
| 10 | |||
| 11 | #ifndef BINDINGS_PYTHON_CROCODDYL_CORE_CONTROL_BASE_HPP_ | ||
| 12 | #define BINDINGS_PYTHON_CROCODDYL_CORE_CONTROL_BASE_HPP_ | ||
| 13 | |||
| 14 | #include "crocoddyl/core/control-base.hpp" | ||
| 15 | #include "python/crocoddyl/core/core.hpp" | ||
| 16 | |||
| 17 | namespace crocoddyl { | ||
| 18 | namespace python { | ||
| 19 | |||
| 20 | template <typename Scalar> | ||
| 21 | class ControlParametrizationModelAbstractTpl_wrap | ||
| 22 | : public ControlParametrizationModelAbstractTpl<Scalar>, | ||
| 23 | public bp::wrapper<ControlParametrizationModelAbstractTpl<Scalar>> { | ||
| 24 | public: | ||
| 25 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
| 26 | ✗ | CROCODDYL_DERIVED_CAST(ControlParametrizationModelBase, | |
| 27 | ControlParametrizationModelAbstractTpl_wrap) | ||
| 28 | |||
| 29 | typedef typename crocoddyl::ControlParametrizationModelAbstractTpl<Scalar> | ||
| 30 | ControlParamModel; | ||
| 31 | typedef typename crocoddyl::ControlParametrizationDataAbstractTpl<Scalar> | ||
| 32 | ControlParamData; | ||
| 33 | typedef typename ControlParamModel::VectorXs VectorXs; | ||
| 34 | typedef typename ControlParamModel::MatrixXs MatrixXs; | ||
| 35 | using ControlParamModel::nu_; | ||
| 36 | using ControlParamModel::nw_; | ||
| 37 | |||
| 38 | ✗ | ControlParametrizationModelAbstractTpl_wrap(std::size_t nw, std::size_t nu) | |
| 39 | ✗ | : ControlParamModel(nw, nu), bp::wrapper<ControlParamModel>() {} | |
| 40 | |||
| 41 | ✗ | void calc(const std::shared_ptr<ControlParamData>& data, const Scalar t, | |
| 42 | const Eigen::Ref<const VectorXs>& u) const override { | ||
| 43 | ✗ | if (static_cast<std::size_t>(u.size()) != nu_) { | |
| 44 | ✗ | throw_pretty( | |
| 45 | "Invalid argument: " << "u has wrong dimension (it should be " + | ||
| 46 | std::to_string(nu_) + ")"); | ||
| 47 | } | ||
| 48 | ✗ | return bp::call<void>(this->get_override("calc").ptr(), data, t, | |
| 49 | ✗ | (VectorXs)u); | |
| 50 | } | ||
| 51 | |||
| 52 | ✗ | void calcDiff(const std::shared_ptr<ControlParamData>& data, const Scalar t, | |
| 53 | const Eigen::Ref<const VectorXs>& u) const override { | ||
| 54 | ✗ | if (static_cast<std::size_t>(u.size()) != nu_) { | |
| 55 | ✗ | throw_pretty( | |
| 56 | "Invalid argument: " << "u has wrong dimension (it should be " + | ||
| 57 | std::to_string(nu_) + ")"); | ||
| 58 | } | ||
| 59 | ✗ | return bp::call<void>(this->get_override("calcDiff").ptr(), data, t, | |
| 60 | ✗ | (VectorXs)u); | |
| 61 | } | ||
| 62 | |||
| 63 | ✗ | void params(const std::shared_ptr<ControlParamData>& data, const Scalar t, | |
| 64 | const Eigen::Ref<const VectorXs>& w) const override { | ||
| 65 | ✗ | if (static_cast<std::size_t>(w.size()) != nw_) { | |
| 66 | ✗ | throw_pretty( | |
| 67 | "Invalid argument: " << "w has wrong dimension (it should be " + | ||
| 68 | std::to_string(nw_) + ")"); | ||
| 69 | } | ||
| 70 | ✗ | return bp::call<void>(this->get_override("params").ptr(), data, t, | |
| 71 | ✗ | (VectorXs)w); | |
| 72 | } | ||
| 73 | |||
| 74 | ✗ | std::shared_ptr<ControlParamData> createData() override { | |
| 75 | ✗ | enableMultithreading() = false; | |
| 76 | ✗ | if (boost::python::override createData = this->get_override("createData")) { | |
| 77 | ✗ | return bp::call<std::shared_ptr<ControlParamData>>(createData.ptr()); | |
| 78 | } | ||
| 79 | ✗ | return ControlParamModel::createData(); | |
| 80 | } | ||
| 81 | |||
| 82 | ✗ | std::shared_ptr<ControlParamData> default_createData() { | |
| 83 | ✗ | return this->ControlParamModel::createData(); | |
| 84 | } | ||
| 85 | |||
| 86 | ✗ | void convertBounds(const Eigen::Ref<const VectorXs>& w_lb, | |
| 87 | const Eigen::Ref<const VectorXs>& w_ub, | ||
| 88 | Eigen::Ref<VectorXs> u_lb, | ||
| 89 | Eigen::Ref<VectorXs> u_ub) const override { | ||
| 90 | ✗ | bp::list res = convertBounds_wrap(w_lb, w_ub); | |
| 91 | ✗ | u_lb.derived() = bp::extract<VectorXs>(res[0])(); | |
| 92 | ✗ | u_ub.derived() = bp::extract<VectorXs>(res[1])(); | |
| 93 | ✗ | } | |
| 94 | |||
| 95 | ✗ | bp::list convertBounds_wrap(const Eigen::Ref<const VectorXs>& w_lb, | |
| 96 | const Eigen::Ref<const VectorXs>& w_ub) const { | ||
| 97 | ✗ | bp::list p_bounds = | |
| 98 | bp::call<bp::list>(this->get_override("convertBounds").ptr(), | ||
| 99 | ✗ | (VectorXs)w_lb, (VectorXs)w_ub); | |
| 100 | ✗ | return p_bounds; | |
| 101 | } | ||
| 102 | |||
| 103 | ✗ | void multiplyByJacobian(const std::shared_ptr<ControlParamData>& data, | |
| 104 | const Eigen::Ref<const MatrixXs>& A, | ||
| 105 | Eigen::Ref<MatrixXs> out, | ||
| 106 | const AssignmentOp op) const override { | ||
| 107 | ✗ | switch (op) { | |
| 108 | ✗ | case setto: { | |
| 109 | ✗ | out = multiplyByJacobian_wrap(data, A); | |
| 110 | ✗ | break; | |
| 111 | } | ||
| 112 | ✗ | case addto: { | |
| 113 | ✗ | out += multiplyByJacobian_wrap(data, A); | |
| 114 | ✗ | break; | |
| 115 | } | ||
| 116 | ✗ | case rmfrom: { | |
| 117 | ✗ | out -= multiplyByJacobian_wrap(data, A); | |
| 118 | ✗ | break; | |
| 119 | } | ||
| 120 | ✗ | default: { | |
| 121 | ✗ | throw_pretty( | |
| 122 | "Invalid argument: allowed operators: setto, addto, rmfrom"); | ||
| 123 | break; | ||
| 124 | } | ||
| 125 | } | ||
| 126 | ✗ | } | |
| 127 | |||
| 128 | ✗ | MatrixXs multiplyByJacobian_wrap( | |
| 129 | const std::shared_ptr<ControlParamData>& data, | ||
| 130 | const Eigen::Ref<const MatrixXs>& A) const { | ||
| 131 | return bp::call<MatrixXs>(this->get_override("multiplyByJacobian").ptr(), | ||
| 132 | ✗ | data, (MatrixXs)A); | |
| 133 | } | ||
| 134 | |||
| 135 | ✗ | void multiplyJacobianTransposeBy( | |
| 136 | const std::shared_ptr<ControlParamData>& data, | ||
| 137 | const Eigen::Ref<const MatrixXs>& A, Eigen::Ref<MatrixXs> out, | ||
| 138 | const AssignmentOp op) const override { | ||
| 139 | ✗ | switch (op) { | |
| 140 | ✗ | case setto: { | |
| 141 | ✗ | out = multiplyJacobianTransposeBy_wrap(data, A); | |
| 142 | ✗ | break; | |
| 143 | } | ||
| 144 | ✗ | case addto: { | |
| 145 | ✗ | out += multiplyJacobianTransposeBy_wrap(data, A); | |
| 146 | ✗ | break; | |
| 147 | } | ||
| 148 | ✗ | case rmfrom: { | |
| 149 | ✗ | out -= multiplyJacobianTransposeBy_wrap(data, A); | |
| 150 | ✗ | break; | |
| 151 | } | ||
| 152 | ✗ | default: { | |
| 153 | ✗ | throw_pretty( | |
| 154 | "Invalid argument: allowed operators: setto, addto, rmfrom"); | ||
| 155 | break; | ||
| 156 | } | ||
| 157 | } | ||
| 158 | ✗ | } | |
| 159 | |||
| 160 | ✗ | MatrixXs multiplyJacobianTransposeBy_wrap( | |
| 161 | const std::shared_ptr<ControlParamData>& data, | ||
| 162 | const Eigen::Ref<const MatrixXs>& A) const { | ||
| 163 | return bp::call<MatrixXs>( | ||
| 164 | this->get_override("multiplyJacobianTransposeBy").ptr(), data, | ||
| 165 | ✗ | (MatrixXs)A); | |
| 166 | } | ||
| 167 | |||
| 168 | template <typename NewScalar> | ||
| 169 | ✗ | ControlParametrizationModelAbstractTpl_wrap<NewScalar> cast() const { | |
| 170 | typedef ControlParametrizationModelAbstractTpl_wrap<NewScalar> ReturnType; | ||
| 171 | ✗ | ReturnType ret(nw_, nu_); | |
| 172 | ✗ | return ret; | |
| 173 | } | ||
| 174 | }; | ||
| 175 | |||
| 176 | } // namespace python | ||
| 177 | } // namespace crocoddyl | ||
| 178 | |||
| 179 | #endif // BINDINGS_PYTHON_CROCODDYL_CORE_CONTROL_BASE_HPP_ | ||
| 180 |