Directory: | ./ |
---|---|
File: | python/bezier_com_traj.cpp |
Date: | 2025-03-18 04:20:50 |
Exec | Total | Coverage | |
---|---|---|---|
Lines: | 190 | 276 | 68.8% |
Branches: | 106 | 286 | 37.1% |
Line | Branch | Exec | Source |
---|---|---|---|
1 | #include <boost/python.hpp> | ||
2 | #include <boost/python/exception_translator.hpp> | ||
3 | #include <eigenpy/eigenpy.hpp> | ||
4 | #include <eigenpy/memory.hpp> | ||
5 | #include <hpp/bezier-com-traj/solve.hh> | ||
6 | #include <hpp/bezier-com-traj/solve_end_effector.hh> | ||
7 | #include <hpp/bezier-com-traj/solver/solver-abstract.hpp> | ||
8 | |||
9 | EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(bezier_com_traj::ContactData) | ||
10 | EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(bezier_com_traj::ProblemData) | ||
11 | EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(bezier_com_traj::ResultData) | ||
12 | EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION( | ||
13 | bezier_com_traj::ResultDataCOMTraj) | ||
14 | EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(bezier_com_traj::bezier_t) | ||
15 | |||
16 | namespace bezier_com_traj { | ||
17 | using namespace boost::python; | ||
18 | typedef double real; | ||
19 | |||
20 | 1 | ResultDataCOMTraj* zeroStepCapturability(centroidal_dynamics::Equilibrium* eq, | |
21 | const Vector3& com, | ||
22 | const Vector3& dCom, const Vector3& l0, | ||
23 | const bool useAngMomentum, | ||
24 | const double timeDuration, | ||
25 | const double timeStep) { | ||
26 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | bezier_com_traj::ContactData data; |
27 | 1 | data.contactPhase_ = eq; | |
28 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | bezier_com_traj::ProblemData pData; |
29 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | pData.c0_ = com; |
30 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | pData.dc0_ = dCom; |
31 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | pData.l0_ = l0; |
32 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | pData.contacts_.push_back(data); |
33 | 1 | pData.useAngularMomentum_ = useAngMomentum; | |
34 | 1 | std::vector<double> Ts; | |
35 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | Ts.push_back(timeDuration); |
36 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | ResultDataCOMTraj res = solve0step(pData, Ts, timeStep); |
37 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | return new ResultDataCOMTraj(res); |
38 | 1 | } | |
39 | |||
40 | ✗ | ResultDataCOMTraj* zeroStepCapturabilityWithKinConstraints( | |
41 | centroidal_dynamics::Equilibrium* eq, const Vector3& com, | ||
42 | const Vector3& dCom, const Vector3& l0, const bool useAngMomentum, | ||
43 | const double timeDuration, const double timeStep, const MatrixXX& Kin, | ||
44 | const MatrixXX& kin) { | ||
45 | ✗ | bezier_com_traj::ContactData data; | |
46 | ✗ | data.Kin_ = Kin; | |
47 | ✗ | data.kin_ = kin; | |
48 | ✗ | data.contactPhase_ = eq; | |
49 | ✗ | bezier_com_traj::ProblemData pData; | |
50 | ✗ | pData.c0_ = com; | |
51 | ✗ | pData.dc0_ = dCom; | |
52 | ✗ | pData.l0_ = l0; | |
53 | ✗ | pData.contacts_.push_back(data); | |
54 | ✗ | pData.useAngularMomentum_ = useAngMomentum; | |
55 | ✗ | std::vector<double> Ts; | |
56 | ✗ | Ts.push_back(timeDuration); | |
57 | ✗ | ResultDataCOMTraj res = solve0step(pData, Ts, timeStep); | |
58 | ✗ | return new ResultDataCOMTraj(res); | |
59 | } | ||
60 | |||
61 | struct res_data_exception : std::exception { | ||
62 | ✗ | char const* what() const throw() { | |
63 | ✗ | return "attributes not accessible for false resData"; | |
64 | } | ||
65 | }; | ||
66 | |||
67 | ✗ | void translate(const res_data_exception& e) { | |
68 | // Use the Python 'C' API to set up an exception object | ||
69 | ✗ | PyErr_SetString(PyExc_RuntimeError, e.what()); | |
70 | } | ||
71 | |||
72 | struct contact_data_exception : std::exception { | ||
73 | 4 | char const* what() const throw() { | |
74 | 4 | return "attribute not defined yet for ContactData"; | |
75 | } | ||
76 | }; | ||
77 | |||
78 | 4 | void translateContactData(const contact_data_exception& e) { | |
79 | // Use the Python 'C' API to set up an exception object | ||
80 | 4 | PyErr_SetString(PyExc_RuntimeError, e.what()); | |
81 | 4 | } | |
82 | |||
83 | ✗ | VectorX get_xD(const ResultData& res) { | |
84 | ✗ | if (res.x.size() > 0) return res.x; | |
85 | ✗ | std::cout << "x is not defined" << std::endl; | |
86 | ✗ | throw res_data_exception(); | |
87 | } | ||
88 | |||
89 | ✗ | double get_costD(const ResultData& res) { return res.cost_; } | |
90 | |||
91 | ✗ | bool get_succD(const ResultData& res) { return res.success_; } | |
92 | |||
93 | 2 | bezier_t* getC_of_t(const ResultDataCOMTraj& res) { | |
94 |
1/2✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
|
2 | return new bezier_t(res.c_of_t_); |
95 | } | ||
96 | |||
97 | 1 | bezier_t* getDL_of_t(const ResultDataCOMTraj& res) { | |
98 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | return new bezier_t(res.dL_of_t_); |
99 | } | ||
100 | |||
101 | ✗ | VectorX get_x(const ResultDataCOMTraj& res) { | |
102 | ✗ | if (res.x.size() > 0) return res.x; | |
103 | ✗ | std::cout << "x is not defined" << std::endl; | |
104 | ✗ | throw res_data_exception(); | |
105 | } | ||
106 | |||
107 | ✗ | double get_cost(const ResultDataCOMTraj& res) { return res.cost_; } | |
108 | |||
109 | 1 | bool get_succ(const ResultDataCOMTraj& res) { return res.success_; } | |
110 | |||
111 | /** BEGIN CONTACT DATA **/ | ||
112 | 5 | centroidal_dynamics::Equilibrium* getContactPhase_(const ContactData& data) { | |
113 |
1/2✓ Branch 0 taken 5 times.
✗ Branch 1 not taken.
|
5 | if (data.contactPhase_) return data.contactPhase_; |
114 | ✗ | std::cout << "contactPhase_ is not assigned" << std::endl; | |
115 | ✗ | throw contact_data_exception(); | |
116 | } | ||
117 | |||
118 | ✗ | void setContactPhase_(ContactData& data, centroidal_dynamics::Equilibrium* eq) { | |
119 | ✗ | data.contactPhase_ = eq; | |
120 | } | ||
121 | |||
122 | 2 | boost::python::tuple get_Ang(const ContactData& res) { | |
123 |
2/2✓ Branch 1 taken 1 times.
✓ Branch 2 taken 1 times.
|
2 | if (res.ang_.size() == 0) { |
124 | 1 | std::cout << " no angular momentum constraints assigned " << std::endl; | |
125 | 1 | throw contact_data_exception(); | |
126 | } | ||
127 | 1 | return boost::python::make_tuple(res.Ang_, res.ang_); | |
128 | } | ||
129 | |||
130 | 2 | boost::python::tuple get_Kin(const ContactData& res) { | |
131 |
2/2✓ Branch 1 taken 1 times.
✓ Branch 2 taken 1 times.
|
2 | if (res.kin_.size() == 0) { |
132 | 1 | std::cout << " no kinematic constraints assigned " << std::endl; | |
133 | 1 | throw contact_data_exception(); | |
134 | } | ||
135 | 1 | return boost::python::make_tuple(res.Kin_, res.kin_); | |
136 | } | ||
137 | |||
138 | 2 | void set_Kin(ContactData& res, const MatrixX3& val, const VectorX& val2) { | |
139 |
2/2✓ Branch 2 taken 1 times.
✓ Branch 3 taken 1 times.
|
2 | if (val2.size() != val.rows()) { |
140 | 1 | std::cout << " Kinematic inequality matrix sizes do not match " | |
141 | 1 | << std::endl; | |
142 | 1 | throw contact_data_exception(); | |
143 | } | ||
144 | 1 | res.Kin_ = val; | |
145 | 1 | res.kin_ = val2; | |
146 | 1 | } | |
147 | |||
148 | 2 | void set_Ang(ContactData& res, const MatrixX3& val, const VectorX& val2) { | |
149 |
2/2✓ Branch 2 taken 1 times.
✓ Branch 3 taken 1 times.
|
2 | if (val2.size() != val.rows()) { |
150 | 1 | std::cout << " Angular inequality matrix sizes do not match " << std::endl; | |
151 | 1 | throw contact_data_exception(); | |
152 | } | ||
153 | 1 | res.Ang_ = val; | |
154 | 1 | res.ang_ = val2; | |
155 | 1 | } | |
156 | |||
157 | /** END CONTACT DATA **/ | ||
158 | |||
159 | /** BEGIN Constraints**/ | ||
160 | 5 | int get_Flag(const Constraints& res) { return (int)res.flag_; } | |
161 | 2 | bool get_ConstrainAcc(const Constraints& res) { | |
162 | 2 | return res.constraintAcceleration_; | |
163 | } | ||
164 | 2 | double get_MaxAcc(const Constraints& res) { return res.maxAcceleration_; } | |
165 | 2 | double get_ReduceH(const Constraints& res) { return res.reduce_h_; } | |
166 | |||
167 | 3 | void set_Flag(Constraints& res, const int val) { | |
168 | 3 | res.flag_ = (ConstraintFlag)val; | |
169 | 3 | } | |
170 | 1 | void set_ConstrainAcc(Constraints& res, const bool val) { | |
171 | 1 | res.constraintAcceleration_ = val; | |
172 | 1 | } | |
173 | |||
174 | 1 | void set_MaxAcc(Constraints& res, const double val) { | |
175 | 1 | res.maxAcceleration_ = val; | |
176 | 1 | } | |
177 | |||
178 | 1 | void set_ReduceH(Constraints& res, const double val) { res.reduce_h_ = val; } | |
179 | |||
180 | /** END Constraints **/ | ||
181 | |||
182 | /** BEGIN ProblemData**/ | ||
183 | 3 | point_t get_c0_(const ProblemData& res) { return res.c0_; } | |
184 | 2 | point_t get_dc0_(const ProblemData& res) { return res.dc0_; } | |
185 | |||
186 | 2 | point_t get_ddc0_(const ProblemData& res) { return res.ddc0_; } | |
187 | |||
188 | 1 | point_t get_c1_(const ProblemData& res) { return res.c1_; } | |
189 | |||
190 | 2 | point_t get_dc1_(const ProblemData& res) { return res.dc1_; } | |
191 | |||
192 | 2 | point_t get_ddc1_(const ProblemData& res) { return res.ddc1_; } | |
193 | |||
194 | 2 | void set_c0_(ProblemData& res, const point_t& val) { res.c0_ = val; } | |
195 | |||
196 | 2 | void set_dc0_(ProblemData& res, const point_t& val) { res.dc0_ = val; } | |
197 | |||
198 | 1 | void set_ddc0_(ProblemData& res, const point_t& val) { res.ddc0_ = val; } | |
199 | |||
200 | 1 | void set_c1_(ProblemData& res, const point_t& val) { res.c1_ = val; } | |
201 | |||
202 | 1 | void set_dc1_(ProblemData& res, const point_t& val) { res.dc1_ = val; } | |
203 | |||
204 | 1 | void set_ddc1_(ProblemData& res, const point_t& val) { res.ddc1_ = val; } | |
205 | |||
206 | 2 | bool get_useAngularMomentum_(const ProblemData& res) { | |
207 | 2 | return res.useAngularMomentum_; | |
208 | } | ||
209 | 1 | void set_useAngularMomentum_(ProblemData& res, const bool val) { | |
210 | 1 | res.useAngularMomentum_ = val; | |
211 | 1 | } | |
212 | |||
213 | ✗ | CostFunction get_costFunction_(const ProblemData& res) { | |
214 | ✗ | return res.costFunction_; | |
215 | } | ||
216 | |||
217 | ✗ | void set_costFunction_(ProblemData& res, const CostFunction val) { | |
218 | ✗ | res.costFunction_ = val; | |
219 | } | ||
220 | |||
221 | ✗ | GIWCRepresentation get_GIWC_representation_(const ProblemData& res) { | |
222 | ✗ | return res.representation_; | |
223 | } | ||
224 | |||
225 | ✗ | void set_GIWC_representation_(ProblemData& res, const GIWCRepresentation val) { | |
226 | ✗ | res.representation_ = val; | |
227 | } | ||
228 | |||
229 | 3 | Constraints* get_constraints_(ProblemData& res) { return &res.constraints_; } | |
230 | |||
231 | ✗ | void set_constraints_(ProblemData& res, const Constraints& val) { | |
232 | ✗ | res.constraints_ = val; | |
233 | } | ||
234 | |||
235 | ✗ | std::vector<ContactData> get_contacts_(const ProblemData& res) { | |
236 | ✗ | return res.contacts_; | |
237 | } | ||
238 | |||
239 | 3 | void addContact(ProblemData& res, const ContactData& val) { | |
240 |
1/2✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
|
3 | res.contacts_.push_back(ContactData(val)); |
241 | 3 | } | |
242 | |||
243 | ✗ | void clearContacts(ProblemData& res) { res.contacts_.clear(); } | |
244 | |||
245 | /** END ProblemData **/ | ||
246 | |||
247 | /** BEGIN computeCOMTraj **/ | ||
248 | |||
249 | 2 | ResultDataCOMTraj* computeCOMTrajPointer(const ProblemData& pData, | |
250 | const VectorX& Ts, | ||
251 | const double timeStep) { | ||
252 |
2/2✓ Branch 1 taken 1 times.
✓ Branch 2 taken 1 times.
|
2 | ResultDataCOMTraj res = computeCOMTraj(pData, Ts, timeStep); |
253 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | return new ResultDataCOMTraj(res); |
254 | 1 | } | |
255 | |||
256 | 1 | ResultDataCOMTraj* computeCOMTrajPointerChooseSolver( | |
257 | const ProblemData& pData, const VectorX& Ts, const double timeStep, | ||
258 | const solvers::SolverType solver) { | ||
259 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | ResultDataCOMTraj res = computeCOMTraj(pData, Ts, timeStep, solver); |
260 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
2 | return new ResultDataCOMTraj(res); |
261 | 1 | } | |
262 | |||
263 | /** END computeCOMTraj **/ | ||
264 | /** BEGIN end effector **/ | ||
265 | |||
266 | struct DummyPath { | ||
267 | ✗ | point3_t operator()(double /*u*/) const { return point3_t::Zero(); } | |
268 | }; | ||
269 | |||
270 | typedef std::pair<MatrixXX, VectorX> linear_points_t; | ||
271 | typedef Eigen::Matrix<real, 3, Eigen::Dynamic> point_list_t; | ||
272 | |||
273 | struct MatrixVector { | ||
274 | linear_points_t res; | ||
275 | ✗ | MatrixXX A() { return res.first; } | |
276 | ✗ | VectorX b() { return res.second; } | |
277 | }; | ||
278 | |||
279 | ✗ | ResultDataCOMTraj* computeEndEffector(const ProblemData& pData, | |
280 | const double time) { | ||
281 | ResultDataCOMTraj res = | ||
282 | ✗ | solveEndEffector<DummyPath>(pData, DummyPath(), time, 0); | |
283 | ✗ | return new ResultDataCOMTraj(res); | |
284 | } | ||
285 | |||
286 | ✗ | MatrixVector* computeEndEffectorConstraintsPython(const ProblemData& pData, | |
287 | const double time) { | ||
288 | ✗ | std::vector<bezier_t::point_t> pi = computeConstantWaypoints(pData, time); | |
289 | ✗ | MatrixVector* res = new MatrixVector(); | |
290 | ✗ | res->res = computeEndEffectorConstraints(pData, time, pi); | |
291 | ✗ | return res; | |
292 | } | ||
293 | |||
294 | ✗ | MatrixVector* computeEndEffectorVelocityCostPython(const ProblemData& pData, | |
295 | const double time) { | ||
296 | ✗ | std::vector<bezier_t::point_t> pi = computeConstantWaypoints(pData, time); | |
297 | ✗ | MatrixVector* res = new MatrixVector(); | |
298 | ✗ | res->res = computeVelocityCost(pData, time, pi); | |
299 | ✗ | return res; | |
300 | } | ||
301 | |||
302 | ✗ | MatrixVector* computeEndEffectorDistanceCostPython(const ProblemData& pData, | |
303 | const double time, | ||
304 | const int numPoints, | ||
305 | point_list_t pts_l) { | ||
306 | ✗ | std::vector<bezier_t::point_t> pi = computeConstantWaypoints(pData, time); | |
307 | // transform the matrice 3xN in a std::vector<point3_t> of size N : | ||
308 | ✗ | std::vector<point3_t> pts_path; | |
309 | ✗ | for (int c = 0; c < pts_l.cols(); ++c) { | |
310 | ✗ | pts_path.push_back(pts_l.block<3, 1>(0, c)); | |
311 | } | ||
312 | ✗ | MatrixVector* res = new MatrixVector(); | |
313 | ✗ | res->res = computeDistanceCostFunction(numPoints, pData, time, pts_path); | |
314 | ✗ | return res; | |
315 | } | ||
316 | |||
317 | Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic> | ||
318 | ✗ | computeEndEffectorConstantWaypoints(const ProblemData& pData, | |
319 | const double time) { | ||
320 | ✗ | std::vector<bezier_t::point_t> pi = computeConstantWaypoints(pData, time); | |
321 | ✗ | Eigen::Matrix<real, Eigen::Dynamic, Eigen::Dynamic> res(3, pi.size()); | |
322 | ✗ | int col = 0; | |
323 | ✗ | for (std::vector<bezier_t::point_t>::const_iterator cit = pi.begin(); | |
324 | ✗ | cit != pi.end(); ++cit, ++col) | |
325 | ✗ | res.block<3, 1>(0, col) = *cit; | |
326 | ✗ | return res; | |
327 | } | ||
328 | |||
329 | /** END end effector **/ | ||
330 | |||
331 |
2/4✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
✓ Branch 3 taken 1 times.
✗ Branch 4 not taken.
|
4 | BOOST_PYTHON_MODULE(hpp_bezier_com_traj) { |
332 | using namespace boost::python; | ||
333 | 2 | register_exception_translator<res_data_exception>(&translate); | |
334 | 2 | register_exception_translator<contact_data_exception>(&translateContactData); | |
335 | /** BEGIN eigenpy init**/ | ||
336 | 2 | eigenpy::enableEigenPy(); | |
337 | 2 | ENABLE_SPECIFIC_MATRIX_TYPE(point_t); | |
338 | 2 | ENABLE_SPECIFIC_MATRIX_TYPE(Vector3); | |
339 | 2 | ENABLE_SPECIFIC_MATRIX_TYPE(VectorX); | |
340 | 2 | ENABLE_SPECIFIC_MATRIX_TYPE(MatrixX3); | |
341 | 2 | ENABLE_SPECIFIC_MATRIX_TYPE(MatrixXX); | |
342 | |||
343 | /** END eigenpy init**/ | ||
344 | /*eigenpy::exposeAngleAxis(); | ||
345 | eigenpy::exposeQuaternion();*/ | ||
346 | |||
347 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | class_<ResultDataCOMTraj>("ResultDataCOMTraj", init<>()) |
348 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | .add_property( |
349 | "c_of_t", | ||
350 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
4 | make_function(&getC_of_t, return_value_policy<manage_new_object>())) |
351 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | .add_property( |
352 | "dL_of_t", | ||
353 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
4 | make_function(&getDL_of_t, return_value_policy<manage_new_object>())) |
354 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | .add_property("success", &get_succ) |
355 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | .add_property("cost", &get_cost) |
356 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | .add_property("x", &get_x); |
357 | |||
358 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
4 | class_<ResultData>("ResultData", init<>()) |
359 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | .add_property("success", &get_succD) |
360 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | .add_property("cost", &get_costD) |
361 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | .add_property("x", &get_xD); |
362 | |||
363 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | class_<ContactData>( |
364 | "ContactData", | ||
365 | 2 | init< | |
366 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | centroidal_dynamics::Equilibrium*>()[with_custodian_and_ward<1, 2>()]) |
367 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | .add_property( |
368 | "contactPhase_", | ||
369 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
4 | make_function(&getContactPhase_, |
370 | 2 | return_value_policy<reference_existing_object>()), | |
371 | &setContactPhase_) | ||
372 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | .add_property("Kin_", &get_Kin) |
373 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | .add_property("Ang_", &get_Ang) |
374 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | .def("setKinematicConstraints", &set_Kin) |
375 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | .def("setAngularConstraints", &set_Ang); |
376 | |||
377 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
2 | class_<ProblemData>("ProblemData", init<>()) |
378 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | .add_property("c0_", &get_c0_, &set_c0_) |
379 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | .add_property("dc0_", &get_dc0_, &set_dc0_) |
380 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | .add_property("ddc0_", &get_ddc0_, &set_ddc0_) |
381 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | .add_property("c1_", &get_c1_, &set_c1_) |
382 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | .add_property("dc1_", &get_dc1_, &set_dc1_) |
383 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | .add_property("ddc1_", &get_ddc1_, &set_ddc1_) |
384 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | .add_property("useAngularMomentum_", &get_useAngularMomentum_, |
385 | &set_useAngularMomentum_) | ||
386 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | .add_property("costFunction_", &get_costFunction_, &set_costFunction_) |
387 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | .add_property("GIWCrepresentation_", &get_GIWC_representation_, |
388 | &set_GIWC_representation_) | ||
389 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | .add_property( |
390 | "constraints_", | ||
391 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
4 | make_function(&get_constraints_, |
392 | 2 | return_value_policy<reference_existing_object>()), | |
393 | &set_constraints_) | ||
394 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | .def("clearContacts", clearContacts) |
395 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | .def("addContact", addContact); |
396 | |||
397 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
4 | class_<Constraints>("Constraints", init<>()) |
398 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | .add_property("flag_", &get_Flag, &set_Flag) |
399 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | .add_property("constrainAcceleration_", &get_ConstrainAcc, |
400 | &set_ConstrainAcc) | ||
401 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | .add_property("maxAcceleration_", &get_MaxAcc, &set_MaxAcc) |
402 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | .add_property("reduce_h_", &get_ReduceH, &set_ReduceH); |
403 | |||
404 | 4 | enum_<CostFunction>("CostFunction") | |
405 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | .value("ACCELERATION", ACCELERATION) |
406 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | .value("DISTANCE_TRAVELED", DISTANCE_TRAVELED) |
407 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | .value("TARGET_END_VELOCITY", TARGET_END_VELOCITY) |
408 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | .value("UNKNOWN_COST", UNKNOWN_COST) |
409 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | .export_values(); |
410 | |||
411 | 4 | enum_<GIWCRepresentation>("GIWCRepresentation") | |
412 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | .value("DOUBLE_DESCRIPTION", DOUBLE_DESCRIPTION) |
413 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | .value("FORCE", FORCE) |
414 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | .value("UNKNOWN_REPRESENTATION", UNKNOWN_REPRESENTATION) |
415 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | .export_values(); |
416 | |||
417 | 4 | enum_<solvers::SolverType>("SolverType") | |
418 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | .value("SOLVER_QUADPROG", solvers::SOLVER_QUADPROG) |
419 | //.value("SOLVER_QUADPROG_SPARSE", solvers::SOLVER_QUADPROG_SPARSE) | ||
420 | #ifdef USE_GLPK_SOLVER | ||
421 | .value("SOLVER_GLPK", solvers::SOLVER_GLPK) | ||
422 | #endif | ||
423 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | .export_values(); |
424 | |||
425 | 4 | enum_<ConstraintFlag>("ConstraintFlag") | |
426 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | .value("INIT_POS", INIT_POS) |
427 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | .value("INIT_VEL", INIT_VEL) |
428 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | .value("INIT_ACC", INIT_ACC) |
429 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | .value("INIT_JERK", INIT_JERK) |
430 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | .value("END_POS", END_POS) |
431 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | .value("END_VEL", END_VEL) |
432 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | .value("END_ACC", END_ACC) |
433 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | .value("END_JERK", END_JERK) |
434 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | .value("ONE_FREE_VAR", ONE_FREE_VAR) |
435 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | .value("TWO_FREE_VAR", TWO_FREE_VAR) |
436 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | .value("THREE_FREE_VAR", THREE_FREE_VAR) |
437 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | .value("FOUR_FREE_VAR", FOUR_FREE_VAR) |
438 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | .value("FIVE_FREE_VAR", FIVE_FREE_VAR) |
439 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | .value("UNKNOWN", UNKNOWN) |
440 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | .export_values(); |
441 | |||
442 | 2 | class_<MatrixVector>("MatrixVector", no_init) | |
443 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | .def_readonly("A", &MatrixVector::A) |
444 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | .def_readonly("b", &MatrixVector::b); |
445 | |||
446 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | def("zeroStepCapturability", &zeroStepCapturability, |
447 | ✗ | return_value_policy<manage_new_object>()); | |
448 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | def("zeroStepCapturability", &zeroStepCapturabilityWithKinConstraints, |
449 | ✗ | return_value_policy<manage_new_object>()); | |
450 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | def("computeCOMTraj", &computeCOMTrajPointer, |
451 | ✗ | return_value_policy<manage_new_object>()); | |
452 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | def("computeCOMTraj", &computeCOMTrajPointerChooseSolver, |
453 | ✗ | return_value_policy<manage_new_object>()); | |
454 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | def("computeEndEffector", &computeEndEffector, |
455 | ✗ | return_value_policy<manage_new_object>()); | |
456 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | def("computeEndEffectorConstraints", &computeEndEffectorConstraintsPython, |
457 | ✗ | return_value_policy<manage_new_object>()); | |
458 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | def("computeEndEffectorVelocityCost", &computeEndEffectorVelocityCostPython, |
459 | ✗ | return_value_policy<manage_new_object>()); | |
460 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
2 | def("computeEndEffectorDistanceCost", &computeEndEffectorDistanceCostPython, |
461 | 2 | return_value_policy<manage_new_object>()); | |
462 | 2 | def("computeEndEffectorConstantWaypoints", | |
463 | &computeEndEffectorConstantWaypoints); | ||
464 | 2 | } | |
465 | |||
466 | } // namespace bezier_com_traj | ||
467 |