Implement the python bindings for the Wrench class

ami-iit · Oct 31, 2023 · 4b444f7 · 4b444f7
1 parent 3819ea3
commit 4b444f7
Show file tree

Hide file tree

Showing 3 changed files with 66 additions and 1 deletion.
diff --git a/bindings/python/Math/CMakeLists.txt b/bindings/python/Math/CMakeLists.txt
@@ -7,6 +7,6 @@ set(H_PREFIX include/BipedalLocomotion/bindings/Math)
 add_bipedal_locomotion_python_module(
   NAME MathBindings
   SOURCES src/Constants.cpp src/SchmittTrigger.cpp src/Module.cpp
-  HEADERS ${H_PREFIX}/Constants.h ${H_PREFIX}/SchmittTrigger.h ${H_PREFIX}/Module.h
+  HEADERS ${H_PREFIX}/Constants.h ${H_PREFIX}/SchmittTrigger.h ${H_PREFIX}/Wrench.h ${H_PREFIX}/Module.h
   LINK_LIBRARIES BipedalLocomotion::Math
   )
diff --git a/bindings/python/Math/include/BipedalLocomotion/bindings/Math/Wrench.h b/bindings/python/Math/include/BipedalLocomotion/bindings/Math/Wrench.h
@@ -0,0 +1,63 @@
+/**
+ * @file Constants.h
+ * @authors Giulio Romualdi
+ * @copyright 2021 Istituto Italiano di Tecnologia (IIT). This software may be modified and
+ * distributed under the terms of the BSD-3-Clause license.
+ */
+
+#ifndef BIPEDAL_LOCOMOTION_BINDINGS_MATH_WRENCH_H
+#define BIPEDAL_LOCOMOTION_BINDINGS_MATH_WRENCH_H
+
+#include <pybind11/eigen.h>
+#include <pybind11/pybind11.h>
+
+#include <BipedalLocomotion/Math/Wrench.h>
+
+namespace BipedalLocomotion
+{
+namespace bindings
+{
+namespace Math
+{
+
+template <typename Scalar> struct WrenchTrampoline
+{
+    BipedalLocomotion::Math::Wrench<Scalar> wrench;
+};
+
+template <typename Scalar> void CreateWrench(pybind11::module& module, const std::string& suffix)
+{
+    namespace py = ::pybind11;
+    using namespace BipedalLocomotion::Math;
+
+    py::class_<WrenchTrampoline<Scalar>>(module, ("Wrench" + suffix).c_str())
+        .def(py::init<>())
+        .def(py::init([](const Eigen::Matrix<Scalar, 6, 1>& vector) -> WrenchTrampoline<Scalar> {
+            WrenchTrampoline<Scalar> tmp;
+            tmp.wrench = vector;
+            return tmp;
+        }))
+        .def("get_local_cop",
+             [](const WrenchTrampoline<Scalar>& impl) -> Eigen::Matrix<Scalar, 3, 1> {
+                 return impl.wrench.getLocalCoP();
+             })
+        .def_property(
+            "force",
+            [](const WrenchTrampoline<Scalar>& impl)
+                -> Eigen::Ref<const Eigen::Matrix<Scalar, 3, 1>> { return impl.wrench.force(); },
+            [](WrenchTrampoline<Scalar>& impl, const Eigen::Matrix<Scalar, 3, 1>& force) {
+                impl.wrench.force() = force;
+            })
+        .def_property(
+            "torque",
+            [](const WrenchTrampoline<Scalar>& impl)
+                -> Eigen::Ref<const Eigen::Matrix<Scalar, 3, 1>> { return impl.wrench.torque(); },
+            [](WrenchTrampoline<Scalar>& impl, const Eigen::Matrix<Scalar, 3, 1>& torque) {
+                impl.wrench.torque() = torque;
+            });
+}
+} // namespace Math
+} // namespace bindings
+} // namespace BipedalLocomotion
+
+#endif // BIPEDAL_LOCOMOTION_BINDINGS_MATH_WRENCH_H
diff --git a/bindings/python/Math/src/Module.cpp b/bindings/python/Math/src/Module.cpp
@@ -10,6 +10,7 @@
 #include <BipedalLocomotion/bindings/Math/Constants.h>
 #include <BipedalLocomotion/bindings/Math/Module.h>
 #include <BipedalLocomotion/bindings/Math/SchmittTrigger.h>
+#include <BipedalLocomotion/bindings/Math/Wrench.h>
 
 namespace BipedalLocomotion
 {
@@ -23,6 +24,7 @@ void CreateModule(pybind11::module& module)
 
     CreateConstants(module);
     CreateSchmittTrigger(module);
+    CreateWrench<double>(module, "d");
 }
 } // namespace Math
 } // namespace bindings