GCC Code Coverage Report

Directory: ./
File: src/solve_0_step.cpp
Date: 2025-05-18 04:15:09
Exec Total Coverage
Lines: 106 158 67.1%
Functions: 13 19 68.4%
Branches: 182 508 35.8%
Line Branch Exec Source
1 /*
2 * Copyright 2017, LAAS-CNRS
3 * Author: Steve Tonneau
4 */
5
6 #include <hpp/bezier-com-traj/common_solve_methods.hh>
7 #include <hpp/bezier-com-traj/solve.hh>
8 #include <hpp/bezier-com-traj/utils.hh>
9
10 using namespace bezier_com_traj;
11 namespace bezier_com_traj {
12 1 waypoint6_t w0(point_t_tC p0, point_t_tC p1, point_t_tC g, const Matrix3& p0X,
13 const Matrix3& /*p1X*/, const Matrix3& /*gX*/,
14 const double alpha) {
15 1 waypoint6_t w = initwp<waypoint6_t>();
16
4/8
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 1 w.first.block<3, 3>(0, 0) = 6 * alpha * Matrix3::Identity();
17
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 1 w.first.block<3, 3>(3, 0) = 6 * alpha * p0X;
18
5/10
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
 1 w.second.head(3) = 6 * alpha * (p0 - 2 * p1);
19
6/12
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
 1 w.second.tail(3) = (-p0).cross(12 * alpha * p1 + g);
20 1 return w;
21 }
22
23 1 waypoint6_t w1(point_t_tC p0, point_t_tC p1, point_t_tC /*g*/,
24 const Matrix3& /*p0X*/, const Matrix3& /*p1X*/,
25 const Matrix3& gX, const double alpha) {
26 1 waypoint6_t w = initwp<waypoint6_t>();
27
4/8
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 1 w.first.block<3, 3>(0, 0) = 3 * alpha * Matrix3::Identity();
28
8/16
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
 1 w.first.block<3, 3>(3, 0) = skew(1.5 * (3 * p1 - p0)) * alpha;
29
6/12
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
 1 w.second.head(3) = 1.5 * alpha * (3 * p0 - 5 * p1);
30
10/20
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 1 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 1 times.
✗ Branch 29 not taken.
 1 w.second.tail(3) = (3 * alpha * p0).cross(-p1) + 0.25 * (gX * (3 * p1 + p0));
31 1 return w;
32 }
33
34 1 waypoint6_t w2(point_t_tC p0, point_t_tC p1, point_t_tC g,
35 const Matrix3& /*p0X*/, const Matrix3& /*p1X*/,
36 const Matrix3& gX, const double alpha) {
37 1 waypoint6_t w = initwp<waypoint6_t>();
38 // w.first.block<3,3>(0,0) = 0;
39
9/18
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 1 times.
✗ Branch 26 not taken.
 1 w.first.block<3, 3>(3, 0) = skew(0.5 * g - 3 * alpha * p0 + 3 * alpha * p1);
40
4/8
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 1 w.second.head(3) = 3 * alpha * (p0 - p1);
41
4/8
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 1 w.second.tail(3) = 0.5 * gX * p1;
42 1 return w;
43 }
44
45 1 waypoint6_t w3(point_t_tC p0, point_t_tC p1, point_t_tC g,
46 const Matrix3& /*p0X*/, const Matrix3& /*p1X*/,
47 const Matrix3& /*gX*/, const double alpha) {
48 1 waypoint6_t w = initwp<waypoint6_t>();
49
4/8
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 1 w.first.block<3, 3>(0, 0) = -3 * alpha * Matrix3::Identity();
50
7/14
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
 1 w.first.block<3, 3>(3, 0) = skew(g - 1.5 * alpha * (p1 + p0));
51
4/8
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 1 w.second.head(3) = 1.5 * alpha * (p1 + p0);
52 // w.second.tail(3) = 0;
53 1 return w;
54 }
55
56 1 waypoint6_t w4(point_t_tC /*p0*/, point_t_tC p1, point_t_tC g,
57 const Matrix3& /*p0X*/, const Matrix3& /*p1X*/,
58 const Matrix3& /*gX*/, const double alpha) {
59 1 waypoint6_t w = initwp<waypoint6_t>();
60
4/8
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 1 w.first.block<3, 3>(0, 0) = -6 * alpha * Matrix3::Identity();
61
6/12
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
 1 w.first.block<3, 3>(3, 0) = skew(g - 6 * alpha * p1);
62
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 1 w.second.head(3) = 6 * alpha * p1;
63 // w.second.tail(3) = 0;
64 1 return w;
65 }
66
67 ✗ waypoint6_t u0(point_t_tC l0, const double alpha) {
68 ✗ waypoint6_t w = initwp<waypoint6_t>();
69 // w.first.block<3,3>(0,0) = 0;
70 ✗ w.first.block<3, 3>(3, 0) = 3 * alpha * Matrix3::Identity();
71 // w.second.head(3) = 0;
72 ✗ w.second.tail(3) = -3 * alpha * l0;
73 ✗ return w;
74 }
75
76 ✗ waypoint6_t u1(point_t_tC l0, const double alpha) {
77 ✗ waypoint6_t w = initwp<waypoint6_t>();
78 // w.first.block<3,3>(0,0) = 0;
79 // w.first.block<3,3>(3,0) = 0;
80 // w.second.head(3) = 0;
81 ✗ w.second.tail(3) = -1.5 * alpha * l0;
82 ✗ return w;
83 }
84
85 ✗ waypoint6_t u2(point_t_tC l0, const double alpha) {
86 ✗ waypoint6_t w = initwp<waypoint6_t>();
87 // w.first.block<3,3>(0,0) = 0;
88 ✗ w.first.block<3, 3>(3, 0) = -1.5 * alpha * Matrix3::Identity();
89 // w.second.head(3) = 0;
90 ✗ w.second.tail(3) = -l0 / 2. * alpha;
91 ✗ return w;
92 }
93
94 ✗ waypoint6_t u3(point_t_tC /*l0*/, const double alpha) {
95 ✗ waypoint6_t w = initwp<waypoint6_t>();
96 ✗ w.first.block<3, 3>(3, 0) = -1.5 * alpha * Matrix3::Identity();
97 // w.second.head(3) = 0;
98 // w.second.tail(3) = 0.;
99 ✗ return w;
100 }
101
102 ✗ waypoint6_t u4(point_t_tC /*l0*/, const double /*alpha*/) {
103 ✗ waypoint6_t w = initwp<waypoint6_t>();
104 // w.first.block<3,3>(0,0) = 0;
105 // w.first.block<3,3>(3,0) = 0;
106 // w.second.head(3) = 0;
107 // w.second.tail(3) = 0.;
108 ✗ return w;
109 }
110
111 1 int computeNumSteps(const double T, const double timeStep) {
112
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 return timeStep > 0. ? int(T / timeStep) : -1;
113 }
114
115 1 std::vector<waypoint6_t> ComputeAllWaypoints(point_t_tC p0, point_t_tC dc0,
116 point_t_tC g, const double T,
117 const double timeStep) {
118
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 int numSteps = computeNumSteps(T, timeStep);
119 static const double n = 3.; // degree
120
4/8
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 1 point_t p1 = dc0 * T / n + p0;
121
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 Matrix3 p0X = skew(p0);
122
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 Matrix3 p1X = skew(p1);
123
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 Matrix3 gX = skew(g);
124 1 double alpha = 1. / (T * T);
125 1 std::vector<waypoint6_t> wps;
126
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 1 wps.push_back(w0(p0, p1, g, p0X, p1X, gX, alpha));
127
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 1 wps.push_back(w1(p0, p1, g, p0X, p1X, gX, alpha));
128
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 1 wps.push_back(w2(p0, p1, g, p0X, p1X, gX, alpha));
129
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 1 wps.push_back(w3(p0, p1, g, p0X, p1X, gX, alpha));
130
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 1 wps.push_back(w4(p0, p1, g, p0X, p1X, gX, alpha));
131
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 if (numSteps > 0) {
132 ✗ std::vector<ndcurves::Bern<double> > berns = ComputeBersteinPolynoms(4);
133 ✗ wps = ComputeDiscretizedWaypoints(wps, berns, numSteps);
134 ✗ }
135 2 return wps;
136 ✗ }
137
138 ✗ std::vector<waypoint6_t> ComputeAllWaypointsAngularMomentum(
139 point_t_tC l0, const double T, const double timeStep) {
140 ✗ int numSteps = computeNumSteps(T, timeStep);
141 ✗ double alpha = 1. / (T);
142 ✗ std::vector<waypoint6_t> wps;
143 ✗ wps.push_back(u0(l0, alpha));
144 ✗ wps.push_back(u1(l0, alpha));
145 ✗ wps.push_back(u2(l0, alpha));
146 ✗ wps.push_back(u3(l0, alpha));
147 ✗ wps.push_back(u4(l0, alpha));
148 ✗ if (numSteps > 0) {
149 ✗ std::vector<ndcurves::Bern<double> > berns = ComputeBersteinPolynoms(4);
150 ✗ wps = ComputeDiscretizedWaypoints(wps, berns, numSteps);
151 ✗ }
152 ✗ return wps;
153 ✗ }
154
155 /* compute the inequality methods that determine the 6D bezier curve w(t)
156 as a function of a variable waypoint for the 3D COM trajectory.
157 The initial curve is of degree 3 (init pos and velocity, 0 velocity constraints
158 + one free variable). The 6d curve is of degree 2*n-2 = 4, thus 5 control points
159 are to be computed. Each control point produces a 6 * 3 inequality matrix wix,
160 and a 6 *1 column right member wsi. Premultiplying it by H gives mH w_xi * x <=
161 mH_wsi where m is the mass Stacking all of these results in a big inequality
162 matrix A and a column vector x that determines the constraints On the 6d curves,
163 Ain x <= Aub
164 */
165 1 std::pair<MatrixXX, VectorX> compute6dControlPointInequalities(
166 const ContactData& cData, point_t_tC c0, point_t_tC dc0, point_t_tC l0,
167 const bool useAngMomentum, const double T, const double timeStep,
168 bool& fail) {
169 1 std::vector<waypoint6_t> wps, wpL;
170 wps =
171
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 ComputeAllWaypoints(c0, dc0, cData.contactPhase_->m_gravity, T, timeStep);
172
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
 1 if (useAngMomentum) wpL = ComputeAllWaypointsAngularMomentum(l0, T, timeStep);
173 return compute6dControlPointInequalities(cData, wps, wpL, useAngMomentum,
174
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 fail);
175 1 }
176
177 1 std::pair<MatrixXX, VectorX> computeCostFunction(point_t_tC p0, point_t_tC l0,
178 const bool useAngMomentum) {
179
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 int dimPb = useAngMomentum ? 6 : 3;
180
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 std::pair<MatrixXX, VectorX> res;
181
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 res.first = MatrixXX::Zero(dimPb, dimPb);
182
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 res.second = VectorX::Zero(dimPb);
183
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 Ref_matrixXX H = res.first;
184
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 Ref_vectorX g = res.second;
185
186 // minimize distance to initial point
187
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 double weightDist = useAngMomentum ? 0.0001 : 1.;
188
4/8
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 1 H.block<3, 3>(0, 0) = Matrix3::Identity() * weightDist;
189
4/8
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 1 g.head(3) = -p0 * weightDist;
190
191 // now angular momentum integral minimization
192
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 if (useAngMomentum) {
193 ✗ H.block<3, 3>(3, 3) = Matrix3::Identity() * 6. / 5.;
194 ✗ g.tail(3) = 0.5 * (-(9. * l0) / 5.);
195 }
196 2 return res;
197 ✗ }
198
199 1 void computeRealCost(const ProblemData& pData, ResultData& resData) {
200
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 if (pData.useAngularMomentum_) {
201 ✗ Vector3 xL = resData.x.tail(3);
202 ✗ resData.cost_ = (1. / 5.) * (9. * pData.l0_.dot(pData.l0_) -
203 ✗ 9. * pData.l0_.dot(xL) + 6. * xL.dot(xL));
204 } else
205
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 resData.cost_ = (pData.c0_ - resData.x).norm();
206 1 }
207
208 1 void computeC_of_T(const ProblemData& pData, const std::vector<double>& Ts,
209 ResultDataCOMTraj& res) {
210 1 std::vector<Vector3> wps;
211
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 wps.push_back(pData.c0_);
212
5/10
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 1 times.
✗ Branch 15 not taken.
 1 wps.push_back(pData.dc0_ * Ts[0] / 3 + pData.c0_);
213
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 1 wps.push_back(res.x.head(3));
214
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 1 wps.push_back(res.x.head(3));
215
2/4
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 1 res.c_of_t_ = bezier_t(wps.begin(), wps.end(), 0., Ts[0]);
216 1 }
217
218 1 void computedL_of_T(const ProblemData& pData, const std::vector<double>& Ts,
219 ResultDataCOMTraj& res) {
220
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 if (pData.useAngularMomentum_) {
221 ✗ std::vector<Vector3> wps;
222 ✗ wps.push_back(3 * (res.x.tail(3) - pData.l0_));
223 ✗ wps.push_back(3 * (-res.x.tail(3)));
224 ✗ wps.push_back(Vector3::Zero());
225 ✗ res.dL_of_t_ = bezier_t(wps.begin(), wps.end(), 0., Ts[0], 1. / Ts[0]);
226 ✗ } else
227
2/4
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
 1 res.dL_of_t_ = bezier_t::zero(3, Ts[0]);
228 1 }
229
230 // no angular momentum for now
231 1 ResultDataCOMTraj solve0step(const ProblemData& pData,
232 const std::vector<double>& Ts,
233 const double timeStep) {
234
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 assert(pData.representation_ == DOUBLE_DESCRIPTION);
235
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1 times.
 1 assert(pData.contacts_.size() == 1);
236
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 1 times.
 1 assert(Ts.size() == pData.contacts_.size());
237 1 bool fail = true;
238 std::pair<MatrixXX, VectorX> Ab = compute6dControlPointInequalities(
239 1 pData.contacts_.front(), pData.c0_, pData.dc0_, pData.l0_,
240
4/8
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 1 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 1 times.
✗ Branch 12 not taken.
 2 pData.useAngularMomentum_, Ts.front(), timeStep, fail);
241
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 ResultDataCOMTraj res;
242
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 if (fail) return res;
243 std::pair<MatrixXX, VectorX> Hg =
244
3/6
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
 1 computeCostFunction(pData.c0_, pData.l0_, pData.useAngularMomentum_);
245
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
 1 int dimPb = pData.useAngularMomentum_ ? 6 : 3;
246
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 VectorX init = VectorX(dimPb);
247
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 init.head(3) = pData.c0_;
248
1/6
✗ Branch 0 not taken.
✓ Branch 1 taken 1 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
 1 if (dimPb > 3) init.tail(3) = pData.l0_;
249 // rewriting 0.5 || Dx -d ||^2 as x'Hx + g'x
250
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 ResultData resQp = solve(Ab, Hg, init);
251
1/2
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
 1 if (resQp.success_) {
252 1 res.success_ = true;
253
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 res.x = resQp.x;
254
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 computeRealCost(pData, res);
255
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 computeC_of_T(pData, Ts, res);
256
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 computedL_of_T(pData, Ts, res);
257 }
258 1 return res;
259 1 }
260
261 } // namespace bezier_com_traj
262