From 45c798504bce5da43a8d8e0b306cdb267b5fd369 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Tue, 23 Feb 2021 14:52:16 +0100 Subject: [PATCH 1/2] Implement ISensorBridge python bindings --- bindings/python/RobotInterface.cpp | 241 ++++++++++++++++++ .../python/bipedal_locomotion_framework.cpp | 2 + .../python/bipedal_locomotion_framework.h | 2 + 3 files changed, 245 insertions(+) diff --git a/bindings/python/RobotInterface.cpp b/bindings/python/RobotInterface.cpp index e178b9e52d..ea62e5c332 100644 --- a/bindings/python/RobotInterface.cpp +++ b/bindings/python/RobotInterface.cpp @@ -12,8 +12,11 @@ #include #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_(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_(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_(module, "SensorBridgeMetaData") + .def(py::init()) + .def_readwrite("sensors_list", &SensorBridgeMetaData::sensorsList) + .def_readwrite("bridge_options", &SensorBridgeMetaData::bridgeOptions); + + py::class_(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_>(module, "SensorBridgeMetaDataAdvanceable"); + + py::class_>(module, + "YarpSensorBridge") + .def(py::init()) + .def( + "initialize", + [](YarpSensorBridge& impl, std::shared_ptr handler) -> bool { + return impl.initialize(handler); + }, + py::arg("handler")) + .def( + "set_drivers_list", + [](YarpSensorBridge& impl, std::vector 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 list; + bool ok = impl.getJointsList(list); + return std::make_tuple(ok, list); + }) + .def("get_imus_list", + [](YarpSensorBridge& impl) { + std::vector list; + bool ok = impl.getIMUsList(list); + return std::make_tuple(ok, list); + }) + .def("get_gyroscopes_list", + [](YarpSensorBridge& impl) { + std::vector list; + bool ok = impl.getGyroscopesList(list); + return std::make_tuple(ok, list); + }) + .def("get_orientation_sensors_list", + [](YarpSensorBridge& impl) { + std::vector list; + bool ok = impl.getOrientationSensorsList(list); + return std::make_tuple(ok, list); + }) + .def("get_six_axis_force_torque_sensors_list", + [](YarpSensorBridge& impl) { + std::vector list; + bool ok = impl.getSixAxisForceTorqueSensorsList(list); + return std::make_tuple(ok, list); + }) + .def("get_cartesian_wrenches_list", + [](YarpSensorBridge& impl) { + std::vector 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 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 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 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); + }); +} diff --git a/bindings/python/bipedal_locomotion_framework.cpp b/bindings/python/bipedal_locomotion_framework.cpp index 171a8f781b..bdb179af0d 100644 --- a/bindings/python/bipedal_locomotion_framework.cpp +++ b/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); } diff --git a/bindings/python/bipedal_locomotion_framework.h b/bindings/python/bipedal_locomotion_framework.h index 487a8c7639..671654b5b9 100644 --- a/bindings/python/bipedal_locomotion_framework.h +++ b/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 From a8500e21c2c52bd9466a8e0b0cf96ae5d2961265 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Tue, 23 Feb 2021 14:54:51 +0100 Subject: [PATCH 2/2] Update the CHANGELOG.md --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 473693099b..9e3e183ef3 100644 --- a/CHANGELOG.md +++ b/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