fix CMakeLists and add tests

Author: mayataka

ocs2_ipm/CMakeLists.txt

@@ -73,7 +73,7 @@ if(cmake_clang_tools_FOUND)
     TARGETS ${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_FIX
+    CF_WERROR
   )
 endif(cmake_clang_tools_FOUND)
 

ocs2_ipm/test/Exp0Test.cpp

@@ -168,4 +168,10 @@ TEST(Exp0Test, Constrained) {
       ASSERT_TRUE(umax - u(0) >= 0);
     }
   }
+
+  // solve with shifted horizon
+  const scalar_array_t shiftTime = {0.05, 0.1, 0.3, 0.5, 0.8, 0.12, 0.16, 0.19};
+  for (const auto e : shiftTime) {
+    solver.run(startTime + e, initState, finalTime + e);
+  }
 }

ocs2_ipm/test/Exp1Test.cpp

@@ -197,6 +197,12 @@ TEST(Exp1Test, Constrained) {
       ASSERT_TRUE(umax - u(0) >= 0);
     }
   }
+
+  // solve with shifted horizon
+  const scalar_array_t shiftTime = {0.05, 0.1, 0.3, 0.5, 0.8, 0.12, 0.16, 0.19};
+  for (const auto e : shiftTime) {
+    solver.run(startTime + e, initState, finalTime + e);
+  }
 }
 
 TEST(Exp1Test, MixedConstrained) {
@@ -257,4 +263,10 @@ TEST(Exp1Test, MixedConstrained) {
     const auto constraintValue = stateInputIneqConstraintCloned->getValue(t, x, u, PreComputation());
     ASSERT_TRUE(constraintValue.minCoeff() >= 0.0);
   }
+
+  // solve with shifted horizon
+  const scalar_array_t shiftTime = {0.05, 0.1, 0.3, 0.5, 0.8, 0.12, 0.16, 0.19};
+  for (const auto e : shiftTime) {
+    solver.run(startTime + e, initState, finalTime + e);
+  }
 }

ocs2_ipm/test/testCircularKinematics.cpp

@@ -238,6 +238,12 @@ TEST(test_circular_kinematics, solve_projected_EqConstraints_IneqConstraints) {
     // Feed forward part
     ASSERT_TRUE(u.isApprox(primalSolution.controllerPtr_->computeInput(t, x)));
   }
+
+  // solve with shifted horizon
+  const scalar_array_t shiftTime = {0.05, 0.1, 0.3, 0.5, 0.8, 0.12, 0.16, 0.19};
+  for (const auto e : shiftTime) {
+    solver.run(startTime + e, initState, finalTime + e);
+  }
 }
 
 TEST(test_circular_kinematics, solve_projected_EqConstraints_MixedIneqConstraints) {
@@ -326,10 +332,15 @@ TEST(test_circular_kinematics, solve_projected_EqConstraints_MixedIneqConstraint
 
   // Check Lagrange multipliers
   for (int i = 0; i < primalSolution.timeTrajectory_.size() - 1; i++) {
-    std::cerr << "i: " << i << std::endl;
     const auto t = primalSolution.timeTrajectory_[i];
     const auto& x = primalSolution.stateTrajectory_[i];
     const auto& u = primalSolution.inputTrajectory_[i];
     ASSERT_NO_THROW(const auto multiplier = solver.getStateInputEqualityConstraintLagrangian(t, x););
   }
+
+  // solve with shifted horizon
+  const scalar_array_t shiftTime = {0.05, 0.1, 0.3, 0.5, 0.8, 0.12, 0.16, 0.19};
+  for (const auto e : shiftTime) {
+    solver.run(startTime + e, initState, finalTime + e);
+  }
 }