Martin  Felis avatar Martin Felis committed a236e4f

[utils] added function that prints out origins of all bodies

Comments (0)

Files changed (3)

addons/luamodel/rbdl_luamodel_util.cc

 	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 << "  -o | --body-origins       print the origins of all bodies that have names" << endl;
 	cerr << "  -h | --help               print this help" << endl;
 	exit (1);
 }
 	bool verbose = false;
 	bool dof_overview = false;
 	bool model_hierarchy = false;
+	bool body_origins = false;
 
 	string filename = argv[1];
 
 			dof_overview = true;
 		else if (string(argv[i]) == "-m" || string (argv[i]) == "--model-hierarchy")
 			model_hierarchy = true;
+		else if (string(argv[i]) == "-o" || string (argv[i]) == "--body-origins")
+			body_origins = true;
 		else if (string(argv[i]) == "-h" || string (argv[i]) == "--help")
 			usage(argv[0]);
 		else
 		cout << RigidBodyDynamics::Utils::GetModelHierarchy (model);
 	}
 
+	if (body_origins) {
+		cout << "Body Origins:" << endl;
+		cout << RigidBodyDynamics::Utils::GetNamedBodyOriginsOverview(model);
+	}
+
 	return 0;
 }

src/rbdl_utils.cc

 
 #include "rbdl_math.h"
 #include "Model.h"
+#include "Kinematics.h"
 
 #include <sstream>
 #include <iomanip>
 	return result.str();
 }
 
+std::string GetNamedBodyOriginsOverview (Model &model) {
+	stringstream result ("");
+
+	VectorNd Q (VectorNd::Zero(model.dof_count));
+	UpdateKinematicsCustom (model, &Q, NULL, NULL);
+
+	for (unsigned int body_id = 0; body_id < model.mBodies.size(); body_id++) {
+		std::string body_name = model.GetBodyName (body_id);
+
+		if (body_name.size() == 0) 
+			continue;
+
+		Vector3d position = CalcBodyToBaseCoordinates (model, Q, body_id, Vector3d (0., 0., 0.), false);
+
+		result << body_name << ": " << position.transpose() << endl;
+	}
+
+	return result.str();
 }
+
 }
+}
 	std::string GetModelHierarchy (const Model &model);
 	/** \brief Creates a human readable overview of the Degrees of Freedom. */
 	std::string GetModelDOFOverview (const Model &model);
-
+	/** \brief Creates a human readable overview of the locations of all bodies that have names. */
+	std::string GetNamedBodyOriginsOverview (Model &model);
 }
 }
 
Tip: Filter by directory path e.g. /media app.js to search for public/media/app.js.
Tip: Use camelCasing e.g. ProjME to search for ProjectModifiedEvent.java.
Tip: Filter by extension type e.g. /repo .js to search for all .js files in the /repo directory.
Tip: Separate your search with spaces e.g. /ssh pom.xml to search for src/ssh/pom.xml.
Tip: Use ↑ and ↓ arrow keys to navigate and return to view the file.
Tip: You can also navigate files with Ctrl+j (next) and Ctrl+k (previous) and view the file with Ctrl+o.
Tip: You can also navigate files with Alt+j (next) and Alt+k (previous) and view the file with Alt+o.