Skip to content

Commit

Permalink
Use new Joint APIs for Parent/Child name (#361)
Browse files Browse the repository at this point in the history 
Follow-up to gazebosim/sdformat#1053.

Signed-off-by: Steve Peters <scpeters@openrobotics.org>
  • Loading branch information
@scpeters
scpeters authored Jun 21, 2022
1 parent a950315 commit 9e7af39
Show file tree
Hide file tree
Showing 4 changed files with 30 additions and 30 deletions.
8 changes: 4 additions & 4 deletions bullet/src/SDFFeatures.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -127,10 +127,10 @@ Identity SDFFeatures::ConstructSdfModel(
}

const std::size_t parent = this->FindOrConstructLink(
modelIdentity, _sdfModel, sdfJoint->ParentLinkName());
modelIdentity, _sdfModel, sdfJoint->ParentName());

const std::size_t child = this->FindOrConstructLink(
modelIdentity, _sdfModel, sdfJoint->ChildLinkName());
modelIdentity, _sdfModel, sdfJoint->ChildName());

this->ConstructSdfJoint(modelIdentity, *sdfJoint, parent, child);
}
Expand Down Expand Up @@ -329,11 +329,11 @@ Identity SDFFeatures::ConstructSdfJoint(
const ::sdf::Model dummyEmptyModel;

// Get the parent and child ids
const std::string parentLinkName = _sdfJoint.ParentLinkName();
const std::string parentLinkName = _sdfJoint.ParentName();
std::size_t parentId =
this->FindOrConstructLink(_modelID, dummyEmptyModel, parentLinkName);

const std::string childLinkName = _sdfJoint.ChildLinkName();
const std::string childLinkName = _sdfJoint.ChildName();
std::size_t childId =
this->FindOrConstructLink(_modelID, dummyEmptyModel, childLinkName);

Expand Down
8 changes: 4 additions & 4 deletions bullet/src/SDFFeatures_TEST.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -78,22 +78,22 @@ auto CreateTestModel(WorldPtr _world, const std::string &_model,
{
auto parent = model->ConstructLink(*_parentLink);
EXPECT_NE(nullptr, parent);
sdfJoint.SetParentLinkName(_parentLink->Name());
sdfJoint.SetParentName(_parentLink->Name());
}
else
{
sdfJoint.SetParentLinkName("world");
sdfJoint.SetParentName("world");
}

if (_childLink)
{
auto child = model->ConstructLink(*_childLink);
EXPECT_NE(nullptr, child);
sdfJoint.SetChildLinkName(_childLink->Name());
sdfJoint.SetChildName(_childLink->Name());
}
else
{
sdfJoint.SetChildLinkName("world");
sdfJoint.SetChildName("world");
}

auto joint0 = model->ConstructJoint(sdfJoint);
Expand Down
32 changes: 16 additions & 16 deletions dartsim/src/SDFFeatures.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -507,7 +507,7 @@ Identity SDFFeatures::ConstructSdfModelImpl(
::sdf::Errors errors = sdfJoint->ResolveParentLink(parentLinkName);
if (!errors.empty())
{
gzerr << "The link of the parent frame [" << sdfJoint->ParentLinkName()
gzerr << "The link of the parent frame [" << sdfJoint->ParentName()
<< "] of joint [" << sdfJoint->Name() << "] in model ["
<< modelName
<< "] could not be resolved. The joint will not be constructed\n";
Expand All @@ -521,7 +521,7 @@ Identity SDFFeatures::ConstructSdfModelImpl(
errors = sdfJoint->ResolveChildLink(childLinkName);
if (!errors.empty())
{
gzerr << "The link of the child frame [" << sdfJoint->ChildLinkName()
gzerr << "The link of the child frame [" << sdfJoint->ChildName()
<< "] of joint [" << sdfJoint->Name() << "] in model ["
<< modelName
<< "] could not be resolved. The joint will not be constructed\n";
Expand All @@ -536,7 +536,7 @@ Identity SDFFeatures::ConstructSdfModelImpl(
if (nullptr == parentSdfLink && parentLinkName != "world")
{
gzerr << "The link [" << parentLinkName << "] of the parent frame ["
<< sdfJoint->ParentLinkName() << "] of joint [" << sdfJoint->Name()
<< sdfJoint->ParentName() << "] of joint [" << sdfJoint->Name()
<< "] in model [" << modelName
<< "] could not be resolved. The joint will not be constructed\n";
continue;
Expand All @@ -546,7 +546,7 @@ Identity SDFFeatures::ConstructSdfModelImpl(
if (nullptr == childSdfLink)
{
gzerr << "The link [" << childLinkName << "] of the child frame ["
<< sdfJoint->ChildLinkName() << "] of joint [" << sdfJoint->Name()
<< sdfJoint->ChildName() << "] of joint [" << sdfJoint->Name()
<< "] in model [" << modelName
<< "] could not be resolved. The joint will not be constructed\n";
continue;
Expand All @@ -557,7 +557,7 @@ Identity SDFFeatures::ConstructSdfModelImpl(

if (nullptr == parent && parentLinkName != "world")
{
gzerr << "The parent [" << sdfJoint->ParentLinkName() << "] of joint ["
gzerr << "The parent [" << sdfJoint->ParentName() << "] of joint ["
<< sdfJoint->Name() << "] in model [" << modelName
<< "] was not found. The joint will not be constructed\n";
continue;
Expand Down Expand Up @@ -684,7 +684,7 @@ Identity SDFFeatures::ConstructSdfJoint(
{
const auto &modelInfo = *this->ReferenceInterface<ModelInfo>(_modelID);

if (_sdfJoint.ChildLinkName() == "world")
if (_sdfJoint.ChildName() == "world")
{
gzerr << "Asked to create a joint with the world as the child in model "
<< "[" << modelInfo.model->getName() << "]. This is currently not "
Expand All @@ -707,7 +707,7 @@ Identity SDFFeatures::ConstructSdfJoint(
// Since (1) is an error that should be reported and (2) might be a valid use
// case, the solution is, when an error is encountered, we first assume (2)
// and set the parent and child link names to whatever returned from
// sdf::Joint::ParentLinkName() and sdf::Joint::ChildLinkName respectively.
// sdf::Joint::ParentName() and sdf::Joint::ChildName respectively.
// Then we check if a body node with the same relative name exists in DART. If
// the link is nested inside a child model, it will be necessary to split the
// name to identify the correct parent skeleton. If the corresponding body
Expand All @@ -720,9 +720,9 @@ Identity SDFFeatures::ConstructSdfJoint(
const auto resolveParentErrors = _sdfJoint.ResolveParentLink(parentLinkName);
if (!resolveParentErrors.empty()) {
// It's possible this wasn't created from an sdf::Model object, like
// SDFFeatures_TEST.WorldIsParentOrChild. Try using raw ParentLinkName() in
// SDFFeatures_TEST.WorldIsParentOrChild. Try using raw ParentName() in
// that case
parentLinkName = _sdfJoint.ParentLinkName();
parentLinkName = _sdfJoint.ParentName();
}

dart::dynamics::BodyNode * const parent =
Expand All @@ -731,23 +731,23 @@ Identity SDFFeatures::ConstructSdfJoint(
std::string childLinkName;
const auto childResolveErrors = _sdfJoint.ResolveChildLink(childLinkName);
if (!childResolveErrors.empty()) {
childLinkName = _sdfJoint.ChildLinkName();
childLinkName = _sdfJoint.ChildName();
}

dart::dynamics::BodyNode * const child =
FindBodyNode(world->getName(), modelInfo.model->getName(), childLinkName);

if (nullptr == parent && parentLinkName != "world")
{
gzerr << "The link of the parent frame [" << _sdfJoint.ParentLinkName()
gzerr << "The link of the parent frame [" << _sdfJoint.ParentName()
<< "] with resolved link name [" << parentLinkName
<< "] of joint [" << _sdfJoint.Name()
<< "] could not be resolved. The joint will not be constructed\n";
return this->GenerateInvalidId();
}
if (nullptr == child)
{
gzerr << "The link of the child frame [" << _sdfJoint.ChildLinkName()
gzerr << "The link of the child frame [" << _sdfJoint.ChildName()
<< "] with resolved link name [" << childLinkName
<< "] of joint [" << _sdfJoint.Name() << "] in model ["
<< modelInfo.model->getName()
Expand Down Expand Up @@ -978,8 +978,8 @@ Identity SDFFeatures::ConstructSdfJoint(
{
// if a specified link is named "world" but cannot be found, we'll assume the
// joint is connected to the world
bool worldParent = (!_parent && _sdfJoint.ParentLinkName() == "world");
bool worldChild = (!_child && _sdfJoint.ChildLinkName() == "world");
bool worldParent = (!_parent && _sdfJoint.ParentName() == "world");
bool worldChild = (!_child && _sdfJoint.ChildName() == "world");

if (worldChild)
{
Expand All @@ -996,8 +996,8 @@ Identity SDFFeatures::ConstructSdfJoint(
{
{
std::stringstream msg;
msg << "Asked to create a joint from link [" << _sdfJoint.ParentLinkName()
<< "] to link [" << _sdfJoint.ChildLinkName() << "] in the model "
msg << "Asked to create a joint from link [" << _sdfJoint.ParentName()
<< "] to link [" << _sdfJoint.ChildName() << "] in the model "
<< "[" << _modelInfo.model->getName() << "], but ";

if (!_parent && !worldParent)
Expand Down
12 changes: 6 additions & 6 deletions dartsim/src/SDFFeatures_TEST.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -272,8 +272,8 @@ WorldPtr LoadWorldPiecemeal(const std::string &_world)
newSdfJoint.SetRawPose(ResolveSdfPose(sdfJoint->SemanticPose()));
newSdfJoint.SetThreadPitch(sdfJoint->ThreadPitch());

newSdfJoint.SetParentLinkName(resolvedParentLinkName);
newSdfJoint.SetChildLinkName(resolvedChildLinkName);
newSdfJoint.SetParentName(resolvedParentLinkName);
newSdfJoint.SetChildName(resolvedChildLinkName);

physModel->ConstructJoint(newSdfJoint);
}
Expand Down Expand Up @@ -473,22 +473,22 @@ auto CreateTestModel(WorldPtr _world, const std::string &_model,
{
auto parent = model->ConstructLink(*_parentLink);
EXPECT_NE(nullptr, parent);
sdfJoint.SetParentLinkName(_parentLink->Name());
sdfJoint.SetParentName(_parentLink->Name());
}
else
{
sdfJoint.SetParentLinkName("world");
sdfJoint.SetParentName("world");
}

if (_childLink)
{
auto child = model->ConstructLink(*_childLink);
EXPECT_NE(nullptr, child);
sdfJoint.SetChildLinkName(_childLink->Name());
sdfJoint.SetChildName(_childLink->Name());
}
else
{
sdfJoint.SetChildLinkName("world");
sdfJoint.SetChildName("world");
}

auto joint0 = model->ConstructJoint(sdfJoint);
Expand Down

0 comments on commit 9e7af39

Please sign in to comment.