Merge branch 'fix/loopshapingPrimalSolution' into feature/perception

rubengrandia · rubengrandia · commit f719cae8dd96 · 2022-07-28T11:11:41.000+02:00
diff --git a/ocs2_oc/src/oc_data/LoopshapingPrimalSolution.cpp b/ocs2_oc/src/oc_data/LoopshapingPrimalSolution.cpp
@@ -40,8 +40,8 @@ PrimalSolution loopshapingToSystemPrimalSolution(const PrimalSolution& primalSol
   systemPrimalSolution.inputTrajectory_.reserve(primalSolution.inputTrajectory_.size());
   for (size_t k = 0; k < primalSolution.stateTrajectory_.size(); ++k) {
     systemPrimalSolution.stateTrajectory_.push_back(loopshapingDefinition.getSystemState(primalSolution.stateTrajectory_[k]));
-    const auto& systemState = systemPrimalSolution.stateTrajectory_.back();
-    systemPrimalSolution.inputTrajectory_.push_back(loopshapingDefinition.getSystemInput(systemState, primalSolution.inputTrajectory_[k]));
+    systemPrimalSolution.inputTrajectory_.push_back(
+        loopshapingDefinition.getSystemInput(primalSolution.stateTrajectory_[k], primalSolution.inputTrajectory_[k]));
   }
   return systemPrimalSolution;
 }

Original file line number	Diff line number	Diff line change
`@@ -40,8 +40,8 @@ PrimalSolution loopshapingToSystemPrimalSolution(const PrimalSolution& primalSol`
`40`	`40`	`systemPrimalSolution.inputTrajectory_.reserve(primalSolution.inputTrajectory_.size());`
`41`	`41`	`for (size_t k = 0; k < primalSolution.stateTrajectory_.size(); ++k) {`
`42`	`42`	`systemPrimalSolution.stateTrajectory_.push_back(loopshapingDefinition.getSystemState(primalSolution.stateTrajectory_[k]));`
`43`		`- const auto& systemState = systemPrimalSolution.stateTrajectory_.back();`
`44`		`- systemPrimalSolution.inputTrajectory_.push_back(loopshapingDefinition.getSystemInput(systemState, primalSolution.inputTrajectory_[k]));`
	`43`	`+ systemPrimalSolution.inputTrajectory_.push_back(`
	`44`	`+ loopshapingDefinition.getSystemInput(primalSolution.stateTrajectory_[k], primalSolution.inputTrajectory_[k]));`
`45`	`45`	`}`
`46`	`46`	`return systemPrimalSolution;`
`47`	`47`	`}`