Skip to content

Commit

Permalink
Allow SdfParser to set default root joint type
Browse files Browse the repository at this point in the history
  • Loading branch information
@jslee02
jslee02 committed Oct 30, 2021
1 parent 6e5488f commit 49eb39f
Show file tree
Hide file tree
Showing 5 changed files with 518 additions and 38 deletions.
93 changes: 70 additions & 23 deletions dart/utils/sdf/SdfParser.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -102,15 +102,15 @@ using JointMap = std::map<std::string, SDFJoint>;
simulation::WorldPtr readWorld(
tinyxml2::XMLElement* worldElement,
const common::Uri& baseUri,
const common::ResourceRetrieverPtr& retriever);
const Options& options);

void readPhysics(
tinyxml2::XMLElement* physicsElement, simulation::WorldPtr world);

dynamics::SkeletonPtr readSkeleton(
tinyxml2::XMLElement* skeletonElement,
const common::Uri& baseUri,
const common::ResourceRetrieverPtr& retriever);
const Options& options);

bool createPair(
dynamics::SkeletonPtr skeleton,
Expand All @@ -123,7 +123,7 @@ enum NextResult
VALID,
CONTINUE,
BREAK,
CREATE_FREEJOINT_ROOT
CREATE_ROOT_JOINT
};

NextResult getNextJointAndNodePair(
Expand Down Expand Up @@ -237,11 +237,23 @@ common::ResourceRetrieverPtr getRetriever(

} // anonymous namespace

//==============================================================================
Options::Options(
common::ResourceRetrieverPtr resourceRetriever,
RootJointType defaultRootJointType)
: mResourceRetriever(std::move(resourceRetriever)),
mDefaultRootJointType(defaultRootJointType)
{
// Do nothing
}

//==============================================================================
simulation::WorldPtr readSdfFile(
const common::Uri& uri, const common::ResourceRetrieverPtr& nullOrRetriever)
{
return readWorld(uri, nullOrRetriever);
Options options;
options.mResourceRetriever = nullOrRetriever;
return readWorld(uri, options);
}

//==============================================================================
Expand All @@ -263,10 +275,9 @@ bool checkVersion(
}

//==============================================================================
simulation::WorldPtr readWorld(
const common::Uri& uri, const common::ResourceRetrieverPtr& nullOrRetriever)
simulation::WorldPtr readWorld(const common::Uri& uri, const Options& options)
{
const auto retriever = getRetriever(nullOrRetriever);
const auto retriever = getRetriever(options.mResourceRetriever);

//--------------------------------------------------------------------------
// Load xml and create Document
Expand Down Expand Up @@ -301,14 +312,23 @@ simulation::WorldPtr readWorld(
if (worldElement == nullptr)
return nullptr;

return readWorld(worldElement, uri, retriever);
return readWorld(worldElement, uri, options);
}

//==============================================================================
dynamics::SkeletonPtr readSkeleton(
simulation::WorldPtr readWorld(
const common::Uri& uri, const common::ResourceRetrieverPtr& nullOrRetriever)
{
const auto retriever = getRetriever(nullOrRetriever);
Options options;
options.mResourceRetriever = nullOrRetriever;
return readWorld(uri, options);
}

//==============================================================================
dynamics::SkeletonPtr readSkeleton(
const common::Uri& uri, const Options& options)
{
const auto retriever = getRetriever(options.mResourceRetriever);

//--------------------------------------------------------------------------
// Load xml and create Document
Expand Down Expand Up @@ -348,13 +368,22 @@ dynamics::SkeletonPtr readSkeleton(
return newSkeleton;
}

//==============================================================================
dynamics::SkeletonPtr readSkeleton(
const common::Uri& uri, const common::ResourceRetrieverPtr& nullOrRetriever)
{
Options options;
options.mResourceRetriever = nullOrRetriever;
return readSkeleton(uri, options);
}

namespace {

//==============================================================================
simulation::WorldPtr readWorld(
tinyxml2::XMLElement* worldElement,
const common::Uri& baseUri,
const common::ResourceRetrieverPtr& retriever)
const Options& options)
{
assert(worldElement != nullptr);

Expand All @@ -381,7 +410,7 @@ simulation::WorldPtr readWorld(
while (skeletonElements.next())
{
dynamics::SkeletonPtr newSkeleton
= readSkeleton(skeletonElements.get(), baseUri, retriever);
= readSkeleton(skeletonElements.get(), baseUri, options);

newWorld->addSkeleton(newSkeleton);
}
Expand Down Expand Up @@ -422,7 +451,7 @@ void readPhysics(
dynamics::SkeletonPtr readSkeleton(
tinyxml2::XMLElement* skeletonElement,
const common::Uri& baseUri,
const common::ResourceRetrieverPtr& retriever)
const Options& options)
{
assert(skeletonElement != nullptr);

Expand All @@ -432,8 +461,8 @@ dynamics::SkeletonPtr readSkeleton(

//--------------------------------------------------------------------------
// Bodies
BodyMap sdfBodyNodes
= readAllBodyNodes(skeletonElement, baseUri, retriever, skeletonFrame);
BodyMap sdfBodyNodes = readAllBodyNodes(
skeletonElement, baseUri, options.mResourceRetriever, skeletonFrame);

//--------------------------------------------------------------------------
// Joints
Expand All @@ -454,14 +483,31 @@ dynamics::SkeletonPtr readSkeleton(
break;
else if (CONTINUE == result)
continue;
else if (CREATE_FREEJOINT_ROOT == result)
else if (CREATE_ROOT_JOINT == result)
{
// If a root FreeJoint is needed for the parent of the current joint, then
// create it
SDFJoint rootJoint;
rootJoint.properties = dynamics::FreeJoint::Properties::createShared(
dynamics::Joint::Properties("root", body->second.initTransform));
rootJoint.type = "free";
if (options.mDefaultRootJointType == RootJointType::FLOATING)
{
// If a root FreeJoint is needed for the parent of the current joint,
// then create it
rootJoint.properties = dynamics::FreeJoint::Properties::createShared(
dynamics::Joint::Properties("root", body->second.initTransform));
rootJoint.type = "free";
}
else if (options.mDefaultRootJointType == RootJointType::FIXED)
{
// If a root WeldJoint is needed for the parent of the current joint,
// then create it
rootJoint.properties = dynamics::WeldJoint::Properties::createShared(
dynamics::Joint::Properties("root", body->second.initTransform));
rootJoint.type = "fixed";
}
else
{
dtwarn << "Unsupported root joint type ["
<< static_cast<int>(options.mDefaultRootJointType)
<< "]. Using FLOATING by default.\n";
}

if (!createPair(newSkeleton, nullptr, rootJoint, body->second))
break;
Expand All @@ -481,7 +527,8 @@ dynamics::SkeletonPtr readSkeleton(

// Read aspects here since aspects cannot be added if the BodyNodes haven't
// created yet.
readAspects(newSkeleton, skeletonElement, baseUri, retriever);
readAspects(
newSkeleton, skeletonElement, baseUri, options.mResourceRetriever);

// Set positions to their initial values
newSkeleton->resetPositions();
Expand Down Expand Up @@ -533,7 +580,7 @@ NextResult getNextJointAndNodePair(
parentJoint = sdfJoints.find(body->first);
if (parentJoint == sdfJoints.end())
{
return CREATE_FREEJOINT_ROOT;
return CREATE_ROOT_JOINT;
}

const std::string& parentBodyName = parentJoint->second.parentName;
Expand Down
38 changes: 36 additions & 2 deletions dart/utils/sdf/SdfParser.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,18 +43,52 @@ namespace utils {

namespace SdfParser {

/// Root joint type to be used when the parent joint of the root link is not
/// specified in the URDF file.
enum class RootJointType
{
/// Floating joint type of URDF.
FLOATING = 0,

/// Fixed joint type of URDF.
FIXED = 1,
};

struct Options
{
/// Resource retriever. LocalResourceRetriever is used if it's nullptr.
common::ResourceRetrieverPtr mResourceRetriever;

/// Default root joint type to be used when the parent joint of the root
/// link is not specified in the URDF file.
RootJointType mDefaultRootJointType;

/// Default constructor
Options(
common::ResourceRetrieverPtr resourceRetriever = nullptr,
RootJointType defaultRootJointType = RootJointType::FLOATING);
};

DART_DEPRECATED(6.0)
simulation::WorldPtr readSdfFile(
const common::Uri& uri,
const common::ResourceRetrieverPtr& retriever = nullptr);

simulation::WorldPtr readWorld(
const common::Uri& uri, const Options& options = Options());

DART_DEPRECATED(6.12)
simulation::WorldPtr readWorld(
const common::Uri& uri,
const common::ResourceRetrieverPtr& retriever = nullptr);
const common::ResourceRetrieverPtr& retriever);

dynamics::SkeletonPtr readSkeleton(
const common::Uri& uri, const Options& options = Options());

DART_DEPRECATED(6.12)
dynamics::SkeletonPtr readSkeleton(
const common::Uri& uri,
const common::ResourceRetrieverPtr& retriever = nullptr);
const common::ResourceRetrieverPtr& retrievers);

} // namespace SdfParser

Expand Down
Loading

0 comments on commit 49eb39f

Please sign in to comment.