Merged in dev/main (pull request #547)

dev/main

Approved-by: Ruben Grandia

ocs2/package.xml

@@ -11,18 +11,19 @@
 
   <buildtool_depend>catkin</buildtool_depend>
   <exec_depend>ocs2_core</exec_depend>
-  <exec_depend>ocs2_ddp</exec_depend>
-  <exec_depend>ocs2_frank_wolfe</exec_depend>
   <exec_depend>ocs2_mpc</exec_depend>
+  <exec_depend>ocs2_frank_wolfe</exec_depend>
   <exec_depend>ocs2_oc</exec_depend>
-  <exec_depend>ocs2_pinocchio</exec_depend>
-  <exec_depend>ocs2_robotic_examples</exec_depend>
+  <exec_depend>ocs2_qp_solver</exec_depend>
+  <exec_depend>ocs2_ddp</exec_depend>
+  <exec_depend>ocs2_sqp</exec_depend>
+  <exec_depend>ocs2_ros_interfaces</exec_depend>
   <exec_depend>ocs2_python_interface</exec_depend>
+  <exec_depend>ocs2_pinocchio</exec_depend>
   <exec_depend>ocs2_robotic_tools</exec_depend>
-  <exec_depend>ocs2_ros_interfaces</exec_depend>
-  <exec_depend>ocs2_sqp</exec_depend>
-  <exec_depend>ocs2_qp_solver</exec_depend>
+  <exec_depend>ocs2_robotic_examples</exec_depend>
   <exec_depend>ocs2_thirdparty</exec_depend>
+  <exec_depend>ocs2_raisim</exec_depend>
 
    <export>
    	  <metapackage />

ocs2_core/CMakeLists.txt

@@ -79,7 +79,6 @@ add_library(${PROJECT_NAME}
   src/control/FeedforwardController.cpp
   src/control/LinearController.cpp
   src/control/StateBasedLinearController.cpp
-  src/control/TrajectorySpreadingControllerAdjustment.cpp
   src/cost/QuadraticStateCost.cpp
   src/cost/QuadraticStateInputCost.cpp
   src/cost/StateCostCollection.cpp
@@ -130,14 +129,11 @@ add_library(${PROJECT_NAME}
   src/model_data/ModelData.cpp
   src/misc/LinearAlgebra.cpp
   src/misc/Log.cpp
-  src/soft_constraint/SoftConstraintPenalty.cpp
   src/soft_constraint/StateSoftConstraint.cpp
   src/soft_constraint/StateInputSoftConstraint.cpp
-  src/soft_constraint/penalties/DoubleSidedPenalty.cpp
-  src/soft_constraint/penalties/QuadraticPenalty.cpp
-  src/soft_constraint/penalties/SmoothAbsolutePenalty.cpp
-  src/soft_constraint/penalties/RelaxedBarrierPenalty.cpp
-  src/soft_constraint/penalties/SquaredHingePenalty.cpp
+  src/penalties/MultidimensionalPenalty.cpp
+  src/penalties/penalties/RelaxedBarrierPenalty.cpp
+  src/penalties/penalties/SquaredHingePenalty.cpp
   src/thread_support/ThreadPool.cpp
 )
 target_link_libraries(${PROJECT_NAME}
@@ -337,6 +333,15 @@ target_link_libraries(test_ModelData
   gtest_main
 )
 
+catkin_add_gtest(test_ModeSchedule
+  test/reference/testModeSchedule.cpp
+)
+target_link_libraries(test_ModeSchedule
+  ${PROJECT_NAME}
+  ${catkin_LIBRARIES}
+  gtest_main
+)
+
 catkin_add_gtest(test_Logging
   test/misc/testLogging.cpp
 )

ocs2_core/include/ocs2_core/Types.h

@@ -104,9 +104,12 @@ struct ScalarFunctionQuadraticApproximation {
   /** Construct and resize the members to given size. */
   ScalarFunctionQuadraticApproximation(size_t nx, size_t nu);
 
-  /** Compound additinon assignment opeartor */
+  /** Compound addition assignment operator */
   ScalarFunctionQuadraticApproximation& operator+=(const ScalarFunctionQuadraticApproximation& rhs);
 
+  /** Compound scalar multiplication and assignment operator */
+  ScalarFunctionQuadraticApproximation& operator*=(scalar_t scalar);
+
   /**
    * Resize the members to the given size
    * @param[in] nx State dimension
@@ -132,6 +135,40 @@ struct ScalarFunctionQuadraticApproximation {
 
 std::ostream& operator<<(std::ostream& out, const ScalarFunctionQuadraticApproximation& f);
 
+/**
+ * Checks that the given matrix is valid, self-adjoint, and positive semi-definite (PSD).
+ * @param[in] data: Given matrix.
+ * @param[in] dataName: The name of the data which appears in the output error message.
+ * @return The description of the error. If there was no error it would be empty;
+ */
+std::string checkBeingPSD(const matrix_t& data, const std::string& dataName);
+
+/**
+ * Checks that the given quadratic approximation is valid, self-adjoint, and positive semi-definite (PSD).
+ * @param[in] data: Given quadratic approximation.
+ * @param[in] dataName: The name of the data which appears in the output error message.
+ * @return The description of the error. If there was no error it would be empty;
+ */
+std::string checkBeingPSD(const ScalarFunctionQuadraticApproximation& data, const std::string& dataName);
+
+/**
+ * Checks the size of the given quadratic approximation.
+ *
+ * @param[in] stateDim: Number of states.
+ * @param[in] inputDim: Number of inputs.
+ * @param[in] data: Given quadratic approximation.
+ * @param[in] dataName: The name of the data which appears in the output error message.
+ * @return The description of the error. If there was no error it would be empty;
+ */
+std::string checkSize(int stateDim, int inputDim, const ScalarFunctionQuadraticApproximation& data, const std::string& dataName);
+
+inline ScalarFunctionQuadraticApproximation operator*(ScalarFunctionQuadraticApproximation lhs, scalar_t scalar) {
+  return lhs *= scalar;
+}
+inline ScalarFunctionQuadraticApproximation operator*(scalar_t scalar, ScalarFunctionQuadraticApproximation rhs) {
+  return rhs *= scalar;
+}
+
 /**
  * Defines the linear model of a vector-valued function
  * f(x,u) = dfdx dx + dfdu du + f
@@ -178,6 +215,19 @@ struct VectorFunctionLinearApproximation {
 
 std::ostream& operator<<(std::ostream& out, const VectorFunctionLinearApproximation& f);
 
+/**
+ * Checks the size of the given vector-function linear approximation.
+ *
+ * @param[in] vectorDim: The vector function dimension.
+ * @param[in] stateDim: Number of states.
+ * @param[in] inputDim: Number of inputs.
+ * @param[in] data: Given linear approximation.
+ * @param[in] dataName: The name of the data which appears in the output error message.
+ * @return The description of the error. If there was no error it would be empty;
+ */
+std::string checkSize(int vectorDim, int stateDim, int inputDim, const VectorFunctionLinearApproximation& data,
+                      const std::string& dataName);
+
 /**
  * Defines quadratic approximation of a vector-valued function
  * f[i](x,u) = 1/2 dx' dfdxx[i] dx + du' dfdux[i] dx + 1/2 du' dfduu[i] du + dfdx[i,:] dx + dfdu[i,:] du + f[i]

ocs2_core/include/ocs2_core/misc/Collection.h

@@ -29,6 +29,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #pragma once
 
+#include <algorithm>
 #include <memory>
 #include <string>
 #include <unordered_map>
@@ -51,6 +52,9 @@ class Collection {
   /** Checks if the collection has no elements */
   bool empty() const { return terms_.empty(); }
 
+  /** Erases all elements from the Collection. */
+  void clear();
+
   /**
    * Adds a term to the collection, and transfer ownership to the collection
    * The provided name must be unique and is later used to access the cost term.
@@ -59,15 +63,40 @@ class Collection {
    */
   void add(std::string name, std::unique_ptr<T> term);
 
+  /**
+   * Erases a term from the collection.
+   *
+   * @param name: Name of the term.
+   * @return True if the term was in the Collection and false if the term was not found in the Collection.
+   */
+  bool erase(const std::string& name) { return (extract(name) != nullptr); }
+
+  /**
+   * Removes a term from the Collection and returns it as a unique_ptr.
+   *
+   * @param name: Name of the term.
+   * @return A unique pointer to the extracted term. If the term was not found it returns nullptr.
+   */
+  std::unique_ptr<T> extract(const std::string& name);
+
   /**
    * Use to modify a term.
    * @tparam Derived: derived class of base type T to cast to. Casts to the base class by default
-   * @param name: Name of the cost term to modify
+   * @param name: Name of the term to modify
    * @return A reference to the underlying term
    */
   template <typename Derived = T>
   Derived& get(const std::string& name);
 
+  /**
+   * Finds the index of the term in the stored map.
+   *
+   * @param [in] name: Name of the term.
+   * @param [out] index : Term index.
+   * @return True if the name found in the collection.
+   */
+  bool getTermIndex(const std::string& name, size_t& index) const;
+
  protected:
   /** Copy constructor */
   Collection(const Collection& other);
@@ -80,6 +109,15 @@ class Collection {
   std::unordered_map<std::string, size_t> termNameMap_;
 };
 
+/******************************************************************************************************/
+/******************************************************************************************************/
+/******************************************************************************************************/
+template <typename T>
+void Collection<T>::clear() {
+  terms_.clear();
+  termNameMap_.clear();
+}
+
 /******************************************************************************************************/
 /******************************************************************************************************/
 /******************************************************************************************************/
@@ -94,6 +132,38 @@ void Collection<T>::add(std::string name, std::unique_ptr<T> term) {
   }
 }
 
+/******************************************************************************************************/
+/******************************************************************************************************/
+/******************************************************************************************************/
+template <typename T>
+std::unique_ptr<T> Collection<T>::extract(const std::string& name) {
+  // find the term iterator with the name
+  const auto termItr = termNameMap_.find(name);
+
+  // term was not found
+  if (termItr == termNameMap_.end()) {
+    return nullptr;
+  }
+
+  // adjust the index of the terms added after the requested term
+  const size_t termInd = termItr->second;
+  for (auto itr = termNameMap_.begin(); itr != termNameMap_.end(); ++itr) {
+    if (itr->second > termInd) {
+      --itr->second;
+    }
+  }
+
+  // remove term from map
+  termNameMap_.erase(termItr);
+
+  // get the term
+  auto term = (std::move(terms_[termInd]));
+  // remove the term
+  terms_.erase(terms_.begin() + termInd);
+
+  return term;
+}
+
 /******************************************************************************************************/
 /******************************************************************************************************/
 /******************************************************************************************************/
@@ -102,8 +172,8 @@ template <typename Derived>
 Derived& Collection<T>::get(const std::string& name) {
   static_assert(std::is_base_of<T, Derived>::value, "Template argument must derive from the base type of this collection");
   // if the key does not exist throws an exception
-  const auto costIndex = termNameMap_.at(name);
-  return dynamic_cast<Derived&>(*terms_[costIndex]);
+  const auto index = termNameMap_.at(name);
+  return dynamic_cast<Derived&>(*terms_[index]);
 }
 
 /******************************************************************************************************/
@@ -118,6 +188,21 @@ Collection<T>::Collection(const Collection& other) : termNameMap_(other.termName
   }
 }
 
+/******************************************************************************************************/
+/******************************************************************************************************/
+/******************************************************************************************************/
+template <typename T>
+bool Collection<T>::getTermIndex(const std::string& name, size_t& index) const {
+  auto itr = termNameMap_.find(name);
+  if (itr != termNameMap_.cend()) {
+    index = itr->second;
+    return true;
+  } else {
+    index = termNameMap_.size();
+    return false;
+  }
+}
+
 /**
  * Helper function for merging two vectors by moving objects.
  * @param v1 : vector to move objects to