Skip to content

Commit

Permalink
Update python bindings accordingly to the Advanceable refactoring
Browse files Browse the repository at this point in the history
  • Loading branch information
GiulioRomualdi committed Mar 31, 2021
1 parent f903e45 commit 691c74b
Show file tree
Hide file tree
Showing 9 changed files with 46 additions and 43 deletions.
13 changes: 6 additions & 7 deletions bindings/python/Contacts/src/ContactDetectors.cpp
Expand Up @@ -28,8 +28,8 @@ void CreateContactDetector(pybind11::module& module)
using namespace BipedalLocomotion::System;
using namespace BipedalLocomotion::ParametersHandler;

py::class_<Advanceable<EstimatedContactList>>(module, "EstimatedContactListAdvanceable");
py::class_<ContactDetector, Advanceable<EstimatedContactList>>(module, "ContactDetector");
py::class_<Source<EstimatedContactList>>(module, "EstimatedContactListSource");
py::class_<ContactDetector, Source<EstimatedContactList>>(module, "ContactDetector");
}

void CreateSchmittTriggerUnit(pybind11::module& module)
Expand Down Expand Up @@ -82,17 +82,16 @@ void CreateSchmittTriggerDetector(pybind11::module& module)
.def(py::init())
.def(
"initialize",
[](SchmittTriggerDetector& impl, std::shared_ptr<IParametersHandler> handler) -> bool {
return impl.initialize(handler);
},
[](SchmittTriggerDetector& impl, std::shared_ptr<const IParametersHandler> handler)
-> bool { return impl.initialize(handler); },
py::arg("handler"))
.def("advance", &SchmittTriggerDetector::advance)
.def("reset_contacts", &SchmittTriggerDetector::resetContacts)
.def("get", py::overload_cast<>(&SchmittTriggerDetector::get, py::const_))
.def("get_output", &SchmittTriggerDetector::getOutput)
.def("get",
py::overload_cast<const std::string&>(&SchmittTriggerDetector::get, py::const_),
py::arg("contact_name"))
.def("is_valid", &SchmittTriggerDetector::isValid)
.def("is_output_valid", &SchmittTriggerDetector::isOutputValid)
.def("set_timed_trigger_input",
&SchmittTriggerDetector::setTimedTriggerInput,
py::arg("contact_name"),
Expand Down
Expand Up @@ -54,7 +54,7 @@ def test_schmitt_trigger_detector():
params.switch_off_after = 0.2
params.switch_on_after = 0.2
detector.add_contact("left", False, params, 0.6)
contacts = detector.get()
contacts = detector.get_output()
assert(len(contacts) == 2)
assert(contacts["right"].is_active == False)

Expand All @@ -71,7 +71,7 @@ def test_schmitt_trigger_detector():

# test removing a contact
assert(detector.remove_contact("left"))
contacts = detector.get()
contacts = detector.get_output()
assert(len(contacts) == 1)

# test resetting a contact
Expand Down
Expand Up @@ -124,11 +124,10 @@ void CreateFloatingBaseEstimator(pybind11::module& module)
.def_readwrite("forward_kinematics_noise", &SensorsStdDev::forwardKinematicsNoise)
.def_readwrite("encoders_noise", &SensorsStdDev::encodersNoise);

py::class_<Advanceable<Output>>(module, "FloatingBaseEstimatorOutputAdvanceable");
py::class_<Source<Output>>(module, "FloatingBaseEstimatorOutputSource");

py::class_<FloatingBaseEstimator, Advanceable<Output>> floatingBaseEstimator(module,
"FloatingBaseEstim"
"ator");
py::class_<FloatingBaseEstimator, Source<Output>> floatingBaseEstimator(module,
"FloatingBaseEstimator");

floatingBaseEstimator.def(py::init())
.def("set_imu_measurement",
Expand All @@ -154,8 +153,8 @@ void CreateFloatingBaseEstimator(pybind11::module& module)
&FloatingBaseEstimator::resetEstimator),
py::arg("new_base_orientation"),
py::arg("new_base_position"))
.def("get", &FloatingBaseEstimator::get)
.def("is_valid", &FloatingBaseEstimator::isValid)
.def("get_output", &FloatingBaseEstimator::getOutput)
.def("is_output_valid", &FloatingBaseEstimator::isOutputValid)
.def(
"initialize",
[](FloatingBaseEstimator& impl,
Expand Down
4 changes: 2 additions & 2 deletions bindings/python/FloatingBaseEstimators/src/LeggedOdometry.cpp
Expand Up @@ -83,8 +83,8 @@ void CreateLeggedOdometry(pybind11::module& module)
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);
.def("get_output", &LeggedOdometry::getOutput)
.def("is_output_valid", &LeggedOdometry::isOutputValid);
}

} // namespace FloatingBaseEstimators
Expand Down
Expand Up @@ -91,6 +91,6 @@ def test_legged_odometry():
assert legged_odom.advance()

# sample asserts for show-casing outputs
out = legged_odom.get()
out = legged_odom.get_output()
assert len(out.base_twist) == 6
assert len(out.state.imu_linear_velocity) == 3
6 changes: 3 additions & 3 deletions bindings/python/Planners/src/DCMPlanner.cpp
Expand Up @@ -10,7 +10,7 @@
#include <pybind11/stl.h>

#include <BipedalLocomotion/Planners/DCMPlanner.h>
#include <BipedalLocomotion/System/Advanceable.h>
#include <BipedalLocomotion/System/Source.h>
#include <BipedalLocomotion/bindings/Planners/DCMPlanner.h>

namespace BipedalLocomotion
Expand All @@ -34,9 +34,9 @@ void CreateDCMPlanner(pybind11::module& module)
.def_readwrite("omega", &DCMPlannerState::omega)
.def_readwrite("omega_dot", &DCMPlannerState::omegaDot);

py::class_<Advanceable<DCMPlannerState>>(module, "DCMPlannerStateAdvanceable");
py::class_<Source<DCMPlannerState>>(module, "DCMPlannerStateSource");

py::class_<DCMPlanner, Advanceable<DCMPlannerState>>(module, "DCMPlanner")
py::class_<DCMPlanner, Source<DCMPlannerState>>(module, "DCMPlanner")
.def("set_initial_state", &DCMPlanner::setInitialState, py::arg("state"))
.def("set_contact_phase_list",
&DCMPlanner::setContactPhaseList,
Expand Down
19 changes: 13 additions & 6 deletions bindings/python/Planners/src/SwingFootPlanner.cpp
Expand Up @@ -9,8 +9,9 @@
#include <pybind11/pybind11.h>
#include <pybind11/stl.h>

#include <BipedalLocomotion/ParametersHandler/IParametersHandler.h>
#include <BipedalLocomotion/Planners/SwingFootPlanner.h>
#include <BipedalLocomotion/System/Advanceable.h>
#include <BipedalLocomotion/System/Source.h>

#include <BipedalLocomotion/bindings/Planners/SwingFootPlanner.h>

Expand All @@ -26,6 +27,7 @@ void CreateSwingFootPlanner(pybind11::module& module)
namespace py = ::pybind11;
using namespace BipedalLocomotion::Planners;
using namespace BipedalLocomotion::System;
using namespace BipedalLocomotion::ParametersHandler;

py::class_<SwingFootPlannerState>(module, "SwingFootPlannerState")
.def(py::init())
Expand All @@ -52,14 +54,19 @@ void CreateSwingFootPlanner(pybind11::module& module)
s.mixedAcceleration.coeffs() = coeffs;
});

py::class_<Advanceable<SwingFootPlannerState>>(module, "SwingFootPlannerStateAdvanceable");
py::class_<Source<SwingFootPlannerState>>(module, "SwingFootPlannerStateSource");

py::class_<SwingFootPlanner, Advanceable<SwingFootPlannerState>>(module, "SwingFootPlanner")
py::class_<SwingFootPlanner, Source<SwingFootPlannerState>>(module, "SwingFootPlanner")
.def(py::init())
.def("initialize", &SwingFootPlanner::initialize, py::arg("handler"))
.def(
"initialize",
[](SwingFootPlanner& impl, std::shared_ptr<const IParametersHandler> handler) -> bool {
return impl.initialize(handler);
},
py::arg("handler"))
.def("set_contact_list", &SwingFootPlanner::setContactList, py::arg("contact_list"))
.def("get", &SwingFootPlanner::get)
.def("is_valid", &SwingFootPlanner::isValid)
.def("get_output", &SwingFootPlanner::getOutput)
.def("is_output_valid", &SwingFootPlanner::isOutputValid)
.def("advance", &SwingFootPlanner::advance);
}

Expand Down
11 changes: 5 additions & 6 deletions bindings/python/Planners/src/TimeVaryingDCMPlanner.cpp
Expand Up @@ -11,7 +11,7 @@

#include <BipedalLocomotion/Planners/DCMPlanner.h>
#include <BipedalLocomotion/Planners/TimeVaryingDCMPlanner.h>
#include <BipedalLocomotion/System/Advanceable.h>
#include <BipedalLocomotion/System/Source.h>
#include <BipedalLocomotion/bindings/Planners/TimeVaryingDCMPlanner.h>

namespace BipedalLocomotion
Expand All @@ -31,12 +31,12 @@ void CreateTimeVaryingDCMPlanner(pybind11::module& module)
.def(
"initialize",
[](TimeVaryingDCMPlanner& impl,
std::shared_ptr<BipedalLocomotion::ParametersHandler::IParametersHandler> handler)
-> bool { return impl.initialize(handler); },
std::shared_ptr<const BipedalLocomotion::ParametersHandler::IParametersHandler>
handler) -> bool { return impl.initialize(handler); },
py::arg("handler"))
.def("compute_trajectory", &TimeVaryingDCMPlanner::computeTrajectory)
.def("get", &TimeVaryingDCMPlanner::get)
.def("is_valid", &TimeVaryingDCMPlanner::isValid)
.def("get_output", &TimeVaryingDCMPlanner::getOutput)
.def("is_output_valid", &TimeVaryingDCMPlanner::isOutputValid)
.def("advance", &TimeVaryingDCMPlanner::advance)
.def("set_contact_phase_list",
&TimeVaryingDCMPlanner::setContactPhaseList,
Expand All @@ -46,7 +46,6 @@ void CreateTimeVaryingDCMPlanner(pybind11::module& module)
py::arg("dcm_reference_matrix"))
.def("set_initial_state", &TimeVaryingDCMPlanner::setInitialState, py::arg("state"));
}

}
}
}
19 changes: 9 additions & 10 deletions bindings/python/RobotInterface/src/SensorBridge.cpp
Expand Up @@ -14,7 +14,7 @@

#include <BipedalLocomotion/RobotInterface/YarpHelper.h>

#include <BipedalLocomotion/System/Advanceable.h>
#include <BipedalLocomotion/System/Source.h>

#include <BipedalLocomotion/bindings/RobotInterface/SensorBridge.h>

Expand Down Expand Up @@ -77,15 +77,14 @@ void CreateYarpSensorBridge(pybind11::module& module)
using namespace BipedalLocomotion::System;
using namespace BipedalLocomotion::ParametersHandler;

py::class_<Advanceable<SensorBridgeMetaData>>(module, "SensorBridgeMetaDataAdvanceable");
py::class_<Source<SensorBridgeMetaData>>(module, "SensorBridgeMetaDataSource");

py::class_<YarpSensorBridge, ISensorBridge, Advanceable<SensorBridgeMetaData>>(module,
"YarpSensorBridg"
"e")
py::class_<YarpSensorBridge, ISensorBridge, Source<SensorBridgeMetaData>>(module,
"YarpSensorBridge")
.def(py::init())
.def(
"initialize",
[](YarpSensorBridge& impl, std::shared_ptr<IParametersHandler> handler) -> bool {
[](YarpSensorBridge& impl, std::shared_ptr<const IParametersHandler> handler) -> bool {
return impl.initialize(handler);
},
py::arg("handler"))
Expand All @@ -100,7 +99,7 @@ void CreateYarpSensorBridge(pybind11::module& module)
},
py::arg("polydrivers"))
.def("advance", &YarpSensorBridge::advance)
.def("is_valid", &YarpSensorBridge::isValid)
.def("is_output_valid", &YarpSensorBridge::isOutputValid)
.def("get_failed_sensor_reads", &YarpSensorBridge::getFailedSensorReads)
.def("get_joints_list",
[](YarpSensorBridge& impl) {
Expand Down Expand Up @@ -148,7 +147,7 @@ void CreateYarpSensorBridge(pybind11::module& module)
py::arg("joint_name"))
.def("get_joint_positions",
[](YarpSensorBridge& impl) {
Eigen::VectorXd joints(impl.get().bridgeOptions.nrJoints);
Eigen::VectorXd joints(impl.getOutput().bridgeOptions.nrJoints);
double receiveTimeInSeconds;
bool ok = impl.getJointPositions(joints, receiveTimeInSeconds);
return std::make_tuple(ok, joints, receiveTimeInSeconds);
Expand All @@ -163,7 +162,7 @@ void CreateYarpSensorBridge(pybind11::module& module)
py::arg("joint_name"))
.def("get_joint_velocities",
[](YarpSensorBridge& impl) {
Eigen::VectorXd joints(impl.get().bridgeOptions.nrJoints);
Eigen::VectorXd joints(impl.getOutput().bridgeOptions.nrJoints);
double receiveTimeInSeconds;
bool ok = impl.getJointVelocities(joints, receiveTimeInSeconds);
return std::make_tuple(ok, joints, receiveTimeInSeconds);
Expand Down Expand Up @@ -256,7 +255,7 @@ void CreateYarpSensorBridge(pybind11::module& module)
},
py::arg("motor_name"))
.def("get_motor_currents", [](YarpSensorBridge& impl) {
Eigen::VectorXd joints(impl.get().bridgeOptions.nrJoints);
Eigen::VectorXd joints(impl.getOutput().bridgeOptions.nrJoints);
double receiveTimeInSeconds;
bool ok = impl.getMotorCurrents(joints, receiveTimeInSeconds);
return std::make_tuple(ok, joints, receiveTimeInSeconds);
Expand Down

0 comments on commit 691c74b

Please sign in to comment.