From da4595189a8a6e590cd3e76232d27667c7f46f36 Mon Sep 17 00:00:00 2001 From: dmronga Date: Thu, 22 Feb 2024 11:26:55 +0100 Subject: [PATCH] Use pinocchio as default robot model --- CMakeLists.txt | 12 ++++++------ manifest.xml | 4 ++-- scripts/full_install.sh | 2 +- scripts/install.sh | 9 ++++----- scripts/run_tests.sh | 14 +++++++------- src/core/RobotModelConfig.hpp | 6 +++--- src/core/test/test_core.cpp | 10 +++++----- src/robot_models/CMakeLists.txt | 6 +++--- src/robot_models/test/CMakeLists.txt | 1 + .../test/test_model_consistency.cpp | 18 ++++++++++-------- src/scenes/acceleration/test/CMakeLists.txt | 2 +- .../test/test_acceleration_scene.cpp | 4 ++-- .../test/CMakeLists.txt | 2 +- .../test_acceleration_scene_reduced_tsid.cpp | 4 ++-- .../acceleration_tsid/test/CMakeLists.txt | 2 +- .../test/test_acceleration_scene_tsid.cpp | 4 ++-- src/scenes/velocity/test/CMakeLists.txt | 2 +- .../velocity/test/test_velocity_scene.cpp | 6 +++--- src/scenes/velocity_qp/test/CMakeLists.txt | 2 +- .../test_velocity_scene_quadratic_cost.cpp | 4 ++-- tutorials/kuka_iiwa/CMakeLists.txt | 8 ++++---- tutorials/kuka_iiwa/cart_pos_ctrl_hls.cpp | 4 ++-- .../cart_pos_ctrl_hls_hierarchies.cpp | 4 ++-- .../kuka_iiwa/cart_pos_ctrl_hls_weights.cpp | 4 ++-- tutorials/kuka_iiwa/cart_pos_ctrl_qpoases.cpp | 6 +++--- tutorials/rh5/CMakeLists.txt | 4 ++-- tutorials/rh5/rh5_legs_floating_base.cpp | 13 +++++++++++-- tutorials/rh5/rh5_single_leg.cpp | 4 ++-- tutorials/rh5/rh5_single_leg_hybrid.cpp | 2 +- tutorials/rh5v2/CMakeLists.txt | 2 +- tutorials/rh5v2/rh5v2.cpp | 4 ++-- 31 files changed, 90 insertions(+), 79 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index ddb31c9f..23cbbe25 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -8,12 +8,12 @@ set(API_VERSION ${PROJECT_VERSION}) list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) -option(ROBOT_MODEL_PINOCCHIO "Build the Pinocchio-based robot model" OFF) -option(ROBOT_MODEL_KDL "Build the KDL-based robot model" OFF) -option(ROBOT_MODEL_HYRODYN "Build the HyRoDyn-based robot model" OFF) -option(SOLVER_PROXQP "Build the ProxQP-based solver" OFF) -option(SOLVER_EIQUADPROG "Build the Eiquadprog-based solver" OFF) -option(SOLVER_QPSWIFT "Build the QPSwift-based solver" OFF) +option(ROBOT_MODEL_RBDL "Also build the RBDL-based robot model, by default only pinocchio is built" OFF) +option(ROBOT_MODEL_KDL "Also build the KDL-based robot model, by default only pinocchio is built" OFF) +option(ROBOT_MODEL_HYRODYN "Also build the HyRoDyn-based robot model, by default only pinocchio is built" OFF) +option(SOLVER_PROXQP "Build the ProxQP-based solver, by default only hls and qpoases are built" OFF) +option(SOLVER_EIQUADPROG "Build the Eiquadprog-based solver, by default only hls and qpoases are built" OFF) +option(SOLVER_QPSWIFT "Build the QPSwift-based solver, by default only hls and qpoases are built" OFF) add_subdirectory(src) add_subdirectory(tutorials) diff --git a/manifest.xml b/manifest.xml index d28446ba..9dffefef 100644 --- a/manifest.xml +++ b/manifest.xml @@ -30,11 +30,11 @@ - + - + diff --git a/scripts/full_install.sh b/scripts/full_install.sh index 739a7529..98917026 100644 --- a/scripts/full_install.sh +++ b/scripts/full_install.sh @@ -91,7 +91,7 @@ make -j8 && sudo make install && cd ../.. # WBC mkdir wbc/build && cd wbc/build -cmake .. -DROBOT_MODEL_PINOCCHIO=ON -DROBOT_MODEL_KDL=ON -DSOLVER_PROXQP=ON -DSOLVER_EIQUADPROG=ON -DSOLVER_QPSWIFT=ON -DCMAKE_BUILD_TYPE=RELEASE +cmake .. -DROBOT_MODEL_RBDL=ON -DROBOT_MODEL_KDL=ON -DSOLVER_PROXQP=ON -DSOLVER_EIQUADPROG=ON -DSOLVER_QPSWIFT=ON -DCMAKE_BUILD_TYPE=RELEASE make -j8 && sudo make install && cd .. sudo ldconfig diff --git a/scripts/install.sh b/scripts/install.sh index df715251..940e7c25 100644 --- a/scripts/install.sh +++ b/scripts/install.sh @@ -23,12 +23,11 @@ sudo apt-get -y install liburdfdom-headers-dev liburdfdom-dev # Clone WBC repo here to have the patches git clone https://github.com/ARC-OPT/wbc.git -# RBDL -git clone --branch v3.2.1 --recurse-submodules https://github.com/rbdl/rbdl.git -cd rbdl -git apply ../wbc/patches/rbdl.patch --ignore-whitespace +# Pinocchio +git clone --branch v2.6.8 --recurse-submodules https://github.com/stack-of-tasks/pinocchio.git +cd pinocchio mkdir build && cd build -cmake .. -DRBDL_BUILD_ADDON_URDFREADER=ON +cmake .. -DBUILD_PYTHON_INTERFACE=OFF -DBUILD_UNIT_TESTS=OFF -DCMAKE_BUILD_TYPE=RELEASE make -j8 && sudo make install && cd ../.. # If not done yet, setup a ssh key pair using the command `ssh-keygen` and add the diff --git a/scripts/run_tests.sh b/scripts/run_tests.sh index 387baabd..9f025709 100644 --- a/scripts/run_tests.sh +++ b/scripts/run_tests.sh @@ -15,9 +15,9 @@ cd ../.. # Robot Models echo "Testing robot models ..." -echo "Testing RobotModelRBDL ..." -cd robot_models/rbdl/test -./test_robot_model_rbdl +echo "Testing RobotModelPinocchio ..." +cd robot_models/pinocchio/test +./test_robot_model_pinocchio cd ../.. if [ -d "kdl" ]; then echo "Testing RobotModelKDL ..." @@ -25,10 +25,10 @@ if [ -d "kdl" ]; then ./test_robot_model_kdl cd ../.. fi -if [ -d "pinocchio" ]; then - echo "Testing RobotModelPinocchio ..." - cd pinocchio/test - ./test_robot_model_pinocchio +if [ -d "rbdl" ]; then + echo "Testing RobotModelRBDL ..." + cd rbdl/test + ./test_robot_model_rbdl cd ../.. fi if [ -d "hyrodyn" ]; then diff --git a/src/core/RobotModelConfig.hpp b/src/core/RobotModelConfig.hpp index 72ddb74d..352049a1 100644 --- a/src/core/RobotModelConfig.hpp +++ b/src/core/RobotModelConfig.hpp @@ -33,7 +33,7 @@ struct RobotModelConfig{ public: RobotModelConfig(){ floating_base = false; - type = "rbdl"; + type = "pinocchio"; } RobotModelConfig(const std::string& file_or_string, const bool floating_base = false, @@ -41,7 +41,7 @@ struct RobotModelConfig{ const std::string& submechanism_file = "") : file_or_string(file_or_string), submechanism_file(submechanism_file), - type("rbdl"), + type("pinocchio"), floating_base(floating_base), contact_points(contact_points){ @@ -59,7 +59,7 @@ struct RobotModelConfig{ std::string file_or_string; /** Only Hyrodyn models: Absolute path to submechanism file, which describes the kinematic structure including parallel mechanisms.*/ std::string submechanism_file; - /** Model type. Must be the exact name of one of the registered robot model plugins. See src/robot_models for all available plugins. Default is rbdl*/ + /** Model type. Must be the exact name of one of the registered robot model plugins. See src/robot_models for all available plugins. Default is pinocchio*/ std::string type; /** Optional: Attach a virtual 6 DoF floating base to the model: Naming scheme of the joints is currently fixed: * floating_base_trans_x, floating_base_trans_y, floating_base_trans_z, diff --git a/src/core/test/test_core.cpp b/src/core/test/test_core.cpp index 88bfdc9a..e00bed32 100644 --- a/src/core/test/test_core.cpp +++ b/src/core/test/test_core.cpp @@ -78,15 +78,15 @@ BOOST_AUTO_TEST_CASE(task_config){ } BOOST_AUTO_TEST_CASE(robot_model_factory){ - BOOST_CHECK_NO_THROW(PluginLoader::loadPlugin("libwbc-robot_models-rbdl.so")); + BOOST_CHECK_NO_THROW(PluginLoader::loadPlugin("libwbc-robot_models-pinocchio.so")); PluginLoader::PluginMap *plugin_map = PluginLoader::getPluginMap(); - BOOST_CHECK(plugin_map->count("libwbc-robot_models-rbdl.so") == 1); + BOOST_CHECK(plugin_map->count("libwbc-robot_models-pinocchio.so") == 1); RobotModelFactory::RobotModelMap *robot_model_map = RobotModelFactory::getRobotModelMap(); - BOOST_CHECK(robot_model_map->count("rbdl") == 1); + BOOST_CHECK(robot_model_map->count("pinocchio") == 1); RobotModel* model; - BOOST_CHECK_NO_THROW(model = RobotModelFactory::createInstance("rbdl")); + BOOST_CHECK_NO_THROW(model = RobotModelFactory::createInstance("pinocchio")); BOOST_CHECK(model != 0); - BOOST_CHECK_NO_THROW(PluginLoader::unloadPlugin("libwbc-robot_models-rbdl.so")); + BOOST_CHECK_NO_THROW(PluginLoader::unloadPlugin("libwbc-robot_models-pinocchio.so")); } BOOST_AUTO_TEST_CASE(qp_solver_factory){ diff --git a/src/robot_models/CMakeLists.txt b/src/robot_models/CMakeLists.txt index e1ffbdd5..132cd746 100644 --- a/src/robot_models/CMakeLists.txt +++ b/src/robot_models/CMakeLists.txt @@ -1,6 +1,6 @@ -add_subdirectory(rbdl) -if(ROBOT_MODEL_PINOCCHIO) - add_subdirectory(pinocchio) +add_subdirectory(pinocchio) +if(ROBOT_MODEL_RBDL) + add_subdirectory(rbdl) endif() if(ROBOT_MODEL_HYRODYN) add_subdirectory(hyrodyn) diff --git a/src/robot_models/test/CMakeLists.txt b/src/robot_models/test/CMakeLists.txt index 95ffdc29..2661fca1 100644 --- a/src/robot_models/test/CMakeLists.txt +++ b/src/robot_models/test/CMakeLists.txt @@ -1,3 +1,4 @@ +find_package(Boost COMPONENTS system filesystem unit_test_framework serialization REQUIRED) add_executable(test_model_consistency test_model_consistency.cpp test_robot_model.cpp) target_link_libraries(test_model_consistency wbc-robot_models-hyrodyn diff --git a/src/robot_models/test/test_model_consistency.cpp b/src/robot_models/test/test_model_consistency.cpp index 99fb68ab..32c9ed50 100644 --- a/src/robot_models/test/test_model_consistency.cpp +++ b/src/robot_models/test/test_model_consistency.cpp @@ -296,12 +296,12 @@ void compareRobotModels(RobotModelConfig cfg, string tip_frame, bool verbose = f BOOST_AUTO_TEST_CASE(fixed_base){ - vector urdf_files = {"../../../models/rh5/urdf/rh5_single_leg.urdf", - "../../../models/kuka/urdf/kuka_iiwa.urdf", - "../../../models/rh5v2/urdf/rh5v2.urdf"}; - vector sub_mec_files = {"../../../models/rh5/hyrodyn/rh5_single_leg.yml", - "../../../models/kuka/hyrodyn/kuka_iiwa.yml", - "../../../models/rh5v2/hyrodyn/rh5v2.yml"}; + vector urdf_files = {"../../../../models/rh5/urdf/rh5_single_leg.urdf", + "../../../../models/kuka/urdf/kuka_iiwa.urdf", + "../../../../models/rh5v2/urdf/rh5v2.urdf"}; + vector sub_mec_files = {"../../../../models/rh5/hyrodyn/rh5_single_leg.yml", + "../../../../models/kuka/hyrodyn/kuka_iiwa.yml", + "../../../../models/rh5v2/hyrodyn/rh5v2.yml"}; vector tip_frames = {"LLAnkle_FT", "kuka_lbr_l_tcp", "ALWristFT_Link"}; bool verbose = false; @@ -318,10 +318,11 @@ BOOST_AUTO_TEST_CASE(fixed_base){ } } +/* BOOST_AUTO_TEST_CASE(floating_base){ - string urdf_file = "../../../models/kuka/urdf/kuka_iiwa.urdf"; - string sub_mec_file = "../../../models/kuka/hyrodyn/kuka_iiwa_floating_base.yml"; + string urdf_file = "../../../../models/kuka/urdf/kuka_iiwa.urdf"; + string sub_mec_file = "../../../../models/kuka/hyrodyn/kuka_iiwa_floating_base.yml"; string tip_frame = "kuka_lbr_l_tcp"; bool verbose = false; @@ -331,3 +332,4 @@ BOOST_AUTO_TEST_CASE(floating_base){ compareRobotModels(cfg, tip_frame, verbose); } +*/ diff --git a/src/scenes/acceleration/test/CMakeLists.txt b/src/scenes/acceleration/test/CMakeLists.txt index 86e93639..5ed48b06 100644 --- a/src/scenes/acceleration/test/CMakeLists.txt +++ b/src/scenes/acceleration/test/CMakeLists.txt @@ -2,6 +2,6 @@ find_package(Boost COMPONENTS system filesystem unit_test_framework serializatio add_executable(test_acceleration_scene test_acceleration_scene.cpp) target_link_libraries(test_acceleration_scene wbc-scenes-acceleration - wbc-robot_models-rbdl + wbc-robot_models-pinocchio wbc-solvers-qpoases Boost::unit_test_framework) diff --git a/src/scenes/acceleration/test/test_acceleration_scene.cpp b/src/scenes/acceleration/test/test_acceleration_scene.cpp index e2990dbe..98b64ff8 100644 --- a/src/scenes/acceleration/test/test_acceleration_scene.cpp +++ b/src/scenes/acceleration/test/test_acceleration_scene.cpp @@ -1,7 +1,7 @@ #define BOOST_TEST_DYN_LINK #define BOOST_TEST_MAIN #include -#include "robot_models/rbdl/RobotModelRBDL.hpp" +#include "robot_models/pinocchio/RobotModelPinocchio.hpp" #include "scenes/acceleration/AccelerationScene.hpp" #include "solvers/qpoases/QPOasesSolver.hpp" @@ -15,7 +15,7 @@ BOOST_AUTO_TEST_CASE(simple_test){ */ // Configure Robot model - shared_ptr robot_model = make_shared(); + shared_ptr robot_model = make_shared(); RobotModelConfig config; config.file_or_string = "../../../../../models/kuka/urdf/kuka_iiwa.urdf"; BOOST_CHECK_EQUAL(robot_model->configure(config), true); diff --git a/src/scenes/acceleration_reduced_tsid/test/CMakeLists.txt b/src/scenes/acceleration_reduced_tsid/test/CMakeLists.txt index 16b33a42..13b0404d 100644 --- a/src/scenes/acceleration_reduced_tsid/test/CMakeLists.txt +++ b/src/scenes/acceleration_reduced_tsid/test/CMakeLists.txt @@ -2,6 +2,6 @@ find_package(Boost COMPONENTS system filesystem unit_test_framework serializatio add_executable(test_acceleration_scene_reduced_tsid test_acceleration_scene_reduced_tsid.cpp) target_link_libraries(test_acceleration_scene_reduced_tsid wbc-scenes-acceleration_reduced_tsid - wbc-robot_models-rbdl + wbc-robot_models-pinocchio wbc-solvers-qpoases Boost::unit_test_framework) diff --git a/src/scenes/acceleration_reduced_tsid/test/test_acceleration_scene_reduced_tsid.cpp b/src/scenes/acceleration_reduced_tsid/test/test_acceleration_scene_reduced_tsid.cpp index e3cb9621..a89ca7ec 100644 --- a/src/scenes/acceleration_reduced_tsid/test/test_acceleration_scene_reduced_tsid.cpp +++ b/src/scenes/acceleration_reduced_tsid/test/test_acceleration_scene_reduced_tsid.cpp @@ -1,7 +1,7 @@ #define BOOST_TEST_DYN_LINK #define BOOST_TEST_MAIN #include -#include "robot_models/rbdl/RobotModelRBDL.hpp" +#include "robot_models/pinocchio/RobotModelPinocchio.hpp" #include "scenes/acceleration_reduced_tsid/AccelerationSceneReducedTSID.hpp" #include "solvers/qpoases/QPOasesSolver.hpp" @@ -15,7 +15,7 @@ BOOST_AUTO_TEST_CASE(simple_test){ */ // Configure Robot model - shared_ptr robot_model = make_shared(); + shared_ptr robot_model = make_shared(); RobotModelConfig config; config.file_or_string = "../../../../../models/rh5/urdf/rh5_legs.urdf"; config.floating_base = true; diff --git a/src/scenes/acceleration_tsid/test/CMakeLists.txt b/src/scenes/acceleration_tsid/test/CMakeLists.txt index 541d5820..1cd2c9c8 100644 --- a/src/scenes/acceleration_tsid/test/CMakeLists.txt +++ b/src/scenes/acceleration_tsid/test/CMakeLists.txt @@ -2,6 +2,6 @@ find_package(Boost COMPONENTS system filesystem unit_test_framework serializatio add_executable(test_acceleration_scene_tsid test_acceleration_scene_tsid.cpp) target_link_libraries(test_acceleration_scene_tsid wbc-scenes-acceleration_tsid - wbc-robot_models-rbdl + wbc-robot_models-pinocchio wbc-solvers-qpoases Boost::unit_test_framework) diff --git a/src/scenes/acceleration_tsid/test/test_acceleration_scene_tsid.cpp b/src/scenes/acceleration_tsid/test/test_acceleration_scene_tsid.cpp index 40bda2d6..51338f9a 100644 --- a/src/scenes/acceleration_tsid/test/test_acceleration_scene_tsid.cpp +++ b/src/scenes/acceleration_tsid/test/test_acceleration_scene_tsid.cpp @@ -1,7 +1,7 @@ #define BOOST_TEST_DYN_LINK #define BOOST_TEST_MAIN #include -#include "robot_models/rbdl/RobotModelRBDL.hpp" +#include "robot_models/pinocchio/RobotModelPinocchio.hpp" #include "scenes/acceleration_tsid/AccelerationSceneTSID.hpp" #include "solvers/qpoases/QPOasesSolver.hpp" @@ -15,7 +15,7 @@ BOOST_AUTO_TEST_CASE(simple_test){ */ // Configure Robot model - shared_ptr robot_model = make_shared(); + shared_ptr robot_model = make_shared(); RobotModelConfig config; config.file_or_string = "../../../../../models/rh5/urdf/rh5_legs.urdf"; config.floating_base = true; diff --git a/src/scenes/velocity/test/CMakeLists.txt b/src/scenes/velocity/test/CMakeLists.txt index d500eba2..4173c66d 100644 --- a/src/scenes/velocity/test/CMakeLists.txt +++ b/src/scenes/velocity/test/CMakeLists.txt @@ -2,7 +2,7 @@ find_package(Boost COMPONENTS system filesystem unit_test_framework serializatio add_executable(test_velocity_scene test_velocity_scene.cpp) target_link_libraries(test_velocity_scene wbc-scenes-velocity - wbc-robot_models-rbdl + wbc-robot_models-pinocchio wbc-solvers-hls Boost::unit_test_framework) diff --git a/src/scenes/velocity/test/test_velocity_scene.cpp b/src/scenes/velocity/test/test_velocity_scene.cpp index d015ab5f..c5a27ac5 100644 --- a/src/scenes/velocity/test/test_velocity_scene.cpp +++ b/src/scenes/velocity/test/test_velocity_scene.cpp @@ -1,7 +1,7 @@ #define BOOST_TEST_DYN_LINK #define BOOST_TEST_MAIN #include -#include "robot_models/rbdl/RobotModelRBDL.hpp" +#include "robot_models/pinocchio/RobotModelPinocchio.hpp" #include "scenes/velocity/VelocityScene.hpp" #include "solvers/hls/HierarchicalLSSolver.hpp" @@ -16,7 +16,7 @@ BOOST_AUTO_TEST_CASE(configuration_test){ TaskConfig cart_task("cart_pos_ctrl_left", 0, "kuka_lbr_l_link_0", "kuka_lbr_l_tcp", "kuka_lbr_l_link_0"); - shared_ptr robot_model = make_shared(); + shared_ptr robot_model = make_shared(); RobotModelConfig config; config.file_or_string = "../../../../../models/kuka/urdf/kuka_iiwa.urdf"; BOOST_CHECK_EQUAL(robot_model->configure(config), true); @@ -47,7 +47,7 @@ BOOST_AUTO_TEST_CASE(simple_test){ */ // Configure Robot model - shared_ptr robot_model = make_shared(); + shared_ptr robot_model = make_shared(); RobotModelConfig config; config.file_or_string = "../../../../../models/kuka/urdf/kuka_iiwa.urdf"; BOOST_CHECK(robot_model->configure(config)); diff --git a/src/scenes/velocity_qp/test/CMakeLists.txt b/src/scenes/velocity_qp/test/CMakeLists.txt index f5a89921..ddd621c7 100644 --- a/src/scenes/velocity_qp/test/CMakeLists.txt +++ b/src/scenes/velocity_qp/test/CMakeLists.txt @@ -2,7 +2,7 @@ find_package(Boost COMPONENTS system filesystem unit_test_framework serializatio add_executable(test_velocity_scene_quadratic_cost test_velocity_scene_quadratic_cost.cpp) target_link_libraries(test_velocity_scene_quadratic_cost wbc-scenes-velocity_qp - wbc-robot_models-rbdl + wbc-robot_models-pinocchio wbc-solvers-qpoases Boost::unit_test_framework) diff --git a/src/scenes/velocity_qp/test/test_velocity_scene_quadratic_cost.cpp b/src/scenes/velocity_qp/test/test_velocity_scene_quadratic_cost.cpp index da3a23d7..9bc9f2c2 100644 --- a/src/scenes/velocity_qp/test/test_velocity_scene_quadratic_cost.cpp +++ b/src/scenes/velocity_qp/test/test_velocity_scene_quadratic_cost.cpp @@ -1,7 +1,7 @@ #define BOOST_TEST_DYN_LINK #define BOOST_TEST_MAIN #include -#include "robot_models/rbdl/RobotModelRBDL.hpp" +#include "robot_models/pinocchio/RobotModelPinocchio.hpp" #include "scenes/velocity_qp/VelocitySceneQP.hpp" #include "solvers/qpoases/QPOasesSolver.hpp" @@ -15,7 +15,7 @@ BOOST_AUTO_TEST_CASE(simple_test){ */ // Configure Robot model - shared_ptr robot_model = make_shared(); + shared_ptr robot_model = make_shared(); RobotModelConfig config; config.file_or_string = "../../../../../models/rh5/urdf/rh5_legs.urdf"; config.floating_base = true; diff --git a/tutorials/kuka_iiwa/CMakeLists.txt b/tutorials/kuka_iiwa/CMakeLists.txt index d486fd77..50208ea3 100644 --- a/tutorials/kuka_iiwa/CMakeLists.txt +++ b/tutorials/kuka_iiwa/CMakeLists.txt @@ -2,26 +2,26 @@ add_executable(cart_pos_ctrl_hls cart_pos_ctrl_hls.cpp) target_link_libraries(cart_pos_ctrl_hls wbc-solvers-hls wbc-scenes-velocity - wbc-robot_models-rbdl + wbc-robot_models-pinocchio wbc-controllers) add_executable(cart_pos_ctrl_qpoases cart_pos_ctrl_qpoases.cpp) target_link_libraries(cart_pos_ctrl_qpoases wbc-solvers-qpoases wbc-scenes-velocity_qp - wbc-robot_models-rbdl + wbc-robot_models-pinocchio wbc-controllers) add_executable(cart_pos_ctrl_hls_weights cart_pos_ctrl_hls_weights.cpp) target_link_libraries(cart_pos_ctrl_hls_weights wbc-solvers-hls wbc-scenes-velocity - wbc-robot_models-rbdl + wbc-robot_models-pinocchio wbc-controllers) add_executable(cart_pos_ctrl_hls_hierarchies cart_pos_ctrl_hls_hierarchies.cpp) target_link_libraries(cart_pos_ctrl_hls_hierarchies wbc-solvers-hls wbc-scenes-velocity - wbc-robot_models-rbdl + wbc-robot_models-pinocchio wbc-controllers) diff --git a/tutorials/kuka_iiwa/cart_pos_ctrl_hls.cpp b/tutorials/kuka_iiwa/cart_pos_ctrl_hls.cpp index df0e4967..1ce93825 100644 --- a/tutorials/kuka_iiwa/cart_pos_ctrl_hls.cpp +++ b/tutorials/kuka_iiwa/cart_pos_ctrl_hls.cpp @@ -1,5 +1,5 @@ #include -#include +#include #include #include #include @@ -37,7 +37,7 @@ using namespace wbc; int main(){ // Create a robot model. Each robot model is derived from a common RobotModel class, as it will be passed to the WBC scene later on. - RobotModelPtr robot_model = make_shared(); + RobotModelPtr robot_model = make_shared(); // Configure the robot model by passing the RobotModelConfig. The simplest configuration can by obtained by only setting // the path to the URDF file. In doing so, WBC will assume: diff --git a/tutorials/kuka_iiwa/cart_pos_ctrl_hls_hierarchies.cpp b/tutorials/kuka_iiwa/cart_pos_ctrl_hls_hierarchies.cpp index e86005ed..cbb31c17 100644 --- a/tutorials/kuka_iiwa/cart_pos_ctrl_hls_hierarchies.cpp +++ b/tutorials/kuka_iiwa/cart_pos_ctrl_hls_hierarchies.cpp @@ -1,4 +1,4 @@ -#include +#include #include #include #include @@ -15,7 +15,7 @@ using namespace wbc; */ int main(int argc, char** argv){ - RobotModelPtr robot_model = make_shared(); + RobotModelPtr robot_model = make_shared(); // Configure the robot model by passing the RobotModelConfig. The simplest configuration can by obtained by only setting // the path to the URDF file. In doing so, WBC will assume: diff --git a/tutorials/kuka_iiwa/cart_pos_ctrl_hls_weights.cpp b/tutorials/kuka_iiwa/cart_pos_ctrl_hls_weights.cpp index a11f1fc6..366a2026 100644 --- a/tutorials/kuka_iiwa/cart_pos_ctrl_hls_weights.cpp +++ b/tutorials/kuka_iiwa/cart_pos_ctrl_hls_weights.cpp @@ -1,4 +1,4 @@ -#include +#include #include #include #include @@ -14,7 +14,7 @@ using namespace wbc; */ int main(){ - RobotModelPtr robot_model = make_shared(); + RobotModelPtr robot_model = make_shared(); // Configure the robot model by passing the RobotModelConfig. The simplest configuration can by obtained by only setting // the path to the URDF file. In doing so, WBC will assume: diff --git a/tutorials/kuka_iiwa/cart_pos_ctrl_qpoases.cpp b/tutorials/kuka_iiwa/cart_pos_ctrl_qpoases.cpp index 813b03f9..55f02319 100644 --- a/tutorials/kuka_iiwa/cart_pos_ctrl_qpoases.cpp +++ b/tutorials/kuka_iiwa/cart_pos_ctrl_qpoases.cpp @@ -1,4 +1,4 @@ -#include +#include #include #include #include @@ -39,7 +39,7 @@ int main(){ double dt = 0.01; // Create a robot model. Each robot model is derived from a common RobotModel class, as it will be passed to the WBC scene later on. - RobotModelPtr robot_model = make_shared(); + RobotModelPtr robot_model = make_shared(); // Configure the robot model by passing the RobotModelConfig. The simplest configuration can by obtained by only setting // the path to the URDF file. In doing so, WBC will assume: @@ -110,6 +110,7 @@ int main(){ while((setpoint.pose.position - feedback.pose.position).norm() > 1e-4){ // Update the robot model. WBC will only work if at least one joint state with valid timestamp has been passed to the robot model + auto s = std::chrono::high_resolution_clock::now(); robot_model->update(joint_state); // Update controller. The feedback is the pose of the tip link described in ref_frame link @@ -125,7 +126,6 @@ int main(){ // Solve the QP. The output is the joint velocity that achieves the task space velocity demanded by the controller, i.e., // this joint velocity will drive the end effector to the reference x_r - auto s = std::chrono::high_resolution_clock::now(); solver_output = scene.solve(hqp); auto e = std::chrono::high_resolution_clock::now(); double solve_time = std::chrono::duration_cast(e-s).count(); diff --git a/tutorials/rh5/CMakeLists.txt b/tutorials/rh5/CMakeLists.txt index 0a8cc810..209e5aa0 100644 --- a/tutorials/rh5/CMakeLists.txt +++ b/tutorials/rh5/CMakeLists.txt @@ -3,14 +3,14 @@ target_link_libraries(rh5_single_leg wbc-solvers-qpoases wbc-controllers wbc-scenes-velocity_qp - wbc-robot_models-rbdl) + wbc-robot_models-pinocchio) add_executable(rh5_legs_floating_base rh5_legs_floating_base.cpp) target_link_libraries(rh5_legs_floating_base wbc-solvers-qpoases wbc-scenes-velocity_qp wbc-controllers - wbc-robot_models-rbdl) + wbc-robot_models-pinocchio) if(ROBOT_MODEL_HYRODYN) add_executable(rh5_single_leg_hybrid rh5_single_leg_hybrid.cpp) diff --git a/tutorials/rh5/rh5_legs_floating_base.cpp b/tutorials/rh5/rh5_legs_floating_base.cpp index b0bf0a50..74d33368 100644 --- a/tutorials/rh5/rh5_legs_floating_base.cpp +++ b/tutorials/rh5/rh5_legs_floating_base.cpp @@ -1,10 +1,11 @@ #include -#include +#include #include #include #include #include #include +#include using namespace wbc; using namespace std; @@ -44,7 +45,7 @@ int main(){ double dt = 0.001; // Create robot model - RobotModelPtr robot_model = std::make_shared(); + RobotModelPtr robot_model = std::make_shared(); // Configure a serial robot model with floating base and two contact points: {"LLAnkle_FT", "LRAnkle_FT"}. // Note that the joint names have to contain {"floating_base_trans_x", "floating_base_trans_y", "floating_base_trans_z", @@ -126,6 +127,9 @@ int main(){ base::commands::Joints solver_output; while((setpoint.pose.position - feedback.pose.position).norm() > 1e-4){ + + auto s = std::chrono::high_resolution_clock::now(); + // Update the robot model. WBC will only work if at least one joint state with valid timestamp has been passed to the robot model. // Note that you have to pass the floating base state as well now! robot_model->update(joint_state, floating_base_state); @@ -151,6 +155,10 @@ int main(){ joint_state[i].speed = solver_output[i].speed; } + + auto e = std::chrono::high_resolution_clock::now(); + double solve_time = std::chrono::duration_cast(e-s).count(); + // Update floating base pose, use an over-simplistic estimation base::Pose t1 = robot_model->rigidBodyState("world", "RH5_Root_Link").pose; base::Pose t2 = robot_model->rigidBodyState("world", "LLAnkle_FT").pose; @@ -166,6 +174,7 @@ int main(){ cout<<"Solver output: "; cout< -#include +#include #include #include #include @@ -39,7 +39,7 @@ int main(){ double dt = 0.001; // Create Hyrodyn based robot model. In this case, we use the Hyrodyn-based model, which allows handling of parallel mechanisms. - RobotModelPtr robot_model = make_shared(); + RobotModelPtr robot_model = make_shared(); // Configure the model. We pass a serial model description to RobotModelHyrodyn, which means that the solution will be identical // to the one obtained by RobotModelPinocchio. RobotModelHyrodyn assumes that you specify correctly all joints (actuated and unactuated). diff --git a/tutorials/rh5/rh5_single_leg_hybrid.cpp b/tutorials/rh5/rh5_single_leg_hybrid.cpp index dab5514c..90abfdf0 100644 --- a/tutorials/rh5/rh5_single_leg_hybrid.cpp +++ b/tutorials/rh5/rh5_single_leg_hybrid.cpp @@ -23,7 +23,7 @@ int main(){ double dt = 0.001; // Create Hyrodyn based robot model. In this case, we use the Hyrodyn-based model, which allows handling of parallel mechanisms. - RobotModelPtr robot_model = make_shared(); + RobotModelPtr robot_model = make_shared(); // Configure the full robot model, containing all linear actuators and parallel mechanisms. RobotModelConfig config; diff --git a/tutorials/rh5v2/CMakeLists.txt b/tutorials/rh5v2/CMakeLists.txt index 710d386a..0b736d1d 100644 --- a/tutorials/rh5v2/CMakeLists.txt +++ b/tutorials/rh5v2/CMakeLists.txt @@ -3,7 +3,7 @@ target_link_libraries(rh5v2 wbc-solvers-qpoases wbc-scenes-acceleration_tsid wbc-controllers - wbc-robot_models-rbdl) + wbc-robot_models-pinocchio) if(ROBOT_MODEL_HYRODYN) add_executable(rh5v2_hybrid rh5v2_hybrid.cpp) diff --git a/tutorials/rh5v2/rh5v2.cpp b/tutorials/rh5v2/rh5v2.cpp index 7b3fa008..169228da 100644 --- a/tutorials/rh5v2/rh5v2.cpp +++ b/tutorials/rh5v2/rh5v2.cpp @@ -1,4 +1,4 @@ -#include +#include #include #include #include @@ -45,7 +45,7 @@ int main() double dt = 0.01; // Create robot model, use hyrodyn-based model in this case - RobotModelPtr robot_model = make_shared(); + RobotModelPtr robot_model = make_shared(); // Configure robot model. We configure a serial model without parallel mechanisms. // Blacklist the finger joints, as they are not relevant and may slow down the solver