GCC Code Coverage Report

Line	Branch	Exec	Source
1			///////////////////////////////////////////////////////////////////////////////
2			// BSD 3-Clause License
3			//
4			// Copyright (C) 2021-2025, Heriot-Watt University, University of Edinburgh
5			// Copyright note valid unless otherwise stated in individual files.
6			// All rights reserved.
7			///////////////////////////////////////////////////////////////////////////////
8
9			#include "crocoddyl/core/constraints/residual.hpp"
10
11			namespace crocoddyl {
12
13			template <typename Scalar>
14		✗	DifferentialActionModelFreeInvDynamicsTpl<Scalar>::
15			DifferentialActionModelFreeInvDynamicsTpl(
16			std::shared_ptr<StateMultibody> state,
17			std::shared_ptr<ActuationModelAbstract> actuation,
18			std::shared_ptr<CostModelSum> costs)
19		✗	: Base(state, state->get_nv(), costs->get_nr(), 0,
20		✗	state->get_nv() - actuation->get_nu()),
21		✗	actuation_(actuation),
22		✗	costs_(costs),
23		✗	constraints_(
24		✗	std::make_shared<ConstraintModelManager>(state, state->get_nv())),
25		✗	pinocchio_(state->get_pinocchio().get()) {
26		✗	init(state);
27		✗	}
28
29			template <typename Scalar>
30		✗	DifferentialActionModelFreeInvDynamicsTpl<Scalar>::
31			DifferentialActionModelFreeInvDynamicsTpl(
32			std::shared_ptr<StateMultibody> state,
33			std::shared_ptr<ActuationModelAbstract> actuation,
34			std::shared_ptr<CostModelSum> costs,
35			std::shared_ptr<ConstraintModelManager> constraints)
36		✗	: Base(state, state->get_nv(), costs->get_nr(), constraints->get_ng(),
37		✗	constraints->get_nh() + state->get_nv() - actuation->get_nu(),
38			constraints->get_ng_T(), constraints->get_nh_T()),
39		✗	actuation_(actuation),
40		✗	costs_(costs),
41		✗	constraints_(constraints),
42		✗	pinocchio_(state->get_pinocchio().get()) {
43		✗	init(state);
44		✗	}
45
46			template <typename Scalar>
47		✗	void DifferentialActionModelFreeInvDynamicsTpl<Scalar>::init(
48			const std::shared_ptr<StateMultibody>& state) {
49		✗	if (costs_->get_nu() != nu_) {
50		✗	throw_pretty(
51			"Invalid argument: "
52			<< "Costs doesn't have the same control dimension (it should be " +
53			std::to_string(nu_) + ")");
54			}
55		✗	if (constraints_->get_nu() != nu_) {
56		✗	throw_pretty("Invalid argument: "
57			<< "Constraints doesn't have the same control dimension (it "
58			"should be " +
59			std::to_string(nu_) + ")");
60			}
61		✗	VectorXs lb =
62		✗	VectorXs::Constant(nu_, -std::numeric_limits<Scalar>::infinity());
63		✗	VectorXs ub =
64		✗	VectorXs::Constant(nu_, std::numeric_limits<Scalar>::infinity());
65		✗	Base::set_u_lb(lb);
66		✗	Base::set_u_ub(ub);
67
68		✗	if (state->get_nv() - actuation_->get_nu() > 0) {
69		✗	constraints_->addConstraint(
70			"tau",
71			std::make_shared<ConstraintModelResidual>(
72		✗	state_,
73			std::make_shared<typename DifferentialActionModelFreeInvDynamicsTpl<
74		✗	Scalar>::ResidualModelActuation>(state, actuation_->get_nu()),
75		✗	false));
76			}
77		✗	}
78
79			template <typename Scalar>
80		✗	void DifferentialActionModelFreeInvDynamicsTpl<Scalar>::calc(
81			const std::shared_ptr<DifferentialActionDataAbstract>& data,
82			const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
83		✗	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
84		✗	throw_pretty(
85			"Invalid argument: " << "x has wrong dimension (it should be " +
86			std::to_string(state_->get_nx()) + ")");
87			}
88		✗	if (static_cast<std::size_t>(u.size()) != nu_) {
89		✗	throw_pretty(
90			"Invalid argument: " << "u has wrong dimension (it should be " +
91			std::to_string(nu_) + ")");
92			}
93		✗	Data* d = static_cast<Data*>(data.get());
94		✗	const std::size_t nv = state_->get_nv();
95			const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q =
96		✗	x.head(state_->get_nq());
97			const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> v =
98		✗	x.tail(nv);
99
100		✗	d->xout = u;
101		✗	pinocchio::rnea(*pinocchio_, d->pinocchio, q, v, u);
102		✗	pinocchio::updateGlobalPlacements(*pinocchio_, d->pinocchio);
103		✗	actuation_->commands(d->multibody.actuation, x, d->pinocchio.tau);
104		✗	d->multibody.joint->a = u;
105		✗	d->multibody.joint->tau = d->multibody.actuation->u;
106		✗	actuation_->calc(d->multibody.actuation, x, d->multibody.joint->tau);
107		✗	costs_->calc(d->costs, x, u);
108		✗	d->cost = d->costs->cost;
109		✗	d->constraints->resize(this, d);
110		✗	constraints_->calc(d->constraints, x, u);
111		✗	}
112
113			template <typename Scalar>
114		✗	void DifferentialActionModelFreeInvDynamicsTpl<Scalar>::calc(
115			const std::shared_ptr<DifferentialActionDataAbstract>& data,
116			const Eigen::Ref<const VectorXs>& x) {
117		✗	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
118		✗	throw_pretty(
119			"Invalid argument: " << "x has wrong dimension (it should be " +
120			std::to_string(state_->get_nx()) + ")");
121			}
122
123		✗	Data* d = static_cast<Data*>(data.get());
124			const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q =
125		✗	x.head(state_->get_nq());
126			const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> v =
127		✗	x.tail(state_->get_nv());
128
129		✗	pinocchio::computeAllTerms(*pinocchio_, d->pinocchio, q, v);
130
131		✗	costs_->calc(d->costs, x);
132		✗	d->cost = d->costs->cost;
133		✗	d->constraints->resize(this, d, false);
134		✗	constraints_->calc(d->constraints, x);
135		✗	}
136
137			template <typename Scalar>
138		✗	void DifferentialActionModelFreeInvDynamicsTpl<Scalar>::calcDiff(
139			const std::shared_ptr<DifferentialActionDataAbstract>& data,
140			const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
141		✗	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
142		✗	throw_pretty(
143			"Invalid argument: " << "x has wrong dimension (it should be " +
144			std::to_string(state_->get_nx()) + ")");
145			}
146		✗	if (static_cast<std::size_t>(u.size()) != nu_) {
147		✗	throw_pretty(
148			"Invalid argument: " << "u has wrong dimension (it should be " +
149			std::to_string(nu_) + ")");
150			}
151		✗	Data* d = static_cast<Data*>(data.get());
152		✗	const std::size_t nv = state_->get_nv();
153			const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q =
154		✗	x.head(state_->get_nq());
155			const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> v =
156		✗	x.tail(nv);
157
158		✗	pinocchio::computeRNEADerivatives(*pinocchio_, d->pinocchio, q, v, u);
159		✗	d->pinocchio.M.template triangularView<Eigen::StrictlyLower>() =
160		✗	d->pinocchio.M.template triangularView<Eigen::StrictlyUpper>()
161		✗	.transpose();
162		✗	actuation_->calcDiff(d->multibody.actuation, x, d->multibody.joint->tau);
163		✗	actuation_->torqueTransform(d->multibody.actuation, x,
164		✗	d->multibody.joint->tau);
165		✗	d->multibody.joint->dtau_dx.leftCols(nv).noalias() =
166		✗	d->multibody.actuation->Mtau * d->pinocchio.dtau_dq;
167		✗	d->multibody.joint->dtau_dx.rightCols(nv).noalias() =
168		✗	d->multibody.actuation->Mtau * d->pinocchio.dtau_dv;
169		✗	d->multibody.joint->dtau_du.noalias() =
170		✗	d->multibody.actuation->Mtau * d->pinocchio.M;
171		✗	costs_->calcDiff(d->costs, x, u);
172		✗	constraints_->calcDiff(d->constraints, x, u);
173		✗	}
174
175			template <typename Scalar>
176		✗	void DifferentialActionModelFreeInvDynamicsTpl<Scalar>::calcDiff(
177			const std::shared_ptr<DifferentialActionDataAbstract>& data,
178			const Eigen::Ref<const VectorXs>& x) {
179		✗	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
180		✗	throw_pretty(
181			"Invalid argument: " << "x has wrong dimension (it should be " +
182			std::to_string(state_->get_nx()) + ")");
183			}
184		✗	Data* d = static_cast<Data*>(data.get());
185
186		✗	costs_->calcDiff(d->costs, x);
187		✗	constraints_->calcDiff(d->constraints, x);
188		✗	}
189
190			template <typename Scalar>
191			std::shared_ptr<DifferentialActionDataAbstractTpl<Scalar> >
192		✗	DifferentialActionModelFreeInvDynamicsTpl<Scalar>::createData() {
193		✗	return std::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
194			}
195
196			template <typename Scalar>
197			template <typename NewScalar>
198			DifferentialActionModelFreeInvDynamicsTpl<NewScalar>
199		✗	DifferentialActionModelFreeInvDynamicsTpl<Scalar>::cast() const {
200			typedef DifferentialActionModelFreeInvDynamicsTpl<NewScalar> ReturnType;
201			typedef StateMultibodyTpl<NewScalar> StateType;
202			typedef CostModelSumTpl<NewScalar> CostType;
203			typedef ConstraintModelManagerTpl<NewScalar> ConstraintType;
204		✗	if (constraints_) {
205		✗	const std::shared_ptr<ConstraintType>& constraints =
206			std::make_shared<ConstraintType>(
207		✗	constraints_->template cast<NewScalar>());
208		✗	if (state_->get_nv() - actuation_->get_nu() > 0) {
209		✗	constraints->removeConstraint("tau");
210			}
211		✗	ReturnType ret(
212		✗	std::static_pointer_cast<StateType>(state_->template cast<NewScalar>()),
213		✗	actuation_->template cast<NewScalar>(),
214		✗	std::make_shared<CostType>(costs_->template cast<NewScalar>()),
215			constraints);
216		✗	return ret;
217		✗	} else {
218		✗	ReturnType ret(
219		✗	std::static_pointer_cast<StateType>(state_->template cast<NewScalar>()),
220		✗	actuation_->template cast<NewScalar>(),
221		✗	std::make_shared<CostType>(costs_->template cast<NewScalar>()));
222		✗	return ret;
223		✗	}
224			}
225
226			template <typename Scalar>
227		✗	bool DifferentialActionModelFreeInvDynamicsTpl<Scalar>::checkData(
228			const std::shared_ptr<DifferentialActionDataAbstract>& data) {
229		✗	std::shared_ptr<Data> d = std::dynamic_pointer_cast<Data>(data);
230		✗	if (d != NULL) {
231		✗	return true;
232			} else {
233		✗	return false;
234			}
235		✗	}
236
237			template <typename Scalar>
238		✗	void DifferentialActionModelFreeInvDynamicsTpl<Scalar>::quasiStatic(
239			const std::shared_ptr<DifferentialActionDataAbstract>&,
240			Eigen::Ref<VectorXs> u, const Eigen::Ref<const VectorXs>&,
241			const std::size_t, const Scalar) {
242		✗	u.setZero();
243		✗	}
244
245			template <typename Scalar>
246		✗	void DifferentialActionModelFreeInvDynamicsTpl<Scalar>::print(
247			std::ostream& os) const {
248		✗	os << "DifferentialActionModelFreeInvDynamics {nx=" << state_->get_nx()
249		✗	<< ", ndx=" << state_->get_ndx() << ", nu=" << nu_ << "}";
250		✗	}
251
252			template <typename Scalar>
253		✗	std::size_t DifferentialActionModelFreeInvDynamicsTpl<Scalar>::get_ng() const {
254		✗	if (constraints_ != nullptr) {
255		✗	return constraints_->get_ng();
256			} else {
257		✗	return Base::get_ng();
258			}
259			}
260
261			template <typename Scalar>
262		✗	std::size_t DifferentialActionModelFreeInvDynamicsTpl<Scalar>::get_nh() const {
263		✗	if (constraints_ != nullptr) {
264		✗	return constraints_->get_nh();
265			} else {
266		✗	return Base::get_nh();
267			}
268			}
269
270			template <typename Scalar>
271		✗	std::size_t DifferentialActionModelFreeInvDynamicsTpl<Scalar>::get_ng_T()
272			const {
273		✗	if (constraints_ != nullptr) {
274		✗	return constraints_->get_ng_T();
275			} else {
276		✗	return Base::get_ng_T();
277			}
278			}
279
280			template <typename Scalar>
281		✗	std::size_t DifferentialActionModelFreeInvDynamicsTpl<Scalar>::get_nh_T()
282			const {
283		✗	if (constraints_ != nullptr) {
284		✗	return constraints_->get_nh_T();
285			} else {
286		✗	return Base::get_nh_T();
287			}
288			}
289
290			template <typename Scalar>
291			const typename MathBaseTpl<Scalar>::VectorXs&
292		✗	DifferentialActionModelFreeInvDynamicsTpl<Scalar>::get_g_lb() const {
293		✗	if (constraints_ != nullptr) {
294		✗	return constraints_->get_lb();
295			} else {
296		✗	return g_lb_;
297			}
298			}
299
300			template <typename Scalar>
301			const typename MathBaseTpl<Scalar>::VectorXs&
302		✗	DifferentialActionModelFreeInvDynamicsTpl<Scalar>::get_g_ub() const {
303		✗	if (constraints_ != nullptr) {
304		✗	return constraints_->get_ub();
305			} else {
306		✗	return g_lb_;
307			}
308			}
309
310			template <typename Scalar>
311			const std::shared_ptr<ActuationModelAbstractTpl<Scalar> >&
312		✗	DifferentialActionModelFreeInvDynamicsTpl<Scalar>::get_actuation() const {
313		✗	return actuation_;
314			}
315
316			template <typename Scalar>
317			const std::shared_ptr<CostModelSumTpl<Scalar> >&
318		✗	DifferentialActionModelFreeInvDynamicsTpl<Scalar>::get_costs() const {
319		✗	return costs_;
320			}
321
322			template <typename Scalar>
323			const std::shared_ptr<ConstraintModelManagerTpl<Scalar> >&
324		✗	DifferentialActionModelFreeInvDynamicsTpl<Scalar>::get_constraints() const {
325		✗	return constraints_;
326			}
327
328			template <typename Scalar>
329			pinocchio::ModelTpl<Scalar>&
330		✗	DifferentialActionModelFreeInvDynamicsTpl<Scalar>::get_pinocchio() const {
331		✗	return *pinocchio_;
332			}
333
334			} // namespace crocoddyl
335