diff --git a/Migration.md b/Migration.md index f301e8d83..0d356ec3f 100644 --- a/Migration.md +++ b/Migration.md @@ -283,6 +283,15 @@ ABI was broken for `sdf::Element`, and restored on version 11.2.1. + std::optional GetMaxValueAsString() const; + bool ValidateValue() const; +## libsdformat 9.8.0 to 9.8.1 + +### Modifications + +1. URDF parser now properly transforms poses for lights, projectors and sensors + from gazebo extension tags that are moved to a new link during fixed joint + reduction. + + [pull request 1114](https://github.com/osrf/sdformat/pull/1114) + ## libsdformat 9.4 to 9.5 ### Additions diff --git a/src/parser_urdf.cc b/src/parser_urdf.cc index 12389d6f3..5fae48884 100644 --- a/src/parser_urdf.cc +++ b/src/parser_urdf.cc @@ -91,17 +91,12 @@ void InsertSDFExtensionJoint(tinyxml2::XMLElement *_elem, /// option is set bool FixedJointShouldBeReduced(urdf::JointSharedPtr _jnt); -/// reduced fixed joints: apply transform reduction for ray sensors +/// reduced fixed joints: apply transform reduction for named elements /// in extensions when doing fixed joint reduction -void ReduceSDFExtensionSensorTransformReduction( +void ReduceSDFExtensionElementTransformReduction( std::vector::iterator _blobIt, - ignition::math::Pose3d _reductionTransform); - -/// reduced fixed joints: apply transform reduction for projectors in -/// extensions when doing fixed joint reduction -void ReduceSDFExtensionProjectorTransformReduction( - std::vector::iterator _blobIt, - ignition::math::Pose3d _reductionTransform); + const ignition::math::Pose3d &_reductionTransform, + const std::string &_elementName); /// reduced fixed joints: apply transform reduction to extensions @@ -409,8 +404,8 @@ void ReduceVisualToParent(urdf::LinkSharedPtr _parentLink, // collision elements of the child link into the parent link void ReduceFixedJoints(tinyxml2::XMLElement *_root, urdf::LinkSharedPtr _link) { - // if child is attached to self by fixed _link first go up the tree, - // check it's children recursively + // if child is attached to self by fixed joint first go up the tree, + // check its children recursively for (unsigned int i = 0 ; i < _link->child_links.size() ; ++i) { if (FixedJointShouldBeReduced(_link->child_links[i]->parent_joint)) @@ -2479,8 +2474,7 @@ void ReduceSDFExtensionToParent(urdf::LinkSharedPtr _link) for (std::vector::iterator ge = ext->second.begin(); ge != ext->second.end(); ++ge) { - (*ge)->reductionTransform = TransformToParentFrame( - (*ge)->reductionTransform, + (*ge)->reductionTransform = CopyPose( _link->parent_joint->parent_to_joint_origin_transform); // for sensor and projector blocks only ReduceSDFExtensionsTransform((*ge)); @@ -2568,11 +2562,12 @@ void ReduceSDFExtensionsTransform(SDFExtensionPtr _ge) for (auto blobIt = _ge->blobs.begin(); blobIt != _ge->blobs.end(); ++blobIt) { - /// @todo make sure we are not missing any additional transform reductions - ReduceSDFExtensionSensorTransformReduction(blobIt, - _ge->reductionTransform); - ReduceSDFExtensionProjectorTransformReduction(blobIt, - _ge->reductionTransform); + ReduceSDFExtensionElementTransformReduction( + blobIt, _ge->reductionTransform, "light"); + ReduceSDFExtensionElementTransformReduction( + blobIt, _ge->reductionTransform, "projector"); + ReduceSDFExtensionElementTransformReduction( + blobIt, _ge->reductionTransform, "sensor"); } } @@ -3364,12 +3359,13 @@ bool FixedJointShouldBeReduced(urdf::JointSharedPtr _jnt) } //////////////////////////////////////////////////////////////////////////////// -void ReduceSDFExtensionSensorTransformReduction( +void ReduceSDFExtensionElementTransformReduction( std::vector::iterator _blobIt, - ignition::math::Pose3d _reductionTransform) + const ignition::math::Pose3d &_reductionTransform, + const std::string &_elementName) { - auto sensorElement = (*_blobIt)->FirstChildElement(); - if ( strcmp(sensorElement->Name(), "sensor") == 0) + auto element = (*_blobIt)->FirstChildElement(); + if ( strcmp(element->Name(), _elementName.c_str()) == 0) { // parse it and add/replace the reduction transform // find first instance of xyz and rpy, replace with reduction transform @@ -3383,166 +3379,51 @@ void ReduceSDFExtensionSensorTransformReduction( // sdfdbg << " " << streamIn.CStr() << "\n"; // } - auto sensorPose {ignition::math::Pose3d::Zero}; + auto pose {ignition::math::Pose3d::Zero}; { - auto sensorPosText = "0 0 0 0 0 0"; - const auto& oldPoseKey = sensorElement->FirstChildElement("pose"); + std::string poseText = "0 0 0 0 0 0"; + + const auto& oldPoseKey = element->FirstChildElement("pose"); if (oldPoseKey) { const auto& poseElemXml = oldPoseKey->ToElement(); if (poseElemXml->Attribute("relative_to")) - return; - - // see below for explanation; if a sibling element exists, it stores the - // original tag content - const auto& sibling = sensorElement->NextSibling(); - if (poseElemXml->GetText() && !sibling) - sensorPosText = poseElemXml->GetText(); - else if (sibling && sibling->ToElement()->GetText()) - sensorPosText = sibling->ToElement()->GetText(); - else { - sdferr << "Unexpected case in sensor pose computation\n"; return; } - // delete the tag, we'll add a new one at the end - sensorElement->DeleteChild(oldPoseKey); - } - - // parse the 6-tuple text into math::Pose3d - std::stringstream ss; - ss.imbue(std::locale::classic()); - ss << sensorPosText; - ss >> sensorPose; - if (ss.fail()) - { - sdferr << "Could not parse : [" << sensorPosText << "]\n"; - return; - } - - // critical part: we store the original tag from actually - // as a sibling of ... only first element of the blob is processed - // further, so its siblings can be used as temporary storage; we store the - // original tag there so that we can use the tag - // for storing the reduced position - auto doc = (*_blobIt)->GetDocument(); - const auto& poseTxt = doc->NewText(sensorPosText); - auto poseKey = doc->NewElement("pose"); - poseKey->LinkEndChild(poseTxt); - (*_blobIt)->LinkEndChild(poseKey); - } - - _reductionTransform = _reductionTransform * sensorPose; - - // convert reductionTransform to values - urdf::Vector3 reductionXyz(_reductionTransform.Pos().X(), - _reductionTransform.Pos().Y(), - _reductionTransform.Pos().Z()); - urdf::Rotation reductionQ(_reductionTransform.Rot().X(), - _reductionTransform.Rot().Y(), - _reductionTransform.Rot().Z(), - _reductionTransform.Rot().W()); - - urdf::Vector3 reductionRpy; - reductionQ.getRPY(reductionRpy.x, reductionRpy.y, reductionRpy.z); - - // output updated pose to text - std::ostringstream poseStream; - poseStream << reductionXyz.x << " " << reductionXyz.y - << " " << reductionXyz.z << " " << reductionRpy.x - << " " << reductionRpy.y << " " << reductionRpy.z; - - auto* doc = (*_blobIt)->GetDocument(); - tinyxml2::XMLText *poseTxt = doc->NewText(poseStream.str().c_str()); - tinyxml2::XMLElement *poseKey = doc->NewElement("pose"); - - poseKey->LinkEndChild(poseTxt); - - sensorElement->LinkEndChild(poseKey); - } -} - -//////////////////////////////////////////////////////////////////////////////// -void ReduceSDFExtensionProjectorTransformReduction( - std::vector::iterator _blobIt, - ignition::math::Pose3d _reductionTransform) -{ - auto projectorElement = (*_blobIt)->FirstChildElement(); - if ( strcmp(projectorElement->Name(), "projector") == 0) - { - // parse it and add/replace the reduction transform - // find first instance of xyz and rpy, replace with reduction transform - // - // for (tinyxml2::XMLNode* elIt = (*_blobIt)->FirstChildElement(); - // elIt; elIt = elIt->NextSibling()) - // { - // std::ostringstream streamIn; - // streamIn << *elIt; - // sdfdbg << " " << streamIn << "\n"; - // } - - auto projectorPose {ignition::math::Pose3d::Zero}; - { - auto projectorPosText = "0 0 0 0 0 0"; - const auto& oldPoseKey = projectorElement->FirstChildElement("pose"); - if (oldPoseKey) - { - const auto& poseElemXml = oldPoseKey->ToElement(); - if (poseElemXml->Attribute("relative_to")) - return; - - // see below for explanation; if a sibling element exists, it stores the - // original tag content - const auto& sibling = projectorElement->NextSibling(); - if (poseElemXml->GetText() && !sibling) - projectorPosText = poseElemXml->GetText(); - else if (sibling && sibling->ToElement()->GetText()) - projectorPosText = sibling->ToElement()->GetText(); - else + if (poseElemXml->GetText()) { - sdferr << "Unexpected case in projector pose computation\n"; - return; + poseText = poseElemXml->GetText(); } // delete the tag, we'll add a new one at the end - projectorElement->DeleteChild(oldPoseKey); + element->DeleteChild(oldPoseKey); } // parse the 6-tuple text into math::Pose3d - std::stringstream ss; + std::stringstream ss; ss.imbue(std::locale::classic()); - ss << projectorPosText; - ss >> projectorPose; + ss << poseText; + ss >> pose; if (ss.fail()) { - sdferr << "Could not parse : [" - << projectorPosText << "]\n"; + sdferr << "Could not parse <" << _elementName << ">: [" + << poseText << "]\n"; return; } - - // critical part: we store the original tag from - // actually as a sibling of ... only first element of the blob - // is processed further, so its siblings can be used as temporary storage; - // we store the original tag there so that we can use the - // tag for storing the reduced position - auto doc = (*_blobIt)->GetDocument(); - const auto& poseTxt = doc->NewText(projectorPosText); - auto poseKey = doc->NewElement("pose"); - poseKey->LinkEndChild(poseTxt); - (*_blobIt)->LinkEndChild(poseKey); } - _reductionTransform = _reductionTransform * projectorPose; + pose = _reductionTransform * pose; // convert reductionTransform to values - urdf::Vector3 reductionXyz(_reductionTransform.Pos().X(), - _reductionTransform.Pos().Y(), - _reductionTransform.Pos().Z()); - urdf::Rotation reductionQ(_reductionTransform.Rot().X(), - _reductionTransform.Rot().Y(), - _reductionTransform.Rot().Z(), - _reductionTransform.Rot().W()); + urdf::Vector3 reductionXyz(pose.Pos().X(), + pose.Pos().Y(), + pose.Pos().Z()); + urdf::Rotation reductionQ(pose.Rot().X(), + pose.Rot().Y(), + pose.Rot().Z(), + pose.Rot().W()); urdf::Vector3 reductionRpy; reductionQ.getRPY(reductionRpy.x, reductionRpy.y, reductionRpy.z); @@ -3555,10 +3436,11 @@ void ReduceSDFExtensionProjectorTransformReduction( auto* doc = (*_blobIt)->GetDocument(); tinyxml2::XMLText *poseTxt = doc->NewText(poseStream.str().c_str()); - auto poseKey = doc->NewElement("pose"); + tinyxml2::XMLElement *poseKey = doc->NewElement("pose"); + poseKey->LinkEndChild(poseTxt); - projectorElement->LinkEndChild(poseKey); + element->LinkEndChild(poseKey); } } diff --git a/test/integration/urdf_gazebo_extensions.cc b/test/integration/urdf_gazebo_extensions.cc index df7a53b33..7a1ffcc0f 100644 --- a/test/integration/urdf_gazebo_extensions.cc +++ b/test/integration/urdf_gazebo_extensions.cc @@ -142,149 +142,156 @@ TEST(SDFParser, UrdfGazeboExtensionURDFTest) } ASSERT_TRUE(link0); - bool foundSensorNoPose {false}; - bool foundSensorPose {false}; - bool foundSensorPoseRelative {false}; - bool foundSensorPoseTwoLevel {false}; - bool foundIssue378Sensor {false}; - bool foundIssue67Sensor {false}; - - for (sdf::ElementPtr sensor = link0->GetElement("sensor"); sensor; - sensor = sensor->GetNextElement("sensor")) + auto checkElementPoses = [&](const std::string &_elementName) -> void { - const auto& sensorName = sensor->Get("name"); - if (sensorName == "sensorNoPose") + bool foundElementNoPose {false}; + bool foundElementPose {false}; + bool foundElementPoseRelative {false}; + bool foundElementPoseTwoLevel {false}; + bool foundIssue378Element {false}; + bool foundIssue67Element {false}; + + for (sdf::ElementPtr element = link0->GetElement(_elementName); element; + element = element->GetNextElement(_elementName)) { - foundSensorNoPose = true; - ASSERT_TRUE(sensor->HasElement("pose")); - const auto poseElem = sensor->GetElement("pose"); + const auto& elementName = element->Get("name"); + if (elementName == _elementName + "NoPose") + { + foundElementNoPose = true; + EXPECT_TRUE(element->HasElement("pose")); + const auto poseElem = element->GetElement("pose"); - const auto& posePair = poseElem->Get( - "", ignition::math::Pose3d::Zero); - ASSERT_TRUE(posePair.second); + const auto& posePair = poseElem->Get( + "", ignition::math::Pose3d::Zero); + ASSERT_TRUE(posePair.second); - const auto& pose = posePair.first; + const auto& pose = posePair.first; - EXPECT_DOUBLE_EQ(pose.X(), 333.0); - EXPECT_DOUBLE_EQ(pose.Y(), 0.0); - EXPECT_DOUBLE_EQ(pose.Z(), 0.0); - EXPECT_DOUBLE_EQ(pose.Roll(), 0.0); - EXPECT_DOUBLE_EQ(pose.Pitch(), 0.0); - EXPECT_NEAR(pose.Yaw(), IGN_PI_2, 1e-5); + EXPECT_DOUBLE_EQ(pose.X(), 333.0); + EXPECT_DOUBLE_EQ(pose.Y(), 0.0); + EXPECT_DOUBLE_EQ(pose.Z(), 0.0); + EXPECT_DOUBLE_EQ(pose.Roll(), 0.0); + EXPECT_DOUBLE_EQ(pose.Pitch(), 0.0); + EXPECT_NEAR(pose.Yaw(), IGN_PI_2, 1e-5); - EXPECT_FALSE(poseElem->GetNextElement("pose")); - } - else if (sensorName == "sensorPose") - { - foundSensorPose = true; - ASSERT_TRUE(sensor->HasElement("pose")); - const auto poseElem = sensor->GetElement("pose"); + EXPECT_FALSE(poseElem->GetNextElement("pose")); + } + else if (elementName == _elementName + "Pose") + { + foundElementPose = true; + EXPECT_TRUE(element->HasElement("pose")); + const auto poseElem = element->GetElement("pose"); - const auto& posePair = poseElem->Get( - "", ignition::math::Pose3d::Zero); - ASSERT_TRUE(posePair.second); + const auto& posePair = poseElem->Get( + "", ignition::math::Pose3d::Zero); + ASSERT_TRUE(posePair.second); - const auto& pose = posePair.first; + const auto& pose = posePair.first; - EXPECT_DOUBLE_EQ(pose.X(), 333.0); - EXPECT_DOUBLE_EQ(pose.Y(), 111.0); - EXPECT_DOUBLE_EQ(pose.Z(), 0.0); - EXPECT_DOUBLE_EQ(pose.Roll(), 0.0); - EXPECT_DOUBLE_EQ(pose.Pitch(), 0.0); - EXPECT_NEAR(pose.Yaw(), IGN_PI_2 - 1, 1e-5); + EXPECT_DOUBLE_EQ(pose.X(), 333.0); + EXPECT_DOUBLE_EQ(pose.Y(), 111.0); + EXPECT_DOUBLE_EQ(pose.Z(), 0.0); + EXPECT_DOUBLE_EQ(pose.Roll(), 0.0); + EXPECT_DOUBLE_EQ(pose.Pitch(), 0.0); + EXPECT_NEAR(pose.Yaw(), IGN_PI_2 - 1, 1e-5); - EXPECT_FALSE(poseElem->GetNextElement("pose")); - } - else if (sensorName == "sensorPoseRelative") - { - foundSensorPoseRelative = true; - ASSERT_TRUE(sensor->HasElement("pose")); - const auto poseElem = sensor->GetElement("pose"); + EXPECT_FALSE(poseElem->GetNextElement("pose")); + } + else if (elementName == _elementName + "PoseRelative") + { + foundElementPoseRelative = true; + EXPECT_TRUE(element->HasElement("pose")); + const auto poseElem = element->GetElement("pose"); - const auto& posePair = poseElem->Get( - "", ignition::math::Pose3d::Zero); - ASSERT_TRUE(posePair.second); + const auto& posePair = poseElem->Get( + "", ignition::math::Pose3d::Zero); + ASSERT_TRUE(posePair.second); - const auto& pose = posePair.first; + const auto& pose = posePair.first; - EXPECT_DOUBLE_EQ(pose.X(), 111.0); - EXPECT_DOUBLE_EQ(pose.Y(), 0.0); - EXPECT_DOUBLE_EQ(pose.Z(), 0.0); - EXPECT_DOUBLE_EQ(pose.Roll(), 0.0); - EXPECT_DOUBLE_EQ(pose.Pitch(), 0.0); - EXPECT_NEAR(pose.Yaw(), -1, 1e-5); + EXPECT_DOUBLE_EQ(pose.X(), 111.0); + EXPECT_DOUBLE_EQ(pose.Y(), 0.0); + EXPECT_DOUBLE_EQ(pose.Z(), 0.0); + EXPECT_DOUBLE_EQ(pose.Roll(), 0.0); + EXPECT_DOUBLE_EQ(pose.Pitch(), 0.0); + EXPECT_NEAR(pose.Yaw(), -1, 1e-5); - EXPECT_FALSE(poseElem->GetNextElement("pose")); - } - else if (sensorName == "sensorPoseTwoLevel") - { - foundSensorPoseTwoLevel = true; - ASSERT_TRUE(sensor->HasElement("pose")); - const auto poseElem = sensor->GetElement("pose"); + EXPECT_FALSE(poseElem->GetNextElement("pose")); + } + else if (elementName == _elementName + "PoseTwoLevel") + { + foundElementPoseTwoLevel = true; + EXPECT_TRUE(element->HasElement("pose")); + const auto poseElem = element->GetElement("pose"); - const auto& posePair = poseElem->Get( - "", ignition::math::Pose3d::Zero); - ASSERT_TRUE(posePair.second); + const auto& posePair = poseElem->Get( + "", ignition::math::Pose3d::Zero); + ASSERT_TRUE(posePair.second); - const auto& pose = posePair.first; + const auto& pose = posePair.first; - EXPECT_DOUBLE_EQ(pose.X(), 333.0); - EXPECT_DOUBLE_EQ(pose.Y(), 111.0); - EXPECT_DOUBLE_EQ(pose.Z(), 222.0); - EXPECT_DOUBLE_EQ(pose.Roll(), 0.0); - EXPECT_DOUBLE_EQ(pose.Pitch(), 0.0); - EXPECT_NEAR(pose.Yaw(), IGN_PI_2 - 1, 1e-5); + EXPECT_DOUBLE_EQ(pose.X(), 333.0); + EXPECT_DOUBLE_EQ(pose.Y(), 111.0); + EXPECT_DOUBLE_EQ(pose.Z(), 222.0); + EXPECT_DOUBLE_EQ(pose.Roll(), 0.0); + EXPECT_DOUBLE_EQ(pose.Pitch(), 0.0); + EXPECT_NEAR(pose.Yaw(), IGN_PI_2 - 1, 1e-5); - EXPECT_FALSE(poseElem->GetNextElement("pose")); - } - else if (sensorName == "issue378_sensor") - { - foundIssue378Sensor = true; - ASSERT_TRUE(sensor->HasElement("pose")); - const auto poseElem = sensor->GetElement("pose"); + EXPECT_FALSE(poseElem->GetNextElement("pose")); + } + else if (elementName == "issue378_" + _elementName) + { + foundIssue378Element = true; + EXPECT_TRUE(element->HasElement("pose")); + const auto poseElem = element->GetElement("pose"); - const auto& posePair = poseElem->Get( - "", ignition::math::Pose3d::Zero); - ASSERT_TRUE(posePair.second); + const auto& posePair = poseElem->Get( + "", ignition::math::Pose3d::Zero); + ASSERT_TRUE(posePair.second); - const auto& pose = posePair.first; + const auto& pose = posePair.first; - EXPECT_DOUBLE_EQ(pose.X(), 1); - EXPECT_DOUBLE_EQ(pose.Y(), 2); - EXPECT_DOUBLE_EQ(pose.Z(), 3); - EXPECT_DOUBLE_EQ(pose.Roll(), 0.1); - EXPECT_DOUBLE_EQ(pose.Pitch(), 0.2); - EXPECT_DOUBLE_EQ(pose.Yaw(), 0.3); + EXPECT_DOUBLE_EQ(pose.X(), 1); + EXPECT_DOUBLE_EQ(pose.Y(), 2); + EXPECT_DOUBLE_EQ(pose.Z(), 3); + EXPECT_DOUBLE_EQ(pose.Roll(), 0.1); + EXPECT_DOUBLE_EQ(pose.Pitch(), 0.2); + EXPECT_DOUBLE_EQ(pose.Yaw(), 0.3); - EXPECT_FALSE(poseElem->GetNextElement("pose")); - } - else if (sensorName == "issue67_sensor") - { - foundIssue67Sensor = true; - ASSERT_TRUE(sensor->HasElement("pose")); - const auto poseElem = sensor->GetElement("pose"); + EXPECT_FALSE(poseElem->GetNextElement("pose")); + } + else if (elementName == "issue67_" + _elementName) + { + foundIssue67Element = true; + EXPECT_TRUE(element->HasElement("pose")); + const auto poseElem = element->GetElement("pose"); - const auto& posePair = poseElem->Get( - "", ignition::math::Pose3d::Zero); - ASSERT_TRUE(posePair.second); + const auto& posePair = poseElem->Get( + "", ignition::math::Pose3d::Zero); + ASSERT_TRUE(posePair.second); - const auto& pose = posePair.first; + const auto& pose = posePair.first; - EXPECT_GT(std::abs(pose.X() - (-0.20115)), 0.1); - EXPECT_GT(std::abs(pose.Y() - 0.42488), 0.1); - EXPECT_GT(std::abs(pose.Z() - 0.30943), 0.1); - EXPECT_GT(std::abs(pose.Roll() - 1.5708), 0.1); - EXPECT_GT(std::abs(pose.Pitch() - (-0.89012)), 0.1); - EXPECT_GT(std::abs(pose.Yaw() - 1.5708), 0.1); + EXPECT_GT(std::abs(pose.X() - (-0.20115)), 0.1); + EXPECT_GT(std::abs(pose.Y() - 0.42488), 0.1); + EXPECT_GT(std::abs(pose.Z() - 0.30943), 0.1); + EXPECT_GT(std::abs(pose.Roll() - 1.5708), 0.1); + EXPECT_GT(std::abs(pose.Pitch() - (-0.89012)), 0.1); + EXPECT_GT(std::abs(pose.Yaw() - 1.5708), 0.1); - EXPECT_FALSE(poseElem->GetNextElement("pose")); + EXPECT_FALSE(poseElem->GetNextElement("pose")); + } } - } - EXPECT_TRUE(foundSensorNoPose); - EXPECT_TRUE(foundSensorPose); - EXPECT_TRUE(foundSensorPoseRelative); - EXPECT_TRUE(foundSensorPoseTwoLevel); - EXPECT_TRUE(foundIssue378Sensor); - EXPECT_TRUE(foundIssue67Sensor); + EXPECT_TRUE(foundElementNoPose) << _elementName; + EXPECT_TRUE(foundElementPose) << _elementName; + EXPECT_TRUE(foundElementPoseRelative) << _elementName; + EXPECT_TRUE(foundElementPoseTwoLevel) << _elementName; + EXPECT_TRUE(foundIssue378Element) << _elementName; + EXPECT_TRUE(foundIssue67Element) << _elementName; + }; + + checkElementPoses("light"); + checkElementPoses("projector"); + checkElementPoses("sensor"); } diff --git a/test/integration/urdf_gazebo_extensions.urdf b/test/integration/urdf_gazebo_extensions.urdf index 1daf5a8cd..a0a92c6c7 100644 --- a/test/integration/urdf_gazebo_extensions.urdf +++ b/test/integration/urdf_gazebo_extensions.urdf @@ -165,6 +165,8 @@ + + 6.0 @@ -195,6 +197,12 @@ + + 111 0 0 0 0 -1 + + + 111 0 0 0 0 -1 + 111 0 0 0 0 -1 6.0 @@ -226,6 +234,12 @@ + + 111 0 0 0 0 -1 + + + 111 0 0 0 0 -1 + 111 0 0 0 0 -1 6.0 @@ -269,8 +283,17 @@ + + + 111 0 0 0 0 -1 + + + + 111 0 0 0 0 -1 + - 111 0 0 0 0 -1 + + 111 0 0 0 0 -1 6.0 1.36869112579 @@ -289,6 +312,12 @@ + + 1.0 2.0 3.0 0.1 0.2 0.3 + + + 1.0 2.0 3.0 0.1 0.2 0.3 + 1 400 @@ -320,6 +349,12 @@ + + 1 1 1 1.570796 1.570796 1.570796 + + + 1 1 1 1.570796 1.570796 1.570796 + true 1 1 1 1.570796 1.570796 1.570796