Implement ISensorBridge python bindings

CHANGELOG.md

@@ -4,6 +4,7 @@ All notable changes to this project are documented in this file.
 ## [Unreleased]
 ### Added
 - Implement IRobotControl python bindings (https://github.com/dic-iit/bipedal-locomotion-framework/pull/200)
+- Implement ISensorBridge python bindings (https://github.com/dic-iit/bipedal-locomotion-framework/pull/203)
 
 ### Changed
 

bindings/python/RobotInterface.cpp

@@ -12,8 +12,11 @@
 #include <yarp/dev/PolyDriver.h>
 
 #include "BipedalLocomotion/RobotInterface/IRobotControl.h"
+#include "BipedalLocomotion/RobotInterface/ISensorBridge.h"
 #include "BipedalLocomotion/RobotInterface/YarpHelper.h"
 #include "BipedalLocomotion/RobotInterface/YarpRobotControl.h"
+#include "BipedalLocomotion/RobotInterface/YarpSensorBridge.h"
+#include "BipedalLocomotion/System/Advanceable.h"
 
 #include "bipedal_locomotion_framework.h"
 
@@ -104,3 +107,241 @@ void BipedalLocomotion::bindings::CreateYarpRobotControl(pybind11::module& modul
  py::arg("control_mode"))
  .def("get_joint_list", &YarpRobotControl::getJointList);
 }
+
+void BipedalLocomotion::bindings::CreateISensorBridge(pybind11::module& module)
+{
+ namespace py = ::pybind11;
+ using namespace BipedalLocomotion::RobotInterface;
+
+ py::class_<SensorBridgeOptions>(module, "SensorBridgeOptions")
+ .def(py::init())
+ .def_readwrite("is_kinematics_enabled", &SensorBridgeOptions::isKinematicsEnabled)
+ .def_readwrite("is_imu_enabled", &SensorBridgeOptions::isIMUEnabled)
+ .def_readwrite("is_linear_accelerometer_enabled",
+ &SensorBridgeOptions::isLinearAccelerometerEnabled)
+ .def_readwrite("is_gyroscope_enabled", &SensorBridgeOptions::isGyroscopeEnabled)
+ .def_readwrite("is_orientation_sensor_enabled",
+ &SensorBridgeOptions::isOrientationSensorEnabled)
+ .def_readwrite("is_magnetometer_enabled", &SensorBridgeOptions::isMagnetometerEnabled)
+ .def_readwrite("is_six_axis_force_torque_sensor_enabled",
+ &SensorBridgeOptions::isSixAxisForceTorqueSensorEnabled)
+ .def_readwrite("is_three_axis_force_enabled",
+ &SensorBridgeOptions::isThreeAxisForceTorqueSensorEnabled)
+ .def_readwrite("is_cartesian_wrench_enabled",
+ &SensorBridgeOptions::isCartesianWrenchEnabled)
+ .def_readwrite("nr_joints", &SensorBridgeOptions::nrJoints);
+
+ py::class_<SensorLists>(module, "SensorLists")
+ .def(py::init())
+ .def_readwrite("joints_list", &SensorLists::jointsList)
+ .def_readwrite("imus_list", &SensorLists::IMUsList)
+ .def_readwrite("linear_accelerometers_list", &SensorLists::linearAccelerometersList)
+ .def_readwrite("gyroscopes_list", &SensorLists::gyroscopesList)
+ .def_readwrite("orientation_sensors_list", &SensorLists::orientationSensorsList)
+ .def_readwrite("magnetometers_list", &SensorLists::magnetometersList)
+ .def_readwrite("six_axis_force_torque_sensors_list",
+ &SensorLists::sixAxisForceTorqueSensorsList)
+ .def_readwrite("three_axis_force_torque_sensors_list",
+ &SensorLists::threeAxisForceTorqueSensorsList)
+ .def_readwrite("cartesian_wrenches_list", &SensorLists::cartesianWrenchesList);
+
+ py::class_<SensorBridgeMetaData>(module, "SensorBridgeMetaData")
+ .def(py::init())
+ .def_readwrite("sensors_list", &SensorBridgeMetaData::sensorsList)
+ .def_readwrite("bridge_options", &SensorBridgeMetaData::bridgeOptions);
+
+ py::class_<ISensorBridge>(module, "ISensorBridge");
+}
+
+void BipedalLocomotion::bindings::CreateYarpSensorBridge(pybind11::module& module)
+{
+ namespace py = ::pybind11;
+ using namespace BipedalLocomotion::RobotInterface;
+ using namespace BipedalLocomotion::System;
+ using namespace BipedalLocomotion::ParametersHandler;
+
+ py::class_<Advanceable<SensorBridgeMetaData>>(module, "SensorBridgeMetaDataAdvanceable");
+
+ py::class_<YarpSensorBridge, ISensorBridge, Advanceable<SensorBridgeMetaData>>(module,
+ "YarpSensorBridge")
+ .def(py::init())
+ .def(
+ "initialize",
+ [](YarpSensorBridge& impl, std::shared_ptr<IParametersHandler> handler) -> bool {
+ return impl.initialize(handler);
+ },
+ py::arg("handler"))
+ .def(
+ "set_drivers_list",
+ [](YarpSensorBridge& impl, std::vector<PolyDriverDescriptor> polydrivers) -> bool {
+ yarp::dev::PolyDriverList list;
+ for (const auto& polydriver : polydrivers)
+ list.push(polydriver.poly.get(), polydriver.key.c_str());
+
+ return impl.setDriversList(list);
+ },
+ py::arg("polydrivers"))
+ .def("advance", &YarpSensorBridge::advance)
+ .def("is_valid", &YarpSensorBridge::isValid)
+ .def("get_failed_sensor_reads", &YarpSensorBridge::getFailedSensorReads)
+ .def("get_joints_list",
+ [](YarpSensorBridge& impl) {
+ std::vector<std::string> list;
+ bool ok = impl.getJointsList(list);
+ return std::make_tuple(ok, list);
+ })
+ .def("get_imus_list",
+ [](YarpSensorBridge& impl) {
+ std::vector<std::string> list;
+ bool ok = impl.getIMUsList(list);
+ return std::make_tuple(ok, list);
+ })
+ .def("get_gyroscopes_list",
+ [](YarpSensorBridge& impl) {
+ std::vector<std::string> list;
+ bool ok = impl.getGyroscopesList(list);
+ return std::make_tuple(ok, list);
+ })
+ .def("get_orientation_sensors_list",
+ [](YarpSensorBridge& impl) {
+ std::vector<std::string> list;
+ bool ok = impl.getOrientationSensorsList(list);
+ return std::make_tuple(ok, list);
+ })
+ .def("get_six_axis_force_torque_sensors_list",
+ [](YarpSensorBridge& impl) {
+ std::vector<std::string> list;
+ bool ok = impl.getSixAxisForceTorqueSensorsList(list);
+ return std::make_tuple(ok, list);
+ })
+ .def("get_cartesian_wrenches_list",
+ [](YarpSensorBridge& impl) {
+ std::vector<std::string> list;
+ bool ok = impl.getCartesianWrenchesList(list);
+ return std::make_tuple(ok, list);
+ })
+ .def(
+ "get_joint_position",
+ [](YarpSensorBridge& impl, const std::string& jointName) {
+ double joint, receiveTimeInSeconds;
+ bool ok = impl.getJointPosition(jointName, joint, &receiveTimeInSeconds);
+ return std::make_tuple(ok, joint, receiveTimeInSeconds);
+ },
+ py::arg("joint_name"))
+ .def("get_joint_positions",
+ [](YarpSensorBridge& impl) {
+ Eigen::VectorXd joints(impl.get().bridgeOptions.nrJoints);
+ double receiveTimeInSeconds;
+ bool ok = impl.getJointPositions(joints, &receiveTimeInSeconds);
+ return std::make_tuple(ok, joints, receiveTimeInSeconds);
+ })
+ .def(
+ "get_joint_velocity",
+ [](YarpSensorBridge& impl, const std::string& name) {
+ double joint, receiveTimeInSeconds;
+ bool ok = impl.getJointVelocity(name, joint, &receiveTimeInSeconds);
+ return std::make_tuple(ok, joint, receiveTimeInSeconds);
+ },
+ py::arg("joint_name"))
+ .def("get_joint_velocities",
+ [](YarpSensorBridge& impl) {
+ Eigen::VectorXd joints(impl.get().bridgeOptions.nrJoints);
+ double receiveTimeInSeconds;
+ bool ok = impl.getJointVelocities(joints, &receiveTimeInSeconds);
+ return std::make_tuple(ok, joints, receiveTimeInSeconds);
+ })
+ .def(
+ "get_imu_measurement",
+ [](YarpSensorBridge& impl, const std::string& name) {
+ Eigen::Matrix<double, 12, 1> measurement;
+ double receiveTimeInSeconds;
+ bool ok = impl.getIMUMeasurement(name, measurement, &receiveTimeInSeconds);
+ return std::make_tuple(ok, measurement, receiveTimeInSeconds);
+ },
+ py::arg("sensor_name"))
+ .def(
+ "get_linear_accelerometer_measurement",
+ [](YarpSensorBridge& impl, const std::string& name) {
+ Eigen::Vector3d measurement;
+ double receiveTimeInSeconds;
+ bool ok = impl.getLinearAccelerometerMeasurement(name,
+ measurement,
+ &receiveTimeInSeconds);
+ return std::make_tuple(ok, measurement, receiveTimeInSeconds);
+ },
+ py::arg("sensor_name"))
+ .def(
+ "get_gyroscope_measurement",
+ [](YarpSensorBridge& impl, const std::string& name) {
+ Eigen::Vector3d measurement;
+ double receiveTimeInSeconds;
+ bool ok = impl.getGyroscopeMeasure(name, measurement, &receiveTimeInSeconds);
+ return std::make_tuple(ok, measurement, receiveTimeInSeconds);
+ },
+ py::arg("sensor_name"))
+ .def(
+ "get_orientation_sensor_measurement",
+ [](YarpSensorBridge& impl, const std::string& name) {
+ Eigen::Vector3d measurement;
+ double receiveTimeInSeconds;
+ bool ok = impl.getOrientationSensorMeasurement(name,
+ measurement,
+ &receiveTimeInSeconds);
+ return std::make_tuple(ok, measurement, receiveTimeInSeconds);
+ },
+ py::arg("sensor_name"))
+ .def(
+ "get_magnetometer_measurement",
+ [](YarpSensorBridge& impl, const std::string& name) {
+ Eigen::Vector3d measurement;
+ double receiveTimeInSeconds;
+ bool ok = impl.getMagnetometerMeasurement(name, measurement, &receiveTimeInSeconds);
+ return std::make_tuple(ok, measurement, receiveTimeInSeconds);
+ },
+ py::arg("sensor_name"))
+ .def(
+ "get_six_axis_force_torque_measurement",
+ [](YarpSensorBridge& impl, const std::string& name) {
+ Eigen::Matrix<double, 6, 1> measurement;
+ double receiveTimeInSeconds;
+ bool ok = impl.getSixAxisForceTorqueMeasurement(name,
+ measurement,
+ &receiveTimeInSeconds);
+ return std::make_tuple(ok, measurement, receiveTimeInSeconds);
+ },
+ py::arg("sensor_name"))
+ .def(
+ "get_three_axis_force_torque_measurement",
+ [](YarpSensorBridge& impl, const std::string& name) {
+ Eigen::Vector3d measurement;
+ double receiveTimeInSeconds;
+ bool ok = impl.getThreeAxisForceTorqueMeasurement(name,
+ measurement,
+ &receiveTimeInSeconds);
+ return std::make_tuple(ok, measurement, receiveTimeInSeconds);
+ },
+ py::arg("sensor_name"))
+ .def(
+ "get_cartesian_wrench",
+ [](YarpSensorBridge& impl, const std::string& name) {
+ Eigen::Matrix<double, 6, 1> measurement;
+ double receiveTimeInSeconds;
+ bool ok = impl.getCartesianWrench(name, measurement, &receiveTimeInSeconds);
+ return std::make_tuple(ok, measurement, receiveTimeInSeconds);
+ },
+ py::arg("wrench_name"))
+ .def(
+ "get_motor_current",
+ [](YarpSensorBridge& impl, const std::string& jointName) {
+ double joint, receiveTimeInSeconds;
+ bool ok = impl.getMotorCurrent(jointName, joint, &receiveTimeInSeconds);
+ return std::make_tuple(ok, joint, receiveTimeInSeconds);
+ },
+ py::arg("motor_name"))
+ .def("get_motor_currents", [](YarpSensorBridge& impl) {
+ Eigen::VectorXd joints(impl.get().bridgeOptions.nrJoints);
+ double receiveTimeInSeconds;
+ bool ok = impl.getMotorCurrents(joints, &receiveTimeInSeconds);
+ return std::make_tuple(ok, joints, receiveTimeInSeconds);
+ });
+}

bindings/python/bipedal_locomotion_framework.cpp

@@ -45,6 +45,8 @@ PYBIND11_MODULE(bindings, m)
  bindings::CreatePolyDriverDescriptor(m);
  bindings::CreateIRobotControl(m);
  bindings::CreateYarpRobotControl(m);
+ bindings::CreateISensorBridge(m);
+ bindings::CreateYarpSensorBridge(m);
 
 }
 

bindings/python/bipedal_locomotion_framework.h

@@ -54,5 +54,7 @@ void CreatePolyDriver(pybind11::module& module);
 void CreatePolyDriverDescriptor(pybind11::module& module);
 void CreateIRobotControl(pybind11::module& module);
 void CreateYarpRobotControl(pybind11::module& module);
+void CreateISensorBridge(pybind11::module& module);
+void CreateYarpSensorBridge(pybind11::module& module);
 
 } // namespace BipedalLocomotion::bindings