on pybinds

mrsp · mrsp · commit dbbbfa1b5228 · 2025-04-04T20:45:29.000+03:00
diff --git a/.gitignore b/.gitignore
@@ -11,3 +11,8 @@ serow_layout.json:Zone.Identifier
 /evaluation/mujoco_test/build/
 /evaluation/mujoco_test/mujoco_data/*/est_*.bin
 /evaluation/mujoco_test/mujoco_data/*/serow_predictions.h5
+/python/serow/__pycache__/
+/python/build/
+/python/serow.egg-info/
+/python/serow/contact_ekf.cpython-310-x86_64-linux-gnu.so
+/python/serow/state.cpython-310-x86_64-linux-gnu.so
diff --git a/core/src/BaseEKF.hpp b/core/src/BaseEKF.hpp
@@ -106,8 +106,7 @@ class BaseEKF {
      * @param angular_velocity Angular velocity measurements.
      * @param linear_acceleration Linear acceleration measurements.
      */
-    void computeDiscreteDynamics(BaseState& state, double dt,
-                                 Eigen::Vector3d angular_velocity,
+    void computeDiscreteDynamics(BaseState& state, double dt, Eigen::Vector3d angular_velocity,
                                  Eigen::Vector3d linear_acceleration);
 
     /**
@@ -132,10 +131,10 @@ class BaseEKF {
      * @param base_position_cov Covariance of base position measurements.
      * @param base_orientation_cov Covariance of base orientation measurements.
      */
-     void updateWithOdometry(BaseState& state, const Eigen::Vector3d& base_position,
-                             const Eigen::Quaterniond& base_orientation,
-                             const Eigen::Matrix3d& base_position_cov,
-                             const Eigen::Matrix3d& base_orientation_cov);
+    void updateWithOdometry(BaseState& state, const Eigen::Vector3d& base_position,
+                            const Eigen::Quaterniond& base_orientation,
+                            const Eigen::Matrix3d& base_position_cov,
+                            const Eigen::Matrix3d& base_orientation_cov);
 
     /**
      * @brief Updates the state of the robot with the provided state change and covariance matrix.
diff --git a/core/src/CoMEKF.cpp b/core/src/CoMEKF.cpp
@@ -69,10 +69,9 @@ void CoMEKF::updateWithKinematics(CentroidalState& state, const KinematicMeasure
 }
 
 void CoMEKF::updateWithImu(CentroidalState& state, const KinematicMeasurement& kin,
-                                      const GroundReactionForceMeasurement& grf) {
+                           const GroundReactionForceMeasurement& grf) {
     updateWithCoMAcceleration(state, kin.com_linear_acceleration, grf.cop, grf.force,
-                                     kin.com_linear_acceleration_cov,
-                                     kin.com_angular_momentum_derivative);
+                              kin.com_linear_acceleration_cov, kin.com_angular_momentum_derivative);
 }
 
 Eigen::Matrix<double, 9, 1> CoMEKF::computeContinuousDynamics(
@@ -127,11 +126,12 @@ CoMEKF::computePredictionJacobians(const CentroidalState& state,
     return std::make_tuple(Ac, Lc);
 }
 
-void CoMEKF::updateWithCoMAcceleration(
-    CentroidalState& state, const Eigen::Vector3d& com_linear_acceleration,
-    const Eigen::Vector3d& cop_position, const Eigen::Vector3d& ground_reaction_force,
-    const Eigen::Matrix3d& com_linear_acceleration_cov,
-    const Eigen::Vector3d& com_angular_momentum_derivative) {
+void CoMEKF::updateWithCoMAcceleration(CentroidalState& state,
+                                       const Eigen::Vector3d& com_linear_acceleration,
+                                       const Eigen::Vector3d& cop_position,
+                                       const Eigen::Vector3d& ground_reaction_force,
+                                       const Eigen::Matrix3d& com_linear_acceleration_cov,
+                                       const Eigen::Vector3d& com_angular_momentum_derivative) {
     double den = state.com_position.z() - cop_position.z();
     if (den == 0.0) {
         std::cerr << "CoM and COP are at the same height, setting to 1e-6" << std::endl;
@@ -168,8 +168,7 @@ void CoMEKF::updateWithCoMAcceleration(
     updateState(state, dx, P_);
 }
 
-void CoMEKF::updateWithCoMPosition(CentroidalState& state,
-                                   const Eigen::Vector3d& com_position,
+void CoMEKF::updateWithCoMPosition(CentroidalState& state, const Eigen::Vector3d& com_position,
                                    const Eigen::Matrix3d& com_position_cov) {
     const Eigen::Vector3d z = com_position - state.com_position;
     Eigen::Matrix<double, 3, 9> H = Eigen::Matrix<double, 3, 9>::Zero();
diff --git a/core/src/ContactEKF.cpp b/core/src/ContactEKF.cpp
@@ -411,8 +411,9 @@ void ContactEKF::updateWithTerrain(BaseState& state,
         // Update only when the elevation at the contact points is available and updated in the map
         if (cs) {
             const std::array<float, 2> terrain_origin = terrain_estimator->getMapOrigin();
-            const std::array<float, 2> con_pos_xy = {static_cast<float>(state.contacts_position.at(cf).x()) - terrain_origin[0],
-                                                     static_cast<float>(state.contacts_position.at(cf).y()) - terrain_origin[1]};
+            const std::array<float, 2> con_pos_xy = {
+                static_cast<float>(state.contacts_position.at(cf).x()) - terrain_origin[0],
+                static_cast<float>(state.contacts_position.at(cf).y()) - terrain_origin[1]};
             auto elevation = terrain_estimator->getElevation(con_pos_xy);
             if (elevation.has_value() && elevation.value().updated) {
                 // Construct the linearized measurement matrix H
@@ -524,8 +525,9 @@ void ContactEKF::update(BaseState& state, const KinematicMeasurement& kin,
                 const Eigen::Vector3d con_pos_world =
                     T_world_to_base * kin.base_to_foot_positions.at(cf);
                 const std::array<float, 2> terrain_origin = terrain_estimator->getMapOrigin();
-                const std::array<float, 2> con_pos_xy = {static_cast<float>(con_pos_world.x()) - terrain_origin[0],
-                                                         static_cast<float>(con_pos_world.y()) - terrain_origin[1]};
+                const std::array<float, 2> con_pos_xy = {
+                    static_cast<float>(con_pos_world.x()) - terrain_origin[0],
+                    static_cast<float>(con_pos_world.y()) - terrain_origin[1]};
                 const float con_pos_z = static_cast<float>(con_pos_world.z());
 
                 // Transform measurement noise to world frame
diff --git a/core/src/ContactEKF.hpp b/core/src/ContactEKF.hpp
@@ -83,8 +83,6 @@ class ContactEKF {
                 std::optional<OdometryMeasurement> odom = std::nullopt,
                 std::shared_ptr<TerrainElevation> terrain_estimator = nullptr);
 
-    const Eigen::Isometry3d& getMapPose() const;
-
 private:
     int num_states_{};                      ///< Number of state variables.
     int num_inputs_{};                      ///< Number of input variables.
diff --git a/core/src/Serow.cpp b/core/src/Serow.cpp
@@ -718,7 +718,8 @@ void Serow::filter(ImuMeasurement imu, std::map<std::string, JointMeasurement> j
         } else if (params_.terrain_estimator_type == "local") {
             terrain_estimator_ = std::make_shared<LocalTerrainMapper>();
         } else {
-            throw std::runtime_error("Invalid terrain estimator type: " + params_.terrain_estimator_type);
+            throw std::runtime_error("Invalid terrain estimator type: " +
+                                     params_.terrain_estimator_type);
         }
         terrain_estimator_->initializeLocalMap(terrain_height, 1e4,
                                                params_.minimum_terrain_height_variance);
@@ -944,7 +945,8 @@ void Serow::filter(ImuMeasurement imu, std::map<std::string, JointMeasurement> j
                             float y = (j - half_map_dim) * resolution;
                             const auto& cell = terrain_map[terrain_estimator_->locationToHashId(
                                 std::array<float, 2>{x, y})];
-                            local_map.data.push_back({x + map_origin_xy[0], y + map_origin_xy[1], cell.height});
+                            local_map.data.push_back(
+                                {x + map_origin_xy[0], y + map_origin_xy[1], cell.height});
                         }
                     }
                     exteroception_logger_.log(local_map);
diff --git a/python/README.md b/python/README.md
@@ -0,0 +1,18 @@
+# Python Bindings
+
+## Install the python package
+
+Open a terminal in the current directory
+
+```
+export PYTHONPATH=$PYTHONPATH:$(pwd)
+source ~/.bashrc
+python3 setup.py build_ext --inplace
+```
+
+## Tests
+Run the example with: 
+
+```
+python3 serow/example.py
+```
diff --git a/python/__init__.py b/python/__init__.py
@@ -0,0 +1 @@
+# This file makes the python directory a Python package 
diff --git a/python/pyproject.toml b/python/pyproject.toml
@@ -0,0 +1,3 @@
+[build-system]
+requires = ["setuptools>=42", "wheel", "cmake>=3.1", "pybind11>=2.6.0"]
+build-backend = "setuptools.build_meta" 
diff --git a/python/serow/CMakeLists.txt b/python/serow/CMakeLists.txt
@@ -0,0 +1,39 @@
+cmake_minimum_required(VERSION 3.10)
+project(serow_python)
+
+# Find required packages
+find_package(pybind11 REQUIRED)
+find_package(Eigen3 REQUIRED)
+
+# Find the core library
+find_library(SEROW_CORE_LIB serow
+    PATHS 
+        ${CMAKE_SOURCE_DIR}/../../build/core/src
+        ${CMAKE_SOURCE_DIR}/../../build
+    NO_DEFAULT_PATH
+)
+
+if(NOT SEROW_CORE_LIB)
+    message(FATAL_ERROR "Could not find serow library. Please build the core library first by running:\n"
+        "cd ${CMAKE_SOURCE_DIR}/../..\n"
+        "mkdir -p build\n"
+        "cd build\n"
+        "cmake ..\n"
+        "make")
+endif()
+
+# Add Python module
+pybind11_add_module(contact_ekf contact_ekf_bindings.cpp)
+pybind11_add_module(state state_bindings.cpp)
+
+# Set include directories
+target_include_directories(contact_ekf PRIVATE ${CMAKE_SOURCE_DIR}/../../core/src)
+target_include_directories(state PRIVATE ${CMAKE_SOURCE_DIR}/../../core/src)
+
+# Link against Eigen and core library
+target_link_libraries(contact_ekf PRIVATE Eigen3::Eigen ${SEROW_CORE_LIB})
+target_link_libraries(state PRIVATE Eigen3::Eigen ${SEROW_CORE_LIB})
+
+# Set C++ standard
+set_target_properties(contact_ekf PROPERTIES CXX_STANDARD 17)
+set_target_properties(state PROPERTIES CXX_STANDARD 17) 
diff --git a/python/serow/__init__.py b/python/serow/__init__.py
@@ -0,0 +1,10 @@
+from .contact_ekf import ContactEKF
+from .state import BaseState, ImuMeasurement, KinematicMeasurement, OdometryMeasurement
+
+__all__ = [
+    'ContactEKF',
+    'BaseState',
+    'ImuMeasurement',
+    'KinematicMeasurement',
+    'OdometryMeasurement'
+] 
diff --git a/python/serow/contact_ekf_bindings.cpp b/python/serow/contact_ekf_bindings.cpp
@@ -0,0 +1,45 @@
+#include <pybind11/eigen.h>
+#include <pybind11/functional.h>
+#include <pybind11/pybind11.h>
+#include <pybind11/stl.h>
+
+#include "ContactEKF.hpp"
+#include "Measurement.hpp"
+#include "State.hpp"
+#include "common.hpp"
+
+namespace py = pybind11;
+
+PYBIND11_MODULE(contact_ekf, m) {
+    m.doc() = "Python bindings for ContactEKF class";
+
+    py::class_<serow::ContactEKF>(m, "ContactEKF")
+        .def(py::init<>())
+        .def("init", &serow::ContactEKF::init, py::arg("state"), py::arg("contacts_frame"),
+             py::arg("point_feet"), py::arg("g"), py::arg("imu_rate"),
+             py::arg("outlier_detection") = false)
+        .def("predict", &serow::ContactEKF::predict, py::arg("state"), py::arg("imu"),
+             py::arg("kin"))
+        .def(
+            "update",
+            [](serow::ContactEKF& self, serow::BaseState& state,
+               const serow::KinematicMeasurement& kin, py::object odom, py::object terrain) {
+                if (odom.is_none()) {
+                    if (terrain.is_none()) {
+                        self.update(state, kin, std::nullopt, nullptr);
+                    } else {
+                        self.update(state, kin, std::nullopt,
+                                    terrain.cast<std::shared_ptr<serow::TerrainElevation>>());
+                    }
+                } else {
+                    if (terrain.is_none()) {
+                        self.update(state, kin, odom.cast<serow::OdometryMeasurement>(), nullptr);
+                    } else {
+                        self.update(state, kin, odom.cast<serow::OdometryMeasurement>(),
+                                    terrain.cast<std::shared_ptr<serow::TerrainElevation>>());
+                    }
+                }
+            },
+            py::arg("state"), py::arg("kin"), py::arg("odom") = py::none(),
+            py::arg("terrain_estimator") = py::none());
+}
diff --git a/python/serow/example.py b/python/serow/example.py
@@ -0,0 +1,126 @@
+#!/usr/bin/env python3
+
+import numpy as np
+from serow import ContactEKF, BaseState, ImuMeasurement, KinematicMeasurement, OdometryMeasurement
+
+def main():
+    # Initialize the EKF
+    ekf = ContactEKF()
+    
+    # Create initial state
+    state = BaseState()
+    state.timestamp = 0.0
+    state.base_position = np.array([0.0, 0.0, 1.0])  # Start 1m above ground
+    state.base_orientation = np.array([1.0, 0.0, 0.0, 0.0])  # Identity quaternion
+    state.base_linear_velocity = np.array([0.0, 0.0, 0.0])
+    state.imu_angular_velocity_bias = np.array([0.0, 0.0, 0.0])
+    state.imu_linear_acceleration_bias = np.array([0.0, 0.0, 0.0])
+    
+    # Initialize contact positions (example for a bipedal robot)
+    state.contacts_position = {
+        "left_foot": np.array([0.0, 0.1, 0.0]),  # 10cm to the left
+        "right_foot": np.array([0.0, -0.1, 0.0])  # 10cm to the right
+    }
+    
+    # Initialize contact orientations
+    state.contacts_orientation = {
+        "left_foot": np.array([1.0, 0.0, 0.0, 0.0]),  # Identity quaternion
+        "right_foot": np.array([1.0, 0.0, 0.0, 0.0])  # Identity quaternion
+    }
+    
+    # Initialize covariances
+    state.base_position_cov = np.eye(3) * 0.01  # 10cm uncertainty
+    state.base_orientation_cov = np.eye(3) * 0.01  # ~5.7 degrees uncertainty
+    state.base_linear_velocity_cov = np.eye(3) * 0.1  # 0.3 m/s uncertainty
+    state.imu_angular_velocity_bias_cov = np.eye(3) * 0.0001
+    state.imu_linear_acceleration_bias_cov = np.eye(3) * 0.0001
+    state.contacts_position_cov = {
+        "left_foot": np.eye(3) * 0.0001,
+        "right_foot": np.eye(3) * 0.0001
+    }
+    state.contacts_orientation_cov = {
+        "left_foot": np.eye(3) * 0.0001,
+        "right_foot": np.eye(3) * 0.0001
+    }
+    
+    # Initialize the EKF
+    contacts_frame = {"left_foot", "right_foot"}
+    point_feet = True  # Assuming point feet
+    g = 9.81  # Gravity constant
+    imu_rate = 1000.0  # IMU update rate in Hz
+    outlier_detection = True  # Enable outlier detection
+    
+    ekf.init(state, contacts_frame, point_feet, g, imu_rate, outlier_detection)
+    
+    # Create IMU measurement
+    imu = ImuMeasurement()
+    imu.timestamp = 0.001  # 1ms after initialization
+    imu.angular_velocity = np.array([0.0, 0.0, 0.0])  # No rotation
+    imu.linear_acceleration = np.array([0.0, 0.0, -g])  # Gravity only
+    imu.angular_velocity_cov = np.eye(3) * 0.0001
+    imu.linear_acceleration_cov = np.eye(3) * 0.0001
+    imu.angular_velocity_bias_cov = np.eye(3) * 0.000001
+    imu.linear_acceleration_bias_cov = np.eye(3) * 0.000001
+    
+    # Create kinematic measurement
+    kin = KinematicMeasurement()
+    kin.timestamp = 0.001
+    kin.contacts_status = {
+        "left_foot": True,  # Left foot in contact
+        "right_foot": True  # Right foot in contact
+    }
+    kin.contacts_probability = {
+        "left_foot": 1.0,
+        "right_foot": 1.0
+    }
+    kin.contacts_position = {
+        "left_foot": np.array([0.0, 0.1, 0.0]),
+        "right_foot": np.array([0.0, -0.1, 0.0])
+    }
+    kin.contacts_orientation = {
+        "left_foot": np.array([1.0, 0.0, 0.0, 0.0]),
+        "right_foot": np.array([1.0, 0.0, 0.0, 0.0])
+    }
+    kin.position_slip_cov = np.eye(3) * 0.0001
+    kin.orientation_slip_cov = np.eye(3) * 0.0001
+    kin.contacts_position_noise = {
+        "left_foot": np.eye(3) * 0.0001,
+        "right_foot": np.eye(3) * 0.0001
+    }
+    kin.contacts_orientation_noise = {
+        "left_foot": np.eye(3) * 0.0001,
+        "right_foot": np.eye(3) * 0.0001
+    }
+    
+    # Create odometry measurement (optional)
+    odom = OdometryMeasurement()
+    odom.timestamp = 0.001
+    odom.base_position = np.array([0.0, 0.0, 1.0])
+    odom.base_orientation = np.array([1.0, 0.0, 0.0, 0.0])
+    odom.base_position_cov = np.eye(3) * 0.01
+    odom.base_orientation_cov = np.eye(3) * 0.01
+    
+    # Run a few prediction/update steps
+    for i in range(10):
+        # Update timestamps
+        dt = 0.001  # 1ms time step
+        imu.timestamp += dt
+        kin.timestamp += dt
+        odom.timestamp += dt
+        
+        # Predict step
+        ekf.predict(state, imu, kin)
+        
+        # Update step (pass None for both optional parameters)
+        ekf.update(state, kin, None, None)
+        
+        # Print some state information
+        print(f"\nStep {i+1}:")
+        print(f"Position: {state.base_position}")
+        print(f"Velocity: {state.base_linear_velocity}")
+        print(f"Orientation: {state.base_orientation}")
+        print(f"Left foot position: {state.contacts_position['left_foot']}")
+        print(f"Right foot position: {state.contacts_position['right_foot']}")
+
+if __name__ == "__main__":
+    main() 
diff --git a/python/serow/state_bindings.cpp b/python/serow/state_bindings.cpp
diff --git a/python/setup.py b/python/setup.py

Original file line number	Diff line number	Diff line change
`@@ -718,7 +718,8 @@ void Serow::filter(ImuMeasurement imu, std::map<std::string, JointMeasurement> j`
`718`	`718`	`} else if (params_.terrain_estimator_type == "local") {`
`719`	`719`	`terrain_estimator_ = std::make_shared<LocalTerrainMapper>();`
`720`	`720`	`} else {`
`721`		`- throw std::runtime_error("Invalid terrain estimator type: " + params_.terrain_estimator_type);`
	`721`	`+ throw std::runtime_error("Invalid terrain estimator type: " +`
	`722`	`+ params_.terrain_estimator_type);`
`722`	`723`	`}`
`723`	`724`	`terrain_estimator_->initializeLocalMap(terrain_height, 1e4,`
`724`	`725`	`params_.minimum_terrain_height_variance);`
`@@ -944,7 +945,8 @@ void Serow::filter(ImuMeasurement imu, std::map<std::string, JointMeasurement> j`
`944`	`945`	`float y = (j - half_map_dim) * resolution;`
`945`	`946`	`const auto& cell = terrain_map[terrain_estimator_->locationToHashId(`
`946`	`947`	`std::array<float, 2>{x, y})];`
`947`		`- local_map.data.push_back({x + map_origin_xy[0], y + map_origin_xy[1], cell.height});`
	`948`	`+ local_map.data.push_back(`
	`949`	`+ {x + map_origin_xy[0], y + map_origin_xy[1], cell.height});`
`948`	`950`	`}`
`949`	`951`	`}`
`950`	`952`	`exteroception_logger_.log(local_map);`
Original file line number	Diff line number	Diff line change
`@@ -0,0 +1 @@`
	`1`	`+# This file makes the python directory a Python package`
Original file line number	Diff line number	Diff line change
`@@ -0,0 +1,3 @@`
	`1`	`+[build-system]`
	`2`	`+requires = ["setuptools>=42", "wheel", "cmake>=3.1", "pybind11>=2.6.0"]`
	`3`	`+build-backend = "setuptools.build_meta"`