GCC Code Coverage Report

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