Merge pull request #53 from ami-iit/add_id_bindings

Add `HumanID` bindings

bindings/python/CMakeLists.txt

@@ -4,6 +4,7 @@
 
 add_subdirectory(type_caster)
 add_subdirectory(IK)
+add_subdirectory(ID)
 
 
 include(ConfigureFileWithCMakeIfTarget)

bindings/python/ID/CMakeLists.txt

@@ -0,0 +1,12 @@
+# Copyright (C) 2021 Istituto Italiano di Tecnologia (IIT). All rights reserved.
+# This software may be modified and distributed under the terms of the
+# BSD-3-Clause license.
+
+
+set(H_PREFIX include/BiomechanicalAnalysis/bindings/ID)
+
+add_baf_python_module(
+    NAME IDBindings
+    SOURCES src/Module.cpp src/InverseDynamics.cpp
+    HEADERS ${H_PREFIX}/Module.h ${H_PREFIX}/InverseDynamics.h
+    LINK_LIBRARIES BiomechanicalAnalysis::ID)

bindings/python/ID/include/BiomechanicalAnalysis/bindings/ID/InverseDynamics.h

@@ -0,0 +1,26 @@
+/**
+ * @file InverseDynamics.h
+ * @authors Davide Gorbani
+ * @copyright 2024 Istituto Italiano di Tecnologia (IIT). This software may be modified and
+ * distributed under the terms of the BSD-3-Clause license.
+ */
+
+#ifndef BIOMECHANICAL_ANALYSIS_BINDINGS_ID_INVERSE_DYNAMICS_H
+#define BIOMECHANICAL_ANALYSIS_BINDINGS_ID_INVERSE_DYNAMICS_H
+
+#include <pybind11/pybind11.h>
+
+namespace BiomechanicalAnalysis
+{
+namespace bindings
+{
+namespace ID
+{
+
+void CreateInverseDynamics(pybind11::module& module);
+
+} // namespace ID
+} // namespace bindings
+} // namespace BiomechanicalAnalysis
+
+#endif // BIOMECHANICAL_ANALYSIS_BINDINGS_ID_INVERSE_DYNAMICS_H

bindings/python/ID/include/BiomechanicalAnalysis/bindings/ID/Module.h

@@ -0,0 +1,25 @@
+/**
+ * @file Module.h
+ * @copyright 2024 Istituto Italiano di Tecnologia (IIT). This software may be modified and
+ * distributed under the terms of the BSD-3-Clause license.
+ */
+
+#ifndef BIOMECHANICAL_ANALYSIS_BINDINGS_ID_MODULE_H
+#define BIOMECHANICAL_ANALYSIS_BINDINGS_ID_MODULE_H
+
+#include <pybind11/pybind11.h>
+
+namespace BiomechanicalAnalysis
+{
+namespace bindings
+{
+namespace ID
+{
+
+void CreateModule(pybind11::module& module);
+
+} // namespace ID
+} // namespace bindings
+} // namespace BiomechanicalAnalysis
+
+#endif // BIOMECHANICAL_ANALYSIS_BINDINGS_ID_MODULE_H

bindings/python/ID/src/InverseDynamics.cpp

@@ -0,0 +1,98 @@
+/**
+ * @file InverseKinematics.cpp
+ * @authors Evelyn D'Elia
+ * @copyright 2024 Istituto Italiano di Tecnologia (IIT). This software may be modified and
+ * distributed under the terms of the BSD-3-Clause license.
+ */
+
+#include <pybind11/chrono.h>
+#include <pybind11/eigen.h>
+#include <pybind11/pybind11.h>
+#include <pybind11/stl.h>
+
+#include <BiomechanicalAnalysis/ID/InverseDynamics.h>
+#include <BiomechanicalAnalysis/bindings/type_caster/swig.h>
+#include <BipedalLocomotion/ParametersHandler/IParametersHandler.h>
+#include <iDynTree/KinDynComputations.h>
+
+namespace BiomechanicalAnalysis
+{
+namespace bindings
+{
+namespace ID
+{
+
+void CreateInverseDynamics(pybind11::module& module)
+{
+    namespace py = ::pybind11;
+
+    using namespace BiomechanicalAnalysis::ID;
+    using namespace BipedalLocomotion::ParametersHandler;
+
+    py::class_<HumanID>(module, "HumanID")
+        .def(py::init())
+        .def(
+            "initialize",
+            [](HumanID& id, std::shared_ptr<const IParametersHandler> handler, py::object& obj) -> bool {
+                std::shared_ptr<iDynTree::KinDynComputations>* cls
+                    = py::detail::swig_wrapped_pointer_to_pybind<std::shared_ptr<iDynTree::KinDynComputations>>(obj);
+
+                if (cls == nullptr)
+                {
+                    throw ::pybind11::value_error("Invalid input for the function. Please provide "
+                                                  "an iDynTree::KinDynComputations object.");
+                }
+
+                return id.initialize(handler, *cls);
+            },
+            py::arg("param_handler"),
+            py::arg("kin_dyn"))
+        .def(
+            "updateExtWrenchesMeasurements",
+            [](HumanID& id, const std::unordered_map<std::string, Eigen::VectorXd>& wrenchesEigen) -> bool {
+                std::unordered_map<std::string, iDynTree::Wrench> wrenches;
+                for (const auto& [key, value] : wrenchesEigen)
+                {
+                    if (value.size() != 6)
+                    {
+                        throw ::pybind11::value_error("Invalid input for the function. The wrenches "
+                                                      "must have 6 elements.");
+                    }
+
+                    iDynTree::Wrench w;
+                    w.setLinearVec3(iDynTree::GeomVector3(value(0), value(1), value(2)));
+                    w.setAngularVec3(iDynTree::GeomVector3(value(3), value(4), value(5)));
+
+                    wrenches[key] = w;
+                }
+                return id.updateExtWrenchesMeasurements(wrenches);
+            },
+            py::arg("wrenches"))
+        .def("solve", &HumanID::solve)
+        .def("getJointTorques",
+             [](HumanID& id) -> Eigen::VectorXd {
+                 Eigen::VectorXd jointTorques(id.getJointTorques().size());
+                 id.getJointTorques(jointTorques);
+                 return jointTorques;
+             })
+        .def("getJointsList", &HumanID::getJointsList)
+        .def("getEstimatedExtWrenches",
+             [](HumanID& id) -> std::vector<Eigen::VectorXd> {
+                 std::vector<Eigen::VectorXd> wrenches;
+                 const auto& estimatedWrenches = id.getEstimatedExtWrenches();
+                 for (int i = 0; i < estimatedWrenches.size(); i++)
+                 {
+                     Eigen::VectorXd w(6);
+                     w << estimatedWrenches[i].getLinearVec3()(0), estimatedWrenches[i].getLinearVec3()(1),
+                         estimatedWrenches[i].getLinearVec3()(2), estimatedWrenches[i].getAngularVec3()(0),
+                         estimatedWrenches[i].getAngularVec3()(1), estimatedWrenches[i].getAngularVec3()(2);
+                     wrenches.push_back(w);
+                 }
+                 return wrenches;
+             })
+        .def("getEstimatedExtWrenchesList", &HumanID::getEstimatedExtWrenchesList);
+}
+
+} // namespace ID
+} // namespace bindings
+} // namespace BiomechanicalAnalysis

bindings/python/ID/src/Module.cpp

@@ -0,0 +1,27 @@
+/**
+ * @file Module.cpp
+ * @copyright 2024 Istituto Italiano di Tecnologia (IIT). This software may be modified and
+ * distributed under the terms of the BSD-3-Clause license.
+ */
+
+#include <pybind11/pybind11.h>
+
+#include <BiomechanicalAnalysis/bindings/ID/InverseDynamics.h>
+#include <BiomechanicalAnalysis/bindings/ID/Module.h>
+
+namespace BiomechanicalAnalysis
+{
+namespace bindings
+{
+namespace ID
+{
+
+void CreateModule(pybind11::module& module)
+{
+    module.doc() = "ID module.";
+
+    CreateInverseDynamics(module);
+}
+} // namespace ID
+} // namespace bindings
+} // namespace BiomechanicalAnalysis

bindings/python/biomechanical_analysis_framework.cpp.in

@@ -11,6 +11,10 @@
 #include <BiomechanicalAnalysis/bindings/IK/Module.h>
 @endcmakeiftarget BiomechanicalAnalysis::IK
 
+@cmakeiftarget BiomechanicalAnalysis::ID
+#include <BiomechanicalAnalysis/bindings/ID/Module.h>
+@endcmakeiftarget BiomechanicalAnalysis::ID
+
 // Create the Python module
 PYBIND11_MODULE(bindings, m)
 {
@@ -26,4 +30,9 @@ PYBIND11_MODULE(bindings, m)
     py::module ikModule = m.def_submodule("ik");
     bindings::IK::CreateModule(ikModule);
     @endcmakeiftarget BiomechanicalAnalysis::IK
+
+    @cmakeiftarget BiomechanicalAnalysis::ID
+    py::module idModule = m.def_submodule("id");
+    bindings::ID::CreateModule(idModule);
+    @endcmakeiftarget BiomechanicalAnalysis::ID
 }

src/ID/src/InverseDynamics.cpp

@@ -177,7 +177,6 @@ bool HumanID::updateExtWrenchesMeasurements(const std::unordered_map<std::string
 
     m_extWrenchesEstimator.measurement.zero(); // Reset the measurement vector to zero
 
-
     // Iterate over each wrench source and update the measurement vector accordingly
     for (int i = 0; i < m_wrenchSources.size(); i++)
     {
@@ -229,7 +228,6 @@ bool HumanID::updateExtWrenchesMeasurements(const std::unordered_map<std::string
     iDynTree::IndexRange rcmSensorRange
         = m_extWrenchesEstimator.berdyHelper.getRangeRCMSensorVariable(iDynTree::BerdySensorTypes::RCM_SENSOR);
 
-
     // Update the measurement vector with the computed RCM wrench components
     for (int i = 0; i < 6; i++)
     {
@@ -239,7 +237,6 @@ bool HumanID::updateExtWrenchesMeasurements(const std::unordered_map<std::string
     return true; // Return true indicating successful update of the measurement vector
 }
 
-
 bool HumanID::solve()
 {
     constexpr auto logPrefix = "[HumanID::solve]";
@@ -581,7 +578,7 @@ bool HumanID::initializeExtWrenchesHelper(const std::shared_ptr<BipedalLocomotio
         if (type == "fixed")
         {
             data.type = WrenchSourceType::Fixed;
-        
+
         } else if (type == "dummy")
         {
             data.type = WrenchSourceType::Dummy;
@@ -738,38 +735,38 @@ bool HumanID::initializeExtWrenchesHelper(const std::shared_ptr<BipedalLocomotio
         switch (berdySensor.type)
         {
 
-            case iDynTree::BerdySensorTypes::NET_EXT_WRENCH_SENSOR: {
-                // Initialize with default covariance
-                iDynTree::Vector6 wrenchCovariance;
+        case iDynTree::BerdySensorTypes::NET_EXT_WRENCH_SENSOR: {
+            // Initialize with default covariance
+            iDynTree::Vector6 wrenchCovariance;
+            for (int i = 0; i < 6; i++)
+                wrenchCovariance.setVal(i, m_extWrenchesEstimator.params.measurementDefaultCovariance);
+            // Set specific covariance if configured
+            auto specificMeasurementsPtr = m_extWrenchesEstimator.params.specificMeasurementsCovariance.find(berdySensor.id);
+            if (specificMeasurementsPtr != m_extWrenchesEstimator.params.specificMeasurementsCovariance.end())
+            {
                 for (int i = 0; i < 6; i++)
-                    wrenchCovariance.setVal(i, m_extWrenchesEstimator.params.measurementDefaultCovariance);
-                // Set specific covariance if configured
-                auto specificMeasurementsPtr = m_extWrenchesEstimator.params.specificMeasurementsCovariance.find(berdySensor.id);
-                if (specificMeasurementsPtr != m_extWrenchesEstimator.params.specificMeasurementsCovariance.end())
-                {
-                    for (int i = 0; i < 6; i++)
-                        wrenchCovariance.setVal(i, specificMeasurementsPtr->second[i]);
-                }
+                    wrenchCovariance.setVal(i, specificMeasurementsPtr->second[i]);
+            }
 
-                // Store triplet for the measurements covariance matrix
-                for (std::size_t i = 0; i < 6; i++)
-                    measurementsCovarianceMatrixTriplets.setTriplet(
-                        {berdySensor.range.offset + i, berdySensor.range.offset + i, wrenchCovariance[i]});
-                }
+            // Store triplet for the measurements covariance matrix
+            for (std::size_t i = 0; i < 6; i++)
+                measurementsCovarianceMatrixTriplets.setTriplet(
+                    {berdySensor.range.offset + i, berdySensor.range.offset + i, wrenchCovariance[i]});
+        }
         break;
-            case iDynTree::BerdySensorTypes::RCM_SENSOR: {
+        case iDynTree::BerdySensorTypes::RCM_SENSOR: {
             auto specificMeasurementsPtr = m_extWrenchesEstimator.params.specificMeasurementsCovariance.find("RCM_"
                                                                                                              "SENS"
                                                                                                              "OR");
-                for (std::size_t i = 0; i < 6; i++)
-                {
-                    measurementsCovarianceMatrixTriplets.setTriplet(
-                        {berdySensor.range.offset + i, berdySensor.range.offset + i, specificMeasurementsPtr->second[i]});
-                }
-                            }
+            for (std::size_t i = 0; i < 6; i++)
+            {
+                measurementsCovarianceMatrixTriplets.setTriplet(
+                    {berdySensor.range.offset + i, berdySensor.range.offset + i, specificMeasurementsPtr->second[i]});
+            }
+        }
+        break;
+        default:
             break;
-            default:
-                break;
         }
     }