Implement a logic to prevent the robot from walking when the temperature is greater then a threshold for a given ammount of time

src/JoypadModule/src/Module.cpp

@@ -131,7 +131,7 @@ bool JoypadModule::configure(yarp::os::ResourceFinder &rf)
         yError() << "[configure] Unable to get a string from searchable";
         return false;
     }
-    yarp::os::Network::connect(m_rpcClientPortName, m_rpcServerPortName);
+    yarp::os::Network::connect(m_rpcClientPortName, m_rpcServerPortName, "shmem");
 
     if (!YarpUtilities::getStringFromSearchable(rf, "robotGoalOutputPort_name", portName))
     {
@@ -146,7 +146,7 @@ bool JoypadModule::configure(yarp::os::ResourceFinder &rf)
         yError() << "[configure] Unable to get a string from searchable";
         return false;
     }
-    yarp::os::Network::connect(m_robotGoalOutputPortName, m_robotGoalInputPortName);
+    yarp::os::Network::connect(m_robotGoalOutputPortName, m_robotGoalInputPortName, "shmem");
 
     return true;
 }
@@ -260,21 +260,21 @@ bool JoypadModule::updateModule()
         {
             yInfo() << "[updateModule] Connecting both ports";
             if (!yarp::os::Network::isConnected(m_rpcClientPortName, m_rpcServerPortName))
-                yarp::os::Network::connect(m_rpcClientPortName, m_rpcServerPortName);
+	      yarp::os::Network::connect(m_rpcClientPortName, m_rpcServerPortName, "shmem");
             if (!yarp::os::Network::isConnected(m_robotGoalOutputPortName, m_robotGoalInputPortName))
-                yarp::os::Network::connect(m_robotGoalOutputPortName, m_robotGoalInputPortName);
+	      yarp::os::Network::connect(m_robotGoalOutputPortName, m_robotGoalInputPortName, "shmem");
         }
         else if (m_connectPortsSeparately && connectGoal > 0)
         {
             yInfo() << "[updateModule] Connecting goal port";
             if (!yarp::os::Network::isConnected(m_robotGoalOutputPortName, m_robotGoalInputPortName))
-                yarp::os::Network::connect(m_robotGoalOutputPortName, m_robotGoalInputPortName);
+	      yarp::os::Network::connect(m_robotGoalOutputPortName, m_robotGoalInputPortName, "shmem");
         }
         else if (m_connectPortsSeparately && connectRpc > 0)
         {
             yInfo() << "[updateModule] Connecting RPC port";
             if (!yarp::os::Network::isConnected(m_rpcClientPortName, m_rpcServerPortName))
-                yarp::os::Network::connect(m_rpcClientPortName, m_rpcServerPortName);
+	      yarp::os::Network::connect(m_rpcClientPortName, m_rpcServerPortName, "shmem");
         }
         // disconnect the ports
         else if (disconnect > 0)

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/Helper.h

@@ -16,6 +16,8 @@
 #include <yarp/dev/IPositionDirect.h>
 #include <yarp/dev/IVelocityControl.h>
 #include <yarp/dev/IInteractionMode.h>
+#include <yarp/dev/IMotor.h>
+
 #include <yarp/sig/Vector.h>
 #include <yarp/os/Timer.h>
 
@@ -48,6 +50,7 @@ namespace WalkingControllers
         yarp::dev::IControlMode *m_controlModeInterface{nullptr}; /**< Control mode interface. */
         yarp::dev::IControlLimits *m_limitsInterface{nullptr}; /**< Encorders interface. */
         yarp::dev::IInteractionMode *m_interactionInterface{nullptr}; /**< Stiff/compliant mode interface. */
+        yarp::dev::IMotor *m_motorInterface{nullptr}; /**< Motor interface. */
 
         std::unique_ptr<WalkingPIDHandler> m_PIDHandler; /**< Pointer to the PID handler object. */
 
@@ -66,6 +69,13 @@ namespace WalkingControllers
         iDynTree::VectorDynSize m_jointVelocitiesBounds; /**< Joint Velocity bounds [rad/s]. */
         iDynTree::VectorDynSize m_jointPositionsUpperBounds; /**< Joint Position upper bound [rad]. */
         iDynTree::VectorDynSize m_jointPositionsLowerBounds; /**< Joint Position lower bound [rad]. */
+
+        iDynTree::VectorDynSize m_motorTemperatures; /**< Motor temperature [Celsius]. */
+        mutable std::mutex m_motorTemperatureMutex; /**< Mutex for the motor temperature. */
+        std::atomic_bool m_motorTemperatureTreadIsRunning; /**< True if the motor temperature is updated. */
+        double m_motorTemperatureDt{0.5};
+        std::thread m_motorTemperatureThread; /**< Thread for the motor temperature. */
+
         // yarp::sig::Vector m_positionFeedbackDegFiltered;
         yarp::sig::Vector m_velocityFeedbackDegFiltered; /**< Vector containing the filtered joint velocity [deg/s]. */
         std::unique_ptr<iCub::ctrl::FirstOrderLowPassFilter> m_positionFilter; /**< Joint position low pass filter .*/
@@ -123,10 +133,13 @@ namespace WalkingControllers
         bool configureForceTorqueSensor(const std::string& portPrefix,
                                         const std::string& portInputName,
                                         const std::string& wholeBodyDynamicsPortName,
+                                        const std::string& carrier,
                                         const double& samplingTime,
                                         bool useWrenchFilter,
                                         double cutFrequency,
                                         MeasuredWrench& measuredWrench);
+
+        void readMotorTemperature();
     public:
 
         /**
@@ -205,6 +218,9 @@ namespace WalkingControllers
          */
         const iDynTree::VectorDynSize& getJointVelocity() const;
 
+
+        iDynTree::VectorDynSize getMotorTemperature() const;
+
         /**
          * Get the joint upper limit
          * @return the joint upper bound in radiants

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/Helper.cpp

@@ -269,6 +269,7 @@ bool RobotInterface::configureRobot(const yarp::os::Searchable& config)
     options.put("localPortPrefix", "/" + name + "/remoteControlBoard");
     yarp::os::Property& remoteControlBoardsOpts = options.addGroup("REMOTE_CONTROLBOARD_OPTIONS");
     remoteControlBoardsOpts.put("writeStrict", "on");
+    remoteControlBoardsOpts.put("carrier", "shmem");
 
     // get the actuated DoFs
     m_actuatedDOFs = m_axesList.size();
@@ -313,6 +314,8 @@ bool RobotInterface::configureRobot(const yarp::os::Searchable& config)
         }
     }
 
+    m_motorTemperatures.resize(m_actuatedDOFs);
+
     // open the device
     if(!m_robotDevice.open(options))
     {
@@ -363,6 +366,12 @@ bool RobotInterface::configureRobot(const yarp::os::Searchable& config)
         return false;
     }
 
+    if (!m_robotDevice.view(m_motorInterface) || !m_motorInterface)
+    {
+        yError() << "[configureRobot] Cannot obtain IMotor interface";
+        return false;
+    }
+
     // resize the buffers
     m_positionFeedbackDeg.resize(m_actuatedDOFs, 0.0);
     m_velocityFeedbackDeg.resize(m_actuatedDOFs, 0.0);
@@ -482,12 +491,35 @@ bool RobotInterface::configureRobot(const yarp::os::Searchable& config)
         return false;
     }
 
+    m_motorTemperatureThread = std::thread(&RobotInterface::readMotorTemperature, this);
+
     return true;
 }
 
+void RobotInterface::readMotorTemperature()
+{
+    m_motorTemperatureTreadIsRunning = true;
+
+    while(m_motorTemperatureTreadIsRunning)
+    {
+        {
+            std::lock_guard<std::mutex> lock(m_motorTemperatureMutex);
+
+            if(!m_motorInterface->getTemperatures(m_motorTemperatures.data()))
+            {
+                yError() << "[RobotInterface::readMotorTemperature] Unable to get the motor temperatures.";
+            }
+        }
+
+        // TODO GR you should load the time required for the  read and close it
+        yarp::os::Time::delay(m_motorTemperatureDt);
+    }
+}
+
 bool RobotInterface::configureForceTorqueSensor(const std::string& portPrefix,
                                                 const std::string& portInputName,
                                                 const std::string& wholeBodyDynamicsPortName,
+                                                const std::string& carrier,
                                                 const double& samplingTime,
                                                 bool useWrenchFilter,
                                                 double cutFrequency,
@@ -497,7 +529,7 @@ bool RobotInterface::configureForceTorqueSensor(const std::string& portPrefix,
     measuredWrench.port = std::make_unique<yarp::os::BufferedPort<yarp::sig::Vector>>();
     measuredWrench.port->open("/" + portPrefix + portInputName);
     // connect port
-    if(!yarp::os::Network::connect(wholeBodyDynamicsPortName, "/" + portPrefix + portInputName))
+    if(!yarp::os::Network::connect(wholeBodyDynamicsPortName, "/" + portPrefix + portInputName, carrier))
     {
         yError() << "[RobotInterface::configureForceTorqueSensors] Unable to connect to port "
                  << wholeBodyDynamicsPortName << " to " << "/" + portPrefix + portInputName;
@@ -643,6 +675,7 @@ bool RobotInterface::configureForceTorqueSensors(const yarp::os::Searchable& con
         ok = ok && this->configureForceTorqueSensor(portPrefix,
                                                     leftFootWrenchInputPorts[i],
                                                     leftFootWrenchOutputPorts[i],
+                                                    "shmem",
                                                     samplingTime,
                                                     useWrenchFilter, cutFrequency,
                                                     m_leftFootMeasuredWrench[i]);
@@ -653,6 +686,7 @@ bool RobotInterface::configureForceTorqueSensors(const yarp::os::Searchable& con
         ok = ok && this->configureForceTorqueSensor(portPrefix,
                                                     rightFootWrenchInputPorts[i],
                                                     rightFootWrenchOutputPorts[i],
+                                                    "shmem",
                                                     samplingTime,
                                                     useWrenchFilter, cutFrequency,
                                                     m_rightFootMeasuredWrench[i]);
@@ -1096,6 +1130,12 @@ bool RobotInterface::setVelocityReferences(const iDynTree::VectorDynSize& desire
 
 bool RobotInterface::close()
 {
+    m_motorTemperatureTreadIsRunning = false;
+    if (m_motorTemperatureThread.joinable())
+    {
+        m_motorTemperatureThread.join();
+    }
+
     // close all the ports
     for(auto& wrench : m_leftFootMeasuredWrench)
         wrench.port->close();
@@ -1119,11 +1159,18 @@ const iDynTree::VectorDynSize& RobotInterface::getJointPosition() const
 {
     return m_positionFeedbackRad;
 }
+
 const iDynTree::VectorDynSize& RobotInterface::getJointVelocity() const
 {
     return m_velocityFeedbackRad;
 }
 
+iDynTree::VectorDynSize RobotInterface::getMotorTemperature() const
+{
+    std::lock_guard lock(m_motorTemperatureMutex);
+    return m_motorTemperatures;
+}
+
 const iDynTree::Wrench& RobotInterface::getLeftWrench() const
 {
     return m_leftWrench;

src/RobotInterface/src/MotorTemperatureChecker.cpp

@@ -0,0 +1,116 @@
+// 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 (m_limits(i) < 0)
+	{
+	    continue;
+	}
+
+	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;
+}