GCC Code Coverage Report

Directory: ./
File: src/util.cpp
Date: 2025-03-17 04:04:52
Exec Total Coverage
Lines: 97 112 86.6%
Branches: 88 170 51.8%
Line Branch Exec Source
1 /*
2 * Copyright 2015, LAAS-CNRS
3 * Author: Andrea Del Prete
4 */
5
6 #include <ctime>
7 #include <hpp/centroidal-dynamics/util.hh>
8
9 namespace centroidal_dynamics {
10
11 11 dd_MatrixPtr cone_span_eigen_to_cdd(Cref_matrixXX input,
12 const bool canonicalize) {
13 11 dd_debug = false;
14 11 dd_MatrixPtr M = NULL;
15 dd_rowrange i;
16 dd_colrange j;
17 11 dd_rowrange m_input = (dd_rowrange)(input.rows());
18 11 dd_colrange d_input = (dd_colrange)(input.cols() + 1);
19 11 dd_RepresentationType rep = dd_Generator;
20 mytype value;
21 11 dd_NumberType NT = dd_Real;
22
1/2
✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
 11 dd_init(value);
23
1/2
✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
 11 M = dd_CreateMatrix(m_input, d_input);
24 11 M->representation = rep;
25 11 M->numbtype = NT;
26
2/2
✓ Branch 1 taken 336 times.
✓ Branch 2 taken 11 times.
 347 for (i = 0; i < input.rows(); i++) {
27
1/2
✓ Branch 1 taken 336 times.
✗ Branch 2 not taken.
 336 dd_set_d(value, 0);
28
1/2
✓ Branch 1 taken 336 times.
✗ Branch 2 not taken.
 336 dd_set(M->matrix[i][0], value);
29
2/2
✓ Branch 0 taken 2016 times.
✓ Branch 1 taken 336 times.
 2352 for (j = 1; j < d_input; j++) {
30
2/4
✓ Branch 1 taken 2016 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2016 times.
✗ Branch 5 not taken.
 2016 dd_set_d(value, input(i, j - 1));
31
1/2
✓ Branch 1 taken 2016 times.
✗ Branch 2 not taken.
 2016 dd_set(M->matrix[i][j], value);
32 }
33 }
34
1/2
✓ Branch 1 taken 11 times.
✗ Branch 2 not taken.
 11 dd_clear(value);
35
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 10 times.
 11 if (canonicalize) {
36 1 dd_ErrorType error = dd_NoError;
37 dd_rowset redset, impl_linset;
38 dd_rowindex newpos;
39
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 dd_MatrixCanonicalize(&M, &impl_linset, &redset, &newpos, &error);
40
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 set_free(redset);
41
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 set_free(impl_linset);
42 1 free(newpos);
43 }
44 11 return M;
45 }
46
47 2 void init_cdd_library() {
48 2 dd_set_global_constants();
49 2 dd_debug = false;
50 2 }
51
52 ✗ void release_cdd_library() {
53 // dd_free_global_constants();
54 }
55
56 1000 void uniform3(Cref_vector3 lower_bounds, Cref_vector3 upper_bounds,
57 Ref_vector3 out) {
58
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 1000 times.
 1000 assert(lower_bounds.rows() == out.rows());
59
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 1000 times.
 1000 assert(upper_bounds.rows() == out.rows());
60
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 1000 times.
 1000 assert(lower_bounds.cols() == out.cols());
61
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 1000 times.
 1000 assert(upper_bounds.cols() == out.cols());
62
2/2
✓ Branch 1 taken 3000 times.
✓ Branch 2 taken 1000 times.
 4000 for (int i = 0; i < out.rows(); i++)
63
2/2
✓ Branch 1 taken 3000 times.
✓ Branch 2 taken 3000 times.
 6000 for (int j = 0; j < out.cols(); j++)
64 6000 out(i, j) = (rand() / value_type(RAND_MAX)) *
65 3000 (upper_bounds(i, j) - lower_bounds(i, j)) +
66 3000 lower_bounds(i, j);
67 1000 }
68
69 40 void uniform(Cref_matrixXX lower_bounds, Cref_matrixXX upper_bounds,
70 Ref_matrixXX out) {
71
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 40 times.
 40 assert(lower_bounds.rows() == out.rows());
72
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 40 times.
 40 assert(upper_bounds.rows() == out.rows());
73
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 40 times.
 40 assert(lower_bounds.cols() == out.cols());
74
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 40 times.
 40 assert(upper_bounds.cols() == out.cols());
75
2/2
✓ Branch 1 taken 40 times.
✓ Branch 2 taken 40 times.
 80 for (int i = 0; i < out.rows(); i++)
76
2/2
✓ Branch 1 taken 120 times.
✓ Branch 2 taken 40 times.
 160 for (int j = 0; j < out.cols(); j++)
77 240 out(i, j) = (rand() / value_type(RAND_MAX)) *
78 120 (upper_bounds(i, j) - lower_bounds(i, j)) +
79 120 lower_bounds(i, j);
80 40 }
81
82 20 void euler_matrix(double roll, double pitch, double yaw, Ref_rotation R) {
83 20 const int i = 0;
84 20 const int j = 1;
85 20 const int k = 2;
86 20 double si = sin(roll);
87 20 double sj = sin(pitch);
88 20 double sk = sin(yaw);
89 20 double ci = cos(roll);
90 20 double cj = cos(pitch);
91 20 double ck = cos(yaw);
92 20 double cc = ci * ck;
93 20 double cs = ci * sk;
94 20 double sc = si * ck;
95 20 double ss = si * sk;
96
97 20 R(i, i) = cj * ck;
98 20 R(i, j) = sj * sc - cs;
99 20 R(i, k) = sj * cc + ss;
100 20 R(j, i) = cj * sk;
101 20 R(j, j) = sj * ss + cc;
102 20 R(j, k) = sj * cs - sc;
103 20 R(k, i) = -sj;
104 20 R(k, j) = cj * si;
105 20 R(k, k) = cj * ci;
106 // R = (angle_axis_t(roll, Vector3::UnitX())
107 // * angle_axis_t(pitch, Vector3::UnitY())
108 // * angle_axis_t(yaw, Vector3::UnitZ())).toRotationMatrix();
109 20 }
110
111 20 bool generate_rectangle_contacts(double lx, double ly, Cref_vector3 pos,
112 Cref_vector3 rpy, Ref_matrix43 p,
113 Ref_matrix43 N) {
114 // compute rotation matrix
115
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 Rotation R;
116
5/10
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 20 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 20 times.
✗ Branch 14 not taken.
 20 euler_matrix(rpy(0), rpy(1), rpy(2), R);
117 // contact points in local frame
118
12/24
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 20 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 20 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 20 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 20 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 20 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 20 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 20 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 20 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 20 times.
✗ Branch 35 not taken.
 20 p << lx, ly, 0, lx, -ly, 0, -lx, -ly, 0, -lx, ly, 0;
119 // contact points in world frame
120
6/12
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 20 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 20 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 20 times.
✗ Branch 17 not taken.
 20 p.row(0) = pos + (R * p.row(0).transpose());
121
6/12
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 20 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 20 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 20 times.
✗ Branch 17 not taken.
 20 p.row(1) = pos + (R * p.row(1).transpose());
122
6/12
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 20 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 20 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 20 times.
✗ Branch 17 not taken.
 20 p.row(2) = pos + (R * p.row(2).transpose());
123
6/12
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 20 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 20 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 20 times.
✗ Branch 17 not taken.
 20 p.row(3) = pos + (R * p.row(3).transpose());
124 // normal direction in local frame
125
1/2
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
 20 RVector3 n;
126
3/6
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20 times.
✗ Branch 8 not taken.
 20 n << 0, 0, 1;
127 // normal directions in world frame
128
4/8
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 20 times.
✗ Branch 11 not taken.
 20 n = (R * n.transpose()).transpose();
129
4/8
✓ Branch 1 taken 20 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 20 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 20 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 20 times.
✗ Branch 11 not taken.
 20 N << n, n, n, n;
130 20 return true;
131 }
132
133 11381 Rotation crossMatrix(Cref_vector3 x) {
134
1/2
✓ Branch 2 taken 11381 times.
✗ Branch 3 not taken.
 11381 Rotation res = Rotation::Zero();
135 11381 res(0, 1) = -x(2);
136 11381 res(0, 2) = x(1);
137 11381 res(1, 0) = x(2);
138 11381 res(1, 2) = -x(0);
139 11381 res(2, 0) = -x(1);
140 11381 res(2, 1) = x(0);
141 11381 return res;
142 }
143
144 ✗ std::string getDateAndTimeAsString() {
145 time_t rawtime;
146 struct tm* timeinfo;
147 char buffer[80];
148 ✗ time(&rawtime);
149 ✗ timeinfo = localtime(&rawtime);
150 ✗ strftime(buffer, 80, "%Y%m%d_%I%M%S", timeinfo);
151 ✗ return std::string(buffer);
152 }
153 /*
154 int fact(const int n)
155 {
156 assert(n>=0);
157 int res = 1;
158 for (int i=2 ; i <= n ; ++i)
159 res *= i;
160 return res;
161 }
162
163 int choosenk(const int n, const int k)
164 {
165 return fact(n) / (fact(k) * fact(n - k));
166 }*/
167
168 /* is this faster ?
169 value_type choosenk(const int n, const int k)
170 {
171 if(k>n/2)
172 return nchoosek(n,n-k);
173 else if(k==1)
174 return n;
175 else
176 {
177 double c = 1;
178 for(int i = 1;i<=k;i++)
179 c *= (((double)n-k+i)/((double)i));
180 return std::round(c);
181 }
182 }*/
183
184 ✗ value_type nchoosek(const int n, const int k) {
185 ✗ if (k > n / 2)
186 ✗ return nchoosek(n, n - k);
187 ✗ else if (k == 1)
188 ✗ return n;
189 else {
190 ✗ value_type c = 1;
191 ✗ for (int i = 1; i <= k; i++)
192 ✗ c *= (((value_type)n - k + i) / ((value_type)i));
193 ✗ return round(c);
194 }
195 }
196
197 } // namespace centroidal_dynamics
198