Directory: | ./ |
---|---|
File: | src/solve_0_step.cpp |
Date: | 2025-03-18 04:20:50 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 106 | 153 | 69.3% |
Branches: | 172 | 482 | 35.7% |
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 |
3/6✓ 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.
|
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 |
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 | 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 |
3/6✓ 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.
|
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 |
3/6✓ 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.
|
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 |
3/6✓ 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.
|
1 | w.first.block<3, 3>(0, 0) = -3 * alpha * Matrix3::Identity(); |
50 |
6/12✓ 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.
✓ Branch 17 taken 1 times.
✗ Branch 18 not taken.
|
1 | w.first.block<3, 3>(3, 0) = skew(g - 1.5 * alpha * (p1 + p0)); |
51 |
3/6✓ 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.
|
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 |
3/6✓ 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.
|
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 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 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 |
1/2✓ Branch 3 taken 1 times.
✗ Branch 4 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 |