GCC Code Coverage Report

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