Skip to content

Commit

Permalink
urdf: fix test and clean up internals (#1126)
Browse files Browse the repository at this point in the history 
The urdf_gazebo_extensions.urdf file had incorrect kinematics
with link3 being the child of two different joints.
This adds a link4 and replaces joint13 with joint14
connecting link1 and link4. This also fixes some
expectations about the implicit_spring_damper tags.

* parser_urdf: clean up internal functions by removing
unused parameters and making other parameters const.

Signed-off-by: Steve Peters <scpeters@openrobotics.org>
  • Loading branch information
@scpeters
scpeters authored Sep 21, 2022
1 parent 3b82d6a commit b7f0cba
Show file tree
Hide file tree
Showing 3 changed files with 42 additions and 35 deletions.
30 changes: 10 additions & 20 deletions src/parser_urdf.cc
View file
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
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
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

0 comments on commit b7f0cba

Please sign in to comment.