<commit_before>//
// Copyright (c) 2009, <NAME>
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright notice,
//   this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright notice,
//   this list of conditions and the following disclaimer in the documentation
//   and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//

#include <iostream> // TODO
#include <unordered_map>
#include <boost/lexical_cast.hpp>
#include <boost/algorithm/string.hpp> 
#include <rl/math/Quaternion.h>
#include <rl/math/Rotation.h>
#include <rl/math/Unit.h>
#include <rl/xml/Attribute.h>
#include <rl/xml/Document.h>
#include <rl/xml/DomParser.h>
#include <rl/xml/Node.h>
#include <rl/xml/Object.h>
#include <rl/xml/Path.h>
#include <rl/xml/Stylesheet.h>

#include "Body.h"
#include "Cylindrical.h"
#include "Dynamic.h"
#include "Exception.h"
#include "Fixed.h"
#include "Helical.h"
#include "Joint.h"
#include "Kinematic.h"
#include "Model.h"
#include "Prismatic.h"
#include "Revolute.h"
#include "Spherical.h"
#include "UrdfFactory.h"
#include "World.h"

namespace rl
{
	namespace mdl
	{
		UrdfFactory::UrdfFactory()
		{
		}
		
		UrdfFactory::~UrdfFactory()
		{
		}
		
		void
		UrdfFactory::load(const ::std::string& filename, Model* model)
		{
			::rl::xml::DomParser parser;
			
			::rl::xml::Document document = parser.readFile(filename, "", XML_PARSE_NOENT);
			document.substitute(XML_PARSE_NOENT);
			
			::rl::xml::Path path(document);
			
			::rl::xml::NodeSet robots = path.eval("/robot").getValue< ::rl::xml::NodeSet>();
			
			if (robots.empty())
			{
				throw Exception("URDF is missing robot node");
			}
			
			path.setNode(robots[0]);
			
			// name
			
			model->setName(path.eval("string(@name)").getValue< ::std::string>());
::std::cout << model->getName() << ::std::endl;
			
			// links
			
			::rl::xml::NodeSet links = path.eval("link").getValue< ::rl::xml::NodeSet>();
			
			::std::unordered_map< ::std::string, Frame*> name2frame;
			
			for (int j = 0; j < links.size(); ++j)
			{
::std::cout << "link: " << j << ::std::endl;
				::rl::xml::Path path(document, links[j]);
				
				if (path.eval("count(collision|inertial|visual) > 0").getValue<bool>())
				{
					Body* b = new Body();
					
					model->add(b);
					
					if (path.eval("count(inertial/origin/@xyz) > 0").getValue<bool>())
					{
						::std::vector< ::std::string> xyz;
						::std::string tmp = path.eval("string(inertial/origin/@xyz)").getValue< ::std::string>();
						::boost::split(xyz, tmp, ::boost::algorithm::is_space(), ::boost::algorithm::token_compress_on);
						
						b->setCenterOfMass(
							::boost::lexical_cast< ::rl::math::Real>(xyz[0]),
							::boost::lexical_cast< ::rl::math::Real>(xyz[1]),
							::boost::lexical_cast< ::rl::math::Real>(xyz[2])
						);
					}
					else
					{
						b->setCenterOfMass(0, 0, 0);
					}
::std::cout << "\tcog: " << b->cm.transpose() << ::std::endl;
					
					if (path.eval("count(inertial/origin/@abc) > 0").getValue<bool>())
					{
						throw Exception("URDF link/inertial/origin/@abc not supported");
					}
					
					b->setInertia(
						path.eval("number(inertial/inertia/@ixx)").getValue< ::rl::math::Real>(1),
						path.eval("number(inertial/inertia/@iyy)").getValue< ::rl::math::Real>(1),
						path.eval("number(inertial/inertia/@izz)").getValue< ::rl::math::Real>(1),
						path.eval("number(inertial/inertia/@iyz)").getValue< ::rl::math::Real>(0),
						path.eval("number(inertial/inertia/@ixz)").getValue< ::rl::math::Real>(0),
						path.eval("number(inertial/inertia/@ixy)").getValue< ::rl::math::Real>(0)
					);
::std::cout << "\tinertia: " << b->ic.voigt6().transpose() << ::std::endl;
					
					b->setMass(
						path.eval("number(inertial/mass/@value)").getValue< ::rl::math::Real>(1)
					);
::std::cout << "\tmass: " << b->m << ::std::endl;
					
					b->setName(path.eval("string(@name)").getValue< ::std::string>());
::std::cout << "\tname: " << b->getName() << ::std::endl;
					
					name2frame[path.eval("string(@name)").getValue< ::std::string>()] = b;
				}
				else
				{
					Frame* f = new Frame();
					
					model->add(f);
					
					f->setName(path.eval("string(@name)").getValue< ::std::string>());
::std::cout << "\tname: " << f->getName() << ::std::endl;
					
					name2frame[path.eval("string(@name)").getValue< ::std::string>()] = f;
				}<commit_msg>Parse all links as bodies in rl::mdl::UrdfFactory<commit_after>//
// Copyright (c) 2009, <NAME>
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright notice,
//   this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright notice,
//   this list of conditions and the following disclaimer in the documentation
//   and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//

#include <iostream> // TODO
#include <unordered_map>
#include <boost/lexical_cast.hpp>
#include <boost/algorithm/string.hpp> 
#include <rl/math/Quaternion.h>
#include <rl/math/Rotation.h>
#include <rl/math/Unit.h>
#include <rl/xml/Attribute.h>
#include <rl/xml/Document.h>
#include <rl/xml/DomParser.h>
#include <rl/xml/Node.h>
#include <rl/xml/Object.h>
#include <rl/xml/Path.h>
#include <rl/xml/Stylesheet.h>

#include "Body.h"
#include "Cylindrical.h"
#include "Dynamic.h"
#include "Exception.h"
#include "Fixed.h"
#include "Helical.h"
#include "Joint.h"
#include "Kinematic.h"
#include "Model.h"
#include "Prismatic.h"
#include "Revolute.h"
#include "Spherical.h"
#include "UrdfFactory.h"
#include "World.h"

namespace rl
{
	namespace mdl
	{
		UrdfFactory::UrdfFactory()
		{
		}
		
		UrdfFactory::~UrdfFactory()
		{
		}
		
		void
		UrdfFactory::load(const ::std::string& filename, Model* model)
		{
			::rl::xml::DomParser parser;
			
			::rl::xml::Document document = parser.readFile(filename, "", XML_PARSE_NOENT);
			document.substitute(XML_PARSE_NOENT);
			
			::rl::xml::Path path(document);
			
			::rl::xml::NodeSet robots = path.eval("/robot").getValue< ::rl::xml::NodeSet>();
			
			if (robots.empty())
			{
				throw Exception("URDF is missing robot node");
			}
			
			path.setNode(robots[0]);
			
			// name
			
			model->setName(path.eval("string(@name)").getValue< ::std::string>());
::std::cout << model->getName() << ::std::endl;
			
			// links
			
			::rl::xml::NodeSet links = path.eval("link").getValue< ::rl::xml::NodeSet>();
			
			::std::unordered_map< ::std::string, Frame*> name2frame;
			
			for (int j = 0; j < links.size(); ++j)
			{
::std::cout << "link: " << j << ::std::endl;
				::rl::xml::Path path(document, links[j]);
				
				Body* b = new Body();
				
				model->add(b);
				
				if (path.eval("count(inertial/origin/@xyz) > 0").getValue<bool>())
				{
					::std::vector< ::std::string> xyz;
					::std::string tmp = path.eval("string(inertial/origin/@xyz)").getValue< ::std::string>();
					::boost::split(xyz, tmp, ::boost::algorithm::is_space(), ::boost::algorithm::token_compress_on);
					
					b->setCenterOfMass(
						::boost::lexical_cast< ::rl::math::Real>(xyz[0]),
						::boost::lexical_cast< ::rl::math::Real>(xyz[1]),
						::boost::lexical_cast< ::rl::math::Real>(xyz[2])
					);
				}
				else
				{
					b->setCenterOfMass(0, 0, 0);
				}
::std::cout << "\tcog: " << b->cm.transpose() << ::std::endl;
				
				if (path.eval("count(inertial/origin/@abc) > 0").getValue<bool>())
				{
					throw Exception("URDF link/inertial/origin/@abc not supported");
				}
				
				b->setInertia(
					path.eval("number(inertial/inertia/@ixx)").getValue< ::rl::math::Real>(1),
					path.eval("number(inertial/inertia/@iyy)").getValue< ::rl::math::Real>(1),
					path.eval("number(inertial/inertia/@izz)").getValue< ::rl::math::Real>(1),
					path.eval("number(inertial/inertia/@iyz)").getValue< ::rl::math::Real>(0),
					path.eval("number(inertial/inertia/@ixz)").getValue< ::rl::math::Real>(0),
					path.eval("number(inertial/inertia/@ixy)").getValue< ::rl::math::Real>(0)
				);
::std::cout << "\tinertia: " << b->ic.voigt6().transpose() << ::std::endl;
				
				b->setMass(
					path.eval("number(inertial/mass/@value)").getValue< ::rl::math::Real>(1)
				);
::std::cout << "\tmass: " << b->m << ::std::endl;
				
				b->setName(path.eval("string(@name)").getValue< ::std::string>());
::std::cout << "\tname: " << b->getName() << ::std::endl;
				
				name2frame[path.eval("string(@name)").getValue< ::std::string>()] = b;
{'fraction_non_alphanumeric': 0.19084778420038537, 'fraction_numerical': 0.004816955684007707, 'mean_word_length': 3.8600187265917603, 'pattern_counts': {'":': 0, '<': 152, '<?xml version=': 0, '>': 108, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 30}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '3485988', 'n_tokens_mistral': 4177, 'n_tokens_neox': 3417, 'n_words': 796}