From 18aea357bcade3dc374b46f82f489d20f3ef3a21 Mon Sep 17 00:00:00 2001 From: EnricoMingo Date: Fri, 3 May 2024 09:21:24 +0200 Subject: [PATCH] Added more bindings related to tasks::velocity --- bindings/python/pyopensot.cpp | 3 ++ bindings/python/tasks/velocity.hpp | 44 ++++++++++++++++++- include/OpenSoT/tasks/velocity/Gaze.h | 9 ++-- .../OpenSoT/tasks/velocity/Manipulability.h | 3 +- .../OpenSoT/tasks/velocity/MinimumEffort.h | 18 ++++---- 5 files changed, 61 insertions(+), 16 deletions(-) diff --git a/bindings/python/pyopensot.cpp b/bindings/python/pyopensot.cpp index 22be6c11..55182b8b 100644 --- a/bindings/python/pyopensot.cpp +++ b/bindings/python/pyopensot.cpp @@ -33,6 +33,9 @@ PYBIND11_MODULE(pyopensot, m) { pyVelocityCartesian(m_tv); pyVelocityAngularMomentum(m_tv); pyVelocityCoM(m_tv); + pyVelocityGaze(m_tv); + pyVelocityManipulability(m_tv); + pyVelocityMinimumEffort(m_tv); auto m_c = m.def_submodule("constraints"); diff --git a/bindings/python/tasks/velocity.hpp b/bindings/python/tasks/velocity.hpp index 4e37515e..895542fb 100644 --- a/bindings/python/tasks/velocity.hpp +++ b/bindings/python/tasks/velocity.hpp @@ -5,6 +5,9 @@ #include #include #include +#include +#include +#include namespace py = pybind11; using namespace OpenSoT::tasks::velocity; @@ -55,7 +58,7 @@ void pyVelocityCartesian(py::module& m) { } void pyVelocityAngularMomentum(py::module& m) { -py::class_, Task>(m, "AngularMomentum") + py::class_, Task>(m, "AngularMomentum") .def(py::init()) .def("setReference", py::overload_cast(&AngularMomentum::setReference)) .def("getReference", py::overload_cast(&AngularMomentum::getReference, py::const_)) @@ -71,7 +74,7 @@ std::tuple com_get_reference(const CoM& com) } void pyVelocityCoM(py::module& m) { - py::class_, OpenSoT::Task>(m, "CoM") + py::class_, Task>(m, "CoM") .def(py::init(), py::arg(), py::arg("id") = "CoM") .def("setReference", py::overload_cast(&CoM::setReference)) @@ -86,4 +89,41 @@ void pyVelocityCoM(py::module& m) { .def("reset", &CoM::reset); } +void pyVelocityGaze(py::module& m) { + py::class_, Task>(m, "Gaze") + .def(py::init()) + .def("setGaze", py::overload_cast(&Gaze::setGaze)) + .def("setOrientationErrorGain", &Gaze::setOrientationErrorGain) + .def("getOrientationErrorGain", &Gaze::getOrientationErrorGain) + .def("setWeight", &Gaze::setWeight) + .def("getConstraints", &Gaze::getConstraints, py::return_value_policy::reference) + .def("getTaskSize", &Gaze::getTaskSize) + .def("getActiveJointsMask", &Gaze::getActiveJointsMask) + .def("setActiveJointsMask", &Gaze::setActiveJointsMask) + .def("getDistalLink", &Gaze::getDistalLink) + .def("setLambda", &Gaze::setLambda) + .def("setBaseLink", &Gaze::setBaseLink); +} + +void pyVelocityManipulability(py::module& m) { + py::class_, Task>(m, "Manipulability") + .def(py::init(), + py::arg(), py::arg(), py::arg("step") = 1E-3) + .def(py::init(), + py::arg(), py::arg(), py::arg("step") = 1E-3) + .def("ComputeManipulabilityIndex", &Manipulability::ComputeManipulabilityIndex) + .def("setW", &Manipulability::setW) + .def("getW", &Manipulability::getW) + .def("setLambda", &Manipulability::setLambda); +} + +void pyVelocityMinimumEffort(py::module& m) { + py::class_, Task>(m, "MinimumEffort") + .def(py::init(), py::arg(), py::arg("step") = 1E-3) + .def("ComputeEffort", &MinimumEffort::computeEffort) + .def("setW", &MinimumEffort::setW) + .def("getW", &MinimumEffort::getW) + .def("setLambda", &MinimumEffort::setLambda); +} + diff --git a/include/OpenSoT/tasks/velocity/Gaze.h b/include/OpenSoT/tasks/velocity/Gaze.h index b8bf9e28..f6326867 100644 --- a/include/OpenSoT/tasks/velocity/Gaze.h +++ b/include/OpenSoT/tasks/velocity/Gaze.h @@ -86,10 +86,6 @@ class Gaze: public OpenSoT::Task @return the number of rows of A */ virtual const unsigned int getTaskSize() const; - /** Updates the A, b, Aeq, beq, Aineq, b*Bound matrices - @param x variable state at the current step (input) */ - virtual void _update(); - /** * @brief getActiveJointsMask return a vector of length NumberOfDOFs. * If an element is false the corresponding column of the task jacobian is set to 0. @@ -138,6 +134,11 @@ class Gaze: public OpenSoT::Task Eigen::Affine3d _tmpEigenM; Eigen::Affine3d _tmpEigenM2; + /** Updates the A, b, Aeq, beq, Aineq, b*Bound matrices + @param x variable state at the current step (input) */ + virtual void _update(); + + }; diff --git a/include/OpenSoT/tasks/velocity/Manipulability.h b/include/OpenSoT/tasks/velocity/Manipulability.h index 276a39ae..fa74982f 100644 --- a/include/OpenSoT/tasks/velocity/Manipulability.h +++ b/include/OpenSoT/tasks/velocity/Manipulability.h @@ -47,8 +47,6 @@ namespace OpenSoT { ~Manipulability(); - void _update(); - /** * @brief ComputeManipulabilityIndex * @return the manipulability index at the actual configuration q (ast from latest update(q)) @@ -82,6 +80,7 @@ namespace OpenSoT { const XBot::ModelInterface& _model; Eigen::VectorXd _q; + void _update(); /** * @brief The ComputeManipulabilityIndexGradient class implements a worker class to computes diff --git a/include/OpenSoT/tasks/velocity/MinimumEffort.h b/include/OpenSoT/tasks/velocity/MinimumEffort.h index 23af8514..99ff65ff 100644 --- a/include/OpenSoT/tasks/velocity/MinimumEffort.h +++ b/include/OpenSoT/tasks/velocity/MinimumEffort.h @@ -44,6 +44,16 @@ protected: const XBot::ModelInterface& _model; Eigen::VectorXd _q; + + /** + * @brief _update updates the minimum effort gradient. + * @detail It also updates the state on the internal robot model so that successive calls to the + * computeEffort() function will take into account the updated posture of the robot. + * @param x the actual posture of the robot + */ + void _update(); + + /** * @brief The ComputeGTauGradient class implements a worker class to computes the effort for a certain configuration. * It will take into account the robot as standing on a flat floor, and while computing the gradient, @@ -101,14 +111,6 @@ ~MinimumEffort(); - /** - * @brief _update updates the minimum effort gradient. - * @detail It also updates the state on the internal robot model so that successive calls to the - * computeEffort() function will take into account the updated posture of the robot. - * @param x the actual posture of the robot - */ - void _update(); - /** * @brief computeEffort * @return the effort at the actual configuration q (ast from latest update(q))