Commits

Martin Felis committed 83a1aee

added utility functions GetModelHierarchy() and GetModelDOFOverview()

Comments (0)

Files changed (5)

 SET ( RBDL_SOURCES 
 	src/rbdl_version.cc
 	src/rbdl_mathutils.cc
+	src/rbdl_utils.cc
 	src/Logging.cc
 	src/Joint.cc
 	src/Model.cc

addons/luamodel/rbdl_luamodel_util.cc

 #include "rbdl.h"
+#include "rbdl_utils.h"
 #include "luamodel.h"
 
 #include <iostream>
 
 using namespace RigidBodyDynamics::Math;
 
-string get_body_name (const RigidBodyDynamics::Model &model, unsigned int body_id) {
-	if (model.mBodies[body_id].mMass == 0.) {
-		// this seems to be a virtual body that was added by a multi dof joint
-		unsigned int child_index = 0;
-
-		// if there is not a unique child we do not know what to do...
-		if (model.mu[body_id].size() != 1)
-			return "";
-
-		unsigned int child_id = model.mu[body_id][0];
-
-		return get_body_name (model, model.mu[body_id][0]);
-	}
-
-	return model.GetBodyName(body_id);
-}
-
-string get_dof_name (const SpatialVector &joint_dof) {
-	if (joint_dof == SpatialVector (1., 0., 0., 0., 0., 0.)) 
-		return "RX";
-	else if (joint_dof == SpatialVector (0., 1., 0., 0., 0., 0.))
-		return "RY";
-	else if (joint_dof == SpatialVector (0., 0., 1., 0., 0., 0.))
-		return "RZ";
-	else if (joint_dof == SpatialVector (0., 0., 0., 1., 0., 0.))
-		return "TX";
-	else if (joint_dof == SpatialVector (0., 0., 0., 0., 1., 0.))
-		return "TY";
-	else if (joint_dof == SpatialVector (0., 0., 0., 0., 0., 1.))
-		return "TZ";
-
-	ostringstream dof_stream(ostringstream::out);
-	dof_stream << "custom (" << joint_dof.transpose() << ")";
-	return dof_stream.str();
-}
-
-void print_dof_overview (const RigidBodyDynamics::Model &model) {
-	cout << "Degree of freedom overview:" << endl;
-	for (unsigned int i = 1; i < model.mBodies.size(); i++) {
-		cout << setfill(' ') << setw(3) << i - 1 << ": " << get_body_name (model, i) << "_" << get_dof_name (model.S[i]) << endl;
-	}
-}
-
-void print_hierarchy (const RigidBodyDynamics::Model &model, unsigned int body_index = 0, int indent = 0) {
-	for (int j = 0; j < indent; j++)
-		cout << "  ";
-
-	cout << get_body_name (model, body_index);
-
-	if (body_index == 0) {
-		cout << endl;
-		print_hierarchy (model, 1, 1);
-		return;
-	} 
-
-	// print the dofs
-	cout << " [ ";
-
-	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;
-			abort();
-		}
-
-		cout << get_dof_name(model.S[body_index]) << ", ";
-
-		body_index = model.mu[body_index][0];
-	}
-	cout << get_dof_name(model.S[body_index]);
-
-	cout << " ]" << endl;
-
-	// print fixed children
-	for (unsigned int fbody_index = 0; fbody_index < model.mFixedBodies.size(); fbody_index++) {
-		if (model.mFixedBodies[fbody_index].mMovableParent == body_index) {
-			for (int j = 0; j < indent; j++)
-				cout << "  ";
-
-			cout << model.GetBodyName(model.fixed_body_discriminator + fbody_index) << " [fixed]" << endl;
-		}
-	}
-
-	unsigned int child_index = 0;
-	for (child_index = 0; child_index < model.mu[body_index].size(); child_index++) {
-		print_hierarchy (model, model.mu[body_index][child_index], indent + 1);
-	}
-}
-
 void usage (const char* argv_0) {
 	cerr << "Usage: " << argv_0 << "[-v] [-m] [-d] <model.lua>" << endl;
 	cerr << "  -v | --verbose            enable additional output" << endl;
 
 	cout << "Model loading successful!" << endl;
 
-	if (dof_overview)
-		print_dof_overview(model);
-	if (model_hierarchy)
-		print_hierarchy(model);
+	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;
 }

addons/luamodel/samplemodel.lua

 	frames = {
 		{
 			name = "pelvis",
-			parent_frame = "BASE",
+			parent = "ROOT",
 			body = bodies.pelvis,
 			joint = joints.freeflyer,
 		},
 		{
 			name = "thigh_right",
-			parent_frame = "pelvis",
+			parent = "pelvis",
 			body = bodies.thigh_right,
 			joint = joints.spherical_zyx,
 		},
 		{
+			name = "shank_right",
+			parent = "thigh_right",
+			body = bodies.thigh_right,
+			joint = joints.rotational_y
+		},
+		{
 			name = "foot_right",
-			parent_frame = "thigh_right",
+			parent = "shank_right",
 			body = bodies.thigh_right,
 			joint = joints.fixed
 		},
 		{
-			name = "foot_left",
-			parent_frame = "thigh_right",
-			body = bodies.thigh_right,
-		},
-		{
 			name = "thigh_left",
-			parent_frame = "pelvis",
+			parent = "pelvis",
 			body = bodies.thigh_left,
 			joint = joints.spherical_zyx
 		},
 		{
 			name = "shank_left",
-			parent_frame = "thigh_left",
+			parent = "thigh_left",
 			body = bodies.thigh_left,
 			joint = joints.rotational_y
 		},
 		{
 			name = "foot_left",
-			parent_frame = "thigh_left",
+			parent = "shank_left",
 			body = bodies.thigh_left,
 			joint = joints.fixed
 		},

src/rbdl_utils.cc

+#include "rbdl_utils.h"
+
+#include "rbdl_math.h"
+#include "Model.h"
+
+#include <sstream>
+#include <iomanip>
+
+namespace RigidBodyDynamics {
+
+namespace Utils {
+
+using namespace std;
+using namespace Math;
+
+string get_dof_name (const SpatialVector &joint_dof) {
+	if (joint_dof == SpatialVector (1., 0., 0., 0., 0., 0.)) 
+		return "RX";
+	else if (joint_dof == SpatialVector (0., 1., 0., 0., 0., 0.))
+		return "RY";
+	else if (joint_dof == SpatialVector (0., 0., 1., 0., 0., 0.))
+		return "RZ";
+	else if (joint_dof == SpatialVector (0., 0., 0., 1., 0., 0.))
+		return "TX";
+	else if (joint_dof == SpatialVector (0., 0., 0., 0., 1., 0.))
+		return "TY";
+	else if (joint_dof == SpatialVector (0., 0., 0., 0., 0., 1.))
+		return "TZ";
+
+	ostringstream dof_stream(ostringstream::out);
+	dof_stream << "custom (" << joint_dof.transpose() << ")";
+	return dof_stream.str();
+}
+
+string get_body_name (const RigidBodyDynamics::Model &model, unsigned int body_id) {
+	if (model.mBodies[body_id].mMass == 0.) {
+		// this seems to be a virtual body that was added by a multi dof joint
+		unsigned int child_index = 0;
+
+		// if there is not a unique child we do not know what to do...
+		if (model.mu[body_id].size() != 1)
+			return "";
+
+		unsigned int child_id = model.mu[body_id][0];
+
+		return get_body_name (model, model.mu[body_id][0]);
+	}
+
+	return model.GetBodyName(body_id);
+}
+
+std::string GetModelDOFOverview (const Model &model) {
+	stringstream result ("");
+
+	for (unsigned int i = 1; i < model.mBodies.size(); i++) {
+		result << setfill(' ') << setw(3) << i - 1 << ": " << get_body_name(model, i) << "_" << get_dof_name (model.S[i]) << endl;
+	}
+
+	return result.str();
+}
+
+std::string print_hierarchy (const RigidBodyDynamics::Model &model, unsigned int body_index = 0, int indent = 0) {
+	stringstream result ("");
+
+	for (int j = 0; j < indent; j++)
+		result << "  ";
+
+	result << get_body_name (model, body_index);
+
+	if (body_index == 0) {
+		result << endl;
+		result << print_hierarchy (model, 1, 1);
+		return result.str();
+	} 
+
+	// print the dofs
+	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;
+			abort();
+		}
+
+		result << get_dof_name(model.S[body_index]) << ", ";
+
+		body_index = model.mu[body_index][0];
+	}
+	result << get_dof_name(model.S[body_index]);
+
+	result << " ]" << endl;
+
+	unsigned int child_index = 0;
+	for (child_index = 0; child_index < model.mu[body_index].size(); child_index++) {
+		result << print_hierarchy (model, model.mu[body_index][child_index], indent + 1);
+	}
+
+	// print fixed children
+	for (unsigned int fbody_index = 0; fbody_index < model.mFixedBodies.size(); fbody_index++) {
+		if (model.mFixedBodies[fbody_index].mMovableParent == body_index) {
+			for (int j = 0; j < indent + 1; j++)
+				result << "  ";
+
+			result << model.GetBodyName(model.fixed_body_discriminator + fbody_index) << " [fixed]" << endl;
+		}
+	}
+
+
+	return result.str();
+}
+
+std::string GetModelHierarchy (const Model &model) {
+	stringstream result ("");
+
+	result << print_hierarchy (model);
+
+	return result.str();
+}
+
+}
+}
+#ifndef _RBDL_UTILS_H
+#define _RBDL_UTILS_H
+
+#include <string>
+
+namespace RigidBodyDynamics {
+
+struct Model;
+
+/** \brief Namespace that contains optional helper functions */
+namespace Utils {
+
+	/** \brief Creates a human readable overview of the model. */
+	std::string GetModelHierarchy (const Model &model);
+	/** \brief Creates a human readable overview of the Degrees of Freedom. */
+	std::string GetModelDOFOverview (const Model &model);
+
+}
+}
+
+/* _RBDL_UTILS_H */
+#endif