GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/core/numdiff/control.hxx
Date: 2025-05-13 10:30:51
Exec Total Coverage
Lines: 0 60 0.0%
Branches: 0 129 0.0%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2021-2025, LAAS-CNRS, University of Edinburgh,
5 // New York University, Heriot-Watt University,
6 // Max Planck Gesellschaft, University of Trento
7 // Copyright note valid unless otherwise stated in individual files.
8 // All rights reserved.
9 ///////////////////////////////////////////////////////////////////////////////
10
11 namespace crocoddyl {
12
13 template <typename Scalar>
14 ✗ ControlParametrizationModelNumDiffTpl<
15 Scalar>::ControlParametrizationModelNumDiffTpl(std::shared_ptr<Base> model)
16 : Base(model->get_nw(), model->get_nu()),
17 ✗ model_(model),
18 ✗ e_jac_(sqrt(Scalar(2.0) * std::numeric_limits<Scalar>::epsilon())) {}
19
20 template <typename Scalar>
21 ✗ void ControlParametrizationModelNumDiffTpl<Scalar>::calc(
22 const std::shared_ptr<ControlParametrizationDataAbstract>& data,
23 const Scalar t, const Eigen::Ref<const VectorXs>& u) const {
24 ✗ std::shared_ptr<Data> data_nd = std::static_pointer_cast<Data>(data);
25 ✗ model_->calc(data_nd->data_0, t, u);
26 }
27
28 template <typename Scalar>
29 ✗ void ControlParametrizationModelNumDiffTpl<Scalar>::calcDiff(
30 const std::shared_ptr<ControlParametrizationDataAbstract>& data,
31 const Scalar t, const Eigen::Ref<const VectorXs>& u) const {
32 ✗ std::shared_ptr<Data> data_nd = std::static_pointer_cast<Data>(data);
33 ✗ data->w = data_nd->data_0->w;
34
35 ✗ data_nd->du.setZero();
36 ✗ const Scalar uh_jac = e_jac_ * std::max(Scalar(1.), u.norm());
37 ✗ for (std::size_t i = 0; i < model_->get_nu(); ++i) {
38 ✗ data_nd->du(i) += uh_jac;
39 ✗ model_->calc(data_nd->data_u[i], t, u + data_nd->du);
40 ✗ data->dw_du.col(i) = data_nd->data_u[i]->w - data->w;
41 ✗ data_nd->du(i) = Scalar(0.);
42 }
43 ✗ data->dw_du /= uh_jac;
44 }
45
46 template <typename Scalar>
47 std::shared_ptr<ControlParametrizationDataAbstractTpl<Scalar> >
48 ✗ ControlParametrizationModelNumDiffTpl<Scalar>::createData() {
49 ✗ return std::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
50 }
51
52 template <typename Scalar>
53 ✗ void ControlParametrizationModelNumDiffTpl<Scalar>::params(
54 const std::shared_ptr<ControlParametrizationDataAbstract>& data,
55 const Scalar t, const Eigen::Ref<const VectorXs>& w) const {
56 ✗ model_->params(data, t, w);
57 }
58
59 template <typename Scalar>
60 ✗ void ControlParametrizationModelNumDiffTpl<Scalar>::convertBounds(
61 const Eigen::Ref<const VectorXs>& w_lb,
62 const Eigen::Ref<const VectorXs>& w_ub, Eigen::Ref<VectorXs> u_lb,
63 Eigen::Ref<VectorXs> u_ub) const {
64 ✗ model_->convertBounds(w_lb, w_ub, u_lb, u_ub);
65 }
66
67 template <typename Scalar>
68 ✗ void ControlParametrizationModelNumDiffTpl<Scalar>::multiplyByJacobian(
69 const std::shared_ptr<ControlParametrizationDataAbstract>& data,
70 const Eigen::Ref<const MatrixXs>& A, Eigen::Ref<MatrixXs> out,
71 const AssignmentOp op) const {
72 ✗ assert_pretty(is_a_AssignmentOp(op),
73 ("op must be one of the AssignmentOp {settop, addto, rmfrom}"));
74 ✗ MatrixXs J(nw_, nu_);
75 ✗ switch (op) {
76 ✗ case setto:
77 ✗ out.noalias() = A * data->dw_du;
78 ✗ break;
79 ✗ case addto:
80 ✗ out.noalias() += A * data->dw_du;
81 ✗ break;
82 ✗ case rmfrom:
83 ✗ out.noalias() -= A * data->dw_du;
84 ✗ break;
85 ✗ default:
86 ✗ throw_pretty("Invalid argument: allowed operators: setto, addto, rmfrom");
87 break;
88 }
89 }
90
91 template <typename Scalar>
92 ✗ void ControlParametrizationModelNumDiffTpl<Scalar>::multiplyJacobianTransposeBy(
93 const std::shared_ptr<ControlParametrizationDataAbstract>& data,
94 const Eigen::Ref<const MatrixXs>& A, Eigen::Ref<MatrixXs> out,
95 const AssignmentOp op) const {
96 ✗ assert_pretty(is_a_AssignmentOp(op),
97 ("op must be one of the AssignmentOp {settop, addto, rmfrom}"));
98 ✗ MatrixXs J(nw_, nu_);
99 ✗ switch (op) {
100 ✗ case setto:
101 ✗ out.noalias() = data->dw_du.transpose() * A;
102 ✗ break;
103 ✗ case addto:
104 ✗ out.noalias() += data->dw_du.transpose() * A;
105 ✗ break;
106 ✗ case rmfrom:
107 ✗ out.noalias() -= data->dw_du.transpose() * A;
108 ✗ break;
109 ✗ default:
110 ✗ throw_pretty("Invalid argument: allowed operators: setto, addto, rmfrom");
111 break;
112 }
113 }
114
115 template <typename Scalar>
116 template <typename NewScalar>
117 ControlParametrizationModelNumDiffTpl<NewScalar>
118 ✗ ControlParametrizationModelNumDiffTpl<Scalar>::cast() const {
119 typedef ControlParametrizationModelNumDiffTpl<NewScalar> ReturnType;
120 ✗ ReturnType res(model_->template cast<NewScalar>());
121 ✗ return res;
122 }
123
124 template <typename Scalar>
125 const std::shared_ptr<ControlParametrizationModelAbstractTpl<Scalar> >&
126 ✗ ControlParametrizationModelNumDiffTpl<Scalar>::get_model() const {
127 ✗ return model_;
128 }
129
130 template <typename Scalar>
131 ✗ const Scalar ControlParametrizationModelNumDiffTpl<Scalar>::get_disturbance()
132 const {
133 ✗ return e_jac_;
134 }
135
136 template <typename Scalar>
137 void ControlParametrizationModelNumDiffTpl<Scalar>::set_disturbance(
138 const Scalar disturbance) {
139 if (disturbance < Scalar(0.)) {
140 throw_pretty("Invalid argument: " << "Disturbance constant is positive");
141 }
142 e_jac_ = disturbance;
143 }
144
145 } // namespace crocoddyl
146