GCC Code Coverage Report

Line	Branch	Exec	Source
1			///////////////////////////////////////////////////////////////////////////////
2			// BSD 3-Clause License
3			//
4			// Copyright (C) 2022-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			namespace crocoddyl {
11
12			template <typename Scalar>
13		✗	ResidualModelStateTpl<Scalar>::ResidualModelStateTpl(
14			std::shared_ptr<typename Base::StateAbstract> state, const VectorXs& xref,
15			const std::size_t nu)
16		✗	: Base(state, state->get_ndx(), nu, true, true, false), xref_(xref) {
17		✗	if (static_cast<std::size_t>(xref_.size()) != state_->get_nx()) {
18		✗	throw_pretty(
19			"Invalid argument: " << "xref has wrong dimension (it should be " +
20			std::to_string(state_->get_nx()) + ")");
21			}
22			// Define the pinocchio model for the multibody state case
23		✗	const std::shared_ptr<StateMultibody>& s =
24			std::dynamic_pointer_cast<StateMultibody>(state);
25		✗	if (s) {
26		✗	pin_model_ = s->get_pinocchio();
27			}
28		✗	}
29
30			template <typename Scalar>
31		✗	ResidualModelStateTpl<Scalar>::ResidualModelStateTpl(
32			std::shared_ptr<typename Base::StateAbstract> state, const VectorXs& xref)
33		✗	: Base(state, state->get_ndx(), true, true, false), xref_(xref) {
34		✗	if (static_cast<std::size_t>(xref_.size()) != state_->get_nx()) {
35		✗	throw_pretty(
36			"Invalid argument: " << "xref has wrong dimension (it should be " +
37			std::to_string(state_->get_nx()) + ")");
38			}
39			// Define the pinocchio model for the multibody state case
40		✗	const std::shared_ptr<StateMultibody>& s =
41			std::dynamic_pointer_cast<StateMultibody>(state);
42		✗	if (s) {
43		✗	pin_model_ = s->get_pinocchio();
44			}
45		✗	}
46
47			template <typename Scalar>
48		✗	ResidualModelStateTpl<Scalar>::ResidualModelStateTpl(
49			std::shared_ptr<typename Base::StateAbstract> state, const std::size_t nu)
50			: Base(state, state->get_ndx(), nu, true, true, false),
51		✗	xref_(state->zero()) {
52			// Define the pinocchio model for the multibody state case
53		✗	const std::shared_ptr<StateMultibody>& s =
54			std::dynamic_pointer_cast<StateMultibody>(state);
55		✗	if (s) {
56		✗	pin_model_ = s->get_pinocchio();
57			}
58		✗	}
59
60			template <typename Scalar>
61		✗	ResidualModelStateTpl<Scalar>::ResidualModelStateTpl(
62			std::shared_ptr<typename Base::StateAbstract> state)
63		✗	: Base(state, state->get_ndx(), true, true, false), xref_(state->zero()) {
64			// Define the pinocchio model for the multibody state case
65		✗	const std::shared_ptr<StateMultibody>& s =
66			std::dynamic_pointer_cast<StateMultibody>(state);
67		✗	if (s) {
68		✗	pin_model_ = s->get_pinocchio();
69			}
70		✗	}
71
72			template <typename Scalar>
73		✗	void ResidualModelStateTpl<Scalar>::calc(
74			const std::shared_ptr<ResidualDataAbstract>& data,
75			const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>&) {
76		✗	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
77		✗	throw_pretty(
78			"Invalid argument: " << "x has wrong dimension (it should be " +
79			std::to_string(state_->get_nx()) + ")");
80			}
81
82		✗	state_->diff(xref_, x, data->r);
83		✗	}
84
85			template <typename Scalar>
86		✗	void ResidualModelStateTpl<Scalar>::calcDiff(
87			const std::shared_ptr<ResidualDataAbstract>& data,
88			const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>&) {
89		✗	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
90		✗	throw_pretty(
91			"Invalid argument: " << "x has wrong dimension (it should be " +
92			std::to_string(state_->get_nx()) + ")");
93			}
94
95		✗	state_->Jdiff(xref_, x, data->Rx, data->Rx, second);
96		✗	}
97
98			template <typename Scalar>
99		✗	void ResidualModelStateTpl<Scalar>::calcCostDiff(
100			const std::shared_ptr<CostDataAbstract>& cdata,
101			const std::shared_ptr<ResidualDataAbstract>& rdata,
102			const std::shared_ptr<ActivationDataAbstract>& adata, const bool) {
103		✗	const std::size_t nv = state_->get_nv();
104		✗	if (pin_model_) {
105			typedef Eigen::Block<MatrixXs> MatrixBlock;
106		✗	for (pinocchio::JointIndex i = 1;
107		✗	i < (pinocchio::JointIndex)pin_model_->njoints; ++i) {
108		✗	const MatrixBlock& RxBlock =
109		✗	rdata->Rx.block(pin_model_->idx_vs[i], pin_model_->idx_vs[i],
110		✗	pin_model_->nvs[i], pin_model_->nvs[i]);
111		✗	cdata->Lx.segment(pin_model_->idx_vs[i], pin_model_->nvs[i]).noalias() =
112		✗	RxBlock.transpose() *
113		✗	adata->Ar.segment(pin_model_->idx_vs[i], pin_model_->nvs[i]);
114		✗	cdata->Lxx
115		✗	.block(pin_model_->idx_vs[i], pin_model_->idx_vs[i],
116		✗	pin_model_->nvs[i], pin_model_->nvs[i])
117		✗	.noalias() = RxBlock.transpose() *
118		✗	adata->Arr.diagonal()
119		✗	.segment(pin_model_->idx_vs[i], pin_model_->nvs[i])
120		✗	.asDiagonal() *
121			RxBlock;
122			}
123		✗	cdata->Lx.tail(nv) = adata->Ar.tail(nv);
124		✗	cdata->Lxx.diagonal().tail(nv) = adata->Arr.diagonal().tail(nv);
125			} else {
126		✗	cdata->Lx = adata->Ar;
127		✗	cdata->Lxx.diagonal() = adata->Arr.diagonal();
128			}
129		✗	}
130
131			template <typename Scalar>
132			template <typename NewScalar>
133		✗	ResidualModelStateTpl<NewScalar> ResidualModelStateTpl<Scalar>::cast() const {
134			typedef ResidualModelStateTpl<NewScalar> ReturnType;
135			typedef StateAbstractTpl<NewScalar> StateType;
136		✗	ReturnType ret(
137		✗	std::static_pointer_cast<StateType>(state_->template cast<NewScalar>()),
138		✗	xref_.template cast<NewScalar>(), nu_);
139		✗	return ret;
140			}
141
142			template <typename Scalar>
143		✗	void ResidualModelStateTpl<Scalar>::print(std::ostream& os) const {
144		✗	os << "ResidualModelState";
145		✗	}
146
147			template <typename Scalar>
148			const typename MathBaseTpl<Scalar>::VectorXs&
149		✗	ResidualModelStateTpl<Scalar>::get_reference() const {
150		✗	return xref_;
151			}
152
153			template <typename Scalar>
154		✗	void ResidualModelStateTpl<Scalar>::set_reference(const VectorXs& reference) {
155		✗	if (static_cast<std::size_t>(reference.size()) != state_->get_nx()) {
156		✗	throw_pretty(
157			"Invalid argument: "
158			<< "the state reference has wrong dimension (" << reference.size()
159			<< " provided - it should be " + std::to_string(state_->get_nx()) + ")")
160			}
161		✗	xref_ = reference;
162		✗	}
163
164			} // namespace crocoddyl
165