Implement get_task_weight method in the python bindings

ami-iit · Dec 2, 2021 · 085f28c · 085f28c
1 parent b294cc3
commit 085f28c
Show file tree

Hide file tree

Showing 2 changed files with 30 additions and 5 deletions.
diff --git a/bindings/python/IK/src/IntegrationBasedIK.cpp b/bindings/python/IK/src/IntegrationBasedIK.cpp
@@ -46,6 +46,20 @@ void CreateIntegrationBasedIK(pybind11::module& module)
  &ILinearTaskSolver<IKLinearTask, IntegrationBasedIKState>::setTaskWeight,
  py::arg("task_name"),
  py::arg("weight"))
+ .def(
+ "get_task_weight",
+ [](const ILinearTaskSolver<IKLinearTask, IntegrationBasedIKState>& impl,
+ const std::string& name) {
+ Eigen::VectorXd weight;
+
+ if (!impl.getTaskWeight(name, weight))
+ {
+ const std::string msg = "Failed to get the weight for the task named " + name + ".";
+ throw py::value_error(msg);
+ }
+ return weight;
+ },
+ py::arg("task_name"))
  .def("get_task_names",
  &ILinearTaskSolver<IKLinearTask, IntegrationBasedIKState>::getTaskNames)
  .def("finalize",

diff --git a/bindings/python/TSID/src/TaskSpaceInverseDynamics.cpp b/bindings/python/TSID/src/TaskSpaceInverseDynamics.cpp
@@ -47,22 +47,33 @@ void CreateTaskSpaceInverseDynamics(pybind11::module& module)
  &ILinearTaskSolver<TSIDLinearTask, TSIDState>::setTaskWeight,
  py::arg("task_name"),
  py::arg("weight"))
- .def("get_task_names",
- &ILinearTaskSolver<TSIDLinearTask, TSIDState>::getTaskNames)
+ .def(
+ "get_task_weight",
+ [](const ILinearTaskSolver<TSIDLinearTask, TSIDState>& impl, const std::string& name) {
+ Eigen::VectorXd weight;
+
+ if (!impl.getTaskWeight(name, weight))
+ {
+ const std::string msg = "Failed to get the weight for the task named " + name + ".";
+ throw py::value_error(msg);
+ }
+ return weight;
+ },
+ py::arg("task_name"))
+ .def("get_task_names", &ILinearTaskSolver<TSIDLinearTask, TSIDState>::getTaskNames)
  .def("finalize",
  &ILinearTaskSolver<TSIDLinearTask, TSIDState>::finalize,
  py::arg("handler"))
  .def("advance", &ILinearTaskSolver<TSIDLinearTask, TSIDState>::advance)
  .def("get_output", &ILinearTaskSolver<TSIDLinearTask, TSIDState>::getOutput)
- .def("is_output_valid",
- &ILinearTaskSolver<TSIDLinearTask, TSIDState>::isOutputValid)
+ .def("is_output_valid", &ILinearTaskSolver<TSIDLinearTask, TSIDState>::isOutputValid)
  .def(
  "initialize",
  [](ILinearTaskSolver<TSIDLinearTask, TSIDState>& impl,
  std::shared_ptr<const BipedalLocomotion::ParametersHandler::IParametersHandler>
  handler) -> bool { return impl.initialize(handler); },
  py::arg("handler"))
- .def("__repr__", &ILinearTaskSolver<TSIDLinearTask, TSIDState>::toString);
+ .def("__repr__", &ILinearTaskSolver<TSIDLinearTask, TSIDState>::toString);
 
  py::class_<TaskSpaceInverseDynamics, //
  ILinearTaskSolver<TSIDLinearTask, TSIDState>>(module, "TaskSpaceInverseDynamics");