From 16336c09da4d293d8187f04d31edb42ea9ff9a20 Mon Sep 17 00:00:00 2001 From: dmronga Date: Wed, 25 Oct 2023 17:13:04 +0200 Subject: [PATCH] Allow loading robot URDF from file or string --- .../robot_models/benchmark_robot_models.cpp | 22 +++++++++---------- benchmarks/scenes/benchmark_scenes.cpp | 18 +++++++-------- benchmarks/solvers/benchmark_solvers.cpp | 10 ++++----- bindings/python/core/core.cpp | 3 +-- bindings/python/test/test_robot_models.py | 2 +- bindings/python/test/test_scenes.py | 4 ++-- bindings/python/wbc_types_conversions.h | 2 +- src/core/RobotModel.cpp | 10 +++++++++ src/core/RobotModel.hpp | 3 +++ src/core/RobotModelConfig.hpp | 12 +++++----- .../hyrodyn/RobotModelHyrodyn.cpp | 4 ++-- src/robot_models/kdl/RobotModelKDL.cpp | 6 ++--- .../pinocchio/RobotModelPinocchio.cpp | 6 ++--- src/robot_models/rbdl/RobotModelRBDL.cpp | 6 ++--- test/scenes/test_acceleration_scene.cpp | 2 +- .../test_acceleration_scene_reduced_tsid.cpp | 8 +++---- test/scenes/test_acceleration_scene_tsid.cpp | 2 +- test/scenes/test_velocity_scene.cpp | 4 ++-- .../test_velocity_scene_quadratic_cost.cpp | 2 +- tutorials/kuka_iiwa/cart_pos_ctrl_hls.cpp | 2 +- .../cart_pos_ctrl_hls_hierarchies.cpp | 2 +- .../kuka_iiwa/cart_pos_ctrl_hls_weights.cpp | 2 +- tutorials/kuka_iiwa/cart_pos_ctrl_qpoases.cpp | 2 +- tutorials/rh5/rh5_legs_floating_base.cpp | 2 +- tutorials/rh5/rh5_single_leg.cpp | 2 +- tutorials/rh5/rh5_single_leg_hybrid.cpp | 2 +- 26 files changed, 76 insertions(+), 64 deletions(-) diff --git a/benchmarks/robot_models/benchmark_robot_models.cpp b/benchmarks/robot_models/benchmark_robot_models.cpp index 338fc598..970c91ee 100644 --- a/benchmarks/robot_models/benchmark_robot_models.cpp +++ b/benchmarks/robot_models/benchmark_robot_models.cpp @@ -129,11 +129,11 @@ map evaluateRobotModel(RobotModelPtr robot_model, const s return results; } -void runKUKAIiwaBenchmarks(int n_samples){ +void runKUKAIiwaBenchmarks(int n_samples){ cout << " ----------- Evaluating KUKA iiwa model -----------" << endl; RobotModelConfig cfg; - cfg.file = "../../../models/kuka/urdf/kuka_iiwa.urdf"; + cfg.file_or_string = "../../../models/kuka/urdf/kuka_iiwa.urdf"; cfg.submechanism_file = "../../../models/kuka/hyrodyn/kuka_iiwa.yml"; RobotModelPtr robot_model_kdl = std::make_shared(); @@ -170,7 +170,7 @@ void runKUKAIiwaBenchmarks(int n_samples){ void runRH5SingleLegBenchmarks(int n_samples){ cout << " ----------- Evaluating RH5 Single Leg model -----------" << endl; RobotModelConfig cfg; - cfg.file = "../../../models/rh5/urdf/rh5_single_leg.urdf"; + cfg.file_or_string = "../../../models/rh5/urdf/rh5_single_leg.urdf"; cfg.submechanism_file = "../../../models/rh5/hyrodyn/rh5_single_leg.yml"; RobotModelPtr robot_model_kdl = std::make_shared(); @@ -183,7 +183,7 @@ void runRH5SingleLegBenchmarks(int n_samples){ if(!robot_model_hyrodyn->configure(cfg))abort(); if(!robot_model_pinocchio->configure(cfg))abort(); if(!robot_model_rbdl->configure(cfg))abort(); - cfg.file = "../../../models/rh5/urdf/rh5_single_leg_hybrid.urdf"; + cfg.file_or_string = "../../../models/rh5/urdf/rh5_single_leg_hybrid.urdf"; cfg.submechanism_file = "../../../models/rh5/hyrodyn/rh5_single_leg_hybrid.yml"; if(!robot_model_hyrodyn_hybrid->configure(cfg))abort(); @@ -196,7 +196,7 @@ void runRH5SingleLegBenchmarks(int n_samples){ toCSV(results_kdl, "results/rh5_single_leg_robot_model_kdl.csv"); toCSV(results_hyrodyn, "results/rh5_single_leg_robot_model_hyrodyn.csv"); - toCSV(results_hyrodyn_hybrid, "results/rh5_single_leg_robot_model_hyrodyn_hybrid.csv"); + toCSV(results_hyrodyn_hybrid, "results/rh5_single_leg_robot_model_hyrodyn_hybrid.csv"); toCSV(results_pinocchio, "results/rh5_single_leg_robot_model_pinocchio.csv"); toCSV(results_rbdl, "results/rh5_single_leg_robot_model_rbdl.csv"); @@ -216,7 +216,7 @@ void runRH5SingleLegBenchmarks(int n_samples){ void runRH5LegsBenchmarks(int n_samples){ cout << " ----------- Evaluating RH5 Legs model -----------" << endl; RobotModelConfig cfg; - cfg.file = "../../../models/rh5/urdf/rh5_legs.urdf"; + cfg.file_or_string = "../../../models/rh5/urdf/rh5_legs.urdf"; cfg.submechanism_file = "../../../models/rh5/hyrodyn/rh5_legs.yml"; cfg.floating_base = true; @@ -230,7 +230,7 @@ void runRH5LegsBenchmarks(int n_samples){ if(!robot_model_hyrodyn->configure(cfg))abort(); if(!robot_model_pinocchio->configure(cfg))abort(); if(!robot_model_rbdl->configure(cfg))abort(); - cfg.file = "../../../models/rh5/urdf/rh5_legs_hybrid.urdf"; + cfg.file_or_string = "../../../models/rh5/urdf/rh5_legs_hybrid.urdf"; cfg.submechanism_file = "../../../models/rh5/hyrodyn/rh5_legs_hybrid.yml"; if(!robot_model_hyrodyn_hybrid->configure(cfg))abort(); @@ -262,7 +262,7 @@ void runRH5LegsBenchmarks(int n_samples){ void runRH5Benchmarks(int n_samples){ cout << " ----------- Evaluating RH5 model -----------" << endl; RobotModelConfig cfg; - cfg.file = "../../../models/rh5/urdf/rh5.urdf"; + cfg.file_or_string = "../../../models/rh5/urdf/rh5.urdf"; cfg.submechanism_file = "../../../models/rh5/hyrodyn/rh5.yml"; cfg.floating_base = true; @@ -276,7 +276,7 @@ void runRH5Benchmarks(int n_samples){ if(!robot_model_hyrodyn->configure(cfg))abort(); if(!robot_model_pinocchio->configure(cfg))abort(); if(!robot_model_rbdl->configure(cfg))abort(); - cfg.file = "../../../models/rh5/urdf/rh5_hybrid.urdf"; + cfg.file_or_string = "../../../models/rh5/urdf/rh5_hybrid.urdf"; cfg.submechanism_file = "../../../models/rh5/hyrodyn/rh5_hybrid.yml"; if(!robot_model_hyrodyn_hybrid->configure(cfg))abort(); @@ -308,7 +308,7 @@ void runRH5Benchmarks(int n_samples){ void runRH5v2Benchmarks(int n_samples){ cout << " ----------- Evaluating RH5v2 model -----------" << endl; RobotModelConfig cfg; - cfg.file = "../../../models/rh5v2/urdf/rh5v2.urdf"; + cfg.file_or_string = "../../../models/rh5v2/urdf/rh5v2.urdf"; cfg.submechanism_file = "../../../models/rh5v2/hyrodyn/rh5v2.yml"; RobotModelPtr robot_model_kdl = std::make_shared(); @@ -321,7 +321,7 @@ void runRH5v2Benchmarks(int n_samples){ if(!robot_model_hyrodyn->configure(cfg))abort(); if(!robot_model_pinocchio->configure(cfg))abort(); if(!robot_model_rbdl->configure(cfg))abort(); - cfg.file = "../../../models/rh5v2/urdf/rh5v2_hybrid.urdf"; + cfg.file_or_string = "../../../models/rh5v2/urdf/rh5v2_hybrid.urdf"; cfg.submechanism_file = "../../../models/rh5v2/hyrodyn/rh5v2_hybrid.yml"; if(!robot_model_hyrodyn_hybrid->configure(cfg))abort(); diff --git a/benchmarks/scenes/benchmark_scenes.cpp b/benchmarks/scenes/benchmark_scenes.cpp index 071d4085..d3345939 100644 --- a/benchmarks/scenes/benchmark_scenes.cpp +++ b/benchmarks/scenes/benchmark_scenes.cpp @@ -76,7 +76,7 @@ void evaluateAccelerationSceneReducedTSID(map robot_models void runKUKAIiwaBenchmarks(int n_samples){ cout << " ----------- Evaluating KUKA iiwa model -----------" << endl; RobotModelConfig cfg; - cfg.file = "../../../models/kuka/urdf/kuka_iiwa.urdf"; + cfg.file_or_string = "../../../models/kuka/urdf/kuka_iiwa.urdf"; cfg.submechanism_file = "../../../models/kuka/hyrodyn/kuka_iiwa.yml"; const string root = "kuka_lbr_l_link_0", tip = "kuka_lbr_l_tcp"; const string robot = "kuka_iiwa"; @@ -107,7 +107,7 @@ void runKUKAIiwaBenchmarks(int n_samples){ void runRH5SingleLegBenchmarks(int n_samples){ cout << " ----------- Evaluating RH5 Single Leg model -----------" << endl; RobotModelConfig cfg; - cfg.file = "../../../models/rh5/urdf/rh5_single_leg.urdf"; + cfg.file_or_string = "../../../models/rh5/urdf/rh5_single_leg.urdf"; cfg.submechanism_file = "../../../models/rh5/hyrodyn/rh5_single_leg.yml"; const string robot = "rh5_single_leg"; @@ -121,7 +121,7 @@ void runRH5SingleLegBenchmarks(int n_samples){ abort(); } robot_models["hyrodyn_hybrid"] = make_shared(); - cfg.file = "../../../models/rh5/urdf/rh5_single_leg_hybrid.urdf"; + cfg.file_or_string = "../../../models/rh5/urdf/rh5_single_leg_hybrid.urdf"; cfg.submechanism_file = "../../../models/rh5/hyrodyn/rh5_single_leg_hybrid.yml"; if(!robot_models["hyrodyn_hybrid"]->configure(cfg)) abort(); for(auto it : robot_models){ @@ -145,7 +145,7 @@ void runRH5SingleLegBenchmarks(int n_samples){ void runRH5LegsBenchmarks(int n_samples){ cout << " ----------- Evaluating RH5 Legs Model -----------" << endl; RobotModelConfig cfg; - cfg.file = "../../../models/rh5/urdf/rh5_legs.urdf"; + cfg.file_or_string = "../../../models/rh5/urdf/rh5_legs.urdf"; cfg.submechanism_file = "../../../models/rh5/hyrodyn/rh5_legs.yml"; cfg.floating_base = true; ActiveContact contact(1,0.6); @@ -164,7 +164,7 @@ void runRH5LegsBenchmarks(int n_samples){ if(!it.second->configure(cfg)) abort(); } robot_models["hyrodyn_hybrid"] = make_shared(); - cfg.file = "../../../models/rh5/urdf/rh5_legs_hybrid.urdf"; + cfg.file_or_string = "../../../models/rh5/urdf/rh5_legs_hybrid.urdf"; cfg.submechanism_file = "../../../models/rh5/hyrodyn/rh5_legs_hybrid.yml"; if(!robot_models["hyrodyn_hybrid"]->configure(cfg)) abort(); for(auto it : robot_models){ @@ -194,7 +194,7 @@ void runRH5LegsBenchmarks(int n_samples){ void runRH5Benchmarks(int n_samples){ cout << " ----------- Evaluating RH5 Model -----------" << endl; RobotModelConfig cfg; - cfg.file = "../../../models/rh5/urdf/rh5.urdf"; + cfg.file_or_string = "../../../models/rh5/urdf/rh5.urdf"; cfg.submechanism_file = "../../../models/rh5/hyrodyn/rh5.yml"; cfg.floating_base = true; ActiveContact contact(1,0.6); @@ -213,7 +213,7 @@ void runRH5Benchmarks(int n_samples){ if(!it.second->configure(cfg)) abort(); } robot_models["hyrodyn_hybrid"] = make_shared(); - cfg.file = "../../../models/rh5/urdf/rh5_hybrid.urdf"; + cfg.file_or_string = "../../../models/rh5/urdf/rh5_hybrid.urdf"; cfg.submechanism_file = "../../../models/rh5/hyrodyn/rh5_hybrid.yml"; if(!robot_models["hyrodyn_hybrid"]->configure(cfg)) abort(); for(auto it : robot_models){ @@ -244,7 +244,7 @@ void runRH5Benchmarks(int n_samples){ void runRH5v2Benchmarks(int n_samples){ cout << " ----------- Evaluating RH5v2 model -----------" << endl; RobotModelConfig cfg; - cfg.file = "../../../models/rh5v2/urdf/rh5v2.urdf"; + cfg.file_or_string = "../../../models/rh5v2/urdf/rh5v2.urdf"; cfg.submechanism_file = "../../../models/rh5v2/hyrodyn/rh5v2.yml"; const string robot = "rh5v2"; @@ -257,7 +257,7 @@ void runRH5v2Benchmarks(int n_samples){ if(!it.second->configure(cfg)) abort(); } robot_models["hyrodyn_hybrid"] = make_shared(); - cfg.file = "../../../models/rh5v2/urdf/rh5v2_hybrid.urdf"; + cfg.file_or_string = "../../../models/rh5v2/urdf/rh5v2_hybrid.urdf"; cfg.submechanism_file = "../../../models/rh5v2/hyrodyn/rh5v2_hybrid.yml"; if(!robot_models["hyrodyn_hybrid"]->configure(cfg)) abort(); for(auto it : robot_models){ diff --git a/benchmarks/solvers/benchmark_solvers.cpp b/benchmarks/solvers/benchmark_solvers.cpp index 97e0b0c1..b51e0e02 100644 --- a/benchmarks/solvers/benchmark_solvers.cpp +++ b/benchmarks/solvers/benchmark_solvers.cpp @@ -89,7 +89,7 @@ map evaluateProxQP(RobotModelPtr robot_model, string roo void runKUKAIiwaBenchmarks(int n_samples){ RobotModelConfig cfg; - cfg.file = "../../../models/kuka/urdf/kuka_iiwa.urdf"; + cfg.file_or_string = "../../../models/kuka/urdf/kuka_iiwa.urdf"; RobotModelPtr robot_model = std::make_shared(); if(!robot_model->configure(cfg))abort(); @@ -136,7 +136,7 @@ void runKUKAIiwaBenchmarks(int n_samples){ void runRH5SingleLegBenchmarks(int n_samples){ RobotModelConfig cfg; - cfg.file = "../../../models/rh5/urdf/rh5_single_leg.urdf"; + cfg.file_or_string = "../../../models/rh5/urdf/rh5_single_leg.urdf"; RobotModelPtr robot_model = std::make_shared(); if(!robot_model->configure(cfg))abort(); @@ -185,7 +185,7 @@ void runRH5SingleLegBenchmarks(int n_samples){ void runRH5LegsBenchmarks(int n_samples){ RobotModelConfig cfg; - cfg.file = "../../../models/rh5/urdf/rh5_legs.urdf"; + cfg.file_or_string = "../../../models/rh5/urdf/rh5_legs.urdf"; cfg.floating_base = true; ActiveContact contact(1,0.6); contact.wx = 0.2; @@ -244,7 +244,7 @@ void runRH5LegsBenchmarks(int n_samples){ void runRH5Benchmarks(int n_samples){ RobotModelConfig cfg; - cfg.file = "../../../models/rh5/urdf/rh5.urdf"; + cfg.file_or_string = "../../../models/rh5/urdf/rh5.urdf"; cfg.floating_base = true; ActiveContact contact(1,0.6); contact.wx = 0.2; @@ -303,7 +303,7 @@ void runRH5Benchmarks(int n_samples){ void runRH5v2Benchmarks(int n_samples){ RobotModelConfig cfg; - cfg.file = "../../../models/rh5v2/urdf/rh5v2.urdf"; + cfg.file_or_string = "../../../models/rh5v2/urdf/rh5v2.urdf"; RobotModelPtr robot_model = std::make_shared(); if(!robot_model->configure(cfg))abort(); diff --git a/bindings/python/core/core.cpp b/bindings/python/core/core.cpp index dcaf351d..c0925413 100644 --- a/bindings/python/core/core.cpp +++ b/bindings/python/core/core.cpp @@ -120,7 +120,7 @@ BOOST_PYTHON_MODULE(core){ py::make_setter(&base::NamedVector::elements)); py::class_("RobotModelConfig") - .def_readwrite("file", &wbc::RobotModelConfig::file) + .def_readwrite("file_or_string", &wbc::RobotModelConfig::file_or_string) .def_readwrite("submechanism_file", &wbc::RobotModelConfig::submechanism_file) .def_readwrite("floating_base", &wbc::RobotModelConfig::floating_base) .def_readwrite("floating_base", &wbc::RobotModelConfig::floating_base) @@ -238,4 +238,3 @@ BOOST_PYTHON_MODULE(core){ py::make_getter(&wbc::TasksStatus::elements, py::return_value_policy()), py::make_setter(&wbc::TasksStatus::elements)); } - diff --git a/bindings/python/test/test_robot_models.py b/bindings/python/test/test_robot_models.py index 35da199c..ad1d744f 100644 --- a/bindings/python/test/test_robot_models.py +++ b/bindings/python/test/test_robot_models.py @@ -17,7 +17,7 @@ def run(robot_model): contacts.elements = [a,a] r=RobotModelConfig() - r.file="../../../models/rh5/urdf/rh5_single_leg.urdf" + r.file_or_string="../../../models/rh5/urdf/rh5_single_leg.urdf" r.floating_base = False assert robot_model.configure(r) == True diff --git a/bindings/python/test/test_scenes.py b/bindings/python/test/test_scenes.py index 3892ed38..63772b08 100644 --- a/bindings/python/test/test_scenes.py +++ b/bindings/python/test/test_scenes.py @@ -150,7 +150,7 @@ def run_acceleration_wbc(scene, robot_model): def test_configure(): robot_model=RobotModelRBDL() r=RobotModelConfig() - r.file="../../../models/kuka/urdf/kuka_iiwa.urdf" + r.file_or_string="../../../models/kuka/urdf/kuka_iiwa.urdf" r.actuated_joint_names = ["kuka_lbr_l_joint_1", "kuka_lbr_l_joint_2", "kuka_lbr_l_joint_3", "kuka_lbr_l_joint_4", "kuka_lbr_l_joint_5", "kuka_lbr_l_joint_6", "kuka_lbr_l_joint_7"] r.joint_names = r.actuated_joint_names assert robot_model.configure(r) == True @@ -175,7 +175,7 @@ def test_configure(): def test_solver_output(): robot_model=RobotModelRBDL() r=RobotModelConfig() - r.file="../../../models/kuka/urdf/kuka_iiwa.urdf" + r.file_or_string="../../../models/kuka/urdf/kuka_iiwa.urdf" r.actuated_joint_names = ["kuka_lbr_l_joint_1", "kuka_lbr_l_joint_2", "kuka_lbr_l_joint_3", "kuka_lbr_l_joint_4", "kuka_lbr_l_joint_5", "kuka_lbr_l_joint_6", "kuka_lbr_l_joint_7"] r.joint_names = r.actuated_joint_names assert robot_model.configure(r) == True diff --git a/bindings/python/wbc_types_conversions.h b/bindings/python/wbc_types_conversions.h index 683eddd9..60d68cf8 100644 --- a/bindings/python/wbc_types_conversions.h +++ b/bindings/python/wbc_types_conversions.h @@ -34,7 +34,7 @@ wbc::RobotModelConfig toRobotModelConfig(RobotModelConfig cfg_in){ RobotModelConfig fromRobotModelConfig(wbc::RobotModelConfig cfg_in){ RobotModelConfig cfg; - cfg.file = cfg_in.file; + cfg.file_or_string = cfg_in.file_or_string; cfg.submechanism_file = cfg_in.submechanism_file; cfg.floating_base = cfg_in.floating_base; cfg.contact_points = cfg_in.contact_points; diff --git a/src/core/RobotModel.cpp b/src/core/RobotModel.cpp index c954a44c..0e9faffa 100644 --- a/src/core/RobotModel.cpp +++ b/src/core/RobotModel.cpp @@ -2,6 +2,8 @@ #include #include #include +#include +#include namespace wbc{ @@ -90,6 +92,14 @@ const base::samples::Joints& RobotModel::jointState(const std::vector RobotModelPtr; diff --git a/src/core/RobotModelConfig.hpp b/src/core/RobotModelConfig.hpp index 3f0b5f86..2a83eaa4 100644 --- a/src/core/RobotModelConfig.hpp +++ b/src/core/RobotModelConfig.hpp @@ -35,11 +35,11 @@ struct RobotModelConfig{ floating_base = false; type = "rbdl"; } - RobotModelConfig(const std::string& file, + RobotModelConfig(const std::string& file_or_string, const bool floating_base = false, const ActiveContacts &contact_points = ActiveContacts(), const std::string& submechanism_file = "") : - file(file), + file_or_string(file_or_string), submechanism_file(submechanism_file), type("rbdl"), floating_base(floating_base), @@ -47,16 +47,16 @@ struct RobotModelConfig{ } void validate(){ - if(file.empty()) - throw std::runtime_error("Invalid Robot model config. File path must not be empty!"); + if(file_or_string.empty()) + throw std::runtime_error("Invalid Robot model config. File path or string must not be empty!"); if(type == "hyrodyn" && submechanism_file.empty()) throw std::runtime_error("Invalid Robot model config. If you choose 'hyrodyn' as type, submechanism_file must not be empty!"); if(floating_base && contact_points.empty()) throw std::runtime_error("Invalid Robot model config. If floating_base is set to true, contact_points must not be empty!"); } - /** Absolute path to URDF file describing the robot model.*/ - std::string file; + /** Absolute path to URDF file describing the robot model or URDF string.*/ + 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*/ diff --git a/src/robot_models/hyrodyn/RobotModelHyrodyn.cpp b/src/robot_models/hyrodyn/RobotModelHyrodyn.cpp index 4427da66..21e7bdc5 100644 --- a/src/robot_models/hyrodyn/RobotModelHyrodyn.cpp +++ b/src/robot_models/hyrodyn/RobotModelHyrodyn.cpp @@ -27,9 +27,9 @@ bool RobotModelHyrodyn::configure(const RobotModelConfig& cfg){ // 1. Load Robot Model robot_model_config = cfg; - robot_urdf = urdf::parseURDFFile(cfg.file); + robot_urdf = loadRobotURDF(cfg.file_or_string); if(!robot_urdf){ - LOG_ERROR("Unable to parse urdf model from file %s", cfg.file.c_str()); + LOG_ERROR("Unable to parse urdf model"); return false; } base_frame = robot_urdf->getRoot()->name; diff --git a/src/robot_models/kdl/RobotModelKDL.cpp b/src/robot_models/kdl/RobotModelKDL.cpp index 4553d3ae..83f66fef 100644 --- a/src/robot_models/kdl/RobotModelKDL.cpp +++ b/src/robot_models/kdl/RobotModelKDL.cpp @@ -33,9 +33,9 @@ bool RobotModelKDL::configure(const RobotModelConfig& cfg){ // 1. Load Robot Model robot_model_config = cfg; - robot_urdf = urdf::parseURDFFile(cfg.file); + robot_urdf = loadRobotURDF(cfg.file_or_string); if(!robot_urdf){ - LOG_ERROR("Unable to parse urdf model from file %s", cfg.file.c_str()); + LOG_ERROR("Unable to parse urdf model") return false; } base_frame = robot_urdf->getRoot()->name; @@ -56,7 +56,7 @@ bool RobotModelKDL::configure(const RobotModelConfig& cfg){ // Parse KDL Tree if(!kdl_parser::treeFromUrdfModel(*robot_urdf, full_tree)){ - LOG_ERROR("Unable to load KDL Tree from file %s", cfg.file.c_str()); + LOG_ERROR("Unable to load KDL Tree"); return false; } diff --git a/src/robot_models/pinocchio/RobotModelPinocchio.cpp b/src/robot_models/pinocchio/RobotModelPinocchio.cpp index d5cc61b4..948751bf 100644 --- a/src/robot_models/pinocchio/RobotModelPinocchio.cpp +++ b/src/robot_models/pinocchio/RobotModelPinocchio.cpp @@ -34,9 +34,9 @@ bool RobotModelPinocchio::configure(const RobotModelConfig& cfg){ // 1. Load Robot Model robot_model_config = cfg; - robot_urdf = urdf::parseURDFFile(cfg.file); + robot_urdf = loadRobotURDF(cfg.file_or_string); if(!robot_urdf){ - LOG_ERROR("Unable to parse urdf model from file %s", cfg.file.c_str()); + LOG_ERROR("Unable to parse urdf model"); return false; } base_frame = robot_urdf->getRoot()->name; @@ -50,7 +50,7 @@ bool RobotModelPinocchio::configure(const RobotModelConfig& cfg){ } } catch(std::invalid_argument e){ - LOG_ERROR_S << "RobotModelPinocchio: Failed to load urdf model from " << cfg.file <(model); diff --git a/src/robot_models/rbdl/RobotModelRBDL.cpp b/src/robot_models/rbdl/RobotModelRBDL.cpp index 7e2774dd..4f1014aa 100644 --- a/src/robot_models/rbdl/RobotModelRBDL.cpp +++ b/src/robot_models/rbdl/RobotModelRBDL.cpp @@ -34,9 +34,9 @@ bool RobotModelRBDL::configure(const RobotModelConfig& cfg){ // 1. Load Robot Model robot_model_config = cfg; - robot_urdf = urdf::parseURDFFile(cfg.file); + robot_urdf = loadRobotURDF(cfg.file_or_string); if(!robot_urdf){ - LOG_ERROR("Unable to parse urdf model from file %s", cfg.file.c_str()); + LOG_ERROR("Unable to parse robot urdf model"); return false; } base_frame = robot_urdf->getRoot()->name; @@ -54,7 +54,7 @@ bool RobotModelRBDL::configure(const RobotModelConfig& cfg){ doc->SaveFile(robot_urdf_file); if(!Addons::URDFReadFromFile(robot_urdf_file.c_str(), rbdl_model.get(), cfg.floating_base)){ - LOG_ERROR_S << "Unable to parse urdf from file " << cfg.file << std::endl; + LOG_ERROR_S << "Unable to parse urdf from file " << robot_urdf_file << std::endl; return false; } diff --git a/test/scenes/test_acceleration_scene.cpp b/test/scenes/test_acceleration_scene.cpp index a4667c04..fce77212 100644 --- a/test/scenes/test_acceleration_scene.cpp +++ b/test/scenes/test_acceleration_scene.cpp @@ -15,7 +15,7 @@ BOOST_AUTO_TEST_CASE(simple_test){ // Configure Robot model shared_ptr robot_model = make_shared(); RobotModelConfig config; - config.file = "../../../models/kuka/urdf/kuka_iiwa.urdf"; + config.file_or_string = "../../../models/kuka/urdf/kuka_iiwa.urdf"; BOOST_CHECK_EQUAL(robot_model->configure(config), true); base::samples::Joints joint_state; diff --git a/test/scenes/test_acceleration_scene_reduced_tsid.cpp b/test/scenes/test_acceleration_scene_reduced_tsid.cpp index e260364a..c5e7f153 100644 --- a/test/scenes/test_acceleration_scene_reduced_tsid.cpp +++ b/test/scenes/test_acceleration_scene_reduced_tsid.cpp @@ -15,7 +15,7 @@ BOOST_AUTO_TEST_CASE(simple_test){ // Configure Robot model shared_ptr robot_model = make_shared(); RobotModelConfig config; - config.file = "../../../models/rh5/urdf/rh5_legs.urdf"; + config.file_or_string = "../../../models/rh5/urdf/rh5_legs.urdf"; config.floating_base = true; config.contact_points.names = {"FL_SupportCenter", "FR_SupportCenter"}; wbc::ActiveContact contact(1,0.6); @@ -89,7 +89,7 @@ BOOST_AUTO_TEST_CASE(simple_test){ BOOST_CHECK(fabs(status[0].y_ref[i] - status[0].y_solution[i]) < 1e-3); BOOST_CHECK(fabs(status[0].y_ref[i+3] - status[0].y_solution[i]) < 1e3); } - + // Check if torques respect equation of motions uint nj = robot_model->noOfJoints(); uint na = robot_model->noOfActuatedJoints(); @@ -103,7 +103,7 @@ BOOST_AUTO_TEST_CASE(simple_test){ for(uint i = 0; i < na; i++){ const std::string& name = robot_model->actuatedJointNames()[i]; uint idx = robot_model->jointIndex(name); - tau(idx-6) = solver_output[name].effort; + tau(idx-6) = solver_output[name].effort; } const auto& contacts = robot_model->getActiveContacts(); @@ -113,6 +113,6 @@ BOOST_AUTO_TEST_CASE(simple_test){ base::VectorXd eq_motion_right = robot_model->selectionMatrix().transpose() * tau; for(uint i=0; i < contacts.size(); ++i) eq_motion_right += robot_model->bodyJacobian(robot_model->worldFrame(), contacts.names[i]).transpose() * f_ext.segment<6>(i*6); - + BOOST_CHECK((eq_motion_left - eq_motion_right).cwiseAbs().maxCoeff() < 1e-3); } diff --git a/test/scenes/test_acceleration_scene_tsid.cpp b/test/scenes/test_acceleration_scene_tsid.cpp index 8422fb3d..b6b57c31 100644 --- a/test/scenes/test_acceleration_scene_tsid.cpp +++ b/test/scenes/test_acceleration_scene_tsid.cpp @@ -15,7 +15,7 @@ BOOST_AUTO_TEST_CASE(simple_test){ // Configure Robot model shared_ptr robot_model = make_shared(); RobotModelConfig config; - config.file = "../../../models/rh5/urdf/rh5_legs.urdf"; + config.file_or_string = "../../../models/rh5/urdf/rh5_legs.urdf"; config.floating_base = true; config.contact_points.names = {"FL_SupportCenter", "FR_SupportCenter"}; wbc::ActiveContact contact(1,0.6); diff --git a/test/scenes/test_velocity_scene.cpp b/test/scenes/test_velocity_scene.cpp index 1abaa28d..8cf2ba72 100644 --- a/test/scenes/test_velocity_scene.cpp +++ b/test/scenes/test_velocity_scene.cpp @@ -16,7 +16,7 @@ BOOST_AUTO_TEST_CASE(configuration_test){ shared_ptr robot_model = make_shared(); RobotModelConfig config; - config.file = "../../../models/kuka/urdf/kuka_iiwa.urdf"; + config.file_or_string = "../../../models/kuka/urdf/kuka_iiwa.urdf"; BOOST_CHECK_EQUAL(robot_model->configure(config), true); QPSolverPtr solver = std::make_shared(); VelocityScene wbc_scene(robot_model, solver, 1e-3); @@ -47,7 +47,7 @@ BOOST_AUTO_TEST_CASE(simple_test){ // Configure Robot model shared_ptr robot_model = make_shared(); RobotModelConfig config; - config.file = "../../../models/kuka/urdf/kuka_iiwa.urdf"; + config.file_or_string = "../../../models/kuka/urdf/kuka_iiwa.urdf"; BOOST_CHECK(robot_model->configure(config)); base::samples::Joints joint_state; diff --git a/test/scenes/test_velocity_scene_quadratic_cost.cpp b/test/scenes/test_velocity_scene_quadratic_cost.cpp index 8a88452f..56749013 100644 --- a/test/scenes/test_velocity_scene_quadratic_cost.cpp +++ b/test/scenes/test_velocity_scene_quadratic_cost.cpp @@ -15,7 +15,7 @@ BOOST_AUTO_TEST_CASE(simple_test){ // Configure Robot model shared_ptr robot_model = make_shared(); RobotModelConfig config; - config.file = "../../../models/rh5/urdf/rh5_legs.urdf"; + config.file_or_string = "../../../models/rh5/urdf/rh5_legs.urdf"; config.floating_base = true; config.contact_points.names = {"FL_SupportCenter", "FR_SupportCenter"}; config.contact_points.elements = {ActiveContact(1,0.6),ActiveContact(1,0.6)}; diff --git a/tutorials/kuka_iiwa/cart_pos_ctrl_hls.cpp b/tutorials/kuka_iiwa/cart_pos_ctrl_hls.cpp index ca658c15..df0e4967 100644 --- a/tutorials/kuka_iiwa/cart_pos_ctrl_hls.cpp +++ b/tutorials/kuka_iiwa/cart_pos_ctrl_hls.cpp @@ -46,7 +46,7 @@ int main(){ // - The order of joints used inside WBC is the same as the one obtained by the URDF parser (which is alphabetic) // For all configuration options, check core/RobotModelConfig.hpp RobotModelConfig config; - config.file = "../../../models/kuka/urdf/kuka_iiwa.urdf"; + config.file_or_string = "../../../models/kuka/urdf/kuka_iiwa.urdf"; if(!robot_model->configure(config)) return -1; diff --git a/tutorials/kuka_iiwa/cart_pos_ctrl_hls_hierarchies.cpp b/tutorials/kuka_iiwa/cart_pos_ctrl_hls_hierarchies.cpp index c0480a3b..e86005ed 100644 --- a/tutorials/kuka_iiwa/cart_pos_ctrl_hls_hierarchies.cpp +++ b/tutorials/kuka_iiwa/cart_pos_ctrl_hls_hierarchies.cpp @@ -24,7 +24,7 @@ int main(int argc, char** argv){ // - The order of joints used inside WBC is the same as the one obtained by the URDF parser (which is alphabetic) // For all configuration options, check core/RobotModelConfig.hpp RobotModelConfig config; - config.file = "../../../models/kuka/urdf/kuka_iiwa.urdf"; + config.file_or_string = "../../../models/kuka/urdf/kuka_iiwa.urdf"; if(!robot_model->configure(config)) return -1; diff --git a/tutorials/kuka_iiwa/cart_pos_ctrl_hls_weights.cpp b/tutorials/kuka_iiwa/cart_pos_ctrl_hls_weights.cpp index ddfd1ec7..a11f1fc6 100644 --- a/tutorials/kuka_iiwa/cart_pos_ctrl_hls_weights.cpp +++ b/tutorials/kuka_iiwa/cart_pos_ctrl_hls_weights.cpp @@ -23,7 +23,7 @@ int main(){ // - The order of joints used inside WBC is the same as the one obtained by the URDF parser (which is alphabetic) // For all configuration options, check core/RobotModelConfig.hpp RobotModelConfig config; - config.file = "../../../models/kuka/urdf/kuka_iiwa.urdf"; + config.file_or_string = "../../../models/kuka/urdf/kuka_iiwa.urdf"; if(!robot_model->configure(config)) return -1; diff --git a/tutorials/kuka_iiwa/cart_pos_ctrl_qpoases.cpp b/tutorials/kuka_iiwa/cart_pos_ctrl_qpoases.cpp index 1ac34a0c..813b03f9 100644 --- a/tutorials/kuka_iiwa/cart_pos_ctrl_qpoases.cpp +++ b/tutorials/kuka_iiwa/cart_pos_ctrl_qpoases.cpp @@ -48,7 +48,7 @@ int main(){ // - The order of joints used inside WBC is the same as the one obtained by the URDF parser (which is alphabetic) // For all configuration options, check core/RobotModelConfig.hpp RobotModelConfig config; - config.file = "../../../models/kuka/urdf/kuka_iiwa.urdf"; + config.file_or_string = "../../../models/kuka/urdf/kuka_iiwa.urdf"; if(!robot_model->configure(config)) return -1; diff --git a/tutorials/rh5/rh5_legs_floating_base.cpp b/tutorials/rh5/rh5_legs_floating_base.cpp index 0b3adbfa..b0bf0a50 100644 --- a/tutorials/rh5/rh5_legs_floating_base.cpp +++ b/tutorials/rh5/rh5_legs_floating_base.cpp @@ -57,7 +57,7 @@ int main(){ floating_base_state.acceleration.setZero(); floating_base_state.time = base::Time::now(); RobotModelConfig config; - config.file = "../../../models/rh5/urdf/rh5_legs.urdf"; + config.file_or_string = "../../../models/rh5/urdf/rh5_legs.urdf"; config.floating_base = true; config.contact_points.names = {"LLAnkle_FT", "LRAnkle_FT"}; config.contact_points.elements = {wbc::ActiveContact(1,0.6),wbc::ActiveContact(1,0.6)}; diff --git a/tutorials/rh5/rh5_single_leg.cpp b/tutorials/rh5/rh5_single_leg.cpp index cbb12bff..92159f6c 100644 --- a/tutorials/rh5/rh5_single_leg.cpp +++ b/tutorials/rh5/rh5_single_leg.cpp @@ -46,7 +46,7 @@ int main(){ // Also, we have to pass a submechanism files, which describes the parallel structures inside the robot architecture // In this case, there are not parallel structure, since we use the abstract model RobotModelConfig config; - config.file = "../../../models/rh5/urdf/rh5_single_leg.urdf"; + config.file_or_string = "../../../models/rh5/urdf/rh5_single_leg.urdf"; config.submechanism_file = "../../../models/rh5/hyrodyn/rh5_single_leg.yml"; if(!robot_model->configure(config)) return -1; diff --git a/tutorials/rh5/rh5_single_leg_hybrid.cpp b/tutorials/rh5/rh5_single_leg_hybrid.cpp index 929a85e6..dab5514c 100644 --- a/tutorials/rh5/rh5_single_leg_hybrid.cpp +++ b/tutorials/rh5/rh5_single_leg_hybrid.cpp @@ -27,7 +27,7 @@ int main(){ // Configure the full robot model, containing all linear actuators and parallel mechanisms. RobotModelConfig config; - config.file = "../../../models/rh5/urdf/rh5_single_leg_hybrid.urdf"; + config.file_or_string = "../../../models/rh5/urdf/rh5_single_leg_hybrid.urdf"; config.submechanism_file = "../../../models/rh5/hyrodyn/rh5_single_leg_hybrid.yml"; if(!robot_model->configure(config)) return -1;