GCC Code Coverage Report

Directory: ./
File: bindings/python/crocoddyl/core/cost-base.hpp
Date: 2025-03-26 19:23:43
Exec Total Coverage
Lines: 8 58 13.8%
Branches: 6 148 4.1%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2025, LAAS-CNRS, University of Edinburgh,
5 // Heriot-Watt University
6 // Copyright note valid unless otherwise stated in individual files.
7 // All rights reserved.
8 ///////////////////////////////////////////////////////////////////////////////
9
10 #ifndef BINDINGS_PYTHON_CROCODDYL_CORE_COST_BASE_HPP_
11 #define BINDINGS_PYTHON_CROCODDYL_CORE_COST_BASE_HPP_
12
13 #include "crocoddyl/core/cost-base.hpp"
14 #include "python/crocoddyl/core/core.hpp"
15
16 namespace crocoddyl {
17 namespace python {
18
19 template <typename _Scalar>
20 class CostModelAbstractTpl_wrap
21 : public CostModelAbstractTpl<_Scalar>,
22 public bp::wrapper<CostModelAbstractTpl<_Scalar>> {
23 public:
24 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
25 ✗ CROCODDYL_DERIVED_CAST(CostModelBase, CostModelAbstractTpl_wrap)
26
27 typedef _Scalar Scalar;
28 typedef typename ScalarSelector<Scalar>::type ScalarType;
29 typedef typename crocoddyl::CostModelAbstractTpl<Scalar> CostModel;
30 typedef typename crocoddyl::CostDataAbstractTpl<Scalar> CostData;
31 typedef typename CostModel::StateAbstract State;
32 typedef typename CostModel::ActivationModelAbstract ActivationModel;
33 typedef typename CostModel::ResidualModelAbstract ResidualModel;
34 typedef typename CostModel::DataCollectorAbstract DataCollector;
35 typedef typename CostModel::VectorXs VectorXs;
36 using CostModel::activation_;
37 using CostModel::nu_;
38 using CostModel::residual_;
39 using CostModel::state_;
40 using CostModel::unone_;
41
42 ✗ CostModelAbstractTpl_wrap(std::shared_ptr<State> state,
43 std::shared_ptr<ActivationModel> activation,
44 std::shared_ptr<ResidualModel> residual)
45 ✗ : CostModel(state, activation, residual), bp::wrapper<CostModel>() {
46 ✗ unone_ = VectorXs::Constant(nu_, Scalar(NAN));
47 }
48
49 2 CostModelAbstractTpl_wrap(std::shared_ptr<State> state,
50 std::shared_ptr<ActivationModel> activation,
51 const std::size_t nu)
52
1/2
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
 2 : CostModel(state, activation, nu), bp::wrapper<CostModel>() {
53
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 2 unone_ = VectorXs::Constant(nu_, Scalar(NAN));
54 }
55
56 12 CostModelAbstractTpl_wrap(std::shared_ptr<State> state,
57 std::shared_ptr<ActivationModel> activation)
58
1/2
✓ Branch 3 taken 6 times.
✗ Branch 4 not taken.
 12 : CostModel(state, activation), bp::wrapper<CostModel>() {
59
2/4
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
 12 unone_ = VectorXs::Constant(nu_, Scalar(NAN));
60 }
61
62 ✗ CostModelAbstractTpl_wrap(std::shared_ptr<State> state,
63 std::shared_ptr<ResidualModel> residual)
64 ✗ : CostModel(state, residual), bp::wrapper<CostModel>() {
65 ✗ unone_ = VectorXs::Constant(nu_, Scalar(NAN));
66 }
67
68 ✗ CostModelAbstractTpl_wrap(std::shared_ptr<State> state, const std::size_t nr,
69 const std::size_t nu)
70 ✗ : CostModel(state, nr, nu), bp::wrapper<CostModel>() {
71 ✗ unone_ = VectorXs::Constant(nu_, Scalar(NAN));
72 }
73
74 ✗ CostModelAbstractTpl_wrap(std::shared_ptr<State> state, const std::size_t nr)
75 ✗ : CostModel(state, nr), bp::wrapper<CostModel>() {
76 ✗ unone_ = VectorXs::Constant(nu_, Scalar(NAN));
77 }
78
79 ✗ void calc(const std::shared_ptr<CostData>& data,
80 const Eigen::Ref<const VectorXs>& x,
81 const Eigen::Ref<const VectorXs>& u) override {
82 ✗ if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
83 ✗ throw_pretty(
84 "Invalid argument: " << "x has wrong dimension (it should be " +
85 std::to_string(state_->get_nx()) + ")");
86 }
87 ✗ if (static_cast<std::size_t>(u.size()) != nu_) {
88 ✗ throw_pretty(
89 "Invalid argument: " << "u has wrong dimension (it should be " +
90 std::to_string(nu_) + ")");
91 }
92 ✗ if (std::isnan(
93 ✗ scalar_cast<ScalarType>(u.template lpNorm<Eigen::Infinity>()))) {
94 ✗ return bp::call<void>(this->get_override("calc").ptr(), data,
95 ✗ (VectorXs)x);
96 } else {
97 ✗ return bp::call<void>(this->get_override("calc").ptr(), data, (VectorXs)x,
98 ✗ (VectorXs)u);
99 }
100 }
101
102 ✗ void calcDiff(const std::shared_ptr<CostData>& data,
103 const Eigen::Ref<const VectorXs>& x,
104 const Eigen::Ref<const VectorXs>& u) override {
105 ✗ if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
106 ✗ throw_pretty(
107 "Invalid argument: " << "x has wrong dimension (it should be " +
108 std::to_string(state_->get_nx()) + ")");
109 }
110 ✗ if (static_cast<std::size_t>(u.size()) != nu_) {
111 ✗ throw_pretty(
112 "Invalid argument: " << "u has wrong dimension (it should be " +
113 std::to_string(nu_) + ")");
114 }
115 ✗ if (std::isnan(
116 ✗ scalar_cast<ScalarType>(u.template lpNorm<Eigen::Infinity>()))) {
117 ✗ return bp::call<void>(this->get_override("calcDiff").ptr(), data,
118 ✗ (VectorXs)x);
119 } else {
120 ✗ return bp::call<void>(this->get_override("calcDiff").ptr(), data,
121 ✗ (VectorXs)x, (VectorXs)u);
122 }
123 }
124
125 ✗ std::shared_ptr<CostData> createData(DataCollector* const data) override {
126 ✗ enableMultithreading() = false;
127 ✗ if (boost::python::override createData = this->get_override("createData")) {
128 return bp::call<std::shared_ptr<CostData>>(createData.ptr(),
129 ✗ boost::ref(data));
130 }
131 ✗ return CostModel::createData(data);
132 }
133
134 30 std::shared_ptr<CostData> default_createData(DataCollector* const data) {
135 30 return this->CostModel::createData(data);
136 }
137
138 template <typename NewScalar>
139 ✗ CostModelAbstractTpl_wrap<NewScalar> cast() const {
140 typedef CostModelAbstractTpl_wrap<NewScalar> ReturnType;
141 ✗ if (residual_) {
142 ✗ ReturnType ret(state_->template cast<NewScalar>(),
143 ✗ activation_->template cast<NewScalar>(),
144 ✗ residual_->template cast<NewScalar>());
145 ✗ return ret;
146 ✗ } else {
147 ✗ ReturnType ret(state_->template cast<NewScalar>(),
148 ✗ activation_->template cast<NewScalar>(), nu_);
149 ✗ return ret;
150 }
151 }
152 };
153
154 } // namespace python
155 } // namespace crocoddyl
156
157 #endif // BINDINGS_PYTHON_CROCODDYL_MULTIBODY_COST_BASE_HPP_
158