Commits

Martin Felis  committed 937bcd3

Adjusted rbdl_luamodel_util and rbdl_urdfreader_util to have the same features.

  • Participants
  • Parent commits 83a1aee

Comments (0)

Files changed (5)

File addons/urdfreader/CMakeLists.txt

 
 ADD_LIBRARY ( rbdl_urdfreader SHARED ${URDFREADER_SOURCES} )
 
-ADD_EXECUTABLE (rbdl_urdf_test rbdl_urdf_test.cc)
+ADD_EXECUTABLE (rbdl_urdfreader_util rbdl_urdfreader_util.cc)
 
 ROSPACK_USE_DEPENDENCY (rbdl_urdfreader urdf)
 ROSPACK_USE_DEPENDENCY (rbdl_urdfreader urdf_interface)
 
-ROSPACK_USE_DEPENDENCY (rbdl_urdf_test urdf)
-ROSPACK_USE_DEPENDENCY (rbdl_urdf_test urdf_interface)
+ROSPACK_USE_DEPENDENCY (rbdl_urdfreader_util urdf)
+ROSPACK_USE_DEPENDENCY (rbdl_urdfreader_util urdf_interface)
 
 IF (BUILD_STATIC)
 	ADD_LIBRARY ( rbdl_urdfreader-static STATIC ${URDFREADER_SOURCES} )
 	${URDF_LIBRARY}
 	)
 
-TARGET_LINK_LIBRARIES (rbdl_urdf_test
+TARGET_LINK_LIBRARIES (rbdl_urdfreader_util
 	rbdl_urdfreader
 	)
 

File addons/urdfreader/rbdl_urdf_test.cc

-#include "rbdl.h"
-#include "rbdl_urdfreader.h"
-
-#include <iostream>
-
-using namespace std;
-
-bool verbose = false;
-string filename = "";
-
-int main (int argc, char *argv[]) {
-	if (argc < 2) {
-		cerr << "Usage: " << argv[0] << "[-v] <robot.urdf>" << endl;
-		return -1;
-	}
-
-	for (int i = 1; i < argc; i++) {
-		string arg(argv[i]);
-		if (arg == "-v")
-			verbose = true;
-		else
-			filename = arg;
-	}
-
-	RigidBodyDynamics::Model model;
-	if (!RigidBodyDynamics::Addons::read_urdf_model(filename.c_str(), &model, verbose)) {
-		cerr << "Loading of urdf model failed!" << endl;
-		return -1;
-	}
-
-	return 0;
-}

File addons/urdfreader/rbdl_urdfreader.cc

 bool read_urdf_model (const char* filename, Model* model, bool verbose) {
 	assert (model);
 
-	model->Init();
-
 	urdf::Model urdf_model;
 
 	bool urdf_result = urdf_model.initFile (filename);
 
 	model->gravity.set (0., 0., -9.81);
 
-	cout << "Model loading succcessful!" << endl;
-
 	return true;
 }
 

File addons/urdfreader/rbdl_urdfreader_util.cc

+#include "rbdl.h"
+#include "rbdl_utils.h"
+#include "rbdl_urdfreader.h"
+
+#include <iostream>
+
+using namespace std;
+
+bool verbose = false;
+string filename = "";
+
+void usage (const char* argv_0) {
+	cerr << "Usage: " << argv_0 << "[-v] [-m] [-d] <robot.urdf>" << endl;
+	cerr << "  -v | --verbose            enable additional output" << endl;
+	cerr << "  -d | --dof-overview       print an overview of the degress of freedom" << endl;
+	cerr << "  -m | --model-hierarchy    print the hierarchy of the model" << endl;
+	cerr << "  -h | --help               print this help" << endl;
+	exit (1);
+}
+
+
+int main (int argc, char *argv[]) {
+	if (argc < 2 || argc > 4) {
+		usage(argv[0]);
+	}
+
+	bool verbose = false;
+	bool dof_overview = false;
+	bool model_hierarchy = false;
+
+	string filename = argv[1];
+
+	for (int i = 1; i < argc; i++) {
+		if (string(argv[i]) == "-v" || string (argv[i]) == "--verbose")
+			verbose = true;
+		else if (string(argv[i]) == "-d" || string (argv[i]) == "--dof-overview")
+			dof_overview = true;
+		else if (string(argv[i]) == "-m" || string (argv[i]) == "--model-hierarchy")
+			model_hierarchy = true;
+		else if (string(argv[i]) == "-h" || string (argv[i]) == "--help")
+			usage(argv[0]);
+		else
+			filename = argv[i];
+	}
+
+	RigidBodyDynamics::Model model;
+	model.Init();
+
+	if (!RigidBodyDynamics::Addons::read_urdf_model(filename.c_str(), &model, verbose)) {
+		cerr << "Loading of urdf model failed!" << endl;
+		return -1;
+	}
+
+	cout << "Model loading successful!" << endl;
+
+	if (dof_overview) {
+		cout << "Degree of freedom overview:" << endl;
+		cout << RigidBodyDynamics::Utils::GetModelDOFOverview(model);
+	}
+
+	if (model_hierarchy) {
+		cout << "Model Hierarchy:" << endl;
+		cout << RigidBodyDynamics::Utils::GetModelHierarchy (model);
+	}
+
+	return 0;
+}

File src/rbdl_utils.cc

 	result << " [ ";
 
 	while (model.mBodies[body_index].mMass == 0.) {
-		if (model.mu[body_index].size() != 1) {
-			cerr << endl << "Error: Cannot determine multi-dof joint as massless body with id " << body_index << " has more than 1 child." << endl;
+		if (model.mu[body_index].size() == 0) {
+			result << " end";
+			break;
+		} else if (model.mu[body_index].size() > 1) {
+			cerr << endl << "Error: Cannot determine multi-dof joint as massless body with id " << body_index << " (name: " << model.GetBodyName(body_index) << ") has more than one child:" << endl;
+			for (unsigned int ci = 0; ci < model.mu[body_index].size(); ci++) {
+				cerr << "  id: " << model.mu[body_index][ci] << " name: " << model.GetBodyName(model.mu[body_index][ci]) << endl;
+			}
 			abort();
 		}