Run torque control loop only when at least one joint torque controlled

ami-iit · Nov 13, 2024 · 73401bd · 73401bd
1 parent 0d681c4
commit 73401bd
Show file tree

Hide file tree

Showing 5 changed files with 23 additions and 239 deletions.
diff --git a/devices/JointTorqueControlDevice/CMakeLists.txt b/devices/JointTorqueControlDevice/CMakeLists.txt
@@ -8,53 +8,29 @@ if(FRAMEWORK_COMPILE_JointTorqueControlDevice)
       THRIFT thrifts/JointTorqueControlCommands.thrift
       INSTALLATION_FOLDER JointTorqueControlDevice)
 
-#   add_bipedal_yarp_device(
-#       NAME JointTorqueControlDevice
-#       TYPE BipedalLocomotion::JointTorqueControlDevice
-#       SOURCES src/JointTorqueControlDevice.cpp
-#               src/PassThroughControlBoard.cpp
-#               src/PINNFrictionEstimator.cpp
-#       PUBLIC_HEADERS include/BipedalLocomotion/JointTorqueControlDevice.h
-#                      include/BipedalLocomotion/PassThroughControlBoard.h
-#                      include/BipedalLocomotion/PINNFrictionEstimator.h
-#       PRIVATE_LINK_LIBRARIES
-#       BipedalLocomotion::YarpUtilities
-#       BipedalLocomotion::SystemYarpImplementation
-#       BipedalLocomotion::RobotInterfaceYarpImplementation
-#       BipedalLocomotion::ParametersHandlerYarpImplementation
-#       BipedalLocomotion::VectorsCollection
-#       BipedalLocomotion::JointTorqueControlCommands
-#       BipedalLocomotion::Math
-#       BipedalLocomotion::ContinuousDynamicalSystem
-#       PUBLIC_LINK_LIBRARIES
-#       onnxruntime::onnxruntime
-#       Eigen3::Eigen
-#       ${YARP_LIBRARIES}
-#       ${iDynTree_LIBRARIES}
-#       CONFIGURE_PACKAGE_NAME joint_torque_control_device)
-
   add_bipedal_yarp_device(
-        NAME PassThroughControlBoard
-        TYPE BipedalLocomotion::PassThroughControlBoard
-        SOURCES src/PassThroughControlBoard.cpp
-        PUBLIC_HEADERS include/BipedalLocomotion/PassThroughControlBoard.h
-        PUBLIC_LINK_LIBRARIES
-        ${YARP_LIBRARIES}
-        CONFIGURE_PACKAGE_NAME pass_through_control_board_device)
+      NAME JointTorqueControlDevice
+      TYPE BipedalLocomotion::JointTorqueControlDevice
+      SOURCES src/JointTorqueControlDevice.cpp
+              src/PassThroughControlBoard.cpp
+              src/PINNFrictionEstimator.cpp
+      PUBLIC_HEADERS include/BipedalLocomotion/JointTorqueControlDevice.h
+                     include/BipedalLocomotion/PassThroughControlBoard.h
+                     include/BipedalLocomotion/PINNFrictionEstimator.h
+      PRIVATE_LINK_LIBRARIES
+      BipedalLocomotion::YarpUtilities
+      BipedalLocomotion::SystemYarpImplementation
+      BipedalLocomotion::RobotInterfaceYarpImplementation
+      BipedalLocomotion::ParametersHandlerYarpImplementation
+      BipedalLocomotion::VectorsCollection
+      BipedalLocomotion::JointTorqueControlCommands
+      BipedalLocomotion::Math
+      BipedalLocomotion::ContinuousDynamicalSystem
+      PUBLIC_LINK_LIBRARIES
+      onnxruntime::onnxruntime
+      Eigen3::Eigen
+      ${YARP_LIBRARIES}
+      ${iDynTree_LIBRARIES}
+      CONFIGURE_PACKAGE_NAME joint_torque_control_device)
 
-#  add_bipedal_yarp_device(
-#        NAME PassThroughControlBoardDevice
-#        TYPE BipedalLocomotion::PassThroughControlBoardDevice
-#        SOURCES src/PassThroughControlBoardDevice.cpp
-#                src/PassThroughControlBoard.cpp
-#        PUBLIC_HEADERS include/BipedalLocomotion/PassThroughControlBoardDevice.h
-#                       include/BipedalLocomotion/PassThroughControlBoard.h
-#        PRIVATE_LINK_LIBRARIES
-#        BipedalLocomotion::SystemYarpImplementation
-#        BipedalLocomotion::JointTorqueControlCommands
-#        PUBLIC_LINK_LIBRARIES
-#        onnxruntime::onnxruntime
-#        ${YARP_LIBRARIES}
-#        CONFIGURE_PACKAGE_NAME pass_through_control_board_device) 
-
 endif()
diff --git a/devices/JointTorqueControlDevice/include/BipedalLocomotion/JointTorqueControlDevice.h b/devices/JointTorqueControlDevice/include/BipedalLocomotion/JointTorqueControlDevice.h
@@ -186,9 +186,6 @@ class BipedalLocomotion::JointTorqueControlDevice
     std::vector<std::string> m_axisNames;
     LowPassFilterParameters m_lowPassFilterParameters;
     bool isTorqueControlEnabled = false;
-
-    // TO DELETE
-    std::chrono::nanoseconds m_time = std::chrono::nanoseconds::zero();
 
     iDynTree::VectorDynSize m_measurementKF;
     iDynTree::VectorDynSize m_estimateKF;
@@ -297,10 +294,6 @@ class BipedalLocomotion::JointTorqueControlDevice
     virtual bool setRefTorque(int j, double t);
 
     // HACK MOTOR ACCELERATION TO PUBLISH FRICTION TORQUES
-    // virtual bool getEncoderSpeed(int j, double* sp);
-    // virtual bool getEncoderSpeeds(double* spds);
-    // virtual bool getMotorEncoderSpeed(int m, double* sp);
-    // virtual bool getMotorEncoderSpeeds(double* spds);
     virtual bool getMotorEncoderAcceleration(int j, double* acc);
     virtual bool getMotorEncoderAccelerations(double* accs);
 

diff --git a/devices/JointTorqueControlDevice/include/BipedalLocomotion/PassThroughControlBoardDevice.h b/devices/JointTorqueControlDevice/include/BipedalLocomotion/PassThroughControlBoardDevice.h
diff --git a/devices/JointTorqueControlDevice/src/JointTorqueControlDevice.cpp b/devices/JointTorqueControlDevice/src/JointTorqueControlDevice.cpp
@@ -1634,34 +1634,6 @@ bool JointTorqueControlDevice::getRefTorque(int j, double* trq)
     return true;
 }
 
-// bool JointTorqueControlDevice::getEncoderSpeeds(double* spds)
-// {
-//     std::lock_guard<std::mutex>(this->globalMutex);
-//     memcpy(spds, measuredJointVelocities.data(), this->axes * sizeof(double));
-//     return true;
-// }
-
-// bool JointTorqueControlDevice::getEncoderSpeed(int j, double* sp)
-// {
-//     std::lock_guard<std::mutex>(this->globalMutex);
-//     *sp = measuredJointVelocities[j];
-//     return true;
-// }
-
-// bool JointTorqueControlDevice::getMotorEncoderSpeeds(double* spds)
-// {
-//     std::lock_guard<std::mutex>(this->globalMutex);
-//     memcpy(spds, measuredMotorVelocities.data(), this->axes * sizeof(double));
-//     return true;
-// }
-
-// bool JointTorqueControlDevice::getMotorEncoderSpeed(int j, double* sp)
-// {
-//     std::lock_guard<std::mutex>(this->globalMutex);
-//     *sp = measuredMotorVelocities[j];
-//     return true;
-// }
-
 // TO BE UPDATE
 // We publish the friction torque by using the motor acceleration as the friction
 // torque does not have a dedicated interface in the remote control board.
@@ -1716,9 +1688,6 @@ void JointTorqueControlDevice::controlLoop()
 {
     if (isTorqueControlEnabled)
     {
-        // Take time now
-        std::chrono::nanoseconds now = BipedalLocomotion::clock().now();
-
         // Read status from the controlboard
         this->readStatus();
 
@@ -1738,12 +1707,5 @@ void JointTorqueControlDevice::controlLoop()
                             desiredMotorCurrentsHijackedMotors.data());
 
         timeOfLastControlLoop = BipedalLocomotion::clock().now();
-
-        // Print time every 10 seconds
-        if ((timeOfLastControlLoop - m_time).count() >= 1.0*1e-9)
-        {
-            log()->error(">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> Control loop time: {} ms", (timeOfLastControlLoop - now).count() * 1e-6);
-            m_time = BipedalLocomotion::clock().now();
-        }
     }
 }
diff --git a/devices/JointTorqueControlDevice/src/PassThroughControlBoardDevice.cpp b/devices/JointTorqueControlDevice/src/PassThroughControlBoardDevice.cpp