Merge pull request #52 from robotology/performanceImprovements

Performance improvements
robotology · Aug 26, 2022 · fc84272 · fc84272
2 parents 48a8a1d + dff2890
commit fc84272
Show file tree

Hide file tree

Showing 2 changed files with 17 additions and 10 deletions.
diff --git a/include/UnicyclePlanner.h b/include/UnicyclePlanner.h
@@ -13,7 +13,7 @@
 #include "UnicycleOptimization.h"
 #include "UnicycleFoot.h"
 #include "FreeSpaceEllipse.h"
-#include <iDynTree/Integrators/RK4.h>
+#include <iDynTree/Integrators/ForwardEuler.h>
 #include <memory>
 #include <mutex>
 
@@ -27,7 +27,7 @@ enum class FreeSpaceEllipseMethod
 class UnicyclePlanner {
  std::shared_ptr<UnicyleController> m_controller;
  std::shared_ptr<ControlledUnicycle> m_unicycle;
- iDynTree::optimalcontrol::integrators::RK4 m_integrator;
+ iDynTree::optimalcontrol::integrators::ForwardEuler m_integrator;
  UnicycleOptimization m_unicycleProblem;
  double m_initTime, m_endTime, m_minTime, m_maxTime, m_nominalTime, m_dT, m_minAngle, m_nominalWidth, m_maxLength, m_minLength, m_maxAngle;
  bool m_addTerminalStep, m_startLeft, m_resetStartingFoot, m_firstStep;

diff --git a/src/UnicyclePlanner.cpp b/src/UnicyclePlanner.cpp
@@ -224,14 +224,21 @@ bool UnicyclePlanner::get_rPl(const UnicycleState &unicycleState, iDynTree::Vect
 
 bool UnicyclePlanner::getIntegratorSolution(double time, UnicycleState &unicycleState) const
 {
- static iDynTree::VectorDynSize stateBuffer(3);
- if(!m_integrator.getSolution(time,stateBuffer)){
- std::cerr << "Error while retrieving the integrator solution." << std::endl;
- return false;
- }
- unicycleState.position(0) = stateBuffer(0);
- unicycleState.position(1) = stateBuffer(1);
- unicycleState.angle = stateBuffer(2);
+ auto& fullSolution = m_integrator.getFullSolution();
+
+ assert(fullSolution.size() != 0 && "Error while retrieving the integrator solution. No solution available.");
+
+ double initialTime = fullSolution.front().time;
+
+ assert(time >= initialTime && time <= fullSolution.back().time && "Error while retrieving the integrator solution. Time out of bounds.");
+
+ size_t index = static_cast<size_t>(std::round((time - initialTime)/m_integrator.maximumStepSize()));
+ index = std::min(index, fullSolution.size() - 1);
+ auto& state = fullSolution[index].stateAtT;
+
+ unicycleState.position(0) = state(0);
+ unicycleState.position(1) = state(1);
+ unicycleState.angle = state(2);
  return true;
 }