Add support of the new power-board hardware

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)

include/odri_control_interface/powerboard.hpp

@@ -0,0 +1,56 @@
+/**
+ * @file powerboard.hpp
+ * @author Pierre-Alexandre Leziart ([email protected])
+ * 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 <math.h>
+#include <unistd.h>
+#include <stdexcept>
+
+#include "master_board_sdk/defines.h"
+#include "master_board_sdk/master_board_interface.h"
+
+#include <odri_control_interface/common.hpp>
+
+namespace odri_control_interface
+{
+/**
+ * @brief Class for dealing with the powerboard.
+ */
+class Powerboard
+{
+protected:
+    std::shared_ptr<MasterBoardInterface> robot_if_;
+
+    // Cache for the results.
+    double current_;
+    double voltage_;
+    double energy_;
+
+public:
+    Powerboard(const std::shared_ptr<MasterBoardInterface>& 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<MasterBoardInterface>& GetMasterBoardInterface();
+    double GetCurrent();
+    double GetVoltage();
+    double GetEnergy();
+};
+
+}  // namespace odri_control_interface

include/odri_control_interface/robot.hpp

@@ -19,6 +19,7 @@
 #include <odri_control_interface/calibration.hpp>
 #include <odri_control_interface/imu.hpp>
 #include <odri_control_interface/joint_modules.hpp>
+#include <odri_control_interface/powerboard.hpp>
 
 namespace odri_control_interface
 {
@@ -31,6 +32,7 @@ class Robot
     std::shared_ptr<MasterBoardInterface> robot_if;
     std::shared_ptr<JointModules> joints;
     std::shared_ptr<IMU> imu;
+    std::shared_ptr<Powerboard> powerboard;
     std::shared_ptr<JointCalibrator> calibrator;
 
 protected:
@@ -43,6 +45,7 @@ class Robot
     Robot(const std::shared_ptr<MasterBoardInterface>& robot_if,
           const std::shared_ptr<JointModules>& joint_modules,
           const std::shared_ptr<IMU>& imu,
+          const std::shared_ptr<Powerboard>& powerboard,
           const std::shared_ptr<JointCalibrator>& calibrator);
 
     /**
@@ -60,6 +63,11 @@ class Robot
      */
     const std::shared_ptr<IMU>& GetIMU();
 
+    /**
+     * @brief Return the powerboard.
+     */
+    const std::shared_ptr<Powerboard>& GetPowerboard();
+
     /**
      * @brief Initializes the connection. Use `SendInit` to initialize the
      * session.

src/powerboard.cpp

@@ -0,0 +1,50 @@
+/**
+ * @file powerboard.hpp
+ * @author Pierre-Alexandre Leziart ([email protected])
+ * 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 <iostream>
+
+namespace odri_control_interface
+{
+Powerboard::Powerboard(const std::shared_ptr<MasterBoardInterface>& robot_if)
+    : robot_if_(robot_if)
+{
+}
+
+const std::shared_ptr<MasterBoardInterface>& 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

src/robot.cpp

@@ -18,9 +18,10 @@ namespace odri_control_interface
 Robot::Robot(const std::shared_ptr<MasterBoardInterface>& robot_if,
              const std::shared_ptr<JointModules>& joint_modules,
              const std::shared_ptr<IMU>& imu,
+             const std::shared_ptr<Powerboard>& powerboard,
              const std::shared_ptr<JointCalibrator>& 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<IMU>& Robot::GetIMU()
     return imu;
 }
 
+const std::shared_ptr<Powerboard>& 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<JointCalibrator>& calibrator,
@@ -264,6 +274,10 @@ bool Robot::HasError()
     {
         saw_error_ |= imu->HasError();
     }
+    if (powerboard)
+    {
+        saw_error_ |= powerboard->HasError();
+    }
 
     if (robot_if->IsTimeout())
     {

src/utils.cpp

@@ -275,13 +275,18 @@ std::shared_ptr<Robot> RobotFromYamlFile(const std::string& if_name,
     assert_yaml_parsing(robot_node, "robot", "imu");
     std::shared_ptr<IMU> imu = IMUFromYaml(robot_if, robot_node["imu"]);
 
-    // 4. Create the calibrator procedure.
+    // 4. Create the powerboard
+    std::shared_ptr<Powerboard> powerboard =
+        std::make_shared<Powerboard>(robot_if);
+
+    // 5. Create the calibrator procedure.
     assert_yaml_parsing(param, file_path, "joint_calibrator");
     std::shared_ptr<JointCalibrator> calibrator =
         JointCalibratorFromYaml(joints, param["joint_calibrator"]);
 
-    // 5. Create the robot instance from the objects.
-    return std::make_shared<Robot>(robot_if, joints, imu, calibrator);
+    // 6. Create the robot instance from the objects.
+    return std::make_shared<Robot>(robot_if, joints, imu,
+                                   powerboard, calibrator);
 }
 
 std::shared_ptr<Robot> RobotFromYamlFile(const std::string& file_path)

srcpy/bindings.cpp

@@ -205,6 +205,18 @@ BOOST_PYTHON_MODULE(libodri_control_interface_pywrap)
                           return_value_policy<copy_const_reference>()));
     register_ptr_to_python<std::shared_ptr<IMU>>();
 
+    // Powerboard bindings and it's std::shared_ptr.
+    class_<Powerboard>("Powerboard", init<std::shared_ptr<MasterBoardInterface>>())
+        .add_property("has_error", &Powerboard::HasError)
+        .add_property(
+            "robot_interface",
+            make_function(&Powerboard::GetMasterBoardInterface,
+                          return_value_policy<copy_const_reference>()))
+        .add_property("current", &Powerboard::GetCurrent)
+        .add_property("voltage", &Powerboard::GetVoltage)
+        .add_property("energy", &Powerboard::GetEnergy);
+    register_ptr_to_python<std::shared_ptr<Powerboard>>();
+
     // CalibrationMethod enum bindings.
     enum_<CalibrationMethod>("CalibrationMethod")
         .value("auto", CalibrationMethod::AUTO)
@@ -220,6 +232,7 @@ BOOST_PYTHON_MODULE(libodri_control_interface_pywrap)
                   init<std::shared_ptr<MasterBoardInterface>,
                        std::shared_ptr<JointModules>,
                        std::shared_ptr<IMU>,
+                       std::shared_ptr<Powerboard>,
                        std::shared_ptr<JointCalibrator>>())
         .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<copy_const_reference>()))
+        .add_property(
+            "powerboard",
+            make_function(&Robot::GetPowerboard,
+                          return_value_policy<copy_const_reference>()))
         .add_property("is_ready", &Robot::IsReady)
         .add_property("is_timeout", &Robot::IsTimeout)
         .add_property("is_ack_msg_received", &Robot::IsAckMsgReceived)