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

urdf: fix test and clean up internals #1126

Merged
merged 3 commits into from
Sep 21, 2022
Merged

urdf: fix test and clean up internals #1126

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
afa24f3
urdf extension test: fix URDF kinematics
scpeters Sep 2, 2022
bffd379
Merge branch 'sdf9' into scpeters/fix_urdf_gz_ext_test
scpeters Sep 13, 2022
90595c0
parser_urdf: clean up internal functions
scpeters Sep 7, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
30 changes: 10 additions & 20 deletions src/parser_urdf.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -123,7 +123,7 @@ void CreateVisual(TiXmlElement *_elem, urdf::LinkConstSharedPtr _link,

/// create SDF Joint block based on URDF
void CreateJoint(TiXmlElement *_root, urdf::LinkConstSharedPtr _link,
ignition::math::Pose3d &_currentTransform);
const ignition::math::Pose3d &_currentTransform);

/// insert extensions into links
void InsertSDFExtensionLink(TiXmlElement *_elem, const std::string &_linkName);
Expand All @@ -142,12 +142,11 @@ void AddTransform(TiXmlElement *_elem,
const ignition::math::Pose3d &_transform);

/// create SDF from URDF link
void CreateSDF(TiXmlElement *_root, urdf::LinkConstSharedPtr _link,
const ignition::math::Pose3d &_transform);
void CreateSDF(TiXmlElement *_root, urdf::LinkConstSharedPtr _link);

/// create SDF Link block based on URDF
void CreateLink(TiXmlElement *_root, urdf::LinkConstSharedPtr _link,
ignition::math::Pose3d &_currentTransform);
const ignition::math::Pose3d &_currentTransform);

/// reduced fixed joints: apply appropriate frame updates in joint
/// inside urdf extensions when doing fixed joint reduction
Expand Down Expand Up @@ -1025,12 +1024,10 @@ void ReduceJointsToParent(urdf::LinkSharedPtr _link)
{
// go down the tree until we hit a parent joint that is not fixed
urdf::LinkSharedPtr newParentLink = _link;
ignition::math::Pose3d jointAnchorTransform;
while (newParentLink->parent_joint &&
newParentLink->getParent()->name != "world" &&
FixedJointShouldBeReduced(newParentLink->parent_joint) )
{
jointAnchorTransform = jointAnchorTransform * jointAnchorTransform;
parentJoint->parent_to_joint_origin_transform =
TransformToParentFrame(
parentJoint->parent_to_joint_origin_transform,
Expand Down Expand Up @@ -2606,11 +2603,8 @@ void URDF2SDF::ListSDFExtensions(const std::string &_reference)

////////////////////////////////////////////////////////////////////////////////
void CreateSDF(TiXmlElement *_root,
urdf::LinkConstSharedPtr _link,
const ignition::math::Pose3d &_transform)
urdf::LinkConstSharedPtr _link)
{
ignition::math::Pose3d _currentTransform = _transform;

// must have an <inertial> block and cannot have zero mass.
// allow det(I) == zero, in the case of point mass geoms.
// @todo: keyword "world" should be a constant defined somewhere else
Expand Down Expand Up @@ -2652,13 +2646,13 @@ void CreateSDF(TiXmlElement *_root,
(!_link->parent_joint ||
!FixedJointShouldBeReduced(_link->parent_joint)))
{
CreateLink(_root, _link, _currentTransform);
CreateLink(_root, _link, ignition::math::Pose3d::Zero);
}

// recurse into children
for (unsigned int i = 0 ; i < _link->child_links.size() ; ++i)
{
CreateSDF(_root, _link->child_links[i], _currentTransform);
CreateSDF(_root, _link->child_links[i]);
}
}

Expand Down Expand Up @@ -2693,7 +2687,7 @@ urdf::Pose CopyPose(ignition::math::Pose3d _pose)
////////////////////////////////////////////////////////////////////////////////
void CreateLink(TiXmlElement *_root,
urdf::LinkConstSharedPtr _link,
ignition::math::Pose3d &_currentTransform)
const ignition::math::Pose3d &_currentTransform)
{
// create new body
TiXmlElement *elem = new TiXmlElement("link");
Expand Down Expand Up @@ -2870,7 +2864,7 @@ void CreateInertial(TiXmlElement *_elem,
////////////////////////////////////////////////////////////////////////////////
void CreateJoint(TiXmlElement *_root,
urdf::LinkConstSharedPtr _link,
ignition::math::Pose3d &/*_currentTransform*/)
const ignition::math::Pose3d &/*_currentTransform*/)
{
// compute the joint tag
std::string jtype;
Expand Down Expand Up @@ -3150,10 +3144,6 @@ TiXmlDocument URDF2SDF::InitModelString(const std::string &_urdfStr,
return sdfXmlOut;
}

// initialize transform for the model, urdf is recursive,
// while sdf defines all links relative to model frame
ignition::math::Pose3d transform;

// parse sdf extension
TiXmlDocument urdfXml;
urdfXml.Parse(_urdfStr.c_str());
Expand Down Expand Up @@ -3196,13 +3186,13 @@ TiXmlDocument URDF2SDF::InitModelString(const std::string &_urdfStr,
child = rootLink->child_links.begin();
child != rootLink->child_links.end(); ++child)
{
CreateSDF(robot, (*child), transform);
CreateSDF(robot, (*child));
}
}
else
{
// convert, starting from root link
CreateSDF(robot, rootLink, transform);
CreateSDF(robot, rootLink);
}

// insert the extensions without reference into <robot> root level
Expand Down
17 changes: 11 additions & 6 deletions test/integration/urdf_gazebo_extensions.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -86,17 +86,22 @@ TEST(SDFParser, UrdfGazeboExtensionURDFTest)
else if (jointName == "joint12")
{
// cfmDamping not provided
ASSERT_TRUE(joint->HasElement("physics"));
EXPECT_TRUE(joint->HasElement("physics"));
sdf::ElementPtr physics = joint->GetElement("physics");
EXPECT_FALSE(physics->HasElement("implicit_spring_damper"));
EXPECT_TRUE(physics->HasElement("ode"));
sdf::ElementPtr ode = physics->GetElement("ode");
EXPECT_TRUE(ode->HasElement("implicit_spring_damper"));
EXPECT_FALSE(ode->Get<bool>("implicit_spring_damper"));
}
else if (jointName == "joint13")
else if (jointName == "joint14")
{
// implicitSpringDamper = 1
ASSERT_TRUE(joint->HasElement("physics"));
EXPECT_TRUE(joint->HasElement("physics"));
sdf::ElementPtr physics = joint->GetElement("physics");
ASSERT_TRUE(physics->HasElement("implicit_spring_damper"));
EXPECT_TRUE(physics->Get<bool>("implicit_spring_damper"));
EXPECT_TRUE(physics->HasElement("ode"));
sdf::ElementPtr ode = physics->GetElement("ode");
EXPECT_TRUE(ode->HasElement("implicit_spring_damper"));
EXPECT_TRUE(ode->Get<bool>("implicit_spring_damper"));
}
}

Expand Down
30 changes: 21 additions & 9 deletions test/integration/urdf_gazebo_extensions.urdf
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -95,14 +95,6 @@
<dampingFactor>0.1</dampingFactor>
</gazebo>

<joint name="joint13" type="revolute">
<origin rpy="-1.57079632679 0 -1.57079632679" xyz="0 -3.0 0.0"/>
<axis xyz="0 1 0"/> <!-- in child (link1) frame -->
<limit lower="0" upper="0" velocity="1000" effort="10000"/>
<parent link="link1"/>
<child link="link3"/>
</joint>

<joint name="joint23" type="revolute">
<origin rpy="-1.57079632679 0 -1.57079632679" xyz="-0.5 0.5 2.5"/>
<axis xyz="0 1 0"/> <!-- in child (link1) frame -->
Expand All @@ -125,7 +117,27 @@
</visual>
</link>

<gazebo reference="joint13">
<joint name="joint14" type="revolute">
<origin rpy="-1.57079632679 0 -1.57079632679" xyz="0 -3.0 0.0"/>
<axis xyz="0 1 0"/> <!-- in child (link1) frame -->
<limit lower="0" upper="0" velocity="1000" effort="10000"/>
<parent link="link1"/>
<child link="link4"/>
</joint>
<link name="link4">
<inertial>
<mass value="400"/>
<origin rpy="2 3 4" xyz="0.1 0.2 0.3"/>
<inertia ixx="8" ixy="0" ixz="0" iyy="9" iyz="0" izz="10"/>
</inertial>
<visual>
<geometry>
<box size="0.3 0.4 0.5"/>
</geometry>
<origin rpy="2 3 4" xyz="0.1 0.2 0.3"/>
</visual>
</link>
<gazebo reference="joint14">
<implicitSpringDamper>true</implicitSpringDamper>
</gazebo>

Expand Down