diff --git a/.github/workflows/ros1-ci.yaml b/.github/workflows/ros1-ci.yaml index 974c89f186..e9a71f7403 100644 --- a/.github/workflows/ros1-ci.yaml +++ b/.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 }} diff --git a/.github/workflows/ros2-ci.yaml b/.github/workflows/ros2-ci.yaml index bbe240f0d7..b85fbb4ba6 100644 --- a/.github/workflows/ros2-ci.yaml +++ b/.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: diff --git a/CMakeLists.txt b/CMakeLists.txt index 31b6d5d8c6..70df2dd5e0 100644 --- a/CMakeLists.txt +++ b/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 diff --git a/benchmark/CMakeLists.txt b/benchmark/CMakeLists.txt index fc45cd1f3a..99c4734aea 100644 --- a/benchmark/CMakeLists.txt +++ b/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() diff --git a/benchmark/all-robots.cpp b/benchmark/all_robots.cpp similarity index 100% rename from benchmark/all-robots.cpp rename to benchmark/all_robots.cpp diff --git a/benchmark/arm-manipulation-optctrl.cpp b/benchmark/arm_manipulation_optctrl.cpp similarity index 87% rename from benchmark/arm-manipulation-optctrl.cpp rename to benchmark/arm_manipulation_optctrl.cpp index 4ea7d9524b..cbbcf540c0 100644 --- a/benchmark/arm-manipulation-optctrl.cpp +++ b/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( 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 > cbs; cbs.push_back(boost::make_shared()); - 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 diff --git a/benchmark/arm_manipulation_optctrl.py b/benchmark/arm_manipulation_optctrl.py index e6d0937273..d8f6978815 100644 --- a/benchmark/arm_manipulation_optctrl.py +++ b/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 + ) +) diff --git a/benchmark/arm-manipulation-timings.cpp b/benchmark/arm_manipulation_timings.cpp similarity index 99% rename from benchmark/arm-manipulation-timings.cpp rename to benchmark/arm_manipulation_timings.cpp index dbc508adfa..763c1cbdf2 100644 --- a/benchmark/arm-manipulation-timings.cpp +++ b/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" diff --git a/benchmark/bipedal-timings.cpp b/benchmark/bipedal_timings.cpp similarity index 99% rename from benchmark/bipedal-timings.cpp rename to benchmark/bipedal_timings.cpp index 980458fc68..c38d347830 100644 --- a/benchmark/bipedal-timings.cpp +++ b/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" diff --git a/benchmark/bipedal_walk_optctrl.py b/benchmark/bipedal_walk_optctrl.py index 3557cfc773..dd779afe73 100644 --- a/benchmark/bipedal_walk_optctrl.py +++ b/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 + ) +) diff --git a/benchmark/factory/arm.hpp b/benchmark/factory/arm.hpp index 1d706a2253..9a976b998f 100644 --- a/benchmark/factory/arm.hpp +++ b/benchmark/factory/arm.hpp @@ -1,7 +1,8 @@ /////////////////////////////////////////////////////////////////////////////// // BSD 3-Clause License // -// Copyright (C) 2019-2021, University of Edinburgh, LAAS-CNRS +// Copyright (C) 2019-2023, University of Edinburgh, LAAS-CNRS, +// Heriot-Watt University // Copyright note valid unless otherwise stated in individual files. // All rights reserved. /////////////////////////////////////////////////////////////////////////////// @@ -46,26 +47,20 @@ void build_arm_action_models( ResidualModelFramePlacement; typedef typename crocoddyl::ResidualModelControlTpl ResidualModelControl; - typedef typename crocoddyl::MathBaseTpl::VectorXs VectorXs; typedef typename crocoddyl::MathBaseTpl::Vector3s Vector3s; typedef typename crocoddyl::MathBaseTpl::Matrix3s Matrix3s; // because urdf is not supported with all scalar types. pinocchio::ModelTpl modeld; pinocchio::urdf::buildModel(EXAMPLE_ROBOT_DATA_MODEL_DIR - "/talos_data/robots/talos_left_arm.urdf", + "/kinova_description/robots/kinova.urdf", modeld); pinocchio::srdf::loadReferenceConfigurations( - modeld, EXAMPLE_ROBOT_DATA_MODEL_DIR "/talos_data/srdf/talos.srdf", + modeld, + EXAMPLE_ROBOT_DATA_MODEL_DIR "/kinova_description/srdf/kinova.srdf", false); - pinocchio::ModelTpl model_full(modeld.cast()), model; - std::vector locked_joints; - locked_joints.push_back(5); - locked_joints.push_back(6); - locked_joints.push_back(7); - pinocchio::buildReducedModel(model_full, locked_joints, - VectorXs::Zero(model_full.nq), model); + pinocchio::ModelTpl model(modeld.cast()); boost::shared_ptr > state = boost::make_shared >( @@ -74,10 +69,10 @@ void build_arm_action_models( boost::shared_ptr goalTrackingCost = boost::make_shared( state, boost::make_shared( - state, model.getFrameId("gripper_left_joint"), + state, model.getFrameId("j2s6s200_end_effector"), pinocchio::SE3Tpl( Matrix3s::Identity(), - Vector3s(Scalar(0), Scalar(0), Scalar(.4))))); + Vector3s(Scalar(0.6), Scalar(0.2), Scalar(0.5))))); boost::shared_ptr xRegCost = boost::make_shared( state, boost::make_shared(state)); @@ -93,9 +88,9 @@ void build_arm_action_models( // Then let's added the running and terminal cost functions runningCostModel->addCost("gripperPose", goalTrackingCost, Scalar(1)); - runningCostModel->addCost("xReg", xRegCost, Scalar(1e-4)); - runningCostModel->addCost("uReg", uRegCost, Scalar(1e-4)); - terminalCostModel->addCost("gripperPose", goalTrackingCost, Scalar(1)); + runningCostModel->addCost("xReg", xRegCost, Scalar(1e-1)); + runningCostModel->addCost("uReg", uRegCost, Scalar(1e-1)); + terminalCostModel->addCost("gripperPose", goalTrackingCost, Scalar(1e3)); // We define an actuation model boost::shared_ptr actuation = @@ -108,12 +103,8 @@ void build_arm_action_models( boost::make_shared( state, actuation, runningCostModel); - // VectorXs armature(state->get_nq()); - // armature << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.; - // runningDAM->set_armature(armature); - // terminalDAM->set_armature(armature); runningModel = - boost::make_shared(runningDAM, Scalar(1e-3)); + boost::make_shared(runningDAM, Scalar(1e-2)); terminalModel = boost::make_shared(runningDAM, Scalar(0.)); } diff --git a/benchmark/lqr-optctrl.cpp b/benchmark/lqr_optctrl.cpp similarity index 79% rename from benchmark/lqr-optctrl.cpp rename to benchmark/lqr_optctrl.cpp index 5a2dbe2977..83b4545070 100644 --- a/benchmark/lqr-optctrl.cpp +++ b/benchmark/lqr_optctrl.cpp @@ -1,13 +1,14 @@ /////////////////////////////////////////////////////////////////////////////// // 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/actions/lqr.hpp" -#include "crocoddyl/core/solvers/ddp.hpp" +#include "crocoddyl/core/solvers/fddp.hpp" #include "crocoddyl/core/states/euclidean.hpp" #include "crocoddyl/core/utils/callbacks.hpp" #include "crocoddyl/core/utils/timer.hpp" @@ -35,25 +36,30 @@ int main(int argc, char* argv[]) { // Formulating the optimal control problem boost::shared_ptr problem = boost::make_shared(x0, runningModels, model); - crocoddyl::SolverDDP ddp(problem); + crocoddyl::SolverFDDP solver(problem); if (CALLBACKS) { std::vector > cbs; cbs.push_back(boost::make_shared()); - ddp.setCallbacks(cbs); + solver.setCallbacks(cbs); } + std::cout << "NQ: " + << solver.get_problem()->get_terminalModel()->get_state()->get_nq() + << std::endl; + std::cout << "Number of nodes: " << N << std::endl; + // 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); + solver.solve(xs, us, MAXITER); 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 @@ -66,7 +72,7 @@ int main(int argc, char* argv[]) { avrg_duration = duration.sum() / T; min_duration = duration.minCoeff(); max_duration = duration.maxCoeff(); - std::cout << "ShootingProblem.calc [ms]: " << avrg_duration << " (" + std::cout << " ShootingProblem.calc [ms]: " << avrg_duration << " (" << min_duration << "-" << max_duration << ")" << std::endl; // Running calcDiff @@ -79,6 +85,6 @@ int main(int argc, char* argv[]) { avrg_duration = duration.sum() / T; min_duration = duration.minCoeff(); max_duration = duration.maxCoeff(); - std::cout << "ShootingProblem.calcDiff [ms]: " << avrg_duration << " (" + std::cout << " ShootingProblem.calcDiff [ms]: " << avrg_duration << " (" << min_duration << "-" << max_duration << ")" << std::endl; } diff --git a/benchmark/lqr_optctrl.py b/benchmark/lqr_optctrl.py index 96c272a119..2fc9f6c329 100644 --- a/benchmark/lqr_optctrl.py +++ b/benchmark/lqr_optctrl.py @@ -1,12 +1,9 @@ -import os -import subprocess import sys import time import numpy as np import crocoddyl -from crocoddyl.utils import LQRModelDerived NX = 37 NU = 12 @@ -28,13 +25,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 i in range(T): c_start = time.time() - ddp.solve(xs, us, MAXITER) + solver.solve(xs, us, MAXITER) c_end = time.time() duration.append(1e3 * (c_end - c_start)) @@ -72,27 +69,20 @@ 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__)) + "/lqr-optctrl", str(T)] -) - -print("Python bindings:") xs, us, problem = createProblem(crocoddyl.ActionModelLQR) +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})") -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(LQRModelDerived) -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 + ) +) diff --git a/benchmark/quadrupedal-gaits-optctrl.cpp b/benchmark/quadrupedal_gaits_optctrl.cpp similarity index 86% rename from benchmark/quadrupedal-gaits-optctrl.cpp rename to benchmark/quadrupedal_gaits_optctrl.cpp index 63baf6afe5..d9be680f03 100644 --- a/benchmark/quadrupedal-gaits-optctrl.cpp +++ b/benchmark/quadrupedal_gaits_optctrl.cpp @@ -1,7 +1,7 @@ /////////////////////////////////////////////////////////////////////////////// // BSD 3-Clause License // -// Copyright (C) 2019, LAAS-CNRS +// Copyright (C) 2019-2023, LAAS-CNRS, Heriot-Watt University // Copyright note valid unless otherwise stated in individual files. // All rights reserved. /////////////////////////////////////////////////////////////////////////////// @@ -44,31 +44,36 @@ int main(int argc, char* argv[]) { boost::shared_ptr problem = gait.createWalkingProblem(x0, stepLength, stepHeight, timeStep, stepKnots, supportKnots); - crocoddyl::SolverFDDP ddp(problem); + crocoddyl::SolverFDDP solver(problem); if (CALLBACKS) { std::vector > cbs; cbs.push_back(boost::make_shared()); - ddp.setCallbacks(cbs); + solver.setCallbacks(cbs); } // Initial State - const std::size_t N = ddp.get_problem()->get_T(); + const std::size_t N = solver.get_problem()->get_T(); std::vector xs(N, x0); std::vector us = problem->quasiStatic_xs(xs); xs.push_back(x0); + std::cout << "NQ: " + << solver.get_problem()->get_terminalModel()->get_state()->get_nq() + << std::endl; + std::cout << "Number of nodes: " << N << std::endl; + // 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.mean(); 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 diff --git a/benchmark/quadrupedal_gaits_optctrl.py b/benchmark/quadrupedal_gaits_optctrl.py index 4cdd2248b9..55c45c9a63 100644 --- a/benchmark/quadrupedal_gaits_optctrl.py +++ b/benchmark/quadrupedal_gaits_optctrl.py @@ -1,5 +1,3 @@ -import os -import subprocess import sys import time @@ -38,7 +36,7 @@ def createProblem(gait_phase): - robot_model = example_robot_data.loadHyQ().model + robot_model = example_robot_data.load("hyq").model lfFoot, rfFoot, lhFoot, rhFoot = "lf_foot", "rf_foot", "lh_foot", "rh_foot" gait = SimpleQuadrupedalGaitProblem(robot_model, lfFoot, rfFoot, lhFoot, rhFoot) q0 = robot_model.referenceConfigurations["standing"].copy() @@ -104,13 +102,13 @@ def createProblem(gait_phase): def runDDPSolveBenchmark(xs, us, problem): - ddp = crocoddyl.SolverFDDP(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)) @@ -200,18 +198,20 @@ def runShootingProblemCalcDiffBenchmark(xs, us, problem): } } -print("\033[1m") -print("C++:") -popen = subprocess.check_call( - [os.path.dirname(os.path.abspath(__file__)) + "/quadrupedal-gaits-optctrl", str(T)] -) - -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 + ) +) diff --git a/benchmark/unicycle-optctrl.cpp b/benchmark/unicycle_optctrl.cpp similarity index 84% rename from benchmark/unicycle-optctrl.cpp rename to benchmark/unicycle_optctrl.cpp index 43a4c6f803..3aa051884d 100644 --- a/benchmark/unicycle-optctrl.cpp +++ b/benchmark/unicycle_optctrl.cpp @@ -1,13 +1,13 @@ /////////////////////////////////////////////////////////////////////////////// // BSD 3-Clause License // -// Copyright (C) 2019, LAAS-CNRS +// Copyright (C) 2019-2023, LAAS-CNRS, Heriot-Watt University // Copyright note valid unless otherwise stated in individual files. // All rights reserved. /////////////////////////////////////////////////////////////////////////////// #include "crocoddyl/core/actions/unicycle.hpp" -#include "crocoddyl/core/solvers/ddp.hpp" +#include "crocoddyl/core/solvers/fddp.hpp" #include "crocoddyl/core/utils/callbacks.hpp" #include "crocoddyl/core/utils/timer.hpp" @@ -32,25 +32,30 @@ int main(int argc, char* argv[]) { // Formulating the optimal control problem boost::shared_ptr problem = boost::make_shared(x0, runningModels, model); - crocoddyl::SolverDDP ddp(problem); + crocoddyl::SolverDDP solver(problem); if (CALLBACKS) { std::vector > cbs; cbs.push_back(boost::make_shared()); - ddp.setCallbacks(cbs); + solver.setCallbacks(cbs); } + std::cout << "NQ: " + << solver.get_problem()->get_terminalModel()->get_state()->get_nq() + << std::endl; + std::cout << "Number of nodes: " << N << std::endl; + // 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); + solver.solve(xs, us, MAXITER); 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 diff --git a/benchmark/unicycle_optctrl.py b/benchmark/unicycle_optctrl.py index 9614eefdf3..1d93d26bc8 100644 --- a/benchmark/unicycle_optctrl.py +++ b/benchmark/unicycle_optctrl.py @@ -1,12 +1,9 @@ -import os -import subprocess import sys import time import numpy as np import crocoddyl -from crocoddyl.utils import UnicycleModelDerived N = 200 # number of nodes T = int(sys.argv[1]) if (len(sys.argv) > 1) else int(5e3) # number of trials @@ -26,13 +23,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 i in range(T): c_start = time.time() - ddp.solve(xs, us, MAXITER) + solver.solve(xs, us, MAXITER) c_end = time.time() duration.append(1e3 * (c_end - c_start)) @@ -70,27 +67,20 @@ def runShootingProblemCalcDiffBenchmark(xs, us, problem): return avrg_dur, min_dur, max_dur -print("\033[1m") -print("C++:") -popen = subprocess.check_call( - [os.path.dirname(os.path.abspath(__file__)) + "/unicycle-optctrl", str(T)] -) - -print("Python bindings:") xs, us, problem = createProblem(crocoddyl.ActionModelUnicycle) +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})") -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(UnicycleModelDerived) -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 + ) +) diff --git a/bindings/CMakeLists.txt b/bindings/CMakeLists.txt index 61ca8d6f6f..a762e146e5 100644 --- a/bindings/CMakeLists.txt +++ b/bindings/CMakeLists.txt @@ -3,4 +3,4 @@ if(BUILD_PYTHON_INTERFACE) set(PYWRAP ${PYWRAP} PARENT_SCOPE) -endif(BUILD_PYTHON_INTERFACE) +endif() diff --git a/bindings/python/crocoddyl/CMakeLists.txt b/bindings/python/crocoddyl/CMakeLists.txt index e48fafb77f..70ada09870 100644 --- a/bindings/python/crocoddyl/CMakeLists.txt +++ b/bindings/python/crocoddyl/CMakeLists.txt @@ -35,7 +35,7 @@ if(GENERATE_PYTHON_STUBS) load_stubgen() generate_stubs(${CMAKE_CURRENT_BINARY_DIR}/.. ${PROJECT_NAME} ${PYTHON_SITELIB} ${PYWRAP}) -endif(GENERATE_PYTHON_STUBS) +endif() install(TARGETS ${PYWRAP} DESTINATION ${${PYWRAP}_INSTALL_DIR}) diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 0f763e1b2b..6679e36ba2 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -20,7 +20,6 @@ set(${PROJECT_NAME}_EXAMPLES_PYTHON install(FILES __init__.py DESTINATION ${PYTHON_SITELIB}/${PROJECT_NAME}/examples) foreach(EXAMPLES ${${PROJECT_NAME}_EXAMPLES_PYTHON}) - python_build(. "${EXAMPLES}.py") install(FILES ${EXAMPLES}.py DESTINATION ${PYTHON_SITELIB}/${PROJECT_NAME}/examples) add_custom_target( @@ -30,15 +29,14 @@ foreach(EXAMPLES ${${PROJECT_NAME}_EXAMPLES_PYTHON}) env PYTHONPATH=${PROJECT_BINARY_DIR}/bindings/python:$ENV{PYTHONPATH} ${PYTHON_EXECUTABLE} - -c - "import ${EXAMPLES}" + "${PROJECT_SOURCE_DIR}/examples/${EXAMPLES}.py" \${INPUT}) # examples are too slow in Debug mode to be used as tests - if(NOT CMAKE_BUILD_TYPE STREQUAL "Debug") + if((NOT CMAKE_BUILD_TYPE STREQUAL "Debug") OR ENABLE_COVERAGE) add_python_unit_test("example-python-${EXAMPLES}" "examples/${EXAMPLES}.py" - bindings/python) - endif(NOT CMAKE_BUILD_TYPE STREQUAL "Debug") + "bindings/python") + endif() endforeach(EXAMPLES ${${PROJECT_NAME}_EXAMPLES_PYTHON}) add_custom_target( @@ -75,5 +73,5 @@ if(NOT CMAKE_BUILD_TYPE STREQUAL "Debug") PYTHON_EXECUTABLE=${PYTHON_EXECUTABLE} /bin/bash ${CMAKE_CURRENT_SOURCE_DIR}/log/check_logfiles.sh) - endif(UNIX) -endif(NOT CMAKE_BUILD_TYPE STREQUAL "Debug") + endif() +endif() diff --git a/package.xml b/package.xml index 7b174a31f6..68015f3a38 100644 --- a/package.xml +++ b/package.xml @@ -20,13 +20,10 @@ catkin ament_cmake - python - python-numpy - python3 - python3-numpy + python3 + python3-numpy coinor-libipopt-dev - python-scipy - python3-scipy + python3-scipy boost eigenpy example-robot-data diff --git a/unittest/CMakeLists.txt b/unittest/CMakeLists.txt index 8146887749..1c1efc4dff 100644 --- a/unittest/CMakeLists.txt +++ b/unittest/CMakeLists.txt @@ -1,6 +1,6 @@ if(BUILD_PYTHON_INTERFACE) add_subdirectory(bindings) -endif(BUILD_PYTHON_INTERFACE) +endif() file(GLOB_RECURSE ${PROJECT_NAME}_FACTORY_TEST ${CMAKE_CURRENT_SOURCE_DIR}/factory/*.cpp) @@ -14,7 +14,7 @@ if(UNIX) if(OPENMP_FOUND) target_link_libraries(${PROJECT_NAME}_unittest ${OpenMP_CXX_LIBRARIES}) endif() -endif(UNIX) +endif() set(${PROJECT_NAME}_CPP_TESTS test_states diff --git a/unittest/bindings/CMakeLists.txt b/unittest/bindings/CMakeLists.txt index 4a6168e657..84de9983c7 100644 --- a/unittest/bindings/CMakeLists.txt +++ b/unittest/bindings/CMakeLists.txt @@ -13,7 +13,7 @@ set(${PROJECT_NAME}_PYTHON_BINDINGS_TESTS foreach(TEST ${${PROJECT_NAME}_PYTHON_BINDINGS_TESTS}) python_build(. "test_${TEST}.py") add_python_unit_test("test_pybinds_${TEST}" - "unittest/bindings/test_${TEST}.py" bindings/python) + "unittest/bindings/test_${TEST}.py" "bindings/python") add_custom_target( "test_pybinds_${TEST}" ${CMAKE_COMMAND} -E env