GCC Code Coverage Report

Directory: ./
File: include/hpp/bezier-com-traj/utils.hh
Date: 2025-03-18 04:20:50
Exec Total Coverage
Lines: 33 37 89.2%
Branches: 29 48 60.4%
Line Branch Exec Source
1 /*
2 * Copyright 2018, LAAS-CNRS
3 * Author: Steve Tonneau
4 */
5
6 #ifndef BEZIER_COM_TRAJ_LIB_UTILS_H
7 #define BEZIER_COM_TRAJ_LIB_UTILS_H
8
9 #include <Eigen/Dense>
10 #include <hpp/bezier-com-traj/definitions.hh>
11 #include <hpp/bezier-com-traj/flags.hh>
12 #include <hpp/bezier-com-traj/local_config.hh>
13 #include <vector>
14
15 namespace bezier_com_traj {
16
17 template <typename T>
18 T initwp();
19 waypoint_t initwp(const size_t rows, const size_t cols);
20 waypoint_t operator+(const waypoint_t& w1, const waypoint_t& w2);
21 waypoint_t operator-(const waypoint_t& w1, const waypoint_t& w2);
22 waypoint_t operator*(const double k, const waypoint_t& w);
23 waypoint_t operator*(const waypoint_t& w, const double k);
24
25 struct waypoint_t {
26 MatrixXX first;
27 VectorX second;
28
29
1/2
✓ Branch 2 taken 2209749 times.
✗ Branch 3 not taken.
 2209749 waypoint_t() : first(MatrixXX()), second(VectorX()) {}
30
31
1/2
✓ Branch 2 taken 7825847 times.
✗ Branch 3 not taken.
 7825847 waypoint_t(MatrixXX A, VectorX b) : first(A), second(b) {}
32
33 ✗ static waypoint_t Zero(size_t dim) { return initwp(dim, dim); }
34
35 2611270 size_t size() const { return second.size(); }
36
37 ✗ bool isApprox(const waypoint_t& other,
38 const value_type prec =
39 Eigen::NumTraits<value_type>::dummy_precision()) const {
40 ✗ return first.isApprox(other.first, prec) &&
41 ✗ second.isApprox(other.second, prec);
42 }
43
44 bool operator==(const waypoint_t& other) const { return isApprox(other); }
45
46 bool operator!=(const waypoint_t& other) const { return !(*this == other); }
47 };
48
49 /**
50 * @brief Compute the Bernstein polynoms for a given degree
51 * @param degree required degree
52 * @return the bernstein polynoms
53 */
54 BEZIER_COM_TRAJ_DLLAPI std::vector<ndcurves::Bern<double> >
55 ComputeBersteinPolynoms(const unsigned int degree);
56
57 /**
58 * @brief given the constraints of the problem, and a set of waypoints, return
59 * the bezier curve corresponding
60 * @param pData problem data
61 * @param T total trajectory time
62 * @param pis list of waypoints
63 * @return the bezier curve
64 */
65 template <typename Bezier, typename Point>
66 BEZIER_COM_TRAJ_DLLAPI Bezier computeBezierCurve(const ConstraintFlag& flag,
67 const double T,
68 const std::vector<Point>& pi,
69 const Point& x);
70
71 /**
72 * @brief computeDiscretizedTime build an array of discretized points in time,
73 * such that there is the same number of point in each phase. Doesn't contain
74 * t=0, is of size pointsPerPhase*phaseTimings.size()
75 * @param phaseTimings
76 * @param pointsPerPhase
77 * @return
78 */
79 T_time computeDiscretizedTimeFixed(const VectorX& phaseTimings,
80 const unsigned int pointsPerPhase);
81
82 /**
83 * @brief computeDiscretizedTime build an array of discretized points in time,
84 * given the timestep. Doesn't contain t=0,
85 * is of size pointsPerPhase*phaseTimings.size()
86 * @param phaseTimings
87 * @param timeStep
88 * @return */
89 T_time computeDiscretizedTime(const VectorX& phaseTimings,
90 const double timeStep);
91
92 /**
93 * @brief write a polytope describe by A x <= b linear constraints in
94 * a given filename
95 * @return the bernstein polynoms
96 */
97 void printQHullFile(const std::pair<MatrixXX, VectorX>& Ab, VectorX intPoint,
98 const std::string& fileName, bool clipZ = false);
99
100 /**
101 * @brief skew symmetric matrix
102 */
103 BEZIER_COM_TRAJ_DLLAPI Matrix3 skew(point_t_tC x);
104
105 /**
106 * @brief normalize inequality constraints
107 */
108 int Normalize(Ref_matrixXX A, Ref_vectorX b);
109
110 } // end namespace bezier_com_traj
111
112 template <typename Bezier, typename Point>
113 49 Bezier bezier_com_traj::computeBezierCurve(const ConstraintFlag& flag,
114 const double T,
115 const std::vector<Point>& pi,
116 const Point& x) {
117 49 std::vector<Point> wps;
118 49 size_t i = 0;
119
1/2
✓ Branch 1 taken 49 times.
✗ Branch 2 not taken.
 49 if (flag & INIT_POS) {
120
1/2
✓ Branch 2 taken 49 times.
✗ Branch 3 not taken.
 49 wps.push_back(pi[i]);
121 49 i++;
122
1/2
✓ Branch 1 taken 49 times.
✗ Branch 2 not taken.
 49 if (flag & INIT_VEL) {
123
1/2
✓ Branch 2 taken 49 times.
✗ Branch 3 not taken.
 49 wps.push_back(pi[i]);
124 49 i++;
125
2/2
✓ Branch 1 taken 20 times.
✓ Branch 2 taken 29 times.
 49 if (flag & INIT_ACC) {
126
1/2
✓ Branch 2 taken 20 times.
✗ Branch 3 not taken.
 20 wps.push_back(pi[i]);
127 20 i++;
128 }
129 }
130 }
131
1/2
✓ Branch 1 taken 49 times.
✗ Branch 2 not taken.
 49 wps.push_back(x);
132 49 i++;
133
6/6
✓ Branch 1 taken 39 times.
✓ Branch 2 taken 10 times.
✓ Branch 4 taken 5 times.
✓ Branch 5 taken 34 times.
✓ Branch 6 taken 5 times.
✓ Branch 7 taken 44 times.
 49 if (flag & (END_VEL) && !(flag & (END_POS))) {
134
1/2
✓ Branch 1 taken 5 times.
✗ Branch 2 not taken.
 5 wps.push_back(x);
135 5 i++;
136 } else {
137
2/2
✓ Branch 1 taken 10 times.
✓ Branch 2 taken 34 times.
 44 if (flag & END_ACC) {
138
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 assert(flag & END_VEL &&
139 "You cannot constrain final acceleration if final velocity is not "
140 "constrained.");
141
1/2
✓ Branch 2 taken 10 times.
✗ Branch 3 not taken.
 10 wps.push_back(pi[i]);
142 10 i++;
143 }
144
2/2
✓ Branch 1 taken 34 times.
✓ Branch 2 taken 10 times.
 44 if (flag & END_VEL) {
145
1/2
✓ Branch 1 taken 34 times.
✗ Branch 2 not taken.
 34 assert(flag & END_POS &&
146 "You cannot constrain final velocity if final position is not "
147 "constrained.");
148
1/2
✓ Branch 2 taken 34 times.
✗ Branch 3 not taken.
 34 wps.push_back(pi[i]);
149 34 i++;
150 }
151
2/2
✓ Branch 1 taken 41 times.
✓ Branch 2 taken 3 times.
 44 if (flag & END_POS) {
152
1/2
✓ Branch 2 taken 41 times.
✗ Branch 3 not taken.
 41 wps.push_back(pi[i]);
153 41 i++;
154 }
155 }
156
1/2
✓ Branch 3 taken 49 times.
✗ Branch 4 not taken.
 98 return Bezier(wps.begin(), wps.end(), 0., T);
157 49 }
158
159 #endif
160