From df1374c65476bfd769fb56bd6506c9a0c450db74 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Mon, 7 Mar 2022 14:03:34 +0100 Subject: [PATCH 1/7] [wholeBodyDynamics] Implement the Time profiler machinery --- devices/wholeBodyDynamics/TimeProfiler.cpp | 163 +++++++++++++++++++++ devices/wholeBodyDynamics/TimeProfiler.h | 115 +++++++++++++++ 2 files changed, 278 insertions(+) create mode 100644 devices/wholeBodyDynamics/TimeProfiler.cpp create mode 100644 devices/wholeBodyDynamics/TimeProfiler.h diff --git a/devices/wholeBodyDynamics/TimeProfiler.cpp b/devices/wholeBodyDynamics/TimeProfiler.cpp new file mode 100644 index 0000000..181de44 --- /dev/null +++ b/devices/wholeBodyDynamics/TimeProfiler.cpp @@ -0,0 +1,163 @@ +/** + * @file TimeProfiler.cpp + * @authors Giulio Romualdi + * @copyright 2018 iCub Facility - Istituto Italiano di Tecnologia + * Released under the terms of the LGPLv2.1 or later, see LGPL.TXT + * @date 2018 + */ + +#include +#include +#include +#include +#include +#include +#include + + +#include "TimeProfiler.h" + +using namespace WholeBodyDynamics; + + + +std::string printTimerDescription(const std::string& name, const TimerHandler::TimerDescription& description) +{ + std::stringstream ss; + ss << "|" << std::setw(30) << name + << "|" << std::setw(15) << std::setprecision(13) << description.averageDuration.count() + << "|" << std::setw(10) << std::setprecision(8) << description.timer.getInfo().deadlineMiss + << "|" << std::setw(15) << std::setprecision(13) << description.timer.getInfo().latestDeadlineMissDuration.count() + << "|"; + + return ss.str(); +}; + +Timer::Timer(const std::string& name, const std::chrono::duration& expectedDuration) +{ + this->setName(name); + this->setExpectedDuration(expectedDuration); +} + +void Timer::tic() +{ + m_initTime = std::chrono::system_clock::now(); + m_info.dealineMissDetected = false; +} + +void Timer::toc() +{ + m_info.duration = std::chrono::system_clock::now() - m_initTime; + if (m_info.duration > m_exprectedDuration) + { + m_info.dealineMissDetected = true; + m_info.deadlineMiss++; + m_info.latestDeadlineMissDuration = m_info.duration; + } +} + +void Timer::setName(const std::string& name) +{ + m_info.name = name; +} + +void Timer::setExpectedDuration(const std::chrono::duration& expectedDuration) +{ + m_exprectedDuration = expectedDuration; +} + +void TimerHandler::setHorizon(unsigned int horizon) +{ + m_horizon = horizon; +} + +bool TimerHandler::addTimer(const std::string& name, const Timer& timer) +{ + auto it = m_timers.find(name); + if (it != m_timers.end()) + { + return false; + } + + m_timers[name].timer = timer; + + return true; +} + +void TimerHandler::tic(const std::string& key) +{ + // if the timer does not exist we add it + m_timers[key].timer.tic(); +} + +bool TimerHandler::toc(const std::string& key) +{ + auto it = m_timers.find(key); + if (it == m_timers.end()) + { + return false; + } + + + it->second.timer.toc(); + return true; +} + +void TimerHandler::setVerbosity(bool verbosity) +{ + m_verbosity = verbosity; +} + +void TimerHandler::profiling() +{ + for (auto& [name, timerDescription] : m_timers) + { + const auto& duration = timerDescription.timer.getInfo().duration; + + // this automatically create the element if does bot exist + auto& queue = m_durations[name]; + queue.push_back(duration); + + // keep the queue size equal to horizon + if (queue.size() >= m_horizon) + { + queue.pop_front(); + } + + // this should never happen + assert(queue.size() == m_horizon); + + timerDescription.averageDuration + = std::accumulate(std::next(queue.begin()), queue.end(), queue.front()); + } + + if(m_verbosity) + { + if(m_verbosityCounter == 0) + { + std::cout << "|" << std::setw(30) << "name |" + << std::setw(15) << "tavarg (s) |" + << std::setw(10) << "dm |" + << std::setw(15) << "tdm (s) |" << std::endl; + std::cout << std::setfill('-') << "|" << std::setw(15) << "|" + << std::setw(15) << "|" + << std::setw(10) << "|" + << std::setw(15) << "|" << std::endl; + + + for (const auto& [name, timerDescription] : m_timers) + { + std::cout << printTimerDescription(name, timerDescription) << std::endl; + } + + std::cout << std::setfill('-') << "|" << std::setw(30) << "|" + << std::setw(15) << "|" + << std::setw(10) << "|" + << std::setw(15) << "|" << std::endl; + } + + m_verbosityCounter++ ; + if(m_verbosityCounter == m_horizon) + m_verbosityCounter = 0; + } +} diff --git a/devices/wholeBodyDynamics/TimeProfiler.h b/devices/wholeBodyDynamics/TimeProfiler.h new file mode 100644 index 0000000..60dbd73 --- /dev/null +++ b/devices/wholeBodyDynamics/TimeProfiler.h @@ -0,0 +1,115 @@ +#ifndef TIME_PROFILER_H +#define TIME_PROFILER_H + +// std +#include +#include +#include + +namespace WholeBodyDynamics +{ + +/** + * Simple timer. + */ +class Timer +{ + +public: + struct Info + { + unsigned int deadlineMiss{0}; /**< Number of deadline miss */ + std::string name{"Timer"}; /**< Name associated to the timer */ + bool dealineMissDetected{false}; + std::chrono::duration latestDeadlineMissDuration; /**< Average duration. */ + std::chrono::duration duration; /**< Latest duration. */ + }; + + Timer(const std::string& name, const std::chrono::duration& expectedDuration); + + Timer() = default; + + void setName(const std::string& name); + + void setExpectedDuration(const std::chrono::duration& expectedDuration); + + /** + * Set initial time. + */ + inline void tic(); + + /** + * Set final time. + */ + inline void toc(); + + /** + * Get the average duration. + * @return average duration. + */ + const Info& getInfo() const; + +private: + std::chrono::time_point m_initTime; /**< Init time. */ + std::chrono::duration m_exprectedDuration{std::chrono::duration::max()}; + + Info m_info; +}; + +/** + * Simple Time profiler class + */ +class TimerHandler +{ +public: + + struct TimerDescription + { + ::WholeBodyDynamics::Timer timer; + std::chrono::duration averageDuration{0}; + }; + + /** + * Set the output period. + * @param maxCounter is the period (expressed in cycles). + */ + void setHorizon(unsigned int horizon); + + /** + * Add a new timer + * @param key is the name of the timer. + * @return true/false in case of success/failure. + */ + void tic(const std::string& key); + + /** + * Set the init time for the timer named "key" + * @param key is the name of the timer. + * @return true/false in case of success/failure. + */ + bool toc(const std::string& key); + + /** + * Print the profiling quantities. + */ + void profiling(); + + void setVerbosity(bool verbosity); + + bool addTimer(const std::string& name, const Timer& timer); + +private: + unsigned int m_horizon; /**< Counter useful to print the profiling quantities only every + m_maxCounter times. */ + + std::unordered_map m_timers; /**< Dictionary that contains all + the timers. */ + + std::unordered_map>> m_durations; + + bool m_verbosity{false}; + unsigned int m_verbosityCounter{0}; +}; +}; // namespace WholeBodyDynamics + +#endif // TIME_PROFILER_H From d49bdd1655f1ce2e2b1e2b7306949b36f3cd5378 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Mon, 7 Mar 2022 14:04:09 +0100 Subject: [PATCH 2/7] [wholeBodyDynamics] Use the time profiler in the whole-body dynamics device --- devices/wholeBodyDynamics/CMakeLists.txt | 3 ++- .../WholeBodyDynamicsDevice.cpp | 27 +++++++++++++++++++ .../WholeBodyDynamicsDevice.h | 3 +++ 3 files changed, 32 insertions(+), 1 deletion(-) diff --git a/devices/wholeBodyDynamics/CMakeLists.txt b/devices/wholeBodyDynamics/CMakeLists.txt index b075062..730b7be 100644 --- a/devices/wholeBodyDynamics/CMakeLists.txt +++ b/devices/wholeBodyDynamics/CMakeLists.txt @@ -22,7 +22,8 @@ if(ENABLE_wholebodydynamics) yarp_add_plugin(wholeBodyDynamicsDevice WholeBodyDynamicsDevice.h WholeBodyDynamicsDevice.cpp SixAxisForceTorqueMeasureHelpers.h SixAxisForceTorqueMeasureHelpers.cpp - GravityCompensationHelpers.h GravityCompensationHelpers.cpp) + GravityCompensationHelpers.h GravityCompensationHelpers.cpp + TimeProfiler.h TimeProfiler.cpp) target_link_libraries(wholeBodyDynamicsDevice wholeBodyDynamicsSettings wholeBodyDynamics_IDLServer diff --git a/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp b/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp index 65e361a..d3aeff2 100644 --- a/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp +++ b/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp @@ -1,3 +1,4 @@ +#include "TimeProfiler.h" #define SKIN_EVENTS_TIMEOUT 0.2 #include "WholeBodyDynamicsDevice.h" @@ -1633,6 +1634,12 @@ bool WholeBodyDynamicsDevice::open(os::Searchable& config) yDebug() << "wholeBodyDynamics Statistics: Filtered FT ports opened in " << yarp::os::Time::now() - tick << "s."; } + // print the clock once every 1.0 seconds + m_timerHandler.setHorizon(1.0/getPeriod()); + m_timerHandler.addTimer("all", + WholeBodyDynamics::Timer("all", + std::chrono::duration(getPeriod()))); + yDebug() << "wholeBodyDynamics Statistics: Configuration finished. Waiting attachAll to be called."; return true; @@ -2870,6 +2877,9 @@ void WholeBodyDynamicsDevice::publishFilteredFTWithoutOffset() void WholeBodyDynamicsDevice::run() { + + m_timerHandler.tic("all"); + std::lock_guard guard(this->deviceMutex); if( correctlyConfigured ) @@ -2878,26 +2888,43 @@ void WholeBodyDynamicsDevice::run() //this->reconfigureClassFromSettings(); // Read sensor readings + m_timerHandler.tic("read sensors"); this->readSensors(); + m_timerHandler.toc("read sensors"); // Filter sensor and remove offset + m_timerHandler.tic("remove offset"); this->filterSensorsAndRemoveSensorOffsets(); + m_timerHandler.toc("remove offset"); // Update kinematics + m_timerHandler.tic("kinematics"); this->updateKinematics(); + m_timerHandler.toc("kinematics"); // Read contacts info from the skin or from assume contact location + m_timerHandler.tic("contact points"); this->readContactPoints(); + m_timerHandler.toc("contact points"); // Compute calibration if we are in calibration mode + m_timerHandler.tic("calibration"); this->computeCalibration(); + m_timerHandler.toc("calibration"); // Compute estimated external forces and internal joint torques + m_timerHandler.tic("compute forces"); this->computeExternalForcesAndJointTorques(); + m_timerHandler.toc("compute forces"); // Publish estimated quantities + m_timerHandler.tic("publish"); this->publishEstimatedQuantities(); + m_timerHandler.toc("publish"); } + + m_timerHandler.toc("all"); + m_timerHandler.profiling(); } bool WholeBodyDynamicsDevice::detachAll() diff --git a/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.h b/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.h index c3bcdbf..5086c2c 100644 --- a/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.h +++ b/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.h @@ -31,6 +31,7 @@ #include #include "SixAxisForceTorqueMeasureHelpers.h" #include "GravityCompensationHelpers.h" +#include "TimeProfiler.h" #include #include @@ -753,6 +754,8 @@ class WholeBodyDynamicsDevice : public yarp::dev::DeviceDriver, iDynTree::JointDOFsDoubleArray m_gravityCompensationTorques; void resetGravityCompensation(); + WholeBodyDynamics::TimerHandler m_timerHandler; + public: // CONSTRUCTOR WholeBodyDynamicsDevice(); From 395ded1c88b69aadbad820fa307dc236f62ad397 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Mon, 7 Mar 2022 16:39:51 +0000 Subject: [PATCH 3/7] Enable the verbosity in the time profiler machinery --- devices/wholeBodyDynamics/TimeProfiler.cpp | 30 +++++++++++-------- devices/wholeBodyDynamics/TimeProfiler.h | 4 +-- .../WholeBodyDynamicsDevice.cpp | 2 +- 3 files changed, 21 insertions(+), 15 deletions(-) diff --git a/devices/wholeBodyDynamics/TimeProfiler.cpp b/devices/wholeBodyDynamics/TimeProfiler.cpp index 181de44..11997a0 100644 --- a/devices/wholeBodyDynamics/TimeProfiler.cpp +++ b/devices/wholeBodyDynamics/TimeProfiler.cpp @@ -14,6 +14,7 @@ #include #include +#include #include "TimeProfiler.h" @@ -28,7 +29,7 @@ std::string printTimerDescription(const std::string& name, const TimerHandler::T << "|" << std::setw(15) << std::setprecision(13) << description.averageDuration.count() << "|" << std::setw(10) << std::setprecision(8) << description.timer.getInfo().deadlineMiss << "|" << std::setw(15) << std::setprecision(13) << description.timer.getInfo().latestDeadlineMissDuration.count() - << "|"; + << "|" << std::endl; return ss.str(); }; @@ -66,6 +67,11 @@ void Timer::setExpectedDuration(const std::chrono::duration& expectedDur m_exprectedDuration = expectedDuration; } +const Timer::Info& Timer::getInfo() const +{ + return m_info; +} + void TimerHandler::setHorizon(unsigned int horizon) { m_horizon = horizon; @@ -119,41 +125,41 @@ void TimerHandler::profiling() queue.push_back(duration); // keep the queue size equal to horizon - if (queue.size() >= m_horizon) + if (queue.size() > m_horizon) { queue.pop_front(); } // this should never happen - assert(queue.size() == m_horizon); + assert(queue.size() <= m_horizon); timerDescription.averageDuration - = std::accumulate(std::next(queue.begin()), queue.end(), queue.front()); + = std::accumulate(std::next(queue.begin()), queue.end(), queue.front()) / double(queue.size()); } if(m_verbosity) { + std::stringstream ss; + std::string output; if(m_verbosityCounter == 0) { - std::cout << "|" << std::setw(30) << "name |" + ss << "|" << std::setw(30) << "name |" << std::setw(15) << "tavarg (s) |" << std::setw(10) << "dm |" - << std::setw(15) << "tdm (s) |" << std::endl; - std::cout << std::setfill('-') << "|" << std::setw(15) << "|" + << std::setw(15) << "tdm (s) |" << std::endl + << " " << std::setfill('-') << "|" << std::setw(30) << "|" << std::setw(15) << "|" << std::setw(10) << "|" << std::setw(15) << "|" << std::endl; + output = ss.str(); for (const auto& [name, timerDescription] : m_timers) { - std::cout << printTimerDescription(name, timerDescription) << std::endl; + output += " " + printTimerDescription(name, timerDescription); } - std::cout << std::setfill('-') << "|" << std::setw(30) << "|" - << std::setw(15) << "|" - << std::setw(10) << "|" - << std::setw(15) << "|" << std::endl; + yDebug() << output; } m_verbosityCounter++ ; diff --git a/devices/wholeBodyDynamics/TimeProfiler.h b/devices/wholeBodyDynamics/TimeProfiler.h index 60dbd73..4474c52 100644 --- a/devices/wholeBodyDynamics/TimeProfiler.h +++ b/devices/wholeBodyDynamics/TimeProfiler.h @@ -21,8 +21,8 @@ class Timer unsigned int deadlineMiss{0}; /**< Number of deadline miss */ std::string name{"Timer"}; /**< Name associated to the timer */ bool dealineMissDetected{false}; - std::chrono::duration latestDeadlineMissDuration; /**< Average duration. */ - std::chrono::duration duration; /**< Latest duration. */ + std::chrono::duration latestDeadlineMissDuration{0}; /**< Average duration. */ + std::chrono::duration duration{0}; /**< Latest duration. */ }; Timer(const std::string& name, const std::chrono::duration& expectedDuration); diff --git a/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp b/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp index d3aeff2..6f0787a 100644 --- a/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp +++ b/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp @@ -1639,7 +1639,7 @@ bool WholeBodyDynamicsDevice::open(os::Searchable& config) m_timerHandler.addTimer("all", WholeBodyDynamics::Timer("all", std::chrono::duration(getPeriod()))); - + m_timerHandler.setVerbosity(true); yDebug() << "wholeBodyDynamics Statistics: Configuration finished. Waiting attachAll to be called."; return true; From 09a87b92821cdf2939e285dc4cdee557ec3b3029 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Mon, 7 Mar 2022 22:11:47 +0100 Subject: [PATCH 4/7] Print all the timers when a deadline miss is detected --- devices/wholeBodyDynamics/TimeProfiler.cpp | 72 +++++++++++++--------- 1 file changed, 44 insertions(+), 28 deletions(-) diff --git a/devices/wholeBodyDynamics/TimeProfiler.cpp b/devices/wholeBodyDynamics/TimeProfiler.cpp index 11997a0..7ede91b 100644 --- a/devices/wholeBodyDynamics/TimeProfiler.cpp +++ b/devices/wholeBodyDynamics/TimeProfiler.cpp @@ -8,11 +8,11 @@ #include #include +#include #include +#include #include #include -#include -#include #include @@ -20,16 +20,14 @@ using namespace WholeBodyDynamics; - - -std::string printTimerDescription(const std::string& name, const TimerHandler::TimerDescription& description) +std::string +printTimerDescription(const std::string& name, const TimerHandler::TimerDescription& description) { std::stringstream ss; - ss << "|" << std::setw(30) << name - << "|" << std::setw(15) << std::setprecision(13) << description.averageDuration.count() - << "|" << std::setw(10) << std::setprecision(8) << description.timer.getInfo().deadlineMiss - << "|" << std::setw(15) << std::setprecision(13) << description.timer.getInfo().latestDeadlineMissDuration.count() - << "|" << std::endl; + ss << "|" << std::setw(30) << name << "|" << std::setw(15) << std::setprecision(13) + << description.averageDuration.count() << "|" << std::setw(10) << std::setprecision(8) + << description.timer.getInfo().deadlineMiss << "|" << std::setw(15) << std::setprecision(13) + << description.timer.getInfo().latestDeadlineMissDuration.count() << "|" << std::endl; return ss.str(); }; @@ -104,7 +102,6 @@ bool TimerHandler::toc(const std::string& key) return false; } - it->second.timer.toc(); return true; } @@ -116,10 +113,13 @@ void TimerHandler::setVerbosity(bool verbosity) void TimerHandler::profiling() { + bool deadlineMissDetected = false; for (auto& [name, timerDescription] : m_timers) { const auto& duration = timerDescription.timer.getInfo().duration; + deadlineMissDetected = deadlineMissDetected || timerDescription.timer.getInfo().dealineMissDetected; + // this automatically create the element if does bot exist auto& queue = m_durations[name]; queue.push_back(duration); @@ -134,36 +134,52 @@ void TimerHandler::profiling() assert(queue.size() <= m_horizon); timerDescription.averageDuration - = std::accumulate(std::next(queue.begin()), queue.end(), queue.front()) / double(queue.size()); + = std::accumulate(std::next(queue.begin()), queue.end(), queue.front()) + / double(queue.size()); } - if(m_verbosity) + if (m_verbosity) { - std::stringstream ss; - std::string output; - if(m_verbosityCounter == 0) + if (m_verbosityCounter == 0) { - ss << "|" << std::setw(30) << "name |" - << std::setw(15) << "tavarg (s) |" - << std::setw(10) << "dm |" - << std::setw(15) << "tdm (s) |" << std::endl - << " " << std::setfill('-') << "|" << std::setw(30) << "|" - << std::setw(15) << "|" - << std::setw(10) << "|" - << std::setw(15) << "|" << std::endl; + std::stringstream ss; + std::string output; + + ss << "|" << std::setw(30) << "name |" << std::setw(15) << "tavarg (s) |" + << std::setw(10) << "dm |" << std::setw(15) << "tdm (s) |" << std::endl + << " " << std::setfill('-') << "|" << std::setw(30) << "|" << std::setw(15) + << "|" << std::setw(10) << "|" << std::setw(15) << "|" << std::endl; - output = ss.str(); + output = ss.str(); for (const auto& [name, timerDescription] : m_timers) { output += " " + printTimerDescription(name, timerDescription); } - yDebug() << output; + yDebug() << output; + } + + if (deadlineMissDetected) + { + std::stringstream ss; + std::string output; + + ss << "---------------------------------- Deadline miss detected ----------------------------------" << std::endl; + ss << "|" << std::setw(30) << "name |" << std::setw(15) << "t (s) |" << std::endl + << " " << std::setfill('-') << "|" << std::setw(30) << "|" << std::setw(15) << std::endl; + + for (const auto& [name, timerDescription] : m_timers) + { + ss << "|" << std::setw(30) << name << "|" << std::setw(15) << std::setprecision(13) + << timerDescription.timer.getInfo().duration.count() << std::endl; + } + + yDebug() << ss.str(); } - m_verbosityCounter++ ; - if(m_verbosityCounter == m_horizon) + m_verbosityCounter++; + if (m_verbosityCounter == m_horizon) m_verbosityCounter = 0; } } From 5c519d25bdbe8f03b2ee1f312f5bea93efea9edf Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Wed, 9 Mar 2022 10:53:36 +0100 Subject: [PATCH 5/7] Update WholeBodyDynamicsDevice.cpp --- devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp b/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp index 6f0787a..bfb04fb 100644 --- a/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp +++ b/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp @@ -1634,8 +1634,8 @@ bool WholeBodyDynamicsDevice::open(os::Searchable& config) yDebug() << "wholeBodyDynamics Statistics: Filtered FT ports opened in " << yarp::os::Time::now() - tick << "s."; } - // print the clock once every 1.0 seconds - m_timerHandler.setHorizon(1.0/getPeriod()); + // print the clock once every 5 minutes + m_timerHandler.setHorizon(5.0 * 60 /getPeriod()); m_timerHandler.addTimer("all", WholeBodyDynamics::Timer("all", std::chrono::duration(getPeriod()))); From 32a93e16b90504835e35bdc31113df75eb2fb71a Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Thu, 25 Aug 2022 17:36:11 +0200 Subject: [PATCH 6/7] Add other timers --- .../wholeBodyDynamics/WholeBodyDynamicsDevice.cpp | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp b/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp index bfb04fb..f8cf7f1 100644 --- a/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp +++ b/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp @@ -2657,27 +2657,40 @@ void WholeBodyDynamicsDevice::publishEstimatedQuantities() // Only send estimation if a valid offset is available if( validOffsetAvailable ) { + + m_timerHandler.tic("Publish - torque"); //Send torques publishTorques(); + m_timerHandler.toc("Publish - torque"); + //Send external contacts if (useSkinForContacts) { + m_timerHandler.tic("Publish - contact"); publishContacts(); + m_timerHandler.toc("Publish - contact"); } + m_timerHandler.tic("Publish - wrench"); //Send external wrench estimates publishExternalWrenches(); + m_timerHandler.toc("Publish - wrench"); + + m_timerHandler.tic("Publish - gravity"); // Send gravity compensation torques publishGravityCompensation(); + m_timerHandler.toc("Publish - gravity"); //Send filtered inertia for gravity compensation //publishFilteredInertialForGravityCompensator(); //Send filtered force torque sensor measurment, if requested if( streamFilteredFT){ + m_timerHandler.tic("Publish - filtered FT"); publishFilteredFTWithoutOffset(); + m_timerHandler.toc("Publish - filtered FT"); } } } From c35ea2b834c2a9f9b262563a2e2f6d82c7c2250a Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Thu, 25 Aug 2022 17:36:36 +0200 Subject: [PATCH 7/7] Attempt to increase the priority --- devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp b/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp index f8cf7f1..10c08a3 100644 --- a/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp +++ b/devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp @@ -40,6 +40,8 @@ WholeBodyDynamicsDevice::WholeBodyDynamicsDevice(): yarp::os::PeriodicThread(def isIMUAttached{false}, settingsEditor(settings) { + this->setPriority(10, 2); + // Calibration quantities calibrationBuffers.ongoingCalibration = false; calibrationBuffers.calibratingFTsensor.resize(0);