From 35a0fc2dad68e0a4e4ce1add637723a0541a7b87 Mon Sep 17 00:00:00 2001 From: Stefano Date: Tue, 23 Aug 2022 18:54:51 +0200 Subject: [PATCH 1/3] Avoid to use getSolution from the integrator. --- src/UnicyclePlanner.cpp | 23 +++++++++++++++-------- 1 file changed, 15 insertions(+), 8 deletions(-) diff --git a/src/UnicyclePlanner.cpp b/src/UnicyclePlanner.cpp index 995a783..19dcda2 100644 --- 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(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; } From 9e116ac586e5b90186dedaebe8defbdfa024bc2d Mon Sep 17 00:00:00 2001 From: Stefano Date: Tue, 23 Aug 2022 18:55:18 +0200 Subject: [PATCH 2/3] Use forward euler instead of RK4. --- include/UnicyclePlanner.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/include/UnicyclePlanner.h b/include/UnicyclePlanner.h index 801dd20..ecd7b42 100644 --- a/include/UnicyclePlanner.h +++ b/include/UnicyclePlanner.h @@ -13,7 +13,7 @@ #include "UnicycleOptimization.h" #include "UnicycleFoot.h" #include "FreeSpaceEllipse.h" -#include +#include #include #include @@ -27,7 +27,7 @@ enum class FreeSpaceEllipseMethod class UnicyclePlanner { std::shared_ptr m_controller; std::shared_ptr 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; From dff28907da7359bf5187807aa1b2de345e79e7a3 Mon Sep 17 00:00:00 2001 From: Stefano Dafarra Date: Wed, 24 Aug 2022 15:20:56 +0200 Subject: [PATCH 3/3] Fixed assert on time validity --- src/UnicyclePlanner.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/UnicyclePlanner.cpp b/src/UnicyclePlanner.cpp index 19dcda2..0d3d604 100644 --- a/src/UnicyclePlanner.cpp +++ b/src/UnicyclePlanner.cpp @@ -230,7 +230,7 @@ bool UnicyclePlanner::getIntegratorSolution(double time, UnicycleState &unicycle double initialTime = fullSolution.front().time; - assert(time > initialTime && time < fullSolution.back().time && "Error while retrieving the integrator solution. Time out of bounds."); + assert(time >= initialTime && time <= fullSolution.back().time && "Error while retrieving the integrator solution. Time out of bounds."); size_t index = static_cast(std::round((time - initialTime)/m_integrator.maximumStepSize())); index = std::min(index, fullSolution.size() - 1);