From 21d49472cb6c82d243bf11529afae1a3f2715f41 Mon Sep 17 00:00:00 2001 From: Giulio Romualdi Date: Thu, 25 Aug 2022 12:42:41 +0000 Subject: [PATCH] Increase the frequency of iCubGenova09 up to 250Hz --- .../iCubGenova09/dcm_walking_with_joypad.ini | 2 +- src/WalkingModule/src/Module.cpp | 16 ++++++++++++---- 2 files changed, 13 insertions(+), 5 deletions(-) diff --git a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking_with_joypad.ini b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking_with_joypad.ini index 8b1a73f45..dbb843e7e 100644 --- a/src/WalkingModule/app/robots/iCubGenova09/dcm_walking_with_joypad.ini +++ b/src/WalkingModule/app/robots/iCubGenova09/dcm_walking_with_joypad.ini @@ -36,7 +36,7 @@ name walking-coordinator # height of the com com_height 0.565 # sampling time -sampling_time 0.01 +sampling_time 0.004 # Specify the frame to use to control the robot height. Currently, we support only the following options: com, root_link height_reference_frame root_link diff --git a/src/WalkingModule/src/Module.cpp b/src/WalkingModule/src/Module.cpp index 18bf2ffff..abe561f03 100644 --- a/src/WalkingModule/src/Module.cpp +++ b/src/WalkingModule/src/Module.cpp @@ -28,6 +28,8 @@ #include #include +constexpr std::size_t askNewTrajectoryIndex = 80; + using namespace WalkingControllers; void WalkingModule::propagateTime() @@ -382,6 +384,7 @@ bool WalkingModule::configure(yarp::os::ResourceFinder& rf) m_profiler->addTimer("MPC"); m_profiler->addTimer("IK"); + m_profiler->addTimer("Send Data"); m_profiler->addTimer("Total"); // initialize some variables @@ -649,7 +652,7 @@ bool WalkingModule::updateModule() if(m_newTrajectoryRequired) { // when we are near to the merge point the new trajectory is evaluated - if(m_newTrajectoryMergeCounter == 20) + if(m_newTrajectoryMergeCounter == askNewTrajectoryIndex) { double initTimeTrajectory; @@ -922,6 +925,9 @@ bool WalkingModule::updateModule() // send data to the logger if(m_dumpData) { + + m_profiler->setInitTime("Send Data"); + iDynTree::Vector2 desiredZMP; if(m_useMPC) desiredZMP = m_walkingController->getControllerOutput(); @@ -1019,6 +1025,8 @@ bool WalkingModule::updateModule() m_loggerPort.write(); + m_profiler->setEndTime("Send Data"); + } // in the approaching phase the robot should not move and the trajectories should not advance @@ -1534,13 +1542,13 @@ bool WalkingModule::setPlannerInput(double x, double y) return true; // Since the evaluation of a new trajectory takes time the new trajectory will be merged after x cycles - m_newTrajectoryMergeCounter = 20; + m_newTrajectoryMergeCounter = askNewTrajectoryIndex; } // the trajectory was not finished the new trajectory will be attached at the next merge point else { - if(m_mergePoints.front() > 20) + if(m_mergePoints.front() > askNewTrajectoryIndex) m_newTrajectoryMergeCounter = m_mergePoints.front(); else if(m_mergePoints.size() > 1) { @@ -1554,7 +1562,7 @@ bool WalkingModule::setPlannerInput(double x, double y) if(m_newTrajectoryRequired) return true; - m_newTrajectoryMergeCounter = 20; + m_newTrajectoryMergeCounter = askNewTrajectoryIndex; } }