Implement the MotorsTemperatureChecker and the logic to prevent the robot to walk when the temperature is greather then a value

Author: GiulioRomualdi

src/RobotInterface/CMakeLists.txt

@@ -4,7 +4,7 @@
 
 add_walking_controllers_library(
   NAME RobotInterface
-  SOURCES src/Helper.cpp src/PIDHandler.cpp
-  PUBLIC_HEADERS include/WalkingControllers/RobotInterface/Helper.h include/WalkingControllers/RobotInterface/PIDHandler.h
+  SOURCES src/Helper.cpp src/PIDHandler.cpp src/MotorTemperatureChecker.cpp
+  PUBLIC_HEADERS include/WalkingControllers/RobotInterface/Helper.h include/WalkingControllers/RobotInterface/PIDHandler.h include/WalkingControllers/RobotInterface/MotorTemperatureChecker.h
   PUBLIC_LINK_LIBRARIES WalkingControllers::YarpUtilities WalkingControllers::iDynTreeUtilities ctrlLib
   PRIVATE_LINK_LIBRARIES Eigen3::Eigen)

src/RobotInterface/include/WalkingControllers/RobotInterface/MotorTemperatureChecker.h

@@ -0,0 +1,33 @@
+// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT)
+// SPDX-License-Identifier: BSD-3-Clause
+
+#ifndef WALKING_CONTROLLERS_MOTOR_TEMPERATURE_CHECKER
+#define WALKING_CONTROLLERS_MOTOR_TEMPERATURE_CHECKER
+
+// std
+#include <vector>
+
+#include <iDynTree/VectorDynSize.h>
+
+#include <yarp/os/Searchable.h>
+
+namespace WalkingControllers
+{
+    class MotorsTemperatureChecker
+    {
+    private:
+        std::vector<double> m_maxTemperature;
+        std::vector<unsigned int> m_samplesAboveTheLimit;
+        iDynTree::VectorDynSize m_limits;
+        int m_maxNumberOfSampleAboveTheLimits;
+
+    public:
+
+        const std::vector<double>& getMaxTemperature() const;
+        std::vector<unsigned int> getMotorsOverLimit() const;
+        bool isThereAMotorOverLimit() const;
+        bool setMotorTemperatures(const iDynTree::VectorDynSize& temperature);
+        bool configure(const yarp::os::Searchable& config, int dofs);
+    };
+};
+#endif

src/RobotInterface/src/MotorTemperatureChecker.cpp

@@ -0,0 +1,111 @@
+// SPDX-FileCopyrightText: Fondazione Istituto Italiano di Tecnologia (IIT)
+// SPDX-License-Identifier: BSD-3-Clause
+
+#include <yarp/os/LogStream.h>
+
+#include <WalkingControllers/RobotInterface/MotorTemperatureChecker.h>
+#include <WalkingControllers/YarpUtilities/Helper.h>
+
+using namespace WalkingControllers;
+
+bool MotorsTemperatureChecker::configure(const yarp::os::Searchable& config, int dofs)
+{
+    m_limits.resize(dofs);
+    if(!YarpUtilities::getVectorFromSearchable(config, "temperature_limits", m_limits))
+    {
+        yError() << "[MotorsTemperatureChecker::configure] Unable to get the parameter temperature limits";
+        return false;
+    }
+
+    m_samplesAboveTheLimit = std::vector<unsigned int>(dofs, 0);
+
+    if(!YarpUtilities::getNumberFromSearchable(config, "max_samples_above_the_limits", m_maxNumberOfSampleAboveTheLimits))
+    {
+        yError() << "[MotorsTemperatureChecker::configure] Unable to get the parameter 'max_samples_above_the_limits'";
+        return false;
+    }
+
+    m_maxTemperature.resize(dofs);
+    for (int i = 0; i < dofs; i++)
+    {
+        m_maxTemperature[i] = 0;
+    }
+
+    return true;
+}
+
+bool MotorsTemperatureChecker::isThereAMotorOverLimit() const
+{
+    if (m_maxNumberOfSampleAboveTheLimits < 0)
+    {
+        return false;
+    }
+
+    for (const auto sample : m_samplesAboveTheLimit)
+    {
+        if (sample > m_maxNumberOfSampleAboveTheLimits)
+        {
+            return true;
+        }
+    }
+    return false;
+}
+
+std::vector<unsigned int> MotorsTemperatureChecker::getMotorsOverLimit() const
+{
+    std::vector<unsigned int>  indeces;
+
+    if (m_maxNumberOfSampleAboveTheLimits < 0)
+    {
+        return indeces;
+    }
+
+    for (int i =0; i < m_samplesAboveTheLimit.size(); i++)
+    {
+        if (m_samplesAboveTheLimit[i] > m_maxNumberOfSampleAboveTheLimits)
+        {
+            indeces.push_back(i);
+        }
+    }
+    return indeces;
+}
+
+bool MotorsTemperatureChecker::setMotorTemperatures(const iDynTree::VectorDynSize& temperature)
+{
+    if (temperature.size() != m_limits.size())
+    {
+        yError() << "[MotorsTemperatureChecker::setMotorTemperatures] Unexpected size of the temperature vector"
+                 << "provided: " << temperature.size() << " expected: " << m_limits.size();
+        return false;
+    }
+
+    if (m_maxNumberOfSampleAboveTheLimits < 0)
+    {
+        return true;
+    }
+
+    for (int i = 0; i < temperature.size(); i++)
+    {
+
+        if (temperature(i) > m_limits(i))
+        {
+            m_samplesAboveTheLimit[i]++;
+        }
+        else
+        {
+            m_samplesAboveTheLimit[i] = 0;
+        }
+
+        if (temperature(i) > m_maxTemperature[i])
+        {
+            m_maxTemperature[i] = temperature(i);
+        }
+    }
+
+    return true;
+}
+
+const std::vector<double>& MotorsTemperatureChecker::getMaxTemperature() const
+{
+    return m_maxTemperature;
+}

src/WalkingModule/include/WalkingControllers/WalkingModule/Module.h

@@ -29,6 +29,8 @@
 // WalkingControllers library
 #include <WalkingControllers/RobotInterface/Helper.h>
 #include <WalkingControllers/RobotInterface/PIDHandler.h>
+#include <WalkingControllers/RobotInterface/MotorTemperatureChecker.h>
+
 #include <WalkingControllers/TrajectoryPlanner/TrajectoryGenerator.h>
 #include <WalkingControllers/TrajectoryPlanner/StableDCMModel.h>
 #include <WalkingControllers/TrajectoryPlanner/FreeSpaceEllipseManager.h>
@@ -75,6 +77,7 @@ namespace WalkingControllers
         iDynTree::Position m_zmpOffsetLocal; /** < Offset in the local frame*/
 
         std::unique_ptr<RobotInterface> m_robotControlHelper; /**< Robot control helper. */
+        std::unique_ptr<MotorsTemperatureChecker> m_motorTemperatureChecker; /**< Robot control helper. */
         std::unique_ptr<TrajectoryGenerator> m_trajectoryGenerator; /**< Pointer to the trajectory generator object. */
         std::unique_ptr<FreeSpaceEllipseManager> m_freeSpaceEllipseManager; /**< Pointer to the free space ellipse manager. */
         std::unique_ptr<WalkingController> m_walkingController; /**< Pointer to the walking DCM MPC object. */
@@ -118,6 +121,7 @@ namespace WalkingControllers
 
         iDynTree::VectorDynSize m_qDesired; /**< Vector containing the results of the IK algorithm [rad]. */
         iDynTree::VectorDynSize m_dqDesired; /**< Vector containing the results of the IK algorithm [rad]. */
+        iDynTree::VectorDynSize m_motorTemperature; /**< Vector containing the results of the IK algorithm [rad]. */
 
         iDynTree::Rotation m_inertial_R_worldFrame; /**< Rotation between the inertial and the world frame. */
 

src/WalkingModule/src/Module.cpp

@@ -182,6 +182,13 @@ bool WalkingModule::configure(yarp::os::ResourceFinder &rf)
         return false;
     }
 
+    m_motorTemperatureChecker = std::make_unique<MotorsTemperatureChecker>();
+    if (!m_motorTemperatureChecker->configure(robotControlHelperOptions, m_robotControlHelper->getActuatedDoFs()))
+    {
+        yError() << "[WalkingModule::configure] Unable to configure the motor temperature helper.";
+        return false;
+    }
+
     yarp::os::Bottle &forceTorqueSensorsOptions = rf.findGroup("FT_SENSORS");
     forceTorqueSensorsOptions.append(generalOptions);
     if (!m_robotControlHelper->configureForceTorqueSensors(forceTorqueSensorsOptions))
@@ -442,6 +449,7 @@ bool WalkingModule::configure(yarp::os::ResourceFinder &rf)
     // resize variables
     m_qDesired.resize(m_robotControlHelper->getActuatedDoFs());
     m_dqDesired.resize(m_robotControlHelper->getActuatedDoFs());
+    m_motorTemperature.resize(m_robotControlHelper->getActuatedDoFs());
 
     yInfo() << "[WalkingModule::configure] Ready to play! Please prepare the robot.";
 
@@ -732,6 +740,14 @@ bool WalkingModule::updateModule()
             return false;
         }
 
+        m_motorTemperature = m_robotControlHelper->getMotorTemperature();
+
+        if (!m_motorTemperatureChecker->setMotorTemperatures(m_motorTemperature))
+        {
+            yError() << "[WalkingModule::updateModule] Unable to set the motor temperature to the helper.";
+            return false;
+        }
+
         m_profiler->setEndTime("Feedback");
 
         auto retargetingPhase = m_isStancePhase.front() ? RetargetingClient::Phase::Stance : RetargetingClient::Phase::Walking;
@@ -1497,8 +1513,26 @@ bool WalkingModule::startWalking()
 
 bool WalkingModule::setPlannerInput(const yarp::sig::Vector &plannerInput)
 {
-    m_plannerInput = plannerInput;
+    if (m_motorTemperatureChecker->isThereAMotorOverLimit())
+    {
+        yWarning() << "[WalkingModule::setPlannerInput] The motor temperature is over the limit.";
+        std::vector<unsigned int> indeces = m_motorTemperatureChecker->getMotorsOverLimit();
+        std::string msg = "The following motors temperature are over the limits: ";
+        for (auto index : indeces)
+        {
+            msg += m_robotControlHelper->getAxesList()[index]
+                + ": Max temperature: "
+                + std::to_string(m_motorTemperatureChecker->getMaxTemperature()[index]) + " celsius.";
+        }
+        msg += "The trajectory will be set to zero.";
+        yWarning() << msg;
 
+        m_plannerInput.zero();
+    }
+    else
+    {
+        m_plannerInput = plannerInput;
+    }
     // the trajectory was already finished the new trajectory will be attached as soon as possible
     if (m_mergePoints.empty())
     {