simulation::WorldPtr DartLoader::parseWorldString(
const std::string& _urdfString, const common::Uri& _baseUri,
const common::ResourceRetrieverPtr& _resourceRetriever)
{
const common::ResourceRetrieverPtr resourceRetriever
= getResourceRetriever(_resourceRetriever);
if(_urdfString.empty())
{
dtwarn << "[DartLoader::parseWorldString] A blank string cannot be "
<< "parsed into a World. Returning a nullptr\n";
return nullptr;
}
std::shared_ptr<urdf_parsing::World> worldInterface =
urdf_parsing::parseWorldURDF(_urdfString, _baseUri);
if(!worldInterface)
{
dtwarn << "[DartLoader::parseWorldString] Failed loading URDF.\n";
return nullptr;
}
simulation::WorldPtr world(new simulation::World);
for(size_t i = 0; i < worldInterface->models.size(); ++i)
{
const urdf_parsing::Entity& entity = worldInterface->models[i];
dynamics::SkeletonPtr skeleton = modelInterfaceToSkeleton(
entity.model.get(), entity.uri, resourceRetriever);
if(!skeleton)
{
dtwarn << "[DartLoader::parseWorldString] Robot " << worldInterface->models[i].model->getName()
<< " was not correctly parsed!\n";
continue;
}
// Initialize position and RPY
dynamics::Joint* rootJoint = skeleton->getRootBodyNode()->getParentJoint();
Eigen::Isometry3d transform = toEigen(worldInterface->models[i].origin);
if (dynamic_cast<dynamics::FreeJoint*>(rootJoint))
rootJoint->setPositions(dynamics::FreeJoint::convertToPositions(transform));
else
rootJoint->setTransformFromParentBodyNode(transform);
world->addSkeleton(skeleton);
}
return world;
}
/**
* @function parseSkeleton
*/
dynamics::Skeleton* DartLoader::parseSkeleton(std::string _urdfFileName) {
boost::shared_ptr<urdf::ModelInterface> skeletonModelPtr = urdf::parseURDF(readFileToString(_urdfFileName));
if(!skeletonModelPtr)
return NULL;
// Change path to a Unix-style path if given a Windows one
// Windows can handle Unix-style paths (apparently)
std::replace(_urdfFileName.begin(), _urdfFileName.end(), '\\' , '/' );
std::string skelDirectory = _urdfFileName.substr(0, _urdfFileName.rfind("/") + 1);
return modelInterfaceToSkeleton(skeletonModelPtr.get(), skelDirectory);
}
/**
* @function parseWorld
*/
simulation::World* DartLoader::parseWorld(std::string _urdfFileName) {
std::string xmlString = readFileToString(_urdfFileName);
// Change path to a Unix-style path if given a Windows one
// Windows can handle Unix-style paths (apparently)
std::replace(_urdfFileName.begin(), _urdfFileName.end(), '\\' , '/');
std::string worldDirectory = _urdfFileName.substr(0, _urdfFileName.rfind("/") + 1);
urdf::World* worldInterface = urdf::parseWorldURDF(xmlString, worldDirectory);
if(!worldInterface)
return NULL;
// Store paths from world to entities
parseWorldToEntityPaths(xmlString);
simulation::World* world = new simulation::World();
for(unsigned int i = 0; i < worldInterface->models.size(); ++i) {
std::string skeletonDirectory = worldDirectory + mWorld_To_Entity_Paths.find(worldInterface->models[i].model->getName())->second;
dynamics::Skeleton* skeleton = modelInterfaceToSkeleton(worldInterface->models[i].model.get(), skeletonDirectory);
if(!skeleton) {
std::cout << "[ERROR] Robot " << worldInterface->models[i].model->getName() << " was not correctly parsed. World is not loaded. Exiting!" << std::endl;
return NULL;
}
// Initialize position and RPY
dynamics::Joint* rootJoint = skeleton->getRootBodyNode()->getParentJoint();
Eigen::Isometry3d transform = toEigen(worldInterface->models[i].origin);
if(dynamic_cast<dynamics::FreeJoint*>(rootJoint)) {
Eigen::Vector6d coordinates;
coordinates << math::logMap(transform.linear()), transform.translation();
rootJoint->set_q(coordinates);
}
else {
rootJoint->setTransformFromParentBodyNode(transform);
}
world->addSkeleton(skeleton);
}
return world;
}
dynamics::SkeletonPtr DartLoader::parseSkeletonString(
const std::string& _urdfString, const common::Uri& _baseUri,
const common::ResourceRetrieverPtr& _resourceRetriever)
{
if(_urdfString.empty())
{
dtwarn << "[DartLoader::parseSkeletonString] A blank string cannot be "
<< "parsed into a Skeleton. Returning a nullptr\n";
return nullptr;
}
ModelInterfacePtr urdfInterface = urdf::parseURDF(_urdfString);
if(!urdfInterface)
{
dtwarn << "[DartLoader::parseSkeletonString] Failed loading URDF.\n";
return nullptr;
}
return modelInterfaceToSkeleton(
urdfInterface.get(), _baseUri, getResourceRetriever(_resourceRetriever));
}
dynamics::SkeletonPtr DartLoader::parseSkeleton(
const common::Uri& _uri,
const common::ResourceRetrieverPtr& _resourceRetriever)
{
const common::ResourceRetrieverPtr resourceRetriever
= getResourceRetriever(_resourceRetriever);
std::string content;
if (!readFileToString(resourceRetriever, _uri, content))
return nullptr;
// Use urdfdom to load the URDF file.
const ModelInterfacePtr urdfInterface = urdf::parseURDF(content);
if(!urdfInterface)
{
dtwarn << "[DartLoader::readSkeleton] Failed loading URDF file '"
<< _uri.toString() << "'.\n";
return nullptr;
}
return modelInterfaceToSkeleton(urdfInterface.get(), _uri, resourceRetriever);
}
