GCC Code Coverage Report

Line	Branch	Exec	Source
1			///////////////////////////////////////////////////////////////////////////////
2			// BSD 3-Clause License
3			//
4			// Copyright (C) 2019-2025, LAAS-CNRS, Heriot-Watt University
5			// Copyright note valid unless otherwise stated in individual files.
6			// All rights reserved.
7			///////////////////////////////////////////////////////////////////////////////
8
9			namespace crocoddyl {
10
11			template <typename Scalar>
12		✗	StateMultibodyTpl<Scalar>::StateMultibodyTpl(
13			std::shared_ptr<PinocchioModel> model)
14		✗	: Base(model->nq + model->nv, 2 * model->nv),
15		✗	pinocchio_(model),
16		✗	x0_(VectorXs::Zero(model->nq + model->nv)) {
17		✗	x0_.head(nq_) = pinocchio::neutral(*pinocchio_.get());
18			// In a multibody system, we could define the first joint using Lie groups.
19			// The current cases are free-flyer (SE3) and spherical (S03).
20			// Instead simple represents any joint that can model within the Euclidean
21			// manifold. The rest of joints use Euclidean algebra. We use this fact for
22			// computing Jdiff.
23			// Define internally the limits of the first joint
24		✗	const std::size_t nq0 =
25		✗	model->existJointName("root_joint")
26		✗	? model->joints[model->getJointId("root_joint")].nq()
27			: 0;
28		✗	lb_.head(nq0) = -std::numeric_limits<Scalar>::max() * VectorXs::Ones(nq0);
29		✗	ub_.head(nq0) = std::numeric_limits<Scalar>::max() * VectorXs::Ones(nq0);
30		✗	lb_.segment(nq0, nq_ - nq0) = pinocchio_->lowerPositionLimit.tail(nq_ - nq0);
31		✗	ub_.segment(nq0, nq_ - nq0) = pinocchio_->upperPositionLimit.tail(nq_ - nq0);
32		✗	lb_.tail(nv_) = -pinocchio_->velocityLimit;
33		✗	ub_.tail(nv_) = pinocchio_->velocityLimit;
34		✗	Base::update_has_limits();
35		✗	}
36
37			template <typename Scalar>
38		✗	StateMultibodyTpl<Scalar>::StateMultibodyTpl()
39		✗	: Base(), x0_(VectorXs::Zero(0)) {}
40
41			template <typename Scalar>
42		✗	StateMultibodyTpl<Scalar>::~StateMultibodyTpl() {}
43
44			template <typename Scalar>
45		✗	typename MathBaseTpl<Scalar>::VectorXs StateMultibodyTpl<Scalar>::zero() const {
46		✗	return x0_;
47			}
48
49			template <typename Scalar>
50		✗	typename MathBaseTpl<Scalar>::VectorXs StateMultibodyTpl<Scalar>::rand() const {
51		✗	VectorXs xrand = VectorXs::Random(nx_);
52		✗	xrand.head(nq_) = pinocchio::randomConfiguration(*pinocchio_.get());
53		✗	return xrand;
54		✗	}
55
56			template <typename Scalar>
57		✗	void StateMultibodyTpl<Scalar>::diff(const Eigen::Ref<const VectorXs>& x0,
58			const Eigen::Ref<const VectorXs>& x1,
59			Eigen::Ref<VectorXs> dxout) const {
60		✗	if (static_cast<std::size_t>(x0.size()) != nx_) {
61		✗	throw_pretty(
62			"Invalid argument: " << "x0 has wrong dimension (it should be " +
63			std::to_string(nx_) + ")");
64			}
65		✗	if (static_cast<std::size_t>(x1.size()) != nx_) {
66		✗	throw_pretty(
67			"Invalid argument: " << "x1 has wrong dimension (it should be " +
68			std::to_string(nx_) + ")");
69			}
70		✗	if (static_cast<std::size_t>(dxout.size()) != ndx_) {
71		✗	throw_pretty(
72			"Invalid argument: " << "dxout has wrong dimension (it should be " +
73			std::to_string(ndx_) + ")");
74			}
75
76		✗	pinocchio::difference(*pinocchio_.get(), x0.head(nq_), x1.head(nq_),
77		✗	dxout.head(nv_));
78		✗	dxout.tail(nv_) = x1.tail(nv_) - x0.tail(nv_);
79		✗	}
80
81			template <typename Scalar>
82		✗	void StateMultibodyTpl<Scalar>::integrate(const Eigen::Ref<const VectorXs>& x,
83			const Eigen::Ref<const VectorXs>& dx,
84			Eigen::Ref<VectorXs> xout) const {
85		✗	if (static_cast<std::size_t>(x.size()) != nx_) {
86		✗	throw_pretty(
87			"Invalid argument: " << "x has wrong dimension (it should be " +
88			std::to_string(nx_) + ")");
89			}
90		✗	if (static_cast<std::size_t>(dx.size()) != ndx_) {
91		✗	throw_pretty(
92			"Invalid argument: " << "dx has wrong dimension (it should be " +
93			std::to_string(ndx_) + ")");
94			}
95		✗	if (static_cast<std::size_t>(xout.size()) != nx_) {
96		✗	throw_pretty(
97			"Invalid argument: " << "xout has wrong dimension (it should be " +
98			std::to_string(nx_) + ")");
99			}
100
101		✗	pinocchio::integrate(*pinocchio_.get(), x.head(nq_), dx.head(nv_),
102		✗	xout.head(nq_));
103		✗	xout.tail(nv_) = x.tail(nv_) + dx.tail(nv_);
104		✗	}
105
106			template <typename Scalar>
107		✗	void StateMultibodyTpl<Scalar>::Jdiff(const Eigen::Ref<const VectorXs>& x0,
108			const Eigen::Ref<const VectorXs>& x1,
109			Eigen::Ref<MatrixXs> Jfirst,
110			Eigen::Ref<MatrixXs> Jsecond,
111			const Jcomponent firstsecond) const {
112		✗	assert_pretty(
113			is_a_Jcomponent(firstsecond),
114			("firstsecond must be one of the Jcomponent {both, first, second}"));
115		✗	if (static_cast<std::size_t>(x0.size()) != nx_) {
116		✗	throw_pretty(
117			"Invalid argument: " << "x0 has wrong dimension (it should be " +
118			std::to_string(nx_) + ")");
119			}
120		✗	if (static_cast<std::size_t>(x1.size()) != nx_) {
121		✗	throw_pretty(
122			"Invalid argument: " << "x1 has wrong dimension (it should be " +
123			std::to_string(nx_) + ")");
124			}
125
126		✗	if (firstsecond == first) {
127		✗	if (static_cast<std::size_t>(Jfirst.rows()) != ndx_ ||
128		✗	static_cast<std::size_t>(Jfirst.cols()) != ndx_) {
129		✗	throw_pretty(
130			"Invalid argument: " << "Jfirst has wrong dimension (it should be " +
131			std::to_string(ndx_) + "," +
132			std::to_string(ndx_) + ")");
133			}
134
135		✗	pinocchio::dDifference(*pinocchio_.get(), x0.head(nq_), x1.head(nq_),
136		✗	Jfirst.topLeftCorner(nv_, nv_), pinocchio::ARG0);
137		✗	Jfirst.bottomRightCorner(nv_, nv_).diagonal().array() = (Scalar)-1;
138		✗	} else if (firstsecond == second) {
139		✗	if (static_cast<std::size_t>(Jsecond.rows()) != ndx_ ||
140		✗	static_cast<std::size_t>(Jsecond.cols()) != ndx_) {
141		✗	throw_pretty(
142			"Invalid argument: " << "Jsecond has wrong dimension (it should be " +
143			std::to_string(ndx_) + "," +
144			std::to_string(ndx_) + ")");
145			}
146		✗	pinocchio::dDifference(*pinocchio_.get(), x0.head(nq_), x1.head(nq_),
147		✗	Jsecond.topLeftCorner(nv_, nv_), pinocchio::ARG1);
148		✗	Jsecond.bottomRightCorner(nv_, nv_).diagonal().array() = (Scalar)1;
149			} else { // computing both
150		✗	if (static_cast<std::size_t>(Jfirst.rows()) != ndx_ ||
151		✗	static_cast<std::size_t>(Jfirst.cols()) != ndx_) {
152		✗	throw_pretty(
153			"Invalid argument: " << "Jfirst has wrong dimension (it should be " +
154			std::to_string(ndx_) + "," +
155			std::to_string(ndx_) + ")");
156			}
157		✗	if (static_cast<std::size_t>(Jsecond.rows()) != ndx_ ||
158		✗	static_cast<std::size_t>(Jsecond.cols()) != ndx_) {
159		✗	throw_pretty(
160			"Invalid argument: " << "Jsecond has wrong dimension (it should be " +
161			std::to_string(ndx_) + "," +
162			std::to_string(ndx_) + ")");
163			}
164		✗	pinocchio::dDifference(*pinocchio_.get(), x0.head(nq_), x1.head(nq_),
165		✗	Jfirst.topLeftCorner(nv_, nv_), pinocchio::ARG0);
166		✗	pinocchio::dDifference(*pinocchio_.get(), x0.head(nq_), x1.head(nq_),
167		✗	Jsecond.topLeftCorner(nv_, nv_), pinocchio::ARG1);
168		✗	Jfirst.bottomRightCorner(nv_, nv_).diagonal().array() = (Scalar)-1;
169		✗	Jsecond.bottomRightCorner(nv_, nv_).diagonal().array() = (Scalar)1;
170			}
171		✗	}
172
173			template <typename Scalar>
174		✗	void StateMultibodyTpl<Scalar>::Jintegrate(const Eigen::Ref<const VectorXs>& x,
175			const Eigen::Ref<const VectorXs>& dx,
176			Eigen::Ref<MatrixXs> Jfirst,
177			Eigen::Ref<MatrixXs> Jsecond,
178			const Jcomponent firstsecond,
179			const AssignmentOp op) const {
180		✗	assert_pretty(
181			is_a_Jcomponent(firstsecond),
182			("firstsecond must be one of the Jcomponent {both, first, second}"));
183		✗	assert_pretty(is_a_AssignmentOp(op),
184			("op must be one of the AssignmentOp {settop, addto, rmfrom}"));
185		✗	if (firstsecond == first || firstsecond == both) {
186		✗	if (static_cast<std::size_t>(Jfirst.rows()) != ndx_ ||
187		✗	static_cast<std::size_t>(Jfirst.cols()) != ndx_) {
188		✗	throw_pretty(
189			"Invalid argument: " << "Jfirst has wrong dimension (it should be " +
190			std::to_string(ndx_) + "," +
191			std::to_string(ndx_) + ")");
192			}
193		✗	switch (op) {
194		✗	case setto:
195		✗	pinocchio::dIntegrate(*pinocchio_.get(), x.head(nq_), dx.head(nv_),
196		✗	Jfirst.topLeftCorner(nv_, nv_), pinocchio::ARG0,
197			pinocchio::SETTO);
198		✗	Jfirst.bottomRightCorner(nv_, nv_).diagonal().array() = (Scalar)1;
199		✗	break;
200		✗	case addto:
201		✗	pinocchio::dIntegrate(*pinocchio_.get(), x.head(nq_), dx.head(nv_),
202		✗	Jfirst.topLeftCorner(nv_, nv_), pinocchio::ARG0,
203			pinocchio::ADDTO);
204		✗	Jfirst.bottomRightCorner(nv_, nv_).diagonal().array() += (Scalar)1;
205		✗	break;
206		✗	case rmfrom:
207		✗	pinocchio::dIntegrate(*pinocchio_.get(), x.head(nq_), dx.head(nv_),
208		✗	Jfirst.topLeftCorner(nv_, nv_), pinocchio::ARG0,
209			pinocchio::RMTO);
210		✗	Jfirst.bottomRightCorner(nv_, nv_).diagonal().array() -= (Scalar)1;
211		✗	break;
212		✗	default:
213		✗	throw_pretty(
214			"Invalid argument: allowed operators: setto, addto, rmfrom");
215			break;
216			}
217			}
218		✗	if (firstsecond == second || firstsecond == both) {
219		✗	if (static_cast<std::size_t>(Jsecond.rows()) != ndx_ ||
220		✗	static_cast<std::size_t>(Jsecond.cols()) != ndx_) {
221		✗	throw_pretty(
222			"Invalid argument: " << "Jsecond has wrong dimension (it should be " +
223			std::to_string(ndx_) + "," +
224			std::to_string(ndx_) + ")");
225			}
226		✗	switch (op) {
227		✗	case setto:
228		✗	pinocchio::dIntegrate(*pinocchio_.get(), x.head(nq_), dx.head(nv_),
229		✗	Jsecond.topLeftCorner(nv_, nv_), pinocchio::ARG1,
230			pinocchio::SETTO);
231		✗	Jsecond.bottomRightCorner(nv_, nv_).diagonal().array() = (Scalar)1;
232		✗	break;
233		✗	case addto:
234		✗	pinocchio::dIntegrate(*pinocchio_.get(), x.head(nq_), dx.head(nv_),
235		✗	Jsecond.topLeftCorner(nv_, nv_), pinocchio::ARG1,
236			pinocchio::ADDTO);
237		✗	Jsecond.bottomRightCorner(nv_, nv_).diagonal().array() += (Scalar)1;
238		✗	break;
239		✗	case rmfrom:
240		✗	pinocchio::dIntegrate(*pinocchio_.get(), x.head(nq_), dx.head(nv_),
241		✗	Jsecond.topLeftCorner(nv_, nv_), pinocchio::ARG1,
242			pinocchio::RMTO);
243		✗	Jsecond.bottomRightCorner(nv_, nv_).diagonal().array() -= (Scalar)1;
244		✗	break;
245		✗	default:
246		✗	throw_pretty(
247			"Invalid argument: allowed operators: setto, addto, rmfrom");
248			break;
249			}
250			}
251		✗	}
252
253			template <typename Scalar>
254		✗	void StateMultibodyTpl<Scalar>::JintegrateTransport(
255			const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& dx,
256			Eigen::Ref<MatrixXs> Jin, const Jcomponent firstsecond) const {
257		✗	assert_pretty(
258			is_a_Jcomponent(firstsecond),
259			("firstsecond must be one of the Jcomponent {both, first, second}"));
260
261		✗	switch (firstsecond) {
262		✗	case first:
263		✗	pinocchio::dIntegrateTransport(*pinocchio_.get(), x.head(nq_),
264		✗	dx.head(nv_), Jin.topRows(nv_),
265			pinocchio::ARG0);
266		✗	break;
267		✗	case second:
268		✗	pinocchio::dIntegrateTransport(*pinocchio_.get(), x.head(nq_),
269		✗	dx.head(nv_), Jin.topRows(nv_),
270			pinocchio::ARG1);
271		✗	break;
272		✗	default:
273		✗	throw_pretty(
274			"Invalid argument: firstsecond must be either first or second. both "
275			"not supported for this operation.");
276			break;
277			}
278		✗	}
279
280			template <typename Scalar>
281			const std::shared_ptr<pinocchio::ModelTpl<Scalar> >&
282		✗	StateMultibodyTpl<Scalar>::get_pinocchio() const {
283		✗	return pinocchio_;
284			}
285
286			template <typename Scalar>
287			template <typename NewScalar>
288		✗	StateMultibodyTpl<NewScalar> StateMultibodyTpl<Scalar>::cast() const {
289			typedef StateMultibodyTpl<NewScalar> ReturnType;
290			typedef pinocchio::ModelTpl<NewScalar> ModelType;
291		✗	ReturnType res(
292		✗	std::make_shared<ModelType>(pinocchio_->template cast<NewScalar>()));
293		✗	return res;
294			}
295
296			template <typename Scalar>
297		✗	void StateMultibodyTpl<Scalar>::print(std::ostream& os) const {
298		✗	os << "StateMultibody {nx=" << nx_ << ", ndx=" << ndx_
299		✗	<< ", pinocchio=" << *pinocchio_.get() << "}";
300		✗	}
301
302			} // namespace crocoddyl
303