GCC Code Coverage Report

Directory: ./
File: include/hpp/bezier-com-traj/data.hh
Date: 2025-03-18 04:20:50
Exec Total Coverage
Lines: 45 45 100.0%
Branches: 41 82 50.0%
Line Branch Exec Source
1 /*
2 * Copyright 2018, LAAS-CNRS
3 * Author: Steve Tonneau
4 */
5
6 #ifndef BEZIER_COM_TRAJ_LIB_DATA_H
7 #define BEZIER_COM_TRAJ_LIB_DATA_H
8
9 #include <ndcurves/bezier_curve.h>
10
11 #include <Eigen/Dense>
12 #include <hpp/bezier-com-traj/definitions.hh>
13 #include <hpp/bezier-com-traj/flags.hh>
14 #include <hpp/bezier-com-traj/local_config.hh>
15 #include <hpp/bezier-com-traj/solver/solver-abstract.hpp>
16 #include <hpp/bezier-com-traj/utils.hh>
17 #include <hpp/centroidal-dynamics/centroidal_dynamics.hh>
18 #include <vector>
19
20 namespace bezier_com_traj {
21 /**
22 * @brief Contact data contains all the contact information
23 * relative to a contact phase: contact points and normals
24 * (within Equilibrium object), as well as any additional
25 * kinematic and angular constraints.
26 */
27 struct BEZIER_COM_TRAJ_DLLAPI ContactData {
28 116 ContactData()
29 116 : contactPhase_(0),
30
1/2
✓ Branch 2 taken 116 times.
✗ Branch 3 not taken.
 116 Kin_(Eigen::Matrix3d::Zero()),
31
2/4
✓ Branch 1 taken 116 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 116 times.
✗ Branch 5 not taken.
 116 kin_(VectorX::Zero(0)),
32
2/4
✓ Branch 1 taken 116 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 116 times.
✗ Branch 5 not taken.
 116 Ang_(Eigen::Matrix3d::Zero()),
33
2/4
✓ Branch 1 taken 116 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 116 times.
✗ Branch 5 not taken.
 116 ang_(VectorX::Zero(0)) {}
34
35 209 ContactData(const ContactData& other)
36 418 : contactPhase_(
37
1/2
✓ Branch 2 taken 209 times.
✗ Branch 3 not taken.
 209 new centroidal_dynamics::Equilibrium(*(other.contactPhase_))),
38 209 Kin_(other.Kin_),
39
1/2
✓ Branch 1 taken 209 times.
✗ Branch 2 not taken.
 209 kin_(other.kin_),
40
1/2
✓ Branch 1 taken 209 times.
✗ Branch 2 not taken.
 209 Ang_(other.Ang_),
41
1/2
✓ Branch 1 taken 209 times.
✗ Branch 2 not taken.
 209 ang_(other.ang_) {}
42
43 18 ContactData(centroidal_dynamics::Equilibrium* contactPhase)
44 18 : contactPhase_(contactPhase),
45
1/2
✓ Branch 2 taken 18 times.
✗ Branch 3 not taken.
 18 Kin_(Eigen::Matrix3d::Zero()),
46
2/4
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18 times.
✗ Branch 5 not taken.
 18 kin_(VectorX::Zero(0)),
47
2/4
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18 times.
✗ Branch 5 not taken.
 18 Ang_(Eigen::Matrix3d::Zero()),
48
2/4
✓ Branch 1 taken 18 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18 times.
✗ Branch 5 not taken.
 18 ang_(VectorX::Zero(0)) {}
49 343 ~ContactData() {}
50
51 centroidal_dynamics::Equilibrium* contactPhase_;
52 MatrixX3 Kin_; // inequality kinematic constraints
53 VectorX kin_;
54 MatrixX3 Ang_; // inequality angular momentum constraints
55 VectorX ang_;
56 };
57
58 /**
59 * @brief Used to define the constraints on the trajectory generation problem.
60 * Flags are used to constrain initial and terminal com positions an
61 * derivatives. Additionally, the maximum acceleration can be bounded.
62 */
63 struct BEZIER_COM_TRAJ_DLLAPI Constraints {
64 41 Constraints()
65 41 : flag_(INIT_POS | INIT_VEL | END_VEL | END_POS),
66 41 constraintAcceleration_(false),
67 41 maxAcceleration_(10.),
68 41 reduce_h_(1e-3) {}
69
70 Constraints(ConstraintFlag flag)
71 : flag_(flag),
72 constraintAcceleration_(false),
73 maxAcceleration_(10.),
74 reduce_h_(1e-3) {}
75
76 41 ~Constraints() {}
77
78 ConstraintFlag flag_;
79 bool constraintAcceleration_;
80 double maxAcceleration_;
81 double reduce_h_;
82 };
83
84 /**
85 * @brief Defines all the inputs of the problem:
86 * Initial and terminal constraints, as well as selected
87 * cost functions. Also,a list of ContactData defines the
88 * different phases of the problem. While the method
89 * can handle any phase greater than one, using more
90 * than three phases is probably too constraining.
91 */
92 struct BEZIER_COM_TRAJ_DLLAPI ProblemData {
93 40 ProblemData()
94
2/4
✓ Branch 1 taken 40 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 40 times.
✗ Branch 5 not taken.
 40 : c0_(point_t::Zero()),
95
2/4
✓ Branch 1 taken 40 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 40 times.
✗ Branch 5 not taken.
 40 dc0_(point_t::Zero()),
96
2/4
✓ Branch 1 taken 40 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 40 times.
✗ Branch 5 not taken.
 40 ddc0_(point_t::Zero()),
97
2/4
✓ Branch 1 taken 40 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 40 times.
✗ Branch 5 not taken.
 40 j0_(point_t::Zero()),
98
2/4
✓ Branch 1 taken 40 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 40 times.
✗ Branch 5 not taken.
 40 c1_(point_t::Zero()),
99
2/4
✓ Branch 1 taken 40 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 40 times.
✗ Branch 5 not taken.
 40 dc1_(point_t::Zero()),
100
2/4
✓ Branch 1 taken 40 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 40 times.
✗ Branch 5 not taken.
 40 ddc1_(point_t::Zero()),
101
2/4
✓ Branch 1 taken 40 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 40 times.
✗ Branch 5 not taken.
 40 j1_(point_t::Zero()),
102 40 useAngularMomentum_(false),
103 40 costFunction_(ACCELERATION),
104
1/2
✓ Branch 2 taken 40 times.
✗ Branch 3 not taken.
 80 representation_(DOUBLE_DESCRIPTION) {}
105
106 std::vector<ContactData> contacts_;
107 point_t c0_, dc0_, ddc0_, j0_, c1_, dc1_, ddc1_, j1_;
108 point_t l0_;
109 bool useAngularMomentum_;
110 Constraints constraints_;
111 CostFunction costFunction_;
112 GIWCRepresentation representation_;
113 };
114
115 typedef solvers::ResultData ResultData;
116
117 /**
118 * @brief Specialized ResultData that computes the Bezier curves
119 * corresponding to the computed trajectory
120 */
121 struct BEZIER_COM_TRAJ_DLLAPI ResultDataCOMTraj : public ResultData {
122 113 ResultDataCOMTraj()
123 113 : ResultData(),
124
1/2
✓ Branch 1 taken 113 times.
✗ Branch 2 not taken.
 113 c_of_t_(bezier_t::zero(3)),
125
1/2
✓ Branch 1 taken 113 times.
✗ Branch 2 not taken.
 113 dL_of_t_(bezier_t::zero(3)),
126
2/4
✓ Branch 1 taken 113 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 113 times.
✗ Branch 5 not taken.
 113 dc1_(point_t::Zero()),
127
2/4
✓ Branch 2 taken 113 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 113 times.
✗ Branch 6 not taken.
 226 ddc1_(point_t::Zero()) {}
128 116 ~ResultDataCOMTraj() {}
129
130 bezier_t c_of_t_; // center of mass trajectory
131 bezier_t dL_of_t_; // angular momentum derivative trajectory
132 point_t dc1_; // terminal velocity
133 point_t ddc1_; // terminal acceleration
134 };
135 } // end namespace bezier_com_traj
136
137 #endif
138