Add python bindings for FloatingBaseEstimator and LeggedOdometry

CHANGELOG.md

@@ -9,6 +9,7 @@ All notable changes to this project are documented in this file.
 - Implement the possibility to set a desired reference trajectory in the TimeVaryingDCMPlanner. (https://github.com/dic-iit/bipedal-locomotion-framework/pull/208)
 - Implement SchmittTriggerDetector python bindings (https://github.com/dic-iit/bipedal-locomotion-framework/pull/213)
 - Implement ModelComputationsHelper for quick construction of KinDynComputations object using parameters handler (https://github.com/dic-iit/bipedal-locomotion-framework/pull/216)
+- Implement FloatingBaseEstimator and LeggedOdometry python bindings (https://github.com/dic-iit/bipedal-locomotion-framework/pull/218)
 
 ### Changed
 - Move all the Contacts related classes in Contacts component (https://github.com/dic-iit/bipedal-locomotion-framework/pull/204)

bindings/python/CMakeLists.txt

@@ -17,6 +17,8 @@ pybind11_add_module(pybind11_blf MODULE
  RobotInterface.cpp
  Constants.cpp
  ContactDetectors.cpp
+ FloatingBaseEstimators.cpp
+ LeggedOdometry.cpp
  )
 
 target_link_libraries(pybind11_blf PUBLIC
@@ -27,6 +29,7 @@ target_link_libraries(pybind11_blf PUBLIC
  BipedalLocomotion::Contacts
  BipedalLocomotion::Math
  BipedalLocomotion::ContactDetectors
+ BipedalLocomotion::FloatingBaseEstimators
  )
 
 target_include_directories(pybind11_blf PRIVATE

bindings/python/FloatingBaseEstimators.cpp

@@ -0,0 +1,167 @@
+/**
+ * @file FloatingBaseEstimators.cpp
+ * @authors Prashanth Ramadoss
+ * @copyright 2020 Istituto Italiano di Tecnologia (IIT). This software may be modified and
+ * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version.
+ */
+
+#include <pybind11/eigen.h>
+#include <pybind11/pybind11.h>
+#include <pybind11/stl.h>
+
+#include <BipedalLocomotion/FloatingBaseEstimators/FloatingBaseEstimator.h>
+#include <BipedalLocomotion/FloatingBaseEstimators/ModelComputationsHelper.h>
+
+#include "bipedal_locomotion_framework.h"
+
+void BipedalLocomotion::bindings::CreateKinDynComputations(pybind11::module& module)
+{
+ namespace py = ::pybind11;
+ using namespace iDynTree;
+ py::class_<KinDynComputations, std::shared_ptr<KinDynComputations>>(module,
+ "KinDynComputations");
+}
+
+void BipedalLocomotion::bindings::CreateKinDynComputationsDescriptor(pybind11::module& module)
+{
+ namespace py = ::pybind11;
+ using namespace BipedalLocomotion::ParametersHandler;
+ using namespace BipedalLocomotion::Estimators;
+
+ py::class_<KinDynComputationsDescriptor>(module, "KinDynComputationsDescriptor")
+ .def(py::init())
+ .def_readwrite("kindyn", &KinDynComputationsDescriptor::kindyn)
+ .def("is_valid", &KinDynComputationsDescriptor::isValid);
+
+ module.def(
+ "construct_kindyncomputations_descriptor",
+ [](std::shared_ptr<IParametersHandler> handler) -> KinDynComputationsDescriptor {
+ return constructKinDynComputationsDescriptor(handler);
+ },
+ py::arg("handler"));
+}
+
+void BipedalLocomotion::bindings::CreateFloatingBaseEstimator(pybind11::module& module)
+{
+ namespace py = ::pybind11;
+
+ using namespace BipedalLocomotion::System;
+ using namespace BipedalLocomotion::ParametersHandler;
+ using namespace BipedalLocomotion::Contacts;
+ using namespace BipedalLocomotion::Estimators;
+ using namespace BipedalLocomotion::Estimators::FloatingBaseEstimators;
+
+ py::class_<InternalState>(module, "FloatingBaseEstimatorInternalState")
+ .def(py::init())
+ .def_readwrite("imu_orientation", &InternalState::imuOrientation)
+ .def_readwrite("imu_position", &InternalState::imuPosition)
+ .def_readwrite("imu_linear_velocity", &InternalState::imuLinearVelocity)
+ .def_readwrite("imu_angular_velocity", &InternalState::imuAngularVelocity)
+ .def_readwrite("support_frame_data", &InternalState::supportFrameData)
+ .def_readwrite("accelerometer_bias", &InternalState::accelerometerBias)
+ .def_readwrite("gyroscope_bias", &InternalState::gyroscopeBias)
+ .def_readwrite("l_contact_frame_orientation", &InternalState::lContactFrameOrientation)
+ .def_readwrite("l_contact_frame_position", &InternalState::lContactFramePosition)
+ .def_readwrite("r_contact_frame_orientation", &InternalState::rContactFrameOrientation)
+ .def_readwrite("r_contact_frame_position", &InternalState::rContactFramePosition);
+
+ py::class_<StateStdDev>(module, "FloatingBaseEstimatorStateStdDev")
+ .def(py::init())
+ .def_readwrite("imu_orientation", &StateStdDev::imuOrientation)
+ .def_readwrite("imu_position", &StateStdDev::imuPosition)
+ .def_readwrite("imu_linear_velocity", &StateStdDev::imuLinearVelocity)
+ .def_readwrite("accelerometer_bias", &StateStdDev::accelerometerBias)
+ .def_readwrite("gyroscope_bias", &StateStdDev::gyroscopeBias)
+ .def_readwrite("l_contact_frame_orientation", &StateStdDev::lContactFrameOrientation)
+ .def_readwrite("l_contact_frame_position", &StateStdDev::lContactFramePosition)
+ .def_readwrite("r_contact_frame_orientation", &StateStdDev::rContactFrameOrientation)
+ .def_readwrite("r_contact_frame_position", &StateStdDev::rContactFramePosition);
+
+ py::class_<Output>(module, "FloatingBaseEstimatorOutput")
+ .def(py::init())
+ .def_readwrite("state", &Output::state)
+ .def_readwrite("state_std_dev", &Output::stateStdDev)
+ .def_readwrite("base_pose", &Output::basePose)
+ .def_readwrite("base_twist", &Output::baseTwist);
+
+ py::class_<Measurements>(module, "FloatingBaseEstimatorMeasurements")
+ .def(py::init())
+ .def_readwrite("acc", &Measurements::acc)
+ .def_readwrite("gyro", &Measurements::gyro)
+ .def_readwrite("encoders", &Measurements::encoders)
+ .def_readwrite("encoder_speeds", &Measurements::encodersSpeed)
+ .def_readwrite("stamped_contact_status", &Measurements::stampedContactsStatus)
+ .def_readwrite("lf_in_contact", &Measurements::lfInContact)
+ .def_readwrite("rf_in_contact", &Measurements::rfInContact);
+
+ py::class_<Options>(module, "FloatingBaseEstimatorOptions")
+ .def(py::init())
+ .def_readwrite("imu_bias_estimation_enabled", &Options::imuBiasEstimationEnabled)
+ .def_readwrite("static_imu_bias_initialization_enabled",
+ &Options::staticImuBiasInitializationEnabled)
+ .def_readwrite("nr_samples_for_imu_bias_initialization",
+ &Options::nrSamplesForImuBiasInitialization)
+ .def_readwrite("ekf_update_enabled", &Options::ekfUpdateEnabled)
+ .def_readwrite("acceleration_due_to_gravity", &Options::accelerationDueToGravity);
+
+ py::class_<SensorsStdDev>(module, "FloatingBaseEstimatorSensorsStdDev")
+ .def(py::init())
+ .def_readwrite("accelerometer_noise", &SensorsStdDev::accelerometerNoise)
+ .def_readwrite("gyroscope_noise", &SensorsStdDev::gyroscopeNoise)
+ .def_readwrite("accelerometer_bias_noise", &SensorsStdDev::accelerometerBiasNoise)
+ .def_readwrite("gyroscope_bias_noise", &SensorsStdDev::gyroscopeBiasNoise)
+ .def_readwrite("contact_foot_lin_vel_noise", &SensorsStdDev::contactFootLinvelNoise)
+ .def_readwrite("contact_foot_ang_vel_noise", &SensorsStdDev::contactFootAngvelNoise)
+ .def_readwrite("swing_foot_lin_vel_noise", &SensorsStdDev::swingFootLinvelNoise)
+ .def_readwrite("swing_foot_ang_vel_noise", &SensorsStdDev::swingFootAngvelNoise)
+ .def_readwrite("forward_kinematics_noise", &SensorsStdDev::forwardKinematicsNoise)
+ .def_readwrite("encoders_noise", &SensorsStdDev::encodersNoise);
+
+ py::class_<Advanceable<Output>>(module, "FloatingBaseEstimatorOutputAdvanceable");
+
+ py::class_<FloatingBaseEstimator, Advanceable<Output>> floatingBaseEstimator(module,
+ "FloatingBaseEstimator");
+
+ floatingBaseEstimator.def(py::init())
+ .def("set_imu_measurement",
+ &FloatingBaseEstimator::setIMUMeasurement,
+ py::arg("acc_meas"),
+ py::arg("gyro_meas"))
+ .def("set_contact_status",
+ &FloatingBaseEstimator::setContactStatus,
+ py::arg("name"),
+ py::arg("contact_status"),
+ py::arg("switch_time"),
+ py::arg("time_now"))
+ .def("set_kinematics",
+ &FloatingBaseEstimator::setKinematics,
+ py::arg("encoders"),
+ py::arg("encoder_speeds"))
+ .def("advance", &FloatingBaseEstimator::advance)
+ .def("reset_estimator",
+ py::overload_cast<const InternalState&>(&FloatingBaseEstimator::resetEstimator),
+ py::arg("new_state"))
+ .def("reset_estimator",
+ py::overload_cast<const Eigen::Quaterniond&, const Eigen::Vector3d&>(
+ &FloatingBaseEstimator::resetEstimator),
+ py::arg("new_base_orientation"),
+ py::arg("new_base_position"))
+ .def("get", &FloatingBaseEstimator::get)
+ .def("is_valid", &FloatingBaseEstimator::isValid)
+ .def(
+ "initialize",
+ [](FloatingBaseEstimator& impl,
+ std::shared_ptr<IParametersHandler> handler,
+ std::shared_ptr<iDynTree::KinDynComputations> kinDyn) -> bool {
+ return impl.initialize(handler, kinDyn);
+ },
+ py::arg("handler"),
+ py::arg("kindyn"));
+
+ // at the moment at an interface level, this nested class need not be exposed
+ // but in case, we want to use the FloatingBaseEstimator as base class
+ // for python based implementations, maybe it could be useful
+ py::class_<FloatingBaseEstimator::ModelComputations>(floatingBaseEstimator,
+ "FloatingBaseEstimatorModelComputations")
+ .def(py::init());
+}

bindings/python/LeggedOdometry.cpp

@@ -0,0 +1,60 @@
+/**
+ * @file LeggedOdometry.cpp
+ * @authors Prashanth Ramadoss
+ * @copyright 2020 Istituto Italiano di Tecnologia (IIT). This software may be modified and
+ * distributed under the terms of the GNU Lesser General Public License v2.1 or any later version.
+ */
+
+#include <pybind11/eigen.h>
+#include <pybind11/pybind11.h>
+#include <pybind11/stl.h>
+
+#include <BipedalLocomotion/FloatingBaseEstimators/FloatingBaseEstimator.h>
+#include <BipedalLocomotion/FloatingBaseEstimators/LeggedOdometry.h>
+#include "bipedal_locomotion_framework.h"
+
+void BipedalLocomotion::bindings::CreateLeggedOdometry(pybind11::module& module)
+{
+ namespace py = ::pybind11;
+
+ using namespace BipedalLocomotion::System;
+ using namespace BipedalLocomotion::ParametersHandler;
+ using namespace BipedalLocomotion::Contacts;
+ using namespace BipedalLocomotion::Estimators;
+ using namespace BipedalLocomotion::Estimators::FloatingBaseEstimators;
+
+ py::class_<LeggedOdometry, FloatingBaseEstimator>(module, "LeggedOdometry")
+ .def(py::init())
+ .def("initialize",
+ [](LeggedOdometry& impl,
+ std::shared_ptr<IParametersHandler> handler,
+ std::shared_ptr<iDynTree::KinDynComputations> kinDyn) -> bool {
+ return impl.initialize(handler, kinDyn);
+ },
+ py::arg("handler"), py::arg("kindyn"))
+ .def("set_imu_measurement", &LeggedOdometry::setIMUMeasurement,
+ py::arg("acc_meas"), py::arg("gyro_meas"))
+ .def("set_contact_status", &LeggedOdometry::setContactStatus,
+ py::arg("name"), py::arg("contact_status"), py::arg("switch_time"), py::arg("time_now"))
+ .def("set_kinematics", &LeggedOdometry::setKinematics,
+ py::arg("encoders"), py::arg("encoder_speeds"))
+ .def("advance", &LeggedOdometry::advance)
+ .def("reset_estimator", py::overload_cast<const InternalState&>(&LeggedOdometry::resetEstimator),
+ py::arg("new_state"))
+ .def("reset_estimator",
+ py::overload_cast<const Eigen::Quaterniond&, const Eigen::Vector3d&>(&LeggedOdometry::resetEstimator),
+ py::arg("new_imu_orientation"), py::arg("new_imu_position"))
+ .def("reset_estimator", py::overload_cast<>(&LeggedOdometry::resetEstimator))
+ .def("reset_estimator",
+ py::overload_cast<const std::string&, const Eigen::Quaterniond&, const Eigen::Vector3d&>(&LeggedOdometry::resetEstimator),
+ py::arg("ref_frame_for_world"),py::arg("world_orientation_in_ref_frame"), py::arg("world_position_in_ref_frame"))
+ .def("change_fixed_frame",
+ py::overload_cast<const std::ptrdiff_t&>(&LeggedOdometry::changeFixedFrame),
+ py::arg("frame_index"))
+ .def("change_fixed_frame",
+ py::overload_cast<const std::ptrdiff_t&, const Eigen::Quaterniond&, const Eigen::Vector3d&>(&LeggedOdometry::changeFixedFrame),
+ py::arg("frame_index"), py::arg("frame_orientation_in_world"), py::arg("frame_position_in_world"))
+ .def("get_fixed_frame_index", &LeggedOdometry::getFixedFrameIdx)
+ .def("get", &LeggedOdometry::get)
+ .def("is_valid", &LeggedOdometry::isValid);
+}

bindings/python/ParametersHandler.cpp

@@ -78,6 +78,7 @@ void BipedalLocomotion::bindings::CreateStdParameterHandler(pybind11::module& mo
  const std::vector<std::string>& vec) { impl.setParameter(name, vec); },
  py::arg("name"),
  py::arg("value"))
+ .def("set_group", &StdImplementation::setGroup, py::arg("name"), py::arg("new_group"))
  .def(
  "get_parameter_bool",
  [](const StdImplementation& impl, const std::string& name) -> bool {

bindings/python/bipedal_locomotion_framework.cpp

@@ -55,6 +55,12 @@ PYBIND11_MODULE(bindings, m)
  bindings::CreateContactDetector(m);
  bindings::CreateSchmittTriggerUnit(m);
  bindings::CreateSchmittTriggerDetector(m);
+
+ // FloatingBaseEstimators.cpp
+ bindings::CreateKinDynComputations(m);
+ bindings::CreateKinDynComputationsDescriptor(m);
+ bindings::CreateFloatingBaseEstimator(m);
+ bindings::CreateLeggedOdometry(m);
 }
 
 std::string bindings::ToString(const manif::SE3d& se3)

bindings/python/bipedal_locomotion_framework.h

@@ -66,4 +66,10 @@ void CreateContactDetector(pybind11::module& module);
 void CreateSchmittTriggerUnit(pybind11::module& module);
 void CreateSchmittTriggerDetector(pybind11::module& module);
 
+// FloatingBaseEstimators.cpp
+void CreateKinDynComputations(pybind11::module& module);
+void CreateKinDynComputationsDescriptor(pybind11::module& module);
+void CreateFloatingBaseEstimator(pybind11::module& module);
+void CreateLeggedOdometry(pybind11::module& module);
+
 } // namespace BipedalLocomotion::bindings

bindings/python/tests/CMakeLists.txt

@@ -6,6 +6,9 @@
 # folder with the build-tree Python package. We use it as working directory so that
 # the tests always load the Python package from the build-tree even if BFL is installed
 # elsewhere.
+
+configure_file(${CMAKE_CURRENT_SOURCE_DIR}/model.urdf ${PROJECT_BINARY_DIR}/model.urdf COPYONLY)
+
 add_test(NAME PythonBindingsUnitTest
  COMMAND ${Python3_EXECUTABLE} -m pytest ${CMAKE_CURRENT_SOURCE_DIR}
  WORKING_DIRECTORY ${PROJECT_BINARY_DIR}