GCC Code Coverage Report

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