renaming multishooting to sqp

rubengrandia · rubengrandia · commit 3c68c05cc979 · 2023-01-29T17:02:21.000+01:00
diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/src/AnymalLoopshapingMpcNode.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/src/AnymalLoopshapingMpcNode.cpp
@@ -38,8 +38,7 @@ int main(int argc, char* argv[]) {
       break;
     }
     case switched_model::Algorithm::SQP: {
-      const auto sqpSettings =
-          ocs2::multiple_shooting::loadSettings(anymal::getConfigFolderLoopshaping(configName) + "/multiple_shooting.info");
+      const auto sqpSettings = ocs2::sqp::loadSettings(anymal::getConfigFolderLoopshaping(configName) + "/multiple_shooting.info");
       auto mpcPtr = getSqpMpc(*anymalInterface, mpcSettings, sqpSettings);
       quadrupedLoopshapingMpcNode(nodeHandle, *anymalInterface, std::move(mpcPtr));
       break;
diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/src/PerceptiveMpcDemo.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/src/PerceptiveMpcDemo.cpp
@@ -188,7 +188,7 @@ int main(int argc, char* argv[]) {
   const auto mpcSettings = ocs2::mpc::loadSettings(taskFolder + configName + "/task.info");
 
   std::unique_ptr<ocs2::MPC_BASE> mpcPtr;
-  const auto sqpSettings = ocs2::multiple_shooting::loadSettings(taskFolder + configName + "/multiple_shooting.info");
+  const auto sqpSettings = ocs2::sqp::loadSettings(taskFolder + configName + "/multiple_shooting.info");
   switch (anymalInterface->modelSettings().algorithm_) {
     case switched_model::Algorithm::DDP: {
       const auto ddpSettings = ocs2::ddp::loadSettings(taskFolder + configName + "/task.info");
diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/test/testMotionTracking.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/test/testMotionTracking.cpp
@@ -37,7 +37,7 @@ class TestAnymalLoopshapingMpc : public ::testing::Test {
     problem.targetTrajectoriesPtr = &targetTrajectories;
 
     const auto mpcSettings = ocs2::mpc::loadSettings(configFolder + "/task.info");
-    const auto sqpSettings = ocs2::multiple_shooting::loadSettings(configFolder + "/multiple_shooting.info");
+    const auto sqpSettings = ocs2::sqp::loadSettings(configFolder + "/multiple_shooting.info");
     mpcPtr = switched_model_loopshaping::getSqpMpc(*anymalInterface, mpcSettings, sqpSettings);
 
     // Initialize
diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/test/testSensitivity.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_loopshaping_mpc/test/testSensitivity.cpp
@@ -25,7 +25,7 @@ TEST(TestAnymalLoopshapingMotionTracking, testSensitivity) {
   const auto csvData = switched_model::readCsv(motionFilesPath + motionName + ".txt");
   const auto motionData = switched_model::readMotion(csvData, 0.05);
   const auto mpcSettings = ocs2::mpc::loadSettings(configFolder + "/task.info");
-  const auto sqpSettings = ocs2::multiple_shooting::loadSettings(configFolder + "/multiple_shooting.info");
+  const auto sqpSettings = ocs2::sqp::loadSettings(configFolder + "/multiple_shooting.info");
   auto quadrupedSettings = switched_model::loadQuadrupedSettings(configFolder + "/task.info");
   const auto frameDecl = anymal::frameDeclarationFromFile(configFolder + "/frame_declaration.info");
   auto loopshapingDefinition = ocs2::loopshaping_property_tree::load(configFolder + "/loopshaping.info");
diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_mpc/src/AnymalMpcNode.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_anymal_mpc/src/AnymalMpcNode.cpp
@@ -41,7 +41,7 @@ int main(int argc, char* argv[]) {
       break;
     }
     case switched_model::Algorithm::SQP: {
-      const auto sqpSettings = ocs2::multiple_shooting::loadSettings(anymal::getConfigFolder(configName) + "/multiple_shooting.info");
+      const auto sqpSettings = ocs2::sqp::loadSettings(anymal::getConfigFolder(configName) + "/multiple_shooting.info");
       auto mpcPtr = getSqpMpc(*anymalInterface, mpcSettings, sqpSettings);
       quadrupedMpcNode(nodeHandle, *anymalInterface, std::move(mpcPtr));
       break;
diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/include/ocs2_quadruped_interface/QuadrupedMpc.h b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/include/ocs2_quadruped_interface/QuadrupedMpc.h
@@ -7,7 +7,7 @@
 #include <ocs2_ddp/DDP_Settings.h>
 #include <ocs2_mpc/MPC_BASE.h>
 #include <ocs2_mpc/MPC_Settings.h>
-#include <ocs2_sqp/MultipleShootingSettings.h>
+#include <ocs2_sqp/SqpSettings.h>
 
 #include "ocs2_quadruped_interface/QuadrupedInterface.h"
 
@@ -19,6 +19,6 @@ std::unique_ptr<ocs2::MPC_BASE> getDdpMpc(const QuadrupedInterface& quadrupedInt
 
 /** Constructs an SQP MPC object */
 std::unique_ptr<ocs2::MPC_BASE> getSqpMpc(const QuadrupedInterface& quadrupedInterface, const ocs2::mpc::Settings& mpcSettings,
-                                          const ocs2::multiple_shooting::Settings& sqpSettings);
+                                          const ocs2::sqp::Settings& sqpSettings);
 
 }  // namespace switched_model
diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/src/QuadrupedMpc.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_interface/src/QuadrupedMpc.cpp
@@ -5,7 +5,7 @@
 #include "ocs2_quadruped_interface/QuadrupedMpc.h"
 
 #include <ocs2_ddp/GaussNewtonDDP_MPC.h>
-#include <ocs2_sqp/MultipleShootingMpc.h>
+#include <ocs2_sqp/SqpMpc.h>
 
 namespace switched_model {
 
@@ -20,9 +20,9 @@ std::unique_ptr<ocs2::MPC_BASE> getDdpMpc(const QuadrupedInterface& quadrupedInt
 }
 
 std::unique_ptr<ocs2::MPC_BASE> getSqpMpc(const QuadrupedInterface& quadrupedInterface, const ocs2::mpc::Settings& mpcSettings,
-                                          const ocs2::multiple_shooting::Settings& sqpSettings) {
-  std::unique_ptr<ocs2::MPC_BASE> mpcPtr(new ocs2::MultipleShootingMpc(
-      mpcSettings, sqpSettings, quadrupedInterface.getOptimalControlProblem(), quadrupedInterface.getInitializer()));
+                                          const ocs2::sqp::Settings& sqpSettings) {
+  std::unique_ptr<ocs2::MPC_BASE> mpcPtr(
+      new ocs2::SqpMpc(mpcSettings, sqpSettings, quadrupedInterface.getOptimalControlProblem(), quadrupedInterface.getInitializer()));
   mpcPtr->getSolverPtr()->setReferenceManager(quadrupedInterface.getReferenceManagerPtr());
   mpcPtr->getSolverPtr()->setSynchronizedModules(quadrupedInterface.getSynchronizedModules());
   return mpcPtr;
diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_loopshaping_interface/include/ocs2_quadruped_loopshaping_interface/QuadrupedLoopshapingMpc.h b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_loopshaping_interface/include/ocs2_quadruped_loopshaping_interface/QuadrupedLoopshapingMpc.h
@@ -7,7 +7,7 @@
 #include <ocs2_ddp/DDP_Settings.h>
 #include <ocs2_mpc/MPC_BASE.h>
 #include <ocs2_mpc/MPC_Settings.h>
-#include <ocs2_sqp/MultipleShootingSettings.h>
+#include <ocs2_sqp/SqpSettings.h>
 
 #include "ocs2_quadruped_loopshaping_interface/QuadrupedLoopshapingInterface.h"
 
@@ -19,6 +19,6 @@ std::unique_ptr<ocs2::MPC_BASE> getDdpMpc(const QuadrupedLoopshapingInterface& q
 
 /** Constructs an SQP MPC object */
 std::unique_ptr<ocs2::MPC_BASE> getSqpMpc(const QuadrupedLoopshapingInterface& quadrupedInterface, const ocs2::mpc::Settings& mpcSettings,
-                                          const ocs2::multiple_shooting::Settings& sqpSettings);
+                                          const ocs2::sqp::Settings& sqpSettings);
 
 }  // namespace switched_model_loopshaping
diff --git a/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_loopshaping_interface/src/QuadrupedLoopshapingMpc.cpp b/ocs2_robotic_examples/ocs2_perceptive_anymal/ocs2_quadruped_loopshaping_interface/src/QuadrupedLoopshapingMpc.cpp
@@ -5,7 +5,7 @@
 #include "ocs2_quadruped_loopshaping_interface/QuadrupedLoopshapingMpc.h"
 
 #include <ocs2_ddp/GaussNewtonDDP_MPC.h>
-#include <ocs2_sqp/MultipleShootingMpc.h>
+#include <ocs2_sqp/SqpMpc.h>
 
 namespace switched_model_loopshaping {
 
@@ -20,9 +20,9 @@ std::unique_ptr<ocs2::MPC_BASE> getDdpMpc(const QuadrupedLoopshapingInterface& q
 }
 
 std::unique_ptr<ocs2::MPC_BASE> getSqpMpc(const QuadrupedLoopshapingInterface& quadrupedInterface, const ocs2::mpc::Settings& mpcSettings,
-                                          const ocs2::multiple_shooting::Settings& sqpSettings) {
-  std::unique_ptr<ocs2::MPC_BASE> mpcPtr(new ocs2::MultipleShootingMpc(
-      mpcSettings, sqpSettings, quadrupedInterface.getOptimalControlProblem(), quadrupedInterface.getInitializer()));
+                                          const ocs2::sqp::Settings& sqpSettings) {
+  std::unique_ptr<ocs2::MPC_BASE> mpcPtr(
+      new ocs2::SqpMpc(mpcSettings, sqpSettings, quadrupedInterface.getOptimalControlProblem(), quadrupedInterface.getInitializer()));
   mpcPtr->getSolverPtr()->setReferenceManager(quadrupedInterface.getReferenceManagerPtr());
   mpcPtr->getSolverPtr()->setSynchronizedModules({quadrupedInterface.getLoopshapingSynchronizedModule()});
   return mpcPtr;
diff --git a/ocs2_sqp/ocs2_sqp/CMakeLists.txt b/ocs2_sqp/ocs2_sqp/CMakeLists.txt
@@ -50,7 +50,7 @@ include_directories(
 
 # Multiple shooting solver library
 add_library(${PROJECT_NAME}
-  src/MultipleShootingLogging.cpp
+  src/SqpLogging.cpp
   src/SqpSettings.cpp
   src/SqpSolver.cpp
 )
diff --git a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpLogging.h b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpLogging.h
@@ -34,12 +34,12 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <ocs2_core/Types.h>
 
-#include <ocs2_oc/oc_solver/PerformanceIndex.h>
+#include <ocs2_oc/oc_data/PerformanceIndex.h>
 
-#include "ocs2_sqp/MultipleShootingSolverStatus.h"
+#include "ocs2_sqp/SqpSolverStatus.h"
 
 namespace ocs2 {
-namespace multiple_shooting {
+namespace sqp {
 
 struct LogEntry {
   // Problem info
@@ -143,5 +143,5 @@ void Logger<T>::write(std::ostream& stream) const {
   }
 }
 
-}  // namespace multiple_shooting
+}  // namespace sqp
 }  // namespace ocs2
diff --git a/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpSolver.h b/ocs2_sqp/ocs2_sqp/include/ocs2_sqp/SqpSolver.h
@@ -42,7 +42,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <hpipm_catkin/HpipmInterface.h>
 
-#include "ocs2_sqp/MultipleShootingLogging.h"
+#include "ocs2_sqp/SqpLogging.h"
 #include "ocs2_sqp/SqpSettings.h"
 #include "ocs2_sqp/SqpSolverStatus.h"
 
@@ -186,8 +186,9 @@ class SqpSolver : public SolverBase {
   ProblemMetrics problemMetrics_;
 
   // Benchmarking
+  size_t numProblems_{0};
   size_t totalNumIterations_{0};
-  multiple_shooting::Logger<multiple_shooting::LogEntry> logger_;
+  sqp::Logger<sqp::LogEntry> logger_;
   benchmark::RepeatedTimer initializationTimer_;
   benchmark::RepeatedTimer linearQuadraticApproximationTimer_;
   benchmark::RepeatedTimer solveQpTimer_;
diff --git a/ocs2_sqp/ocs2_sqp/src/SqpLogging.cpp b/ocs2_sqp/ocs2_sqp/src/SqpLogging.cpp
@@ -27,12 +27,12 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 ******************************************************************************/
 
-#include "ocs2_sqp/MultipleShootingLogging.h"
+#include "ocs2_sqp/SqpLogging.h"
 
 #include <iomanip>
 
 namespace ocs2 {
-namespace multiple_shooting {
+namespace sqp {
 
 std::ostream& operator<<(std::ostream& stream, const LogEntry& logEntry) {
   const std::string delim = ", ";
@@ -90,5 +90,5 @@ std::string logHeader() {
   return stream.str();
 }
 
-}  // namespace multiple_shooting
+}  // namespace sqp
 }  // namespace ocs2
diff --git a/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp b/ocs2_sqp/ocs2_sqp/src/SqpSolver.cpp
@@ -33,6 +33,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <iostream>
 #include <numeric>
 
+#include <boost/filesystem.hpp>
+
 #include <ocs2_oc/multiple_shooting/Helpers.h>
 #include <ocs2_oc/multiple_shooting/Initialization.h>
 #include <ocs2_oc/multiple_shooting/MetricsComputation.h>
@@ -98,11 +100,11 @@ SqpSolver::~SqpSolver() {
     // Write to file
     const std::string logFileName = settings_.logFilePath + "log_" + timeStamp + ".txt";
     if (std::ofstream logfile{logFileName}) {
-      logfile << multiple_shooting::logHeader();
+      logfile << sqp::logHeader();
       logger_.write(logfile);
-      std::cerr << "[MultipleShootingSolver] Log written to '" << logFileName << "'\n";
+      std::cerr << "[SqpSolver] Log written to '" << logFileName << "'\n";
     } else {
-      std::cerr << "[MultipleShootingSolver] Unable to open '" << logFileName << "'\n";
+      std::cerr << "[SqpSolver] Unable to open '" << logFileName << "'\n";
     }
   }
 }
@@ -114,8 +116,9 @@ void SqpSolver::reset() {
   performanceIndeces_.clear();
 
   // reset timers
+  numProblems_ = 0;
   totalNumIterations_ = 0;
-  logger_ = multiple_shooting::Logger<multiple_shooting::LogEntry>(settings_.logSize);
+  logger_ = sqp::Logger<sqp::LogEntry>(settings_.logSize);
   linearQuadraticApproximationTimer_.reset();
   solveQpTimer_.reset();
   linesearchTimer_.reset();
@@ -244,7 +247,7 @@ void SqpSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t f
       logEntry.solveQpTime = solveQpTimer_.getLastIntervalInMilliseconds();
       logEntry.linesearchTime = linesearchTimer_.getLastIntervalInMilliseconds();
       logEntry.baselinePerformanceIndex = baselinePerformance;
-      logEntry.totalConstraintViolationBaseline = totalConstraintViolation(baselinePerformance);
+      logEntry.totalConstraintViolationBaseline = FilterLinesearch::totalConstraintViolation(baselinePerformance);
       logEntry.stepInfo = stepInfo;
       logEntry.convergence = convergence;
       logger_.advance();
@@ -255,6 +258,8 @@ void SqpSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t f
     ++totalNumIterations_;
   }
 
+  ++numProblems_;
+
   computeControllerTimer_.startTimer();
   primalSolution_ = toPrimalSolution(timeDiscretization, std::move(x), std::move(u));
   problemMetrics_ = multiple_shooting::toProblemMetrics(timeDiscretization, std::move(metrics));
diff --git a/ocs2_sqp/ocs2_sqp/test/testLogging.cpp b/ocs2_sqp/ocs2_sqp/test/testLogging.cpp
@@ -29,13 +29,13 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <gtest/gtest.h>
 
-#include "ocs2_sqp/MultipleShootingLogging.h"
+#include "ocs2_sqp/SqpLogging.h"
 
 using namespace ocs2;
 
 TEST(test_logging, wrap_around) {
   const int Ntest = 3;
-  multiple_shooting::Logger<int> logger(Ntest);
+  sqp::Logger<int> logger(Ntest);
 
   std::stringstream stream;
 

Original file line number	Diff line number	Diff line change
`@@ -41,7 +41,7 @@ int main(int argc, char* argv[]) {`
`41`	`41`	`break;`
`42`	`42`	`}`
`43`	`43`	`case switched_model::Algorithm::SQP: {`
`44`		`- const auto sqpSettings = ocs2::multiple_shooting::loadSettings(anymal::getConfigFolder(configName) + "/multiple_shooting.info");`
	`44`	`+ const auto sqpSettings = ocs2::sqp::loadSettings(anymal::getConfigFolder(configName) + "/multiple_shooting.info");`
`45`	`45`	`auto mpcPtr = getSqpMpc(*anymalInterface, mpcSettings, sqpSettings);`
`46`	`46`	`quadrupedMpcNode(nodeHandle, *anymalInterface, std::move(mpcPtr));`
`47`	`47`	`break;`
Original file line number	Diff line number	Diff line change
`@@ -50,7 +50,7 @@ include_directories(`
`50`	`50`
`51`	`51`	`# Multiple shooting solver library`
`52`	`52`	`add_library(${PROJECT_NAME}`
`53`		`- src/MultipleShootingLogging.cpp`
	`53`	`+ src/SqpLogging.cpp`
`54`	`54`	`src/SqpSettings.cpp`
`55`	`55`	`src/SqpSolver.cpp`
`56`	`56`	`)`