Skip to content

Commit 1a05444

Browse files
author
Addisu Z. Taddese
authored
Merge pull request #738 from ignitionrobotics/chapulina/6_to_9
6 ➡️ 9
2 parents f805c95 + 38be09c commit 1a05444

17 files changed

Lines changed: 370 additions & 88 deletions

‎.github/CODEOWNERS‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,4 @@
1+
# More info:
2+
# https://help.github.com/en/github/creating-cloning-and-archiving-repositories/about-code-owners
3+
4+
* @azeey @scpeters

‎.github/workflows/ci.yml‎

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -23,4 +23,3 @@ jobs:
2323
- name: Compile and test
2424
id: ci
2525
uses: ignition-tooling/action-ignition-ci@focal
26-

‎Changelog.md‎

Lines changed: 15 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -749,7 +749,21 @@
749749

750750
## SDFormat 6.0
751751

752-
### SDFormat 6.X.X (20XX-XX-XX)
752+
### SDFormat 6.3.1 (2021-07-06)
753+
754+
1. Fix flattening logic for nested model names
755+
* [Pull request 597](https://github.com/osrf/sdformat/pull/597)
756+
757+
1. Translate poses of nested models inside other nested models
758+
* [Pull request 596](https://github.com/osrf/sdformat/pull/596)
759+
760+
### SDFormat 6.3.0 (2021-06-21)
761+
762+
1. Move recursiveSameTypeUniqueNames from ign.cc to parser.cc and make public.
763+
* [Pull request 580](https://github.com/osrf/sdformat/pull/580)
764+
765+
1. Parse rpyOffset as radians
766+
* [Pull request 497](https://github.com/osrf/sdformat/pull/497)
753767

754768
1. Parse urdf files to sdf 1.5 instead of 1.4 to avoid `use_parent_model_frame`.
755769
* [BitBucket pull request 575](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/575)

‎Migration.md‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -145,6 +145,11 @@ but with improved human-readability..
145145

146146
## SDFormat 5.x to 6.x
147147

148+
### Additions
149+
150+
1. **sdf/parser.hh**
151+
+ bool recursiveSameTypeUniqueNames(sdf::ElementPtr)
152+
148153
### Deprecations
149154

150155
1. **sdf/Types.hh**

‎bitbucket-pipelines.yml‎

Lines changed: 0 additions & 47 deletions
This file was deleted.

‎doc/header.html‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -41,7 +41,7 @@ <h2 style="text-align:center;">
4141
<dd><a href="http://sdf.com/wiki/Tutorials">Tutorials</a></dd>
4242
<dd><a href="http://sdf.com/downloads.html">Download</a></dd>
4343
-->
44-
<dd><a href="https://github.com/osrf/sdformat/issues/new">Report Documentation Issues</a></dd>
44+
<dd><a href="https://github.com/ignitionrobotics/sdformat/issues/new">Report Documentation Issues</a></dd>
4545
</dl>
4646
</div>
4747
<div>

‎doc/mainpage.html‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -5,7 +5,7 @@
55
Desctiption Format API. The code reference is divided into the groups below.
66
Should you find problems with this documentation - typos, unclear phrases,
77
or insufficient detail - please create a <a
8-
href="https://github.com/osrf/sdformat/issues/new">new GitHub issue</a>.
8+
href="https://github.com/ignitionrobotics/sdformat/issues/new">new GitHub issue</a>.
99
Include sufficient detail to quickly locate the problematic documentation,
1010
and set the issue's fields accordingly: Assignee - blank; Kind - bug;
1111
Priority - minor; Version - blank.

‎src/parser.cc‎

Lines changed: 12 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -1358,12 +1358,18 @@ void addNestedModel(ElementPtr _sdf, ElementPtr _includeSDF, Errors &_errors)
13581358
for (std::map<std::string, std::string>::iterator iter = replace.begin();
13591359
iter != replace.end(); ++iter)
13601360
{
1361-
replace_all(str, std::string("\"") + iter->first + "\"",
1362-
std::string("\"") + iter->second + "\"");
1363-
replace_all(str, std::string("'") + iter->first + "'",
1364-
std::string("'") + iter->second + "'");
1365-
replace_all(str, std::string(">") + iter->first + "<",
1366-
std::string(">") + iter->second + "<");
1361+
std::string oldName(iter->first);
1362+
std::string nameWithNestedPrefix(iter->second);
1363+
replace_all(str, std::string("\"") + oldName + "\"",
1364+
std::string("\"") + nameWithNestedPrefix + "\"");
1365+
replace_all(str, std::string("'") + oldName + "'",
1366+
std::string("'") + nameWithNestedPrefix + "'");
1367+
replace_all(str, std::string(">") + oldName + "<",
1368+
std::string(">") + nameWithNestedPrefix + "<");
1369+
// Deal with nested model inside other nested model and already with
1370+
// ::namespace:: entries in the name
1371+
replace_all(str, std::string(">") + oldName + "::",
1372+
std::string(">") + nameWithNestedPrefix + "::");
13671373
}
13681374

13691375
_includeSDF->ClearElements();

‎src/parser_urdf.cc‎

Lines changed: 12 additions & 31 deletions
Original file line numberDiff line numberDiff line change
@@ -221,10 +221,6 @@ std::string Values2str(unsigned int _count, const double *_values);
221221

222222
void CreateGeometry(TiXmlElement *_elem, urdf::GeometrySharedPtr _geometry);
223223

224-
ignition::math::Pose3d inverseTransformToParentFrame(
225-
ignition::math::Pose3d _transformInLinkFrame,
226-
urdf::Pose _parentToLinkTransform);
227-
228224
/// reduced fixed joints: transform to parent frame
229225
urdf::Pose TransformToParentFrame(urdf::Pose _transformInLinkFrame,
230226
urdf::Pose _parentToLinkTransform);
@@ -2429,31 +2425,6 @@ ignition::math::Pose3d TransformToParentFrame(
24292425
return transformInParentLinkFrame;
24302426
}
24312427

2432-
/////////////////////////////////////////////////
2433-
/// reduced fixed joints: transform to parent frame
2434-
ignition::math::Pose3d inverseTransformToParentFrame(
2435-
ignition::math::Pose3d _transformInLinkFrame,
2436-
urdf::Pose _parentToLinkTransform)
2437-
{
2438-
ignition::math::Pose3d transformInParentLinkFrame;
2439-
// rotate link pose to parentLink frame
2440-
urdf::Rotation ri = _parentToLinkTransform.rotation.GetInverse();
2441-
ignition::math::Quaterniond q1(ri.w, ri.x, ri.y, ri.z);
2442-
transformInParentLinkFrame.Pos() = q1 * _transformInLinkFrame.Pos();
2443-
urdf::Rotation r2 = _parentToLinkTransform.rotation.GetInverse();
2444-
ignition::math::Quaterniond q3(r2.w, r2.x, r2.y, r2.z);
2445-
transformInParentLinkFrame.Rot() = q3 * _transformInLinkFrame.Rot();
2446-
// translate link to parentLink frame
2447-
transformInParentLinkFrame.Pos().X() = transformInParentLinkFrame.Pos().X()
2448-
- _parentToLinkTransform.position.x;
2449-
transformInParentLinkFrame.Pos().Y() = transformInParentLinkFrame.Pos().Y()
2450-
- _parentToLinkTransform.position.y;
2451-
transformInParentLinkFrame.Pos().Z() = transformInParentLinkFrame.Pos().Z()
2452-
- _parentToLinkTransform.position.z;
2453-
2454-
return transformInParentLinkFrame;
2455-
}
2456-
24572428
////////////////////////////////////////////////////////////////////////////////
24582429
void ReduceSDFExtensionToParent(urdf::LinkSharedPtr _link)
24592430
{
@@ -3499,20 +3470,27 @@ void ReduceSDFExtensionPluginFrameReplace(
34993470
TiXmlNode* rpyKey = (*_blobIt)->FirstChild("rpyOffset");
35003471
if (rpyKey)
35013472
{
3502-
urdf::Vector3 rpy = ParseVector3(rpyKey, M_PI/180.0);
3473+
urdf::Vector3 rpy = ParseVector3(rpyKey);
35033474
_reductionTransform.Rot() =
35043475
ignition::math::Quaterniond::EulerToQuaternion(rpy.x, rpy.y, rpy.z);
35053476
// remove xyzOffset and rpyOffset
35063477
(*_blobIt)->RemoveChild(rpyKey);
35073478
}
3479+
TiXmlNode* correctedOffsetKey =
3480+
(*_blobIt)->FirstChild("ignition::corrected_offsets");
3481+
if (correctedOffsetKey)
3482+
{
3483+
(*_blobIt)->RemoveChild(correctedOffsetKey);
3484+
}
35083485

35093486
// pass through the parent transform from fixed joint reduction
3510-
_reductionTransform = inverseTransformToParentFrame(_reductionTransform,
3487+
_reductionTransform = TransformToParentFrame(_reductionTransform,
35113488
_link->parent_joint->parent_to_joint_origin_transform);
35123489

35133490
// create new offset xml blocks
35143491
xyzKey = new TiXmlElement("xyzOffset");
35153492
rpyKey = new TiXmlElement("rpyOffset");
3493+
correctedOffsetKey = new TiXmlElement("ignition::corrected_offsets");
35163494

35173495
// create new offset xml blocks
35183496
urdf::Vector3 reductionXyz(_reductionTransform.Pos().X(),
@@ -3533,12 +3511,15 @@ void ReduceSDFExtensionPluginFrameReplace(
35333511

35343512
TiXmlText* xyzTxt = new TiXmlText(xyzStream.str());
35353513
TiXmlText* rpyTxt = new TiXmlText(rpyStream.str());
3514+
TiXmlText* correctedOffsetTxt = new TiXmlText("1");
35363515

35373516
xyzKey->LinkEndChild(xyzTxt);
35383517
rpyKey->LinkEndChild(rpyTxt);
3518+
correctedOffsetKey->LinkEndChild(correctedOffsetTxt);
35393519

35403520
(*_blobIt)->LinkEndChild(xyzKey);
35413521
(*_blobIt)->LinkEndChild(rpyKey);
3522+
(*_blobIt)->LinkEndChild(correctedOffsetKey);
35423523
}
35433524
}
35443525
}

‎test/integration/fixed_joint_reduction.cc‎

Lines changed: 30 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -36,6 +36,8 @@ const char SDF_TEST_FILE_COLLISION_VISUAL_EXTENSION_EMPTY_ROOT[] =
3636
"fixed_joint_reduction_collision_visual_empty_root.urdf";
3737
const char SDF_TEST_FILE_COLLISION_VISUAL_EXTENSION_EMPTY_ROOT_SDF[] =
3838
"fixed_joint_reduction_collision_visual_empty_root.sdf";
39+
const char SDF_TEST_FILE_PLUGIN_FRAME_EXTENSION[] =
40+
"fixed_joint_reduction_plugin_frame_extension.urdf";
3941

4042
static std::string GetFullTestFilePath(const char *_input)
4143
{
@@ -734,3 +736,31 @@ TEST(SDFParser, FixedJointReductionSimple)
734736
EXPECT_NEAR(iyz, mapIxyIxzIyz[linkName].Z(), gc_tolerance);
735737
}
736738
}
739+
740+
/////////////////////////////////////////////////
741+
// This test uses a urdf that has chained fixed joints with plugin that
742+
// contains bodyName, xyzOffset and rpyOffset.
743+
// Test to make sure the offsets have the correct transfrom and frame of
744+
// reference
745+
TEST(SDFParser, FixedJointReductionPluginFrameExtensionTest)
746+
{
747+
sdf::SDFPtr robot(new sdf::SDF());
748+
sdf::init(robot);
749+
ASSERT_TRUE(sdf::readFile(
750+
GetFullTestFilePath(SDF_TEST_FILE_PLUGIN_FRAME_EXTENSION),
751+
robot));
752+
753+
sdf::ElementPtr model = robot->Root()->GetElement("model");
754+
sdf::ElementPtr plugin = model->GetElement("plugin");
755+
756+
auto xyzOffset = plugin->Get<ignition::math::Vector3d>("xyzOffset");
757+
auto rpyOffset = plugin->Get<ignition::math::Vector3d>("rpyOffset");
758+
auto bodyName = plugin->Get<std::string>("bodyName");
759+
EXPECT_EQ("base_link", bodyName);
760+
EXPECT_EQ(ignition::math::Vector3d(-0.707108, 1.70711, 0), xyzOffset);
761+
EXPECT_EQ(ignition::math::Vector3d(0, 0, 1.5708), rpyOffset);
762+
763+
bool correctedOffset = plugin->Get<bool>("ignition::corrected_offsets");
764+
EXPECT_TRUE(correctedOffset);
765+
}
766+

0 commit comments

Comments
 (0)