Commits

Martin Felis  committed 571fa79

luatables: added documentation, improved test program

  • Participants
  • Parent commits fde7ac4
  • Branches dev

Comments (0)

Files changed (4)

File addons/luamodel/README

-rbdl_luamodel - load models from Lua files
+rbdl_luamodel - load RBDL models from Lua files
 Copyright (c) 2012 Martin Felis <martin.felis@iwr.uni-heidelberg.de>
 
+DESCRIPTION
+
+rbdl_luamodel allows to load models that are specified as Lua scripts. Lua
+is an open-source light-weight scripting language (http://www.lua.org).
+
+Note: this addon is not even remotely as thoroughly tested as the RBDL
+itself so please use it with some suspicion.
+
+DOCUMENTATION
+
+Models have to be specified as a specially formatted Lua table which must
+be returned by the script, i.e. if the model is specified in the table
+"model = { ... }" the script has to return this when executed. Within the
+returned table, rbdl_luamodel goes through the table "frames" and builds
+the model from the individual Frame Information Tables (see further down
+for more information about those).
+
+A valid file could look like this:
+
+--
+model = { 
+	frames = {
+		{
+			<frame 1 information table>
+		},
+		{
+			<frame 2 information table>
+		}
+	}
+}
+
+return model
+--
+
+Note: the table frames must contain all Frame Information Tables as a list
+and individual tables must not be specified with a key, i.e.
+  frames = {
+	  some_frame = {
+		  ...
+		},
+		{
+		  ..
+		}
+	}
+is not possible!
+
+FRAME INFORMATION TABLE
+
+The Frame Information Table is searched for values needed to call
+Model::AddBody(). The following entries are used by rbdl_luamodel
+(everything else is ignored):
+
+  name (optional, type: string):
+	  Name of the body that is being added.
+
+  parent_body (type: 0 or table of the parent body):
+	  In case of 0 the parent is assumed to be the base coordinate system,
+		otherwise it should be the same table as used for the "child_body"
+		field of the parent frame.
+	
+	child_body (optional, type: table)
+	  Specification of the dynamical parameters of the body. It uses the
+		values (if existing):
+		  mass (scalar value, default 0.),
+			com (3-d vector, default:	(0., 0., 0.))
+			inertia (3x3	matrix, default: identity matrix)
+    to build create a body.
+
+	joint (optional, type: table)
+	  Specifies the type of joint, fixed or up to 6 degrees of freedom. Each
+		entry in the joint table should be a 6-d that defines the motion
+		subspace of a single degree of freedom.
+
+		Default joint type is a fixed joint that attaches the body rigidly to
+		its parent.
+		
+		Examples
+      joint_fixed = {}
+			joint_translate_x = { {0., 0., 0., 1., 0., 0.} }
+			joint_translate_xy = { 
+				{0., 0., 0., 1., 0., 0.},
+				{0., 0., 0., 0., 1., 0.}
+			}
+			joint_rotate_zyx = {
+				{0., 0., 1., 0., 0., 0.},
+				{0., 1., 0., 0., 0., 0.},
+				{1., 0., 0., 0., 0., 0.},
+			}
+			
+	joint_frame (optional, type: table)
+	  Specifies the origin of the joint in the frame of the parent. It uses
+		the values (if existing):
+		  r (3-d vector, default: (0., 0., 0.))
+			E (3x3 matrix, default: identity matrix)
+	  for which r is the translation and E the rotation of the joint frame.
+
+Note: it is highly recommended to define the child body tables outside of
+the Frame Information Table. As the field parent_body has to point to the
+exact same table that was previously used as child_body. Best practise
+would be something like this:
+
+--
+  some_body = { mass = ..., com = ... }
+  other_body = { mass = ..., com = ... }
+
+	model = {
+	  frames = {
+ 		  {
+			  parent_body = 0.
+		 		child_body = some_body
+				...
+			},
+			{
+			  parent_body = some_body
+				child_body = other_body
+				...
+			},
+			...
+		}
+	}
+
+	return model
+--
+
 LICENSING
 
 This code is published under the zlib license, however some parts of the
 Full license text:
 
 -------
-rbdl_luamodel - load models from Lua files
+rbdl_luamodel - load RBDL models from Lua files
 Copyright (c) 2011-2012 Martin Felis <martin.felis@iwr.uni-heidelberg.de>
 
 This software is provided 'as-is', without any express or implied

File addons/luamodel/rbdl_luamodel.cc

 
 using namespace Math;
 
-map<void *, unsigned int> body_table_id_map;
+typedef map<void *, unsigned int> PtrUIntMap;
+PtrUIntMap body_table_id_map;
 
 SpatialVector get_spatial_vector (lua_State *L, const string &path, int index = -1) {
 	SpatialVector result;
 bool read_frame_params (lua_State *L,
 		const string &path,
 		unsigned int &parent_id,
-		SpatialTransform &joint_transform,
+		SpatialTransform &joint_frame,
 		Joint &joint,
 		Body &body,
-		std::string &body_name) {
+		std::string &body_name,
+		bool verbose) {
 
 	void *parent_table = (void*) get_pointer (L, path + ".parent_body");
 	void *body_table = (void*) get_pointer (L, path + ".child_body");
+	body_name = get_string (L, path + ".name");
+
+	PtrUIntMap::iterator parent_iter = body_table_id_map.find (parent_table);
+	if (parent_iter == body_table_id_map.end()) {
+		cerr << "Error: could not find table parent_body for frame '" << body_name << "'!" << endl;
+		return false;
+	}
 	parent_id = body_table_id_map[parent_table];
 
-	cout << "frame name = " << get_string (L, path + ".name") << endl;
-	cout << "  parent_table = " << hex << parent_table << endl;
-	cout << "  parent_id = " << parent_id << endl;
-	cout << "  body_table   = " << hex << body_table << endl;
+	if (verbose) {
+		cout << "frame name = " << body_name << endl;
+		cout << "  parent_table = " << hex << parent_table << endl;
+		cout << "  parent_id = " << parent_id << endl;
+		cout << "  body_table   = " << hex << body_table << endl;
+	}
 
-
-	// create the joint_transform
+	// create the joint_frame
 	if (get_pointer (L, path + ".joint_frame") == NULL) {
-		joint_transform = SpatialTransform();
+		joint_frame = SpatialTransform();
 	} else {
 		Vector3d r (0., 0., 0.);
 		Matrix3d E (Matrix3d::Identity(3,3));
 			E = get_matrix3d (L, path + ".joint_frame.E");
 		}
 
-		joint_transform = SpatialTransform (E, r);
+		joint_frame = SpatialTransform (E, r);
 	}
 
-	cout << "  joint_transform = " << joint_transform << endl;
+	if (verbose)
+		cout << "  joint_frame = " << joint_frame << endl;
 
 	// create the joint
 	if (get_pointer (L, path + ".joint") == NULL) {
-		joint = Joint();
+		joint = Joint(JointTypeFixed);
 	} else {
 		unsigned int joint_dofs = static_cast<unsigned int> (get_length (L, path + ".joint"));
-		cout << "  joint_dofs   = " << joint_dofs << endl;
+
+		if (verbose)
+			cout << "  joint_dofs   = " << joint_dofs << endl;
 
 		switch (joint_dofs) {
 			case 0: joint = Joint(JointTypeFixed);
 		}
 	}
 
-	body_name = "";
 	if (get_pointer (L, path + ".child_body") == NULL) {
 		body = Body();
 	} else {
 			mass = get_number (L, path + ".child_body.mass");
 		}
 
-		cout << "  mass = " << mass << endl;
-
 		if (get_pointer (L, path + ".child_body.com") != NULL) {
 			com = get_vector3d (L, path + ".child_body.com");
 		}
 
-		cout << "  com = " << com << endl;
-
 		if (get_pointer (L, path + ".child_body.inertia") != NULL) {
 			inertia = get_matrix3d (L, path + ".child_body.inertia");
 		}
 
-		cout << "  inertia = " << inertia << endl;
+		if (verbose) {
+			cout << "  mass = " << mass << endl;
+			cout << "  com = " << com << endl;
+			cout << "  inertia = " << inertia << endl;
+		}
 
 		body = Body (mass, com, inertia);
 	}
-	cout << "  Body = " << endl << body.mSpatialInertia << endl;
+
+	if (verbose)
+		cout << "  Body = " << endl << body.mSpatialInertia << endl;
 
 	return true;
 }
 	vector<string> frame_names = get_keys (L, "frames");
 
 	unsigned int parent_id;
-	Math::SpatialTransform joint_transform;
+	Math::SpatialTransform joint_frame;
 	Joint joint;
 	Body body;
 	string body_name;
 
 		if (!read_frame_params(L, frame_path,
 					parent_id,
-					joint_transform,
+					joint_frame,
 					joint,
 					body,
-					body_name
+					body_name,
+					verbose
 					)) {
 			cerr << "Error reading frame " << frame_names[i] << "." << endl;
 			return false;
 		}
 
-		unsigned int body_id = model->AddBody (parent_id, joint_transform, joint, body, body_name);
+		if (verbose)
+			cout << "   Adding Body name = " << body_name << endl;
+
+		unsigned int body_id = model->AddBody (parent_id, joint_frame, joint, body, body_name);
 		void *body_table = (void*) get_pointer (L, frame_path + ".child_body");
 		body_table_id_map[body_table] = body_id;
 	}

File addons/luamodel/rbdl_luamodel_test.cc

 #include "rbdl_luamodel.h"
 
 #include <iostream>
+#include <iomanip>
+#include <sstream>
 
 using namespace std;
 
+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 usage (const char* argv_0) {
+	cerr << "Usage: " << argv_0 << "[-v] <model.lua>" << endl;
+	exit (1);
+}
+
 int main (int argc, char *argv[]) {
-	if (argc != 2) {
-		cerr << "Usage: " << argv[0] << " <model.lua>" << endl;
-		return -1;
+	if (argc < 2 || argc > 3) {
+		usage(argv[0]);
+	}
+
+	bool verbose = false;
+	string filename = argv[1];
+
+	if (argc == 3) {
+		if (string(argv[1]) == "-v" || string(argv[1]) == "--verbose") {
+			verbose = true;
+			filename = argv[2];
+		} else if (string(argv[2]) == "-v" || string(argv[2]) == "--verbose") {
+			verbose = true;
+			filename = argv[1];
+		} else
+			usage(argv[0]);
 	}
 
 	RigidBodyDynamics::Model model;
-	if (!RigidBodyDynamics::Addons::read_luamodel(argv[1], &model, true)) {
+	if (!RigidBodyDynamics::Addons::read_luamodel(filename.c_str(), &model, verbose)) {
 		cerr << "Loading of lua model failed!" << endl;
 		return -1;
 	}
 
-	cout << "Model loading successful: " << model.dof_count << " dofs." << endl;
+	cout << "Model loading successful!" << endl;
+
+	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;
+	}
 
 	return 0;
 }

File addons/luamodel/samplemodel.lua

-print ("heyho!")
+print ("Hello, this is output from the model Lua file!")
 
 inertia = { 
 	{1.1, 0.1, 0.2},
 	{0.5, 0.6, 1.3}
 }
 
-pelvis = { name = "pelvis", mass = 1.1, com = { 1.1, 1.2, 1.3}, inertia = inertia }
-thigh_right = { name = "thigh_right", mass = 91., com = { 1.1, 1.2, 1.3}, inertia = inertia }
-shank_right = { name = "shank_right", mass = 91., com = { 1.1, 1.2, 1.3}, inertia = inertia }
-thigh_left = { name = "thigh_left", mass = 91., com = { 1.1, 1.2, 1.3}, inertia = inertia }
-shank_left = { name = "shank_left", mass = 91., com = { 1.1, 1.2, 1.3}, inertia = inertia }
+pelvis = { mass = 1.1, com = { 1.1, 1.2, 1.3}, inertia = inertia }
+thigh_right = { mass = 91., com = { 1.1, 1.2, 1.3}, inertia = inertia }
+shank_right = { mass = 91., com = { 1.1, 1.2, 1.3}, inertia = inertia }
+thigh_left = { mass = 91., com = { 1.1, 1.2, 1.3}, inertia = inertia }
+shank_left = { mass = 91., com = { 1.1, 1.2, 1.3}, inertia = inertia }
 
 bodies = {
 	pelvis = pelvis,
 			name = "thigh_right",
 			parent_body = bodies.pelvis,
 			child_body = bodies.thigh_right,
-			joint = joints.fixed
+			joint = joints.spherical_zyx
 		},
 	}
 }