Skip to content

Commit efa18fb

Browse files
EnricoMingoEnricoMingo
committed
still not fixed...
1 parent 70fee32 commit efa18fb

File tree

3 files changed

+116
-175
lines changed

3 files changed

+116
-175
lines changed

‎bindings/python/aggregated.hpp‎

Lines changed: 26 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -7,9 +7,33 @@
77

88
namespace py = pybind11;
99

10+
struct PyAggregatedTaskTrampoline : OpenSoT::tasks::Aggregated {
11+
using OpenSoT::tasks::Aggregated::Aggregated;
12+
13+
void _update() override {
14+
PYBIND11_OVERRIDE_PURE(
15+
void, // Return type
16+
OpenSoT::tasks::Aggregated,// Parent class
17+
_update // Name of function in C++
18+
);
19+
}
20+
};
21+
22+
struct PyAggregatedConstraintTrampoline : OpenSoT::constraints::Aggregated {
23+
using OpenSoT::constraints::Aggregated::Aggregated;
24+
25+
void _update() override {
26+
PYBIND11_OVERRIDE_PURE(
27+
void, // Return type
28+
OpenSoT::constraints::Aggregated,// Parent class
29+
_update // Name of function in C++
30+
);
31+
}
32+
};
33+
1034
void pyAggregatedTask(py::module& m) {
1135
// Aggregated Task
12-
py::class_<OpenSoT::tasks::Aggregated,
36+
py::class_<OpenSoT::tasks::Aggregated, PyAggregatedTaskTrampoline,
1337
std::shared_ptr<OpenSoT::tasks::Aggregated>,
1438
OpenSoT::Task<Eigen::MatrixXd, Eigen::VectorXd>>(m, "AggregatedTask")
1539
// ctor(list<TaskPtr>, x_size) -- keep the list (arg 2) alive while returned Aggregated (1) lives
@@ -45,7 +69,7 @@ void pyAggregatedConstraint(py::module& m) {
4569
.export_values();
4670

4771
// Aggregated Constraint
48-
py::class_<OpenSoT::constraints::Aggregated,
72+
py::class_<OpenSoT::constraints::Aggregated, PyAggregatedConstraintTrampoline,
4973
std::shared_ptr<OpenSoT::constraints::Aggregated>,
5074
OpenSoT::Constraint<Eigen::MatrixXd, Eigen::VectorXd>>(m, "AggregatedConstraint")
5175
// ctor(list<ConstraintPtr>, x_size, aggregationPolicy)

‎bindings/python/autostack.hpp‎

Lines changed: 88 additions & 171 deletions
Original file line numberDiff line numberDiff line change
@@ -1,195 +1,112 @@
1-
// py_autostack_bindings.cpp
1+
// py_autostack.cpp
2+
23
#include <pybind11/pybind11.h>
34
#include <pybind11/eigen.h>
45
#include <pybind11/stl.h>
56
#include <OpenSoT/utils/AutoStack.h>
6-
7-
#include <cxxabi.h>
7+
#include <OpenSoT/tasks/Aggregated.h>
8+
#include <OpenSoT/constraints/Aggregated.h>
89
#include <iostream>
910
#include <typeinfo>
1011
#include <memory>
11-
12-
// helper to demangle type names
13-
static std::string demangle(const char* name) {
14-
int status = 0;
15-
std::unique_ptr<char, void(*)(void*)> res{
16-
abi::__cxa_demangle(name, NULL, NULL, &status),
17-
std::free
18-
};
19-
return (status == 0 && res) ? std::string(res.get()) : std::string(name);
20-
}
12+
#include <cxxabi.h>
2113

2214
namespace py = pybind11;
2315
using namespace OpenSoT;
2416

17+
// helper to demangle type names
18+
// static std::string demangle(const char* name) {
19+
// int status = 0;
20+
// std::unique_ptr<char, void(*)(void*)> res{
21+
// abi::__cxa_demangle(name, nullptr, nullptr, &status),
22+
// std::free
23+
// };
24+
// return (status == 0 && res) ? std::string(res.get()) : std::string(name);
25+
// }
26+
27+
2528
void pyAutostack(py::module& m) {
2629

2730
py::class_<AutoStack, AutoStack::Ptr>(m, "AutoStack")
28-
.def(py::init<int>())
29-
.def(py::init<tasks::Aggregated::TaskPtr>(),
30-
py::keep_alive<1,2>()) // keep the input task alive while AutoStack exists
31+
.def(py::init<int>(), py::arg("x_size"))
32+
.def(py::init<tasks::Aggregated::TaskPtr>(), py::arg("task"))
3133
.def(py::init<tasks::Aggregated::TaskPtr, std::list<constraints::Aggregated::ConstraintPtr>>(),
32-
py::keep_alive<1,2>()) // keep the aggregated task (arg 2) alive while AutoStack (1) lives
33-
.def(py::init<solvers::iHQP::Stack>())
34-
.def(py::init<solvers::iHQP::Stack, std::list<constraints::Aggregated::ConstraintPtr>>())
35-
//.def("update", &AutoStack::update)
36-
37-
.def("update", [](AutoStack::Ptr stack) {
38-
try {
39-
std::cerr << "=== AutoStack::update() diagnostic ===\n";
40-
// Try to get the underlying iHQP::Stack
41-
auto &cstack = stack->getStack(); // iHQP::Stack&
42-
43-
// The internal iHQP::Stack type is usually a vector/list of TaskPtr
44-
std::cerr << "iHQP::Stack size: " << cstack.size() << "\n";
45-
46-
for (size_t i = 0; i < cstack.size(); ++i) {
47-
auto taskPtr = cstack[i]; // iHQP::TaskPtr
48-
// Print address
49-
std::cerr << "[" << i << "] iHQP::TaskPtr addr: " << taskPtr.get();
50-
51-
// Print dynamic type name
52-
try {
53-
const std::type_info &ti = typeid(*taskPtr);
54-
std::string name = demangle(ti.name());
55-
std::cerr << ", dynamic type: " << name;
56-
} catch (...) {
57-
std::cerr << ", dynamic type: <typeid failed>";
58-
}
59-
60-
// print newline
61-
std::cerr << "\n";
62-
}
63-
64-
std::cerr << "=== end diagnostic list ===\n";
65-
66-
// Now call the real update (this may throw)
67-
stack->update();
68-
69-
std::cerr << "AutoStack::update() completed without throwing.\n";
70-
} catch (const std::exception &e) {
71-
std::cerr << "AutoStack::update() threw std::exception: " << e.what() << "\n";
72-
throw; // rethrow to Python (so you still see the Python traceback)
73-
} catch (...) {
74-
std::cerr << "AutoStack::update() threw unknown exception\n";
75-
throw;
76-
}
77-
})
34+
py::arg("task"), py::arg("bounds"))
35+
.def(py::init<solvers::iHQP::Stack>(), py::arg("stack"))
36+
.def(py::init<solvers::iHQP::Stack, std::list<constraints::Aggregated::ConstraintPtr>>(),
37+
py::arg("stack"), py::arg("bounds"))
38+
39+
.def("update", &AutoStack::update)
40+
// .def("update", [](AutoStack::Ptr stack) {
41+
// try {
42+
// std::cerr << "=== AutoStack::update() diagnostic ===\n";
43+
// auto &cstack = stack->getStack();
44+
// std::cerr << "iHQP::Stack size: " << cstack.size() << "\n";
45+
// for (size_t i = 0; i < cstack.size(); ++i) {
46+
// auto taskPtr = cstack[i];
47+
// std::cerr << "[" << i << "] iHQP::TaskPtr addr: " << taskPtr.get();
48+
// try {
49+
// const std::type_info &ti = typeid(*taskPtr);
50+
// std::cerr << ", dynamic type: " << demangle(ti.name()) << "\n";
51+
// } catch (...) {
52+
// std::cerr << ", dynamic type: <typeid failed>\n";
53+
// }
54+
// }
55+
// std::cerr << "=== end diagnostic list ===\n";
56+
57+
// stack->update();
58+
// std::cerr << "AutoStack::update() completed without throwing.\n";
59+
// } catch (const std::exception &e) {
60+
// std::cerr << "AutoStack::update() threw std::exception: " << e.what() << "\n";
61+
// throw;
62+
// } catch (...) {
63+
// std::cerr << "AutoStack::update() threw unknown exception\n";
64+
// throw;
65+
// }
66+
// })
7867

7968
.def("log", &AutoStack::log)
8069
.def("checkConsistency", &AutoStack::checkConsistency)
8170
.def("getStack", &AutoStack::getStack, py::return_value_policy::reference_internal)
8271
.def("getBoundsList", &AutoStack::getBoundsList, py::return_value_policy::reference_internal)
83-
.def("setRegularisationTask", &AutoStack::setRegularisationTask, py::keep_alive<1,2>()) // keep reg task alive while stack lives
84-
.def("getRegularisationTask", &AutoStack::getRegularisationTask)
72+
.def("setRegularisationTask", &AutoStack::setRegularisationTask)
73+
.def("getRegularisationTask", &AutoStack::getRegularisationTask, py::return_value_policy::reference_internal)
8574
.def("setBoundsAggregationPolicy", &AutoStack::setBoundsAggregationPolicy,
8675
py::arg("aggregationPolicy") = constraints::Aggregated::EQUALITIES_TO_INEQUALITIES |
8776
constraints::Aggregated::UNILATERAL_TO_BILATERAL)
88-
.def("getBounds", &AutoStack::getBounds)
89-
.def("getTask", &AutoStack::getTask)
90-
91-
// AutoStack operators: ensure returned AutoStack / Aggregated keeps args alive
92-
.def("__lshift__",
93-
[](AutoStack::Ptr stack, constraints::Aggregated::ConstraintPtr bound) { return stack << bound; },
94-
py::keep_alive<0,1>(), // keep returned (0) alive while 'stack' (1) is alive (and vice-versa)
95-
py::keep_alive<0,2>()) // keep bound (2) alive while returned (0) lives
96-
.def("__lshift__",
97-
[](AutoStack::Ptr stack, tasks::Aggregated::TaskPtr task) { return stack << task; },
98-
py::keep_alive<0,1>(),
99-
py::keep_alive<0,2>())
100-
101-
.def("__truediv__",
102-
[](AutoStack::Ptr stack, tasks::Aggregated::TaskPtr task) { return stack / task; },
103-
py::keep_alive<0,1>(),
104-
py::keep_alive<0,2>())
105-
.def("__truediv__",
106-
[](tasks::Aggregated::TaskPtr task1, tasks::Aggregated::TaskPtr task2) { return task1 / task2; },
107-
py::keep_alive<0,1>(),
108-
py::keep_alive<0,2>())
109-
.def("__truediv__",
110-
[](tasks::Aggregated::TaskPtr task, AutoStack::Ptr stack) { return task / stack; },
111-
py::keep_alive<0,1>(),
112-
py::keep_alive<0,2>())
113-
.def("__truediv__",
114-
[](AutoStack::Ptr stack1, AutoStack::Ptr stack2) { return stack1 / stack2; },
115-
py::keep_alive<0,1>(),
116-
py::keep_alive<0,2>());
117-
118-
// Free functions
119-
// mul: keep the returned Aggregated (0) alive while the task arg (2) is alive
120-
m.def("mul",
121-
[](const Eigen::MatrixXd& W, tasks::Aggregated::TaskPtr task) { return W * task; },
122-
py::keep_alive<0,2>());
123-
124-
m.def("mul",
125-
[](double w, tasks::Aggregated::TaskPtr task) { return w * task; },
126-
py::keep_alive<0,2>());
127-
128-
m.def("mul",
129-
[](double w, tasks::Aggregated::Ptr task) { return w * task; },
130-
py::keep_alive<0,2>());
131-
132-
// sub: returns a SubTask/SubConstraint that holds pointer to original -> keep original alive
133-
m.def("sub",
134-
[](tasks::Aggregated::TaskPtr task, const std::list<unsigned int>& rows) { return task % rows; },
135-
py::keep_alive<0,1>());
136-
137-
m.def("sub",
138-
[](constraints::Aggregated::ConstraintPtr constraint, const std::list<unsigned int>& rows) { return constraint % rows; },
139-
py::keep_alive<0,1>());
140-
141-
// sum overloads: result aggregates tasks; keep inputs alive
142-
m.def("sum",
143-
[](tasks::Aggregated::TaskPtr t1, tasks::Aggregated::TaskPtr t2) { return t1 + t2; },
144-
py::keep_alive<0,1>(), py::keep_alive<0,2>());
145-
146-
m.def("sum",
147-
[](tasks::Aggregated::Ptr agg, tasks::Aggregated::TaskPtr t) { return agg + t; },
148-
py::keep_alive<0,1>(), py::keep_alive<0,2>());
149-
150-
m.def("sum",
151-
[](tasks::Aggregated::TaskPtr t, tasks::Aggregated::Ptr agg) { return t + agg; },
152-
py::keep_alive<0,1>(), py::keep_alive<0,2>());
153-
154-
m.def("sum",
155-
[](tasks::Aggregated::Ptr agg1, tasks::Aggregated::Ptr agg2) { return agg1 + agg2; },
156-
py::keep_alive<0,1>(), py::keep_alive<0,2>());
157-
158-
// hard (/) overloads: keep args alive while returned object lives
159-
m.def("hard",
160-
[](tasks::Aggregated::TaskPtr t1, tasks::Aggregated::TaskPtr t2) { return t1 / t2; },
161-
py::keep_alive<0,1>(), py::keep_alive<0,2>());
162-
163-
m.def("hard",
164-
[](AutoStack::Ptr stack, tasks::Aggregated::TaskPtr t) { return stack / t; },
165-
py::keep_alive<0,1>(), py::keep_alive<0,2>());
166-
167-
m.def("hard",
168-
[](tasks::Aggregated::TaskPtr t, AutoStack::Ptr stack) { return t / stack; },
169-
py::keep_alive<0,1>(), py::keep_alive<0,2>());
170-
171-
m.def("hard",
172-
[](AutoStack::Ptr stack1, AutoStack::Ptr stack2) { return stack1 / stack2; },
173-
py::keep_alive<0,1>(), py::keep_alive<0,2>());
174-
175-
// subj (<<) overloads: keep both args alive while returned object lives
176-
m.def("subj",
177-
[](tasks::Aggregated::TaskPtr t, constraints::Aggregated::ConstraintPtr c) { return t << c; },
178-
py::keep_alive<0,1>(), py::keep_alive<0,2>());
179-
180-
m.def("subj",
181-
[](tasks::Aggregated::Ptr t, constraints::Aggregated::ConstraintPtr c) { return t << c; },
182-
py::keep_alive<0,1>(), py::keep_alive<0,2>());
183-
184-
m.def("subj",
185-
[](AutoStack::Ptr stack, constraints::Aggregated::ConstraintPtr c) { return stack << c; },
186-
py::keep_alive<0,1>(), py::keep_alive<0,2>());
187-
188-
m.def("subj",
189-
[](tasks::Aggregated::TaskPtr t1, tasks::Aggregated::TaskPtr t2) { return t1 << t2; },
190-
py::keep_alive<0,1>(), py::keep_alive<0,2>());
191-
192-
m.def("subj",
193-
[](AutoStack::Ptr stack, tasks::Aggregated::TaskPtr t) { return stack << t; },
194-
py::keep_alive<0,1>(), py::keep_alive<0,2>());
77+
.def("getBounds", &AutoStack::getBounds, py::return_value_policy::reference_internal)
78+
.def("getTask", &AutoStack::getTask, py::return_value_policy::reference_internal)
79+
80+
// operator overloads with shared_ptr
81+
.def("__lshift__", [](AutoStack::Ptr stack, constraints::Aggregated::ConstraintPtr bound) -> AutoStack::Ptr { return stack << bound; })
82+
.def("__lshift__", [](AutoStack::Ptr stack, tasks::Aggregated::TaskPtr task) -> AutoStack::Ptr { return stack << task; })
83+
84+
.def("__truediv__", [](AutoStack::Ptr stack, tasks::Aggregated::TaskPtr task) -> AutoStack::Ptr { return stack / task; })
85+
.def("__truediv__", [](tasks::Aggregated::TaskPtr task1, tasks::Aggregated::TaskPtr task2) -> OpenSoT::AutoStack::Ptr { return task1 / task2; })
86+
.def("__truediv__", [](tasks::Aggregated::TaskPtr task, AutoStack::Ptr stack) -> AutoStack::Ptr { return task / stack; })
87+
.def("__truediv__", [](AutoStack::Ptr stack1, AutoStack::Ptr stack2) -> AutoStack::Ptr { return stack1 / stack2; });
88+
89+
// Free functions (operators)
90+
m.def("mul", [](const Eigen::MatrixXd& W, tasks::Aggregated::TaskPtr task) -> tasks::Aggregated::TaskPtr { return W * task; });
91+
m.def("mul", [](double w, tasks::Aggregated::TaskPtr task) -> tasks::Aggregated::TaskPtr { return w * task; });
92+
m.def("mul", [](double w, tasks::Aggregated::Ptr task) -> tasks::Aggregated::Ptr { return w * task; });
93+
94+
m.def("sub", [](tasks::Aggregated::TaskPtr task, const std::list<unsigned int>& rows) -> tasks::Aggregated::TaskPtr { return task % rows; });
95+
m.def("sub", [](constraints::Aggregated::ConstraintPtr constraint, const std::list<unsigned int>& rows) -> constraints::Aggregated::ConstraintPtr { return constraint % rows; });
96+
97+
m.def("sum", [](tasks::Aggregated::TaskPtr t1, tasks::Aggregated::TaskPtr t2) -> tasks::Aggregated::TaskPtr { return t1 + t2; });
98+
m.def("sum", [](tasks::Aggregated::Ptr agg, tasks::Aggregated::TaskPtr t) -> tasks::Aggregated::Ptr { return agg + t; });
99+
m.def("sum", [](tasks::Aggregated::TaskPtr t, tasks::Aggregated::Ptr agg) -> tasks::Aggregated::Ptr { return t + agg; });
100+
m.def("sum", [](tasks::Aggregated::Ptr agg1, tasks::Aggregated::Ptr agg2) -> tasks::Aggregated::Ptr { return agg1 + agg2; });
101+
102+
m.def("hard", [](tasks::Aggregated::TaskPtr t1, tasks::Aggregated::TaskPtr t2) -> OpenSoT::AutoStack::Ptr { return t1 / t2; });
103+
m.def("hard", [](AutoStack::Ptr stack, tasks::Aggregated::TaskPtr t) -> AutoStack::Ptr { return stack / t; });
104+
m.def("hard", [](tasks::Aggregated::TaskPtr t, AutoStack::Ptr stack) -> AutoStack::Ptr { return t / stack; });
105+
m.def("hard", [](AutoStack::Ptr stack1, AutoStack::Ptr stack2) -> AutoStack::Ptr { return stack1 / stack2; });
106+
107+
m.def("subj", [](tasks::Aggregated::TaskPtr t, constraints::Aggregated::ConstraintPtr c) -> tasks::Aggregated::TaskPtr { return t << c; });
108+
m.def("subj", [](tasks::Aggregated::Ptr t, constraints::Aggregated::ConstraintPtr c) -> tasks::Aggregated::Ptr { return t << c; });
109+
m.def("subj", [](AutoStack::Ptr stack, constraints::Aggregated::ConstraintPtr c) -> AutoStack::Ptr { return stack << c; });
110+
m.def("subj", [](tasks::Aggregated::TaskPtr t1, tasks::Aggregated::TaskPtr t2) -> tasks::Aggregated::TaskPtr { return t1 << t2; });
111+
m.def("subj", [](AutoStack::Ptr stack, tasks::Aggregated::TaskPtr t) -> AutoStack::Ptr { return stack << t; });
195112
}

‎bindings/python/examples/wblipm_ms.py‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -299,8 +299,8 @@ def min_x(x, Q=np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]
299299
for foot_frame in foot_frames:
300300
cost = cost + contact_tasks[foot_frame]
301301

302-
#lipmc = lipm_constraint.create(variables.getVariable("x0")[0:2], rddot0, variables.getVariable("u0"), h)
303-
constraints = integration_constraint + initial_state + lipm_constraint.create(variables.getVariable("x0")[0:2], rddot0, variables.getVariable("u0"), h)
302+
lipmc = lipm_constraint.create(variables.getVariable("x0")[0:2], rddot0, variables.getVariable("u0"), h)
303+
constraints = integration_constraint + initial_state + lipmc
304304

305305
Ns_ref = 40
306306
zmp_refs = zmp_pattern(Ns_ref, offset_y=model.getCOM()[1])

0 commit comments

Comments
 (0)