GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/multibody/actions/free-fwddyn.hxx
Date: 2025-06-03 08:14:12
Exec Total Coverage
Lines: 0 181 0.0%
Functions: 0 44 0.0%
Branches: 0 470 0.0%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-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 ✗ DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::
14 DifferentialActionModelFreeFwdDynamicsTpl(
15 std::shared_ptr<StateMultibody> state,
16 std::shared_ptr<ActuationModelAbstract> actuation,
17 std::shared_ptr<CostModelSum> costs,
18 std::shared_ptr<ConstraintModelManager> constraints)
19 : Base(state, actuation->get_nu(), costs->get_nr(),
20 ✗ constraints != nullptr ? constraints->get_ng() : 0,
21 ✗ constraints != nullptr ? constraints->get_nh() : 0,
22 ✗ constraints != nullptr ? constraints->get_ng_T() : 0,
23 ✗ constraints != nullptr ? constraints->get_nh_T() : 0),
24 ✗ actuation_(actuation),
25 ✗ costs_(costs),
26 ✗ constraints_(constraints),
27 ✗ pinocchio_(state->get_pinocchio().get()),
28 ✗ without_armature_(true),
29 ✗ armature_(VectorXs::Zero(state->get_nv())) {
30 ✗ if (costs_->get_nu() != nu_) {
31 ✗ throw_pretty(
32 "Invalid argument: "
33 << "Costs doesn't have the same control dimension (it should be " +
34 std::to_string(nu_) + ")");
35 }
36 ✗ Base::set_u_lb(Scalar(-1.) * pinocchio_->effortLimit.tail(nu_));
37 ✗ Base::set_u_ub(Scalar(+1.) * pinocchio_->effortLimit.tail(nu_));
38 ✗ }
39
40 template <typename Scalar>
41 ✗ void DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::calc(
42 const std::shared_ptr<DifferentialActionDataAbstract>& data,
43 const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
44 ✗ if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
45 ✗ throw_pretty(
46 "Invalid argument: " << "x has wrong dimension (it should be " +
47 std::to_string(state_->get_nx()) + ")");
48 }
49 ✗ if (static_cast<std::size_t>(u.size()) != nu_) {
50 ✗ throw_pretty(
51 "Invalid argument: " << "u has wrong dimension (it should be " +
52 std::to_string(nu_) + ")");
53 }
54
55 ✗ Data* d = static_cast<Data*>(data.get());
56 const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q =
57 ✗ x.head(state_->get_nq());
58 const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> v =
59 ✗ x.tail(state_->get_nv());
60
61 ✗ actuation_->calc(d->multibody.actuation, x, u);
62
63 // Computing the dynamics using ABA or manually for armature case
64 ✗ if (without_armature_) {
65 ✗ d->xout = pinocchio::aba(*pinocchio_, d->pinocchio, q, v,
66 ✗ d->multibody.actuation->tau);
67 ✗ pinocchio::updateGlobalPlacements(*pinocchio_, d->pinocchio);
68 } else {
69 ✗ pinocchio::computeAllTerms(*pinocchio_, d->pinocchio, q, v);
70 ✗ d->pinocchio.M.diagonal() += armature_;
71 ✗ pinocchio::cholesky::decompose(*pinocchio_, d->pinocchio);
72 ✗ d->Minv.setZero();
73 ✗ pinocchio::cholesky::computeMinv(*pinocchio_, d->pinocchio, d->Minv);
74 ✗ d->u_drift = d->multibody.actuation->tau - d->pinocchio.nle;
75 ✗ d->xout.noalias() = d->Minv * d->u_drift;
76 }
77 ✗ d->multibody.joint->a = d->xout;
78 ✗ d->multibody.joint->tau = u;
79 ✗ costs_->calc(d->costs, x, u);
80 ✗ d->cost = d->costs->cost;
81 ✗ if (constraints_ != nullptr) {
82 ✗ d->constraints->resize(this, d);
83 ✗ constraints_->calc(d->constraints, x, u);
84 }
85 ✗ }
86
87 template <typename Scalar>
88 ✗ void DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::calc(
89 const std::shared_ptr<DifferentialActionDataAbstract>& data,
90 const Eigen::Ref<const VectorXs>& x) {
91 ✗ if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
92 ✗ throw_pretty(
93 "Invalid argument: " << "x has wrong dimension (it should be " +
94 std::to_string(state_->get_nx()) + ")");
95 }
96
97 ✗ Data* d = static_cast<Data*>(data.get());
98 const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q =
99 ✗ x.head(state_->get_nq());
100 const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> v =
101 ✗ x.tail(state_->get_nv());
102
103 ✗ pinocchio::computeAllTerms(*pinocchio_, d->pinocchio, q, v);
104
105 ✗ costs_->calc(d->costs, x);
106 ✗ d->cost = d->costs->cost;
107 ✗ if (constraints_ != nullptr) {
108 ✗ d->constraints->resize(this, d, false);
109 ✗ constraints_->calc(d->constraints, x);
110 }
111 ✗ }
112
113 template <typename Scalar>
114 ✗ void DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::calcDiff(
115 const std::shared_ptr<DifferentialActionDataAbstract>& data,
116 const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
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 ✗ if (static_cast<std::size_t>(u.size()) != nu_) {
123 ✗ throw_pretty(
124 "Invalid argument: " << "u has wrong dimension (it should be " +
125 std::to_string(nu_) + ")");
126 }
127
128 ✗ const std::size_t nv = state_->get_nv();
129 const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q =
130 ✗ x.head(state_->get_nq());
131 const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> v =
132 ✗ x.tail(nv);
133
134 ✗ Data* d = static_cast<Data*>(data.get());
135
136 ✗ actuation_->calcDiff(d->multibody.actuation, x, u);
137
138 // Computing the dynamics derivatives
139 ✗ if (without_armature_) {
140 ✗ pinocchio::computeABADerivatives(
141 ✗ *pinocchio_, d->pinocchio, q, v, d->multibody.actuation->tau,
142 ✗ d->Fx.leftCols(nv), d->Fx.rightCols(nv), d->pinocchio.Minv);
143 ✗ d->Fx.noalias() += d->pinocchio.Minv * d->multibody.actuation->dtau_dx;
144 ✗ d->Fu.noalias() = d->pinocchio.Minv * d->multibody.actuation->dtau_du;
145 } else {
146 ✗ pinocchio::computeRNEADerivatives(*pinocchio_, d->pinocchio, q, v, d->xout);
147 ✗ d->dtau_dx.leftCols(nv) =
148 ✗ d->multibody.actuation->dtau_dx.leftCols(nv) - d->pinocchio.dtau_dq;
149 ✗ d->dtau_dx.rightCols(nv) =
150 ✗ d->multibody.actuation->dtau_dx.rightCols(nv) - d->pinocchio.dtau_dv;
151 ✗ d->Fx.noalias() = d->Minv * d->dtau_dx;
152 ✗ d->Fu.noalias() = d->Minv * d->multibody.actuation->dtau_du;
153 }
154 ✗ d->multibody.joint->da_dx = d->Fx;
155 ✗ d->multibody.joint->da_du = d->Fu;
156 ✗ costs_->calcDiff(d->costs, x, u);
157 ✗ if (constraints_ != nullptr) {
158 ✗ constraints_->calcDiff(d->constraints, x, u);
159 }
160 ✗ }
161
162 template <typename Scalar>
163 ✗ void DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::calcDiff(
164 const std::shared_ptr<DifferentialActionDataAbstract>& data,
165 const Eigen::Ref<const VectorXs>& x) {
166 ✗ if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
167 ✗ throw_pretty(
168 "Invalid argument: " << "x has wrong dimension (it should be " +
169 std::to_string(state_->get_nx()) + ")");
170 }
171 ✗ Data* d = static_cast<Data*>(data.get());
172
173 ✗ costs_->calcDiff(d->costs, x);
174 ✗ if (constraints_ != nullptr) {
175 ✗ constraints_->calcDiff(d->constraints, x);
176 }
177 ✗ }
178
179 template <typename Scalar>
180 std::shared_ptr<DifferentialActionDataAbstractTpl<Scalar> >
181 ✗ DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::createData() {
182 ✗ return std::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
183 }
184
185 template <typename Scalar>
186 template <typename NewScalar>
187 DifferentialActionModelFreeFwdDynamicsTpl<NewScalar>
188 ✗ DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::cast() const {
189 typedef DifferentialActionModelFreeFwdDynamicsTpl<NewScalar> ReturnType;
190 typedef StateMultibodyTpl<NewScalar> StateType;
191 typedef CostModelSumTpl<NewScalar> CostType;
192 typedef ConstraintModelManagerTpl<NewScalar> ConstraintType;
193 ✗ if (constraints_) {
194 ✗ ReturnType ret(
195 ✗ std::static_pointer_cast<StateType>(state_->template cast<NewScalar>()),
196 ✗ actuation_->template cast<NewScalar>(),
197 ✗ std::make_shared<CostType>(costs_->template cast<NewScalar>()),
198 std::make_shared<ConstraintType>(
199 ✗ constraints_->template cast<NewScalar>()));
200 ✗ return ret;
201 ✗ } else {
202 ✗ ReturnType ret(
203 ✗ std::static_pointer_cast<StateType>(state_->template cast<NewScalar>()),
204 ✗ actuation_->template cast<NewScalar>(),
205 ✗ std::make_shared<CostType>(costs_->template cast<NewScalar>()));
206 ✗ return ret;
207 ✗ }
208 }
209
210 template <typename Scalar>
211 ✗ bool DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::checkData(
212 const std::shared_ptr<DifferentialActionDataAbstract>& data) {
213 ✗ std::shared_ptr<Data> d = std::dynamic_pointer_cast<Data>(data);
214 ✗ if (d != NULL) {
215 ✗ return true;
216 } else {
217 ✗ return false;
218 }
219 ✗ }
220
221 template <typename Scalar>
222 ✗ void DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::quasiStatic(
223 const std::shared_ptr<DifferentialActionDataAbstract>& data,
224 Eigen::Ref<VectorXs> u, const Eigen::Ref<const VectorXs>& x,
225 const std::size_t, const Scalar) {
226 ✗ if (static_cast<std::size_t>(u.size()) != nu_) {
227 ✗ throw_pretty(
228 "Invalid argument: " << "u has wrong dimension (it should be " +
229 std::to_string(nu_) + ")");
230 }
231 ✗ if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
232 ✗ throw_pretty(
233 "Invalid argument: " << "x has wrong dimension (it should be " +
234 std::to_string(state_->get_nx()) + ")");
235 }
236 // Static casting the data
237 ✗ Data* d = static_cast<Data*>(data.get());
238 const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q =
239 ✗ x.head(state_->get_nq());
240
241 ✗ const std::size_t nq = state_->get_nq();
242 ✗ const std::size_t nv = state_->get_nv();
243
244 ✗ d->tmp_xstatic.head(nq) = q;
245 ✗ d->tmp_xstatic.tail(nv).setZero();
246 ✗ u.setZero();
247
248 ✗ pinocchio::rnea(*pinocchio_, d->pinocchio, q, d->tmp_xstatic.tail(nv),
249 ✗ d->tmp_xstatic.tail(nv));
250 ✗ actuation_->calc(d->multibody.actuation, d->tmp_xstatic, u);
251 ✗ actuation_->calcDiff(d->multibody.actuation, d->tmp_xstatic, u);
252
253 ✗ u.noalias() =
254 ✗ pseudoInverse(d->multibody.actuation->dtau_du) * d->pinocchio.tau;
255 ✗ d->pinocchio.tau.setZero();
256 ✗ }
257
258 template <typename Scalar>
259 ✗ std::size_t DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::get_ng() const {
260 ✗ if (constraints_ != nullptr) {
261 ✗ return constraints_->get_ng();
262 } else {
263 ✗ return Base::get_ng();
264 }
265 }
266
267 template <typename Scalar>
268 ✗ std::size_t DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::get_nh() const {
269 ✗ if (constraints_ != nullptr) {
270 ✗ return constraints_->get_nh();
271 } else {
272 ✗ return Base::get_nh();
273 }
274 }
275
276 template <typename Scalar>
277 ✗ std::size_t DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::get_ng_T()
278 const {
279 ✗ if (constraints_ != nullptr) {
280 ✗ return constraints_->get_ng_T();
281 } else {
282 ✗ return Base::get_ng_T();
283 }
284 }
285
286 template <typename Scalar>
287 ✗ std::size_t DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::get_nh_T()
288 const {
289 ✗ if (constraints_ != nullptr) {
290 ✗ return constraints_->get_nh_T();
291 } else {
292 ✗ return Base::get_nh_T();
293 }
294 }
295
296 template <typename Scalar>
297 const typename MathBaseTpl<Scalar>::VectorXs&
298 ✗ DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::get_g_lb() const {
299 ✗ if (constraints_ != nullptr) {
300 ✗ return constraints_->get_lb();
301 } else {
302 ✗ return g_lb_;
303 }
304 }
305
306 template <typename Scalar>
307 const typename MathBaseTpl<Scalar>::VectorXs&
308 ✗ DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::get_g_ub() const {
309 ✗ if (constraints_ != nullptr) {
310 ✗ return constraints_->get_ub();
311 } else {
312 ✗ return g_lb_;
313 }
314 }
315
316 template <typename Scalar>
317 ✗ void DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::print(
318 std::ostream& os) const {
319 ✗ os << "DifferentialActionModelFreeFwdDynamics {nx=" << state_->get_nx()
320 ✗ << ", ndx=" << state_->get_ndx() << ", nu=" << nu_ << "}";
321 ✗ }
322
323 template <typename Scalar>
324 pinocchio::ModelTpl<Scalar>&
325 ✗ DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::get_pinocchio() const {
326 ✗ return *pinocchio_;
327 }
328
329 template <typename Scalar>
330 const std::shared_ptr<ActuationModelAbstractTpl<Scalar> >&
331 ✗ DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::get_actuation() const {
332 ✗ return actuation_;
333 }
334
335 template <typename Scalar>
336 const std::shared_ptr<CostModelSumTpl<Scalar> >&
337 ✗ DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::get_costs() const {
338 ✗ return costs_;
339 }
340
341 template <typename Scalar>
342 const std::shared_ptr<ConstraintModelManagerTpl<Scalar> >&
343 ✗ DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::get_constraints() const {
344 ✗ return constraints_;
345 }
346
347 template <typename Scalar>
348 const typename MathBaseTpl<Scalar>::VectorXs&
349 ✗ DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::get_armature() const {
350 ✗ return armature_;
351 }
352
353 template <typename Scalar>
354 ✗ void DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::set_armature(
355 const VectorXs& armature) {
356 ✗ if (static_cast<std::size_t>(armature.size()) != state_->get_nv()) {
357 ✗ throw_pretty("Invalid argument: "
358 << "The armature dimension is wrong (it should be " +
359 std::to_string(state_->get_nv()) + ")");
360 }
361
362 ✗ armature_ = armature;
363 ✗ without_armature_ = false;
364 ✗ }
365
366 } // namespace crocoddyl
367