rename from ocs2_pipg to ocs2_slp

Author: farbod-farshidian

ocs2/package.xml

@@ -16,8 +16,8 @@
   <exec_depend>ocs2_oc</exec_depend>
   <exec_depend>ocs2_qp_solver</exec_depend>
   <exec_depend>ocs2_ddp</exec_depend>
+  <exec_depend>ocs2_slp</exec_depend>
   <exec_depend>ocs2_sqp</exec_depend>
-  <exec_depend>ocs2_pipg</exec_depend>
   <exec_depend>ocs2_ros_interfaces</exec_depend>
   <exec_depend>ocs2_python_interface</exec_depend>
   <exec_depend>ocs2_pinocchio</exec_depend>

ocs2_pipg/CMakeLists.txt renamed to ocs2_slp/CMakeLists.txt

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.0.2)
-project(ocs2_pipg)
+project(ocs2_slp)
 
 set(CATKIN_PACKAGE_DEPENDENCIES
   ocs2_core
@@ -45,11 +45,11 @@ include_directories(
 )
 
 add_library(${PROJECT_NAME}
+  src/pipg/PipgSettings.cpp
+  src/pipg/PipgSolver.cpp
   src/Helpers.cpp
-  src/PipgSettings.cpp
-  src/PipgSolver.cpp
   src/SlpSettings.cpp
-  src/mpc/PipgMpcSolver.cpp
+  src/SlpSolver.cpp
 )
 add_dependencies(${PROJECT_NAME}
   ${catkin_EXPORTED_TARGETS}
@@ -60,14 +60,18 @@ target_link_libraries(${PROJECT_NAME}
 )
 target_compile_options(${PROJECT_NAME} PUBLIC ${OCS2_CXX_FLAGS})
 
+add_executable(${PROJECT_NAME}_lintTarget
+  src/lintTarget.cpp
+)
+
 #########################
 ###   CLANG TOOLING   ###
 #########################
 find_package(cmake_clang_tools QUIET)
 if(cmake_clang_tools_FOUND)
-  message(STATUS "Run clang tooling for target " ${PROJECT_NAME})
+  message(STATUS "Run clang tooling for target " ${PROJECT_NAME}_lintTarget)
   add_clang_tooling(
-    TARGETS ${PROJECT_NAME}
+    TARGETS ${PROJECT_NAME}_lintTarget ${PROJECT_NAME}
     SOURCE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/include ${CMAKE_CURRENT_SOURCE_DIR}/test
     CT_HEADER_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include
     CF_WERROR

ocs2_pipg/include/ocs2_pipg/Helpers.h renamed to ocs2_slp/include/ocs2_slp/Helpers.h

@@ -29,32 +29,32 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #pragma once
 
-#include "ocs2_pipg/mpc/PipgMpcSolver.h"
-
 #include <ocs2_mpc/MPC_BASE.h>
 
+#include "ocs2_slp/SlpSolver.h"
+
 namespace ocs2 {
-class PipgMpc final : public MPC_BASE {
+class SlpMpc final : public MPC_BASE {
  public:
   /**
    * Constructor
    *
    * @param mpcSettings : settings for the mpc wrapping of the solver. Do not use this for maxIterations and stepsize, use multiple shooting
    * settings directly.
-   * @param settings : settings for the multiple shooting solver.
+   * @param settings : settings for the multiple shooting SLP solver.
    * @param [in] optimalControlProblem: The optimal control problem formulation.
    * @param [in] initializer: This class initializes the state-input for the time steps that no controller is available.
    */
-  PipgMpc(mpc::Settings mpcSettings, multiple_shooting::Settings settings, pipg::Settings pipgSetting,
-          const OptimalControlProblem& optimalControlProblem, const Initializer& initializer)
+  SlpMpc(mpc::Settings mpcSettings, slp::Settings slpSettings, const OptimalControlProblem& optimalControlProblem,
+         const Initializer& initializer)
       : MPC_BASE(std::move(mpcSettings)) {
-    solverPtr_.reset(new PipgMpcSolver(std::move(settings), std::move(pipgSetting), optimalControlProblem, initializer));
+    solverPtr_.reset(new SlpSolver(std::move(slpSettings), optimalControlProblem, initializer));
   };
 
-  ~PipgMpc() override = default;
+  ~SlpMpc() override = default;
 
-  PipgMpcSolver* getSolverPtr() override { return solverPtr_.get(); }
-  const PipgMpcSolver* getSolverPtr() const override { return solverPtr_.get(); }
+  SlpSolver* getSolverPtr() override { return solverPtr_.get(); }
+  const SlpSolver* getSolverPtr() const override { return solverPtr_.get(); }
 
  protected:
   void calculateController(scalar_t initTime, const vector_t& initState, scalar_t finalTime) override {
@@ -65,6 +65,6 @@ class PipgMpc final : public MPC_BASE {
   }
 
  private:
-  std::unique_ptr<PipgMpcSolver> solverPtr_;
+  std::unique_ptr<SlpSolver> solverPtr_;
 };
 }  // namespace ocs2

ocs2_pipg/include/ocs2_pipg/mpc/PipgMpc.h renamed to ocs2_slp/include/ocs2_slp/SlpMpc.h

@@ -32,7 +32,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <ocs2_core/Types.h>
 #include <ocs2_core/integration/SensitivityIntegrator.h>
 
-#include "ocs2_pipg/PipgSettings.h"
+#include "ocs2_slp/pipg/PipgSettings.h"
 
 namespace ocs2 {
 namespace slp {

ocs2_pipg/include/ocs2_pipg/SlpSettings.h renamed to ocs2_slp/include/ocs2_slp/SlpSettings.h

@@ -39,13 +39,13 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <ocs2_oc/oc_solver/SolverBase.h>
 #include <ocs2_oc/search_strategy/FilterLinesearch.h>
 
-#include "ocs2_pipg/PipgSolver.h"
-#include "ocs2_pipg/SlpSettings.h"
-#include "ocs2_pipg/SlpSolverStatus.h"
+#include "ocs2_slp/SlpSettings.h"
+#include "ocs2_slp/SlpSolverStatus.h"
+#include "ocs2_slp/pipg/PipgSolver.h"
 
 namespace ocs2 {
 
-class PipgMpcSolver : public SolverBase {
+class SlpSolver : public SolverBase {
  public:
   /**
    * Constructor
@@ -54,9 +54,9 @@ class PipgMpcSolver : public SolverBase {
    * @param [in] optimalControlProblem: The optimal control problem formulation.
    * @param [in] initializer: This class initializes the state-input for the time steps that no controller is available.
    */
-  PipgMpcSolver(slp::Settings settings, const OptimalControlProblem& optimalControlProblem, const Initializer& initializer);
+  SlpSolver(slp::Settings settings, const OptimalControlProblem& optimalControlProblem, const Initializer& initializer);
 
-  ~PipgMpcSolver() override;
+  ~SlpSolver() override;
 
   void reset() override;
 
@@ -79,15 +79,15 @@ class PipgMpcSolver : public SolverBase {
   const std::vector<PerformanceIndex>& getIterationsLog() const override;
 
   ScalarFunctionQuadraticApproximation getValueFunction(scalar_t time, const vector_t& state) const override {
-    throw std::runtime_error("[PipgMpcSolver] getValueFunction() not available yet.");
+    throw std::runtime_error("[SlpSolver] getValueFunction() not available yet.");
   };
 
   ScalarFunctionQuadraticApproximation getHamiltonian(scalar_t time, const vector_t& state, const vector_t& input) override {
-    throw std::runtime_error("[PipgMpcSolver] getHamiltonian() not available yet.");
+    throw std::runtime_error("[SlpSolver] getHamiltonian() not available yet.");
   }
 
   vector_t getStateInputEqualityConstraintLagrangian(scalar_t time, const vector_t& state) const override {
-    throw std::runtime_error("[PipgMpcSolver] getStateInputEqualityConstraintLagrangian() not available yet.");
+    throw std::runtime_error("[SlpSolver] getStateInputEqualityConstraintLagrangian() not available yet.");
   }
 
   MultiplierCollection getIntermediateDualSolution(scalar_t time) const override {
@@ -101,7 +101,7 @@ class PipgMpcSolver : public SolverBase {
     if (externalControllerPtr == nullptr) {
       runImpl(initTime, initState, finalTime);
     } else {
-      throw std::runtime_error("[PipgMpcSolver::run] This solver does not support external controller!");
+      throw std::runtime_error("[SlpSolver::run] This solver does not support external controller!");
     }
   }
 

ocs2_pipg/include/ocs2_pipg/mpc/PipgMpcSolver.h renamed to ocs2_slp/include/ocs2_slp/SlpSolver.h

@@ -38,8 +38,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <ocs2_core/thread_support/ThreadPool.h>
 #include <ocs2_oc/oc_problem/OcpSize.h>
 
-#include "ocs2_pipg/PipgSettings.h"
-#include "ocs2_pipg/PipgSolverStatus.h"
+#include "ocs2_slp/pipg/PipgSettings.h"
+#include "ocs2_slp/pipg/PipgSolverStatus.h"
 
 namespace ocs2 {
 

ocs2_pipg/include/ocs2_pipg/SlpSolverStatus.h renamed to ocs2_slp/include/ocs2_slp/SlpSolverStatus.h

@@ -1,8 +1,8 @@
 <?xml version="1.0"?>
 <package format="2">
-  <name>ocs2_pipg</name>
+  <name>ocs2_slp</name>
   <version>0.0.0</version>
-  <description>A numerical implementation of PIPG.</description>
+  <description>A numerical implementation of a first order primal-dual MPC basee on PIPG.</description>
 
   <maintainer email="[email protected]">Farbod Farshidian</maintainer>
   <maintainer email="[email protected]">Zhengyu Fu</maintainer>

ocs2_pipg/include/ocs2_pipg/PipgSettings.h renamed to ocs2_slp/include/ocs2_slp/pipg/PipgSettings.h

@@ -27,7 +27,7 @@ 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_pipg/Helpers.h"
+#include "ocs2_slp/Helpers.h"
 
 #include <atomic>
 #include <numeric>

ocs2_pipg/include/ocs2_pipg/PipgSolver.h renamed to ocs2_slp/include/ocs2_slp/pipg/PipgSolver.h

@@ -27,7 +27,7 @@ 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_pipg/SlpSettings.h"
+#include "ocs2_slp/SlpSettings.h"
 
 #include <iostream>
 

ocs2_pipg/include/ocs2_pipg/PipgSolverStatus.h renamed to ocs2_slp/include/ocs2_slp/pipg/PipgSolverStatus.h

@@ -27,7 +27,7 @@ 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_pipg/mpc/PipgMpcSolver.h"
+#include "ocs2_slp/SlpSolver.h"
 
 #include <iostream>
 #include <numeric>
@@ -38,11 +38,11 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <ocs2_oc/multiple_shooting/Transcription.h>
 #include <ocs2_oc/pre_condition/Scaling.h>
 
-#include "ocs2_pipg/Helpers.h"
+#include "ocs2_slp/Helpers.h"
 
 namespace ocs2 {
 
-PipgMpcSolver::PipgMpcSolver(slp::Settings settings, const OptimalControlProblem& optimalControlProblem, const Initializer& initializer)
+SlpSolver::SlpSolver(slp::Settings settings, const OptimalControlProblem& optimalControlProblem, const Initializer& initializer)
     : settings_(std::move(settings)),
       pipgSolver_(settings_.pipgSettings),
       threadPool_(std::max(settings_.nThreads, size_t(1)) - 1, settings_.threadPriority) {
@@ -68,13 +68,13 @@ PipgMpcSolver::PipgMpcSolver(slp::Settings settings, const OptimalControlProblem
   filterLinesearch_.armijoFactor = settings_.armijoFactor;
 }
 
-PipgMpcSolver::~PipgMpcSolver() {
+SlpSolver::~SlpSolver() {
   if (settings_.printSolverStatistics) {
     std::cerr << getBenchmarkingInformationPIPG() << "\n" << getBenchmarkingInformation() << std::endl;
   }
 }
 
-void PipgMpcSolver::reset() {
+void SlpSolver::reset() {
   // Clear solution
   primalSolution_ = PrimalSolution();
   performanceIndeces_.clear();
@@ -88,7 +88,7 @@ void PipgMpcSolver::reset() {
   computeControllerTimer_.reset();
 }
 
-std::string PipgMpcSolver::getBenchmarkingInformationPIPG() const {
+std::string SlpSolver::getBenchmarkingInformationPIPG() const {
   const auto GGTMultiplication = GGTMultiplication_.getTotalInMilliseconds();
   const auto preConditioning = preConditioning_.getTotalInMilliseconds();
   const auto lambdaEstimation = lambdaEstimation_.getTotalInMilliseconds();
@@ -117,7 +117,7 @@ std::string PipgMpcSolver::getBenchmarkingInformationPIPG() const {
   return infoStream.str();
 }
 
-std::string PipgMpcSolver::getBenchmarkingInformation() const {
+std::string SlpSolver::getBenchmarkingInformation() const {
   const auto linearQuadraticApproximationTotal = linearQuadraticApproximationTimer_.getTotalInMilliseconds();
   const auto solveQpTotal = solveQpTimer_.getTotalInMilliseconds();
   const auto linesearchTotal = linesearchTimer_.getTotalInMilliseconds();
@@ -143,18 +143,18 @@ std::string PipgMpcSolver::getBenchmarkingInformation() const {
   return infoStream.str();
 }
 
-const std::vector<PerformanceIndex>& PipgMpcSolver::getIterationsLog() const {
+const std::vector<PerformanceIndex>& SlpSolver::getIterationsLog() const {
   if (performanceIndeces_.empty()) {
-    throw std::runtime_error("[PipgMpcSolver]: No performance log yet, no problem solved yet?");
+    throw std::runtime_error("[SlpSolver]: No performance log yet, no problem solved yet?");
   } else {
     return performanceIndeces_;
   }
 }
 
-void PipgMpcSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t finalTime) {
+void SlpSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar_t finalTime) {
   if (settings_.printSolverStatus || settings_.printLinesearch) {
     std::cerr << "\n++++++++++++++++++++++++++++++++++++++++++++++++++++++";
-    std::cerr << "\n+++++++++++++ PIPG solver is initialized +++++++++++++";
+    std::cerr << "\n+++++++++++++ SLP solver is initialized ++++++++++++++";
     std::cerr << "\n++++++++++++++++++++++++++++++++++++++++++++++++++++++\n";
   }
 
@@ -215,16 +215,16 @@ void PipgMpcSolver::runImpl(scalar_t initTime, const vector_t& initState, scalar
   if (settings_.printSolverStatus || settings_.printLinesearch) {
     std::cerr << "\nConvergence : " << toString(convergence) << "\n";
     std::cerr << "\n++++++++++++++++++++++++++++++++++++++++++++++++++++++";
-    std::cerr << "\n+++++++++++++ PIPG solver has terminated +++++++++++++";
+    std::cerr << "\n+++++++++++++ SLP solver has terminated ++++++++++++++";
     std::cerr << "\n++++++++++++++++++++++++++++++++++++++++++++++++++++++\n";
   }
 }
 
-void PipgMpcSolver::runParallel(std::function<void(int)> taskFunction) {
+void SlpSolver::runParallel(std::function<void(int)> taskFunction) {
   threadPool_.runParallel(std::move(taskFunction), settings_.nThreads);
 }
 
-PipgMpcSolver::OcpSubproblemSolution PipgMpcSolver::getOCPSolution(const vector_t& delta_x0) {
+SlpSolver::OcpSubproblemSolution SlpSolver::getOCPSolution(const vector_t& delta_x0) {
   // Solve the QP
   OcpSubproblemSolution solution;
   auto& deltaXSol = solution.deltaXSol;
@@ -283,13 +283,13 @@ PipgMpcSolver::OcpSubproblemSolution PipgMpcSolver::getOCPSolution(const vector_
   return solution;
 }
 
-PrimalSolution PipgMpcSolver::toPrimalSolution(const std::vector<AnnotatedTime>& time, vector_array_t&& x, vector_array_t&& u) {
+PrimalSolution SlpSolver::toPrimalSolution(const std::vector<AnnotatedTime>& time, vector_array_t&& x, vector_array_t&& u) {
   ModeSchedule modeSchedule = this->getReferenceManager().getModeSchedule();
   return multiple_shooting::toPrimalSolution(time, std::move(modeSchedule), std::move(x), std::move(u));
 }
 
-PerformanceIndex PipgMpcSolver::setupQuadraticSubproblem(const std::vector<AnnotatedTime>& time, const vector_t& initState,
-                                                         const vector_array_t& x, const vector_array_t& u) {
+PerformanceIndex SlpSolver::setupQuadraticSubproblem(const std::vector<AnnotatedTime>& time, const vector_t& initState,
+                                                     const vector_array_t& x, const vector_array_t& u) {
   // Problem horizon
   const int N = static_cast<int>(time.size()) - 1;
 
@@ -362,8 +362,8 @@ PerformanceIndex PipgMpcSolver::setupQuadraticSubproblem(const std::vector<Annot
   return totalPerformance;
 }
 
-PerformanceIndex PipgMpcSolver::computePerformance(const std::vector<AnnotatedTime>& time, const vector_t& initState,
-                                                   const vector_array_t& x, const vector_array_t& u) {
+PerformanceIndex SlpSolver::computePerformance(const std::vector<AnnotatedTime>& time, const vector_t& initState, const vector_array_t& x,
+                                               const vector_array_t& u) {
   // Problem horizon
   const int N = static_cast<int>(time.size()) - 1;
 
@@ -408,9 +408,9 @@ PerformanceIndex PipgMpcSolver::computePerformance(const std::vector<AnnotatedTi
   return totalPerformance;
 }
 
-slp::StepInfo PipgMpcSolver::takeStep(const PerformanceIndex& baseline, const std::vector<AnnotatedTime>& timeDiscretization,
-                                      const vector_t& initState, const OcpSubproblemSolution& subproblemSolution, vector_array_t& x,
-                                      vector_array_t& u) {
+slp::StepInfo SlpSolver::takeStep(const PerformanceIndex& baseline, const std::vector<AnnotatedTime>& timeDiscretization,
+                                  const vector_t& initState, const OcpSubproblemSolution& subproblemSolution, vector_array_t& x,
+                                  vector_array_t& u) {
   using StepType = FilterLinesearch::StepType;
 
   /*
@@ -501,7 +501,7 @@ slp::StepInfo PipgMpcSolver::takeStep(const PerformanceIndex& baseline, const st
   return stepInfo;
 }
 
-slp::Convergence PipgMpcSolver::checkConvergence(int iteration, const PerformanceIndex& baseline, const slp::StepInfo& stepInfo) const {
+slp::Convergence SlpSolver::checkConvergence(int iteration, const PerformanceIndex& baseline, const slp::StepInfo& stepInfo) const {
   using Convergence = slp::Convergence;
   if ((iteration + 1) >= settings_.slpIteration) {
     // Converged because the next iteration would exceed the specified number of iterations