Merge branch 'feature/interior_point_example' into feature/interior_point/trajectory_adjustment

Author: mayataka

ocs2_robotic_examples/ocs2_cartpole_ros/launch/multiplot.launch

@@ -1,6 +1,6 @@
 <launch>
   <arg name="observation_config"  default="$(find ocs2_cartpole)/config/multiplot/mpc_observation.xml" />
-  <arg name="metrics_config"      default="$(find ocs2_cartpole)/config/multiplot/zero_velocity.xml" />
+  <arg name="metrics_config"      default="$(find ocs2_cartpole)/config/multiplot/mpc_metrics.xml" />
 
   <!-- Launch RQT Multi-plot -->
   <node name="mpc_observation" pkg="rqt_multiplot" type="rqt_multiplot" 

ocs2_robotic_examples/ocs2_legged_robot/CMakeLists.txt

@@ -122,7 +122,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_WERROR
+     CF_FIX
 )
 endif(cmake_clang_tools_FOUND)
 

ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info

@@ -54,11 +54,11 @@ ipm
   ipmIteration                          1
   deltaTol                              1e-4
   g_max                                 10.0
-  g_min                                 1e-4
+  g_min                                 1e-6
   computeLagrangeMultipliers            true
   printSolverStatistics                 true
   printSolverStatus                     false
-  printLinesearch                       true
+  printLinesearch                       false
   useFeedbackPolicy                     true
   integratorType                        RK2
   threadPriority                        50

ocs2_robotic_examples/ocs2_legged_robot/include/ocs2_legged_robot/LeggedRobotInterface.h

@@ -111,7 +111,7 @@ class LeggedRobotInterface final : public RobotInterface {
   mpc::Settings mpcSettings_;
   sqp::Settings sqpSettings_;
   ipm::Settings ipmSettings_;
-  bool useHardFrictionConeConstraint_;
+  const bool useHardFrictionConeConstraint_;
 
   std::unique_ptr<PinocchioInterface> pinocchioInterfacePtr_;
   CentroidalModelInfo centroidalModelInfo_;

ocs2_robotic_examples/ocs2_legged_robot/src/LeggedRobotInterface.cpp

@@ -65,7 +65,8 @@ namespace legged_robot {
 /******************************************************************************************************/
 /******************************************************************************************************/
 LeggedRobotInterface::LeggedRobotInterface(const std::string& taskFile, const std::string& urdfFile, const std::string& referenceFile,
-                                           bool useHardFrictionConeConstraint) {
+                                           bool useHardFrictionConeConstraint)
+    : useHardFrictionConeConstraint_(useHardFrictionConeConstraint) {
   // check that task file exists
   boost::filesystem::path taskFilePath(taskFile);
   if (boost::filesystem::exists(taskFilePath)) {
@@ -93,12 +94,11 @@ LeggedRobotInterface::LeggedRobotInterface(const std::string& taskFile, const st
 
   // load setting from loading file
   modelSettings_ = loadModelSettings(taskFile, "model_settings", verbose);
-  ddpSettings_ = ddp::loadSettings(taskFile, "ddp", verbose);
   mpcSettings_ = mpc::loadSettings(taskFile, "mpc", verbose);
-  rolloutSettings_ = rollout::loadSettings(taskFile, "rollout", verbose);
+  ddpSettings_ = ddp::loadSettings(taskFile, "ddp", verbose);
   sqpSettings_ = sqp::loadSettings(taskFile, "sqp", verbose);
   ipmSettings_ = ipm::loadSettings(taskFile, "ipm", verbose);
-  useHardFrictionConeConstraint_ = useHardFrictionConeConstraint;
+  rolloutSettings_ = rollout::loadSettings(taskFile, "rollout", verbose);
 
   // OptimalConrolProblem
   setupOptimalConrolProblem(taskFile, urdfFile, referenceFile, verbose);
@@ -329,13 +329,8 @@ std::unique_ptr<StateInputConstraint> LeggedRobotInterface::getFrictionConeConst
 /******************************************************************************************************/
 std::unique_ptr<StateInputCost> LeggedRobotInterface::getFrictionConeSoftConstraint(
     size_t contactPointIndex, scalar_t frictionCoefficient, const RelaxedBarrierPenalty::Config& barrierPenaltyConfig) {
-  FrictionConeConstraint::Config frictionConeConConfig(frictionCoefficient);
-  auto frictionConeConstraintPtr = std::make_unique<FrictionConeConstraint>(*referenceManagerPtr_, std::move(frictionConeConConfig),
-                                                                            contactPointIndex, centroidalModelInfo_);
-
-  auto penalty = std::make_unique<RelaxedBarrierPenalty>(barrierPenaltyConfig);
-
-  return std::make_unique<StateInputSoftConstraint>(std::move(frictionConeConstraintPtr), std::move(penalty));
+  return std::make_unique<StateInputSoftConstraint>(getFrictionConeConstraint(contactPointIndex, frictionCoefficient),
+                                                    std::make_unique<RelaxedBarrierPenalty>(barrierPenaltyConfig));
 }
 
 /******************************************************************************************************/

ocs2_robotic_examples/ocs2_legged_robot_ros/src/LeggedRobotIpmMpcNode.cpp

@@ -72,7 +72,7 @@ int main(int argc, char** argv) {
   mpc.getSolverPtr()->setReferenceManager(rosReferenceManagerPtr);
   mpc.getSolverPtr()->addSynchronizedModule(gaitReceiverPtr);
 
-  // observer for zero velocity constraints (only add this for debugging as it slows down the solver)
+  // observer for friction cone constraints (only add this for debugging as it slows down the solver)
   if (multiplot) {
     auto createStateInputBoundsObserver = [&](const std::string& termName) {
       const ocs2::scalar_array_t observingTimePoints{0.0};