GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/multibody/residuals/contact-cop-position.hxx
Date: 2025-01-16 08:47:40
Exec Total Coverage
Lines: 42 56 75.0%
Branches: 15 62 24.2%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2020-2023, University of Duisburg-Essen, University of
5 // Edinburgh,
6 // Heriot-Watt University
7 // Copyright note valid unless otherwise stated in individual files.
8 // All rights reserved.
9 ///////////////////////////////////////////////////////////////////////////////
10
11 #include "crocoddyl/multibody/residuals/contact-cop-position.hpp"
12
13 namespace crocoddyl {
14
15 template <typename _Scalar>
16 32 ResidualModelContactCoPPositionTpl<_Scalar>::ResidualModelContactCoPPositionTpl(
17 boost::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
18 const CoPSupport& cref, const std::size_t nu, const bool fwddyn)
19 : Base(state, 4, nu, fwddyn ? true : false, fwddyn ? true : false, true),
20 32 fwddyn_(fwddyn),
21 32 update_jacobians_(true),
22 32 id_(id),
23
4/8
✓ Branch 0 taken 32 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 32 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 32 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 32 times.
✗ Branch 11 not taken.
 32 cref_(cref) {}
24
25 template <typename _Scalar>
26 1 ResidualModelContactCoPPositionTpl<_Scalar>::ResidualModelContactCoPPositionTpl(
27 boost::shared_ptr<StateMultibody> state, const pinocchio::FrameIndex id,
28 const CoPSupport& cref)
29 : Base(state, 4),
30 1 fwddyn_(true),
31 1 update_jacobians_(true),
32 1 id_(id),
33
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1 times.
✗ Branch 7 not taken.
 1 cref_(cref) {}
34
35 template <typename Scalar>
36 70 ResidualModelContactCoPPositionTpl<
37 70 Scalar>::~ResidualModelContactCoPPositionTpl() {}
38
39 template <typename Scalar>
40 3416 void ResidualModelContactCoPPositionTpl<Scalar>::calc(
41 const boost::shared_ptr<ResidualDataAbstract>& data,
42 const Eigen::Ref<const VectorXs>&, const Eigen::Ref<const VectorXs>&) {
43 3416 Data* d = static_cast<Data*>(data.get());
44
45 // Compute the residual residual r = A * f
46
2/4
✓ Branch 6 taken 3416 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 3416 times.
✗ Branch 10 not taken.
 3416 data->r.noalias() = cref_.get_A() * d->contact->f.toVector();
47 3416 }
48
49 template <typename Scalar>
50 2628 void ResidualModelContactCoPPositionTpl<Scalar>::calc(
51 const boost::shared_ptr<ResidualDataAbstract>& data,
52 const Eigen::Ref<const VectorXs>&) {
53 2628 data->r.setZero();
54 2628 }
55
56 template <typename Scalar>
57 38 void ResidualModelContactCoPPositionTpl<Scalar>::calcDiff(
58 const boost::shared_ptr<ResidualDataAbstract>& data,
59 const Eigen::Ref<const VectorXs>&, const Eigen::Ref<const VectorXs>&) {
60
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 38 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 38 if (fwddyn_ || update_jacobians_) {
61 38 updateJacobians(data);
62 }
63 38 }
64
65 template <typename Scalar>
66 34 void ResidualModelContactCoPPositionTpl<Scalar>::calcDiff(
67 const boost::shared_ptr<ResidualDataAbstract>& data,
68 const Eigen::Ref<const VectorXs>&) {
69 34 data->Rx.setZero();
70 34 }
71
72 template <typename Scalar>
73 boost::shared_ptr<ResidualDataAbstractTpl<Scalar> >
74 3418 ResidualModelContactCoPPositionTpl<Scalar>::createData(
75 DataCollectorAbstract* const data) {
76
1/2
✓ Branch 1 taken 3418 times.
✗ Branch 2 not taken.
 6836 boost::shared_ptr<ResidualDataAbstract> d = boost::allocate_shared<Data>(
77 3418 Eigen::aligned_allocator<Data>(), this, data);
78
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 3418 times.
 3418 if (!fwddyn_) {
79 ✗ updateJacobians(d);
80 }
81 3418 return d;
82 }
83
84 template <typename Scalar>
85 38 void ResidualModelContactCoPPositionTpl<Scalar>::updateJacobians(
86 const boost::shared_ptr<ResidualDataAbstract>& data) {
87 38 Data* d = static_cast<Data*>(data.get());
88
89 38 const MatrixXs& df_dx = d->contact->df_dx;
90 38 const MatrixXs& df_du = d->contact->df_du;
91 38 const Matrix46& A = cref_.get_A();
92
2/4
✓ Branch 3 taken 38 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 38 times.
✗ Branch 7 not taken.
 38 data->Rx.noalias() = A * df_dx;
93
2/4
✓ Branch 3 taken 38 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 38 times.
✗ Branch 7 not taken.
 38 data->Ru.noalias() = A * df_du;
94 38 update_jacobians_ = false;
95 38 }
96
97 template <typename Scalar>
98 ✗ void ResidualModelContactCoPPositionTpl<Scalar>::print(std::ostream& os) const {
99 ✗ boost::shared_ptr<StateMultibody> s =
100 ✗ boost::static_pointer_cast<StateMultibody>(state_);
101 ✗ const Eigen::IOFormat fmt(2, Eigen::DontAlignCols, ", ", ";\n", "", "", "[",
102 "]");
103 os << "ResidualModelContactCoPPosition {frame="
104 ✗ << s->get_pinocchio()->frames[id_].name
105 ✗ << ", box=" << cref_.get_box().transpose().format(fmt) << "}";
106 }
107
108 template <typename Scalar>
109 bool ResidualModelContactCoPPositionTpl<Scalar>::is_fwddyn() const {
110 return fwddyn_;
111 }
112
113 template <typename Scalar>
114 3418 pinocchio::FrameIndex ResidualModelContactCoPPositionTpl<Scalar>::get_id()
115 const {
116 3418 return id_;
117 }
118
119 template <typename Scalar>
120 const CoPSupportTpl<Scalar>&
121 ✗ ResidualModelContactCoPPositionTpl<Scalar>::get_reference() const {
122 ✗ return cref_;
123 }
124
125 template <typename Scalar>
126 ✗ void ResidualModelContactCoPPositionTpl<Scalar>::set_id(
127 const pinocchio::FrameIndex id) {
128 ✗ id_ = id;
129 }
130
131 template <typename Scalar>
132 ✗ void ResidualModelContactCoPPositionTpl<Scalar>::set_reference(
133 const CoPSupport& reference) {
134 ✗ cref_ = reference;
135 ✗ update_jacobians_ = true;
136 }
137
138 } // namespace crocoddyl
139