Merge pull request #82 from dmronga/load_from_string

Allow loading robot URDF from file or string

benchmarks/robot_models/benchmark_robot_models.cpp

@@ -129,11 +129,11 @@ map<string,base::VectorXd> 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<RobotModelKDL>();
@@ -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<RobotModelKDL>();
@@ -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<RobotModelKDL>();
@@ -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();
 

benchmarks/scenes/benchmark_scenes.cpp

@@ -76,7 +76,7 @@ void evaluateAccelerationSceneReducedTSID(map<string,RobotModelPtr> 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<RobotModelHyrodyn>();
-    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<RobotModelHyrodyn>();
-    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<RobotModelHyrodyn>();
-    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<RobotModelHyrodyn>();
-    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){

benchmarks/solvers/benchmark_solvers.cpp

@@ -89,7 +89,7 @@ map<string, base::VectorXd> 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<RobotModelPinocchio>();
     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<RobotModelPinocchio>();
     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<RobotModelPinocchio>();
     if(!robot_model->configure(cfg))abort();
 

bindings/python/core/core.cpp

@@ -120,7 +120,7 @@ BOOST_PYTHON_MODULE(core){
                 py::make_setter(&base::NamedVector<base::JointLimitRange>::elements));
 
    py::class_<wbc_py::RobotModelConfig>("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::copy_non_const_reference>()),
                py::make_setter(&wbc::TasksStatus::elements));
 }
-

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
 

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

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;

src/core/RobotModel.cpp

@@ -2,6 +2,8 @@
 #include <base-logging/Logging.hpp>
 #include <base/samples/RigidBodyStateSE3.hpp>
 #include <base/samples/Joints.hpp>
+#include <fstream>
+#include <urdf_parser/urdf_parser.h>
 
 namespace wbc{
 
@@ -90,6 +92,14 @@ const base::samples::Joints& RobotModel::jointState(const std::vector<std::strin
     return joint_state_out;
 }
 
+urdf::ModelInterfaceSharedPtr RobotModel::loadRobotURDF(const std::string& file_or_string){
+    std::ifstream fs(file_or_string.c_str());
+    if(fs)
+        return urdf::parseURDFFile(file_or_string);
+    else
+        return urdf::parseURDF(file_or_string);
+}
+
 RobotModelFactory::RobotModelMap* RobotModelFactory::robot_model_map = 0;
 
 } // namespace wbc

src/core/RobotModel.hpp

@@ -198,6 +198,9 @@ class RobotModel{
     /** @brief Is floating base robot?*/
     bool hasFloatingBase(){return has_floating_base;}
 
+    /** @brief Load URDF model from either file or string*/
+    urdf::ModelInterfaceSharedPtr loadRobotURDF(const std::string& file_or_string);
+
 };
 typedef std::shared_ptr<RobotModel> RobotModelPtr;
 

src/core/RobotModelConfig.hpp

@@ -35,28 +35,28 @@ 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),
         contact_points(contact_points){
 
     }
     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*/

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;

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;
     }
 

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 <<std::endl;
+        LOG_ERROR_S << "RobotModelPinocchio: Failed to load urdf model"<<std::endl;
         return false;
     }
     data = std::make_shared<pinocchio::Data>(model);