diff --git a/CMakeLists.txt b/CMakeLists.txt index ee4ddfb..19d89fb 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -63,7 +63,7 @@ endif(BUILD_PYTHON_INTERFACE) # --- MAIN LIBRARY ------------------------------------------------------------- set(ODRI_CONTROL_INTERFACE_SRC src/joint_modules.cpp src/imu.cpp src/robot.cpp - src/calibration.cpp src/utils.cpp) + src/calibration.cpp src/utils.cpp src/powerboard.cpp) add_library(${PROJECT_NAME} SHARED ${ODRI_CONTROL_INTERFACE_SRC}) target_link_libraries(${PROJECT_NAME} yaml-cpp) target_link_libraries(${PROJECT_NAME} master_board_sdk::master_board_sdk) diff --git a/include/odri_control_interface/powerboard.hpp b/include/odri_control_interface/powerboard.hpp new file mode 100644 index 0000000..43efaf0 --- /dev/null +++ b/include/odri_control_interface/powerboard.hpp @@ -0,0 +1,56 @@ +/** + * @file powerboard.hpp + * @author Pierre-Alexandre Leziart (paleziart@laas.fr) + * license License BSD-3-Clause + * @copyright Copyright (c) 2022, New York University, Max Planck + * Gesellschaft and LAAS-CNRS + * @date 2022-04-19 + * + * @brief Powerboard abstraction. + */ + +#pragma once + +#include +#include +#include + +#include "master_board_sdk/defines.h" +#include "master_board_sdk/master_board_interface.h" + +#include + +namespace odri_control_interface +{ +/** + * @brief Class for dealing with the powerboard. + */ +class Powerboard +{ +protected: + std::shared_ptr robot_if_; + + // Cache for the results. + double current_; + double voltage_; + double energy_; + +public: + Powerboard(const std::shared_ptr& robot_if); + + // If needed, add some error handling for the powerboard as well. + // For instance, check for low voltage or high current + bool HasError() + { + return false; + } + + void ParseSensorData(); + + const std::shared_ptr& GetMasterBoardInterface(); + double GetCurrent(); + double GetVoltage(); + double GetEnergy(); +}; + +} // namespace odri_control_interface diff --git a/include/odri_control_interface/robot.hpp b/include/odri_control_interface/robot.hpp index e0b566f..dfd75e6 100644 --- a/include/odri_control_interface/robot.hpp +++ b/include/odri_control_interface/robot.hpp @@ -19,6 +19,7 @@ #include #include #include +#include namespace odri_control_interface { @@ -31,6 +32,7 @@ class Robot std::shared_ptr robot_if; std::shared_ptr joints; std::shared_ptr imu; + std::shared_ptr powerboard; std::shared_ptr calibrator; protected: @@ -43,6 +45,7 @@ class Robot Robot(const std::shared_ptr& robot_if, const std::shared_ptr& joint_modules, const std::shared_ptr& imu, + const std::shared_ptr& powerboard, const std::shared_ptr& calibrator); /** @@ -60,6 +63,11 @@ class Robot */ const std::shared_ptr& GetIMU(); + /** + * @brief Return the powerboard. + */ + const std::shared_ptr& GetPowerboard(); + /** * @brief Initializes the connection. Use `SendInit` to initialize the * session. diff --git a/src/powerboard.cpp b/src/powerboard.cpp new file mode 100644 index 0000000..508436f --- /dev/null +++ b/src/powerboard.cpp @@ -0,0 +1,50 @@ +/** + * @file powerboard.hpp + * @author Pierre-Alexandre Leziart (paleziart@laas.fr) + * license License BSD-3-Clause + * @copyright Copyright (c) 2020, New York University and Max Planck + * Gesellschaft. + * @date 2021-08-23 + * + * @brief Powerboard abstraction. + */ + +#include "odri_control_interface/powerboard.hpp" + +#include + +namespace odri_control_interface +{ +Powerboard::Powerboard(const std::shared_ptr& robot_if) + : robot_if_(robot_if) +{ +} + +const std::shared_ptr& Powerboard::GetMasterBoardInterface() +{ + return robot_if_; +} + +double Powerboard::GetCurrent() +{ + return current_; +} + +double Powerboard::GetVoltage() +{ + return voltage_; +} + +double Powerboard::GetEnergy() +{ + return energy_; +} + +void Powerboard::ParseSensorData() +{ + current_ = robot_if_->powerboard_current(); + voltage_ = robot_if_->powerboard_voltage(); + energy_ = robot_if_->powerboard_energy(); +} + +} // namespace odri_control_interface diff --git a/src/robot.cpp b/src/robot.cpp index 159c00d..bba7100 100644 --- a/src/robot.cpp +++ b/src/robot.cpp @@ -18,9 +18,10 @@ namespace odri_control_interface Robot::Robot(const std::shared_ptr& robot_if, const std::shared_ptr& joint_modules, const std::shared_ptr& imu, + const std::shared_ptr& powerboard, const std::shared_ptr& calibrator) : robot_if(robot_if), joints(joint_modules), imu(imu), - calibrator(calibrator), timeout_counter_(0), saw_error_(false) + powerboard(powerboard), calibrator(calibrator), timeout_counter_(0), saw_error_(false) { last_time_ = std::chrono::system_clock::now(); } @@ -40,6 +41,11 @@ const std::shared_ptr& Robot::GetIMU() return imu; } +const std::shared_ptr& Robot::GetPowerboard() +{ + return powerboard; +} + void Robot::Init() { // Init the robot. @@ -137,6 +143,10 @@ void Robot::ParseSensorData() { imu->ParseSensorData(); } + if (powerboard) + { + powerboard->ParseSensorData(); + } } bool Robot::RunCalibration(const std::shared_ptr& calibrator, @@ -264,6 +274,10 @@ bool Robot::HasError() { saw_error_ |= imu->HasError(); } + if (powerboard) + { + saw_error_ |= powerboard->HasError(); + } if (robot_if->IsTimeout()) { diff --git a/src/utils.cpp b/src/utils.cpp index 1e407cb..7133ad3 100644 --- a/src/utils.cpp +++ b/src/utils.cpp @@ -275,13 +275,18 @@ std::shared_ptr RobotFromYamlFile(const std::string& if_name, assert_yaml_parsing(robot_node, "robot", "imu"); std::shared_ptr imu = IMUFromYaml(robot_if, robot_node["imu"]); - // 4. Create the calibrator procedure. + // 4. Create the powerboard + std::shared_ptr powerboard = + std::make_shared(robot_if); + + // 5. Create the calibrator procedure. assert_yaml_parsing(param, file_path, "joint_calibrator"); std::shared_ptr calibrator = JointCalibratorFromYaml(joints, param["joint_calibrator"]); - // 5. Create the robot instance from the objects. - return std::make_shared(robot_if, joints, imu, calibrator); + // 6. Create the robot instance from the objects. + return std::make_shared(robot_if, joints, imu, + powerboard, calibrator); } std::shared_ptr RobotFromYamlFile(const std::string& file_path) diff --git a/srcpy/bindings.cpp b/srcpy/bindings.cpp index f37c685..a3f6d24 100644 --- a/srcpy/bindings.cpp +++ b/srcpy/bindings.cpp @@ -205,6 +205,18 @@ BOOST_PYTHON_MODULE(libodri_control_interface_pywrap) return_value_policy())); register_ptr_to_python>(); + // Powerboard bindings and it's std::shared_ptr. + class_("Powerboard", init>()) + .add_property("has_error", &Powerboard::HasError) + .add_property( + "robot_interface", + make_function(&Powerboard::GetMasterBoardInterface, + return_value_policy())) + .add_property("current", &Powerboard::GetCurrent) + .add_property("voltage", &Powerboard::GetVoltage) + .add_property("energy", &Powerboard::GetEnergy); + register_ptr_to_python>(); + // CalibrationMethod enum bindings. enum_("CalibrationMethod") .value("auto", CalibrationMethod::AUTO) @@ -220,6 +232,7 @@ BOOST_PYTHON_MODULE(libodri_control_interface_pywrap) init, std::shared_ptr, std::shared_ptr, + std::shared_ptr, std::shared_ptr>()) .def("init", &Robot::Init) .def("sendInit", &Robot::SendInit) @@ -246,6 +259,10 @@ BOOST_PYTHON_MODULE(libodri_control_interface_pywrap) "imu", make_function(&Robot::GetIMU, return_value_policy())) + .add_property( + "powerboard", + make_function(&Robot::GetPowerboard, + return_value_policy())) .add_property("is_ready", &Robot::IsReady) .add_property("is_timeout", &Robot::IsTimeout) .add_property("is_ack_msg_received", &Robot::IsAckMsgReceived)