From 7811d7b61fc16a3c1b48fce5c38b7e7d187629b9 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Sat, 13 Mar 2021 23:57:58 -0800 Subject: [PATCH 1/3] dartsim: fdir1 expressed_in frame from SDFormat There is a new API in dartsim 6.10 that allows specifying the frame in which the fdir1 vector is expressed. This is not yet part of the SDFormat specification, but for now, look for link names (body nodes) that match the value of this namespaced //fdir1/@gazebo:expressed_in attribute. Signed-off-by: Steve Peters --- dartsim/src/SDFFeatures.cc | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/dartsim/src/SDFFeatures.cc b/dartsim/src/SDFFeatures.cc index 6ee918f8b..81d2302e0 100644 --- a/dartsim/src/SDFFeatures.cc +++ b/dartsim/src/SDFFeatures.cc @@ -760,8 +760,18 @@ Identity SDFFeatures::ConstructSdfCollision( } if (odeFriction->HasElement("fdir1")) { - math::Vector3d fdir1 = odeFriction->Get("fdir1"); + auto frictionDirectionElem = odeFriction->GetElement("fdir1"); + math::Vector3d fdir1 = frictionDirectionElem->Get(); aspect->setFirstFrictionDirection(math::eigen3::convert(fdir1)); + + const std::string kExpressedIn = "gazebo:expressed_in"; + if (frictionDirectionElem->HasAttribute(kExpressedIn)) + { + auto skeleton = bn->getSkeleton(); + auto directionFrameBodyNode = skeleton->getBodyNode( + frictionDirectionElem->Get(kExpressedIn)); + aspect->setFirstFrictionDirectionFrame(directionFrameBodyNode); + } } const auto &surfaceBounce = _collision.Element() From 09c23a1ad8ecff7e545d685c4ed4495c69c23561 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Fri, 19 Mar 2021 09:44:51 -0700 Subject: [PATCH 2/3] Use ignition as xml namespace Signed-off-by: Steve Peters --- dartsim/src/SDFFeatures.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dartsim/src/SDFFeatures.cc b/dartsim/src/SDFFeatures.cc index 81d2302e0..1d2840c8e 100644 --- a/dartsim/src/SDFFeatures.cc +++ b/dartsim/src/SDFFeatures.cc @@ -764,7 +764,7 @@ Identity SDFFeatures::ConstructSdfCollision( math::Vector3d fdir1 = frictionDirectionElem->Get(); aspect->setFirstFrictionDirection(math::eigen3::convert(fdir1)); - const std::string kExpressedIn = "gazebo:expressed_in"; + const std::string kExpressedIn = "ignition:expressed_in"; if (frictionDirectionElem->HasAttribute(kExpressedIn)) { auto skeleton = bn->getSkeleton(); From 6f61a9262aa5a67f515859b338be3d1947db5a03 Mon Sep 17 00:00:00 2001 From: Louise Poubel Date: Wed, 24 Mar 2021 05:39:40 -0700 Subject: [PATCH 3/3] Check if there's a bodynode Signed-off-by: Louise Poubel --- dartsim/src/SDFFeatures.cc | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/dartsim/src/SDFFeatures.cc b/dartsim/src/SDFFeatures.cc index 4e6ccbde0..4763a49e0 100644 --- a/dartsim/src/SDFFeatures.cc +++ b/dartsim/src/SDFFeatures.cc @@ -783,7 +783,15 @@ Identity SDFFeatures::ConstructSdfCollision( auto skeleton = bn->getSkeleton(); auto directionFrameBodyNode = skeleton->getBodyNode( frictionDirectionElem->Get(kExpressedIn)); - aspect->setFirstFrictionDirectionFrame(directionFrameBodyNode); + if (nullptr != directionFrameBodyNode) + { + aspect->setFirstFrictionDirectionFrame(directionFrameBodyNode); + } + else + { + ignwarn << "Failed to get body node for [" << _collision.Name() + << "], not setting friction direction frame." << std::endl; + } } }