Commits

Martin Felis committed aad37fe

urdfreader can read urdf files and create RBDL models but is not verified

  • Participants
  • Parent commits 958a7fd

Comments (0)

Files changed (4)

File addons/urdfreader/README

-urdfreader - load models from (URDF Unified Robot Description Format) files
-Copyright (c) 2012 Martin Felis <martin.felis@iwr.uni-heidelberg.de>
-
-REQUIREMENTS
-
-This addon depends on ROS, the Robot Operating System (http://www.ros.org)
-and uses its urdf_parser and urdf_interface to load and access the model
-data in the files. You therefore need to have ROS and the package
-"robot-model" installed on your system to be able to compile this addon.
-
-See http://www.ros.org/wiki/ROS/Installation for more details on how to
-install ROS.
-
-LICENSING
-
-This code is published under the zlib license, however some parts of the
-CMake scripts are taken from other projects and are licensed under
-different terms.
-
-Full license text:
-
--------
-urdfreader - load models from URDF (Unified Robot Description Format) files
-Copyright (c) 2011-2012 Martin Felis <martin.felis@iwr.uni-heidelberg.de>
-
-This software is provided 'as-is', without any express or implied
-warranty. In no event will the authors be held liable for any damages
-arising from the use of this software.
-
-Permission is granted to anyone to use this software for any purpose,
-including commercial applications, and to alter it and redistribute it
-freely, subject to the following restrictions:
-
-   1. The origin of this software must not be misrepresented; you must not
-   claim that you wrote the original software. If you use this software
-   in a product, an acknowledgment in the product documentation would be
-   appreciated but is not required.
-
-   2. Altered source versions must be plainly marked as such, and must not be
-   misrepresented as being the original software.
-
-   3. This notice may not be removed or altered from any source
-   distribution.

File addons/urdfreader/README.md

+urdfreader - load models from (URDF Unified Robot Description Format) files
+Copyright (c) 2012 Martin Felis <martin.felis@iwr.uni-heidelberg.de>
+
+Requirements
+============
+
+This addon depends on ROS, the Robot Operating System (http://www.ros.org)
+and uses its urdf_parser and urdf_interface to load and access the model
+data in the files. You therefore need to have ROS and the package
+"robot-model" installed on your system to be able to compile this addon. So
+far only the ROS release "Fuerte" can be used with this code.
+
+See http://www.ros.org/wiki/ROS/Installation for more details on how to
+install ROS.
+
+Licensing
+=========
+
+This code is published under the zlib license, however some parts of the
+CMake scripts are taken from other projects and are licensed under
+different terms.
+
+Full license text:
+
+-------
+urdfreader - load models from URDF (Unified Robot Description Format) files
+Copyright (c) 2011-2012 Martin Felis <martin.felis@iwr.uni-heidelberg.de>
+
+This software is provided 'as-is', without any express or implied
+warranty. In no event will the authors be held liable for any damages
+arising from the use of this software.
+
+Permission is granted to anyone to use this software for any purpose,
+including commercial applications, and to alter it and redistribute it
+freely, subject to the following restrictions:
+
+   1. The origin of this software must not be misrepresented; you must not
+   claim that you wrote the original software. If you use this software
+   in a product, an acknowledgment in the product documentation would be
+   appreciated but is not required.
+
+   2. Altered source versions must be plainly marked as such, and must not be
+   misrepresented as being the original software.
+
+   3. This notice may not be removed or altered from any source
+   distribution.

File addons/urdfreader/rbdl_urdf_test.cc

 
 using namespace std;
 
+bool verbose = false;
+string filename = "";
+
 int main (int argc, char *argv[]) {
-	if (argc != 2) {
-		cerr << "Usage: " << argv[0] << " <robot.urdf>" << endl;
+	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(argv[1], &model, true)) {
+	if (!RigidBodyDynamics::Addons::read_urdf_model(filename.c_str(), &model, verbose)) {
 		cerr << "Loading of urdf model failed!" << endl;
 		return -1;
 	}

File addons/urdfreader/rbdl_urdfreader.cc

 typedef map<string, LinkPtr > URDFLinkMap;
 typedef map<string, JointPtr > URDFJointMap;
 
-bool construct_model (Model* rbdl_model, urdf::Model *urdf_model) {
+bool construct_model (Model* rbdl_model, urdf::Model *urdf_model, bool verbose) {
 	boost::shared_ptr<urdf::Link> urdf_root_link;
 
 	URDFLinkMap link_map;
 			link_stack.push (link_map[cur_joint->child_link_name]);
 			joint_index_stack.push(0);
 
-			for (int i = 1; i < joint_index_stack.size() - 1; i++) {
-				cout << "  ";
+			if (verbose) {
+				for (int i = 1; i < joint_index_stack.size() - 1; i++) {
+					cout << "  ";
+				}
+				cout << "joint '" << cur_joint->name << "' child link '" << link_stack.top()->name << " type = " << cur_joint->type << endl;
 			}
-			cout << "joint '" << cur_joint->name << "' child link '" << link_stack.top()->name << " type = " << cur_joint->type << endl;
+
 			joint_names.push_back(cur_joint->name);
 		} else {
 			link_stack.pop();
 
 		Body rbdl_body = Body (link_inertial_mass, link_inertial_position, link_inertial_inertia);
 
+		if (verbose) {
+			cout << "+ Adding Body " << endl;
+			cout << "  parent_id  : " << rbdl_parent_id << endl;
+			cout << "  joint_frame: " << rbdl_joint_frame << endl;
+			cout << "  joint dofs : " << rbdl_joint.mDoFCount << endl;
+			for (unsigned int j = 0; j < rbdl_joint.mDoFCount; j++) {
+				cout << "    " << j << ": " << rbdl_joint.mJointAxes[j].transpose() << endl;
+			}
+			cout << "  body inertia: " << endl << rbdl_body.mSpatialInertia << endl;
+			cout << "  body_name  : " << urdf_child->name << endl;
+		}
+
 		rbdl_model->AddBody (rbdl_parent_id, rbdl_joint_frame, rbdl_joint, rbdl_body, urdf_child->name);
 	}
 
-	return false;
+	return true;
 }
 
 bool read_urdf_model (const char* filename, Model* model, bool verbose) {
 		cerr << "Error opening urdf file" << endl;
 	}
 
-	if (!construct_model (model, &urdf_model)) {
+	if (!construct_model (model, &urdf_model, verbose)) {
 		cerr << "Error constructing model from urdf file." << endl;
 		return false;
 	}
 
+	model->gravity.set (0., 0., -9.81);
+
+	cout << "Model loading succcessful!" << endl;
+
 	return true;
 }