Fixed broken bench + updated ROS CI

.github/workflows/ros1-ci.yaml

@@ -9,12 +9,8 @@ jobs:
       fail-fast: false
       matrix:
         env:
-          - {name: "(noetic)", ROS_DISTRO: noetic}
-          - {name: "(noetic, clang)", ROS_DISTRO: noetic, ADDITIONAL_DEBS: clang, CC: clang, CXX: clang++}
-          - {name: "(noetic, clang, multi-threading)", ROS_DISTRO: noetic, ADDITIONAL_DEBS: "clang libomp-dev", CC: clang, CXX: clang++, CMAKE_ARGS: "-DBUILD_WITH_MULTITHREADS=ON -DBUILD_WITH_NTHREADS=2"}
-          - {name: "(noetic, clang, Debug)", ROS_DISTRO: noetic, ADDITIONAL_DEBS: clang, CC: clang, CXX: clang++, CMAKE_ARGS: "-DCMAKE_BUILD_TYPE=Debug"}
-          # Format check
-          #- {name: "Format check", ROS_DISTRO: noetic, CLANG_FORMAT_CHECK: file, CLANG_FORMAT_VERSION: "6.0", BEFORE_RUN_CLANG_FORMAT_CHECK: "wget https://raw.githubusercontent.com/Gepetto/linters/master/.clang-format-6.0 -O /tmp/clang_format_check/crocoddyl/.clang-format", ADDITIONAL_DEBS: wget}
+          - {name: "(noetic, Release)", ROS_DISTRO: noetic}
+          # - {name: "(noetic, clang, multi-threading, Release)", ROS_DISTRO: noetic, ADDITIONAL_DEBS: "clang libomp-dev", CC: clang, CXX: clang++, CMAKE_ARGS: "-DBUILD_WITH_MULTITHREADS=ON -DBUILD_WITH_NTHREADS=2"}
     name: ${{ matrix.env.name }}
     env:
       CCACHE_DIR: /github/home/.ccache             # Enable ccache
@@ -29,20 +25,17 @@ jobs:
       # target after completion of the regular test target. The output of this step does affect the output of the CI process.
       # Note, this does not affect projects that do not have pure CMake projects in their upstream_ws.
       BUILDER: catkin_tools
-      AFTER_RUN_TARGET_TEST: 'source /root/target_ws/install/setup.bash && cd /root/target_ws/build/crocoddyl && env && make test'
     runs-on: ubuntu-latest
+    continue-on-error: true
     steps:
-      - uses: actions/checkout@v2
+      - uses: actions/checkout@v3
         with:
           submodules: recursive
       # This step will fetch/store the directory used by ccache before/after the ci run
-      - uses: actions/cache@v2
+      - uses: actions/cache@v3
         with:
           path: ${{ env.CCACHE_DIR }}
           key: ccache-${{ matrix.env.ROS_DISTRO }}-${{ matrix.env.ROS_REPO }}
-      # Run industrial_ci - use fork until awaits https://github.com/ros-industrial/industrial_ci/issues/767 is resolved
-      - uses: 'wxmerkt/industrial_ci@topic/clang-format-check-in-deterministic-location'
+      # Run industrial_ci
+      - uses: 'ros-industrial/industrial_ci@master'
         env: ${{ matrix.env }}
-      # # Run industrial_ci
-      # - uses: 'ros-industrial/industrial_ci@master'
-      #   env: ${{ matrix.env }}

.github/workflows/ros2-ci.yaml

@@ -10,11 +10,13 @@ jobs:
       matrix:
         env:
           - {name: "(humble, Release)", ROS_DISTRO: humble}
-          # - {name: "humble, multi-threading", ROS_DISTRO: humble, ADDITIONAL_DEBS: "libomp-dev", CMAKE_ARGS: "-DBUILD_WITH_MULTITHREADS=ON -DBUILD_WITH_NTHREADS=2"}
-          # - {name: "humble, Debug", ROS_DISTRO: noetic, CMAKE_ARGS: "-DCMAKE_BUILD_TYPE=Debug"}
           - {name: "(rolling, Release)", ROS_DISTRO: rolling}
-          # - {name: "rolling, multi-threading", ROS_DISTRO: rolling, ADDITIONAL_DEBS: "libomp-dev", CMAKE_ARGS: "-DBUILD_WITH_MULTITHREADS=ON -DBUILD_WITH_NTHREADS=2"}
-          # - {name: "rolling, Debug", ROS_DISTRO: rolling, CMAKE_ARGS: "-DCMAKE_BUILD_TYPE=Debug"}
+          - {name: "(iron, Release)", ROS_DISTRO: iron}
+          # - {name: "(humble, clang, multi-threading, Release)", ROS_DISTRO: humble, ADDITIONAL_DEBS: "clang libomp-dev", CC: clang, CXX: clang++, CMAKE_ARGS: "-DBUILD_WITH_MULTITHREADS=ON -DBUILD_WITH_NTHREADS=2"}
+          # - {name: "(rolling, clang, multi-threading, Release)", ROS_DISTRO: rolling, ADDITIONAL_DEBS: "clang libomp-dev", CC: clang, CXX: clang++, CMAKE_ARGS: "-DBUILD_WITH_MULTITHREADS=ON -DBUILD_WITH_NTHREADS=2"}
+          # - {name: "(iron, clang, multi-threading, Release)", ROS_DISTRO: iron, ADDITIONAL_DEBS: "clang libomp-dev", CC: clang, CXX: clang++, CMAKE_ARGS: "-DBUILD_WITH_MULTITHREADS=ON -DBUILD_WITH_NTHREADS=2"}
+          # - {name: "(humble, Debug)", ROS_DISTRO: noetic, CMAKE_ARGS: "-DCMAKE_BUILD_TYPE=Debug"}
+          # - {name: "(rolling, Debug)", ROS_DISTRO: rolling, CMAKE_ARGS: "-DCMAKE_BUILD_TYPE=Debug"}
     name: ${{ matrix.env.name }}
     env:
       CCACHE_DIR: /github/home/.ccache             # Enable ccache
@@ -25,6 +27,7 @@ jobs:
       # It seems target_ws is unable to properly overlay upstream_ws.
       AFTER_SETUP_UPSTREAM_WORKSPACE: 'pip install example-robot-data'
     runs-on: ubuntu-latest
+    continue-on-error: true
     steps:
       - uses: actions/checkout@v3
         with:

CMakeLists.txt

@@ -258,10 +258,10 @@ endif()
 if(BUILD_EXAMPLES)
   if(BUILD_PYTHON_INTERFACE)
     add_subdirectory(examples)
-  else(BUILD_PYTHON_INTERFACE)
+  else()
     message(
       WARNING "Python interface is not built, hence cannot build examples.")
-  endif(BUILD_PYTHON_INTERFACE)
+  endif()
 endif()
 
 # PkgConfig packaging of the project

benchmark/CMakeLists.txt

@@ -1,13 +1,13 @@
 set(${PROJECT_NAME}_BENCHMARK
     boxqp
-    unicycle-optctrl
-    lqr-optctrl
-    arm-manipulation-optctrl
-    arm-manipulation-timings
-    quadrupedal-gaits-optctrl
-    bipedal-timings)
+    unicycle_optctrl
+    lqr_optctrl
+    arm_manipulation_optctrl
+    quadrupedal_gaits_optctrl
+    arm_manipulation_timings
+    bipedal_timings)
 
-set(${PROJECT_NAME}_CODEGEN_BENCHMARK all-robots)
+set(${PROJECT_NAME}_CODEGEN_BENCHMARK all_robots)
 list(APPEND ${PROJECT_NAME}_BENCHMARK ${${PROJECT_NAME}_CODEGEN_BENCHMARK})
 
 foreach(BENCHMARK_NAME ${${PROJECT_NAME}_BENCHMARK})
@@ -42,4 +42,4 @@ if(BUILD_PYTHON_INTERFACE)
       "from ${BENCHMARK_NAME} import *"
       \${INPUT})
   endforeach(BENCHMARK_NAME ${${PROJECT_NAME}_BENCHMARK_PYTHON})
-endif(BUILD_PYTHON_INTERFACE)
+endif()

benchmark/arm-manipulation-optctrl.cpp → benchmark/arm_manipulation_optctrl.cpp

@@ -1,12 +1,13 @@
 ///////////////////////////////////////////////////////////////////////////////
 // BSD 3-Clause License
 //
-// Copyright (C) 2019-2020, LAAS-CNRS, University of Edinburgh
+// Copyright (C) 2019-2023, LAAS-CNRS, University of Edinburgh
+//                          Heriot-Watt University
 // Copyright note valid unless otherwise stated in individual files.
 // All rights reserved.
 ///////////////////////////////////////////////////////////////////////////////
 
-#include "crocoddyl/core/solvers/ddp.hpp"
+#include "crocoddyl/core/solvers/fddp.hpp"
 #include "crocoddyl/core/utils/callbacks.hpp"
 #include "crocoddyl/core/utils/timer.hpp"
 #include "factory/arm.hpp"
@@ -29,8 +30,9 @@ int main(int argc, char* argv[]) {
       boost::static_pointer_cast<crocoddyl::StateMultibody>(
           runningModel->get_state());
   std::cout << "NQ: " << state->get_nq() << std::endl;
-  std::cout << "Number of nodes: " << N << std::endl << std::endl;
-  Eigen::VectorXd q0 = Eigen::VectorXd::Random(state->get_nq());
+  std::cout << "Number of nodes: " << N << std::endl;
+  Eigen::VectorXd q0 =
+      state->get_pinocchio()->referenceConfigurations["arm_up"];
   Eigen::VectorXd x0(state->get_nx());
   x0 << q0, Eigen::VectorXd::Random(state->get_nv());
 
@@ -53,25 +55,25 @@ int main(int argc, char* argv[]) {
   }
 
   // Formulating the optimal control problem
-  crocoddyl::SolverDDP ddp(problem);
+  crocoddyl::SolverFDDP solver(problem);
   if (CALLBACKS) {
     std::vector<boost::shared_ptr<crocoddyl::CallbackAbstract> > cbs;
     cbs.push_back(boost::make_shared<crocoddyl::CallbackVerbose>());
-    ddp.setCallbacks(cbs);
+    solver.setCallbacks(cbs);
   }
 
   // Solving the optimal control problem
   Eigen::ArrayXd duration(T);
   for (unsigned int i = 0; i < T; ++i) {
     crocoddyl::Timer timer;
-    ddp.solve(xs, us, MAXITER, false, 0.1);
+    solver.solve(xs, us, MAXITER, false, 0.1);
     duration[i] = timer.get_duration();
   }
 
   double avrg_duration = duration.sum() / T;
   double min_duration = duration.minCoeff();
   double max_duration = duration.maxCoeff();
-  std::cout << "  DDP.solve [ms]: " << avrg_duration << " (" << min_duration
+  std::cout << "  FDDP.solve [ms]: " << avrg_duration << " (" << min_duration
             << "-" << max_duration << ")" << std::endl;
 
   // Running calc

benchmark/arm_manipulation_optctrl.py

@@ -1,5 +1,3 @@
-import os
-import subprocess
 import sys
 import time
 
@@ -8,12 +6,9 @@
 import pinocchio
 
 import crocoddyl
-from crocoddyl.utils import DifferentialFreeFwdDynamicsModelDerived
-
-crocoddyl.switchToNumpyMatrix()
 
 # First, let's load the Pinocchio model for the Talos arm.
-ROBOT = example_robot_data.loadTalosArm()
+ROBOT = example_robot_data.load("kinova")
 N = 100  # number of nodes
 T = int(sys.argv[1]) if (len(sys.argv) > 1) else int(5e3)  # number of trials
 MAXITER = 1
@@ -22,8 +17,8 @@
 
 def createProblem(model):
     robot_model = ROBOT.model
-    q0 = np.matrix([0.173046, 1.0, -0.52366, 0.0, 0.0, 0.1, -0.005]).T
-    x0 = np.vstack([q0, np.zeros((robot_model.nv, 1))])
+    q0 = robot_model.referenceConfigurations["arm_up"]
+    x0 = np.concatenate([q0, np.zeros(robot_model.nv)])
 
     # Note that we need to include a cost model (i.e. set of cost functions) in
     # order to fully define the action model for our optimal control problem.
@@ -35,8 +30,8 @@ def createProblem(model):
         state,
         crocoddyl.ResidualModelFramePlacement(
             state,
-            robot_model.getFrameId("gripper_left_joint"),
-            pinocchio.SE3(np.eye(3), np.matrix([[0.0], [0.0], [0.4]])),
+            robot_model.getFrameId("j2s6s200_end_effector"),
+            pinocchio.SE3(np.eye(3), np.array([0.6, 0.2, 0.5])),
         ),
     )
     xRegCost = crocoddyl.CostModelResidual(state, crocoddyl.ResidualModelState(state))
@@ -48,26 +43,20 @@ def createProblem(model):
 
     # Then let's added the running and terminal cost functions
     runningCostModel.addCost("gripperPose", goalTrackingCost, 1)
-    runningCostModel.addCost("xReg", xRegCost, 1e-4)
-    runningCostModel.addCost("uReg", uRegCost, 1e-4)
-    terminalCostModel.addCost("gripperPose", goalTrackingCost, 1)
+    runningCostModel.addCost("xReg", xRegCost, 1e-1)
+    runningCostModel.addCost("uReg", uRegCost, 1e-1)
+    terminalCostModel.addCost("gripperPose", goalTrackingCost, 1e3)
 
     # Next, we need to create an action model for running and terminal knots. The
     # forward dynamics (computed using ABA) are implemented
     # inside DifferentialActionModelFullyActuated.
     actuation = crocoddyl.ActuationModelFull(state)
     runningModel = crocoddyl.IntegratedActionModelEuler(
-        model(state, actuation, runningCostModel), 1e-3
+        model(state, actuation, runningCostModel), 1e-2
     )
-    runningModel.differential.armature = np.matrix(
-        [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.0]
-    ).T
     terminalModel = crocoddyl.IntegratedActionModelEuler(
-        model(state, actuation, terminalCostModel), 1e-3
+        model(state, actuation, terminalCostModel), 0.0
     )
-    terminalModel.differential.armature = np.matrix(
-        [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.0]
-    ).T
 
     # For this optimal control problem, we define 100 knots (or running action
     # models) plus a terminal knot
@@ -81,13 +70,13 @@ def createProblem(model):
 
 
 def runDDPSolveBenchmark(xs, us, problem):
-    ddp = crocoddyl.SolverDDP(problem)
+    solver = crocoddyl.SolverFDDP(problem)
     if CALLBACKS:
-        ddp.setCallbacks([crocoddyl.CallbackVerbose()])
+        solver.setCallbacks([crocoddyl.CallbackVerbose()])
     duration = []
     for _ in range(T):
         c_start = time.time()
-        ddp.solve(xs, us, MAXITER, False, 0.1)
+        solver.solve(xs, us, MAXITER, False, 0.1)
         c_end = time.time()
         duration.append(1e3 * (c_end - c_start))
 
@@ -125,27 +114,21 @@ def runShootingProblemCalcDiffBenchmark(xs, us, problem):
     return avrg_duration, min_duration, max_duration
 
 
-print("\033[1m")
-print("C++:")
-popen = subprocess.check_call(
-    [os.path.dirname(os.path.abspath(__file__)) + "/arm-manipulation-optctrl", str(T)]
-)
-
-print("Python bindings:")
+np.set_printoptions(precision=2)
 xs, us, problem = createProblem(crocoddyl.DifferentialActionModelFreeFwdDynamics)
+print("NQ:", problem.terminalModel.state.nq)
+print("Number of nodes:", problem.T)
 avrg_dur, min_dur, max_dur = runDDPSolveBenchmark(xs, us, problem)
-print(f"  DDP.solve [ms]: {avrg_dur} ({min_dur}, {max_dur})")
+print("  FDDP.solve [ms]: {:.4f} ({:.4f}-{:.4f})".format(avrg_dur, min_dur, max_dur))
 avrg_dur, min_dur, max_dur = runShootingProblemCalcBenchmark(xs, us, problem)
-print(f"  ShootingProblem.calc [ms]: {avrg_dur} ({min_dur}, {max_dur})")
-avrg_dur, min_dur, max_dur = runShootingProblemCalcDiffBenchmark(xs, us, problem)
-print(f"  ShootingProblem.calcDiff [ms]: {avrg_dur} ({min_dur}, {max_dur})")
-
-print("Python:")
-xs, us, problem = createProblem(DifferentialFreeFwdDynamicsModelDerived)
-avrg_dur, min_dur, max_dur = runDDPSolveBenchmark(xs, us, problem)
-print(f"  DDP.solve [ms]: {avrg_dur} ({min_dur}, {max_dur})")
-avrg_dur, min_dur, max_dur = runShootingProblemCalcBenchmark(xs, us, problem)
-print(f"  ShootingProblem.calc [ms]: {avrg_dur} ({min_dur}, {max_dur})")
+print(
+    "  ShootingProblem.calc [ms]: {:.4f} ({:.4f}-{:.4f})".format(
+        avrg_dur, min_dur, max_dur
+    )
+)
 avrg_dur, min_dur, max_dur = runShootingProblemCalcDiffBenchmark(xs, us, problem)
-print(f"  ShootingProblem.calcDiff [ms]: {avrg_dur} ({min_dur}, {max_dur})")
-print("\033[0m")
+print(
+    "  ShootingProblem.calcDiff [ms]: {:.4f} ({:.4f}-{:.4f})".format(
+        avrg_dur, min_dur, max_dur
+    )
+)

benchmark/arm-manipulation-timings.cpp → benchmark/arm_manipulation_timings.cpp

@@ -1,7 +1,8 @@
 ///////////////////////////////////////////////////////////////////////////////
 // BSD 3-Clause License
 //
-// Copyright (C) 2019-2021, LAAS-CNRS, University of Edinburgh
+// Copyright (C) 2019-2023, LAAS-CNRS, University of Edinburgh,
+//                          Heriot-Watt University
 // Copyright note valid unless otherwise stated in individual files.
 // All rights reserved.
 ///////////////////////////////////////////////////////////////////////////////
@@ -15,9 +16,8 @@
 #include "crocoddyl/core/integrator/euler.hpp"
 #include "crocoddyl/core/integrator/rk.hpp"
 #include "crocoddyl/core/mathbase.hpp"
+#include "crocoddyl/core/optctrl/shooting.hpp"
 #include "crocoddyl/core/residuals/control.hpp"
-#include "crocoddyl/core/solvers/ddp.hpp"
-#include "crocoddyl/core/utils/callbacks.hpp"
 #include "crocoddyl/core/utils/timer.hpp"
 #include "crocoddyl/multibody/actions/free-fwddyn.hpp"
 #include "crocoddyl/multibody/actuations/full.hpp"

benchmark/bipedal-timings.cpp → benchmark/bipedal_timings.cpp

@@ -17,9 +17,8 @@
 #include "crocoddyl/core/integrator/euler.hpp"
 #include "crocoddyl/core/integrator/rk.hpp"
 #include "crocoddyl/core/mathbase.hpp"
+#include "crocoddyl/core/optctrl/shooting.hpp"
 #include "crocoddyl/core/residuals/control.hpp"
-#include "crocoddyl/core/solvers/ddp.hpp"
-#include "crocoddyl/core/utils/callbacks.hpp"
 #include "crocoddyl/core/utils/timer.hpp"
 #include "crocoddyl/multibody/actions/contact-fwddyn.hpp"
 #include "crocoddyl/multibody/actuations/floating-base.hpp"

benchmark/bipedal_walk_optctrl.py

@@ -14,7 +14,7 @@
 
 
 def createProblem(gait_phase):
-    robot_model = example_robot_data.loadTalosLegs().model
+    robot_model = example_robot_data.load("talos_legs").model
     rightFoot, leftFoot = "right_sole_link", "left_sole_link"
     gait = SimpleBipedGaitProblem(robot_model, rightFoot, leftFoot)
     q0 = robot_model.referenceConfigurations["half_sitting"].copy()
@@ -43,12 +43,12 @@ def createProblem(gait_phase):
 
 
 def runDDPSolveBenchmark(xs, us, problem):
-    ddp = crocoddyl.SolverDDP(problem)
+    solver = crocoddyl.SolverFDDP(problem)
 
     duration = []
     for _ in range(T):
         c_start = time.time()
-        ddp.solve(xs, us, MAXITER, False, 0.1)
+        solver.solve(xs, us, MAXITER, False, 0.1)
         c_end = time.time()
         duration.append(1e3 * (c_end - c_start))
 
@@ -98,13 +98,20 @@ def runShootingProblemCalcDiffBenchmark(xs, us, problem):
         }
     }
 
-print("\033[1m")
-print("Python bindings:")
 xs, us, problem = createProblem(GAITPHASE)
+print("NQ:", problem.terminalModel.state.nq)
+print("Number of nodes:", problem.T)
 avrg_dur, min_dur, max_dur = runDDPSolveBenchmark(xs, us, problem)
-print(f"  DDP.solve [ms]: {avrg_dur} ({min_dur}, {max_dur})")
+print("  FDDP.solve [ms]: {:.4f} ({:.4f}-{:.4f})".format(avrg_dur, min_dur, max_dur))
 avrg_dur, min_dur, max_dur = runShootingProblemCalcBenchmark(xs, us, problem)
-print(f"  ShootingProblem.calc [ms]: {avrg_dur} ({min_dur}, {max_dur})")
+print(
+    "  ShootingProblem.calc [ms]: {:.4f} ({:.4f}-{:.4f})".format(
+        avrg_dur, min_dur, max_dur
+    )
+)
 avrg_dur, min_dur, max_dur = runShootingProblemCalcDiffBenchmark(xs, us, problem)
-print(f"  ShootingProblem.calcDiff [ms]: {avrg_dur} ({min_dur}, {max_dur})")
-print("\033[0m")
+print(
+    "  ShootingProblem.calcDiff [ms]: {:.4f} ({:.4f}-{:.4f})".format(
+        avrg_dur, min_dur, max_dur
+    )
+)