GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/multibody/cop-support.hxx
Date: 2025-03-26 19:23:43
Exec Total Coverage
Lines: 57 79 72.2%
Branches: 34 80 42.5%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2021-2025, University of Edinburgh, University of Oxford,
5 // Heriot-Watt University
6 // Copyright note valid unless otherwise stated in individual files.
7 // All rights reserved.
8 ///////////////////////////////////////////////////////////////////////////////
9
10 #include <iostream>
11
12 namespace crocoddyl {
13
14 template <typename Scalar>
15 62 CoPSupportTpl<Scalar>::CoPSupportTpl(const Matrix3s& R, const Vector2s& box)
16 62 : R_(R), box_(box) {
17 62 A_.setZero();
18 62 ub_.setZero();
19 62 lb_.setZero();
20
21 // Update the inequality matrix and bounds
22 62 update();
23 62 }
24
25 template <typename Scalar>
26 ✗ CoPSupportTpl<Scalar>::CoPSupportTpl(const WrenchConeTpl<Scalar>& other)
27 ✗ : A_(other.get_A()),
28 ✗ ub_(other.get_ub()),
29 ✗ lb_(other.get_lb()),
30 ✗ R_(other.get_R()),
31 ✗ box_(other.get_box()) {}
32
33 template <typename Scalar>
34 39 CoPSupportTpl<Scalar>::CoPSupportTpl(const CoPSupportTpl<Scalar>& other)
35 39 : A_(other.get_A()),
36 39 ub_(other.get_ub()),
37 39 lb_(other.get_lb()),
38 39 R_(other.get_R()),
39 39 box_(other.get_box()) {}
40
41 template <typename Scalar>
42 3 CoPSupportTpl<Scalar>::CoPSupportTpl()
43
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 : R_(Matrix3s::Identity()),
44
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 box_(std::numeric_limits<Scalar>::infinity(),
45 6 std::numeric_limits<Scalar>::infinity()) {
46 3 A_.setZero();
47 3 ub_.setZero();
48 3 lb_.setZero();
49
50 // Update the inequality matrix and bounds
51 3 update();
52 3 }
53
54 template <typename Scalar>
55 104 CoPSupportTpl<Scalar>::~CoPSupportTpl() {}
56
57 template <typename Scalar>
58 65 void CoPSupportTpl<Scalar>::update() {
59 // Initialize the matrix and bounds
60 65 A_.setZero();
61 65 ub_.setZero();
62 65 lb_.setOnes();
63
1/2
✓ Branch 2 taken 65 times.
✗ Branch 3 not taken.
 65 lb_ *= -std::numeric_limits<Scalar>::infinity();
64
65 // CoP information
66 // This matrix is defined as
67 // [0 0 -W 1 0 0;
68 // 0 0 -W -1 0 0;
69 // 0 0 -L 0 1 0;
70 // 0 0 -L 0 -1 0]
71 65 const Scalar L = box_(0) / Scalar(2.);
72 65 const Scalar W = box_(1) / Scalar(2.);
73
7/14
✓ Branch 2 taken 65 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 65 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 65 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 65 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 65 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 65 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 65 times.
✗ Branch 21 not taken.
 65 A_.row(0) << -W * R_.col(2).transpose(), R_.col(0).transpose();
74
8/16
✓ Branch 2 taken 65 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 65 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 65 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 65 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 65 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 65 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 65 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 65 times.
✗ Branch 24 not taken.
 65 A_.row(1) << -W * R_.col(2).transpose(), -R_.col(0).transpose();
75
7/14
✓ Branch 2 taken 65 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 65 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 65 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 65 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 65 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 65 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 65 times.
✗ Branch 21 not taken.
 65 A_.row(2) << -L * R_.col(2).transpose(), R_.col(1).transpose();
76
8/16
✓ Branch 2 taken 65 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 65 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 65 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 65 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 65 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 65 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 65 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 65 times.
✗ Branch 24 not taken.
 65 A_.row(3) << -L * R_.col(2).transpose(), -R_.col(1).transpose();
77 65 }
78
79 template <typename Scalar>
80 template <typename NewScalar>
81 6 CoPSupportTpl<NewScalar> CoPSupportTpl<Scalar>::cast() const {
82 typedef CoPSupportTpl<NewScalar> ReturnType;
83
0/8
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
 6 ReturnType ret(R_.template cast<NewScalar>(),
84 6 box_.template cast<NewScalar>());
85 6 return ret;
86 }
87
88 template <typename Scalar>
89 3578 const typename MathBaseTpl<Scalar>::Matrix46s& CoPSupportTpl<Scalar>::get_A()
90 const {
91 3578 return A_;
92 }
93
94 template <typename Scalar>
95 114 const typename MathBaseTpl<Scalar>::Vector4s& CoPSupportTpl<Scalar>::get_ub()
96 const {
97 114 return ub_;
98 }
99
100 template <typename Scalar>
101 94 const typename MathBaseTpl<Scalar>::Vector4s& CoPSupportTpl<Scalar>::get_lb()
102 const {
103 94 return lb_;
104 }
105
106 template <typename Scalar>
107 57 const typename MathBaseTpl<Scalar>::Vector2s& CoPSupportTpl<Scalar>::get_box()
108 const {
109 57 return box_;
110 }
111
112 template <typename Scalar>
113 59 const typename MathBaseTpl<Scalar>::Matrix3s& CoPSupportTpl<Scalar>::get_R()
114 const {
115 59 return R_;
116 }
117
118 template <typename Scalar>
119 ✗ void CoPSupportTpl<Scalar>::set_R(const Matrix3s& R) {
120 ✗ R_ = R;
121 }
122
123 template <typename Scalar>
124 ✗ void CoPSupportTpl<Scalar>::set_box(const Vector2s& box) {
125 ✗ box_ = box;
126 ✗ if (box_(0) < Scalar(0.)) {
127 ✗ box_(0) = std::numeric_limits<Scalar>::infinity();
128 ✗ std::cerr << "Warning: box(0) has to be a positive value, set to inf float"
129 ✗ << std::endl;
130 }
131 ✗ if (box_(1) < Scalar(0.)) {
132 ✗ box_(1) = std::numeric_limits<Scalar>::infinity();
133 ✗ std::cerr << "Warning: box(0) has to be a positive value, set to inf float"
134 ✗ << std::endl;
135 }
136 }
137
138 template <typename Scalar>
139 6 CoPSupportTpl<Scalar>& CoPSupportTpl<Scalar>::operator=(
140 const CoPSupportTpl<Scalar>& other) {
141
1/2
✓ Branch 0 taken 6 times.
✗ Branch 1 not taken.
 6 if (this != &other) {
142 6 A_ = other.get_A();
143 6 ub_ = other.get_ub();
144 6 lb_ = other.get_lb();
145 6 R_ = other.get_R();
146 6 box_ = other.get_box();
147 }
148 6 return *this;
149 }
150
151 template <typename Scalar>
152 ✗ std::ostream& operator<<(std::ostream& os, const CoPSupportTpl<Scalar>& X) {
153 ✗ os << " R: " << X.get_R() << std::endl;
154 ✗ os << " box: " << X.get_box().transpose() << std::endl;
155 ✗ return os;
156 }
157
158 } // namespace crocoddyl
159