Branch data     Line data    Source code

       1                 :            : // Copyright (c) 2017 Jordan Lack <jlack1987@gmail.com>
       2                 :            : // Copyright (c) 2011-2016 Martin Felis <martin.felis@iwr.uni-heidelberg.de>
       3                 :            : // RDL - Robot Dynamics Library
       4                 :            : // Licensed under the zlib license. See LICENSE for more details.
       5                 :            : 
       6                 :            : #include <rdl_dynamics/Model.hpp>
       7                 :            : #include <rdl_dynamics/rdl_utils.hpp>
       8                 :            : 
       9                 :            : #include <rdl_urdfreader/urdfreader.hpp>
      10                 :            : 
      11                 :            : using namespace std;
      12                 :            : 
      13                 :            : bool verbose = false;
      14                 :            : string filename = "";
      15                 :            : 
      16                 :          0 : void usage(const char* argv_0)
      17                 :            : {
      18                 :          0 :     cerr << "Usage: " << argv_0 << "[-v] [-m] [-d] <robot.urdf>" << endl;
      19                 :          0 :     cerr << "  -v | --verbose            enable additional output" << endl;
      20                 :          0 :     cerr << "  -d | --dof-overview       print an overview of the degress of freedom" << endl;
      21                 :          0 :     cerr << "  -m | --model-hierarchy    print the hierarchy of the model" << endl;
      22                 :          0 :     cerr << "  -h | --help               print this help" << endl;
      23                 :          0 :     exit(1);
      24                 :            : }
      25                 :            : 
      26                 :          0 : int main(int argc, char* argv[])
      27                 :            : {
      28                 :          0 :     if (argc < 2 || argc > 4)
      29                 :            :     {
      30                 :          0 :         usage(argv[0]);
      31                 :            :     }
      32                 :            : 
      33                 :          0 :     bool verbose = false;
      34                 :          0 :     bool dof_overview = false;
      35                 :          0 :     bool model_hierarchy = false;
      36                 :            : 
      37                 :          0 :     string filename = argv[1];
      38                 :            : 
      39                 :          0 :     for (int i = 1; i < argc; ++i)
      40                 :            :     {
      41                 :          0 :         if (string(argv[i]) == "-v" || string(argv[i]) == "--verbose")
      42                 :            :         {
      43                 :          0 :             verbose = true;
      44                 :            :         }
      45                 :          0 :         else if (string(argv[i]) == "-d" || string(argv[i]) == "--dof-overview")
      46                 :            :         {
      47                 :          0 :             dof_overview = true;
      48                 :            :         }
      49                 :          0 :         else if (string(argv[i]) == "-m" || string(argv[i]) == "--model-hierarchy")
      50                 :            :         {
      51                 :          0 :             model_hierarchy = true;
      52                 :            :         }
      53                 :          0 :         else if (string(argv[i]) == "-h" || string(argv[i]) == "--help")
      54                 :            :         {
      55                 :          0 :             usage(argv[0]);
      56                 :            :         }
      57                 :            :         else
      58                 :            :         {
      59                 :          0 :             filename = argv[i];
      60                 :            :         }
      61                 :            :     }
      62                 :            : 
      63                 :          0 :     RobotDynamics::ModelPtr model(new RobotDynamics::Model());
      64                 :            : 
      65                 :          0 :     if (!RobotDynamics::Urdf::urdfReadFromFile(filename.c_str(), model, RobotDynamics::JointTypeFloatingBase, verbose))
      66                 :            :     {
      67                 :          0 :         cerr << "Loading of urdf model failed!" << endl;
      68                 :          0 :         return -1;
      69                 :            :     }
      70                 :            : 
      71                 :          0 :     cout << "Model loading successful!" << endl;
      72                 :            : 
      73                 :          0 :     if (dof_overview)
      74                 :            :     {
      75                 :          0 :         cout << "Degree of freedom overview:" << endl;
      76                 :          0 :         cout << RobotDynamics::Utils::getModelDOFOverview(*model);
      77                 :            :     }
      78                 :            : 
      79                 :          0 :     if (model_hierarchy)
      80                 :            :     {
      81                 :          0 :         cout << "Model Hierarchy:" << endl;
      82                 :          0 :         cout << RobotDynamics::Utils::getModelHierarchy(*model);
      83                 :            :     }
      84                 :            : 
      85                 :          0 :     return 0;
      86                 :          0 : }