GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/core/integ-action-base.hxx
Date: 2025-05-13 10:30:51
Exec Total Coverage
Lines: 0 65 0.0%
Branches: 0 150 0.0%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2021-2025, LAAS-CNRS, University of Edinburgh,
5 // University of Oxford, University of Trento,
6 // Heriot-Watt University
7 // Copyright note valid unless otherwise stated in individual files.
8 // All rights reserved.
9 ///////////////////////////////////////////////////////////////////////////////
10
11 #include "crocoddyl/core/controls/poly-zero.hpp"
12 #include "crocoddyl/core/integ-action-base.hpp"
13
14 namespace crocoddyl {
15
16 template <typename Scalar>
17 ✗ IntegratedActionModelAbstractTpl<Scalar>::IntegratedActionModelAbstractTpl(
18 std::shared_ptr<DifferentialActionModelAbstract> model,
19 std::shared_ptr<ControlParametrizationModelAbstract> control,
20 const Scalar time_step, const bool with_cost_residual)
21 : Base(model->get_state(), control->get_nu(), model->get_nr(),
22 ✗ model->get_ng(), model->get_nh()),
23 ✗ differential_(model),
24 ✗ control_(control),
25 ✗ time_step_(time_step),
26 ✗ with_cost_residual_(with_cost_residual) {
27 ✗ if (control->get_nw() != model->get_nu()) {
28 ✗ throw_pretty("Invalid argument: "
29 << "control.nw (" + std::to_string(control->get_nw()) +
30 ") is not equal to model.nu (" +
31 std::to_string(model->get_nu()) + ")");
32 }
33 ✗ init();
34 }
35
36 template <typename Scalar>
37 ✗ IntegratedActionModelAbstractTpl<Scalar>::IntegratedActionModelAbstractTpl(
38 std::shared_ptr<DifferentialActionModelAbstract> model,
39 const Scalar time_step, const bool with_cost_residual)
40 : Base(model->get_state(), model->get_nu(), model->get_nr(),
41 ✗ model->get_ng(), model->get_nh()),
42 ✗ differential_(model),
43 ✗ control_(
44 ✗ new ControlParametrizationModelPolyZeroTpl<Scalar>(model->get_nu())),
45 ✗ time_step_(time_step),
46 ✗ with_cost_residual_(with_cost_residual) {
47 ✗ init();
48 }
49
50 template <typename Scalar>
51 ✗ void IntegratedActionModelAbstractTpl<Scalar>::init() {
52 ✗ time_step2_ = time_step_ * time_step_;
53 ✗ VectorXs u_lb(nu_), u_ub(nu_);
54 ✗ control_->convertBounds(differential_->get_u_lb(), differential_->get_u_ub(),
55 u_lb, u_ub);
56 ✗ Base::set_u_lb(u_lb);
57 ✗ Base::set_u_ub(u_ub);
58 ✗ if (time_step_ < Scalar(0.)) {
59 ✗ time_step_ = Scalar(1e-3);
60 ✗ time_step2_ = time_step_ * time_step_;
61 ✗ std::cerr << "Warning: dt should be positive, set to 1e-3" << std::endl;
62 }
63 }
64
65 template <typename Scalar>
66 std::shared_ptr<ActionDataAbstractTpl<Scalar> >
67 ✗ IntegratedActionModelAbstractTpl<Scalar>::createData() {
68 ✗ if (control_->get_nu() > differential_->get_nu())
69 ✗ std::cerr << "Warning: It is useless to use an Euler integrator with a "
70 "control parametrization larger than PolyZero"
71 ✗ << std::endl;
72 ✗ return std::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
73 }
74
75 template <typename Scalar>
76 ✗ std::size_t IntegratedActionModelAbstractTpl<Scalar>::get_ng() const {
77 ✗ return differential_->get_ng();
78 }
79
80 template <typename Scalar>
81 ✗ std::size_t IntegratedActionModelAbstractTpl<Scalar>::get_nh() const {
82 ✗ return differential_->get_nh();
83 }
84
85 template <typename Scalar>
86 ✗ std::size_t IntegratedActionModelAbstractTpl<Scalar>::get_ng_T() const {
87 ✗ return differential_->get_ng_T();
88 }
89
90 template <typename Scalar>
91 ✗ std::size_t IntegratedActionModelAbstractTpl<Scalar>::get_nh_T() const {
92 ✗ return differential_->get_nh_T();
93 }
94
95 template <typename Scalar>
96 const typename MathBaseTpl<Scalar>::VectorXs&
97 ✗ IntegratedActionModelAbstractTpl<Scalar>::get_g_lb() const {
98 ✗ return differential_->get_g_lb();
99 }
100
101 template <typename Scalar>
102 const typename MathBaseTpl<Scalar>::VectorXs&
103 ✗ IntegratedActionModelAbstractTpl<Scalar>::get_g_ub() const {
104 ✗ return differential_->get_g_ub();
105 }
106
107 template <typename Scalar>
108 const std::shared_ptr<DifferentialActionModelAbstractTpl<Scalar> >&
109 ✗ IntegratedActionModelAbstractTpl<Scalar>::get_differential() const {
110 ✗ return differential_;
111 }
112
113 template <typename Scalar>
114 const std::shared_ptr<ControlParametrizationModelAbstractTpl<Scalar> >&
115 ✗ IntegratedActionModelAbstractTpl<Scalar>::get_control() const {
116 ✗ return control_;
117 }
118
119 template <typename Scalar>
120 ✗ const Scalar IntegratedActionModelAbstractTpl<Scalar>::get_dt() const {
121 ✗ return time_step_;
122 }
123
124 template <typename Scalar>
125 ✗ void IntegratedActionModelAbstractTpl<Scalar>::set_dt(const Scalar dt) {
126 ✗ if (dt < 0.) {
127 ✗ throw_pretty("Invalid argument: " << "dt has positive value");
128 }
129 ✗ time_step_ = dt;
130 ✗ time_step2_ = dt * dt;
131 }
132
133 template <typename Scalar>
134 ✗ void IntegratedActionModelAbstractTpl<Scalar>::set_differential(
135 std::shared_ptr<DifferentialActionModelAbstract> model) {
136 ✗ if (control_->get_nw() != model->get_nu()) {
137 ✗ throw_pretty("Invalid argument: "
138 << "control.nw (" + std::to_string(control_->get_nw()) +
139 ") is not equal to model.nu (" +
140 std::to_string(model->get_nu()) + ")");
141 }
142
143 ✗ nr_ = model->get_nr();
144 ✗ state_ = model->get_state();
145 ✗ differential_ = model;
146
147 ✗ VectorXs p_lb(nu_), p_ub(nu_);
148 ✗ control_->convertBounds(differential_->get_u_lb(), differential_->get_u_ub(),
149 p_lb, p_ub);
150 ✗ Base::set_u_lb(p_lb);
151 ✗ Base::set_u_ub(p_ub);
152 }
153
154 } // namespace crocoddyl
155