Add the possibility to log the motor temperature in the YarpRobotLogg…

…erDevice.
ami-iit · Mar 7, 2025 · 5585086 · 5585086
1 parent f5b26ab
commit 5585086
Show file tree

Hide file tree

Showing 9 changed files with 39 additions and 0 deletions.
diff --git a/devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1/yarp-robot-logger.xml b/devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1/yarp-robot-logger.xml
@@ -71,6 +71,8 @@ BSD-3-Clause license. -->
     <param name="stream_pids">false</param>
     <param name="stream_motor_PWM">false</param>
     <param name="stream_temperatures">false</param>
+    <param name="stream_joint_accelerations">true</param>
+    <param name="stream_motor_temperature">false</param>
 
     <group name="RemoteControlBoardRemapper">
       <param name="joints_list">("neck_pitch", "neck_roll", "neck_yaw", "camera_tilt", "torso_roll", "torso_yaw", "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow", "l_wrist_roll", "l_wrist_pitch",  "l_wrist_yaw",  "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow", "r_wrist_roll", "r_wrist_pitch",  "r_wrist_yaw", "l_hip_pitch", "l_hip_roll", "l_hip_yaw", "l_knee", "l_ankle_pitch", "l_ankle_roll", "r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll")</param>

diff --git a/devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1_1/yarp-robot-logger.xml b/devices/YarpRobotLoggerDevice/app/robots/ergoCubGazeboV1_1/yarp-robot-logger.xml
@@ -58,6 +58,8 @@ BSD-3-Clause license. -->
     <param name="stream_pids">false</param>
     <param name="stream_motor_PWM">false</param>
     <param name="stream_temperatures">true</param>
+    <param name="stream_joint_accelerations">true</param>
+    <param name="stream_motor_temperature">false</param>
 
     <group name="RemoteControlBoardRemapper">
       <param name="joints_list">("neck_pitch", "neck_roll", "neck_yaw", "camera_tilt", "torso_pitch", "torso_roll", "torso_yaw", "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow", "l_wrist_roll", "l_wrist_pitch",  "l_wrist_yaw",  "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow", "r_wrist_roll", "r_wrist_pitch",  "r_wrist_yaw", "l_hip_pitch", "l_hip_roll", "l_hip_yaw", "l_knee", "l_ankle_pitch", "l_ankle_roll", "r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll")</param>

diff --git a/devices/YarpRobotLoggerDevice/app/robots/ergoCubSN000/yarp-robot-logger.xml b/devices/YarpRobotLoggerDevice/app/robots/ergoCubSN000/yarp-robot-logger.xml
@@ -60,6 +60,8 @@ BSD-3-Clause license. -->
     <param name="stream_pids">false</param>
     <param name="stream_motor_PWM">true</param>
     <param name="stream_temperatures">true</param>
+    <param name="stream_joint_accelerations">true</param>
+    <param name="stream_motor_temperature">false</param>
 
     <group name="RemoteControlBoardRemapper">
       <param name="joints_list">("neck_pitch", "neck_roll", "neck_yaw", "camera_tilt", "torso_pitch", "torso_roll", "torso_yaw", "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow", "l_wrist_roll", "l_wrist_pitch",  "l_wrist_yaw", "l_thumb_add", "l_thumb_oc", "l_index_oc", "l_middle_oc", "l_ring_pinky_oc", "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow", "r_wrist_roll", "r_wrist_pitch",  "r_wrist_yaw", "r_thumb_add", "r_thumb_oc", "r_index_oc", "r_middle_oc", "r_ring_pinky_oc", "l_hip_pitch", "l_hip_roll", "l_hip_yaw", "l_knee", "l_ankle_pitch", "l_ankle_roll", "r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll")</param>

diff --git a/devices/YarpRobotLoggerDevice/app/robots/ergoCubSN001/yarp-robot-logger.xml b/devices/YarpRobotLoggerDevice/app/robots/ergoCubSN001/yarp-robot-logger.xml
@@ -95,6 +95,8 @@ BSD-3-Clause license. -->
     <param name="stream_pids">false</param>
     <param name="stream_motor_PWM">true</param>
     <param name="stream_temperatures">true</param>
+    <param name="stream_joint_accelerations">true</param>
+    <param name="stream_motor_temperature">false</param>
 
     <group name="RemoteControlBoardRemapper">
       <param name="joints_list">("neck_pitch", "neck_roll", "neck_yaw", "camera_tilt", "torso_pitch", "torso_roll", "torso_yaw", "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow", "l_wrist_pitch", "l_wrist_roll", "l_wrist_yaw",  "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow",  "r_wrist_pitch", "r_wrist_roll", "r_wrist_yaw", "l_hip_pitch", "l_hip_roll", "l_hip_yaw", "l_knee", "l_ankle_pitch", "l_ankle_roll", "r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll")</param>

diff --git a/devices/YarpRobotLoggerDevice/app/robots/ergoCubSN002/yarp-robot-logger.xml b/devices/YarpRobotLoggerDevice/app/robots/ergoCubSN002/yarp-robot-logger.xml
@@ -88,6 +88,8 @@ BSD-3-Clause license. -->
     <param name="stream_pids">false</param>
     <param name="stream_motor_PWM">true</param>
     <param name="stream_temperatures">true</param>
+    <param name="stream_joint_accelerations">true</param>
+    <param name="stream_motor_temperature">false</param>
 
     <group name="RemoteControlBoardRemapper">
       <param name="joints_list">("neck_pitch", "neck_roll", "neck_yaw", "camera_tilt", "torso_pitch", "torso_roll", "torso_yaw", "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow", "l_wrist_pitch", "l_wrist_roll", "l_wrist_yaw",  "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow",  "r_wrist_pitch", "r_wrist_roll", "r_wrist_yaw", "l_hip_pitch", "l_hip_roll", "l_hip_yaw", "l_knee", "l_ankle_pitch", "l_ankle_roll", "r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll")</param>

diff --git a/devices/YarpRobotLoggerDevice/app/robots/iCubGazeboV3/yarp-robot-logger.xml b/devices/YarpRobotLoggerDevice/app/robots/iCubGazeboV3/yarp-robot-logger.xml
@@ -43,6 +43,8 @@ BSD-3-Clause license. -->
     <param name="stream_inertials">false</param>
     <param name="stream_cartesian_wrenches">true</param>
     <param name="stream_forcetorque_sensors">true</param>
+    <param name="stream_joint_accelerations">true</param>
+    <param name="stream_motor_temperature">false</param>
 
     <group name="RemoteControlBoardRemapper">
       <param name="joints_list">("neck_pitch", "neck_roll", "neck_yaw", "torso_pitch", "torso_roll", "torso_yaw", "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow", "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow", "l_hip_pitch", "l_hip_roll", "l_hip_yaw", "l_knee", "l_ankle_pitch", "l_ankle_roll", "r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll")</param>

diff --git a/devices/YarpRobotLoggerDevice/app/robots/iCubGenova09/yarp-robot-logger.xml b/devices/YarpRobotLoggerDevice/app/robots/iCubGenova09/yarp-robot-logger.xml
@@ -54,6 +54,8 @@ BSD-3-Clause license. -->
     <param name="stream_pids">false</param>
     <param name="stream_motor_PWM">true</param>
     <param name="stream_temperatures">true</param>
+    <param name="stream_joint_accelerations">true</param>
+    <param name="stream_motor_temperature">false</param>
 
     <group name="RemoteControlBoardRemapper">
       <param name="joints_list">("neck_pitch", "neck_roll", "neck_yaw", "torso_pitch", "torso_roll", "torso_yaw", "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow", "l_wrist_prosup", "l_wrist_pitch",  "l_wrist_yaw", "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow", "r_wrist_prosup", "r_wrist_pitch", "r_wrist_yaw", "l_hip_pitch", "l_hip_roll", "l_hip_yaw", "l_knee", "l_ankle_pitch", "l_ankle_roll", "r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll")</param>

diff --git a/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h b/devices/YarpRobotLoggerDevice/include/BipedalLocomotion/YarpRobotLoggerDevice.h
@@ -168,6 +168,7 @@ class YarpRobotLoggerDevice : public yarp::dev::DeviceDriver,
     bool m_streamMotorStates{false};
     bool m_streamJointStates{false};
     bool m_streamJointAccelerations{true};
+    bool m_streamMotorTemperature{false};
     bool m_streamMotorPWM{false};
     bool m_streamPIDs{false};
     bool m_streamInertials{false};
@@ -226,6 +227,7 @@ class YarpRobotLoggerDevice : public yarp::dev::DeviceDriver,
     const std::string motorStateVelocitiesName = "motors_state::velocities";
     const std::string motorStateAccelerationsName = "motors_state::accelerations";
     const std::string motorStateCurrentsName = "motors_state::currents";
+    const std::string motorStateTemperaturesName = "motors_state::temperatures";
     const std::string motorStatePwmName = "motors_state::PWM";
 
     const std::string motorStatePidsName = "PIDs";

diff --git a/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp b/devices/YarpRobotLoggerDevice/src/YarpRobotLoggerDevice.cpp
@@ -611,6 +611,17 @@ bool YarpRobotLoggerDevice::setupRobotSensorBridge(
         log()->info("{} The joint accelerations is not logged", logPrefix);
     }
 
+    if (!ptr->getParameter("stream_motor_temperature", m_streamMotorTemperature))
+    {
+        log()->info("{} The 'stream_motor_temperature' parameter is not found. The motor "
+                    "temperature is not logged",
+                    logPrefix);
+    }
+    if (!m_streamMotorTemperature)
+    {
+        log()->info("{} The motor temperature is not logged", logPrefix);
+    }
+
     if (!ptr->getParameter("stream_motor_states", m_streamMotorStates))
     {
         log()->info("{} The 'stream_motor_states' parameter is not found. The motor states is not "
@@ -822,6 +833,10 @@ bool YarpRobotLoggerDevice::attachAll(const yarp::dev::PolyDriverList& poly)
         ok = ok && addChannel(motorStateVelocitiesName, joints.size(), joints);
         ok = ok && addChannel(motorStateAccelerationsName, joints.size(), joints);
         ok = ok && addChannel(motorStateCurrentsName, joints.size(), joints);
+        if (m_streamMotorTemperature)
+        {
+            ok = ok && addChannel(motorStateTemperaturesName, joints.size(), joints);
+        }
     }
 
     if (m_streamMotorPWM)
@@ -1580,6 +1595,14 @@ void YarpRobotLoggerDevice::run()
         {
             logData(motorStateCurrentsName, m_jointSensorBuffer, time);
         }
+
+        if (m_streamMotorTemperature)
+        {
+            if (m_robotSensorBridge->getMotorTemperatures(m_jointSensorBuffer))
+            {
+                logData(motorStateTemperaturesName, m_jointSensorBuffer, time);
+            }
+        }
     }
 
     if (m_streamMotorPWM)