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

Jump to bottom

Add option for parsing root joint type without world link #1399

Merged
merged 4 commits into from
Aug 17, 2019
Merged

Add option for parsing root joint type without world link #1399

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
477342c
Add option for parsing root joint type without world link
jslee02 Aug 16, 2019
3cfa2b0
Add comment on the flags
jslee02 Aug 16, 2019
7954f87
Merge branch 'master' into feature/urdf_parser
jslee02 Aug 16, 2019
0b13b0e
Update changelog
jslee02 Aug 17, 2019
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
3 changes: 2 additions & 1 deletion CHANGELOG.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,7 @@

* Dynamics

* Fixex friction and restitution of individual shapes in a body: [#1369](https://github.com/dartsim/dart/pull/1369)
* Fixed friction and restitution of individual shapes in a body: [#1369](https://github.com/dartsim/dart/pull/1369)
* Fixed soft body simulation when command input is not resetted: [#1372](https://github.com/dartsim/dart/pull/1372)

* GUI
Expand All @@ -18,6 +18,7 @@

* Allowed parsing SDF up to version 1.6: [#1385](https://github.com/dartsim/dart/pull/1385)
* Fixed SDF parser not creating dynamics aspect for collision shape: [#1386](https://github.com/dartsim/dart/pull/1386)
* Added root joint parsing option in URDF parser: [#1399](https://github.com/dartsim/dart/pull/1399)

* dartpy

Expand Down
4 changes: 2 additions & 2 deletions dart/dynamics/SharedLibraryIkFast.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -56,8 +56,8 @@ bool loadFunction(

if (!symbol)
{
dterr << "Failed to load the symbol '" << symbolName
<< "' from the file '" << fileName << "'.\n";
dterr << "Failed to load the symbol '" << symbolName << "' from the file '"
<< fileName << "'.\n";
return false;
}

Expand Down
139 changes: 83 additions & 56 deletions dart/utils/urdf/DartLoader.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -80,7 +80,8 @@ void DartLoader::addPackageDirectory(

dynamics::SkeletonPtr DartLoader::parseSkeleton(
const common::Uri& _uri,
const common::ResourceRetrieverPtr& _resourceRetriever)
const common::ResourceRetrieverPtr& _resourceRetriever,
unsigned int flags)
{
const common::ResourceRetrieverPtr resourceRetriever
= getResourceRetriever(_resourceRetriever);
Expand All @@ -98,13 +99,15 @@ dynamics::SkeletonPtr DartLoader::parseSkeleton(
return nullptr;
}

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

dynamics::SkeletonPtr DartLoader::parseSkeletonString(
const std::string& _urdfString,
const common::Uri& _baseUri,
const common::ResourceRetrieverPtr& _resourceRetriever)
const common::ResourceRetrieverPtr& _resourceRetriever,
unsigned int flags)
{
if (_urdfString.empty())
{
Expand All @@ -121,12 +124,16 @@ dynamics::SkeletonPtr DartLoader::parseSkeletonString(
}

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

simulation::WorldPtr DartLoader::parseWorld(
const common::Uri& _uri,
const common::ResourceRetrieverPtr& _resourceRetriever)
const common::ResourceRetrieverPtr& _resourceRetriever,
unsigned int flags)
{
const common::ResourceRetrieverPtr resourceRetriever
= getResourceRetriever(_resourceRetriever);
Expand All @@ -135,13 +142,14 @@ simulation::WorldPtr DartLoader::parseWorld(
if (!readFileToString(resourceRetriever, _uri, content))
return nullptr;

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

simulation::WorldPtr DartLoader::parseWorldString(
const std::string& _urdfString,
const common::Uri& _baseUri,
const common::ResourceRetrieverPtr& _resourceRetriever)
const common::ResourceRetrieverPtr& _resourceRetriever,
unsigned int flags)
{
const common::ResourceRetrieverPtr resourceRetriever
= getResourceRetriever(_resourceRetriever);
Expand All @@ -168,7 +176,7 @@ simulation::WorldPtr DartLoader::parseWorldString(
{
const urdf_parsing::Entity& entity = worldInterface->models[i];
dynamics::SkeletonPtr skeleton = modelInterfaceToSkeleton(
entity.model.get(), entity.uri, resourceRetriever);
entity.model.get(), entity.uri, resourceRetriever, flags);

if (!skeleton)
{
Expand Down Expand Up @@ -198,16 +206,16 @@ simulation::WorldPtr DartLoader::parseWorldString(
dynamics::SkeletonPtr DartLoader::modelInterfaceToSkeleton(
const urdf::ModelInterface* model,
const common::Uri& baseUri,
const common::ResourceRetrieverPtr& resourceRetriever)
const common::ResourceRetrieverPtr& resourceRetriever,
unsigned int flags)
{
dynamics::SkeletonPtr skeleton = dynamics::Skeleton::create(model->getName());

dynamics::BodyNode* rootNode = nullptr;
const urdf::Link* root = model->getRoot().get();

// If the link name is "world" then the link is regarded as the inertial frame
// rather than a body in the robot model so we don't create a BodyNode for it.
// This is not officially specified in the URDF spec, but "world" is
// If the link name is "world" then the link is regarded as the inertial
// frame rather than a body in the robot model so we don't create a BodyNode
// for it. This is not officially specified in the URDF spec, but "world" is
// practically treated as a keyword.
if (root->name == "world")
{
Expand All @@ -219,45 +227,26 @@ dynamics::SkeletonPtr DartLoader::modelInterfaceToSkeleton(
<< "not the URDF standard. Please consider changing the robot "
<< "model as a single tree robot.\n";
}
}
else
{
// If the root link is not "world" then it means the robot model is a free
// floating robot (like humanoid robots). So we create the root body with
// FreeJoint.

dynamics::BodyNode::Properties rootProperties;
if (!createDartNodeProperties(
root, rootProperties, baseUri, resourceRetriever))
for (std::size_t i = 0; i < root->child_links.size(); i++)
{
return nullptr;
if (!createSkeletonRecursive(
model,
skeleton,
root->child_links[i].get(),
nullptr,
baseUri,
resourceRetriever,
flags))
{
return nullptr;
}
}

std::pair<dynamics::Joint*, dynamics::BodyNode*> pair
= skeleton->createJointAndBodyNodePair<dynamics::FreeJoint>(
nullptr,
dynamics::FreeJoint::Properties(
dynamics::GenericJoint<math::SE3Space>::Properties(
dynamics::Joint::Properties("rootJoint"))),
rootProperties);
rootNode = pair.second;

const auto result
= createShapeNodes(model, root, rootNode, baseUri, resourceRetriever);

if (!result)
return nullptr;
}

for (std::size_t i = 0; i < root->child_links.size(); i++)
else
{
if (!createSkeletonRecursive(
model,
skeleton,
root->child_links[i].get(),
rootNode,
baseUri,
resourceRetriever))
model, skeleton, root, nullptr, baseUri, resourceRetriever, flags))
{
return nullptr;
}
Expand All @@ -277,19 +266,23 @@ bool DartLoader::createSkeletonRecursive(
const urdf::Link* lk,
dynamics::BodyNode* parentNode,
const common::Uri& baseUri,
const common::ResourceRetrieverPtr& resourceRetriever)
const common::ResourceRetrieverPtr& resourceRetriever,
unsigned int flags)
{
assert(lk);

if (parentNode != nullptr && lk->name == "world")
{
dtwarn << "[DartLoader] Link name 'world' is reserved for the inertial "
<< "frame. Consider changing the name to something else.\n";
}

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

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

if (!node)
return false;
Expand All @@ -308,7 +301,8 @@ bool DartLoader::createSkeletonRecursive(
lk->child_links[i].get(),
node,
baseUri,
resourceRetriever))
resourceRetriever,
flags))
{
return false;
}
Expand Down Expand Up @@ -382,6 +376,32 @@ bool DartLoader::readFileToString(
return true;
}

dynamics::BodyNode* createDartJointAndNodeForRoot(
const dynamics::BodyNode::Properties& _body,
dynamics::BodyNode* _parent,
dynamics::SkeletonPtr _skeleton,
unsigned int flags)
{
dynamics::Joint::Properties basicProperties;

dynamics::GenericJoint<math::R1Space>::UniqueProperties singleDof;
std::pair<dynamics::Joint*, dynamics::BodyNode*> pair;
if (flags & DartLoader::Flags::FIXED_BASE_LINK)
{
pair = _skeleton->createJointAndBodyNodePair<dynamics::WeldJoint>(
_parent, basicProperties, _body);
}
else
{
dynamics::GenericJoint<math::SE3Space>::Properties properties(
basicProperties);
pair = _skeleton->createJointAndBodyNodePair<dynamics::FreeJoint>(
_parent, properties, _body);
}

return pair.second;
}

/**
* @function createDartJoint
*/
Expand All @@ -390,9 +410,16 @@ dynamics::BodyNode* DartLoader::createDartJointAndNode(
const dynamics::BodyNode::Properties& _body,
dynamics::BodyNode* _parent,
dynamics::SkeletonPtr _skeleton,
const common::Uri& /*_baseUri*/,
const common::ResourceRetrieverPtr& /*_resourceRetriever*/)
unsigned int flags)
{
// 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);
}

dynamics::Joint::Properties basicProperties;

basicProperties.mName = _jt->name;
Expand Down
34 changes: 26 additions & 8 deletions dart/utils/urdf/DartLoader.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -76,6 +76,19 @@ namespace utils {
class DartLoader
{
public:
/// Flags for specifying URDF file parsing policies.
enum Flags
{
NONE = 0,

/// Parser the root link's joint type to be "fixed" joint when not
/// specified.
FIXED_BASE_LINK = 1 << 1,

/// The default flgas
DEFAULT = NONE,
};

/// Constructor with the default ResourceRetriever.
DartLoader();

Expand Down Expand Up @@ -104,24 +117,28 @@ class DartLoader
/// Parse a file to produce a Skeleton
dynamics::SkeletonPtr parseSkeleton(
const common::Uri& _uri,
const common::ResourceRetrieverPtr& _resourceRetriever = nullptr);
const common::ResourceRetrieverPtr& _resourceRetriever = nullptr,
unsigned int flags = DEFAULT);

/// Parse a text string to produce a Skeleton
dynamics::SkeletonPtr parseSkeletonString(
const std::string& _urdfString,
const common::Uri& _baseUri,
const common::ResourceRetrieverPtr& _resourceRetriever = nullptr);
const common::ResourceRetrieverPtr& _resourceRetriever = nullptr,
unsigned int flags = DEFAULT);

/// Parse a file to produce a World
dart::simulation::WorldPtr parseWorld(
const common::Uri& _uri,
const common::ResourceRetrieverPtr& _resourceRetriever = nullptr);
const common::ResourceRetrieverPtr& _resourceRetriever = nullptr,
unsigned int flags = DEFAULT);

/// Parse a text string to produce a World
dart::simulation::WorldPtr parseWorldString(
const std::string& _urdfString,
const common::Uri& _baseUri,
const common::ResourceRetrieverPtr& _resourceRetriever = nullptr);
const common::ResourceRetrieverPtr& _resourceRetriever = nullptr,
unsigned int flags = DEFAULT);

private:
typedef std::shared_ptr<dynamics::BodyNode::Properties> BodyPropPtr;
Expand All @@ -131,15 +148,17 @@ class DartLoader
static dart::dynamics::SkeletonPtr modelInterfaceToSkeleton(
const urdf::ModelInterface* model,
const common::Uri& baseUri,
const common::ResourceRetrieverPtr& resourceRetriever);
const common::ResourceRetrieverPtr& resourceRetriever,
unsigned int flags);

static bool createSkeletonRecursive(
const urdf::ModelInterface* model,
dynamics::SkeletonPtr skel,
const urdf::Link* lk,
dynamics::BodyNode* parent,
const common::Uri& baseUri,
const common::ResourceRetrieverPtr& _resourceRetriever);
const common::ResourceRetrieverPtr& _resourceRetriever,
unsigned int flags);

static bool addMimicJointsRecursive(
const urdf::ModelInterface* model,
Expand All @@ -157,8 +176,7 @@ class DartLoader
const dynamics::BodyNode::Properties& _body,
dynamics::BodyNode* _parent,
dynamics::SkeletonPtr _skeleton,
const common::Uri& _baseUri,
const common::ResourceRetrieverPtr& _resourceRetriever);
unsigned int flgas);

static bool createDartNodeProperties(
const urdf::Link* _lk,
Expand Down
Loading