Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add default options to DartLoader for missing properties in URDF files #1605

Merged
merged 2 commits into from
Sep 19, 2021
Merged

Add default options to DartLoader for missing properties in URDF files #1605

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
63b6a74
Add options to DartLoader
jslee02 Sep 18, 2021
7b2a635
Update changelog
jslee02 Sep 18, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 4 additions & 0 deletions CHANGELOG.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,10 @@

* Remove DART_BUILD_DARTPY option: [#1600](https://github.com/dartsim/dart/pull/1600)

* Parsers

* Added default options to DartLoader for missing properties in URDF files: : [#1605](https://github.com/dartsim/dart/pull/1605)

### [DART 6.11.1 (2021-08-23)](https://github.com/dartsim/dart/milestone/67?closed=1)

* Dynamics
Expand Down
198 changes: 159 additions & 39 deletions dart/utils/urdf/DartLoader.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -62,8 +62,21 @@ namespace utils {

using ModelInterfacePtr = urdf_shared_ptr<urdf::ModelInterface>;

DartLoader::DartLoader()
: mLocalRetriever(new common::LocalResourceRetriever),
DartLoader::Options::Options(
common::ResourceRetrieverPtr resourceRetriever,
RootJointType defaultRootJointType,
const dynamics::Inertia& defaultInertia)
: mResourceRetriever(std::move(resourceRetriever)),
mDefaultRootJointType(defaultRootJointType),
mDefaultInertia(defaultInertia)
{
// Do nothing
}

//==============================================================================
DartLoader::DartLoader(const Options& options)
: mOptions(options),
mLocalRetriever(new common::LocalResourceRetriever),
mPackageRetriever(new utils::PackageResourceRetriever(mLocalRetriever)),
mRetriever(new utils::CompositeResourceRetriever)
{
Expand All @@ -72,51 +85,99 @@ DartLoader::DartLoader()
mRetriever->addSchemaRetriever("dart", DartResourceRetriever::create());
}

//==============================================================================
void DartLoader::setOptions(const Options& options)
{
mOptions = options;
}

//==============================================================================
const DartLoader::Options& DartLoader::getOptions() const
{
return mOptions;
}

//==============================================================================
void DartLoader::addPackageDirectory(
const std::string& _packageName, const std::string& _packageDirectory)
{
mPackageRetriever->addPackageDirectory(_packageName, _packageDirectory);
}

//==============================================================================
dynamics::SkeletonPtr DartLoader::parseSkeleton(
const common::Uri& _uri,
const common::ResourceRetrieverPtr& _resourceRetriever,
unsigned int flags)
{
const auto oldOptions = mOptions;
auto options = mOptions;
options.mResourceRetriever = _resourceRetriever;
if (flags & DartLoader::Flags::FIXED_BASE_LINK)
options.mDefaultRootJointType = RootJointType::FIXED;
else
options.mDefaultRootJointType = RootJointType::FLOATING;
mOptions = options;
auto out = parseSkeleton(_uri);
mOptions = oldOptions;
return out;
}

//==============================================================================
dynamics::SkeletonPtr DartLoader::parseSkeleton(const common::Uri& uri)
{
const common::ResourceRetrieverPtr resourceRetriever
= getResourceRetriever(_resourceRetriever);
= getResourceRetriever(mOptions.mResourceRetriever);

std::string content;
if (!readFileToString(resourceRetriever, _uri, 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";
<< uri.toString() << "'.\n";
return nullptr;
}

return modelInterfaceToSkeleton(
urdfInterface.get(), _uri, resourceRetriever, flags);
urdfInterface.get(), uri, resourceRetriever, mOptions);
}

//==============================================================================
dynamics::SkeletonPtr DartLoader::parseSkeletonString(
const std::string& _urdfString,
const common::Uri& _baseUri,
const common::ResourceRetrieverPtr& _resourceRetriever,
unsigned int flags)
{
if (_urdfString.empty())
const auto oldOptions = mOptions;
auto options = mOptions;
options.mResourceRetriever = _resourceRetriever;
if (flags & DartLoader::Flags::FIXED_BASE_LINK)
options.mDefaultRootJointType = RootJointType::FIXED;
else
options.mDefaultRootJointType = RootJointType::FLOATING;
mOptions = options;
auto out = parseSkeletonString(_urdfString, _baseUri);
mOptions = oldOptions;
return out;
}

//==============================================================================
dynamics::SkeletonPtr DartLoader::parseSkeletonString(
const std::string& urdfString, const common::Uri& baseUri)
{
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);
ModelInterfacePtr urdfInterface = urdf::parseURDF(urdfString);
if (!urdfInterface)
{
dtwarn << "[DartLoader::parseSkeletonString] Failed loading URDF.\n";
Expand All @@ -125,44 +186,79 @@ dynamics::SkeletonPtr DartLoader::parseSkeletonString(

return modelInterfaceToSkeleton(
urdfInterface.get(),
_baseUri,
getResourceRetriever(_resourceRetriever),
flags);
baseUri,
getResourceRetriever(mOptions.mResourceRetriever),
mOptions);
}

//==============================================================================
simulation::WorldPtr DartLoader::parseWorld(
const common::Uri& _uri,
const common::ResourceRetrieverPtr& _resourceRetriever,
unsigned int flags)
{
const auto oldOptions = mOptions;
auto options = mOptions;
options.mResourceRetriever = _resourceRetriever;
if (flags & DartLoader::Flags::FIXED_BASE_LINK)
options.mDefaultRootJointType = RootJointType::FIXED;
else
options.mDefaultRootJointType = RootJointType::FLOATING;
mOptions = options;
auto out = parseWorld(_uri);
mOptions = oldOptions;
return out;
}

//==============================================================================
simulation::WorldPtr DartLoader::parseWorld(const common::Uri& uri)
{
const common::ResourceRetrieverPtr resourceRetriever
= getResourceRetriever(_resourceRetriever);
= getResourceRetriever(mOptions.mResourceRetriever);

std::string content;
if (!readFileToString(resourceRetriever, _uri, content))
if (!readFileToString(resourceRetriever, uri, content))
return nullptr;

return parseWorldString(content, _uri, _resourceRetriever, flags);
return parseWorldString(content, uri);
}

//==============================================================================
simulation::WorldPtr DartLoader::parseWorldString(
const std::string& _urdfString,
const common::Uri& _baseUri,
const common::ResourceRetrieverPtr& _resourceRetriever,
unsigned int flags)
{
const auto oldOptions = mOptions;
auto options = mOptions;
options.mResourceRetriever = _resourceRetriever;
if (flags & DartLoader::Flags::FIXED_BASE_LINK)
options.mDefaultRootJointType = RootJointType::FIXED;
else
options.mDefaultRootJointType = RootJointType::FLOATING;
mOptions = options;
auto out = parseWorldString(_urdfString, _baseUri);
mOptions = oldOptions;
return out;
}

//==============================================================================
simulation::WorldPtr DartLoader::parseWorldString(
const std::string& urdfString, const common::Uri& baseUri)
{
const common::ResourceRetrieverPtr resourceRetriever
= getResourceRetriever(_resourceRetriever);
= getResourceRetriever(mOptions.mResourceRetriever);

if (_urdfString.empty())
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, resourceRetriever);
= urdf_parsing::parseWorldURDF(urdfString, baseUri, resourceRetriever);

if (!worldInterface)
{
Expand All @@ -176,7 +272,7 @@ simulation::WorldPtr DartLoader::parseWorldString(
{
const urdf_parsing::Entity& entity = worldInterface->models[i];
dynamics::SkeletonPtr skeleton = modelInterfaceToSkeleton(
entity.model.get(), entity.uri, resourceRetriever, flags);
entity.model.get(), entity.uri, resourceRetriever, mOptions);

if (!skeleton)
{
Expand Down Expand Up @@ -207,7 +303,7 @@ dynamics::SkeletonPtr DartLoader::modelInterfaceToSkeleton(
const urdf::ModelInterface* model,
const common::Uri& baseUri,
const common::ResourceRetrieverPtr& resourceRetriever,
unsigned int flags)
const Options& options)
{
dynamics::SkeletonPtr skeleton = dynamics::Skeleton::create(model->getName());

Expand Down Expand Up @@ -237,7 +333,7 @@ dynamics::SkeletonPtr DartLoader::modelInterfaceToSkeleton(
nullptr,
baseUri,
resourceRetriever,
flags))
options))
{
return nullptr;
}
Expand All @@ -246,7 +342,13 @@ dynamics::SkeletonPtr DartLoader::modelInterfaceToSkeleton(
else
{
if (!createSkeletonRecursive(
model, skeleton, root, nullptr, baseUri, resourceRetriever, flags))
model,
skeleton,
root,
nullptr,
baseUri,
resourceRetriever,
options))
{
return nullptr;
}
Expand All @@ -267,7 +369,7 @@ bool DartLoader::createSkeletonRecursive(
dynamics::BodyNode* parentNode,
const common::Uri& baseUri,
const common::ResourceRetrieverPtr& resourceRetriever,
unsigned int flags)
const Options& options)
{
assert(lk);

Expand All @@ -278,11 +380,12 @@ bool DartLoader::createSkeletonRecursive(
}

dynamics::BodyNode::Properties properties;
if (!createDartNodeProperties(lk, properties, baseUri, resourceRetriever))
if (!createDartNodeProperties(
lk, properties, baseUri, resourceRetriever, options))
return false;

dynamics::BodyNode* node = createDartJointAndNode(
lk->parent_joint.get(), properties, parentNode, skel, flags);
lk->parent_joint.get(), properties, parentNode, skel, options);

if (!node)
return false;
Expand All @@ -302,7 +405,7 @@ bool DartLoader::createSkeletonRecursive(
node,
baseUri,
resourceRetriever,
flags))
options))
{
return false;
}
Expand Down Expand Up @@ -380,23 +483,35 @@ dynamics::BodyNode* createDartJointAndNodeForRoot(
const dynamics::BodyNode::Properties& _body,
dynamics::BodyNode* _parent,
dynamics::SkeletonPtr _skeleton,
unsigned int flags)
const DartLoader::Options& options)
{
dynamics::Joint::Properties basicProperties("rootJoint");

dynamics::GenericJoint<math::R1Space>::UniqueProperties singleDof;
std::pair<dynamics::Joint*, dynamics::BodyNode*> pair;
if (flags & DartLoader::Flags::FIXED_BASE_LINK)
switch (options.mDefaultRootJointType)
{
pair = _skeleton->createJointAndBodyNodePair<dynamics::WeldJoint>(
_parent, basicProperties, _body);
}
else
{
dynamics::GenericJoint<math::SE3Space>::Properties properties(
basicProperties);
pair = _skeleton->createJointAndBodyNodePair<dynamics::FreeJoint>(
_parent, properties, _body);
case DartLoader::RootJointType::FLOATING: {
dynamics::GenericJoint<math::SE3Space>::Properties properties(
basicProperties);
pair = _skeleton->createJointAndBodyNodePair<dynamics::FreeJoint>(
_parent, properties, _body);
break;
}
case DartLoader::RootJointType::FIXED: {
pair = _skeleton->createJointAndBodyNodePair<dynamics::WeldJoint>(
_parent, basicProperties, _body);
break;
}
default: {
dterr << "Unsupported RootJointType '"
<< static_cast<int>(options.mDefaultRootJointType)
<< "'. Using FLOATING instead.\n";
dynamics::GenericJoint<math::SE3Space>::Properties properties(
basicProperties);
pair = _skeleton->createJointAndBodyNodePair<dynamics::FreeJoint>(
_parent, properties, _body);
}
}

return pair.second;
Expand All @@ -410,14 +525,14 @@ dynamics::BodyNode* DartLoader::createDartJointAndNode(
const dynamics::BodyNode::Properties& _body,
dynamics::BodyNode* _parent,
dynamics::SkeletonPtr _skeleton,
unsigned int flags)
const Options& options)
{
// Special case for the root link (A root link doesn't have the parent joint).
// We don't have sufficient information what the joint type should be for the
// root link. So we create the root joint by the specified flgas.
if (!_jt)
{
return createDartJointAndNodeForRoot(_body, _parent, _skeleton, flags);
return createDartJointAndNodeForRoot(_body, _parent, _skeleton, options);
}

dynamics::Joint::Properties basicProperties;
Expand Down Expand Up @@ -545,7 +660,8 @@ bool DartLoader::createDartNodeProperties(
const urdf::Link* _lk,
dynamics::BodyNode::Properties& node,
const common::Uri& /*_baseUri*/,
const common::ResourceRetrieverPtr& /*_resourceRetriever*/)
const common::ResourceRetrieverPtr& /*_resourceRetriever*/,
const Options& options)
{
node.mName = _lk->name;

Expand All @@ -570,6 +686,10 @@ bool DartLoader::createDartNodeProperties(
node.mInertia.setMoment(
J(0, 0), J(1, 1), J(2, 2), J(0, 1), J(0, 2), J(1, 2));
}
else
{
node.mInertia = options.mDefaultInertia;
}

return true;
}
Expand Down
Loading