GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/multibody/residuals/impulse-com.hxx
Date: 2025-03-26 19:23:43
Exec Total Coverage
Lines: 29 36 80.6%
Branches: 22 46 47.8%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2021-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 #include <pinocchio/algorithm/center-of-mass-derivatives.hpp>
11 #include <pinocchio/algorithm/center-of-mass.hpp>
12
13 #include "crocoddyl/multibody/residuals/impulse-com.hpp"
14
15 namespace crocoddyl {
16
17 template <typename Scalar>
18 8 ResidualModelImpulseCoMTpl<Scalar>::ResidualModelImpulseCoMTpl(
19 std::shared_ptr<StateMultibody> state)
20 : Base(state, 3, 0, true, true, false),
21
2/4
✓ Branch 2 taken 8 times.
✗ Branch 3 not taken.
✓ Branch 7 taken 8 times.
✗ Branch 8 not taken.
 8 pin_model_(state->get_pinocchio()) {}
22
23 template <typename Scalar>
24 2016 void ResidualModelImpulseCoMTpl<Scalar>::calc(
25 const std::shared_ptr<ResidualDataAbstract>& data,
26 const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>&) {
27 // Compute the residual residual give the reference CoM position
28 2016 Data* d = static_cast<Data*>(data.get());
29
1/2
✓ Branch 2 taken 2016 times.
✗ Branch 3 not taken.
 2016 const std::size_t nq = state_->get_nq();
30
1/2
✓ Branch 2 taken 2016 times.
✗ Branch 3 not taken.
 2016 const std::size_t nv = state_->get_nv();
31 const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q =
32
1/2
✓ Branch 1 taken 2016 times.
✗ Branch 2 not taken.
 2016 x.head(nq);
33 const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> v =
34
1/2
✓ Branch 1 taken 2016 times.
✗ Branch 2 not taken.
 2016 x.tail(nv);
35
36
1/2
✓ Branch 2 taken 2016 times.
✗ Branch 3 not taken.
 2016 pinocchio::centerOfMass(*pin_model_.get(), d->pinocchio_internal, q,
37
1/2
✓ Branch 2 taken 2016 times.
✗ Branch 3 not taken.
 2016 d->impulses->vnext - v);
38
1/2
✓ Branch 3 taken 2016 times.
✗ Branch 4 not taken.
 2016 data->r = d->pinocchio_internal.vcom[0];
39 2016 }
40
41 template <typename Scalar>
42 26 void ResidualModelImpulseCoMTpl<Scalar>::calcDiff(
43 const std::shared_ptr<ResidualDataAbstract>& data,
44 const Eigen::Ref<const VectorXs>&, const Eigen::Ref<const VectorXs>&) {
45 26 Data* d = static_cast<Data*>(data.get());
46
47 // Compute the derivatives of the frame placement
48 26 const std::size_t nv = state_->get_nv();
49 26 const std::size_t ndx = state_->get_ndx();
50 26 pinocchio::getCenterOfMassVelocityDerivatives(
51 26 *pin_model_.get(), d->pinocchio_internal, d->dvc_dq);
52 26 pinocchio::jacobianCenterOfMass(*pin_model_.get(), d->pinocchio_internal,
53 false);
54
1/2
✓ Branch 3 taken 26 times.
✗ Branch 4 not taken.
 26 d->ddv_dv = d->impulses->dvnext_dx.rightCols(ndx - nv);
55
3/6
✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 26 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 26 times.
✗ Branch 8 not taken.
 26 d->ddv_dv.diagonal().array() -= Scalar(1);
56
1/2
✓ Branch 3 taken 26 times.
✗ Branch 4 not taken.
 26 data->Rx.leftCols(nv) = d->dvc_dq;
57
3/6
✓ Branch 2 taken 26 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 26 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 26 times.
✗ Branch 9 not taken.
 26 data->Rx.leftCols(nv).noalias() +=
58
1/2
✓ Branch 3 taken 26 times.
✗ Branch 4 not taken.
 26 d->pinocchio_internal.Jcom * d->impulses->dvnext_dx.leftCols(nv);
59
3/6
✓ Branch 2 taken 26 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 26 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 26 times.
✗ Branch 9 not taken.
 26 data->Rx.rightCols(ndx - nv).noalias() =
60 26 d->pinocchio_internal.Jcom * d->ddv_dv;
61 26 }
62
63 template <typename Scalar>
64 std::shared_ptr<ResidualDataAbstractTpl<Scalar> >
65 1080 ResidualModelImpulseCoMTpl<Scalar>::createData(
66 DataCollectorAbstract* const data) {
67 ✗ return std::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this,
68
1/2
✓ Branch 2 taken 1080 times.
✗ Branch 3 not taken.
 1080 data);
69 }
70
71 template <typename Scalar>
72 template <typename NewScalar>
73 ✗ ResidualModelImpulseCoMTpl<NewScalar> ResidualModelImpulseCoMTpl<Scalar>::cast()
74 const {
75 typedef ResidualModelImpulseCoMTpl<NewScalar> ReturnType;
76 typedef StateMultibodyTpl<NewScalar> StateType;
77 ✗ ReturnType ret(
78 ✗ std::static_pointer_cast<StateType>(state_->template cast<NewScalar>()));
79 ✗ return ret;
80 }
81
82 template <typename Scalar>
83 ✗ void ResidualModelImpulseCoMTpl<Scalar>::print(std::ostream& os) const {
84 ✗ os << "ResidualModelImpulseCoM";
85 }
86
87 } // namespace crocoddyl
88